310 lines
9 KiB
Text
310 lines
9 KiB
Text
|
/*
|
||
|
|
* icp6Dapx implementation
|
||
|
|
*
|
||
|
|
* Copyright (C) Andreas Nuechter
|
||
|
|
*
|
||
|
|
* Released under the GPL version 3.
|
||
|
|
*
|
||
|
|
*/
|
||
|
|
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @file
|
||
|
|
* @brief Implementation of the ICP error function minimization via approximation
|
||
|
|
* @author Andreas Nuechter. Jacobs University Bremen gGmbH, Germany
|
||
|
|
*/
|
||
|
|
|
||
|
|
#include "slam6d/icp6Dapx.h"
|
||
|
|
|
||
|
|
#include "slam6d/globals.icc"
|
||
|
|
#include <iomanip>
|
||
|
|
#include <cstring>
|
||
|
|
using std::ios;
|
||
|
|
using std::resetiosflags;
|
||
|
|
using std::setiosflags;
|
||
|
|
|
||
|
|
/**
|
||
|
|
* computes the rotation matrix consisting
|
||
|
|
* of a rotation and translation that
|
||
|
|
* minimizes the root-mean-square error
|
||
|
|
* of the point pairs, using the <b>approximation</b>
|
||
|
|
* sin(x) = x.
|
||
|
|
*
|
||
|
|
* @param Pairs Vector of point pairs (pairs of corresponding points)
|
||
|
|
* @param alignxf The resulting transformation matrix
|
||
|
|
* @return Error estimation of the matching (rms)
|
||
|
|
*/
|
||
|
|
double icp6D_APX::Point_Point_Align(const vector<PtPair>& Pairs, double *alignxf,
|
||
|
|
const double centroid_m[3], const double centroid_d[3])
|
||
|
|
{
|
||
|
|
int n = Pairs.size();
|
||
|
|
|
||
|
|
// ?!? <= 3
|
||
|
|
if (n <= 3) {
|
||
|
|
M4identity(alignxf);
|
||
|
|
return 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
int i;
|
||
|
|
|
||
|
|
double A[3][3];
|
||
|
|
double B[3];
|
||
|
|
memset(&A[0][0], 0, 9 * sizeof(double));
|
||
|
|
memset(&B[0], 0, 3 * sizeof(double));
|
||
|
|
|
||
|
|
double sum = 0;
|
||
|
|
double p1[3], p2[3];
|
||
|
|
|
||
|
|
for (i = 0; i < n; i++) {
|
||
|
|
p1[0] = Pairs[i].p1.x;
|
||
|
|
p1[1] = Pairs[i].p1.y;
|
||
|
|
p1[2] = Pairs[i].p1.z;
|
||
|
|
p2[0] = Pairs[i].p2.x;
|
||
|
|
p2[1] = Pairs[i].p2.y;
|
||
|
|
p2[2] = Pairs[i].p2.z;
|
||
|
|
|
||
|
|
double p12[3] = { p1[0] - p2[0], p1[1] - p2[1], p1[2] - p2[2] };
|
||
|
|
double p2c[3] = { p2[0] - centroid_d[0], p2[1] - centroid_d[1], p2[2] - centroid_d[2] };
|
||
|
|
|
||
|
|
sum += Len2(p12);
|
||
|
|
|
||
|
|
B[0] += (p12[2]*p2c[1] - p12[1]*p2c[2]);
|
||
|
|
B[1] += (p12[0]*p2c[2] - p12[2]*p2c[0]);
|
||
|
|
B[2] += (p12[1]*p2c[0] - p12[0]*p2c[1]);
|
||
|
|
A[0][0] += (sqr(p2c[1]) + sqr(p2c[2]));
|
||
|
|
A[0][1] -= p2c[0] * p2c[1];
|
||
|
|
A[0][2] -= p2c[0] * p2c[2];
|
||
|
|
A[1][1] += (sqr(p2c[0]) + sqr(p2c[2]));
|
||
|
|
A[1][2] -= p2c[1] * p2c[2];
|
||
|
|
A[2][2] += (sqr(p2c[0]) + sqr(p2c[1]));
|
||
|
|
}
|
||
|
|
|
||
|
|
double error = sqrt(sum / n);
|
||
|
|
if (!quiet) {
|
||
|
|
cout.setf(ios::basefield);
|
||
|
|
cout << "APX 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;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Solve eqns
|
||
|
|
double diag[3];
|
||
|
|
if (!choldc(A, diag)) {
|
||
|
|
printf("Couldn't find transform.\n");
|
||
|
|
return -1.0;
|
||
|
|
}
|
||
|
|
double x[3];
|
||
|
|
cholsl(A, diag, B, x);
|
||
|
|
|
||
|
|
// Interpret results
|
||
|
|
double sx = x[0];
|
||
|
|
double cx = sqrt(1.0 - sx*sx);
|
||
|
|
double sy = x[1];
|
||
|
|
double cy = sqrt(1.0 - sy*sy);
|
||
|
|
double sz = x[2];
|
||
|
|
double cz = sqrt(1.0 - sz*sz);
|
||
|
|
|
||
|
|
alignxf[0] = cy*cz;
|
||
|
|
alignxf[1] = sx*sy*cz + cx*sz;
|
||
|
|
alignxf[2] = -cx*sy*cz + sx*sz;
|
||
|
|
alignxf[3] = 0;
|
||
|
|
alignxf[4] = -cy*sz;
|
||
|
|
alignxf[5] = -sx*sy*sz + cx*cz;
|
||
|
|
alignxf[6] = cx*sy*sz + sx*cz;
|
||
|
|
alignxf[7] = 0;
|
||
|
|
alignxf[8] = sy;
|
||
|
|
alignxf[9] = -sx*cy;
|
||
|
|
alignxf[10] = cx*cy;
|
||
|
|
alignxf[11] = 0;
|
||
|
|
alignxf[12] = centroid_m[0] - alignxf[0]*centroid_d[0] - alignxf[4]*centroid_d[1] - alignxf[8]*centroid_d[2];
|
||
|
|
alignxf[13] = centroid_m[1] - alignxf[1]*centroid_d[0] - alignxf[5]*centroid_d[1] - alignxf[9]*centroid_d[2];
|
||
|
|
alignxf[14] = centroid_m[2] - alignxf[2]*centroid_d[0] - alignxf[6]*centroid_d[1] - alignxf[10]*centroid_d[2];
|
||
|
|
alignxf[15] = 1;
|
||
|
|
|
||
|
|
return error;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
double icp6D_APX::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 vector<PtPair> pairs[OPENMP_NUM_THREADS],
|
||
|
|
double *alignxf)
|
||
|
|
|
||
|
|
{
|
||
|
|
|
||
|
|
#ifdef _OPENMP
|
||
|
|
|
||
|
|
double At[OPENMP_NUM_THREADS][3][3];
|
||
|
|
double Bt[OPENMP_NUM_THREADS][3];
|
||
|
|
|
||
|
|
for (int j=0;j < OPENMP_NUM_THREADS; j++)
|
||
|
|
for (int k = 0;k < 3; k++) {
|
||
|
|
for (int l = 0; l < 3; l++)
|
||
|
|
At[j][k][l] = 0.0;
|
||
|
|
Bt[j][k] = 0.0;
|
||
|
|
}
|
||
|
|
double A[3][3];
|
||
|
|
double B[3];
|
||
|
|
memset(&A[0][0], 0, 9 * sizeof(double));
|
||
|
|
memset(&B[0], 0, 3 * sizeof(double));
|
||
|
|
|
||
|
|
double s = 0.0;
|
||
|
|
double error;
|
||
|
|
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
|
||
|
|
|
||
|
|
double cms[3], cds[3];
|
||
|
|
for (int i = 0; i < 3; i++) cms[i] = cds[i] = 0.0;
|
||
|
|
|
||
|
|
for (int i = 0; i < openmp_num_threads; i++) {
|
||
|
|
s += sum[i];
|
||
|
|
pairs_size += n[i];
|
||
|
|
|
||
|
|
// compute centroids for all the pairs
|
||
|
|
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];
|
||
|
|
|
||
|
|
cms[0] += centroid_m[i][0];
|
||
|
|
cms[1] += centroid_m[i][1];
|
||
|
|
cms[2] += centroid_m[i][2];
|
||
|
|
cds[0] += centroid_d[i][0];
|
||
|
|
cds[1] += centroid_d[i][1];
|
||
|
|
cds[2] += 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;
|
||
|
|
|
||
|
|
error = sqrt(s / (double)pairs_size);
|
||
|
|
|
||
|
|
#pragma omp parallel
|
||
|
|
{
|
||
|
|
int thread_num = omp_get_thread_num();
|
||
|
|
for (unsigned int i = 0 ; i < (unsigned int)pairs[thread_num].size() ; i++)
|
||
|
|
{
|
||
|
|
At[thread_num][0][0] += (pairs[thread_num][i].p2.y - cd[1])*(pairs[thread_num][i].p2.y - cd[1]) +
|
||
|
|
(pairs[thread_num][i].p2.z - cd[2])*(pairs[thread_num][i].p2.z - cd[2]);
|
||
|
|
At[thread_num][0][1] -= (pairs[thread_num][i].p2.x - cd[0])*(pairs[thread_num][i].p2.y - cd[1]);
|
||
|
|
At[thread_num][0][2] -= (pairs[thread_num][i].p2.x - cd[0])*(pairs[thread_num][i].p2.z - cd[2]);
|
||
|
|
At[thread_num][1][1] += (pairs[thread_num][i].p2.x - cd[0])*(pairs[thread_num][i].p2.x - cd[0]) +
|
||
|
|
(pairs[thread_num][i].p2.z - cd[2])*(pairs[thread_num][i].p2.z - cd[2]);
|
||
|
|
At[thread_num][1][2] -= (pairs[thread_num][i].p2.y - cd[1])*(pairs[thread_num][i].p2.z - cd[2]);
|
||
|
|
At[thread_num][2][2] += (pairs[thread_num][i].p2.x - cd[0])*(pairs[thread_num][i].p2.x - cd[0]) +
|
||
|
|
(pairs[thread_num][i].p2.y - cd[1])*(pairs[thread_num][i].p2.y - cd[1]);
|
||
|
|
|
||
|
|
Bt[thread_num][0] += (pairs[thread_num][i].p1.z - pairs[thread_num][i].p2.z)
|
||
|
|
* (pairs[thread_num][i].p2.y - cd[1]) - (pairs[thread_num][i].p1.y - pairs[thread_num][i].p2.y)
|
||
|
|
* (pairs[thread_num][i].p2.z - cd[2]);
|
||
|
|
Bt[thread_num][1] += (pairs[thread_num][i].p1.x - pairs[thread_num][i].p2.x)
|
||
|
|
* (pairs[thread_num][i].p2.z - cd[2]) - (pairs[thread_num][i].p1.z - pairs[thread_num][i].p2.z)
|
||
|
|
* (pairs[thread_num][i].p2.x - cd[0]);
|
||
|
|
Bt[thread_num][2] += (pairs[thread_num][i].p1.y - pairs[thread_num][i].p2.y) *
|
||
|
|
(pairs[thread_num][i].p2.x - cd[0]) - (pairs[thread_num][i].p1.x - pairs[thread_num][i].p2.x)
|
||
|
|
* (pairs[thread_num][i].p2.y - cd[1]);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
for (int j = 0;j < OPENMP_NUM_THREADS; j++)
|
||
|
|
for (int k = 0; k < 3; k++) {
|
||
|
|
for (int l = 0; l < 3; l++)
|
||
|
|
A[k][l] += At[j][k][l] ;
|
||
|
|
B[k] += Bt[j][k];
|
||
|
|
}
|
||
|
|
|
||
|
|
// continue with linear solution
|
||
|
|
|
||
|
|
if (!quiet) {
|
||
|
|
cout.setf(ios::basefield);
|
||
|
|
cout << "PAPX 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) << pairs_size << " points" << endl;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Solve eqns
|
||
|
|
double diag[3];
|
||
|
|
if (!choldc(A, diag)) {
|
||
|
|
printf("Couldn't find transform.\n");
|
||
|
|
return -1.0;
|
||
|
|
}
|
||
|
|
double x[3];
|
||
|
|
cholsl(A, diag, B, x);
|
||
|
|
|
||
|
|
// Interpret results
|
||
|
|
double sx = x[0];
|
||
|
|
double cx = sqrt(1.0 - sx*sx);
|
||
|
|
double sy = x[1];
|
||
|
|
double cy = sqrt(1.0 - sy*sy);
|
||
|
|
double sz = x[2];
|
||
|
|
double cz = sqrt(1.0 - sz*sz);
|
||
|
|
|
||
|
|
alignxf[0] = cy*cz;
|
||
|
|
alignxf[1] = sx*sy*cz + cx*sz;
|
||
|
|
alignxf[2] = -cx*sy*cz + sx*sz;
|
||
|
|
alignxf[3] = 0;
|
||
|
|
alignxf[4] = -cy*sz;
|
||
|
|
alignxf[5] = -sx*sy*sz + cx*cz;
|
||
|
|
alignxf[6] = cx*sy*sz + sx*cz;
|
||
|
|
alignxf[7] = 0;
|
||
|
|
alignxf[8] = sy;
|
||
|
|
alignxf[9] = -sx*cy;
|
||
|
|
alignxf[10] = cx*cy;
|
||
|
|
alignxf[11] = 0;
|
||
|
|
alignxf[12] = cm[0] - alignxf[0]*cd[0] - alignxf[4]*cd[1] - alignxf[8]*cd[2];
|
||
|
|
alignxf[13] = cm[1] - alignxf[1]*cd[0] - alignxf[5]*cd[1] - alignxf[9]*cd[2];
|
||
|
|
alignxf[14] = cm[2] - alignxf[2]*cd[0] - alignxf[6]*cd[1] - alignxf[10]*cd[2];
|
||
|
|
alignxf[15] = 1;
|
||
|
|
|
||
|
|
return error;
|
||
|
|
#else
|
||
|
|
cout << "Point_Point_Align_Parallel:"<< endl
|
||
|
|
<< "Please compile with OpenMP support to use this function" << endl;
|
||
|
|
exit(-1);
|
||
|
|
#endif
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
void icp6D_APX::computeRt(const double *x, const double *dx, double *alignxf)
|
||
|
|
{
|
||
|
|
double sx = x[0];
|
||
|
|
double cx = sqrt(1.0 - sx*sx);
|
||
|
|
double sy = x[1];
|
||
|
|
double cy = sqrt(1.0 - sy*sy);
|
||
|
|
double sz = x[2];
|
||
|
|
double cz = sqrt(1.0 - sz*sz);
|
||
|
|
|
||
|
|
alignxf[0] = cy*cz;
|
||
|
|
alignxf[1] = sx*sy*cz + cx*sz;
|
||
|
|
alignxf[2] = -cx*sy*cz + sx*sz;
|
||
|
|
alignxf[3] = 0;
|
||
|
|
alignxf[4] = -cy*sz;
|
||
|
|
alignxf[5] = -sx*sy*sz + cx*cz;
|
||
|
|
alignxf[6] = cx*sy*sz + sx*cz;
|
||
|
|
alignxf[7] = 0;
|
||
|
|
alignxf[8] = sy;
|
||
|
|
alignxf[9] = -sx*cy;
|
||
|
|
alignxf[10] = cx*cy;
|
||
|
|
alignxf[11] = 0;
|
||
|
|
alignxf[12] = dx[0];
|
||
|
|
alignxf[13] = dx[1];
|
||
|
|
alignxf[14] = dx[2];
|
||
|
|
alignxf[15] = 1;
|
||
|
|
}
|