107 lines
2.6 KiB
Text
107 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
|