/*---------------------------------------------------------------
HornMethod
---------------------------------------------------------------*/
double scanmatching::HornMethod(
const vector_double &inVector,
vector_double &outVector, // The output vector
bool forceScaleToUnity )
{
MRPT_START
vector_double input;
input.resize( inVector.size() );
input = inVector;
outVector.resize( 7 );
// Compute the centroids
TPoint3D cL(0,0,0), cR(0,0,0);
const size_t nMatches = input.size()/6;
ASSERT_EQUAL_(input.size()%6, 0)
for( unsigned int i = 0; i < nMatches; i++ )
{
cL.x += input[i*6+3];
cL.y += input[i*6+4];
cL.z += input[i*6+5];
cR.x += input[i*6+0];
cR.y += input[i*6+1];
cR.z += input[i*6+2];
}
ASSERT_ABOVE_(nMatches,0)
const double F = 1.0/nMatches;
cL *= F;
cR *= F;
CMatrixDouble33 S; // S.zeros(); // Zeroed by default
// Substract the centroid and compute the S matrix of cross products
for( unsigned int i = 0; i < nMatches; i++ )
{
input[i*6+3] -= cL.x;
input[i*6+4] -= cL.y;
input[i*6+5] -= cL.z;
input[i*6+0] -= cR.x;
input[i*6+1] -= cR.y;
input[i*6+2] -= cR.z;
S.get_unsafe(0,0) += input[i*6+3]*input[i*6+0];
S.get_unsafe(0,1) += input[i*6+3]*input[i*6+1];
S.get_unsafe(0,2) += input[i*6+3]*input[i*6+2];
S.get_unsafe(1,0) += input[i*6+4]*input[i*6+0];
S.get_unsafe(1,1) += input[i*6+4]*input[i*6+1];
S.get_unsafe(1,2) += input[i*6+4]*input[i*6+2];
S.get_unsafe(2,0) += input[i*6+5]*input[i*6+0];
S.get_unsafe(2,1) += input[i*6+5]*input[i*6+1];
S.get_unsafe(2,2) += input[i*6+5]*input[i*6+2];
}
// Construct the N matrix
CMatrixDouble44 N; // N.zeros(); // Zeroed by default
N.set_unsafe( 0,0,S.get_unsafe(0,0) + S.get_unsafe(1,1) + S.get_unsafe(2,2) );
N.set_unsafe( 0,1,S.get_unsafe(1,2) - S.get_unsafe(2,1) );
N.set_unsafe( 0,2,S.get_unsafe(2,0) - S.get_unsafe(0,2) );
N.set_unsafe( 0,3,S.get_unsafe(0,1) - S.get_unsafe(1,0) );
N.set_unsafe( 1,0,N.get_unsafe(0,1) );
N.set_unsafe( 1,1,S.get_unsafe(0,0) - S.get_unsafe(1,1) - S.get_unsafe(2,2) );
N.set_unsafe( 1,2,S.get_unsafe(0,1) + S.get_unsafe(1,0) );
N.set_unsafe( 1,3,S.get_unsafe(2,0) + S.get_unsafe(0,2) );
N.set_unsafe( 2,0,N.get_unsafe(0,2) );
N.set_unsafe( 2,1,N.get_unsafe(1,2) );
N.set_unsafe( 2,2,-S.get_unsafe(0,0) + S.get_unsafe(1,1) - S.get_unsafe(2,2) );
N.set_unsafe( 2,3,S.get_unsafe(1,2) + S.get_unsafe(2,1) );
N.set_unsafe( 3,0,N.get_unsafe(0,3) );
N.set_unsafe( 3,1,N.get_unsafe(1,3) );
N.set_unsafe( 3,2,N.get_unsafe(2,3) );
N.set_unsafe( 3,3,-S.get_unsafe(0,0) - S.get_unsafe(1,1) + S.get_unsafe(2,2) );
// q is the quaternion correspondent to the greatest eigenvector of the N matrix (last column in Z)
CMatrixDouble44 Z, D;
vector_double v;
N.eigenVectors( Z, D );
Z.extractCol( Z.getColCount()-1, v );
ASSERTDEB_( fabs( sqrt( v[0]*v[0] + v[1]*v[1] + v[2]*v[2] + v[3]*v[3] ) - 1.0 ) < 0.1 );
// Make q_r > 0
if( v[0] < 0 ){ v[0] *= -1; v[1] *= -1; v[2] *= -1; v[3] *= -1; }
CPose3DQuat q; // Create a pose rotation with the quaternion
for(unsigned int i = 0; i < 4; i++ ) // insert the quaternion part
outVector[i+3] = q[i+3] = v[i];
// Compute scale
double num = 0.0;
double den = 0.0;
for( unsigned int i = 0; i < nMatches; i++ )
{
num += square( input[i*6+0] ) + square( input[i*6+1] ) + square( input[i*6+2] );
den += square( input[i*6+3] ) + square( input[i*6+4] ) + square( input[i*6+5] );
} // end-for
// The scale:
double s = std::sqrt( num/den );
// Enforce scale to be 1
if( forceScaleToUnity )
s = 1.0;
TPoint3D pp;
q.composePoint( cL.x, cL.y, cL.z, pp.x, pp.y, pp.z );
pp*=s;
outVector[0] = cR.x - pp.x; // X
outVector[1] = cR.y - pp.y; // Y
outVector[2] = cR.z - pp.z; // Z
return s; // return scale
MRPT_END
}
