bool nmrRegistrationRigid(vctDynamicConstVectorBase<_vectorOwnerType, vct3> &dataSet1,
vctDynamicConstVectorBase<_vectorOwnerType, vct3> &dataSet2,
vctFrm3 &transform, double *fre)
{
size_t npoints = dataSet1.size();
size_t i;
if (npoints != dataSet2.size()) {
CMN_LOG_RUN_WARNING << "nmrRegistrationPairedPoint: incompatible sizes: " << npoints << ", "
<< dataSet2.size() << std::endl;
return false;
}
else if (npoints <= 0) {
CMN_LOG_RUN_WARNING << "nmrRegistrationPairedPoint called for " << npoints << " points." << std::endl;
return false;
}
// Compute averages
vct3 avg1 = dataSet1.SumOfElements();
avg1.Divide(static_cast<double>(npoints));
vct3 avg2 = dataSet2.SumOfElements();
avg2.Divide(static_cast<double>(npoints));
// Compute the sum of the outer products of (dataSet1-avg1) and (dataSet2-avg2)
vctDouble3x3 H, sumH;
sumH.SetAll(0.0);
for (i = 0; i < npoints; i++) {
H.OuterProductOf(dataSet1[i]-avg1, dataSet2[i]-avg2);
sumH.Add(H);
}
// Now, compute SVD of sumH
vctDouble3x3 U, Vt;
vctDouble3 S;
nmrSVD(sumH, U, S, Vt);
// Compute X = V*U' = (U*V')'
vctDouble3x3 X = (U*Vt).Transpose();
double det = vctDeterminant<3>::Compute(X);
// If determinant is not 1, apply correction from Umeyama(1991)
if (fabs(det-1.0) > 1e-6) {
vctDouble3x3 Fix(0.0);
Fix.Diagonal() = vct3(1.0, 1.0, -1.0);
X = (U*Fix*Vt).Transpose();
det = vctDeterminant<3>::Compute(X);
if (fabs(det-1.0) > 1e-6) {
CMN_LOG_RUN_WARNING << "nmrRegistrationPairedPoint: registration failed!!" << std::endl;
return false;
}
}
vctMatRot3 R;
R.Assign(X);
transform = vctFrm3(R, avg2-R*avg1);
// Now, compute residual error if fre is not null
if (fre) {
double err2 = 0.0;
for (i = 0; i < npoints; i++)
err2 += (dataSet2[i] - transform*dataSet1[i]).NormSquare();
*fre = sqrt(err2/npoints);
}
return true;
}
int main(){
cmnLogger::SetMask( CMN_LOG_ALLOW_ALL );
cmnLogger::SetMaskFunction( CMN_LOG_ALLOW_ALL );
cmnLogger::SetMaskDefaultLog( CMN_LOG_ALLOW_ALL );
// Create the world
osaODEWorld* world = new osaODEWorld( 0.001 );
// Create a camera
int x = 0, y = 0;
int width = 640, height = 480;
double Znear = 0.01, Zfar = 10.0;
osg::ref_ptr<osaOSGMono> camera;
camera = new osaOSGMono( world,
x, y, width, height,
55, ((double)width)/((double)height),
Znear, Zfar );
camera->Initialize();
// Create objects
cmnPath path;
path.AddRelativeToCisstShare("/models");
path.AddRelativeToCisstShare("/models/hubble");
// Create a rigid body. Make up some mass + com + moit
double mass = 1.0;
vctFixedSizeVector<double,3> com( 0.0 );
vctFixedSizeMatrix<double,3,3> moit = vctFixedSizeMatrix<double,3,3>::Eye();
vctFixedSizeVector<double,3> u( 0.780004, 0.620257, 0.082920 );
u.NormalizedSelf();
vctFrame4x4<double> Rtwh( vctAxisAngleRotation3<double>( u, 0.7391 ),
vctFixedSizeVector<double,3>( 0.0, 0.0, 1.0 ) );
osg::ref_ptr<osaODEBody> hubble;
hubble = new osaODEBody( path.Find("hst.3ds"), world, Rtwh, mass, com, moit );
// Create a static body. This body has no mass and cannot be moved
osg::ref_ptr<osaODEBody> background;
background = new osaODEBody( path.Find("background.3ds"), world, vctFrm3() );
std::cout << "ESC to quit" << std::endl;
while( !camera->done() ){
world->Step();
camera->frame();
}
return 0;
}
void mtsMedtronicStealthlink::Tool::Assign(const MNavStealthLink::DataItem & item_in)
{
if (typeid(*&item_in) == typeid(const MNavStealthLink::Instrument) ){
const MNavStealthLink::Instrument & tool_in = dynamic_cast<const MNavStealthLink::Instrument & >(item_in);
frameConversion(this->MarkerPosition.Position(),tool_in.localizer_T_instrument);
this->MarkerPosition.SetValid(tool_in.visibility == MNavStealthLink::Instrument::VISIBLE
|| tool_in.visibility == MNavStealthLink::Instrument::ALMOST_BLOCKED);
this->TooltipPosition.SetValid(tool_in.isCalibrated && this->MarkerPosition.Valid());
if (this->TooltipPosition.Valid()){
this->TooltipPosition.Position() = vctFrm3(this->MarkerPosition.Position().Rotation(),
vct3(tool_in.localizerPosition.tip.x,tool_in.localizerPosition.tip.y,tool_in.localizerPosition.tip.z));
//this->TooltipPosition.Position() = this->MarkerPosition.Position();
}
}else if(typeid(*&item_in) == typeid(const MNavStealthLink::Frame)){
const MNavStealthLink::Frame & frame_in = dynamic_cast<const MNavStealthLink::Frame & >(item_in);
frameConversion(this->MarkerPosition.Position(),frame_in.frame_T_localizer);
//Trek expects localizer_T_frame
this->MarkerPosition.Position() = this->MarkerPosition.Position().Inverse();
this->MarkerPosition.SetValid(frame_in.visibility == MNavStealthLink::Frame::VISIBLE
|| frame_in.visibility == MNavStealthLink::Frame::ALMOST_BLOCKED);
}
}
// computes a local reference frame for this node based on the
// covariances of the datum sort positions; returns a
// transformation that converts points from world -> node coordinates
// NOTE: the origin of node coordinates is placed at the data centroid
// with the x-axis oriented in the direction of largest data spread
vctFrm3 cisstCovTreeNode::ComputeCovFrame(int i0, int i1)
{
vct3 p(0, 0, 0);
vctRot3 R;
vctDouble3x3 C(0.0); // covariances
vctDouble3x3 Q; // Eigen vectors
vct3 e; // Eigen values
int i;
if (i1 <= i0) return vctFrm3();
int N = i1 - i0;
if (N < 5)
{
// since we can't create a fully determined covariance matrix
// from only a few points, use parent frame or identity frame
return (pParent != NULL) ? pParent->F : vctFrm3();
}
//compute centroid of sort positions
for (i = i0; i < i1; i++)
{
AccumulateCentroid(Datum(i), p);
}
p *= (1.0 / (i1 - i0));
// compute covariance of sort positions
for (i = i0; i < i1; i++)
{
AccumulateVariances(Datum(i), p, C);
}
// compute eigen decomposition of covariances
ComputeCovEigenDecomposition_SVD(C, e, Q);
//int rc = nmrJacobi(C, e, Q); // e=eigen values, Q=eigenVectors
// find largest eigenvalue
int j = 0;
for (i = 1; i < 3; i++)
{
if (fabs(e(i)) > fabs(e(j))) j = i;
}
switch (j)
{
case 0: // E[0] is biggest eigen value
R = Q;
break;
case 1: // E[1] is biggest eigen value
// by right hand rule, map x->y, y->-x, z->z
// (assuming Q is a valid rotation matrix)
R.Column(0) = Q.Column(1);
R.Column(1) = -Q.Column(0);
R.Column(2) = Q.Column(2);
break;
case 2: // E[2] is biggest eigen value
// by right hand rule: x->z, y->y, z->-x
R.Column(0) = Q.Column(2);
R.Column(1) = Q.Column(1);
R.Column(2) = -Q.Column(0);
}
// [R,p] is the node -> world transform
// what we want is the inverse of this
return vctFrm3(R, p).Inverse();
}
