3dpcp/.svn/pristine/df/dfdc1a7176bf01e0aaf156bac66d7c2477456407.svn-base

157 lines
5.1 KiB
Text
Raw Normal View History

2012-09-16 12:33:11 +00:00
/*
* icp6Dquatscale implementation
*
* Copyright (C) Flavia Grosan, Alex Tandrau, Dorit Borrmann, Andreas Nuechter
*
* Released under the GPL version 3.
*
*/
/** @file
* @brief Implementation of the ICP error function minimization via quaternions and scale factor
* @author Flavia Grosan, Alex Tandrau. Jacobs University Bremen gGmbH, Germany.
* @author Andreas Nuechter. Jacobs University Bremen gGmbH, Germany.
* @author Dorit borrmann. Jacobs University Bremen gGmbH, Germany.
*/
#include "slam6d/icp6Dquatscale.h"
#include "slam6d/globals.icc"
#include <iomanip>
using std::ios;
using std::resetiosflags;
using std::setiosflags;
#include <cfloat>
#include <cmath>
#include <iostream>
using std::cout;
using std::cerr;
using std::endl;
/**
* computes the rotation matrix consisting
* of a rotation and translation that
* minimizes the root-mean-square error of the
* point pairs using the Quaternion method of Horn
* 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_QUAT_SCALE::Point_Point_Align(const vector<PtPair>& pairs, double *alignfx,
const double centroid_m[3], const double centroid_d[3])
{
int n = pairs.size();
double sum = 0.0;
double sums[2]; sums[0] = sums[1] = 0.0;
// the quaternion
double q[7];
double S[3][3]; // Cross Covariance Matrix
double Q[4][4];
int i,j;
// calculate the cross covariance matrix
for (i = 0; i < 3; i++)
for (j = 0; j < 3; j++)
S[i][j] = 0;
for (i=0; i<n; i++) {
// cout << setprecision (10) << pairs[i].p1.x << " " << pairs[i].p1.y << " " << pairs[i].p1.z << endl;
// cout << pairs[i].p2.x << " " << pairs[i].p2.y << " " << pairs[i].p2.z << endl;
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);
sums[0] += sqr(pairs[i].p1.x - centroid_m[0]) +
sqr(pairs[i].p1.y - centroid_m[1]) +
sqr(pairs[i].p1.z - centroid_m[2]);
sums[1] += sqr(pairs[i].p2.x - centroid_d[0]) +
sqr(pairs[i].p2.y - centroid_d[1]) +
sqr(pairs[i].p2.z - centroid_d[2]);
S[0][0] += pairs[i].p2.x * pairs[i].p1.x;
S[0][1] += pairs[i].p2.x * pairs[i].p1.y;
S[0][2] += pairs[i].p2.x * pairs[i].p1.z;
S[1][0] += pairs[i].p2.y * pairs[i].p1.x;
S[1][1] += pairs[i].p2.y * pairs[i].p1.y;
S[1][2] += pairs[i].p2.y * pairs[i].p1.z;
S[2][0] += pairs[i].p2.z * pairs[i].p1.x;
S[2][1] += pairs[i].p2.z * pairs[i].p1.y;
S[2][2] += pairs[i].p2.z * pairs[i].p1.z;
}
cout << "Centroid_d " << centroid_d[0] << " " << centroid_d[1] << " " << centroid_d[2] << endl;
cout << "Centroid_m " << centroid_m[0] << " " << centroid_m[1] << " " << centroid_m[2] << endl;
double error = sqrt(sum / n);
if (!quiet) {
cout.setf(ios::basefield);
cout << "QUAT SCALE 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;
}
double fact = 1 / double(n);
for (i = 0; i < 3; i++)
for (j = 0; j < 3; j++)
S[i][j] *= fact;
S[0][0] -= centroid_d[0] * centroid_m[0];
S[0][1] -= centroid_d[0] * centroid_m[1];
S[0][2] -= centroid_d[0] * centroid_m[2];
S[1][0] -= centroid_d[1] * centroid_m[0];
S[1][1] -= centroid_d[1] * centroid_m[1];
S[1][2] -= centroid_d[1] * centroid_m[2];
S[2][0] -= centroid_d[2] * centroid_m[0];
S[2][1] -= centroid_d[2] * centroid_m[1];
S[2][2] -= centroid_d[2] * centroid_m[2];
// calculate the 4x4 symmetric matrix Q
double trace = S[0][0] + S[1][1] + S[2][2];
double A23 = S[1][2] - S[2][1];
double A31 = S[2][0] - S[0][2];
double A12 = S[0][1] - S[1][0];
Q[0][0] = trace;
Q[0][1] = Q[1][0] = A23;
Q[0][2] = Q[2][0] = A31;
Q[0][3] = Q[3][0] = A12;
for (i = 0; i < 3; i++)
for (j = 0; j < 3; j++)
Q[i+1][j+1] = S[i][j] + S[j][i] - ( i==j ? trace : 0);
maxEigenVector(Q, q);
// calculate the rotation matrix
double m[3][3]; // rot matrix
quaternion2matrix(q, m);
M4identity(alignfx);
double scale_s = sqrt(sums[0] / sums[1]);
alignfx[0] = m[0][0] * scale_s;
alignfx[1] = m[1][0] * scale_s;
alignfx[2] = m[2][0] * scale_s;
alignfx[3] = 0.0;
alignfx[4] = m[0][1] * scale_s;
alignfx[5] = m[1][1] * scale_s;
alignfx[6] = m[2][1] * scale_s;
alignfx[7] = 0.0;
alignfx[8] = m[0][2] * scale_s;
alignfx[9] = m[1][2] * scale_s;
alignfx[10] = m[2][2] * scale_s;
alignfx[11] = 0.0;
// calculate the translation vector,
alignfx[12] = centroid_m[0] - m[0][0]*centroid_d[0]*scale_s - m[0][1]*scale_s*centroid_d[1] - m[0][2]*centroid_d[2]*scale_s;
alignfx[13] = centroid_m[1] - m[1][0]*centroid_d[0]*scale_s - m[1][1]*scale_s*centroid_d[1] - m[1][2]*centroid_d[2]*scale_s;
alignfx[14] = centroid_m[2] - m[2][0]*centroid_d[0]*scale_s - m[2][1]*scale_s*centroid_d[1] - m[2][2]*centroid_d[2]*scale_s;
return error;
}