You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
146 lines
4.2 KiB
Plaintext
146 lines
4.2 KiB
Plaintext
/*
|
|
* icp6Ddual implementation
|
|
*
|
|
* Copyright (C) Andreas Nuechter, Alexandru-Eugen Ichim
|
|
*
|
|
* Released under the GPL version 3.
|
|
*
|
|
*/
|
|
|
|
/** @file
|
|
* @brief Implementation of the ICP error function minimization
|
|
* via dual quaternions
|
|
*
|
|
* @author Andreas Nuechter. Jacobs University Bremen gGmbH, Germany
|
|
* @author Alexandru-Eugen Ichim. Jacobs University Bremen gGmbH, Germany
|
|
*
|
|
*/
|
|
|
|
#include "slam6d/icp6Ddual.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 dual quaternions
|
|
* @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_DUAL::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;
|
|
|
|
for(unsigned int i = 0; i < pairs.size(); i++){
|
|
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 << "DUALQUAT 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;
|
|
}
|
|
|
|
|
|
|
|
/////////////////////////////////////////////////////////
|
|
/////////////////////////////////////////////////////////
|
|
|
|
Matrix C1(4, 4); C1 = 0.0;
|
|
Matrix C2(4, 4); C2 = 0.0;
|
|
|
|
/// build up matrices C1 and C2
|
|
for(unsigned int i = 0; i < pairs.size(); ++i) {
|
|
ColumnVector m(3), d(3);
|
|
m << pairs[i].p1.x << pairs[i].p1.y << pairs[i].p1.z;
|
|
d << pairs[i].p2.x << pairs[i].p2.y << pairs[i].p2.z;
|
|
Matrix Cm(3, 3); Cm << 0 << -m(3) << m(2) << m(3) << 0 << -m(1) << -m(2) << m(1) << 0;
|
|
Matrix Cd(3, 3); Cd << 0 << -d(3) << d(2) << d(3) << 0 << -d(1) << -d(2) << d(1) << 0;
|
|
|
|
C1.SubMatrix(1, 1, 1, 1) += m.t()*d;
|
|
C1.SubMatrix(1, 1, 2, 4) += -m.t()*Cd;
|
|
C1.SubMatrix(2, 4, 1, 1) += -Cm*d;
|
|
C1.SubMatrix(2, 4, 2, 4) += m*d.t() + Cm*Cd;
|
|
|
|
C2.SubMatrix(1, 1, 2, 4) += -d.t() + m.t();
|
|
C2.SubMatrix(2, 4, 1, 1) += d - m;
|
|
C2.SubMatrix(2, 4, 2, 4) += -Cd - Cm;
|
|
}
|
|
|
|
/// the sums need to be multiplied by a scalar
|
|
C1 = C1 * (-2);
|
|
C2 = C2 * 2;
|
|
|
|
/// matrix A from C1 and C2
|
|
Matrix A = (C2.t()*C2 * 1.0/(2*pairs.size()) - C1 - C1.t()) * 0.5;
|
|
|
|
/// the quaternion qdot is the eigenvector of matrix A corresponding to the largest eigenvalue,
|
|
/// which is always found in the first column of matrix U
|
|
Matrix U(4, 4); DiagonalMatrix D(4);
|
|
SVD(A, D, U);
|
|
ColumnVector qdot = U.Column(1);
|
|
ColumnVector q(3); q << qdot(2) << qdot(3) << qdot(4);
|
|
Matrix Cq(3, 3); Cq << 0 << -q(3) << q(2) << q(3) << 0 << -q(1) << -q(2) << q(1) << 0;
|
|
|
|
ColumnVector s = C2*qdot * (-1.0)/(2*pairs.size());
|
|
|
|
Matrix Q(4, 4);
|
|
Q(1, 1) = qdot(1);
|
|
Q.SubMatrix(1, 1, 2, 4) = q.t();
|
|
Q.SubMatrix(2, 4, 1, 1) = -q;
|
|
Q.SubMatrix(2, 4, 2, 4) = IdentityMatrix(3)*qdot(1) + Cq;
|
|
|
|
/// get the translation from the double quaternion
|
|
ColumnVector p = Q*s;
|
|
ColumnVector translation(3); translation << p(2) << p(3) << p(4);
|
|
|
|
/// compute the rotation matrix from the quaternion
|
|
ColumnVector qtqx = q.t()*q;
|
|
Matrix R = IdentityMatrix(3) * (qdot(1)*qdot(1) - qtqx(1)) + q*q.t()*2 + Cq*qdot(1)*2;
|
|
|
|
/// 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;
|
|
|
|
/////////////////////////////////////////////////////////
|
|
/////////////////////////////////////////////////////////
|
|
|
|
return error;
|
|
}
|
|
|