262 lines
7.1 KiB
Text
262 lines
7.1 KiB
Text
/*
|
|
* icp6Dsvd implementation
|
|
*
|
|
* Copyright (C) Kai Lingemann, Andreas Nuechter
|
|
*
|
|
* Released under the GPL version 3.
|
|
*
|
|
*/
|
|
|
|
/** @file
|
|
* @brief Implementation of the ICP error function minimization via SVD
|
|
* @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany.
|
|
* @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany.
|
|
*/
|
|
|
|
#include "slam6d/icp6Dsvd.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 the SVD PARAMETERS
|
|
* 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_SVD::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 << "SVD 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;
|
|
}
|
|
|
|
// Fill H matrix
|
|
Matrix H(3,3), R(3,3);
|
|
for(int j = 0; j < 3; j++){
|
|
for(int k = 0; k < 3; k++){
|
|
H(j+1, k+1) = 0.0;
|
|
}
|
|
}
|
|
|
|
for(unsigned int i = 0; i < pairs.size(); i++){
|
|
for(int j = 0; j < 3; j++){
|
|
for(int k = 0; k < 3; k++){
|
|
H(j+1, k+1) += d[i][j]*m[i][k];
|
|
}
|
|
}
|
|
}
|
|
|
|
Matrix U(3,3);
|
|
DiagonalMatrix Lamda(3);
|
|
Matrix V(3,3);
|
|
// Make SVD
|
|
SVD(H, Lamda, U, V);
|
|
|
|
// Get rotation
|
|
R = V*(U.t());
|
|
|
|
// Calculate translation
|
|
double translation[3];
|
|
ColumnVector col_vec(3);
|
|
for(int j = 0; j < 3; j++)
|
|
col_vec(j+1) = centroid_d[j];
|
|
ColumnVector r_time_colVec = ColumnVector(R*col_vec);
|
|
translation[0] = centroid_m[0] - r_time_colVec(1);
|
|
translation[1] = centroid_m[1] - r_time_colVec(2);
|
|
translation[2] = centroid_m[2] - r_time_colVec(3);
|
|
|
|
|
|
// Fill result
|
|
alignfx[0] = R(1,1);
|
|
alignfx[1] = R(2,1);
|
|
alignfx[2] = 0;
|
|
alignfx[2] = R(3,1);
|
|
alignfx[3] = 0;
|
|
alignfx[4] = R(1,2);
|
|
alignfx[5] = R(2,2);
|
|
alignfx[6] = R(3,2);
|
|
alignfx[7] = 0;
|
|
alignfx[8] = R(1,3);
|
|
alignfx[9] = R(2,3);
|
|
alignfx[10] = R(3,3);
|
|
alignfx[11] = 0;
|
|
alignfx[12] = translation[0];
|
|
alignfx[13] = translation[1];
|
|
alignfx[14] = translation[2];
|
|
|
|
alignfx[15] = 1;
|
|
|
|
|
|
for(unsigned int i = 0; i < pairs.size(); i++){
|
|
delete [] m[i];
|
|
delete [] d[i];
|
|
}
|
|
delete [] m;
|
|
delete [] d;
|
|
|
|
return error;
|
|
}
|
|
|
|
/**
|
|
* computes the rotation matrix consisting
|
|
* of a rotation and translation that
|
|
* minimizes the root-mean-square error of the
|
|
* point pairs using the SVD PARAMETERS
|
|
* 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_SVD::Point_Point_Align_Parallel(const int openmp_num_threads,
|
|
const unsigned int n[OPENMP_NUM_THREADS],
|
|
const double sum[OPENMP_NUM_THREADS],
|
|
const double centroid_m[OPENMP_NUM_THREADS][3],
|
|
const double centroid_d[OPENMP_NUM_THREADS][3],
|
|
const double Si[OPENMP_NUM_THREADS][9], double *alignxf)
|
|
{
|
|
double s = 0.0;
|
|
double ret;
|
|
unsigned int pairs_size = 0;
|
|
double cm[3] = {0.0, 0.0, 0.0}; // centroid m
|
|
double cd[3] = {0.0, 0.0, 0.0}; // centroid d
|
|
|
|
// Implementation according to the paper
|
|
// "The Parallel Iterative Closest Point Algorithm"
|
|
// by Langis / Greenspan / Godin, IEEE 3DIM 2001
|
|
// formula (4)
|
|
//
|
|
// The same information are given in (ecrm2007.pdf)
|
|
// Andreas Nüchter. Parallelization of Scan Matching
|
|
// for Robotic 3D Mapping. In Proceedings of the 3rd
|
|
// European Conference on Mobile Robots (ECMR '07),
|
|
// Freiburg, Germany, September 2007
|
|
for (int i = 0; i < openmp_num_threads; i++) {
|
|
s += sum[i];
|
|
pairs_size += n[i];
|
|
cm[0] += n[i] * centroid_m[i][0];
|
|
cm[1] += n[i] * centroid_m[i][1];
|
|
cm[2] += n[i] * centroid_m[i][2];
|
|
cd[0] += n[i] * centroid_d[i][0];
|
|
cd[1] += n[i] * centroid_d[i][1];
|
|
cd[2] += n[i] * centroid_d[i][2];
|
|
}
|
|
cm[0] /= pairs_size;
|
|
cm[1] /= pairs_size;
|
|
cm[2] /= pairs_size;
|
|
cd[0] /= pairs_size;
|
|
cd[1] /= pairs_size;
|
|
cd[2] /= pairs_size;
|
|
|
|
ret = sqrt(s / (double)pairs_size);
|
|
if (!quiet) {
|
|
cout.setf(ios::basefield);
|
|
cout << "PSVD RMS point-to-point error = "
|
|
<< resetiosflags(ios::adjustfield) << setiosflags(ios::internal)
|
|
<< resetiosflags(ios::floatfield) << setiosflags(ios::fixed)
|
|
<< std::setw(10) << std::setprecision(7)
|
|
<< ret
|
|
<< " using " << std::setw(6) << pairs_size << " points" << endl;
|
|
}
|
|
|
|
Matrix H(3,3), R(3,3);
|
|
for(int j = 0; j < 3; j++){
|
|
for(int k = 0; k < 3; k++){
|
|
H(j+1, k+1) = 0.0;
|
|
}
|
|
}
|
|
// formula (5)
|
|
for (int i = 0; i < openmp_num_threads; i++) {
|
|
for(int j = 0; j < 3; j++){
|
|
for(int k = 0; k < 3; k++){
|
|
// H(j+1, k+1) += Si[i][j*3+k] + n[i] * ((centroid_m[i][j] - cm[j]) * (centroid_d[i][k] - cd[k])) ;
|
|
H(j+1, k+1) += Si[i][k*3+j] + n[i] * ((centroid_d[i][j] - cd[j]) * (centroid_m[i][k] - cm[k])) ;
|
|
}
|
|
}
|
|
}
|
|
|
|
Matrix U(3,3);
|
|
DiagonalMatrix Lamda(3);
|
|
Matrix V(3,3);
|
|
// Make SVD
|
|
SVD(H, Lamda, U, V);
|
|
|
|
// Get rotation
|
|
R = V*(U.t());
|
|
|
|
// Calculate translation
|
|
double translation[3];
|
|
ColumnVector col_vec(3);
|
|
for(int j = 0; j < 3; j++) {
|
|
col_vec(j+1) = cd[j];
|
|
}
|
|
ColumnVector r_time_colVec = ColumnVector(R * col_vec);
|
|
translation[0] = cm[0] - r_time_colVec(1);
|
|
translation[1] = cm[1] - r_time_colVec(2);
|
|
translation[2] = cm[2] - r_time_colVec(3);
|
|
|
|
// Fill result
|
|
alignxf[0] = R(1,1);
|
|
alignxf[1] = R(2,1);
|
|
alignxf[2] = 0;
|
|
alignxf[2] = R(3,1);
|
|
alignxf[3] = 0;
|
|
alignxf[4] = R(1,2);
|
|
alignxf[5] = R(2,2);
|
|
alignxf[6] = R(3,2);
|
|
alignxf[7] = 0;
|
|
alignxf[8] = R(1,3);
|
|
alignxf[9] = R(2,3);
|
|
alignxf[10] = R(3,3);
|
|
alignxf[11] = 0;
|
|
alignxf[12] = translation[0];
|
|
alignxf[13] = translation[1];
|
|
alignxf[14] = translation[2];
|
|
|
|
alignxf[15] = 1;
|
|
|
|
return ret;
|
|
|
|
}
|