3dpcp/.svn/pristine/2c/2ce0727ff133f9885eb9ca366c2a315a26cdd466.svn-base
2012-09-16 14:33:11 +02:00

490 lines
12 KiB
Text

/*
* graphSlam6D implementation
*
* Copyright (C) Dorit Borrmann, Jan Elseberg, Kai Lingemann, Andreas Nuechter
*
* Released under the GPL version 3.
*
*/
/**
* @file
* @brief The implementation of globally consistent scan matching algorithm
* @author Dorit Borrman. Institute of Computer Science, University of Osnabrueck, Germany.
* @author Jan Elseberg. Institute of Computer Science, University of Osnabrueck, Germany.
* @author Kai Lingemann. 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/graphSlam6D.h"
#include "sparse/csparse.h"
#include <cfloat>
#include <fstream>
using std::ofstream;
using std::flush;
#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 to use
* @param epsilonLUM Termination criterion for LUM
*/
graphSlam6D::graphSlam6D(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)
{
this->nns_method = nns_method;
this->quiet = quiet;
this->epsilonLUM = epsilonLUM;
this->max_dist_match2_LUM = sqr(max_dist_match);
ctime = 0;
this->my_icp = new icp6D(my_icp6Dminimizer, mdm, max_num_iterations,
quiet, meta, rnd, eP, anim, epsilonICP, nns_method);
}
graphSlam6D::~graphSlam6D()
{
cout << "Time spent in the SLAM backend:" << ctime << endl;
}
/**
* 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 allScans Contains all laser scans
* @param nrIt The number of iterations the LUM-algorithm will run
* @param clpairs minimal number of points aximal distance for closing loops
* @param loopsize minimal loop size
*/
void graphSlam6D::matchGraph6Dautomatic(vector <Scan *> allScans, int nrIt, int clpairs, int loopsize)
{
// the IdentityMatrix to transform some Scans with
double id[16];
M4identity(id);
Graph *gr = 0;
int i = 0;
do {
cout << "Generate graph ... " << flush;
i++;
if (gr) delete gr;
gr = new Graph(0, false);
int j, maxj = (int)allScans.size();
#ifdef _OPENMP
omp_set_num_threads(OPENMP_NUM_THREADS);
#pragma omp parallel for schedule(dynamic)
#endif
for (j = 0; j < maxj; j++) {
#ifdef _OPENMP
int thread_num = omp_get_thread_num();
#else
int thread_num = 0;
#endif
for (int k = 0; k < (int)allScans.size(); k++) {
if (j == k) continue;
Scan * FirstScan = allScans[j];
Scan * SecondScan = allScans[k];
double centroid_d[3] = {0.0, 0.0, 0.0};
double centroid_m[3] = {0.0, 0.0, 0.0};
vPtPair temp;
double sum_dummy;
Scan::getPtPairs(&temp, FirstScan, SecondScan, thread_num,
my_icp->get_rnd(), (int)max_dist_match2_LUM, sum_dummy,
centroid_m, centroid_d);
if ((int)temp.size() > clpairs) {
#ifdef _OPENMP
#pragma omp critical
#endif
gr->addLink(j, k);
}
}
}
cout << "done" << endl;
} while ((doGraphSlam6D(*gr, allScans, 1) > 0.001) && (i < nrIt));
return;
}
Graph *graphSlam6D::computeGraph6Dautomatic(vector <Scan *> allScans, int clpairs)
{
// the IdentityMatrix to transform some Scans with
double id[16];
M4identity(id);
int i = 0;
cout << "Generate graph ... " << flush;
i++;
Graph *gr = new Graph(0, false);
int j, maxj = (int)allScans.size();
#ifdef _OPENMP
omp_set_num_threads(OPENMP_NUM_THREADS);
#pragma omp parallel for schedule(dynamic)
#endif
for (j = 0; j < maxj; j++) {
#ifdef _OPENMP
int thread_num = omp_get_thread_num();
#else
int thread_num = 0;
#endif
for (int k = 0; k < (int)allScans.size(); k++) {
if (j == k) continue;
Scan * FirstScan = allScans[j];
Scan * SecondScan = allScans[k];
double centroid_d[3] = {0.0, 0.0, 0.0};
double centroid_m[3] = {0.0, 0.0, 0.0};
vPtPair temp;
double sum_dummy;
Scan::getPtPairs(&temp, FirstScan, SecondScan, thread_num,
my_icp->get_rnd(), (int)max_dist_match2_LUM, sum_dummy,
centroid_m, centroid_d);
if ((int)temp.size() > clpairs) {
#ifdef _OPENMP
#pragma omp critical
#endif
gr->addLink(j, k);
}
}
}
cout << "done" << endl;
return gr;
}
/**
* This function is used to solve the system of linear eq.
*
* @param G symmetric, positive definite Matrix, thus invertable
* @param B column vector
*/
void graphSlam6D::writeMatrixPGM(const Matrix &G)
{
int n = G.Ncols();
static int matrixnum = 0;
string mf = "matrix" + to_string(matrixnum,4) + ".pgm";
ofstream matrixout(mf.c_str());
matrixout << "P2" << endl
<< "# CREATOR slam6D (c) Andreas Nuechter, 05/2007" << endl
<< n << " " << n << endl
<< 255 << endl;
for (int i = 0; i < n; i++) {
for (int j = 0; j < n; j++) {
if (G.element(i, j) > 0.001) {
matrixout << 0 << " ";
} else {
matrixout << 255 << " ";
}
}
matrixout << endl;
}
// matrixout << G << endl;
matrixout.close();
matrixout.clear();
matrixnum++;
}
/**
* This function is used to solve the system of linear eq.
*
* @param G Matrix, invertable
* @param B column vector
*/
ColumnVector graphSlam6D::solve(const Matrix &G, const ColumnVector &B)
{
#ifdef WRITE_MATRIX_PGM
writeMatrixPGM(G);
#endif
// ----------------------------------
// solve eqn via inverting the matrix
// ----------------------------------
return (ColumnVector)(G.i() * B);
}
/**
* This function is used to solve the system of linear eq.
* The implementation from the numerical recepies are used
*
* @param G symmetric, positive definite Matrix, thus invertable
* @param B column vector
*/
ColumnVector graphSlam6D::solveCholesky(const Matrix &G, const ColumnVector &B)
{
#ifdef WRITE_MATRIX_PGM
writeMatrixPGM(G);
#endif
// We copy the newmat matrices and use our own
// Cholesky decomposition code here. The Cholesky
// decomosition is based on the Numerical Recipes in C.
// This speed ups computation time
// copy values
int n = G.Ncols();
double **A = new double*[n];
double *C = new double[n];
double *diag = new double[n];
double *x = new double[n];
ColumnVector X(n);
for (int i = 0; i < n; i++) {
A[i] = new double[n];
for (int j = 0; j < n; j++) {
A[i][j] = G.element(i, j);
}
C[i] = B.element(i);
}
// --------------------------------------------------
// make cholesky dekomposition with numerical recipes
// --------------------------------------------------
if (!choldc(n, A, diag)) {
cout << "cannot perfom cholesky decomposition" << endl;
}
// solve A x = C
cholsl(n, A, diag, C, x);
// copy values back
for (int i = 0; i < n; i++) {
X.element(i) = x[i];
}
// clean up
for (int i = 0; i < n; i++) {
delete [] A[i];
}
delete [] x;
delete [] diag;
delete [] C;
delete [] A;
return X;
}
/**
* This function is used to solve the system of linear eq.
*
* @param G symmetric, positive definite Matrix, thus invertable
* @param B column vector
*/
ColumnVector graphSlam6D::solveSparseCholesky(const Matrix &G, const ColumnVector &B)
{
long starttime = GetCurrentTimeInMilliSec();
#ifdef WRITE_MATRIX_PGM
writeMatrixPGM(G);
#endif
int n = G.Ncols();
// ------------------------------
// Sparse Cholsekey decomposition
// ------------------------------
ColumnVector X(n);
cs *A, *T = cs_spalloc (0, 0, 1, 1, 1) ;
double *x = new double[n];
for (int i = 0; i < n; i++) {
for (int j = 0; j < n; j++) {
if (fabs(G.element(i, j)) > 0.00001) {
cs_entry (T, i, j, G.element(i, j));
}
}
x[i] = B.element(i);
}
A = cs_triplet (T);
cs_dropzeros (A) ; // drop zero entries
cs_cholsol (A, x, 1) ;
// copy values back
for (int i = 0; i < n; i++) {
X.element(i) = x[i];
}
cs_spfree(A);
cs_spfree(T);
delete [] x;
ctime += GetCurrentTimeInMilliSec() - starttime;
return X;
}
ColumnVector graphSlam6D::solveSparseCholesky(GraphMatrix *G, const ColumnVector &B)
{
long starttime = GetCurrentTimeInMilliSec();
int n = B.Nrows();
ColumnVector X(n);
// ------------------------------
// Sparse Cholsekey decomposition
// ------------------------------
cs *A, *T = cs_spalloc (0, 0, 1, 1, 1) ;
double *x = new double[n];
for (int i = 0; i < n; i++) {
x[i] = B.element(i);
}
G->convertToCS(T);
A = cs_triplet (T);
cs_dropzeros (A) ; // drop zero entries
// cs_print(T, 0);
cs_cholsol (A, x, 1) ;
// copy values back
for (int i = 0; i < n; i++) {
X.element(i) = x[i];
}
cs_spfree(A);
cs_spfree(T);
delete [] x;
ctime += GetCurrentTimeInMilliSec() - starttime;
return X;
}
/**
* This function is used to solve the system of linear eq.
*
* @param G invertable Matrix
* @param B column vector
*/
ColumnVector graphSlam6D::solveSparseQR(const Matrix &G, const ColumnVector &B)
{
#ifdef WRITE_MATRIX_PGM
writeMatrixPGM(G);
#endif
int n = B.Ncols();
ColumnVector X(n);
cs *A, *T = cs_spalloc (0, 0, 1, 1, 1) ;
double *x = new double[n];
for (int i = 0; i < n; i++) {
for (int j = 0; j < n; j++) {
if (fabs(G.element(i, j)) > 0.00001) {
cs_entry (T, i, j, G.element(i, j));
}
}
x[i] = B.element(i);
}
A = cs_triplet (T);
cs_dropzeros (A) ; // drop zero entries
int order = 3; // for qr-ordering
cs_qrsol ( A, x, order) ;
// copy values back
for (int i = 0; i < n; i++) {
X.element(i) = x[i];
}
cs_spfree(A);
cs_spfree(T);
delete [] x;
return X;
}
void graphSlam6D::set_mdmll(double mdmll) {
max_dist_match2_LUM = sqr(mdmll);
}
void GraphMatrix::add(const unsigned int i, const unsigned int j, Matrix &Cij) {
uipair ui(i,j);
it = matrix.find( ui );
if (it != matrix.end()) {
(*(it->second)) += Cij;
} else {
Matrix *C = new Matrix(6,6);
*C = Cij;
matrix.insert( uimpair( ui, C));
}
}
void GraphMatrix::subtract(const unsigned int i, const unsigned int j,Matrix &Cij) {
uipair ui(i,j);
it = matrix.find( ui );
if (it != matrix.end()) {
(*it->second) -= Cij;
} else {
Matrix *C = new Matrix(6,6);
*C = Cij;
*C *= -1.0;
matrix.insert( uimpair( ui, C));
}
}
void GraphMatrix::print() {
for ( it = matrix.begin() ; it != matrix.end(); it++ ) {
uimpair uim = *it;
uipair ui = uim.first;
cout << ui.first << " " << ui.second << " :" << endl << *uim.second << endl;
}
}
GraphMatrix::~GraphMatrix() {
for ( it = matrix.begin() ; it != matrix.end(); it++ ) {
uimpair uim = *it;
delete uim.second;
}
}
void GraphMatrix::convertToCS(cs *T) {
unsigned int a,b;
int imin,imax,jmin,jmax;
for ( it = matrix.begin() ; it != matrix.end(); it++ ) {
Matrix *C = it->second;
a = it->first.first;
b = it->first.second;
// cout << a << " " << b << " " << C << endl;
imin = a*6;
jmin = b*6;
imax = a*6 + 6;
jmax = b*6 + 6;
a = b = 0;
for (int i = imin; i < imax; i++, a++) {
b = 0;
for (int j = jmin; j < jmax; j++, b++) {
if (fabs(C->element(a, b)) > 0.00001) {
cs_entry (T, i, j, C->element(a, b));
}
}
}
}
// print();
// cs_print(T, 0);
}