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.
448 lines
14 KiB
Plaintext
448 lines
14 KiB
Plaintext
/*
|
|
* 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;
|
|
}
|
|
|