159 lines
4.3 KiB
Text
159 lines
4.3 KiB
Text
/*
|
|
* icp6Dortho implementation
|
|
*
|
|
* Copyright (C) Andreas Nuechter, Alexandru-Eugen Ichim
|
|
*
|
|
* Released under the GPL version 3.
|
|
*
|
|
*/
|
|
|
|
/** @file
|
|
* @brief Implementation of the ICP error function minimization via
|
|
* orthonormal matrices
|
|
*
|
|
* @author Andreas Nuechter. Jacobs University Bremen gGmbH, Germany
|
|
* @author Alexandru-Eugen Ichim. Jacobs University Bremen gGmbH, Germany
|
|
*
|
|
*/
|
|
|
|
#include "slam6d/icp6Dortho.h"
|
|
|
|
#include "slam6d/globals.icc"
|
|
#include <iomanip>
|
|
using std::ios;
|
|
using std::resetiosflags;
|
|
using std::setiosflags;
|
|
|
|
#include "newmat/newmat.h"
|
|
#include "newmat/newmatap.h"
|
|
using namespace NEWMAT;
|
|
|
|
/**
|
|
* computes the rotation matrix consisting
|
|
* of a rotation and translation that
|
|
* minimizes the root-mean-square error of the
|
|
* point pairs using orthonormal matrices
|
|
* vector of point pairs, rotation matrix
|
|
* @param pairs Vector of point pairs (pairs of corresponding points)
|
|
* @param *alignfx The resulting transformation matrix
|
|
* @return Error estimation of the matching (rms)
|
|
*/
|
|
double icp6D_ORTHO::Point_Point_Align(const vector<PtPair>& pairs, double *alignfx,
|
|
const double centroid_m[3], const double centroid_d[3])
|
|
{
|
|
double error = 0;
|
|
double sum = 0.0;
|
|
|
|
/// Get centered PtPairs
|
|
double** m = new double*[pairs.size()];
|
|
double** d = new double*[pairs.size()];
|
|
|
|
for(unsigned int i = 0; i < pairs.size(); i++){
|
|
m[i] = new double[3];
|
|
d[i] = new double[3];
|
|
m[i][0] = pairs[i].p1.x - centroid_m[0];
|
|
m[i][1] = pairs[i].p1.y - centroid_m[1];
|
|
m[i][2] = pairs[i].p1.z - centroid_m[2];
|
|
d[i][0] = pairs[i].p2.x - centroid_d[0];
|
|
d[i][1] = pairs[i].p2.y - centroid_d[1];
|
|
d[i][2] = pairs[i].p2.z - centroid_d[2];
|
|
|
|
sum += sqr(pairs[i].p1.x - pairs[i].p2.x)
|
|
+ sqr(pairs[i].p1.y - pairs[i].p2.y)
|
|
+ sqr(pairs[i].p1.z - pairs[i].p2.z) ;
|
|
}
|
|
|
|
error = sqrt(sum / (double)pairs.size());
|
|
|
|
if (!quiet) {
|
|
cout.setf(ios::basefield);
|
|
cout << "ORTHO RMS point-to-point error = "
|
|
<< resetiosflags(ios::adjustfield) << setiosflags(ios::internal)
|
|
<< resetiosflags(ios::floatfield) << setiosflags(ios::fixed)
|
|
<< std::setw(10) << std::setprecision(7)
|
|
<< error
|
|
<< " using " << std::setw(6) << (int)pairs.size() << " points" << endl;
|
|
}
|
|
|
|
|
|
|
|
|
|
/////////////////////////////////////////////////////////
|
|
/////////////////////////////////////////////////////////
|
|
|
|
/// generate matrix H
|
|
Matrix H (3, 3); H = 0.0;
|
|
for(unsigned int n = 0; n < pairs.size(); ++n)
|
|
for(int i = 0; i < 3; ++i)
|
|
for(int j = 0; j < 3; ++j)
|
|
H(i+1, j+1) += m[n][i] * d[n][j];
|
|
Matrix HH = H.t() * H;
|
|
|
|
/// create a new matrix HHs equal to HH, but is of type SymmetricMatrix - needed for calculating the eigenvalues
|
|
SymmetricMatrix HHs(3);
|
|
HHs(1, 2) = HH(2, 1);
|
|
HHs(1, 3) = HH(3, 1);
|
|
HHs(2, 3) = HH(3, 2);
|
|
HHs(1, 1) = HH(1, 1);
|
|
HHs(2, 2) = HH(2, 2);
|
|
HHs(3, 3) = HH(3, 3);
|
|
|
|
/// get the eigenvalues of HH
|
|
DiagonalMatrix eigenvalues(3);
|
|
Matrix eigenvectorsMatrix(3, 3);
|
|
EigenValues(HHs, eigenvalues, eigenvectorsMatrix);
|
|
|
|
/// extract the eigenvectors from the matrix to separate column vectors
|
|
ColumnVector ev1 = eigenvectorsMatrix.Column(1);
|
|
ColumnVector ev2 = eigenvectorsMatrix.Column(2);
|
|
ColumnVector ev3 = eigenvectorsMatrix.Column(3);
|
|
|
|
|
|
/// compute the rotation matrix as H * ( SUM ( 1/lambda_i * ev_i * ev_i^T ))
|
|
Matrix R(3, 3);
|
|
Matrix ev11 = ev1 * ev1.t() * 1/sqrt( eigenvalues(1));
|
|
Matrix ev22 = ev2 * ev2.t() * 1/sqrt( eigenvalues(2));
|
|
Matrix ev33 = ev3 * ev3.t() * 1/sqrt( eigenvalues(3));
|
|
R = H * (ev11 + ev22 + ev33);
|
|
|
|
/// calculate the translation based on the rotation matrix and the centroids
|
|
ColumnVector cm(3); cm << centroid_m;
|
|
ColumnVector cd(3); cd << centroid_d;
|
|
ColumnVector translation = cm - R * cd;
|
|
|
|
|
|
/// create the final transformation matrix (OpenGL column-wise ordering)
|
|
alignfx[0] = R(1, 1);
|
|
alignfx[1] = R(2, 1);
|
|
alignfx[2] = R(3, 1);
|
|
alignfx[3] = 0.0;
|
|
alignfx[4] = R(1, 2);
|
|
alignfx[5] = R(2, 2);
|
|
alignfx[6] = R(3, 2);
|
|
alignfx[7] = 0.0;
|
|
alignfx[8] = R(1, 3);
|
|
alignfx[9] = R(2, 3);
|
|
alignfx[10]= R(3, 3);
|
|
alignfx[11]= 0.0;
|
|
alignfx[12]= translation(1);
|
|
alignfx[13]= translation(2);
|
|
alignfx[14]= translation(3);
|
|
alignfx[15]= 1.0;
|
|
|
|
|
|
/////////////////////////////////////////////////////////
|
|
/////////////////////////////////////////////////////////
|
|
|
|
|
|
/// clean up memory used by centered point pairs
|
|
for(unsigned int i = 0; i < pairs.size(); i++){
|
|
delete [] m[i];
|
|
delete [] d[i];
|
|
}
|
|
delete [] m;
|
|
delete [] d;
|
|
|
|
return error;
|
|
}
|
|
|
|
|