3dpcp/.svn/pristine/60/609f39aa96725d6a75c76ce1c87016ab532a11f4.svn-base
2012-09-16 14:33:11 +02:00

511 lines
14 KiB
Text

/*
* gapx6D implementation
*
* Copyright (C) Jan Elseberg, Andreas Nuechter
*
* Released under the GPL version 3.
*
*/
/**
* @file
* @brief The implementation of globally consistent scan matching algorithm by using APX correction
* @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany.
* @author Jan Elseberg. Institute of Computer Science, University of Osnabrueck, Germany.
*/
#ifdef _MSC_VER
#if !defined _OPENMP && defined OPENMP
#define _OPENMP
#endif
#endif
#include "slam6d/gapx6D.h"
#include "slam6d/icp6Dapx.h"
#include "sparse/csparse.h"
#include <cfloat>
#include <fstream>
using std::flush;
#include <cstring>
#include "slam6d/globals.icc"
using std::ofstream;
using namespace NEWMAT;
/**
* Constructor
*
* @param my_icp6Dminimizer Pointer to ICP minimization functor
* @param mdm Maximum PtoP distance to which point pairs are collected for ICP
* @param max_dist_match Maximum PtoP distance to which point pairs are collected for LUM
* @param max_num_iterations Maximal number of iterations for ICP
* @param quiet Suspesses all output to std out
* @param meta Indicates if metascan matching has to be used
* @param rnd Indicates if randomization has to be used
* @param eP Extrapolate odometry?
* @param anim Animate which frames?
* @param epsilonICP Termination criterion for ICP
* @param nns_method Specifies which NNS method to use
* @param epsilonLUM Termination criterion for LUM
*/
gapx6D::gapx6D(icp6Dminimizer *my_icp6Dminimizer,
double mdm, double max_dist_match,
int max_num_iterations, bool quiet, bool meta, int rnd,
bool eP, int anim, double epsilonICP, int nns_method, double epsilonLUM)
: graphSlam6D(my_icp6Dminimizer,
mdm, max_dist_match,
max_num_iterations, quiet, meta, rnd,
eP, anim, epsilonICP, nns_method, epsilonLUM)
{ }
/**
* Destructor
*/
gapx6D::~gapx6D()
{
delete my_icp;
}
double gapx6D::genBAtransForLinkedPair( int firstScanNum, int secondScanNum,
double *centroids_m, double *centroids_d,
SymmetricMatrix *B, ColumnVector *A, ColumnVector &X)
{
Point cm(centroids_m);
Point cd(centroids_d);
double x[3], dx[3] = {0.0, 0.0, 0.0};
unsigned int vectorOffset;
double alignxf[16];
vectorOffset = (firstScanNum-1) * 3;
if (firstScanNum != 0) {
x[0] = X(vectorOffset + 1);
x[1] = X(vectorOffset + 2);
x[2] = X(vectorOffset + 3);
} else {
x[0] = x[1] = x[2] = 0.0;
}
icp6D_APX::computeRt(x, dx, alignxf);
cm.transform(alignxf);
vectorOffset = (secondScanNum-1) * 3;
x[0] = X(vectorOffset + 1);
x[1] = X(vectorOffset + 2);
x[2] = X(vectorOffset + 3);
icp6D_APX::computeRt(x, dx, alignxf);
cd.transform(alignxf);
ColumnVector Ak1(3);
Ak1(1) = cm.x - cd.x;
Ak1(2) = cm.y - cd.y;
Ak1(3) = cm.z - cd.z;
#ifdef _OPENMP
#pragma omp critical (enterB)
#endif
{
if(firstScanNum != 0) {
A->Rows((firstScanNum-1)*3+1, (firstScanNum-1)*3+3) -= Ak1;
B->element(firstScanNum -1, firstScanNum - 1) += 1;
B->element(firstScanNum -1, secondScanNum - 1) -= 1;
}
A->Rows((secondScanNum-1)*3+1, (secondScanNum-1)*3+3) += Ak1;
B->element(secondScanNum - 1, secondScanNum - 1) += 1;
} // of pragma omp critical
return 1.0;
}
/**
* This function generates the matrices B and Bd that are used for solving B * c = Bd.
* This function has to be called once for every linked scan-pair.
*
* @param firstScanNum The number of the first scan of the linked scan-pair
* @param secondScanNum The number of the second scan of the linked scan-pair
* @param ptpairs Vector that holds all point-pairs for the actual scan-pair
* @param B Matrix with dimension (6*(number of scans-1)) x (6 * (number of scans-1))
* @param Bd Vector with dimension (6*(number of scans-1))
* @return returns the sum of square distance
*/
double gapx6D::genBArotForLinkedPair( int firstScanNum, int secondScanNum, vPtPair *ptpairs,
double *centroids_m, double *centroids_d,
Matrix *B, ColumnVector *A)
{
Matrix Mk(3,3), Dk(3,3);
Matrix MkMkt(3,3), DkDkt(3,3), DkMkt(3,3), MkDkt(3,3);
ColumnVector Ak1(3), Ak2(3);
double p1x, p1y, p1z, p2x, p2y, p2z;
double p1x2, p1y2, p1z2, p2x2, p2y2, p2z2;
double p1xp1y, p1xp1z, p1yp1z, p1y2p1z2, p1x2p1z2, p1x2p1y2;
double p2xp2y, p2xp2z, p2yp2z, p2y2p2z2, p2x2p2z2, p2x2p2y2;
double p2zp1x, p2zp1y, p2yp1x, p1yp2x, p1zp2x, p1zp2y;
double p1xp2xp1yp2y, p1xp2xp1zp2z, p1yp2yp1zp2z;
int n = (*ptpairs).size();
MkMkt = 0;
DkDkt = 0;
DkMkt = 0;
MkDkt = 0;
Ak1 = 0;
Ak2 = 0;
for (int i = 0; i < n; i++) {
p1x = (*ptpairs)[i].p1.x - centroids_m[0]; // << korrekt
p1y = (*ptpairs)[i].p1.y - centroids_m[1]; // << korrekt
p1z = (*ptpairs)[i].p1.z - centroids_m[2]; // << korrekt
p2x = (*ptpairs)[i].p2.x - centroids_m[0]; // << korrekt
p2y = (*ptpairs)[i].p2.y - centroids_m[1]; // << korrekt
p2z = (*ptpairs)[i].p2.z - centroids_m[2]; // << korrekt
p1x2 = sqr(p1x);
p1y2 = sqr(p1y);
p1z2 = sqr(p1z);
p2x2 = sqr(p2x);
p2y2 = sqr(p2y);
p2z2 = sqr(p2z);
p1x2p1z2 = p1x2 + p1z2;
p1x2p1y2 = p1x2 + p1y2;
p1y2p1z2 = p1y2 + p1z2;
p2x2p2z2 = p2x2 + p2z2;
p2x2p2y2 = p2x2 + p2y2;
p2y2p2z2 = p2y2 + p2z2;
p1xp2xp1yp2y = p1x * p2x + p1y + p2y;
p1xp2xp1zp2z = p1x * p2x + p1z + p2z;
p1yp2yp1zp2z = p1y * p2y + p1z + p2z;
p1xp1y = p1x * p1y;
p1xp1z = p1x * p1z;
p1yp1z = p1y * p1z;
//p2xp1y = p2x * p1y;
//p2xp1z = p2x * p1z;
p2yp1x = p2y * p1x;
//p2yp1z = p2y * p1z;
p2zp1x = p2z * p1x;
p2zp1y = p2z * p1y;
/*
p1xp2y = p1x * p2y;
p1xp2z = p1x * p2z;
p1yp2z = p1y * p2z;
*/
p1yp2x = p1y * p2x;
p1zp2x = p1z * p2x;
p1zp2y = p1z * p2y;
p2xp2y = p2x * p2y;
p2xp2z = p2x * p2z;
p2yp2z = p2y * p2z;
MkMkt(1,1) += p1y2p1z2;
MkMkt(1,2) -= p1xp1y;
MkMkt(1,3) -= p1xp1z;
MkMkt(2,1) -= p1xp1y;
MkMkt(2,2) += p1x2p1z2;
MkMkt(2,3) -= p1yp1z;
MkMkt(3,1) -= p1xp1z;
MkMkt(3,2) -= p1yp1z;
MkMkt(3,3) += p1x2p1y2;
DkDkt(1,1) += p2y2p2z2;
DkDkt(1,2) -= p2xp2y;
DkDkt(1,3) -= p2xp2z;
DkDkt(2,1) -= p2xp2y;
DkDkt(2,2) += p2x2p2z2;
DkDkt(2,3) -= p2yp2z;
DkDkt(3,1) -= p2xp2z;
DkDkt(3,2) -= p2yp2z;
DkDkt(3,3) += p2x2p2y2;
MkDkt(1,1) += p1yp2yp1zp2z;
MkDkt(1,2) -= p1yp2x;
MkDkt(1,3) -= p1zp2x;
MkDkt(2,1) -= p1yp2x;
MkDkt(2,2) += p1xp2xp1zp2z;
MkDkt(2,3) -= p1zp2y;
MkDkt(3,1) -= p1zp2x;
MkDkt(3,2) -= p1zp2y;
MkDkt(3,3) += p1xp2xp1yp2y;
DkMkt(1,1) += p1yp2yp1zp2z;
DkMkt(1,2) -= p2yp1x;
DkMkt(1,3) -= p2zp1x;
DkMkt(2,1) -= p2yp1x;
DkMkt(2,2) += p1xp2xp1zp2z;
DkMkt(2,3) -= p2zp1y;
DkMkt(3,1) -= p2zp1x;
DkMkt(3,2) -= p2zp1y;
DkMkt(3,3) += p1xp2xp1yp2y;
Ak1(1) -= (p1z - p2z) * p2y - (p1y - p2y) * p2z;
Ak1(2) -= (p1x - p2x) * p2z - (p1z - p2z) * p2x;
Ak1(3) -= (p1y - p2y) * p2x - (p1x - p2x) * p2y;
Ak2(1) += (p1z - p2z) * p1y - (p1y - p2y) * p1z;
Ak2(2) += (p1x - p2x) * p1z - (p1z - p2z) * p1x;
Ak2(3) += (p1y - p2y) * p1x - (p1x - p2x) * p1y;
}
#ifdef _OPENMP
#pragma omp critical (enterB)
#endif
{
if(firstScanNum != 0) {
A->Rows((firstScanNum-1)*3+1, (firstScanNum-1)*3+3) += Ak1;
B->SubMatrix((firstScanNum-1)*3+1,(firstScanNum-1)*3+3, (firstScanNum-1)*3+1,(firstScanNum-1)*3+3) += MkMkt;
B->SubMatrix((firstScanNum-1)*3+1,(firstScanNum-1)*3+3, (secondScanNum-1)*3+1,(secondScanNum-1)*3+3) += DkMkt;
B->SubMatrix((secondScanNum-1)*3+1,(secondScanNum-1)*3+3, (firstScanNum-1)*3+1,(firstScanNum-1)*3+3) += MkDkt;
}
A->Rows((secondScanNum-1)*3+1, (secondScanNum-1)*3+3) += Ak2;
B->SubMatrix((secondScanNum-1)*3+1,(secondScanNum-1)*3+3, (secondScanNum-1)*3+1,(secondScanNum-1)*3+3) += DkDkt;
} // of pragma omp critical
return 1.0;
}
/**
* This function is used to match a set of laser scans with any minimally
* connected Graph, using the globally consistent LUM-algorithm in 3D.
*
* @param gr Some Graph with no real subgraphs except for itself
* @param allScans Contains all laser scans
* @param nrIt The number of iterations the LUM-algorithm will run
* @return Euclidian distance of all pose shifts
*/
double gapx6D::doGraphSlam6D(Graph gr, vector <Scan *> allScans, int nrIt)
{
#ifdef WRITE_GRAPH_NET
// for debug only:
static int d = 0;
cout << "writing graph.dat ....................................." << endl;
d++;
string gfilename = "graph_" + to_string(d, 3) + ".net";
ofstream out(gfilename.c_str());
for (int i=0; i < gr.getNrLinks(); i++) {
int from = gr.getLink(i,0);
int to = gr.getLink(i,1);
// shouldn't be necessary, just in case a (out of date) graph file is loaded:
if (from < (int)allScans.size() && to < (int)allScans.size()) {
out << allScans[from]->get_rPos()[0] << " "
<< allScans[from]->get_rPos()[1] << " "
<< allScans[from]->get_rPos()[2] << endl
<< allScans[to ]->get_rPos()[0] << " "
<< allScans[to ]->get_rPos()[1] << " "
<< allScans[to ]->get_rPos()[2] << endl << endl;
}
}
out.close();
out.clear();
#endif
// the IdentityMatrix to transform some Scans with
double id[16];
M4identity(id);
vPtPair **ptpairs = 0; // Contains sets of point pairs for all links
double **centroids_m = 0, **centroids_d = 0; // Contains centroids for all links
Matrix B ( 3 * (gr.getNrScans()-1), 3 * (gr.getNrScans()-1) );
SymmetricMatrix Bt ( gr.getNrScans()-1 ); Bt = 0;
ColumnVector X( 3*(gr.getNrScans()-1) ); X = 0;
ColumnVector T( 3*(gr.getNrScans()-1) ); T = 0;
ColumnVector A( 3*(gr.getNrScans()-1) ); A = 0;
B = 0.0;
A = 0.0;
double sum_position_diff = 0;
double ret = DBL_MAX;
for(int iteration = 0;
iteration < nrIt && ret > epsilonLUM;
iteration++) {
sum_position_diff = 0;
if (nrIt > 1) cout << "Iteration match " << iteration << endl;
// Transform first scan to zero, otherwise updating poses would yield incorrect values
// if (iteration == 0) {
// double* tin = allScans[0]->transMat;
// double tout[16];
// M4inv(tin, tout);
// for(unsigned int i = 0; i < allScans.size(); i++){
// allScans[i]->transform(tout, Scan::INVALID);
// }
// }
if (ptpairs != 0) {
delete [] ptpairs;
}
if (centroids_m != 0) {
delete [] centroids_m;
delete [] centroids_d;
}
centroids_m = new double *[gr.getNrLinks()];
centroids_d = new double *[gr.getNrLinks()];
for (int i = 0; i < gr.getNrLinks(); i++){
centroids_m[i] = new double[3];
centroids_m[i][0] = centroids_m[i][1] = centroids_m[i][2] = 0.0;
centroids_d[i] = new double[3];
centroids_d[i][0] = centroids_d[i][1] = centroids_d[i][2] = 0.0;
}
ptpairs = new vPtPair*[gr.getNrLinks()];
for (int i = 0; i < gr.getNrLinks(); i++) {
ptpairs[i] = new vPtPair;
}
// Get all point pairs after ICP
int end_loop = gr.getNrLinks();
#ifdef _OPENMP
omp_set_num_threads(OPENMP_NUM_THREADS);
#pragma omp parallel for schedule(dynamic)
#endif
for (int i = 0; i < end_loop; i++) {
cout << "P" << i << flush;
Scan * FirstScan = allScans[gr.getLink(i,0)];
Scan * SecondScan = allScans[gr.getLink(i,1)];
#ifdef _OPENMP
int thread_num = omp_get_thread_num();
#else
int thread_num = 0;
#endif
double dummy_sum;
Scan::getPtPairs(ptpairs[i], FirstScan, SecondScan, thread_num,
(int)my_icp->get_rnd(), (int)max_dist_match2_LUM, dummy_sum,
centroids_m[i], centroids_d[i]);
// faulty network
if (ptpairs[i]->size() <= 1) {
cout << "Error: Link (" << gr.getLink(i,0)
<< " - " << gr.getLink(i, 1) << " ) is empty with "
<< ptpairs[i]->size() << " Corr. points. iteration = "
<< iteration << endl;
// exit(1);
} else
sum_position_diff += genBArotForLinkedPair( gr.getLink(i,0), gr.getLink(i,1), ptpairs[i],
centroids_m[i], centroids_d[i], &B, &A);
}
cout << " building rotation matrices done! " << endl;
X = solveSparseCholesky(B, A);
// TODO transformation bestimmen
Bt = 0.0;
A = 0.0;
for ( int i = 0; i < end_loop; i++) {
genBAtransForLinkedPair( gr.getLink(i,0), gr.getLink(i,1),
centroids_m[i], centroids_d[i], &Bt, &A, X);
}
cout << " building translation matrices done! "<<endl;
Bt = Bt.i();
for (int i = 0; i < gr.getNrScans()-1; i++) {
for (int j = 0; j < gr.getNrScans()-1; j++) {
double bt = Bt.element(i, j);
T.Rows(i*3 +1, i*3+3 ) += A.Rows(j*3 +1, j*3 +3 ) * bt;
}
}
// delete ptPairs
for (int i = 0; i < gr.getNrLinks(); i++) {
ptpairs[i]->clear();
delete (ptpairs[i]);
}
// delete centroids
for (int i = 0; i < gr.getNrLinks(); i++){
delete [] centroids_m[i];
delete [] centroids_d[i];
}
ColumnVector t0(3), t(3), tlast(3);
int vectorOffset;
int loop_end = gr.getNrScans();
double alignxf[16];
for(int i = 1; i < loop_end; i++)
{
vectorOffset = (i-1) * 3;
// Interpret results
double x[3] = { X(vectorOffset + 1), X(vectorOffset + 2), X(vectorOffset + 3) };
double dx[3] = { T(vectorOffset + 1), T(vectorOffset + 2), T(vectorOffset + 3) };
icp6D_APX::computeRt(x, dx, alignxf);
// Update the Pose
cout << "Old pose estimate, Scan " << i << endl;
cout << "x: " << allScans[i]->get_rPos()[0]
<< " y: " << allScans[i]->get_rPos()[1]
<< " z: " << allScans[i]->get_rPos()[2]
<< " tx: " << allScans[i]->get_rPosTheta()[0]
<< " ty: " << allScans[i]->get_rPosTheta()[1]
<< " tz: " << allScans[i]->get_rPosTheta()[2]
<< endl;
if (i < loop_end - 1) {
allScans[i]->transform(alignxf, Scan::LUM, 1);
} else {
allScans[i]->transform(alignxf, Scan::LUM, 2);
}
cout << "x: " << allScans[i]->get_rPos()[0]
<< " y: " << allScans[i]->get_rPos()[1]
<< " z: " << allScans[i]->get_rPos()[2]
<< " tx: " << allScans[i]->get_rPosTheta()[0]
<< " ty: " << allScans[i]->get_rPosTheta()[1]
<< " tz: " << allScans[i]->get_rPosTheta()[2] << endl << endl;
sum_position_diff += Len(dx);
}
cout << "Sum of Position differences = " << sum_position_diff << endl << endl;
ret = (sum_position_diff / (double)gr.getNrScans());
}
delete [] centroids_m;
delete [] centroids_d;
delete [] ptpairs;
ptpairs = 0;
return ret;
}