3dpcp/.svn/pristine/0d/0db5f030f4213d96d832db268261073267a83f29.svn-base
2012-09-16 14:33:11 +02:00

447 lines
14 KiB
Text

/*
* ghelix6DQ2 implementation
*
* Copyright (C) Peter Schneider, Jan Elseberg, Andreas Nuechter
*
* Released under the GPL version 3.
*
*/
/**
* @file
* @brief The implementation of globally consistent scan matching algorithm by using helix correction
* @author Peter Schneider. Institute of Computer Science, University of Koblenz, Germany.
* @author Andreas Nuechter. Jacobs University Bremen gGmbH, Germany
* @author Jan Elseberg. Jacobs University Bremen gGmbH, Germany
*/
#ifdef _MSC_VER
#if !defined _OPENMP && defined OPENMP
#define _OPENMP
#endif
#endif
#include "slam6d/ghelix6DQ2.h"
#include "slam6d/icp6Dhelix.h"
#include "sparse/csparse.h"
#include <cfloat>
#include <fstream>
using std::flush;
#include <cstring>
#include "slam6d/globals.icc"
using std::ofstream;
/**
* 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
*/
ghelix6DQ2::ghelix6DQ2(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
*/
ghelix6DQ2::~ghelix6DQ2()
{
delete my_icp;
}
/**
* 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 ghelix6DQ2::genBBdForLinkedPair( int firstScanNum, int secondScanNum, vPtPair *ptpairs,
Matrix *B, ColumnVector *Bd )
{
double Btemp1[6][3];
double Btemp2[6][3];
memset(&Btemp1[0][0], 0, 18 * sizeof(double));
memset(&Btemp2[0][0], 0, 18 * sizeof(double));
double bd1[6];
double bd2[6];
memset(&bd1[0], 0, 6 * sizeof(double));
memset(&bd2[0], 0, 6 * sizeof(double));
double p1x, p1y, p1z, p2x, p2y, p2z, pDistX, pDistY, pDistZ, px2Sq, py2Sq, pz2Sq;
int n = (*ptpairs).size();
double sum = 0;
for (int i = 0; i < n; i++) {
p1x = (*ptpairs)[i].p1.x;
p1y = (*ptpairs)[i].p1.y;
p1z = (*ptpairs)[i].p1.z;
p2x = (*ptpairs)[i].p2.x;
p2y = (*ptpairs)[i].p2.y;
p2z = (*ptpairs)[i].p2.z;
//px1Sq = p1x * p1x;
//py1Sq = p1y * p1y;
//pz1Sq = p1z * p1z;
px2Sq = p2x * p2x;
py2Sq = p2y * p2y;
pz2Sq = p2z * p2z;
Btemp1[4][0] += -p2z;
// Btemp1[3][1] += p2z;
Btemp1[5][0] += p2y;
// Btemp1[3][2] += -p2y;
Btemp1[4][2] += p2x;
// Btemp1[5][1] += -p2x;
Btemp1[0][0] += pz2Sq + py2Sq;
Btemp1[1][0] += p2y*-p2x;
Btemp1[2][0] += -p2z*p2x;
Btemp1[1][1] += pz2Sq + px2Sq;
Btemp1[2][1] += p2z*-p2y;
Btemp1[2][2] += px2Sq + py2Sq;
pDistX = p1x - p2x;
pDistY = p1y - p2y;
pDistZ = p1z - p2z;
bd1[0] += -p1z*pDistY + p1y*pDistZ;
bd1[1] += p1z*pDistX - p1x*pDistZ;
bd1[2] += -p1y*pDistX + p1x*pDistY;
bd1[3] += pDistX;
bd1[4] += pDistY;
bd1[5] += pDistZ;
bd2[0] += -p2z*-pDistY + p2y*-pDistZ;
bd2[1] += p2z*-pDistX - p2x*-pDistZ;
bd2[2] += -p2y*-pDistX + p2x*-pDistY;
bd2[3] += -pDistX;
bd2[4] += -pDistY;
bd2[5] += -pDistZ;
sum += pDistX*pDistX + pDistY*pDistY + pDistZ*pDistZ;
}
#ifdef _OPENMP
#pragma omp critical (enterB)
#endif
{
int matPlace1 = (firstScanNum-1) * 6;
if(firstScanNum != 0)
{
(*B)(matPlace1+4,matPlace1+4) += n;
(*B)(matPlace1+5,matPlace1+5) += n;
(*B)(matPlace1+6,matPlace1+6) += n;
(*B)(matPlace1+1,matPlace1+5) += Btemp1[4][0];
(*B)(matPlace1+5,matPlace1+1) += Btemp1[4][0];
(*B)(matPlace1+2,matPlace1+4) += -Btemp1[4][0];
(*B)(matPlace1+4,matPlace1+2) += -Btemp1[4][0];
(*B)(matPlace1+1,matPlace1+6) += Btemp1[5][0];
(*B)(matPlace1+6,matPlace1+1) += Btemp1[5][0];
(*B)(matPlace1+3,matPlace1+4) += -Btemp1[5][0];
(*B)(matPlace1+4,matPlace1+3) += -Btemp1[5][0];
(*B)(matPlace1+3,matPlace1+5) += Btemp1[4][2];
(*B)(matPlace1+5,matPlace1+3) += Btemp1[4][2];
(*B)(matPlace1+2,matPlace1+6) += -Btemp1[4][2];
(*B)(matPlace1+6,matPlace1+2) += -Btemp1[4][2];
(*B)(matPlace1+1,matPlace1+2) += Btemp1[1][0];
(*B)(matPlace1+2,matPlace1+1) += Btemp1[1][0];
(*B)(matPlace1+1,matPlace1+3) += Btemp1[2][0];
(*B)(matPlace1+3,matPlace1+1) += Btemp1[2][0];
(*B)(matPlace1+2,matPlace1+3) += Btemp1[2][1];
(*B)(matPlace1+3,matPlace1+2) += Btemp1[2][1];
(*B)(matPlace1+1,matPlace1+1) += Btemp1[0][0];
(*B)(matPlace1+2,matPlace1+2) += Btemp1[1][1];
(*B)(matPlace1+3,matPlace1+3) += Btemp1[2][2];
(*Bd)(matPlace1+1) += bd1[0];
(*Bd)(matPlace1+2) += bd1[1];
(*Bd)(matPlace1+3) += bd1[2];
(*Bd)(matPlace1+4) += bd1[3];
(*Bd)(matPlace1+5) += bd1[4];
(*Bd)(matPlace1+6) += bd1[5];
}
//-------------------------------------------
unsigned int matPlace2 = (secondScanNum-1) * 6;
(*B)(matPlace2+4,matPlace2+4) += n;
(*B)(matPlace2+5,matPlace2+5) += n;
(*B)(matPlace2+6,matPlace2+6) += n;
(*B)(matPlace2+1,matPlace2+5) += Btemp1[4][0];
(*B)(matPlace2+5,matPlace2+1) += Btemp1[4][0];
(*B)(matPlace2+2,matPlace2+4) += -Btemp1[4][0];
(*B)(matPlace2+4,matPlace2+2) += -Btemp1[4][0];
(*B)(matPlace2+1,matPlace2+6) += Btemp1[5][0];
(*B)(matPlace2+6,matPlace2+1) += Btemp1[5][0];
(*B)(matPlace2+3,matPlace2+4) += -Btemp1[5][0];
(*B)(matPlace2+4,matPlace2+3) += -Btemp1[5][0];
(*B)(matPlace2+3,matPlace2+5) += Btemp1[4][2];
(*B)(matPlace2+5,matPlace2+3) += Btemp1[4][2];
(*B)(matPlace2+2,matPlace2+6) += -Btemp1[4][2];
(*B)(matPlace2+6,matPlace2+2) += -Btemp1[4][2];
(*B)(matPlace2+1,matPlace2+2) += Btemp1[1][0];
(*B)(matPlace2+2,matPlace2+1) += Btemp1[1][0];
(*B)(matPlace2+1,matPlace2+3) += Btemp1[2][0];
(*B)(matPlace2+3,matPlace2+1) += Btemp1[2][0];
(*B)(matPlace2+2,matPlace2+3) += Btemp1[2][1];
(*B)(matPlace2+3,matPlace2+2) += Btemp1[2][1];
(*B)(matPlace2+1,matPlace2+1) += Btemp1[0][0];
(*B)(matPlace2+2,matPlace2+2) += Btemp1[1][1];
(*B)(matPlace2+3,matPlace2+3) += Btemp1[2][2];
(*Bd)(matPlace2+1) += bd2[0];
(*Bd)(matPlace2+2) += bd2[1];
(*Bd)(matPlace2+3) += bd2[2];
(*Bd)(matPlace2+4) += bd2[3];
(*Bd)(matPlace2+5) += bd2[4];
(*Bd)(matPlace2+6) += bd2[5];
//-------------------------------------------
if( firstScanNum != 0)
{
(*B)(matPlace1+4,matPlace2+4) -= n;
(*B)(matPlace1+5,matPlace2+5) -= n;
(*B)(matPlace1+6,matPlace2+6) -= n;
(*B)(matPlace1+1,matPlace2+5) += -Btemp1[4][0];
(*B)(matPlace1+5,matPlace2+1) += -Btemp1[4][0];
(*B)(matPlace1+2,matPlace2+4) += Btemp1[4][0];
(*B)(matPlace1+4,matPlace2+2) += Btemp1[4][0];
(*B)(matPlace1+1,matPlace2+6) += -Btemp1[5][0];
(*B)(matPlace1+6,matPlace2+1) += -Btemp1[5][0];
(*B)(matPlace1+3,matPlace2+4) += Btemp1[5][0];
(*B)(matPlace1+4,matPlace2+3) += Btemp1[5][0];
(*B)(matPlace1+3,matPlace2+5) += -Btemp1[4][2];
(*B)(matPlace1+5,matPlace2+3) += -Btemp1[4][2];
(*B)(matPlace1+2,matPlace2+6) += Btemp1[4][2];
(*B)(matPlace1+6,matPlace2+2) += Btemp1[4][2];
(*B)(matPlace1+1,matPlace2+2) += -Btemp1[1][0];
(*B)(matPlace1+2,matPlace2+1) += -Btemp1[1][0];
(*B)(matPlace1+1,matPlace2+3) += -Btemp1[2][0];
(*B)(matPlace1+3,matPlace2+1) += -Btemp1[2][0];
(*B)(matPlace1+2,matPlace2+3) += -Btemp1[2][1];
(*B)(matPlace1+3,matPlace2+2) += -Btemp1[2][1];
(*B)(matPlace1+1,matPlace2+1) += -Btemp1[0][0];
(*B)(matPlace1+2,matPlace2+2) += -Btemp1[1][1];
(*B)(matPlace1+3,matPlace2+3) += -Btemp1[2][2];
//----------------------------------------
(*B)(matPlace2+4,matPlace1+4) -= n;
(*B)(matPlace2+5,matPlace1+5) -= n;
(*B)(matPlace2+6,matPlace1+6) -= n;
(*B)(matPlace2+1,matPlace1+5) += -Btemp1[4][0];
(*B)(matPlace2+5,matPlace1+1) += -Btemp1[4][0];
(*B)(matPlace2+2,matPlace1+4) += Btemp1[4][0];
(*B)(matPlace2+4,matPlace1+2) += Btemp1[4][0];
(*B)(matPlace2+1,matPlace1+6) += -Btemp1[5][0];
(*B)(matPlace2+6,matPlace1+1) += -Btemp1[5][0];
(*B)(matPlace2+3,matPlace1+4) += Btemp1[5][0];
(*B)(matPlace2+4,matPlace1+3) += Btemp1[5][0];
(*B)(matPlace2+3,matPlace1+5) += -Btemp1[4][2];
(*B)(matPlace2+5,matPlace1+3) += -Btemp1[4][2];
(*B)(matPlace2+2,matPlace1+6) += Btemp1[4][2];
(*B)(matPlace2+6,matPlace1+2) += Btemp1[4][2];
(*B)(matPlace2+1,matPlace1+2) += -Btemp1[1][0];
(*B)(matPlace2+2,matPlace1+1) += -Btemp1[1][0];
(*B)(matPlace2+1,matPlace1+3) += -Btemp1[2][0];
(*B)(matPlace2+3,matPlace1+1) += -Btemp1[2][0];
(*B)(matPlace2+2,matPlace1+3) += -Btemp1[2][1];
(*B)(matPlace2+3,matPlace1+2) += -Btemp1[2][1];
(*B)(matPlace2+1,matPlace1+1) += -Btemp1[0][0];
(*B)(matPlace2+2,matPlace1+2) += -Btemp1[1][1];
(*B)(matPlace2+3,matPlace1+3) += -Btemp1[2][2];
}
} // of pragma omp critical
return sqrt( sum / (double) n );
}
/**
* 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 ghelix6DQ2::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
Matrix B ( 6 * (gr.getNrScans()-1), 6 * (gr.getNrScans()-1) );
ColumnVector ccs( 6*(gr.getNrScans()-1) ), bd( 6*(gr.getNrScans()-1) );
B = 0.0;
bd = 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;
if (ptpairs != 0) delete [] ptpairs;
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_centroid_m[3];
double dummy_centroid_d[3];
double dummy_sum;
Scan::getPtPairs(ptpairs[i], FirstScan, SecondScan, thread_num,
(int)my_icp->get_rnd(), (int)max_dist_match2_LUM, dummy_sum,
dummy_centroid_m, dummy_centroid_d);
// 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
// build the matrix B and vector bd
genBBdForLinkedPair( gr.getLink(i,0), gr.getLink(i,1), ptpairs[i], &B, &bd );
}
cout <<" building matrices done! "<<endl;
ccs = solveSparseCholesky(B, bd);
// delete ptPairs
for (int i = 0; i < gr.getNrLinks(); i++) {
ptpairs[i]->clear();
delete (ptpairs[i]);
}
ColumnVector t0(3), t(3), tlast(3);
int vectorOffset;
int loop_end = gr.getNrScans();
double alignxfLum[16];
for(int i = 1; i < loop_end; i++)
{
vectorOffset = (i-1) * 6;
icp6D_HELIX::computeRt( &ccs, vectorOffset, alignxfLum);
// 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(alignxfLum, Scan::LUM, 1);
} else {
allScans[i]->transform(alignxfLum, 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 += sqrt( sqr(alignxfLum[12]) + sqr(alignxfLum[13]) + sqr(alignxfLum[14]));
}
cout << "Sum of Position differences = " << sum_position_diff << endl << endl;
ret = (sum_position_diff / (double)gr.getNrScans());
}
delete [] ptpairs;
ptpairs = 0;
return ret;
}