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.
481 lines
13 KiB
C++
481 lines
13 KiB
C++
/*
|
|
* lum6Dquat implementation
|
|
*
|
|
* Copyright (C) Andreas Nuechter, Jan Elseberg
|
|
*
|
|
* Released under the GPL version 3.
|
|
*
|
|
*/
|
|
|
|
/**
|
|
* @file
|
|
* @brief The implementation of globally consistent scan matching algorithm
|
|
* @author Jan Elseberg. Institute of Computer Science, University of Osnabrueck, Germany.
|
|
* @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany.
|
|
*/
|
|
|
|
#ifdef _MSC_VER
|
|
#if !defined _OPENMP && defined OPENMP
|
|
#define _OPENMP
|
|
#endif
|
|
#endif
|
|
|
|
#include "slam6d/lum6Dquat.h"
|
|
#include "sparse/csparse.h"
|
|
|
|
#include <cfloat>
|
|
#include <fstream>
|
|
using std::ofstream;
|
|
using std::cerr;
|
|
#include "slam6d/globals.icc"
|
|
|
|
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 is used
|
|
* @param epsilonLUM Termination criterion for LUM
|
|
*/
|
|
lum6DQuat::lum6DQuat(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
|
|
*/
|
|
lum6DQuat::~lum6DQuat()
|
|
{
|
|
delete my_icp;
|
|
}
|
|
|
|
|
|
/**
|
|
* This function calculates the inverse covariances Cij and the Vector Cij*Dij for
|
|
* two scans by finding pointpairs.
|
|
*
|
|
* @param first pointer to the first scan of the link
|
|
* @param second pointer to the second scan of the link
|
|
* @param nns_method Specifies which NNS method is used
|
|
* @param rnd shall we use randomization for computing the point pairs?
|
|
* @param max_dist_match2 maximal distance allowed for point pairs
|
|
* @param C pointer to the inverse of the covariance matrix Cij
|
|
* @param CD pointer to the vector Cij*Dij
|
|
*/
|
|
void lum6DQuat::covarianceQuat(Scan *first, Scan *second,
|
|
int nns_method, int rnd, double max_dist_match2,
|
|
Matrix *C, ColumnVector *CD)
|
|
{
|
|
// x,y,z denote the coordinates of uk (Here averaged over ak and bk)
|
|
// sx,sy,sz are the sums of their respective coordinates of uk over each paint pair
|
|
// xpy,xpz,ypz are the sums over x*x + y*y ,x*x + z*z and y*y + z*z respectively over each point pair
|
|
// xy,yz,xz are the sums over each respective multiplication
|
|
// dx,dy,dz are the deltas in each coordinate of a point pair
|
|
// ss is the estimation of the covariance of sensing error
|
|
double x, y, z, sx, sy, sz, xy, yz, xz, ypz, xpz, xpy, dx, dy, dz, ss, xpypz;
|
|
|
|
// D is needed to calculate the estimation of the covariance s
|
|
ColumnVector D(7);
|
|
// Almost Cij*Dij
|
|
ColumnVector MZ(7);
|
|
// Almost the covarianve
|
|
Matrix MM(7,7);
|
|
// A set of point pairs
|
|
vector <PtPair> uk;
|
|
// A point pair
|
|
Point ak, bk;
|
|
// number of pairs in a set
|
|
int m;
|
|
|
|
#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(&uk, first, second, thread_num,
|
|
rnd, max_dist_match2, dummy_sum, dummy_centroid_m, dummy_centroid_d);
|
|
|
|
m = uk.size();
|
|
|
|
MZ = 0.0;
|
|
MM = 0.0;
|
|
sx = sy = sz = xy = yz = xz = ypz = xpz = xpy = xpypz = ss = 0.0;
|
|
|
|
if (m > 2) {
|
|
// for each point pair
|
|
for(int j = 0; j < m; j++){
|
|
ak = uk[j].p1;
|
|
bk = uk[j].p2;
|
|
|
|
// Some temporary values
|
|
x = (ak.x + bk.x)/2.0;
|
|
y = (ak.y + bk.y)/2.0;
|
|
z = (ak.z + bk.z)/2.0;
|
|
dx = ak.x - bk.x;
|
|
dy = ak.y - bk.y;
|
|
dz = ak.z - bk.z;
|
|
|
|
// Sum up all necessary values to construct MM
|
|
sx += x;
|
|
sy += y;
|
|
sz += z;
|
|
|
|
xpy += x*x + y*y;
|
|
xpz += x*x + z*z;
|
|
ypz += y*y + z*z;
|
|
xpypz += x*x + y*y + z*z;
|
|
|
|
xy += x*y;
|
|
xz += x*z;
|
|
yz += y*z;
|
|
|
|
// Sum up each part of MZ
|
|
MZ(1) += dx;
|
|
MZ(2) += dy;
|
|
MZ(3) += dz;
|
|
MZ(4) += x * dx + y * dy + z * dz;
|
|
MZ(5) += z * dy - y * dz;
|
|
MZ(6) += x * dz - z * dx;
|
|
MZ(7) += y * dx - x * dy;
|
|
}
|
|
// Now construct the symmetrical matrix MM
|
|
MM(1,1) = MM(2,2) = MM(3,3) = m;
|
|
|
|
MM(4,4) = xpypz;
|
|
MM(5,5) = ypz;
|
|
MM(6,6) = xpz;
|
|
MM(7,7) = xpy;
|
|
|
|
MM(1,4) = MM(4,1) = sx;
|
|
MM(1,6) = MM(6,1) = -sz;
|
|
MM(1,7) = MM(7,1) = sy;
|
|
|
|
MM(2,4) = MM(4,2) = sy;
|
|
MM(2,5) = MM(5,2) = sz;
|
|
MM(2,7) = MM(7,2) = -sx;
|
|
|
|
MM(3,4) = MM(4,3) = sz;
|
|
MM(3,5) = MM(5,3) = -sy;
|
|
MM(3,6) = MM(6,3) = sx;
|
|
|
|
MM(5,6) = MM(6,5) = -xy;
|
|
MM(5,7) = MM(7,5) = -xz;
|
|
MM(6,7) = MM(7,6) = -yz;
|
|
|
|
// Calculate the pose difference estimation
|
|
D = MM.i() * MZ ;
|
|
|
|
// Again going through all point pairs to faster calculate s.
|
|
// This cannot be done earlier as we need D, and therefore MM and MZ to do this
|
|
for(int j = 0; j < m; j++){
|
|
ak = uk[j].p1;
|
|
bk = uk[j].p2;
|
|
|
|
x = (ak.x + bk.x) / 2.0;
|
|
y = (ak.y + bk.y) / 2.0;
|
|
z = (ak.z + bk.z) / 2.0;
|
|
|
|
ss += sqr(ak.x - bk.x - (D(1) + x * D(4) - z * D(6) + y * D(7)))
|
|
+ sqr(ak.y - bk.y - (D(2) + y * D(4) + z * D(5) - x * D(7)))
|
|
+ sqr(ak.z - bk.z - (D(3) + z * D(4) - y * D(5) + x * D(6)));
|
|
}
|
|
|
|
ss = ss / (2*m - 3);
|
|
ss = 1.0 / ss;
|
|
|
|
if (CD) {
|
|
*CD = MZ * ss;
|
|
}
|
|
*C = MM * ss;
|
|
|
|
} else {
|
|
|
|
// This case should not occur
|
|
ss = 0.0;
|
|
MM(1,1) = MM(1,2) = MM(1,3) = 0.0;
|
|
MM(2,1) = MM(2,2) = MM(2,3) = 0.0;
|
|
MM(3,1) = MM(3,2) = MM(3,3) = 0.0;
|
|
MZ(6) = MZ(1) = MZ(2) = MZ(7) = 0.0;
|
|
MZ(3) = MZ(4) = MZ(5) = 0.0;
|
|
*C = 0;
|
|
if(CD)
|
|
*CD = 0;
|
|
cerr << "Error calculating covariance matrix" << endl;
|
|
|
|
}
|
|
}
|
|
|
|
/**
|
|
* A function to fill the linear system G X = B.
|
|
*
|
|
* @param gr the Graph is used to map the given covariances C and CD matrices to the correct link
|
|
* @param CD A vector containing all covariances C multiplied with their respective estimations D
|
|
* @param C A vector containing all covariances C of the pose difference estimations D
|
|
* @param G The matrix G specifying the linear equation
|
|
* @param B The vector B
|
|
*/
|
|
void lum6DQuat::FillGB3D(Graph *gr,Matrix* G, ColumnVector* B, vector<Scan *> allScans)
|
|
{
|
|
#ifdef _OPENMP
|
|
#pragma omp parallel for
|
|
#endif
|
|
for(int i = 0; i < gr->getNrLinks(); i++){
|
|
int a = gr->getLink(i,0) - 1;
|
|
int b = gr->getLink(i,1) - 1;
|
|
Scan *FirstScan = allScans[gr->getLink(i,0)];
|
|
Scan *SecondScan = allScans[gr->getLink(i,1)];
|
|
|
|
// cout << "***i " << i << " a: " << a << " b: " << b << endl;
|
|
|
|
Matrix Cab;
|
|
ColumnVector CDab;
|
|
covarianceQuat(FirstScan, SecondScan, nns_method, (int)my_icp->get_rnd(),
|
|
(int)max_dist_match2_LUM, &Cab, &CDab);
|
|
|
|
if(a >= 0){
|
|
B->Rows(a*7+1,a*7+7) += CDab;
|
|
G->SubMatrix(a*7+1,a*7+7,a*7+1,a*7+7) += Cab;
|
|
}
|
|
if(b >= 0){
|
|
B->Rows(b*7+1,b*7+7) -= CDab;
|
|
G->SubMatrix(b*7+1,b*7+7,b*7+1,b*7+7) += Cab;
|
|
}
|
|
if(a >= 0 && b >= 0) {
|
|
G->SubMatrix(a*7+1,a*7+7,b*7+1,b*7+7) = -Cab;
|
|
G->SubMatrix(b*7+1,b*7+7,a*7+1,a*7+7) = -Cab;
|
|
}
|
|
}
|
|
}
|
|
|
|
/**
|
|
* 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 lum6DQuat::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);
|
|
|
|
double ret = DBL_MAX;
|
|
|
|
for(int iteration = 0;
|
|
iteration < nrIt && ret > epsilonLUM;
|
|
iteration++) {
|
|
|
|
if (nrIt > 1) cout << "Iteration " << iteration << endl;
|
|
|
|
|
|
// * Calculate X and CX from all Dij and Cij
|
|
int n = (gr.getNrScans() - 1);
|
|
|
|
// Construct the linear equation system..
|
|
Matrix G(7*n,7*n);
|
|
ColumnVector B(7*n);
|
|
G = 0.0;
|
|
B = 0.0;
|
|
// ...fill G and B...
|
|
FillGB3D(&gr, &G, &B, allScans);
|
|
// ...and solve it
|
|
ColumnVector X = solveSparseCholesky(G, B);
|
|
|
|
//cout << "X done!" << endl;
|
|
|
|
double sum_position_diff = 0.0;
|
|
|
|
// Start with second Scan
|
|
int loop_end = gr.getNrScans();
|
|
#ifdef _OPENMP
|
|
#pragma omp parallel for reduction(+:sum_position_diff)
|
|
#endif
|
|
for(int i = 1; i < loop_end; i++){
|
|
|
|
// Now update the Poses
|
|
Matrix Ha = IdentityMatrix(7);
|
|
|
|
double xa = allScans[i]->get_rPos()[0];
|
|
double ya = allScans[i]->get_rPos()[1];
|
|
double za = allScans[i]->get_rPos()[2];
|
|
|
|
double p = allScans[i]->get_rPosQuat()[0];
|
|
double q = allScans[i]->get_rPosQuat()[1];
|
|
double r = allScans[i]->get_rPosQuat()[2];
|
|
double s = allScans[i]->get_rPosQuat()[3];
|
|
|
|
double px = p * xa;
|
|
double py = p * ya;
|
|
double pz = p * za;
|
|
|
|
double qx = q * xa;
|
|
double qy = q * ya;
|
|
double qz = q * za;
|
|
|
|
double rx = r * xa;
|
|
double ry = r * ya;
|
|
double rz = r * za;
|
|
|
|
double sx = s * xa;
|
|
double sy = s * ya;
|
|
double sz = s * za;
|
|
|
|
// Fill Ha
|
|
Ha.element(3,3) = 2 * p;
|
|
Ha.element(4,3) = 2 * q;
|
|
Ha.element(5,3) = 2 * r;
|
|
Ha.element(6,3) = 2 * s;
|
|
|
|
Ha.element(3,4) = 2 * q;
|
|
Ha.element(4,4) = -2 * p;
|
|
Ha.element(5,4) = -2 * s;
|
|
Ha.element(6,4) = 2 * r;
|
|
|
|
Ha.element(3,5) = 2 * r;
|
|
Ha.element(4,5) = 2 * s;
|
|
Ha.element(5,5) = -2 * p;
|
|
Ha.element(6,5) = -2 * q;
|
|
|
|
Ha.element(3,6) = 2 * s;
|
|
Ha.element(4,6) = -2 * r;
|
|
Ha.element(5,6) = 2 * q;
|
|
Ha.element(6,6) = -2 * p;
|
|
|
|
Ha.element(0,3) = -2 * (px + sy - rz);
|
|
Ha.element(1,3) = -2 * (-sx + py + qz);
|
|
Ha.element(2,3) = -2 * (rx - qy + pz);
|
|
|
|
Ha.element(0,4) = -2 * (qx + ry + sz);
|
|
Ha.element(1,4) = -2 * (-rx + qy - pz);
|
|
Ha.element(2,4) = -2 * (-sx + py + qz);
|
|
|
|
Ha.element(0,5) = -2 * (rx - qy + pz);
|
|
Ha.element(1,5) = -2 * (qx + ry + sz);
|
|
Ha.element(2,5) = -2 * (-px - sy + rz);
|
|
|
|
Ha.element(0,6) = -2 * (sx - py - qz);
|
|
Ha.element(1,6) = -2 * (px + sy - rz);
|
|
Ha.element(2,6) = -2 * (qx + ry + sz);
|
|
|
|
// Invert it
|
|
Ha = Ha.i();
|
|
|
|
// Get pose estimate
|
|
ColumnVector Xtmp = X.Rows((i-1)*7+1,(i-1)*7+7);
|
|
|
|
// Correct pose estimate
|
|
ColumnVector result = Ha * Xtmp;
|
|
|
|
if(!quiet) {
|
|
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]
|
|
<< " p: " << allScans[i]->get_rPosQuat()[0]
|
|
<< " q: " << allScans[i]->get_rPosQuat()[1]
|
|
<< " r: " << allScans[i]->get_rPosQuat()[2]
|
|
<< " s: " << allScans[i]->get_rPosQuat()[3]
|
|
<< endl;
|
|
}
|
|
|
|
double rPos[3];
|
|
double rPosQuat[4];
|
|
|
|
// calculate the updated Pose
|
|
for (int k = 0; k < 3; k++) {
|
|
rPos[k] = allScans[i]->get_rPos()[k] - result.element(k);
|
|
}
|
|
|
|
double qtmp[4];
|
|
qtmp[0] = result.element(3);
|
|
qtmp[1] = result.element(4);
|
|
qtmp[2] = result.element(5);
|
|
qtmp[3] = result.element(6);
|
|
|
|
for (int k = 0; k < 4; k++) {
|
|
rPosQuat[k] = allScans[i]->get_rPosQuat()[k] - qtmp[k];
|
|
}
|
|
|
|
Normalize4(rPosQuat);
|
|
|
|
// Update the Pose
|
|
if (i != gr.getNrScans() - 1) {
|
|
allScans[i]->transformToQuat(rPos, rPosQuat, Scan::LUM, 1);
|
|
} else {
|
|
allScans[i]->transformToQuat(rPos, rPosQuat, Scan::LUM, 2);
|
|
}
|
|
|
|
if(!quiet) {
|
|
cout << "x: " << allScans[i]->get_rPos()[0]
|
|
<< " y: " << allScans[i]->get_rPos()[1]
|
|
<< " z: " << allScans[i]->get_rPos()[2]
|
|
<< " p: " << allScans[i]->get_rPosQuat()[0]
|
|
<< " q: " << allScans[i]->get_rPosQuat()[1]
|
|
<< " r: " << allScans[i]->get_rPosQuat()[2]
|
|
<< " s: " << allScans[i]->get_rPosQuat()[3]
|
|
<< endl << endl;
|
|
}
|
|
|
|
double x[3];
|
|
x[0] = result.element(0);
|
|
x[1] = result.element(1);
|
|
x[2] = result.element(2);
|
|
sum_position_diff += Len(x);
|
|
}
|
|
cout << "Sum of Position differences = " << sum_position_diff << endl;
|
|
ret = (sum_position_diff / (double)gr.getNrScans());
|
|
}
|
|
|
|
return ret;
|
|
}
|
|
|
|
|