3dpcp/.svn/pristine/c2/c246459b04658455b330bed65dcf91f79634da47.svn-base

481 lines
13 KiB
Text
Raw Normal View History

2012-09-16 12:33:11 +00:00
/*
* 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;
}