3dpcp/.svn/pristine/94/940861b51daaf2ac827a498a2ac881b29f2875aa.svn-base
2012-09-16 14:33:11 +02:00

106 lines
2.6 KiB
Text

/** @file
* @brief Implementation of the virtual functor for graphslam
* @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany.
*/
#ifndef __GRAPHSLAM_6D_H__
#define __GRAPHSLAM_6D_H__
#include <vector>
using std::vector;
#include <map>
using std::map;
/*#include <pair>
using std::pair;*/
#include "icp6D.h"
#include "graph.h"
#include "newmat/newmatio.h"
#include "sparse/csparse.h"
typedef vector <PtPair> vPtPair; ///< just a typedef: vPtPair = vector of type PtPair
using namespace NEWMAT;
typedef pair<unsigned int, unsigned int> uipair;
typedef pair< uipair, Matrix* > uimpair;
class GraphMatrix {
public:
void add(const unsigned int i, const unsigned int j, Matrix &Cij);
void subtract(const unsigned int i, const unsigned int j, Matrix &Cij);
void print() ;
void convertToCS(cs* T);
~GraphMatrix();
private:
map< uipair, Matrix* > matrix;
map< uipair, Matrix* >::iterator it;
};
class graphSlam6D {
public:
/**
* Constructor
*/
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);
/**
* Destructor
*/
virtual ~graphSlam6D();
virtual double doGraphSlam6D(Graph gr, vector <Scan*> MetaScan, int nrIt) = 0;
void matchGraph6Dautomatic(vector <Scan*> MetaScan, int nrIt, double cldist, int loopsize);
void matchGraph6Dautomatic(vector <Scan*> MetaScan, int nrIt, int clpairs, int loopsize);
Graph *computeGraph6Dautomatic(vector <Scan *> allScans, int clpairs);
NEWMAT::ColumnVector solveSparseCholesky(GraphMatrix *G, const NEWMAT::ColumnVector &B);
NEWMAT::ColumnVector solveSparseCholesky(const NEWMAT::Matrix &G, const NEWMAT::ColumnVector &B);
NEWMAT::ColumnVector solveSparseQR(const NEWMAT::Matrix &G, const NEWMAT::ColumnVector &B);
NEWMAT::ColumnVector solveCholesky(const NEWMAT::Matrix &G, const NEWMAT::ColumnVector &B);
NEWMAT::ColumnVector solve(const NEWMAT::Matrix &G, const NEWMAT::ColumnVector &B);
void writeMatrixPGM(const NEWMAT::Matrix &G);
void set_mdmll(double mdmll);
inline void set_quiet(bool _quiet) { quiet = _quiet;};
protected:
/**
* pointer to the ICP framework
*/
icp6D *my_icp;
/**
* the epsilon for LUM
*/
double epsilonLUM;
/**
* the maximal distance (^2 !!!) for matching in LUM
*/
double max_dist_match2_LUM;
/**
* indicates the NNS method being used
*/
int nns_method;
/**
* be quiet
*/
bool quiet;
long ctime;
};
#endif