// compute optimal pose and scale using a procedure described by Umeyame,
// Least-squares estimation of transformation parameters between two point patterns (IEEE Pami 1991)
double computeScalingFactor(MeshType* mesh1, MeshType* mesh2)
{
if (mesh1->GetNumberOfPoints() != mesh2->GetNumberOfPoints()) {
throw std::runtime_error("meshes should be in correspondence when computing specificity");
}
PointListType points1;
PointListType points2;
for (unsigned i = 0; i < mesh1->GetNumberOfPoints(); i++) {
MeshType::PointType pt1;
mesh1->GetPoint(i, &pt1);
points1.push_back(pt1);
MeshType::PointType pt2;
mesh2->GetPoint(i, &pt2);
points2.push_back(pt2);
}
vnlMatrixType X = createMatrixFromPoints(points1);
vnlMatrixType Y = createMatrixFromPoints(points2);
vnlVectorType mu_x = calcMean(X);
vnlVectorType mu_y = calcMean(Y);
ElementType sigma2_x = calcVar(X, mu_x);
ElementType sigma2_y = calcVar(Y, mu_y);
vnlMatrixType Sigma_xy = calcCov(X, Y, mu_x, mu_y);
vnl_svd<ElementType> SVD(Sigma_xy);
unsigned m = X.rows();
vnlMatrixType S(m,m);
S.set_identity();
ElementType detU = vnl_qr<ElementType>(SVD.U()).determinant();
ElementType detV = vnl_qr<ElementType>(SVD.V()).determinant();
if ( detU * detV == -1) {
S[m-1][m-1] = -1;
}
// the procedure actually computes the optimal rotation, translation and scale. We only
// use the scaling factor
vnlMatrixType R = SVD.U() * S * SVD.V().transpose();
ElementType c = 1/sigma2_x * vnl_trace(SVD.W()*S);
vnlVectorType t = mu_y - c * R * mu_x;
ElementType epsilon2 = sigma2_y - pow(vnl_trace(SVD.W()*S), 2) / sigma2_x;
return c;
}
double TpsRegistration::BendingEnergy() {
return vnl_trace(param_tps_.transpose() * kernel_ * param_tps_);
}
bool arlCore::ICP::solve( void )
{
const double Tau = 10e-8;
const bool Verbose = false;
m_startError = -1.0;
m_nbIterations = 0;
#ifdef ANN
if(!m_point2PointMode) return false;
m_endError = FLT_MAX/2.0;
double previousRMS = FLT_MAX;
if(!m_initialization)
if(!initialization())
return false;
// http://csdl2.computer.org/persagen/DLAbsToc.jsp?resourcePath=/dl/trans/tp/&toc=comp/trans/tp/1992/02/i2toc.xml&DOI=10.1109/34.121791
// http://ieeexplore.ieee.org/iel1/34/3469/00121791.pdf?tp=&arnumber=121791&isnumber=3469
unsigned int i, j, k;
vnl_quaternion<double> qr(0,0,0,1);
vnl_matrix_fixed<double,3,3> Id3, Ricp, Spy;
Id3.set_identity();
vnl_matrix_fixed<double,4,4> QSpy;
vnl_vector_fixed<double,3> Delta;
while( fabs(m_endError-previousRMS)>Tau && m_nbIterations<m_maxIterations ) //&& RMS>RMSMax )
{
++m_nbIterations;
std::cout<<m_nbIterations<<" ";
previousRMS = m_endError;
computeError(); // step 1
if(m_startError<0) m_startError = m_endError;
// step 2
// Calculate the cross-variance matrix between m_Pk and Yo
// Cov( P0, m_Yk ) = 1/n*somme(P0*m_Yk') - ComP0*Comm_Yk'
Spy.set_identity();
for( i=0 ; i<m_cloudSize ; ++i )
for( j=0 ; j<3 ; ++j )
for( k=0 ; k<3 ; ++k )
Spy[j][k]+= m_Pi[i][j]*m_Yk[i][k];
for( i=0 ; i<3 ; ++i )
for( j=0 ; j<3 ; ++j )
Spy[i][j] = Spy[i][j]/(double)m_cloudSize - m_cloudGravity[i]*m_modelGravity[j];
// Delta = [A23 A31 A12] with Amn = Spy[m][n]-Spy[n][m]
Delta[0] = Spy[1][2]-Spy[2][1];
Delta[1] = Spy[2][0]-Spy[0][2];
Delta[2] = Spy[0][1]-Spy[1][0];
// calculate the symmetric 4x4 matrix needed to determine
// the max eigenvalue
QSpy[0][0] = vnl_trace( Spy );
for( i=1 ; i<4 ; ++i )
{
QSpy[i][0] = Delta[i-1];
for( j = 1; j < 4; ++j )
{
QSpy[0][j] = Delta[j-1];
QSpy[i][j] = Spy[i-1][j-1]+Spy[j-1][i-1] - QSpy[0][0]*Id3[i-1][j-1];
}
}
vnl_symmetric_eigensystem<double> eigQSpy(QSpy);
// Optimal rotation matrix calculate from the quaternion
// vector qr=[q0 q1 q2 q3] obtained by obtained the max eigen value
// http://paine.wiau.man.ac.uk/pub/doc_vxl/core/vnl/html/vnl__symmetric__eigensystem_8h.html
qr.update(eigQSpy.get_eigenvector(3));
qr = vnl_quaternion<double>(qr[1], qr[2], qr[3], qr[0]);
Ricp = vnl_transpose(qr.rotation_matrix_transpose()).asMatrix();
// Optimal translation vector Ticp T = ComY - Ricp.ComP
vnl_vector<double> TraTemp = m_modelGravity - Ricp*m_cloudGravity;
// step 3 : Application of the transformation
for( i=0 ; i<m_cloudSize ; ++i )
for( j=0 ; j<3 ; ++j )
m_Pk[i][j] = Ricp[j][0]*m_Pi[i][0] + Ricp[j][1]*m_Pi[i][1] + Ricp[j][2]*m_Pi[i][2] + TraTemp[j];
}
// Give the transformation from model ==> cloud
// First we add the first transformation to Rend
vnl_matrix_fixed<double,3,3> Rend = Ricp.transpose();
vnl_vector_fixed<double,3> Tend;
for( i=0 ; i<3 ; ++i )
Tend[i] = m_cloudGravity[i]-(Rend[i][0]*m_modelGravity[0]+Rend[i][1]*m_modelGravity[1]+Rend[i][2]*m_modelGravity[2]);
// Initial matrix [m_rotInit ; m_traInit ; 0 0 0 1]
// Found matrix [Rend ; Tend ; 0 0 0 1]
m_solution.vnl_matrix_fixed<double,4,4>::update(vnl_inverse(m_rotInit)*Rend);
m_solution.set_column(3,vnl_inverse(m_rotInit)*(Tend-m_traInit));
m_solution[3][3] = 1.0;
m_solution.setRMS( m_endError );
if(Verbose)
{
std::cout<<"ICP registration\n";
std::cout<<"First RMS="<<m_startError<<"\nLast RMS="<<m_endError<<"\nIterations="<<m_nbIterations<<"\nPtsModel="<<m_modelSize<<"\nPtsCloud="<<m_cloudSize<<"\n";
std::cout<<"Tau ="<<fabs(m_endError-previousRMS)<<"\n";
std::cout<<"Matrix ="<<m_solution<<"\n";
}
return true;
#else // ANN
m_endError = -1.0;
return false;
#endif // ANN
}
