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.

263 lines
7.1 KiB
C++

/*
* 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;
}