add slam_gmapping
This commit is contained in:
X-lanni
@@ -0,0 +1,5 @@
|
||||
include_directories(./)
|
||||
add_library(utils movement.cpp stat.cpp)
|
||||
install(TARGETS utils DESTINATION lib)
|
||||
|
||||
ament_export_libraries(utils)
|
||||
@@ -0,0 +1,29 @@
|
||||
#include <iostream>
|
||||
#include "autoptr.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace GMapping;
|
||||
|
||||
typedef autoptr<double> DoubleAutoPtr;
|
||||
|
||||
int main(int argc, const char * const * argv){
|
||||
double* d1=new double(10.);
|
||||
double* d2=new double(20.);
|
||||
cout << "Construction test" << endl;
|
||||
DoubleAutoPtr pd1(d1);
|
||||
DoubleAutoPtr pd2(d2);
|
||||
cout << *pd1 << " " << *pd2 << endl;
|
||||
cout << "Copy Construction" << endl;
|
||||
DoubleAutoPtr pd3(pd1);
|
||||
cout << *pd3 << endl;
|
||||
cout << "assignment" << endl;
|
||||
pd3=pd2;
|
||||
pd1=pd2;
|
||||
cout << *pd1 << " " << *pd2 << " " << *pd3 << " " << endl;
|
||||
cout << "conversion operator" << endl;
|
||||
DoubleAutoPtr nullPtr;
|
||||
cout << "conversion operator " << !(nullPtr) << endl;
|
||||
cout << "neg conversion operator " << nullPtr << endl;
|
||||
cout << "conversion operator " << (int)pd1 << endl;
|
||||
cout << "neg conversion operator " << !(pd1) << endl;
|
||||
}
|
||||
@@ -0,0 +1,451 @@
|
||||
#ifndef DATASMOOTHER_H
|
||||
#define DATASMOOTHER_H
|
||||
|
||||
#include <list>
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
#include <values.h>
|
||||
#include "stat.h"
|
||||
#include <assert.h>
|
||||
|
||||
namespace GMapping {
|
||||
|
||||
class DataSmoother {
|
||||
public:
|
||||
struct DataPoint {
|
||||
DataPoint(double _x=0.0, double _y=0.0) { x=_x;y=_y;}
|
||||
double x;
|
||||
double y;
|
||||
};
|
||||
|
||||
typedef std::vector<DataPoint> Data;
|
||||
|
||||
DataSmoother(double parzenWindow) {
|
||||
init(parzenWindow);
|
||||
};
|
||||
|
||||
virtual ~DataSmoother() {
|
||||
m_data.clear();
|
||||
m_cummulated.clear();
|
||||
};
|
||||
|
||||
void init(double parzenWindow) {
|
||||
m_data.clear();
|
||||
m_cummulated.clear();
|
||||
m_int=-1;
|
||||
m_parzenWindow = parzenWindow;
|
||||
m_from = MAXDOUBLE;
|
||||
m_to = -MAXDOUBLE;
|
||||
m_lastStep = 0.001;
|
||||
};
|
||||
|
||||
|
||||
double sqr(double x) {
|
||||
return x*x;
|
||||
}
|
||||
|
||||
|
||||
void setMinToZero() {
|
||||
double minval=MAXDOUBLE;
|
||||
|
||||
for (Data::const_iterator it = m_data.begin(); it != m_data.end(); it++) {
|
||||
const DataPoint& d = *it;
|
||||
if (minval > d.y)
|
||||
minval = d.y;
|
||||
}
|
||||
|
||||
for (Data::iterator it = m_data.begin(); it != m_data.end(); it++) {
|
||||
DataPoint& d = *it;
|
||||
d.y = d.y - minval;
|
||||
}
|
||||
|
||||
m_cummulated.clear();
|
||||
}
|
||||
|
||||
void add(double x, double p) {
|
||||
m_data.push_back(DataPoint(x,p));
|
||||
m_int=-1;
|
||||
|
||||
if (x-3.0*m_parzenWindow < m_from)
|
||||
m_from = x - 3.0*m_parzenWindow;
|
||||
|
||||
if (x+3.0*m_parzenWindow > m_to)
|
||||
m_to = x + 3.0*m_parzenWindow;
|
||||
|
||||
m_cummulated.clear();
|
||||
}
|
||||
|
||||
void integrate(double step) {
|
||||
m_lastStep = step;
|
||||
double sum=0;
|
||||
for (double x=m_from; x<=m_to; x+=step)
|
||||
sum += smoothedData(x)*step;
|
||||
m_int = sum;
|
||||
}
|
||||
|
||||
double integral(double step, double xTo) {
|
||||
double sum=0;
|
||||
for (double x=m_from; x<=xTo; x+=step)
|
||||
sum += smoothedData(x)*step;
|
||||
return sum;
|
||||
}
|
||||
|
||||
|
||||
double smoothedData(double x) {
|
||||
assert( m_data.size() > 0 );
|
||||
|
||||
double p=0;
|
||||
double sum_y=0;
|
||||
for (Data::const_iterator it = m_data.begin(); it != m_data.end(); it++) {
|
||||
const DataPoint& d = *it;
|
||||
double dist = fabs(x - d.x);
|
||||
p += d.y * exp( -0.5 * sqr ( dist/m_parzenWindow ) );
|
||||
sum_y += d.y;
|
||||
}
|
||||
double denom = sqrt(2.0 * M_PI) * (sum_y) * m_parzenWindow;
|
||||
p *= 1./denom;
|
||||
|
||||
return p;
|
||||
}
|
||||
|
||||
double sampleNumeric(double step) {
|
||||
|
||||
assert( m_data.size() > 0 );
|
||||
|
||||
if (m_int <0 || step != m_lastStep)
|
||||
integrate(step);
|
||||
|
||||
double r = sampleUniformDouble(0.0, m_int);
|
||||
double sum2=0;
|
||||
for (double x=m_from; x<=m_to; x+=step) {
|
||||
sum2 += smoothedData(x)*step;
|
||||
if (sum2 > r)
|
||||
return x-0.5*step;
|
||||
}
|
||||
return m_to;
|
||||
}
|
||||
|
||||
void computeCummuated() {
|
||||
assert( m_data.size() > 0 );
|
||||
m_cummulated.resize(m_data.size());
|
||||
std::vector<double>::iterator cit = m_cummulated.begin();
|
||||
double sum=0;
|
||||
for (Data::const_iterator it = m_data.begin(); it != m_data.end(); ++it) {
|
||||
sum += it->y;
|
||||
(*cit) = sum;
|
||||
++cit;
|
||||
}
|
||||
}
|
||||
|
||||
double sample() {
|
||||
|
||||
assert( m_data.size() > 0 );
|
||||
|
||||
if (m_cummulated.size() == 0) {
|
||||
computeCummuated();
|
||||
}
|
||||
double maxval = m_cummulated.back();
|
||||
|
||||
double random = sampleUniformDouble(0.0, maxval);
|
||||
int nCum = (int) m_cummulated.size();
|
||||
double sum=0;
|
||||
int i=0;
|
||||
while (i<nCum) {
|
||||
sum += m_cummulated[i];
|
||||
|
||||
if (sum >= random) {
|
||||
return m_data[i].x + sampleGaussian(m_parzenWindow);
|
||||
}
|
||||
i++;
|
||||
}
|
||||
assert(0);
|
||||
}
|
||||
|
||||
|
||||
void sampleMultiple(std::vector<double>& samples, int num) {
|
||||
|
||||
assert( m_data.size() > 0 );
|
||||
samples.clear();
|
||||
|
||||
if (m_cummulated.size() == 0) {
|
||||
computeCummuated();
|
||||
}
|
||||
double maxval = m_cummulated.back();
|
||||
|
||||
std::vector<double> randoms(num);
|
||||
for (int i=0; i<num; i++)
|
||||
randoms[i] = sampleUniformDouble(0.0, maxval);
|
||||
|
||||
std::sort(randoms.begin(), randoms.end());
|
||||
|
||||
int nCum = (int) m_cummulated.size();
|
||||
|
||||
double sum=0;
|
||||
int i=0;
|
||||
int j=0;
|
||||
while (i<nCum && j < num) {
|
||||
sum += m_cummulated[i];
|
||||
|
||||
while (sum >= randoms[j] && j < num) {
|
||||
samples.push_back( m_data[i].x + sampleGaussian(m_parzenWindow) );
|
||||
j++;
|
||||
}
|
||||
i++;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
void approxGauss(double step, double* mean, double* sigma) {
|
||||
|
||||
assert( m_data.size() > 0 );
|
||||
|
||||
double sum=0;
|
||||
double d=0;
|
||||
|
||||
*mean=0;
|
||||
for (double x=m_from; x<=m_to; x+=step) {
|
||||
d = smoothedData(x);
|
||||
sum += d;
|
||||
*mean += x*d;
|
||||
}
|
||||
*mean /= sum;
|
||||
|
||||
double var=0;
|
||||
for (double x=m_from; x<=m_to; x+=step) {
|
||||
d = smoothedData(x);
|
||||
var += sqr(x-*mean) * d;
|
||||
}
|
||||
var /= sum;
|
||||
|
||||
*sigma = sqrt(var);
|
||||
}
|
||||
|
||||
double gauss(double x, double mean, double sigma) {
|
||||
return 1.0/(sqrt(2.0*M_PI)*sigma) * exp(-0.5 * sqr( (x-mean)/sigma));
|
||||
}
|
||||
|
||||
double cramerVonMisesToGauss(double step, double mean, double sigma) {
|
||||
|
||||
double p=0;
|
||||
double s=0;
|
||||
double g=0;
|
||||
double sint=0;
|
||||
double gint=0;
|
||||
|
||||
for (double x=m_from; x<=m_to; x+=step) {
|
||||
s = smoothedData(x);
|
||||
sint += s * step;
|
||||
|
||||
g = gauss(x, mean, sigma);
|
||||
gint += g * step;
|
||||
|
||||
p += sqr( (sint - gint) );
|
||||
}
|
||||
|
||||
return p;
|
||||
}
|
||||
|
||||
double kldToGauss(double step, double mean, double sigma) {
|
||||
|
||||
double p=0;
|
||||
double d=0;
|
||||
double g=0;
|
||||
|
||||
double sd=0;
|
||||
double sg=0;
|
||||
|
||||
for (double x=m_from; x<=m_to; x+=step) {
|
||||
|
||||
d = 1e-10 + smoothedData(x);
|
||||
g = 1e-10 + gauss(x, mean, sigma);
|
||||
|
||||
sd += d;
|
||||
sg += g;
|
||||
|
||||
p += d * log(d/g);
|
||||
}
|
||||
|
||||
sd *= step;
|
||||
sg *= step;
|
||||
|
||||
if (fabs(sd-sg) > 0.1)
|
||||
assert(0);
|
||||
|
||||
p *= step;
|
||||
return p;
|
||||
}
|
||||
|
||||
|
||||
void gnuplotDumpData(FILE* fp) {
|
||||
for (Data::const_iterator it = m_data.begin(); it != m_data.end(); it++) {
|
||||
const DataPoint& d = *it;
|
||||
fprintf(fp, "%f %f\n", d.x, d.y);
|
||||
}
|
||||
}
|
||||
|
||||
void gnuplotDumpSmoothedData(FILE* fp, double step) {
|
||||
for (double x=m_from; x<=m_to; x+=step)
|
||||
fprintf(fp, "%f %f\n", x, smoothedData(x));
|
||||
}
|
||||
|
||||
protected:
|
||||
Data m_data;
|
||||
std::vector<double> m_cummulated;
|
||||
double m_int;
|
||||
double m_lastStep;
|
||||
|
||||
double m_parzenWindow;
|
||||
double m_from;
|
||||
double m_to;
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
/* class DataSmoother3D { */
|
||||
/* public: */
|
||||
/* struct InputPoint { */
|
||||
/* InputPoint(double _x=0.0, double _y=0.0, double _t=0.0) { x=_x;y=_y;t=_t;} */
|
||||
/* double x; */
|
||||
/* double y; */
|
||||
/* double t; */
|
||||
/* }; */
|
||||
|
||||
/* struct DataPoint { */
|
||||
/* DataPoint(const InputPoint& _p, double _val=0.0;) { p=_p;val=_val;} */
|
||||
/* InputPoint p; */
|
||||
/* double val; */
|
||||
/* }; */
|
||||
|
||||
/* typedef std::list<DataPoint> Data; */
|
||||
|
||||
/* DataSmoother(double parzenWindow) { */
|
||||
/* m_int=-1; */
|
||||
/* m_parzenWindow = parzenWindow; */
|
||||
/* m_from = InputPoint(MAXDOUBLE,MAXDOUBLE,MAXDOUBLE); */
|
||||
/* m_to = InputPoint(-MAXDOUBLE,-MAXDOUBLE,-MAXDOUBLE); */
|
||||
/* }; */
|
||||
|
||||
/* virtual ~DataSmoother() { */
|
||||
/* m_data.clear(); */
|
||||
/* }; */
|
||||
|
||||
/* double sqr(double x) { */
|
||||
/* return x*x; */
|
||||
/* } */
|
||||
|
||||
|
||||
/* void setMinToZero() { */
|
||||
/* double minval=MAXDOUBLE; */
|
||||
/* for (Data::const_iterator it = m_data.begin(); it != m_data.end(); it++) { */
|
||||
/* const DataPoint& d = *it; */
|
||||
/* if (minval > d.val) */
|
||||
/* minval = d.val; */
|
||||
/* } */
|
||||
|
||||
/* for (Data::iterator it = m_data.begin(); it != m_data.end(); it++) { */
|
||||
/* DataPoint& d = *it; */
|
||||
/* d.val = d.val - minval; */
|
||||
/* } */
|
||||
|
||||
/* } */
|
||||
|
||||
/* void add(double x, double y, double t, double v) { */
|
||||
/* m_data.push_back(DataPoint(InputPoint(x,y,t),v)); */
|
||||
/* m_int=-1; */
|
||||
|
||||
/* if (x-3.0*m_parzenWindow < m_from.x) */
|
||||
/* m_from.x = x - 3.0*m_parzenWindow.x; */
|
||||
/* if (x+3.0*m_parzenWindow.x > m_to.x) */
|
||||
/* m_to.x = x + 3.0*m_parzenWindow.x; */
|
||||
|
||||
/* if (y-3.0*m_parzenWindow < m_from.y) */
|
||||
/* m_from.y = y - 3.0*m_parzenWindow.y; */
|
||||
/* if (y+3.0*m_parzenWindow.y > m_to.y) */
|
||||
/* m_to.y = y + 3.0*m_parzenWindow.y; */
|
||||
|
||||
/* if (t-3.0*m_parzenWindow < m_from.t) */
|
||||
/* m_from.t = t - 3.0*m_parzenWindow.t; */
|
||||
/* if (t+3.0*m_parzenWindow.t > m_to.t) */
|
||||
/* m_to.t = t + 3.0*m_parzenWindow.t; */
|
||||
/* } */
|
||||
|
||||
/* void integrate(InputPoint step) { */
|
||||
/* m_lastStep = step; */
|
||||
/* double sum=0; */
|
||||
/* for (double x=m_from.x; x<=m_to.x; x+=step.x) { */
|
||||
/* for (double y=m_from.y; x<=m_to.y; y+=step.y) { */
|
||||
/* for (double t=m_from.t; t<=m_to.t; t+=step.t) { */
|
||||
/* sum += smoothedData(InputPoint(x,y,t)) * step.x * step.y * step.t; */
|
||||
/* } */
|
||||
/* } */
|
||||
/* } */
|
||||
/* m_int = sum; */
|
||||
/* } */
|
||||
|
||||
|
||||
/* double smoothedData(InputPoint pnt) { */
|
||||
/* assert( m_data.size() > 0 ); */
|
||||
/* double p=0; */
|
||||
/* double sum_y=0; */
|
||||
/* for (Data::const_iterator it = m_data.begin(); it != m_data.end(); it++) { */
|
||||
/* const DataPoint& d = *it; */
|
||||
/* double u = sqr( (pnt.x-d.x)/m_parzenWindow.x) + */
|
||||
/* sqr((pnt.y-d.y)/m_parzenWindow.y) + */
|
||||
/* sqr((pnt.t-d.t)/m_parzenWindow.t); */
|
||||
/* p += d.val * exp( -0.5 * u); */
|
||||
/* sum_y += d.y; */
|
||||
/* } */
|
||||
/* double denom = sqr(m_parzenWindow.x)*sqr(m_parzenWindow.x)*sqr(m_parzenWindow.x) * (sum_y) * */
|
||||
/* sqrt(sqr(m_parzenWindow.x) + sqr(m_parzenWindow.y) + sqr(m_parzenWindow.t)); */
|
||||
/* p *= 1./denom; */
|
||||
|
||||
/* return p; */
|
||||
/* } */
|
||||
|
||||
/* double sample(const InputPoint& step) { */
|
||||
|
||||
/* assert( m_data.size() > 0 ); */
|
||||
|
||||
/* if (m_int <0 || step != m_lastStep) */
|
||||
/* integrate(step); */
|
||||
|
||||
/* double r = sampleUniformDouble(0.0, m_int); */
|
||||
/* double sum2=0; */
|
||||
/* for (double x=m_from; x<=m_to; x+=step) { */
|
||||
/* sum2 += smoothedData(x)*step; */
|
||||
/* if (sum2 > r) */
|
||||
/* return x-0.5*step; */
|
||||
/* } */
|
||||
/* return m_to; */
|
||||
/* } */
|
||||
|
||||
/* void gnuplotDumpData(FILE* fp) { */
|
||||
/* for (Data::const_iterator it = m_data.begin(); it != m_data.end(); it++) { */
|
||||
/* const DataPoint& d = *it; */
|
||||
/* fprintf(fp, "%f %f %f %f\n", d.x, d.y, d.t, d.val); */
|
||||
/* } */
|
||||
/* } */
|
||||
|
||||
/* void gnuplotDumpSmoothedData(FILE* fp, double step) { */
|
||||
/* for (double x=m_from; x<=m_to; x+=step) */
|
||||
/* fprintf(fp, "%f %f %f %f\n", x, ,y, t, smoothedData(x,y,t)); */
|
||||
/* } */
|
||||
|
||||
/* protected: */
|
||||
/* Data m_data; */
|
||||
/* vector<double> m_intdata; */
|
||||
/* double m_int; */
|
||||
/* double m_lastStep; */
|
||||
|
||||
/* double m_parzenWindow; */
|
||||
/* double m_from; */
|
||||
/* double m_to; */
|
||||
|
||||
/* }; */
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,232 @@
|
||||
#ifndef DMATRIX_HXX
|
||||
#define DMATRIX_HXX
|
||||
|
||||
#include <iostream>
|
||||
#include <exception>
|
||||
namespace GMapping {
|
||||
|
||||
class DNotInvertibleMatrixException: public std::exception {};
|
||||
class DIncompatibleMatrixException: public std::exception {};
|
||||
class DNotSquareMatrixException: public std::exception {};
|
||||
|
||||
template <class X> class DMatrix {
|
||||
public:
|
||||
DMatrix(int n=0,int m=0);
|
||||
~DMatrix();
|
||||
|
||||
DMatrix(const DMatrix&);
|
||||
DMatrix& operator=(const DMatrix&);
|
||||
|
||||
X * operator[](int i) {
|
||||
if ((*shares)>1) detach();
|
||||
return mrows[i];
|
||||
}
|
||||
|
||||
const X * operator[](int i) const { return mrows[i]; }
|
||||
|
||||
const X det() const;
|
||||
DMatrix inv() const;
|
||||
DMatrix transpose() const;
|
||||
DMatrix operator*(const DMatrix&) const;
|
||||
DMatrix operator+(const DMatrix&) const;
|
||||
DMatrix operator-(const DMatrix&) const;
|
||||
DMatrix operator*(const X&) const;
|
||||
|
||||
int rows() const { return nrows; }
|
||||
int columns() const { return ncols; }
|
||||
|
||||
void detach();
|
||||
|
||||
static DMatrix I(int);
|
||||
|
||||
protected:
|
||||
int nrows,ncols;
|
||||
X * elems;
|
||||
X ** mrows;
|
||||
|
||||
int * shares;
|
||||
};
|
||||
|
||||
template <class X> DMatrix<X>::DMatrix(int n,int m) {
|
||||
if (n<1) n=1;
|
||||
if (m<1) m=1;
|
||||
nrows=n;
|
||||
ncols=m;
|
||||
elems=new X[nrows*ncols];
|
||||
mrows=new X* [nrows];
|
||||
for (int i=0;i<nrows;i++) mrows[i]=elems+ncols*i;
|
||||
for (int i=0;i<nrows*ncols;i++) elems[i]=X(0);
|
||||
shares=new int;
|
||||
(*shares)=1;
|
||||
}
|
||||
|
||||
template <class X> DMatrix<X>::~DMatrix() {
|
||||
if (--(*shares)) return;
|
||||
delete [] elems;
|
||||
delete [] mrows;
|
||||
delete shares;
|
||||
}
|
||||
|
||||
template <class X> DMatrix<X>::DMatrix(const DMatrix& m) {
|
||||
shares=m.shares;
|
||||
elems=m.elems;
|
||||
nrows=m.nrows;
|
||||
ncols=m.ncols;
|
||||
mrows=m.mrows;
|
||||
(*shares)++;
|
||||
}
|
||||
|
||||
template <class X> DMatrix<X>& DMatrix<X>::operator=(const DMatrix& m) {
|
||||
if (!--(*shares)) {
|
||||
delete [] elems;
|
||||
delete [] mrows;
|
||||
delete shares;
|
||||
}
|
||||
shares=m.shares;
|
||||
elems=m.elems;
|
||||
nrows=m.nrows;
|
||||
ncols=m.ncols;
|
||||
mrows=m.mrows;
|
||||
(*shares)++;
|
||||
return *this;
|
||||
}
|
||||
|
||||
template <class X> DMatrix<X> DMatrix<X>::inv() const {
|
||||
if (nrows!=ncols) throw DNotInvertibleMatrixException();
|
||||
DMatrix<X> aux1(*this),aux2(I(nrows));
|
||||
aux1.detach();
|
||||
for (int i=0;i<nrows;i++) {
|
||||
int k=i;
|
||||
for (;k<nrows&&aux1.mrows[k][i]==X(0);k++);
|
||||
if (k>=nrows) throw DNotInvertibleMatrixException();
|
||||
X val=aux1.mrows[k][i];
|
||||
for (int j=0;j<nrows;j++) {
|
||||
aux1.mrows[k][j]=aux1.mrows[k][j]/val;
|
||||
aux2.mrows[k][j]=aux2.mrows[k][j]/val;
|
||||
}
|
||||
if (k!=i) {
|
||||
for (int j=0;j<nrows;j++) {
|
||||
X tmp=aux1.mrows[k][j];
|
||||
aux1.mrows[k][j]=aux1.mrows[i][j];
|
||||
aux1.mrows[i][j]=tmp;
|
||||
tmp=aux2.mrows[k][j];
|
||||
aux2.mrows[k][j]=aux2.mrows[i][j];
|
||||
aux2.mrows[i][j]=tmp;
|
||||
}
|
||||
}
|
||||
for (int j=0;j<nrows;j++)
|
||||
if (j!=i) {
|
||||
X tmp=aux1.mrows[j][i];
|
||||
for (int l=0;l<nrows;l++) {
|
||||
aux1.mrows[j][l]=aux1.mrows[j][l]-tmp*aux1.mrows[i][l];
|
||||
aux2.mrows[j][l]=aux2.mrows[j][l]-tmp*aux2.mrows[i][l];
|
||||
}
|
||||
}
|
||||
}
|
||||
return aux2;
|
||||
}
|
||||
|
||||
template <class X> const X DMatrix<X>::det() const {
|
||||
if (nrows!=ncols) throw DNotSquareMatrixException();
|
||||
DMatrix<X> aux(*this);
|
||||
X d=X(1);
|
||||
aux.detach();
|
||||
for (int i=0;i<nrows;i++) {
|
||||
int k=i;
|
||||
for (;k<nrows&&aux.mrows[k][i]==X(0);k++);
|
||||
if (k>=nrows) return X(0);
|
||||
X val=aux.mrows[k][i];
|
||||
for (int j=0;j<nrows;j++) {
|
||||
aux.mrows[k][j]/=val;
|
||||
}
|
||||
d=d*val;
|
||||
if (k!=i) {
|
||||
for (int j=0;j<nrows;j++) {
|
||||
X tmp=aux.mrows[k][j];
|
||||
aux.mrows[k][j]=aux.mrows[i][j];
|
||||
aux.mrows[i][j]=tmp;
|
||||
}
|
||||
d=-d;
|
||||
}
|
||||
for (int j=i+1;j<nrows;j++){
|
||||
X tmp=aux.mrows[j][i];
|
||||
if (!(tmp==X(0)) ){
|
||||
for (int l=0;l<nrows;l++) {
|
||||
aux.mrows[j][l]=aux.mrows[j][l]-tmp*aux.mrows[i][l];
|
||||
}
|
||||
//d=d*tmp;
|
||||
}
|
||||
}
|
||||
}
|
||||
return d;
|
||||
}
|
||||
|
||||
template <class X> DMatrix<X> DMatrix<X>::transpose() const {
|
||||
DMatrix<X> aux(ncols, nrows);
|
||||
for (int i=0; i<nrows; i++)
|
||||
for (int j=0; j<ncols; j++)
|
||||
aux[j][i]=mrows[i][j];
|
||||
return aux;
|
||||
}
|
||||
|
||||
template <class X> DMatrix<X> DMatrix<X>::operator*(const DMatrix<X>& m) const {
|
||||
if (ncols!=m.nrows) throw DIncompatibleMatrixException();
|
||||
DMatrix<X> aux(nrows,m.ncols);
|
||||
for (int i=0;i<nrows;i++)
|
||||
for (int j=0;j<m.ncols;j++){
|
||||
X a=0;
|
||||
for (int k=0;k<ncols;k++)
|
||||
a+=mrows[i][k]*m.mrows[k][j];
|
||||
aux.mrows[i][j]=a;
|
||||
}
|
||||
return aux;
|
||||
}
|
||||
|
||||
template <class X> DMatrix<X> DMatrix<X>::operator+(const DMatrix<X>& m) const {
|
||||
if (ncols!=m.ncols||nrows!=m.nrows) throw DIncompatibleMatrixException();
|
||||
DMatrix<X> aux(nrows,ncols);
|
||||
for (int i=0;i<nrows*ncols;i++) aux.elems[i]=elems[i]+m.elems[i];
|
||||
return aux;
|
||||
}
|
||||
|
||||
template <class X> DMatrix<X> DMatrix<X>::operator-(const DMatrix<X>& m) const {
|
||||
if (ncols!=m.ncols||nrows!=m.nrows) throw DIncompatibleMatrixException();
|
||||
DMatrix<X> aux(nrows,ncols);
|
||||
for (int i=0;i<nrows*ncols;i++) aux.elems[i]=elems[i]-m.elems[i];
|
||||
return aux;
|
||||
}
|
||||
|
||||
template <class X> DMatrix<X> DMatrix<X>::operator*(const X& e) const {
|
||||
DMatrix<X> aux(nrows,ncols);
|
||||
for (int i=0;i<nrows*ncols;i++) aux.elems[i]=elems[i]*e;
|
||||
return aux;
|
||||
}
|
||||
|
||||
template <class X> void DMatrix<X>::detach() {
|
||||
DMatrix<X> aux(nrows,ncols);
|
||||
for (int i=0;i<nrows*ncols;i++) aux.elems[i]=elems[i];
|
||||
operator=(aux);
|
||||
}
|
||||
|
||||
template <class X> DMatrix<X> DMatrix<X>::I(int n) {
|
||||
DMatrix<X> aux(n,n);
|
||||
for (int i=0;i<n;i++) aux[i][i]=X(1);
|
||||
return aux;
|
||||
}
|
||||
|
||||
template <class X> std::ostream& operator<<(std::ostream& os, const DMatrix<X> &m) {
|
||||
os << "{";
|
||||
for (int i=0;i<m.rows();i++) {
|
||||
if (i>0) os << ",";
|
||||
os << "{";
|
||||
for (int j=0;j<m.columns();j++) {
|
||||
if (j>0) os << ",";
|
||||
os << m[i][j];
|
||||
}
|
||||
os << "}";
|
||||
}
|
||||
return os << "}";
|
||||
}
|
||||
|
||||
}; //namespace GMapping
|
||||
#endif
|
||||
@@ -0,0 +1,108 @@
|
||||
#include "movement.h"
|
||||
#include <gmapping/utils/gvalues.h>
|
||||
|
||||
namespace GMapping {
|
||||
|
||||
|
||||
FSRMovement::FSRMovement(double f, double s, double r) {
|
||||
this->f = f;
|
||||
this->s = s;
|
||||
this->r = r;
|
||||
}
|
||||
|
||||
FSRMovement::FSRMovement(const FSRMovement& src) {
|
||||
*this = src;
|
||||
}
|
||||
|
||||
FSRMovement::FSRMovement(const OrientedPoint& pt1, const OrientedPoint& pt2) {
|
||||
*this = moveBetweenPoints(pt1, pt2);
|
||||
}
|
||||
|
||||
|
||||
FSRMovement::FSRMovement(const FSRMovement& move1, const FSRMovement& move2) {
|
||||
*this = composeMoves(move1, move2);
|
||||
}
|
||||
|
||||
void FSRMovement::normalize()
|
||||
{
|
||||
if (r >= -M_PI && r < M_PI)
|
||||
return;
|
||||
|
||||
int multiplier = (int)(r / (2*M_PI));
|
||||
r = r - multiplier*2*M_PI;
|
||||
if (r >= M_PI)
|
||||
r -= 2*M_PI;
|
||||
if (r < -M_PI)
|
||||
r += 2*M_PI;
|
||||
}
|
||||
|
||||
OrientedPoint FSRMovement::move(const OrientedPoint& pt) const {
|
||||
return movePoint(pt, *this);
|
||||
}
|
||||
|
||||
void FSRMovement::invert() {
|
||||
*this = invertMove(*this);
|
||||
}
|
||||
|
||||
void FSRMovement::compose(const FSRMovement& move2) {
|
||||
*this = composeMoves(*this, move2);
|
||||
}
|
||||
|
||||
|
||||
FSRMovement FSRMovement::composeMoves(const FSRMovement& move1,
|
||||
const FSRMovement& move2) {
|
||||
FSRMovement comp;
|
||||
comp.f = cos(move1.r) * move2.f - sin(move1.r) * move2.s + move1.f;
|
||||
comp.s = sin(move1.r) * move2.f + cos(move1.r) * move2.s + move1.s;
|
||||
comp.r = (move1.r + move2.r);
|
||||
comp.normalize();
|
||||
return comp;
|
||||
}
|
||||
|
||||
OrientedPoint FSRMovement::movePoint(const OrientedPoint& pt, const FSRMovement& move1) {
|
||||
OrientedPoint pt2(pt);
|
||||
pt2.x += move1.f * cos(pt.theta) - move1.s * sin(pt.theta);
|
||||
pt2.y += move1.f * sin(pt.theta) + move1.s * cos(pt.theta);
|
||||
pt2.theta = (move1.r + pt.theta);
|
||||
pt2.normalize();
|
||||
return pt2;
|
||||
}
|
||||
|
||||
FSRMovement FSRMovement::moveBetweenPoints(const OrientedPoint& pt1,
|
||||
const OrientedPoint& pt2) {
|
||||
FSRMovement move;
|
||||
move.f = (pt2.y - pt1.y) * sin(pt1.theta) + (pt2.x - pt1.x) * cos(pt1.theta);
|
||||
move.s = + (pt2.y - pt1.y) * cos(pt1.theta) - (pt2.x - pt1.x) * sin(pt1.theta);
|
||||
move.r = (pt2.theta - pt1.theta);
|
||||
move.normalize();
|
||||
return move;
|
||||
|
||||
}
|
||||
|
||||
FSRMovement FSRMovement::invertMove(const FSRMovement& move1) {
|
||||
FSRMovement p_inv;
|
||||
p_inv.f = - cos(move1.r) * move1.f - sin(move1.r) * move1.s;
|
||||
p_inv.s = sin(move1.r) * move1.f - cos(move1.r) * move1.s;
|
||||
p_inv.r = (-move1.r);
|
||||
p_inv.normalize();
|
||||
return p_inv;
|
||||
}
|
||||
|
||||
|
||||
OrientedPoint FSRMovement::frameTransformation(const OrientedPoint& reference_pt_frame1,
|
||||
const OrientedPoint& reference_pt_frame2,
|
||||
const OrientedPoint& pt_frame1) {
|
||||
OrientedPoint zero;
|
||||
|
||||
FSRMovement itrans_refp1(zero, reference_pt_frame1);
|
||||
itrans_refp1.invert();
|
||||
|
||||
FSRMovement trans_refp2(zero, reference_pt_frame2);
|
||||
FSRMovement trans_pt(zero, pt_frame1);
|
||||
|
||||
FSRMovement tmp = composeMoves( composeMoves(trans_refp2, itrans_refp1), trans_pt);
|
||||
return tmp.move(zero);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
@@ -0,0 +1,45 @@
|
||||
#ifndef FSRMOVEMENT_H
|
||||
#define FSRMOVEMENT_H
|
||||
|
||||
#include <gmapping/utils/point.h>
|
||||
|
||||
namespace GMapping {
|
||||
|
||||
/** fsr-movement (forward, sideward, rotate) **/
|
||||
class FSRMovement {
|
||||
public:
|
||||
FSRMovement(double f=0.0, double s=0.0, double r=0.0);
|
||||
FSRMovement(const FSRMovement& src);
|
||||
FSRMovement(const OrientedPoint& pt1, const OrientedPoint& pt2);
|
||||
FSRMovement(const FSRMovement& move1, const FSRMovement& move2);
|
||||
|
||||
|
||||
void normalize();
|
||||
void invert();
|
||||
void compose(const FSRMovement& move2);
|
||||
OrientedPoint move(const OrientedPoint& pt) const;
|
||||
|
||||
|
||||
/* static members */
|
||||
|
||||
static OrientedPoint movePoint(const OrientedPoint& pt, const FSRMovement& move1);
|
||||
|
||||
static FSRMovement composeMoves(const FSRMovement& move1,
|
||||
const FSRMovement& move2);
|
||||
|
||||
static FSRMovement moveBetweenPoints(const OrientedPoint& pt1,
|
||||
const OrientedPoint& pt2);
|
||||
|
||||
static FSRMovement invertMove(const FSRMovement& move1);
|
||||
|
||||
static OrientedPoint frameTransformation(const OrientedPoint& reference_pt_frame1,
|
||||
const OrientedPoint& reference_pt_frame2,
|
||||
const OrientedPoint& pt_frame1);
|
||||
|
||||
public:
|
||||
double f;
|
||||
double s;
|
||||
double r;
|
||||
};
|
||||
}
|
||||
#endif
|
||||
@@ -0,0 +1,159 @@
|
||||
#ifndef _OPTIMIZER_H_
|
||||
#define _OPTIMIZER_H_
|
||||
|
||||
#include "point.h"
|
||||
|
||||
namespace GMapping {
|
||||
|
||||
struct OptimizerParams{
|
||||
double discretization;
|
||||
double angularStep, linearStep;
|
||||
int iterations;
|
||||
double maxRange;
|
||||
};
|
||||
|
||||
template <typename Likelihood, typename Map>
|
||||
struct Optimizer {
|
||||
Optimizer(const OptimizerParams& params);
|
||||
OptimizerParams params;
|
||||
Map lmap;
|
||||
Likelihood likelihood;
|
||||
OrientedPoint gradientDescent(const RangeReading& oldReading, const RangeReading& newReading);
|
||||
OrientedPoint gradientDescent(const RangeReading& oldReading, const OrientedPoint& pose, OLocalMap& Map);
|
||||
enum Move {Forward, Backward, Left, Right, TurnRight, TurnLeft};
|
||||
};
|
||||
|
||||
template <typename Likelihood, typename Map>
|
||||
Optimizer<Likelihood, Map>::Optimizer(const OptimizerParams& p):
|
||||
params(p),
|
||||
lmap(p.discretization){}
|
||||
|
||||
template <typename Likelihood, typename Map>
|
||||
OrientedPoint Optimizer<Likelihood, Map>::gradientDescent(const RangeReading& oldReading, const RangeReading& newReading){
|
||||
lmap.clear();
|
||||
lmap.update(oldReading, OrientedPoint(0,0,0), params.maxRange);
|
||||
OrientedPoint delta=absoluteDifference(newReading.getPose(), oldReading.getPose());
|
||||
OrientedPoint bestPose=delta;
|
||||
double bestScore=likelihood(lmap, newReading, bestPose, params.maxRange);
|
||||
int it=0;
|
||||
double lstep=params.linearStep, astep=params.angularStep;
|
||||
bool increase;
|
||||
/* cerr << "bestScore=" << bestScore << endl;;*/
|
||||
do {
|
||||
increase=false;
|
||||
OrientedPoint itBestPose=bestPose;
|
||||
double itBestScore=bestScore;
|
||||
bool itIncrease;
|
||||
do {
|
||||
itIncrease=false;
|
||||
OrientedPoint testBestPose=itBestPose;
|
||||
double testBestScore=itBestScore;
|
||||
for (Move move=Forward; move<=TurnLeft; move=(Move)((int)move+1)){
|
||||
OrientedPoint testPose=itBestPose;
|
||||
switch(move){
|
||||
case Forward: testPose.x+=lstep;
|
||||
break;
|
||||
case Backward: testPose.x-=lstep;
|
||||
break;
|
||||
case Left: testPose.y+=lstep;
|
||||
break;
|
||||
case Right: testPose.y-=lstep;
|
||||
break;
|
||||
case TurnRight: testPose.theta-=astep;
|
||||
break;
|
||||
case TurnLeft: testPose.theta+=astep;
|
||||
break;
|
||||
}
|
||||
double score=likelihood(lmap, newReading, testPose, params.maxRange);
|
||||
if (score>testBestScore){
|
||||
testBestScore=score;
|
||||
testBestPose=testPose;
|
||||
}
|
||||
}
|
||||
if (testBestScore > itBestScore){
|
||||
itBestScore=testBestScore;
|
||||
itBestPose=testBestPose;
|
||||
/* cerr << "s=" << itBestScore << " ";*/
|
||||
itIncrease=true;
|
||||
}
|
||||
} while(itIncrease);
|
||||
if (itBestScore > bestScore){
|
||||
/* cerr << "S(" << itBestScore << "," << bestScore<< ")";*/
|
||||
bestScore=itBestScore;
|
||||
bestPose=itBestPose;
|
||||
increase=true;
|
||||
} else {
|
||||
it++;
|
||||
lstep*=0.5;
|
||||
astep*=0.5;
|
||||
}
|
||||
} while (it<params.iterations);
|
||||
/* cerr << "FinalBestScore" << bestScore << endl;*/
|
||||
cerr << endl;
|
||||
return bestPose;
|
||||
}
|
||||
|
||||
template <typename Likelihood, typename Map>
|
||||
OrientedPoint Optimizer<Likelihood, Map>::gradientDescent(const RangeReading& reading, const OrientedPoint& pose, OLocalMap& lmap){
|
||||
OrientedPoint bestPose=pose;
|
||||
double bestScore=likelihood(lmap, reading, bestPose, params.maxRange);
|
||||
int it=0;
|
||||
double lstep=params.linearStep, astep=params.angularStep;
|
||||
bool increase;
|
||||
/* cerr << "bestScore=" << bestScore << endl;;*/
|
||||
do {
|
||||
increase=false;
|
||||
OrientedPoint itBestPose=bestPose;
|
||||
double itBestScore=bestScore;
|
||||
bool itIncrease;
|
||||
do {
|
||||
itIncrease=false;
|
||||
OrientedPoint testBestPose=itBestPose;
|
||||
double testBestScore=itBestScore;
|
||||
for (Move move=Forward; move<=TurnLeft; move=(Move)((int)move+1)){
|
||||
OrientedPoint testPose=itBestPose;
|
||||
switch(move){
|
||||
case Forward: testPose.x+=lstep;
|
||||
break;
|
||||
case Backward: testPose.x-=lstep;
|
||||
break;
|
||||
case Left: testPose.y+=lstep;
|
||||
break;
|
||||
case Right: testPose.y-=lstep;
|
||||
break;
|
||||
case TurnRight: testPose.theta-=astep;
|
||||
break;
|
||||
case TurnLeft: testPose.theta+=astep;
|
||||
break;
|
||||
}
|
||||
double score=likelihood(lmap, reading, testPose, params.maxRange);
|
||||
if (score>testBestScore){
|
||||
testBestScore=score;
|
||||
testBestPose=testPose;
|
||||
}
|
||||
}
|
||||
if (testBestScore > itBestScore){
|
||||
itBestScore=testBestScore;
|
||||
itBestPose=testBestPose;
|
||||
/* cerr << "s=" << itBestScore << " ";*/
|
||||
itIncrease=true;
|
||||
}
|
||||
} while(itIncrease);
|
||||
if (itBestScore > bestScore){
|
||||
/* cerr << "S(" << itBestScore << "," << bestScore<< ")";*/
|
||||
bestScore=itBestScore;
|
||||
bestPose=itBestPose;
|
||||
increase=true;
|
||||
} else {
|
||||
it++;
|
||||
lstep*=0.5;
|
||||
astep*=0.5;
|
||||
}
|
||||
} while (it<params.iterations);
|
||||
/* cerr << "FinalBestScore" << bestScore << endl;*/
|
||||
cerr << endl;
|
||||
return bestPose;
|
||||
}
|
||||
|
||||
} // end namespace GMapping
|
||||
#endif
|
||||
@@ -0,0 +1,31 @@
|
||||
#ifndef ORIENTENDBOUNDINGBOX_H
|
||||
#define ORIENTENDBOUNDINGBOX_H
|
||||
|
||||
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
|
||||
#include <utils/point.h>
|
||||
|
||||
namespace GMapping{
|
||||
|
||||
template<class NUMERIC>
|
||||
class OrientedBoundingBox {
|
||||
|
||||
public:
|
||||
OrientedBoundingBox(std::vector< point<NUMERIC> > p);
|
||||
double area();
|
||||
|
||||
protected:
|
||||
Point ul;
|
||||
Point ur;
|
||||
Point ll;
|
||||
Point lr;
|
||||
};
|
||||
|
||||
#include "orientedboundingbox.hxx"
|
||||
|
||||
};// end namespace
|
||||
|
||||
#endif
|
||||
|
||||
@@ -0,0 +1,116 @@
|
||||
template <class NUMERIC>
|
||||
double OrientedBoundingBox<NUMERIC>::area() {
|
||||
return sqrt((ul.x - ll.x)*(ul.x - ll.x) + (ul.y - ll.y)*(ul.y - ll.y)) *
|
||||
sqrt((ul.x - ur.x)*(ul.x - ur.x) + (ul.y - ur.y)*(ul.y - ur.y)) ;
|
||||
}
|
||||
|
||||
template<class NUMERIC>
|
||||
OrientedBoundingBox<NUMERIC>::OrientedBoundingBox(std::vector< point<NUMERIC> > p) {
|
||||
|
||||
int nOfPoints = (int) p.size();
|
||||
|
||||
// calculate the center of all points (schwerpunkt)
|
||||
// -------------------------------------------------
|
||||
double centerx = 0;
|
||||
double centery = 0;
|
||||
for (int i=0; i < nOfPoints; i++) {
|
||||
centerx += p[i].x;
|
||||
centery += p[i].y;
|
||||
}
|
||||
centerx /= (double) nOfPoints;
|
||||
centery /= (double) nOfPoints;
|
||||
|
||||
|
||||
|
||||
// calcutae the covariance matrix
|
||||
// -------------------------------
|
||||
// covariance matrix (x1 x2, x3 x4)
|
||||
double x1 = 0.0;
|
||||
double x2 = 0.0;
|
||||
double x3 = 0.0;
|
||||
double x4 = 0.0;
|
||||
|
||||
for (int i=0; i < nOfPoints; i++) {
|
||||
double cix = p[i].x - centerx;
|
||||
double ciy = p[i].y - centery;
|
||||
|
||||
x1 += cix*cix;
|
||||
x2 += cix*ciy;
|
||||
x4 += ciy*ciy;
|
||||
}
|
||||
x1 /= (double) nOfPoints;
|
||||
x2 /= (double) nOfPoints;
|
||||
x3 = x2;
|
||||
x4 /= (double) nOfPoints;
|
||||
// covariance & center done
|
||||
|
||||
|
||||
// calculate the eigenvectors
|
||||
// ---------------------------
|
||||
// catch 1/0 or sqrt(<0)
|
||||
if ((x3 == 0) || (x2 == 0)|| (x4*x4-2*x1*x4+x1*x1+4*x2*x3 < 0 )) {
|
||||
fprintf(stderr,"error computing the Eigenvectors (%s, line %d)\nx3=%lf, x2=%lf, term=%lf\n\n",
|
||||
__FILE__, __LINE__, x3,x2, (x4*x4-2*x1*x4+x1*x1+4*x2*x3) );
|
||||
|
||||
ul.x = 0;
|
||||
ul.y = 0;
|
||||
ur.x = 0;
|
||||
ur.y = 0;
|
||||
ll.x = 0;
|
||||
ll.y = 0;
|
||||
lr.x = 0;
|
||||
lr.y = 0;
|
||||
}
|
||||
|
||||
// eigenvalues
|
||||
double lamda1 = 0.5* (x4 + x1 + sqrt(x4*x4 - 2.0*x1*x4 + x1*x1 + 4.0*x2*x3));
|
||||
double lamda2 = 0.5* (x4 + x1 - sqrt(x4*x4 - 2.0*x1*x4 + x1*x1 + 4.0*x2*x3));
|
||||
|
||||
// eigenvector 1 with (x,y)
|
||||
double v1x = - (x4-lamda1) * (x4-lamda1) * (x1-lamda1) / (x2 * x3 * x3);
|
||||
double v1y = (x4-lamda1) * (x1-lamda1) / (x2 * x3);
|
||||
// eigenvector 2 with (x,y)
|
||||
double v2x = - (x4-lamda2) * (x4-lamda2) * (x1-lamda2) / (x2 * x3 * x3);
|
||||
double v2y = (x4-lamda2) * (x1-lamda2) / (x2 * x3);
|
||||
|
||||
// norm the eigenvectors
|
||||
double lv1 = sqrt ( (v1x*v1x) + (v1y*v1y) );
|
||||
double lv2 = sqrt ( (v2x*v2x) + (v2y*v2y) );
|
||||
v1x /= lv1;
|
||||
v1y /= lv1;
|
||||
v2x /= lv2;
|
||||
v2y /= lv2;
|
||||
// eigenvectors done
|
||||
|
||||
// get the points with maximal dot-product
|
||||
double x = 0.0;
|
||||
double y = 0.0;
|
||||
double xmin = 1e20;
|
||||
double xmax = -1e20;
|
||||
double ymin = 1e20;
|
||||
double ymax = -1e20;
|
||||
for(int i = 0; i< nOfPoints; i++) {
|
||||
// dot-product of relativ coordinates of every point
|
||||
x = (p[i].x - centerx) * v1x + (p[i].y - centery) * v1y;
|
||||
y = (p[i].x - centerx) * v2x + (p[i].y - centery) * v2y;
|
||||
|
||||
if( x > xmax) xmax = x;
|
||||
if( x < xmin) xmin = x;
|
||||
if( y > ymax) ymax = y;
|
||||
if( y < ymin) ymin = y;
|
||||
}
|
||||
|
||||
// now we can compute the corners of the bounding box
|
||||
ul.x = centerx + xmin * v1x + ymin * v2x;
|
||||
ul.y = centery + xmin * v1y + ymin * v2y;
|
||||
|
||||
ur.x = centerx + xmax * v1x + ymin * v2x;
|
||||
ur.y = centery + xmax * v1y + ymin * v2y;
|
||||
|
||||
ll.x = centerx + xmin * v1x + ymax * v2x;
|
||||
ll.y = centery + xmin * v1y + ymax * v2y;
|
||||
|
||||
lr.x = centerx + xmax * v1x + ymax * v2x;
|
||||
lr.y = centery + xmax * v1y + ymax * v2y;
|
||||
|
||||
}
|
||||
@@ -0,0 +1,27 @@
|
||||
#include "printmemusage.h"
|
||||
|
||||
namespace GMapping{
|
||||
|
||||
using namespace std;
|
||||
void printmemusage(){
|
||||
pid_t pid=getpid();
|
||||
char procfilename[1000];
|
||||
sprintf(procfilename, "/proc/%d/status", pid);
|
||||
ifstream is(procfilename);
|
||||
string line;
|
||||
while (is){
|
||||
is >> line;
|
||||
if (line=="VmData:"){
|
||||
is >> line;
|
||||
cerr << "#VmData:\t" << line << endl;
|
||||
}
|
||||
if (line=="VmSize:"){
|
||||
is >> line;
|
||||
cerr << "#VmSize:\t" << line << endl;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
@@ -0,0 +1,13 @@
|
||||
#ifndef PRINTMEMUSAGE_H
|
||||
#define PRINTMEMUSAGE_H
|
||||
#include <sys/types.h>
|
||||
#include <unistd.h>
|
||||
#include <iostream>
|
||||
#include <fstream>
|
||||
#include <string>
|
||||
|
||||
namespace GMapping{
|
||||
void printmemusage();
|
||||
};
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,19 @@
|
||||
#include <fstream>
|
||||
#include <iostream>
|
||||
#include <sstream>
|
||||
#include <unistd.h>
|
||||
|
||||
|
||||
using namespace std;
|
||||
ostream& printpgm(ostream& os, int xsize, int ysize, const double * const * matrix){
|
||||
if (!os)
|
||||
return os;
|
||||
os<< "P5" << endl << xsize << endl << ysize << endl << 255 << endl;
|
||||
for (int y=ysize-1; y>=0; y--){
|
||||
for (int x=0;x<xsize; x++){
|
||||
unsigned char c=(unsigned char)(255*fabs(1.-matrix[x][y]));
|
||||
os.put(c);
|
||||
}
|
||||
}
|
||||
return os;
|
||||
}
|
||||
@@ -0,0 +1,279 @@
|
||||
#include <stdlib.h>
|
||||
|
||||
//#include <gsl/gsl_rng.h>
|
||||
//#include <gsl/gsl_randist.h>
|
||||
//#include <gsl/gsl_eigen.h>
|
||||
//#include <gsl/gsl_blas.h>
|
||||
#include <math.h>
|
||||
#include <gmapping/utils/gvalues.h>
|
||||
#include <gmapping/utils/stat.h>
|
||||
|
||||
namespace GMapping {
|
||||
|
||||
#if 0
|
||||
|
||||
int sampleUniformInt(int max)
|
||||
{
|
||||
return (int)(max*(rand()/(RAND_MAX+1.0)));
|
||||
}
|
||||
|
||||
double sampleUniformDouble(double min, double max)
|
||||
{
|
||||
return min + (rand() / (double)RAND_MAX) * (max - min);
|
||||
}
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
// Draw randomly from a zero-mean Gaussian distribution, with standard
|
||||
// deviation sigma.
|
||||
// We use the polar form of the Box-Muller transformation, explained here:
|
||||
// http://www.taygeta.com/random/gaussian.html
|
||||
double pf_ran_gaussian(double sigma)
|
||||
{
|
||||
double x1, x2, w;
|
||||
double r;
|
||||
|
||||
do
|
||||
{
|
||||
do { r = drand48(); } while (r == 0.0);
|
||||
x1 = 2.0 * r - 1.0;
|
||||
do { r = drand48(); } while (r == 0.0);
|
||||
x2 = 2.0 * drand48() - 1.0;
|
||||
w = x1*x1 + x2*x2;
|
||||
} while(w > 1.0 || w==0.0);
|
||||
|
||||
return(sigma * x2 * sqrt(-2.0*log(w)/w));
|
||||
}
|
||||
|
||||
double sampleGaussian(double sigma, unsigned int S) {
|
||||
/*
|
||||
static gsl_rng * r = NULL;
|
||||
if(r==NULL) {
|
||||
gsl_rng_env_setup();
|
||||
r = gsl_rng_alloc (gsl_rng_default);
|
||||
}
|
||||
*/
|
||||
if (S!=0)
|
||||
{
|
||||
//gsl_rng_set(r, S);
|
||||
srand(S);
|
||||
}
|
||||
if (sigma==0)
|
||||
return 0;
|
||||
//return gsl_ran_gaussian (r,sigma);
|
||||
return pf_ran_gaussian (sigma);
|
||||
}
|
||||
#if 0
|
||||
|
||||
double evalGaussian(double sigmaSquare, double delta){
|
||||
if (sigmaSquare<=0)
|
||||
sigmaSquare=1e-4;
|
||||
return exp(-.5*delta*delta/sigmaSquare)/sqrt(2*M_PI*sigmaSquare);
|
||||
}
|
||||
|
||||
#endif
|
||||
double evalLogGaussian(double sigmaSquare, double delta){
|
||||
if (sigmaSquare<=0)
|
||||
sigmaSquare=1e-4;
|
||||
return -.5*delta*delta/sigmaSquare-.5*log(2*M_PI*sigmaSquare);
|
||||
}
|
||||
#if 0
|
||||
|
||||
|
||||
Covariance3 Covariance3::zero={0.,0.,0.,0.,0.,0.};
|
||||
|
||||
Covariance3 Covariance3::operator + (const Covariance3 & cov) const{
|
||||
Covariance3 r(*this);
|
||||
r.xx+=cov.xx;
|
||||
r.yy+=cov.yy;
|
||||
r.tt+=cov.tt;
|
||||
r.xy+=cov.xy;
|
||||
r.yt+=cov.yt;
|
||||
r.xt+=cov.xt;
|
||||
return r;
|
||||
}
|
||||
|
||||
EigenCovariance3::EigenCovariance3(){}
|
||||
|
||||
EigenCovariance3::EigenCovariance3(const Covariance3& cov){
|
||||
static gsl_eigen_symmv_workspace * m_eigenspace=NULL;
|
||||
static gsl_matrix * m_cmat=NULL;
|
||||
static gsl_matrix * m_evec=NULL;
|
||||
static gsl_vector * m_eval=NULL;
|
||||
static gsl_vector * m_noise=NULL;
|
||||
static gsl_vector * m_pnoise=NULL;
|
||||
|
||||
if (m_eigenspace==NULL){
|
||||
m_eigenspace=gsl_eigen_symmv_alloc(3);
|
||||
m_cmat=gsl_matrix_alloc(3,3);
|
||||
m_evec=gsl_matrix_alloc(3,3);
|
||||
m_eval=gsl_vector_alloc(3);
|
||||
m_noise=gsl_vector_alloc(3);
|
||||
m_pnoise=gsl_vector_alloc(3);
|
||||
}
|
||||
|
||||
gsl_matrix_set(m_cmat,0,0,cov.xx); gsl_matrix_set(m_cmat,0,1,cov.xy); gsl_matrix_set(m_cmat,0,2,cov.xt);
|
||||
gsl_matrix_set(m_cmat,1,0,cov.xy); gsl_matrix_set(m_cmat,1,1,cov.yy); gsl_matrix_set(m_cmat,1,2,cov.yt);
|
||||
gsl_matrix_set(m_cmat,2,0,cov.xt); gsl_matrix_set(m_cmat,2,1,cov.yt); gsl_matrix_set(m_cmat,2,2,cov.tt);
|
||||
gsl_eigen_symmv (m_cmat, m_eval, m_evec, m_eigenspace);
|
||||
for (int i=0; i<3; i++){
|
||||
eval[i]=gsl_vector_get(m_eval,i);
|
||||
for (int j=0; j<3; j++)
|
||||
evec[i][j]=gsl_matrix_get(m_evec,i,j);
|
||||
}
|
||||
}
|
||||
|
||||
EigenCovariance3 EigenCovariance3::rotate(double angle) const{
|
||||
static gsl_matrix * m_rmat=NULL;
|
||||
static gsl_matrix * m_vmat=NULL;
|
||||
static gsl_matrix * m_result=NULL;
|
||||
if (m_rmat==NULL){
|
||||
m_rmat=gsl_matrix_alloc(3,3);
|
||||
m_vmat=gsl_matrix_alloc(3,3);
|
||||
m_result=gsl_matrix_alloc(3,3);
|
||||
}
|
||||
|
||||
double c=cos(angle);
|
||||
double s=sin(angle);
|
||||
gsl_matrix_set(m_rmat,0,0, c ); gsl_matrix_set(m_rmat,0,1, -s); gsl_matrix_set(m_rmat,0,2, 0.);
|
||||
gsl_matrix_set(m_rmat,1,0, s ); gsl_matrix_set(m_rmat,1,1, c); gsl_matrix_set(m_rmat,1,2, 0.);
|
||||
gsl_matrix_set(m_rmat,2,0, 0.); gsl_matrix_set(m_rmat,2,1, 0.); gsl_matrix_set(m_rmat,2,2, 1.);
|
||||
|
||||
for (unsigned int i=0; i<3; i++)
|
||||
for (unsigned int j=0; j<3; j++)
|
||||
gsl_matrix_set(m_vmat,i,j,evec[i][j]);
|
||||
gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, 1., m_rmat, m_vmat, 0., m_result);
|
||||
EigenCovariance3 ecov(*this);
|
||||
for (int i=0; i<3; i++){
|
||||
for (int j=0; j<3; j++)
|
||||
ecov.evec[i][j]=gsl_matrix_get(m_result,i,j);
|
||||
}
|
||||
return ecov;
|
||||
}
|
||||
|
||||
OrientedPoint EigenCovariance3::sample() const{
|
||||
static gsl_matrix * m_evec=NULL;
|
||||
static gsl_vector * m_noise=NULL;
|
||||
static gsl_vector * m_pnoise=NULL;
|
||||
if (m_evec==NULL){
|
||||
m_evec=gsl_matrix_alloc(3,3);
|
||||
m_noise=gsl_vector_alloc(3);
|
||||
m_pnoise=gsl_vector_alloc(3);
|
||||
}
|
||||
for (int i=0; i<3; i++){
|
||||
for (int j=0; j<3; j++)
|
||||
gsl_matrix_set(m_evec,i,j, evec[i][j]);
|
||||
}
|
||||
for (int i=0; i<3; i++){
|
||||
double v=sampleGaussian(sqrt(eval[i]));
|
||||
if(isnan(v))
|
||||
v=0;
|
||||
gsl_vector_set(m_pnoise,i, v);
|
||||
}
|
||||
gsl_blas_dgemv (CblasNoTrans, 1., m_evec, m_pnoise, 0, m_noise);
|
||||
OrientedPoint ret(gsl_vector_get(m_noise,0),gsl_vector_get(m_noise,1),gsl_vector_get(m_noise,2));
|
||||
ret.theta=atan2(sin(ret.theta), cos(ret.theta));
|
||||
return ret;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
double Gaussian3::eval(const OrientedPoint& p) const{
|
||||
OrientedPoint q=p-mean;
|
||||
q.theta=atan2(sin(p.theta-mean.theta),cos(p.theta-mean.theta));
|
||||
double v1,v2,v3;
|
||||
v1 = covariance.evec[0][0]*q.x+covariance.evec[1][0]*q.y+covariance.evec[2][0]*q.theta;
|
||||
v2 = covariance.evec[0][1]*q.x+covariance.evec[1][1]*q.y+covariance.evec[2][1]*q.theta;
|
||||
v3 = covariance.evec[0][2]*q.x+covariance.evec[1][2]*q.y+covariance.evec[2][2]*q.theta;
|
||||
return evalLogGaussian(covariance.eval[0], v1)+evalLogGaussian(covariance.eval[1], v2)+evalLogGaussian(covariance.eval[2], v3);
|
||||
}
|
||||
|
||||
#if 0
|
||||
void Gaussian3::computeFromSamples(const std::vector<OrientedPoint> & poses, const std::vector<double>& weights ){
|
||||
OrientedPoint mean=OrientedPoint(0,0,0);
|
||||
double wcum=0;
|
||||
double s=0, c=0;
|
||||
std::vector<double>::const_iterator w=weights.begin();
|
||||
for (std::vector<OrientedPoint>::const_iterator p=poses.begin(); p!=poses.end(); p++){
|
||||
s+=*w*sin(p->theta);
|
||||
c+=*w*cos(p->theta);
|
||||
mean.x+=*w*p->x;
|
||||
mean.y+=*w*p->y;
|
||||
wcum+=*w;
|
||||
w++;
|
||||
}
|
||||
mean.x/=wcum;
|
||||
mean.y/=wcum;
|
||||
s/=wcum;
|
||||
c/=wcum;
|
||||
mean.theta=atan2(s,c);
|
||||
|
||||
Covariance3 cov=Covariance3::zero;
|
||||
w=weights.begin();
|
||||
for (std::vector<OrientedPoint>::const_iterator p=poses.begin(); p!=poses.end(); p++){
|
||||
OrientedPoint delta=(*p)-mean;
|
||||
delta.theta=atan2(sin(delta.theta),cos(delta.theta));
|
||||
cov.xx+=*w*delta.x*delta.x;
|
||||
cov.yy+=*w*delta.y*delta.y;
|
||||
cov.tt+=*w*delta.theta*delta.theta;
|
||||
cov.xy+=*w*delta.x*delta.y;
|
||||
cov.yt+=*w*delta.y*delta.theta;
|
||||
cov.xt+=*w*delta.x*delta.theta;
|
||||
w++;
|
||||
}
|
||||
cov.xx/=wcum;
|
||||
cov.yy/=wcum;
|
||||
cov.tt/=wcum;
|
||||
cov.xy/=wcum;
|
||||
cov.yt/=wcum;
|
||||
cov.xt/=wcum;
|
||||
EigenCovariance3 ecov(cov);
|
||||
this->mean=mean;
|
||||
this->covariance=ecov;
|
||||
this->cov=cov;
|
||||
}
|
||||
|
||||
void Gaussian3::computeFromSamples(const std::vector<OrientedPoint> & poses){
|
||||
OrientedPoint mean=OrientedPoint(0,0,0);
|
||||
double wcum=1;
|
||||
double s=0, c=0;
|
||||
for (std::vector<OrientedPoint>::const_iterator p=poses.begin(); p!=poses.end(); p++){
|
||||
s+=sin(p->theta);
|
||||
c+=cos(p->theta);
|
||||
mean.x+=p->x;
|
||||
mean.y+=p->y;
|
||||
wcum+=1.;
|
||||
}
|
||||
mean.x/=wcum;
|
||||
mean.y/=wcum;
|
||||
s/=wcum;
|
||||
c/=wcum;
|
||||
mean.theta=atan2(s,c);
|
||||
|
||||
Covariance3 cov=Covariance3::zero;
|
||||
for (std::vector<OrientedPoint>::const_iterator p=poses.begin(); p!=poses.end(); p++){
|
||||
OrientedPoint delta=(*p)-mean;
|
||||
delta.theta=atan2(sin(delta.theta),cos(delta.theta));
|
||||
cov.xx+=delta.x*delta.x;
|
||||
cov.yy+=delta.y*delta.y;
|
||||
cov.tt+=delta.theta*delta.theta;
|
||||
cov.xy+=delta.x*delta.y;
|
||||
cov.yt+=delta.y*delta.theta;
|
||||
cov.xt+=delta.x*delta.theta;
|
||||
}
|
||||
cov.xx/=wcum;
|
||||
cov.yy/=wcum;
|
||||
cov.tt/=wcum;
|
||||
cov.xy/=wcum;
|
||||
cov.yt/=wcum;
|
||||
cov.xt/=wcum;
|
||||
EigenCovariance3 ecov(cov);
|
||||
this->mean=mean;
|
||||
this->covariance=ecov;
|
||||
this->cov=cov;
|
||||
}
|
||||
#endif
|
||||
|
||||
}// end namespace
|
||||
|
||||
@@ -0,0 +1,66 @@
|
||||
#include <iostream>
|
||||
#include <fstream>
|
||||
#include <math.h>
|
||||
#include "stat.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace GMapping;
|
||||
|
||||
// struct Covariance3{
|
||||
// double xx, yy, tt, xy, xt, yt;
|
||||
// };
|
||||
|
||||
#define SAMPLES_NUMBER 10000
|
||||
|
||||
int main(int argc, char** argv){
|
||||
Covariance3 cov={1.,0.01,0.01,0,0,0};
|
||||
EigenCovariance3 ecov(cov);
|
||||
cout << "EigenValues: " << ecov.eval[0] << " "<< ecov.eval[1] << " " << ecov.eval[2] << endl;
|
||||
|
||||
cout << "EigenVectors:" <<endl;
|
||||
cout<< ecov.evec[0][0] << " "<< ecov.evec[0][1] << " " << ecov.evec[0][2] << endl;
|
||||
cout<< ecov.evec[1][0] << " "<< ecov.evec[1][1] << " " << ecov.evec[1][2] << endl;
|
||||
cout<< ecov.evec[2][0] << " "<< ecov.evec[2][1] << " " << ecov.evec[2][2] << endl;
|
||||
|
||||
EigenCovariance3 rcov(ecov.rotate(M_PI/4));
|
||||
cout << "*************** Rotated ***************" << endl;
|
||||
cout << "EigenValues: " << rcov.eval[0] << " "<< rcov.eval[1] << " " << rcov.eval[2] << endl;
|
||||
|
||||
cout << "EigenVectors:" <<endl;
|
||||
cout<< rcov.evec[0][0] << " "<< rcov.evec[0][1] << " " << rcov.evec[0][2] << endl;
|
||||
cout<< rcov.evec[1][0] << " "<< rcov.evec[1][1] << " " << rcov.evec[1][2] << endl;
|
||||
cout<< rcov.evec[2][0] << " "<< rcov.evec[2][1] << " " << rcov.evec[2][2] << endl;
|
||||
|
||||
cout << "sampling:" << endl;
|
||||
ofstream fs("stat_test.dat");
|
||||
std::vector<OrientedPoint> points;
|
||||
for (unsigned int i=0; i<SAMPLES_NUMBER; i++){
|
||||
OrientedPoint op=rcov.sample();
|
||||
points.push_back(op);
|
||||
fs << op.x << " " << op.y << " " << op.theta << endl;
|
||||
}
|
||||
fs.close();
|
||||
std::vector<OrientedPoint>::iterator b = points.begin();
|
||||
std::vector<OrientedPoint>::iterator e = points.end();
|
||||
Gaussian3 gaussian=computeGaussianFromSamples(b, e);
|
||||
cov=gaussian.cov;
|
||||
ecov=gaussian.covariance;
|
||||
cout << "*************** Estimated with Templates ***************" << endl;
|
||||
cout << "EigenValues: " << ecov.eval[0] << " "<< ecov.eval[1] << " " << ecov.eval[2] << endl;
|
||||
cout << "EigenVectors:" <<endl;
|
||||
cout<< ecov.evec[0][0] << " "<< ecov.evec[0][1] << " " << ecov.evec[0][2] << endl;
|
||||
cout<< ecov.evec[1][0] << " "<< ecov.evec[1][1] << " " << ecov.evec[1][2] << endl;
|
||||
cout<< ecov.evec[2][0] << " "<< ecov.evec[2][1] << " " << ecov.evec[2][2] << endl;
|
||||
gaussian.computeFromSamples(points);
|
||||
ecov=gaussian.covariance;
|
||||
cout << "*************** Estimated without Templates ***************" << endl;
|
||||
cout << "EigenValues: " << ecov.eval[0] << " "<< ecov.eval[1] << " " << ecov.eval[2] << endl;
|
||||
cout << "EigenVectors:" <<endl;
|
||||
cout<< ecov.evec[0][0] << " "<< ecov.evec[0][1] << " " << ecov.evec[0][2] << endl;
|
||||
cout<< ecov.evec[1][0] << " "<< ecov.evec[1][1] << " " << ecov.evec[1][2] << endl;
|
||||
cout<< ecov.evec[2][0] << " "<< ecov.evec[2][1] << " " << ecov.evec[2][2] << endl;
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user