511 lines
14 KiB
Text
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;
|
|
}
|
|
|
|
|