add slam_gmapping
This commit is contained in:
X-lanni
@@ -0,0 +1,9 @@
|
||||
#ifndef ACCESSTATE_H
|
||||
#define ACCESSTATE_H
|
||||
|
||||
namespace GMapping {
|
||||
enum AccessibilityState{Outside=0x0, Inside=0x1, Allocated=0x2};
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
@@ -0,0 +1,188 @@
|
||||
#ifndef ARRAY2D_H
|
||||
#define ARRAY2D_H
|
||||
|
||||
#include <assert.h>
|
||||
#include <gmapping/utils/point.h>
|
||||
#include "accessstate.h"
|
||||
|
||||
#include <iostream>
|
||||
|
||||
#ifndef __PRETTY_FUNCTION__
|
||||
#define __FUNCDNAME__
|
||||
#endif
|
||||
|
||||
namespace GMapping {
|
||||
|
||||
template<class Cell, const bool debug=false> class Array2D{
|
||||
public:
|
||||
Array2D(int xsize=0, int ysize=0);
|
||||
Array2D& operator=(const Array2D &);
|
||||
Array2D(const Array2D<Cell,debug> &);
|
||||
~Array2D();
|
||||
void clear();
|
||||
void resize(int xmin, int ymin, int xmax, int ymax);
|
||||
|
||||
|
||||
inline bool isInside(int x, int y) const;
|
||||
inline const Cell& cell(int x, int y) const;
|
||||
inline Cell& cell(int x, int y);
|
||||
inline AccessibilityState cellState(int x, int y) const { return (AccessibilityState) (isInside(x,y)?(Inside|Allocated):Outside);}
|
||||
|
||||
inline bool isInside(const IntPoint& p) const { return isInside(p.x, p.y);}
|
||||
inline const Cell& cell(const IntPoint& p) const {return cell(p.x,p.y);}
|
||||
inline Cell& cell(const IntPoint& p) {return cell(p.x,p.y);}
|
||||
inline AccessibilityState cellState(const IntPoint& p) const { return cellState(p.x, p.y);}
|
||||
|
||||
inline int getPatchSize() const{return 0;}
|
||||
inline int getPatchMagnitude() const{return 0;}
|
||||
inline int getXSize() const {return m_xsize;}
|
||||
inline int getYSize() const {return m_ysize;}
|
||||
inline Cell** cells() {return m_cells;}
|
||||
Cell ** m_cells;
|
||||
protected:
|
||||
int m_xsize, m_ysize;
|
||||
};
|
||||
|
||||
|
||||
template <class Cell, const bool debug>
|
||||
Array2D<Cell,debug>::Array2D(int xsize, int ysize){
|
||||
// assert(xsize>0);
|
||||
// assert(ysize>0);
|
||||
m_xsize=xsize;
|
||||
m_ysize=ysize;
|
||||
if (m_xsize>0 && m_ysize>0){
|
||||
m_cells=new Cell*[m_xsize];
|
||||
for (int i=0; i<m_xsize; i++)
|
||||
m_cells[i]=new Cell[m_ysize];
|
||||
}
|
||||
else{
|
||||
m_xsize=m_ysize=0;
|
||||
m_cells=0;
|
||||
}
|
||||
if (debug){
|
||||
std::cerr << __PRETTY_FUNCTION__ << std::endl;
|
||||
std::cerr << "m_xsize= " << m_xsize<< std::endl;
|
||||
std::cerr << "m_ysize= " << m_ysize<< std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
template <class Cell, const bool debug>
|
||||
Array2D<Cell,debug> & Array2D<Cell,debug>::operator=(const Array2D<Cell,debug> & g){
|
||||
if (debug || m_xsize!=g.m_xsize || m_ysize!=g.m_ysize){
|
||||
for (int i=0; i<m_xsize; i++)
|
||||
delete [] m_cells[i];
|
||||
delete [] m_cells;
|
||||
m_xsize=g.m_xsize;
|
||||
m_ysize=g.m_ysize;
|
||||
m_cells=new Cell*[m_xsize];
|
||||
for (int i=0; i<m_xsize; i++)
|
||||
m_cells[i]=new Cell[m_ysize];
|
||||
}
|
||||
for (int x=0; x<m_xsize; x++)
|
||||
for (int y=0; y<m_ysize; y++)
|
||||
m_cells[x][y]=g.m_cells[x][y];
|
||||
|
||||
if (debug){
|
||||
std::cerr << __PRETTY_FUNCTION__ << std::endl;
|
||||
std::cerr << "m_xsize= " << m_xsize<< std::endl;
|
||||
std::cerr << "m_ysize= " << m_ysize<< std::endl;
|
||||
}
|
||||
return *this;
|
||||
}
|
||||
|
||||
template <class Cell, const bool debug>
|
||||
Array2D<Cell,debug>::Array2D(const Array2D<Cell,debug> & g){
|
||||
m_xsize=g.m_xsize;
|
||||
m_ysize=g.m_ysize;
|
||||
m_cells=new Cell*[m_xsize];
|
||||
for (int x=0; x<m_xsize; x++){
|
||||
m_cells[x]=new Cell[m_ysize];
|
||||
for (int y=0; y<m_ysize; y++)
|
||||
m_cells[x][y]=g.m_cells[x][y];
|
||||
}
|
||||
if (debug){
|
||||
std::cerr << __PRETTY_FUNCTION__ << std::endl;
|
||||
std::cerr << "m_xsize= " << m_xsize<< std::endl;
|
||||
std::cerr << "m_ysize= " << m_ysize<< std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
template <class Cell, const bool debug>
|
||||
Array2D<Cell,debug>::~Array2D(){
|
||||
if (debug){
|
||||
std::cerr << __PRETTY_FUNCTION__ << std::endl;
|
||||
std::cerr << "m_xsize= " << m_xsize<< std::endl;
|
||||
std::cerr << "m_ysize= " << m_ysize<< std::endl;
|
||||
}
|
||||
for (int i=0; i<m_xsize; i++){
|
||||
delete [] m_cells[i];
|
||||
m_cells[i]=0;
|
||||
}
|
||||
delete [] m_cells;
|
||||
m_cells=0;
|
||||
}
|
||||
|
||||
template <class Cell, const bool debug>
|
||||
void Array2D<Cell,debug>::clear(){
|
||||
if (debug){
|
||||
std::cerr << __PRETTY_FUNCTION__ << std::endl;
|
||||
std::cerr << "m_xsize= " << m_xsize<< std::endl;
|
||||
std::cerr << "m_ysize= " << m_ysize<< std::endl;
|
||||
}
|
||||
for (int i=0; i<m_xsize; i++){
|
||||
delete [] m_cells[i];
|
||||
m_cells[i]=0;
|
||||
}
|
||||
delete [] m_cells;
|
||||
m_cells=0;
|
||||
m_xsize=0;
|
||||
m_ysize=0;
|
||||
}
|
||||
|
||||
|
||||
template <class Cell, const bool debug>
|
||||
void Array2D<Cell,debug>::resize(int xmin, int ymin, int xmax, int ymax){
|
||||
int xsize=xmax-xmin;
|
||||
int ysize=ymax-ymin;
|
||||
Cell ** newcells=new Cell *[xsize];
|
||||
for (int x=0; x<xsize; x++){
|
||||
newcells[x]=new Cell[ysize];
|
||||
}
|
||||
int dx= xmin < 0 ? 0 : xmin;
|
||||
int dy= ymin < 0 ? 0 : ymin;
|
||||
int Dx=xmax<this->m_xsize?xmax:this->m_xsize;
|
||||
int Dy=ymax<this->m_ysize?ymax:this->m_ysize;
|
||||
for (int x=dx; x<Dx; x++){
|
||||
for (int y=dy; y<Dy; y++){
|
||||
newcells[x-xmin][y-ymin]=this->m_cells[x][y];
|
||||
}
|
||||
delete [] this->m_cells[x];
|
||||
}
|
||||
delete [] this->m_cells;
|
||||
this->m_cells=newcells;
|
||||
this->m_xsize=xsize;
|
||||
this->m_ysize=ysize;
|
||||
}
|
||||
|
||||
template <class Cell, const bool debug>
|
||||
inline bool Array2D<Cell,debug>::isInside(int x, int y) const{
|
||||
return x>=0 && y>=0 && x<m_xsize && y<m_ysize;
|
||||
}
|
||||
|
||||
template <class Cell, const bool debug>
|
||||
inline const Cell& Array2D<Cell,debug>::cell(int x, int y) const{
|
||||
assert(isInside(x,y));
|
||||
return m_cells[x][y];
|
||||
}
|
||||
|
||||
|
||||
template <class Cell, const bool debug>
|
||||
inline Cell& Array2D<Cell,debug>::cell(int x, int y){
|
||||
assert(isInside(x,y));
|
||||
return m_cells[x][y];
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
@@ -0,0 +1,200 @@
|
||||
#ifndef HARRAY2D_H
|
||||
#define HARRAY2D_H
|
||||
#include <set>
|
||||
#include <gmapping/utils/point.h>
|
||||
#include <gmapping/utils/autoptr.h>
|
||||
#include "array2d.h"
|
||||
|
||||
namespace GMapping {
|
||||
|
||||
template <class Cell>
|
||||
class HierarchicalArray2D: public Array2D<autoptr< Array2D<Cell> > >{
|
||||
public:
|
||||
typedef std::set< point<int>, pointcomparator<int> > PointSet;
|
||||
HierarchicalArray2D(int xsize, int ysize, int patchMagnitude=5);
|
||||
HierarchicalArray2D(const HierarchicalArray2D& hg);
|
||||
HierarchicalArray2D& operator=(const HierarchicalArray2D& hg);
|
||||
virtual ~HierarchicalArray2D(){}
|
||||
void resize(int ixmin, int iymin, int ixmax, int iymax);
|
||||
inline int getPatchSize() const {return m_patchMagnitude;}
|
||||
inline int getPatchMagnitude() const {return m_patchMagnitude;}
|
||||
|
||||
inline const Cell& cell(int x, int y) const;
|
||||
inline Cell& cell(int x, int y);
|
||||
inline bool isAllocated(int x, int y) const;
|
||||
inline AccessibilityState cellState(int x, int y) const ;
|
||||
inline IntPoint patchIndexes(int x, int y) const;
|
||||
|
||||
inline const Cell& cell(const IntPoint& p) const { return cell(p.x,p.y); }
|
||||
inline Cell& cell(const IntPoint& p) { return cell(p.x,p.y); }
|
||||
inline bool isAllocated(const IntPoint& p) const { return isAllocated(p.x,p.y);}
|
||||
inline AccessibilityState cellState(const IntPoint& p) const { return cellState(p.x,p.y); }
|
||||
inline IntPoint patchIndexes(const IntPoint& p) const { return patchIndexes(p.x,p.y);}
|
||||
|
||||
inline void setActiveArea(const PointSet&, bool patchCoords=false);
|
||||
const PointSet& getActiveArea() const {return m_activeArea; }
|
||||
inline void allocActiveArea();
|
||||
protected:
|
||||
virtual Array2D<Cell> * createPatch(const IntPoint& p) const;
|
||||
PointSet m_activeArea;
|
||||
int m_patchMagnitude;
|
||||
int m_patchSize;
|
||||
};
|
||||
|
||||
template <class Cell>
|
||||
HierarchicalArray2D<Cell>::HierarchicalArray2D(int xsize, int ysize, int patchMagnitude)
|
||||
:Array2D<autoptr< Array2D<Cell> > >::Array2D((xsize>>patchMagnitude), (ysize>>patchMagnitude)){
|
||||
m_patchMagnitude=patchMagnitude;
|
||||
m_patchSize=1<<m_patchMagnitude;
|
||||
}
|
||||
|
||||
template <class Cell>
|
||||
HierarchicalArray2D<Cell>::HierarchicalArray2D(const HierarchicalArray2D& hg)
|
||||
:Array2D<autoptr< Array2D<Cell> > >::Array2D((hg.m_xsize>>hg.m_patchMagnitude), (hg.m_ysize>>hg.m_patchMagnitude)) // added by cyrill: if you have a resize error, check this again
|
||||
{
|
||||
this->m_xsize=hg.m_xsize;
|
||||
this->m_ysize=hg.m_ysize;
|
||||
this->m_cells=new autoptr< Array2D<Cell> >*[this->m_xsize];
|
||||
for (int x=0; x<this->m_xsize; x++){
|
||||
this->m_cells[x]=new autoptr< Array2D<Cell> >[this->m_ysize];
|
||||
for (int y=0; y<this->m_ysize; y++)
|
||||
this->m_cells[x][y]=hg.m_cells[x][y];
|
||||
}
|
||||
this->m_patchMagnitude=hg.m_patchMagnitude;
|
||||
this->m_patchSize=hg.m_patchSize;
|
||||
}
|
||||
|
||||
template <class Cell>
|
||||
void HierarchicalArray2D<Cell>::resize(int xmin, int ymin, int xmax, int ymax){
|
||||
int xsize=xmax-xmin;
|
||||
int ysize=ymax-ymin;
|
||||
autoptr< Array2D<Cell> > ** newcells=new autoptr< Array2D<Cell> > *[xsize];
|
||||
for (int x=0; x<xsize; x++){
|
||||
newcells[x]=new autoptr< Array2D<Cell> >[ysize];
|
||||
for (int y=0; y<ysize; y++){
|
||||
newcells[x][y]=autoptr< Array2D<Cell> >(0);
|
||||
}
|
||||
}
|
||||
int dx= xmin < 0 ? 0 : xmin;
|
||||
int dy= ymin < 0 ? 0 : ymin;
|
||||
int Dx=xmax<this->m_xsize?xmax:this->m_xsize;
|
||||
int Dy=ymax<this->m_ysize?ymax:this->m_ysize;
|
||||
for (int x=dx; x<Dx; x++){
|
||||
for (int y=dy; y<Dy; y++){
|
||||
newcells[x-xmin][y-ymin]=this->m_cells[x][y];
|
||||
}
|
||||
delete [] this->m_cells[x];
|
||||
}
|
||||
delete [] this->m_cells;
|
||||
this->m_cells=newcells;
|
||||
this->m_xsize=xsize;
|
||||
this->m_ysize=ysize;
|
||||
}
|
||||
|
||||
template <class Cell>
|
||||
HierarchicalArray2D<Cell>& HierarchicalArray2D<Cell>::operator=(const HierarchicalArray2D& hg){
|
||||
// Array2D<autoptr< Array2D<Cell> > >::operator=(hg);
|
||||
if (this->m_xsize!=hg.m_xsize || this->m_ysize!=hg.m_ysize){
|
||||
for (int i=0; i<this->m_xsize; i++)
|
||||
delete [] this->m_cells[i];
|
||||
delete [] this->m_cells;
|
||||
this->m_xsize=hg.m_xsize;
|
||||
this->m_ysize=hg.m_ysize;
|
||||
this->m_cells=new autoptr< Array2D<Cell> >*[this->m_xsize];
|
||||
for (int i=0; i<this->m_xsize; i++)
|
||||
this->m_cells[i]=new autoptr< Array2D<Cell> > [this->m_ysize];
|
||||
}
|
||||
for (int x=0; x<this->m_xsize; x++)
|
||||
for (int y=0; y<this->m_ysize; y++)
|
||||
this->m_cells[x][y]=hg.m_cells[x][y];
|
||||
|
||||
m_activeArea.clear();
|
||||
m_patchMagnitude=hg.m_patchMagnitude;
|
||||
m_patchSize=hg.m_patchSize;
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
||||
template <class Cell>
|
||||
void HierarchicalArray2D<Cell>::setActiveArea(const typename HierarchicalArray2D<Cell>::PointSet& aa, bool patchCoords){
|
||||
m_activeArea.clear();
|
||||
for (PointSet::const_iterator it= aa.begin(); it!=aa.end(); ++it) {
|
||||
IntPoint p;
|
||||
if (patchCoords)
|
||||
p=*it;
|
||||
else
|
||||
p=patchIndexes(*it);
|
||||
m_activeArea.insert(p);
|
||||
}
|
||||
}
|
||||
|
||||
template <class Cell>
|
||||
Array2D<Cell>* HierarchicalArray2D<Cell>::createPatch(const IntPoint& ) const{
|
||||
return new Array2D<Cell>(1<<m_patchMagnitude, 1<<m_patchMagnitude);
|
||||
}
|
||||
|
||||
|
||||
template <class Cell>
|
||||
AccessibilityState HierarchicalArray2D<Cell>::cellState(int x, int y) const {
|
||||
if (this->isInside(patchIndexes(x,y))) {
|
||||
if(isAllocated(x,y))
|
||||
return (AccessibilityState)((int)Inside|(int)Allocated);
|
||||
else
|
||||
return Inside;
|
||||
}
|
||||
return Outside;
|
||||
}
|
||||
|
||||
template <class Cell>
|
||||
void HierarchicalArray2D<Cell>::allocActiveArea(){
|
||||
for (PointSet::const_iterator it= m_activeArea.begin(); it!=m_activeArea.end(); ++it){
|
||||
const autoptr< Array2D<Cell> >& ptr=this->m_cells[it->x][it->y];
|
||||
Array2D<Cell>* patch=0;
|
||||
if (!ptr){
|
||||
patch=createPatch(*it);
|
||||
} else{
|
||||
patch=new Array2D<Cell>(*ptr);
|
||||
}
|
||||
this->m_cells[it->x][it->y]=autoptr< Array2D<Cell> >(patch);
|
||||
}
|
||||
}
|
||||
|
||||
template <class Cell>
|
||||
bool HierarchicalArray2D<Cell>::isAllocated(int x, int y) const{
|
||||
IntPoint c=patchIndexes(x,y);
|
||||
autoptr< Array2D<Cell> >& ptr=this->m_cells[c.x][c.y];
|
||||
return (ptr != 0);
|
||||
}
|
||||
|
||||
template <class Cell>
|
||||
IntPoint HierarchicalArray2D<Cell>::patchIndexes(int x, int y) const{
|
||||
if (x>=0 && y>=0)
|
||||
return IntPoint(x>>m_patchMagnitude, y>>m_patchMagnitude);
|
||||
return IntPoint(-1, -1);
|
||||
}
|
||||
|
||||
template <class Cell>
|
||||
Cell& HierarchicalArray2D<Cell>::cell(int x, int y){
|
||||
IntPoint c=patchIndexes(x,y);
|
||||
assert(this->isInside(c.x, c.y));
|
||||
if (!this->m_cells[c.x][c.y]){
|
||||
Array2D<Cell>* patch=createPatch(IntPoint(x,y));
|
||||
this->m_cells[c.x][c.y]=autoptr< Array2D<Cell> >(patch);
|
||||
//cerr << "!!! FATAL: your dick is going to fall down" << endl;
|
||||
}
|
||||
autoptr< Array2D<Cell> >& ptr=this->m_cells[c.x][c.y];
|
||||
return (*ptr).cell(IntPoint(x-(c.x<<m_patchMagnitude),y-(c.y<<m_patchMagnitude)));
|
||||
}
|
||||
|
||||
template <class Cell>
|
||||
const Cell& HierarchicalArray2D<Cell>::cell(int x, int y) const{
|
||||
assert(isAllocated(x,y));
|
||||
IntPoint c=patchIndexes(x,y);
|
||||
const autoptr< Array2D<Cell> >& ptr=this->m_cells[c.x][c.y];
|
||||
return (*ptr).cell(IntPoint(x-(c.x<<m_patchMagnitude),y-(c.y<<m_patchMagnitude)));
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
@@ -0,0 +1,265 @@
|
||||
#ifndef MAP_H
|
||||
#define MAP_H
|
||||
#include <gmapping/utils/point.h>
|
||||
#include <assert.h>
|
||||
#include "accessstate.h"
|
||||
#include "array2d.h"
|
||||
|
||||
namespace GMapping {
|
||||
/**
|
||||
The cells have to define the special value Cell::Unknown to handle with the unallocated areas.
|
||||
The cells have to define (int) constructor;
|
||||
*/
|
||||
typedef Array2D<double> DoubleArray2D;
|
||||
|
||||
template <class Cell, class Storage, const bool isClass=true>
|
||||
class Map{
|
||||
public:
|
||||
Map(int mapSizeX, int mapSizeY, double delta);
|
||||
Map(const Point& center, double worldSizeX, double worldSizeY, double delta);
|
||||
Map(const Point& center, double xmin, double ymin, double xmax, double ymax, double delta);
|
||||
/* the standard implementation works filen in this case*/
|
||||
//Map(const Map& g);
|
||||
//Map& operator =(const Map& g);
|
||||
void resize(double xmin, double ymin, double xmax, double ymax);
|
||||
void grow(double xmin, double ymin, double xmax, double ymax);
|
||||
inline IntPoint world2map(const Point& p) const;
|
||||
inline Point map2world(const IntPoint& p) const;
|
||||
inline IntPoint world2map(double x, double y) const
|
||||
{ return world2map(Point(x,y)); }
|
||||
inline Point map2world(int x, int y) const
|
||||
{ return map2world(IntPoint(x,y)); }
|
||||
|
||||
inline Point getCenter() const {return m_center;}
|
||||
inline double getWorldSizeX() const {return m_worldSizeX;}
|
||||
inline double getWorldSizeY() const {return m_worldSizeY;}
|
||||
inline int getMapSizeX() const {return m_mapSizeX;}
|
||||
inline int getMapSizeY() const {return m_mapSizeY;}
|
||||
inline double getDelta() const { return m_delta;}
|
||||
inline double getMapResolution() const { return m_delta;}
|
||||
inline double getResolution() const { return m_delta;}
|
||||
inline void getSize(double & xmin, double& ymin, double& xmax, double& ymax) const {
|
||||
Point min=map2world(0,0), max=map2world(IntPoint(m_mapSizeX-1, m_mapSizeY-1));
|
||||
xmin=min.x, ymin=min.y, xmax=max.x, ymax=max.y;
|
||||
}
|
||||
|
||||
inline Cell& cell(int x, int y) {
|
||||
return cell(IntPoint(x, y));
|
||||
}
|
||||
inline Cell& cell(const IntPoint& p);
|
||||
|
||||
inline const Cell& cell(int x, int y) const {
|
||||
return cell(IntPoint(x, y));
|
||||
}
|
||||
inline const Cell& cell(const IntPoint& p) const;
|
||||
|
||||
inline Cell& cell(double x, double y) {
|
||||
return cell(Point(x, y));
|
||||
}
|
||||
inline Cell& cell(const Point& p);
|
||||
|
||||
inline const Cell& cell(double x, double y) const {
|
||||
return cell(Point(x, y));
|
||||
}
|
||||
|
||||
inline bool isInside(int x, int y) const {
|
||||
return m_storage.cellState(IntPoint(x,y))&Inside;
|
||||
}
|
||||
inline bool isInside(const IntPoint& p) const {
|
||||
return m_storage.cellState(p)&Inside;
|
||||
}
|
||||
|
||||
inline bool isInside(double x, double y) const {
|
||||
return m_storage.cellState(world2map(x,y))&Inside;
|
||||
}
|
||||
inline bool isInside(const Point& p) const {
|
||||
return m_storage.cellState(world2map(p))&Inside;
|
||||
}
|
||||
|
||||
inline const Cell& cell(const Point& p) const;
|
||||
|
||||
inline Storage& storage() { return m_storage; }
|
||||
inline const Storage& storage() const { return m_storage; }
|
||||
DoubleArray2D* toDoubleArray() const;
|
||||
Map<double, DoubleArray2D, false>* toDoubleMap() const;
|
||||
|
||||
protected:
|
||||
Point m_center;
|
||||
double m_worldSizeX, m_worldSizeY, m_delta;
|
||||
Storage m_storage;
|
||||
int m_mapSizeX, m_mapSizeY;
|
||||
int m_sizeX2, m_sizeY2;
|
||||
static const Cell m_unknown;
|
||||
};
|
||||
|
||||
typedef Map<double, DoubleArray2D, false> DoubleMap;
|
||||
|
||||
template <class Cell, class Storage, const bool isClass>
|
||||
const Cell Map<Cell,Storage,isClass>::m_unknown = Cell(-1);
|
||||
|
||||
template <class Cell, class Storage, const bool isClass>
|
||||
Map<Cell,Storage,isClass>::Map(int mapSizeX, int mapSizeY, double delta):
|
||||
m_storage(mapSizeX, mapSizeY){
|
||||
m_worldSizeX=mapSizeX * delta;
|
||||
m_worldSizeY=mapSizeY * delta;
|
||||
m_delta=delta;
|
||||
m_center=Point(0.5*m_worldSizeX, 0.5*m_worldSizeY);
|
||||
m_sizeX2=m_mapSizeX>>1;
|
||||
m_sizeY2=m_mapSizeY>>1;
|
||||
}
|
||||
|
||||
template <class Cell, class Storage, const bool isClass>
|
||||
Map<Cell,Storage,isClass>::Map(const Point& center, double worldSizeX, double worldSizeY, double delta):
|
||||
m_storage((int)ceil(worldSizeX/delta), (int)ceil(worldSizeY/delta)){
|
||||
m_center=center;
|
||||
m_worldSizeX=worldSizeX;
|
||||
m_worldSizeY=worldSizeY;
|
||||
m_delta=delta;
|
||||
m_mapSizeX=m_storage.getXSize()<<m_storage.getPatchSize();
|
||||
m_mapSizeY=m_storage.getYSize()<<m_storage.getPatchSize();
|
||||
m_sizeX2=m_mapSizeX>>1;
|
||||
m_sizeY2=m_mapSizeY>>1;
|
||||
}
|
||||
|
||||
template <class Cell, class Storage, const bool isClass>
|
||||
Map<Cell,Storage,isClass>::Map(const Point& center, double xmin, double ymin, double xmax, double ymax, double delta):
|
||||
m_storage((int)ceil((xmax-xmin)/delta), (int)ceil((ymax-ymin)/delta)){
|
||||
m_center=center;
|
||||
m_worldSizeX=xmax-xmin;
|
||||
m_worldSizeY=ymax-ymin;
|
||||
m_delta=delta;
|
||||
m_mapSizeX=m_storage.getXSize()<<m_storage.getPatchSize();
|
||||
m_mapSizeY=m_storage.getYSize()<<m_storage.getPatchSize();
|
||||
m_sizeX2=(int)round((m_center.x-xmin)/m_delta);
|
||||
m_sizeY2=(int)round((m_center.y-ymin)/m_delta);
|
||||
}
|
||||
|
||||
template <class Cell, class Storage, const bool isClass>
|
||||
void Map<Cell,Storage,isClass>::resize(double xmin, double ymin, double xmax, double ymax){
|
||||
IntPoint imin=world2map(xmin, ymin);
|
||||
IntPoint imax=world2map(xmax, ymax);
|
||||
int pxmin, pymin, pxmax, pymax;
|
||||
pxmin=(int)floor((float)imin.x/(1<<m_storage.getPatchMagnitude()));
|
||||
pxmax=(int)ceil((float)imax.x/(1<<m_storage.getPatchMagnitude()));
|
||||
pymin=(int)floor((float)imin.y/(1<<m_storage.getPatchMagnitude()));
|
||||
pymax=(int)ceil((float)imax.y/(1<<m_storage.getPatchMagnitude()));
|
||||
m_storage.resize(pxmin, pymin, pxmax, pymax);
|
||||
m_mapSizeX=m_storage.getXSize()<<m_storage.getPatchSize();
|
||||
m_mapSizeY=m_storage.getYSize()<<m_storage.getPatchSize();
|
||||
m_worldSizeX=xmax-xmin;
|
||||
m_worldSizeY=ymax-ymin;
|
||||
m_sizeX2-=pxmin*(1<<m_storage.getPatchMagnitude());
|
||||
m_sizeY2-=pymin*(1<<m_storage.getPatchMagnitude());
|
||||
}
|
||||
|
||||
template <class Cell, class Storage, const bool isClass>
|
||||
void Map<Cell,Storage,isClass>::grow(double xmin, double ymin, double xmax, double ymax){
|
||||
IntPoint imin=world2map(xmin, ymin);
|
||||
IntPoint imax=world2map(xmax, ymax);
|
||||
if (isInside(imin) && isInside(imax))
|
||||
return;
|
||||
imin=min(imin, IntPoint(0,0));
|
||||
imax=max(imax, IntPoint(m_mapSizeX-1,m_mapSizeY-1));
|
||||
int pxmin, pymin, pxmax, pymax;
|
||||
pxmin=(int)floor((float)imin.x/(1<<m_storage.getPatchMagnitude()));
|
||||
pxmax=(int)ceil((float)imax.x/(1<<m_storage.getPatchMagnitude()));
|
||||
pymin=(int)floor((float)imin.y/(1<<m_storage.getPatchMagnitude()));
|
||||
pymax=(int)ceil((float)imax.y/(1<<m_storage.getPatchMagnitude()));
|
||||
m_storage.resize(pxmin, pymin, pxmax, pymax);
|
||||
m_mapSizeX=m_storage.getXSize()<<m_storage.getPatchSize();
|
||||
m_mapSizeY=m_storage.getYSize()<<m_storage.getPatchSize();
|
||||
m_worldSizeX=xmax-xmin;
|
||||
m_worldSizeY=ymax-ymin;
|
||||
m_sizeX2-=pxmin*(1<<m_storage.getPatchMagnitude());
|
||||
m_sizeY2-=pymin*(1<<m_storage.getPatchMagnitude());
|
||||
}
|
||||
|
||||
|
||||
template <class Cell, class Storage, const bool isClass>
|
||||
IntPoint Map<Cell,Storage,isClass>::world2map(const Point& p) const{
|
||||
return IntPoint( (int)round((p.x-m_center.x)/m_delta)+m_sizeX2, (int)round((p.y-m_center.y)/m_delta)+m_sizeY2);
|
||||
}
|
||||
|
||||
template <class Cell, class Storage, const bool isClass>
|
||||
Point Map<Cell,Storage,isClass>::map2world(const IntPoint& p) const{
|
||||
return Point( (p.x-m_sizeX2)*m_delta,
|
||||
(p.y-m_sizeY2)*m_delta)+m_center;
|
||||
}
|
||||
|
||||
|
||||
template <class Cell, class Storage, const bool isClass>
|
||||
Cell& Map<Cell,Storage,isClass>::cell(const IntPoint& p) {
|
||||
AccessibilityState s=m_storage.cellState(p);
|
||||
if (! (s&Inside))
|
||||
assert(0);
|
||||
//if (s&Allocated) return m_storage.cell(p); assert(0);
|
||||
|
||||
// this will never happend. Just to satify the compiler..
|
||||
return m_storage.cell(p);
|
||||
|
||||
}
|
||||
|
||||
template <class Cell, class Storage, const bool isClass>
|
||||
Cell& Map<Cell,Storage,isClass>::cell(const Point& p) {
|
||||
IntPoint ip=world2map(p);
|
||||
AccessibilityState s=m_storage.cellState(ip);
|
||||
if (! (s&Inside))
|
||||
assert(0);
|
||||
//if (s&Allocated) return m_storage.cell(ip); assert(0);
|
||||
|
||||
// this will never happend. Just to satify the compiler..
|
||||
return m_storage.cell(ip);
|
||||
}
|
||||
|
||||
template <class Cell, class Storage, const bool isClass>
|
||||
const Cell& Map<Cell,Storage,isClass>::cell(const IntPoint& p) const {
|
||||
AccessibilityState s=m_storage.cellState(p);
|
||||
//if (! s&Inside) assert(0);
|
||||
if (s&Allocated)
|
||||
return m_storage.cell(p);
|
||||
return m_unknown;
|
||||
}
|
||||
|
||||
template <class Cell, class Storage, const bool isClass>
|
||||
const Cell& Map<Cell,Storage,isClass>::cell(const Point& p) const {
|
||||
IntPoint ip=world2map(p);
|
||||
AccessibilityState s=m_storage.cellState(ip);
|
||||
//if (! s&Inside) assert(0);
|
||||
if (s&Allocated)
|
||||
return m_storage.cell(ip);
|
||||
return m_unknown;
|
||||
}
|
||||
|
||||
|
||||
//FIXME check why the last line of the map is corrupted.
|
||||
template <class Cell, class Storage, const bool isClass>
|
||||
DoubleArray2D* Map<Cell,Storage,isClass>::toDoubleArray() const{
|
||||
DoubleArray2D* darr=new DoubleArray2D(getMapSizeX()-1, getMapSizeY()-1);
|
||||
for(int x=0; x<getMapSizeX()-1; x++)
|
||||
for(int y=0; y<getMapSizeY()-1; y++){
|
||||
IntPoint p(x,y);
|
||||
darr->cell(p)=cell(p);
|
||||
}
|
||||
return darr;
|
||||
}
|
||||
|
||||
|
||||
template <class Cell, class Storage, const bool isClass>
|
||||
Map<double, DoubleArray2D, false>* Map<Cell,Storage,isClass>::toDoubleMap() const{
|
||||
//FIXME size the map so that m_center will be setted accordingly
|
||||
Point pmin=map2world(IntPoint(0,0));
|
||||
Point pmax=map2world(getMapSizeX()-1,getMapSizeY()-1);
|
||||
Point center=(pmax+pmin)*0.5;
|
||||
Map<double, DoubleArray2D, false>* plainMap=new Map<double, DoubleArray2D, false>(center, (pmax-pmin).x, (pmax-pmin).y, getDelta());
|
||||
for(int x=0; x<getMapSizeX()-1; x++)
|
||||
for(int y=0; y<getMapSizeY()-1; y++){
|
||||
IntPoint p(x,y);
|
||||
plainMap->cell(p)=cell(p);
|
||||
}
|
||||
return plainMap;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
@@ -0,0 +1,337 @@
|
||||
#ifndef GRIDSLAMPROCESSOR_H
|
||||
#define GRIDSLAMPROCESSOR_H
|
||||
|
||||
#include <climits>
|
||||
#include <limits>
|
||||
#include <fstream>
|
||||
#include <vector>
|
||||
#include <deque>
|
||||
#include <gmapping/particlefilter/particlefilter.h>
|
||||
#include <gmapping/utils/point.h>
|
||||
#include <gmapping/utils/macro_params.h>
|
||||
#include <gmapping/log/sensorlog.h>
|
||||
#include <gmapping/sensor/sensor_range/rangesensor.h>
|
||||
#include <gmapping/sensor/sensor_range/rangereading.h>
|
||||
#include <gmapping/scanmatcher/scanmatcher.h>
|
||||
#include "motionmodel.h"
|
||||
|
||||
|
||||
namespace GMapping {
|
||||
|
||||
/**This class defines the basic GridFastSLAM algorithm. It
|
||||
implements a rao blackwellized particle filter. Each particle
|
||||
has its own map and robot pose.<br> This implementation works
|
||||
as follows: each time a new pair odometry/laser reading is
|
||||
received, the particle's robot pose is updated according to the
|
||||
motion model. This pose is subsequently used for initalizing a
|
||||
scan matching algorithm. The scanmatcher performs a local
|
||||
optimization for each particle. It is initialized with the
|
||||
pose drawn from the motion model, and the pose is corrected
|
||||
according to the each particle map.<br>
|
||||
In order to avoid unnecessary computation the filter state is updated
|
||||
only when the robot moves more than a given threshold.
|
||||
*/
|
||||
class GridSlamProcessor{
|
||||
public:
|
||||
|
||||
|
||||
/**This class defines the the node of reversed tree in which the trajectories are stored.
|
||||
Each node of a tree has a pointer to its parent and a counter indicating the number of childs of a node.
|
||||
The tree is updated in a way consistent with the operation performed on the particles.
|
||||
*/
|
||||
struct TNode{
|
||||
/**Constructs a node of the trajectory tree.
|
||||
@param pose: the pose of the robot in the trajectory
|
||||
@param weight: the weight of the particle at that point in the trajectory
|
||||
@param accWeight: the cumulative weight of the particle
|
||||
@param parent: the parent node in the tree
|
||||
@param childs: the number of childs
|
||||
*/
|
||||
TNode(const OrientedPoint& pose, double weight, TNode* parent=0, unsigned int childs=0);
|
||||
|
||||
/**Destroys a tree node, and consistently updates the tree. If a node whose parent has only one child is deleted,
|
||||
also the parent node is deleted. This because the parent will not be reacheable anymore in the trajectory tree.*/
|
||||
~TNode();
|
||||
|
||||
/**The pose of the robot*/
|
||||
OrientedPoint pose;
|
||||
|
||||
/**The weight of the particle*/
|
||||
double weight;
|
||||
|
||||
/**The sum of all the particle weights in the previous part of the trajectory*/
|
||||
double accWeight;
|
||||
|
||||
double gweight;
|
||||
|
||||
|
||||
/**The parent*/
|
||||
TNode* parent;
|
||||
|
||||
/**The range reading to which this node is associated*/
|
||||
const RangeReading* reading;
|
||||
|
||||
/**The number of childs*/
|
||||
unsigned int childs;
|
||||
|
||||
/**counter in visiting the node (internally used)*/
|
||||
mutable unsigned int visitCounter;
|
||||
|
||||
/**visit flag (internally used)*/
|
||||
mutable bool flag;
|
||||
};
|
||||
|
||||
typedef std::vector<GridSlamProcessor::TNode*> TNodeVector;
|
||||
typedef std::deque<GridSlamProcessor::TNode*> TNodeDeque;
|
||||
|
||||
/**This class defines a particle of the filter. Each particle has a map, a pose, a weight and retains the current node in the trajectory tree*/
|
||||
struct Particle{
|
||||
/**constructs a particle, given a map
|
||||
@param map: the particle map
|
||||
*/
|
||||
Particle(const ScanMatcherMap& map);
|
||||
|
||||
/** @returns the weight of a particle */
|
||||
inline operator double() const {return weight;}
|
||||
/** @returns the pose of a particle */
|
||||
inline operator OrientedPoint() const {return pose;}
|
||||
/** sets the weight of a particle
|
||||
@param w the weight
|
||||
*/
|
||||
inline void setWeight(double w) {weight=w;}
|
||||
/** The map */
|
||||
ScanMatcherMap map;
|
||||
/** The pose of the robot */
|
||||
OrientedPoint pose;
|
||||
|
||||
/** The pose of the robot at the previous time frame (used for computing thr odometry displacements) */
|
||||
OrientedPoint previousPose;
|
||||
|
||||
/** The weight of the particle */
|
||||
double weight;
|
||||
|
||||
/** The cumulative weight of the particle */
|
||||
double weightSum;
|
||||
|
||||
double gweight;
|
||||
|
||||
/** The index of the previous particle in the trajectory tree */
|
||||
int previousIndex;
|
||||
|
||||
/** Entry to the trajectory tree */
|
||||
TNode* node;
|
||||
};
|
||||
|
||||
|
||||
typedef std::vector<Particle> ParticleVector;
|
||||
|
||||
/** Constructs a GridSlamProcessor, initialized with the default parameters */
|
||||
GridSlamProcessor();
|
||||
|
||||
/** Constructs a GridSlamProcessor, whose output is routed to a stream.
|
||||
@param infoStr: the output stream
|
||||
*/
|
||||
GridSlamProcessor(std::ostream& infoStr);
|
||||
|
||||
/** @returns a deep copy of the grid slam processor with all the internal structures.
|
||||
*/
|
||||
GridSlamProcessor* clone() const;
|
||||
|
||||
/**Deleted the gridslamprocessor*/
|
||||
virtual ~GridSlamProcessor();
|
||||
|
||||
//methods for accessing the parameters
|
||||
void setSensorMap(const SensorMap& smap);
|
||||
void init(unsigned int size, double xmin, double ymin, double xmax, double ymax, double delta,
|
||||
OrientedPoint initialPose=OrientedPoint(0,0,0));
|
||||
void setMatchingParameters(double urange, double range, double sigma, int kernsize, double lopt, double aopt,
|
||||
int iterations, double likelihoodSigma=1, double likelihoodGain=1, unsigned int likelihoodSkip=0);
|
||||
void setMotionModelParameters(double srr, double srt, double str, double stt);
|
||||
void setUpdateDistances(double linear, double angular, double resampleThreshold);
|
||||
void setUpdatePeriod(double p) {period_=p;}
|
||||
|
||||
//the "core" algorithm
|
||||
void processTruePos(const OdometryReading& odometry);
|
||||
bool processScan(const RangeReading & reading, int adaptParticles=0);
|
||||
|
||||
/**This method copies the state of the filter in a tree.
|
||||
The tree is represented through reversed pointers (each node has a pointer to its parent).
|
||||
The leafs are stored in a vector, whose size is the same as the number of particles.
|
||||
@returns the leafs of the tree
|
||||
*/
|
||||
TNodeVector getTrajectories() const;
|
||||
void integrateScanSequence(TNode* node);
|
||||
|
||||
/**the scanmatcher algorithm*/
|
||||
ScanMatcher m_matcher;
|
||||
/**the stream used for writing the output of the algorithm*/
|
||||
std::ofstream& outputStream();
|
||||
/**the stream used for writing the info/debug messages*/
|
||||
std::ostream& infoStream();
|
||||
/**@returns the particles*/
|
||||
inline const ParticleVector& getParticles() const {return m_particles; }
|
||||
|
||||
inline const std::vector<unsigned int>& getIndexes() const{return m_indexes; }
|
||||
int getBestParticleIndex() const;
|
||||
//callbacks
|
||||
virtual void onOdometryUpdate();
|
||||
virtual void onResampleUpdate();
|
||||
virtual void onScanmatchUpdate();
|
||||
|
||||
//accessor methods
|
||||
/**the maxrange of the laser to consider */
|
||||
MEMBER_PARAM_SET_GET(m_matcher, double, laserMaxRange, protected, public, public);
|
||||
|
||||
/**the maximum usable range of the laser. A beam is cropped to this value. [scanmatcher]*/
|
||||
MEMBER_PARAM_SET_GET(m_matcher, double, usableRange, protected, public, public);
|
||||
|
||||
/**The sigma used by the greedy endpoint matching. [scanmatcher]*/
|
||||
MEMBER_PARAM_SET_GET(m_matcher,double, gaussianSigma, protected, public, public);
|
||||
|
||||
/**The sigma of a beam used for likelihood computation [scanmatcher]*/
|
||||
MEMBER_PARAM_SET_GET(m_matcher,double, likelihoodSigma, protected, public, public);
|
||||
|
||||
/**The kernel in which to look for a correspondence[scanmatcher]*/
|
||||
MEMBER_PARAM_SET_GET(m_matcher, int, kernelSize, protected, public, public);
|
||||
|
||||
/**The optimization step in rotation [scanmatcher]*/
|
||||
MEMBER_PARAM_SET_GET(m_matcher, double, optAngularDelta, protected, public, public);
|
||||
|
||||
/**The optimization step in translation [scanmatcher]*/
|
||||
MEMBER_PARAM_SET_GET(m_matcher, double, optLinearDelta, protected, public, public);
|
||||
|
||||
/**The number of iterations of the scanmatcher [scanmatcher]*/
|
||||
MEMBER_PARAM_SET_GET(m_matcher, unsigned int, optRecursiveIterations, protected, public, public);
|
||||
|
||||
/**the beams to skip for computing the likelihood (consider a beam every likelihoodSkip) [scanmatcher]*/
|
||||
MEMBER_PARAM_SET_GET(m_matcher, unsigned int, likelihoodSkip, protected, public, public);
|
||||
|
||||
/**translational sampling range for the likelihood [scanmatcher]*/
|
||||
MEMBER_PARAM_SET_GET(m_matcher, double, llsamplerange, protected, public, public);
|
||||
|
||||
/**angular sampling range for the likelihood [scanmatcher]*/
|
||||
MEMBER_PARAM_SET_GET(m_matcher, double, lasamplerange, protected, public, public);
|
||||
|
||||
/**translational sampling range for the likelihood [scanmatcher]*/
|
||||
MEMBER_PARAM_SET_GET(m_matcher, double, llsamplestep, protected, public, public);
|
||||
|
||||
/**angular sampling step for the likelihood [scanmatcher]*/
|
||||
MEMBER_PARAM_SET_GET(m_matcher, double, lasamplestep, protected, public, public);
|
||||
|
||||
/**generate an accupancy grid map [scanmatcher]*/
|
||||
MEMBER_PARAM_SET_GET(m_matcher, bool, generateMap, protected, public, public);
|
||||
|
||||
/**enlarge the map when the robot goes out of the boundaries [scanmatcher]*/
|
||||
MEMBER_PARAM_SET_GET(m_matcher, bool, enlargeStep, protected, public, public);
|
||||
|
||||
/**pose of the laser wrt the robot [scanmatcher]*/
|
||||
MEMBER_PARAM_SET_GET(m_matcher, OrientedPoint, laserPose, protected, public, public);
|
||||
|
||||
|
||||
/**odometry error in translation as a function of translation (rho/rho) [motionmodel]*/
|
||||
STRUCT_PARAM_SET_GET(m_motionModel, double, srr, protected, public, public);
|
||||
|
||||
/**odometry error in translation as a function of rotation (rho/theta) [motionmodel]*/
|
||||
STRUCT_PARAM_SET_GET(m_motionModel, double, srt, protected, public, public);
|
||||
|
||||
/**odometry error in rotation as a function of translation (theta/rho) [motionmodel]*/
|
||||
STRUCT_PARAM_SET_GET(m_motionModel, double, str, protected, public, public);
|
||||
|
||||
/**odometry error in rotation as a function of rotation (theta/theta) [motionmodel]*/
|
||||
STRUCT_PARAM_SET_GET(m_motionModel, double, stt, protected, public, public);
|
||||
|
||||
/**minimum score for considering the outcome of the scanmatching good*/
|
||||
PARAM_SET_GET(double, minimumScore, protected, public, public);
|
||||
|
||||
protected:
|
||||
/**Copy constructor*/
|
||||
GridSlamProcessor(const GridSlamProcessor& gsp);
|
||||
|
||||
/**the laser beams*/
|
||||
unsigned int m_beams;
|
||||
double last_update_time_;
|
||||
double period_;
|
||||
|
||||
/**the particles*/
|
||||
ParticleVector m_particles;
|
||||
|
||||
/**the particle indexes after resampling (internally used)*/
|
||||
std::vector<unsigned int> m_indexes;
|
||||
|
||||
/**the particle weights (internally used)*/
|
||||
std::vector<double> m_weights;
|
||||
|
||||
/**the motion model*/
|
||||
MotionModel m_motionModel;
|
||||
|
||||
/**this sets the neff based resampling threshold*/
|
||||
PARAM_SET_GET(double, resampleThreshold, protected, public, public);
|
||||
|
||||
//state
|
||||
int m_count, m_readingCount;
|
||||
OrientedPoint m_lastPartPose;
|
||||
OrientedPoint m_odoPose;
|
||||
OrientedPoint m_pose;
|
||||
double m_linearDistance, m_angularDistance;
|
||||
PARAM_GET(double, neff, protected, public);
|
||||
|
||||
//processing parameters (size of the map)
|
||||
PARAM_GET(double, xmin, protected, public);
|
||||
PARAM_GET(double, ymin, protected, public);
|
||||
PARAM_GET(double, xmax, protected, public);
|
||||
PARAM_GET(double, ymax, protected, public);
|
||||
//processing parameters (resolution of the map)
|
||||
PARAM_GET(double, delta, protected, public);
|
||||
|
||||
//registration score (if a scan score is above this threshold it is registered in the map)
|
||||
PARAM_SET_GET(double, regScore, protected, public, public);
|
||||
//registration score (if a scan score is below this threshold a scan matching failure is reported)
|
||||
PARAM_SET_GET(double, critScore, protected, public, public);
|
||||
//registration score maximum move allowed between consecutive scans
|
||||
PARAM_SET_GET(double, maxMove, protected, public, public);
|
||||
|
||||
//process a scan each time the robot translates of linearThresholdDistance
|
||||
PARAM_SET_GET(double, linearThresholdDistance, protected, public, public);
|
||||
|
||||
//process a scan each time the robot rotates more than angularThresholdDistance
|
||||
PARAM_SET_GET(double, angularThresholdDistance, protected, public, public);
|
||||
|
||||
//smoothing factor for the likelihood
|
||||
PARAM_SET_GET(double, obsSigmaGain, protected, public, public);
|
||||
|
||||
//stream in which to write the gfs file
|
||||
std::ofstream m_outputStream;
|
||||
|
||||
// stream in which to write the messages
|
||||
std::ostream& m_infoStream;
|
||||
|
||||
|
||||
// the functions below performs side effect on the internal structure,
|
||||
//should be called only inside the processScan method
|
||||
private:
|
||||
|
||||
/**scanmatches all the particles*/
|
||||
inline void scanMatch(const double *plainReading);
|
||||
/**normalizes the particle weights*/
|
||||
inline void normalize();
|
||||
|
||||
// return if a resampling occured or not
|
||||
inline bool resample(const double* plainReading, int adaptParticles,
|
||||
const RangeReading* rr=0);
|
||||
|
||||
//tree utilities
|
||||
|
||||
void updateTreeWeights(bool weightsAlreadyNormalized = false);
|
||||
void resetTree();
|
||||
double propagateWeights();
|
||||
|
||||
};
|
||||
|
||||
typedef std::multimap<const GridSlamProcessor::TNode*, GridSlamProcessor::TNode*> TNodeMultimap;
|
||||
|
||||
|
||||
#include "gridslamprocessor.hxx"
|
||||
|
||||
};
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,177 @@
|
||||
|
||||
#ifdef MACOSX
|
||||
// This is to overcome a possible bug in Apple's GCC.
|
||||
#define isnan(x) (x==FP_NAN)
|
||||
#endif
|
||||
|
||||
/**Just scan match every single particle.
|
||||
If the scan matching fails, the particle gets a default likelihood.*/
|
||||
inline void GridSlamProcessor::scanMatch(const double* plainReading){
|
||||
// sample a new pose from each scan in the reference
|
||||
|
||||
double sumScore=0;
|
||||
for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
|
||||
OrientedPoint corrected;
|
||||
double score, l, s;
|
||||
score=m_matcher.optimize(corrected, it->map, it->pose, plainReading);
|
||||
// it->pose=corrected;
|
||||
if (score>m_minimumScore){
|
||||
it->pose=corrected;
|
||||
} else {
|
||||
if (m_infoStream){
|
||||
m_infoStream << "Scan Matching Failed, using odometry. Likelihood=" << l <<std::endl;
|
||||
m_infoStream << "lp:" << m_lastPartPose.x << " " << m_lastPartPose.y << " "<< m_lastPartPose.theta <<std::endl;
|
||||
m_infoStream << "op:" << m_odoPose.x << " " << m_odoPose.y << " "<< m_odoPose.theta <<std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
m_matcher.likelihoodAndScore(s, l, it->map, it->pose, plainReading);
|
||||
sumScore+=score;
|
||||
it->weight+=l;
|
||||
it->weightSum+=l;
|
||||
|
||||
//set up the selective copy of the active area
|
||||
//by detaching the areas that will be updated
|
||||
m_matcher.invalidateActiveArea();
|
||||
m_matcher.computeActiveArea(it->map, it->pose, plainReading);
|
||||
}
|
||||
if (m_infoStream)
|
||||
m_infoStream << "Average Scan Matching Score=" << sumScore/m_particles.size() << std::endl;
|
||||
}
|
||||
|
||||
inline void GridSlamProcessor::normalize(){
|
||||
//normalize the log m_weights
|
||||
double gain=1./(m_obsSigmaGain*m_particles.size());
|
||||
double lmax= -std::numeric_limits<double>::max();
|
||||
for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
|
||||
lmax=it->weight>lmax?it->weight:lmax;
|
||||
}
|
||||
//cout << "!!!!!!!!!!! maxwaight= "<< lmax << endl;
|
||||
|
||||
m_weights.clear();
|
||||
double wcum=0;
|
||||
m_neff=0;
|
||||
for (std::vector<Particle>::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
|
||||
m_weights.push_back(exp(gain*(it->weight-lmax)));
|
||||
wcum+=m_weights.back();
|
||||
//cout << "l=" << it->weight<< endl;
|
||||
}
|
||||
|
||||
m_neff=0;
|
||||
for (std::vector<double>::iterator it=m_weights.begin(); it!=m_weights.end(); it++){
|
||||
*it=*it/wcum;
|
||||
double w=*it;
|
||||
m_neff+=w*w;
|
||||
}
|
||||
m_neff=1./m_neff;
|
||||
|
||||
}
|
||||
|
||||
inline bool GridSlamProcessor::resample(const double* plainReading, int adaptSize, const RangeReading* reading){
|
||||
|
||||
bool hasResampled = false;
|
||||
|
||||
TNodeVector oldGeneration;
|
||||
for (unsigned int i=0; i<m_particles.size(); i++){
|
||||
oldGeneration.push_back(m_particles[i].node);
|
||||
}
|
||||
|
||||
if (m_neff<m_resampleThreshold*m_particles.size()){
|
||||
|
||||
if (m_infoStream)
|
||||
m_infoStream << "*************RESAMPLE***************" << std::endl;
|
||||
|
||||
uniform_resampler<double, double> resampler;
|
||||
m_indexes=resampler.resampleIndexes(m_weights, adaptSize);
|
||||
|
||||
if (m_outputStream.is_open()){
|
||||
m_outputStream << "RESAMPLE "<< m_indexes.size() << " ";
|
||||
for (std::vector<unsigned int>::const_iterator it=m_indexes.begin(); it!=m_indexes.end(); it++){
|
||||
m_outputStream << *it << " ";
|
||||
}
|
||||
m_outputStream << std::endl;
|
||||
}
|
||||
|
||||
onResampleUpdate();
|
||||
//BEGIN: BUILDING TREE
|
||||
ParticleVector temp;
|
||||
unsigned int j=0;
|
||||
std::vector<unsigned int> deletedParticles; //this is for deleteing the particles which have been resampled away.
|
||||
|
||||
// cerr << "Existing Nodes:" ;
|
||||
for (unsigned int i=0; i<m_indexes.size(); i++){
|
||||
// cerr << " " << m_indexes[i];
|
||||
while(j<m_indexes[i]){
|
||||
deletedParticles.push_back(j);
|
||||
j++;
|
||||
}
|
||||
if (j==m_indexes[i])
|
||||
j++;
|
||||
Particle & p=m_particles[m_indexes[i]];
|
||||
TNode* node=0;
|
||||
TNode* oldNode=oldGeneration[m_indexes[i]];
|
||||
// cerr << i << "->" << m_indexes[i] << "B("<<oldNode->childs <<") ";
|
||||
node=new TNode(p.pose, 0, oldNode, 0);
|
||||
//node->reading=0;
|
||||
node->reading=reading;
|
||||
// cerr << "A("<<node->parent->childs <<") " <<endl;
|
||||
|
||||
temp.push_back(p);
|
||||
temp.back().node=node;
|
||||
temp.back().previousIndex=m_indexes[i];
|
||||
}
|
||||
while(j<m_indexes.size()){
|
||||
deletedParticles.push_back(j);
|
||||
j++;
|
||||
}
|
||||
// cerr << endl;
|
||||
std::cerr << "Deleting Nodes:";
|
||||
for (unsigned int i=0; i<deletedParticles.size(); i++){
|
||||
std::cerr <<" " << deletedParticles[i];
|
||||
delete m_particles[deletedParticles[i]].node;
|
||||
m_particles[deletedParticles[i]].node=0;
|
||||
}
|
||||
std::cerr << " Done" <<std::endl;
|
||||
|
||||
//END: BUILDING TREE
|
||||
std::cerr << "Deleting old particles..." ;
|
||||
m_particles.clear();
|
||||
std::cerr << "Done" << std::endl;
|
||||
std::cerr << "Copying Particles and Registering scans...";
|
||||
for (ParticleVector::iterator it=temp.begin(); it!=temp.end(); it++){
|
||||
it->setWeight(0);
|
||||
m_matcher.invalidateActiveArea();
|
||||
m_matcher.registerScan(it->map, it->pose, plainReading);
|
||||
m_particles.push_back(*it);
|
||||
}
|
||||
std::cerr << " Done" <<std::endl;
|
||||
hasResampled = true;
|
||||
} else {
|
||||
int index=0;
|
||||
std::cerr << "Registering Scans:";
|
||||
TNodeVector::iterator node_it=oldGeneration.begin();
|
||||
for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
|
||||
//create a new node in the particle tree and add it to the old tree
|
||||
//BEGIN: BUILDING TREE
|
||||
TNode* node=0;
|
||||
node=new TNode(it->pose, 0.0, *node_it, 0);
|
||||
|
||||
//node->reading=0;
|
||||
node->reading=reading;
|
||||
it->node=node;
|
||||
|
||||
//END: BUILDING TREE
|
||||
m_matcher.invalidateActiveArea();
|
||||
m_matcher.registerScan(it->map, it->pose, plainReading);
|
||||
it->previousIndex=index;
|
||||
index++;
|
||||
node_it++;
|
||||
|
||||
}
|
||||
std::cerr << "Done" <<std::endl;
|
||||
|
||||
}
|
||||
//END: BUILDING TREE
|
||||
|
||||
return hasResampled;
|
||||
}
|
||||
@@ -0,0 +1,19 @@
|
||||
#ifndef MOTIONMODEL_H
|
||||
#define MOTIONMODEL_H
|
||||
|
||||
#include <gmapping/utils/point.h>
|
||||
#include <gmapping/utils/stat.h>
|
||||
#include <gmapping/utils/macro_params.h>
|
||||
|
||||
namespace GMapping {
|
||||
|
||||
struct MotionModel{
|
||||
OrientedPoint drawFromMotion(const OrientedPoint& p, double linearMove, double angularMove) const;
|
||||
OrientedPoint drawFromMotion(const OrientedPoint& p, const OrientedPoint& pnew, const OrientedPoint& pold) const;
|
||||
Covariance3 gaussianApproximation(const OrientedPoint& pnew, const OrientedPoint& pold) const;
|
||||
double srr, str, srt, stt;
|
||||
};
|
||||
|
||||
};
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,17 @@
|
||||
#ifndef CONFIGURATION_H
|
||||
#define CONFIGURATION_H
|
||||
|
||||
#include <istream>
|
||||
#include <gmapping/sensor/sensor_base/sensor.h>
|
||||
|
||||
namespace GMapping {
|
||||
|
||||
class Configuration{
|
||||
public:
|
||||
virtual ~Configuration();
|
||||
virtual SensorMap computeSensorMap() const=0;
|
||||
};
|
||||
|
||||
};
|
||||
#endif
|
||||
|
||||
@@ -0,0 +1,29 @@
|
||||
#ifndef SENSORLOG_H
|
||||
#define SENSORLOG_H
|
||||
|
||||
#include <list>
|
||||
#include <istream>
|
||||
#include <gmapping/sensor/sensor_base/sensorreading.h>
|
||||
#include <gmapping/sensor/sensor_odometry/odometrysensor.h>
|
||||
#include <gmapping/sensor/sensor_range/rangesensor.h>
|
||||
#include <gmapping/sensor/sensor_odometry/odometryreading.h>
|
||||
#include <gmapping/sensor/sensor_range/rangereading.h>
|
||||
#include "configuration.h"
|
||||
|
||||
namespace GMapping {
|
||||
|
||||
class SensorLog : public std::list<SensorReading*>{
|
||||
public:
|
||||
SensorLog(const SensorMap&);
|
||||
~SensorLog();
|
||||
std::istream& load(std::istream& is);
|
||||
OrientedPoint boundingBox(double& xmin, double& ymin, double& xmax, double& ymax) const;
|
||||
protected:
|
||||
const SensorMap& m_sensorMap;
|
||||
OdometryReading* parseOdometry(std::istream& is, const OdometrySensor* ) const;
|
||||
RangeReading* parseRange(std::istream& is, const RangeSensor* ) const;
|
||||
};
|
||||
|
||||
};
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,328 @@
|
||||
#ifndef PARTICLEFILTER_H
|
||||
#define PARTICLEFILTER_H
|
||||
#include <stdlib.h>
|
||||
#include <sys/types.h>
|
||||
#include <vector>
|
||||
#include <utility>
|
||||
#include <cmath>
|
||||
#include <limits>
|
||||
#include <gmapping/utils/gvalues.h>
|
||||
|
||||
|
||||
/**
|
||||
the particle class has to be convertible into numeric data type;
|
||||
That means that a particle must define the Numeric conversion operator;
|
||||
operator Numeric() const.
|
||||
that returns the weight, and the method
|
||||
setWeight(Numeric)
|
||||
that sets the weight.
|
||||
|
||||
*/
|
||||
|
||||
typedef std::pair<uint,uint> UIntPair;
|
||||
|
||||
template <class OutputIterator, class Iterator>
|
||||
double toNormalForm(OutputIterator& out, const Iterator & begin, const Iterator & end){
|
||||
//determine the maximum
|
||||
double lmax = -std::numeric_limits<double>::max();
|
||||
for (Iterator it=begin; it!=end; it++){
|
||||
lmax=lmax>((double)(*it))? lmax: (double)(*it);
|
||||
}
|
||||
//convert to raw form
|
||||
for (Iterator it=begin; it!=end; it++){
|
||||
*out=exp((double)(*it)-lmax);
|
||||
out++;
|
||||
}
|
||||
return lmax;
|
||||
}
|
||||
|
||||
template <class OutputIterator, class Iterator, class Numeric>
|
||||
void toLogForm(OutputIterator& out, const Iterator & begin, const Iterator & end, Numeric lmax){
|
||||
//determine the maximum
|
||||
for (Iterator it=begin; it!=end; it++){
|
||||
*out=log((Numeric)(*it))-lmax;
|
||||
out++;
|
||||
}
|
||||
return lmax;
|
||||
}
|
||||
|
||||
template <class WeightVector>
|
||||
void resample(std::vector<int>& indexes, const WeightVector& weights, unsigned int nparticles=0){
|
||||
double cweight=0;
|
||||
|
||||
//compute the cumulative weights
|
||||
unsigned int n=0;
|
||||
for (typename WeightVector::const_iterator it=weights.begin(); it!=weights.end(); ++it){
|
||||
cweight+=(double)*it;
|
||||
n++;
|
||||
}
|
||||
|
||||
if (nparticles>0)
|
||||
n=nparticles;
|
||||
|
||||
//compute the interval
|
||||
double interval=cweight/n;
|
||||
|
||||
//compute the initial target weight
|
||||
double target=interval*::drand48();
|
||||
//compute the resampled indexes
|
||||
|
||||
cweight=0;
|
||||
indexes.resize(n);
|
||||
|
||||
n=0;
|
||||
unsigned int i=0;
|
||||
for (typename WeightVector::const_iterator it=weights.begin(); it!=weights.end(); ++it, ++i){
|
||||
cweight+=(double)* it;
|
||||
while(cweight>target){
|
||||
indexes[n++]=i;
|
||||
target+=interval;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
template <typename Vector>
|
||||
void repeatIndexes(Vector& dest, const std::vector<int>& indexes, const Vector& particles){
|
||||
assert(indexes.size()==particles.size());
|
||||
dest.resize(particles.size());
|
||||
unsigned int i=0;
|
||||
for (std::vector<int>::const_iterator it=indexes.begin(); it!=indexes.end(); ++it){
|
||||
dest[i]=particles[*it];
|
||||
i++;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
template <class Iterator>
|
||||
double neff(const Iterator& begin, const Iterator& end){
|
||||
double sum=0;
|
||||
for (Iterator it=begin; it!=end; ++it){
|
||||
sum+=*it;
|
||||
}
|
||||
double cum=0;
|
||||
for (Iterator it=begin; it!=end; ++it){
|
||||
double w=*it/sum;
|
||||
cum+=w*w;
|
||||
}
|
||||
return 1./cum;
|
||||
}
|
||||
|
||||
template <class Iterator>
|
||||
void normalize(const Iterator& begin, const Iterator& end){
|
||||
double sum=0;
|
||||
for (Iterator it=begin; it!=end; ++it){
|
||||
sum+=*it;
|
||||
}
|
||||
for (Iterator it=begin; it!=end; ++it){
|
||||
*it=*it/sum;
|
||||
}
|
||||
}
|
||||
|
||||
template <class OutputIterator, class Iterator>
|
||||
void rle(OutputIterator& out, const Iterator & begin, const Iterator & end){
|
||||
unsigned int current=0;
|
||||
unsigned int count=0;
|
||||
for (Iterator it=begin; it!=end; it++){
|
||||
if (it==begin){
|
||||
current=*it;
|
||||
count=1;
|
||||
continue;
|
||||
}
|
||||
if (((uint)*it) ==current)
|
||||
count++;
|
||||
if (((uint)*it)!=current){
|
||||
*out=std::make_pair(current,count);
|
||||
out++;
|
||||
current=*it;
|
||||
count=1;
|
||||
}
|
||||
}
|
||||
if (count>0)
|
||||
*out=std::make_pair(current,count);
|
||||
out++;
|
||||
}
|
||||
|
||||
//BEGIN legacy
|
||||
template <class Particle, class Numeric>
|
||||
struct uniform_resampler{
|
||||
std::vector<unsigned int> resampleIndexes(const std::vector<Particle> & particles, int nparticles=0) const;
|
||||
std::vector<Particle> resample(const std::vector<Particle> & particles, int nparticles=0) const;
|
||||
Numeric neff(const std::vector<Particle> & particles) const;
|
||||
};
|
||||
|
||||
/*Implementation of the above stuff*/
|
||||
template <class Particle, class Numeric>
|
||||
std::vector<unsigned int> uniform_resampler<Particle, Numeric>:: resampleIndexes(const std::vector<Particle>& particles, int nparticles) const{
|
||||
Numeric cweight=0;
|
||||
|
||||
//compute the cumulative weights
|
||||
unsigned int n=0;
|
||||
for (typename std::vector<Particle>::const_iterator it=particles.begin(); it!=particles.end(); ++it){
|
||||
cweight+=(Numeric)*it;
|
||||
n++;
|
||||
}
|
||||
|
||||
if (nparticles>0)
|
||||
n=nparticles;
|
||||
|
||||
//compute the interval
|
||||
Numeric interval=cweight/n;
|
||||
|
||||
//compute the initial target weight
|
||||
Numeric target=interval*::drand48();
|
||||
//compute the resampled indexes
|
||||
|
||||
cweight=0;
|
||||
std::vector<unsigned int> indexes(n);
|
||||
n=0;
|
||||
unsigned int i=0;
|
||||
for (typename std::vector<Particle>::const_iterator it=particles.begin(); it!=particles.end(); ++it, ++i){
|
||||
cweight+=(Numeric)* it;
|
||||
while(cweight>target){
|
||||
indexes[n++]=i;
|
||||
target+=interval;
|
||||
}
|
||||
}
|
||||
return indexes;
|
||||
}
|
||||
|
||||
template <class Particle, class Numeric>
|
||||
std::vector<Particle> uniform_resampler<Particle,Numeric>::resample
|
||||
(const typename std::vector<Particle>& particles, int nparticles) const{
|
||||
Numeric cweight=0;
|
||||
|
||||
//compute the cumulative weights
|
||||
unsigned int n=0;
|
||||
for (typename std::vector<Particle>::const_iterator it=particles.begin(); it!=particles.end(); ++it){
|
||||
cweight+=(Numeric)*it;
|
||||
n++;
|
||||
}
|
||||
|
||||
if (nparticles>0)
|
||||
n=nparticles;
|
||||
|
||||
//weight of the particles after resampling
|
||||
double uw=1./n;
|
||||
|
||||
//compute the interval
|
||||
Numeric interval=cweight/n;
|
||||
|
||||
//compute the initial target weight
|
||||
Numeric target=interval*::drand48();
|
||||
//compute the resampled indexes
|
||||
|
||||
cweight=0;
|
||||
std::vector<Particle> resampled;
|
||||
n=0;
|
||||
unsigned int i=0;
|
||||
for (typename std::vector<Particle>::const_iterator it=particles.begin(); it!=particles.end(); ++it, ++i){
|
||||
cweight+=(Numeric)*it;
|
||||
while(cweight>target){
|
||||
resampled.push_back(*it);
|
||||
resampled.back().setWeight(uw);
|
||||
target+=interval;
|
||||
}
|
||||
}
|
||||
return resampled;
|
||||
}
|
||||
|
||||
template <class Particle, class Numeric>
|
||||
Numeric uniform_resampler<Particle,Numeric>::neff(const std::vector<Particle> & particles) const{
|
||||
double cum=0;
|
||||
double sum=0;
|
||||
for (typename std::vector<Particle>::const_iterator it=particles.begin(); it!=particles.end(); ++it){
|
||||
Numeric w=(Numeric)*it;
|
||||
cum+=w*w;
|
||||
sum+=w;
|
||||
}
|
||||
return sum*sum/cum;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
|
||||
The following are patterns for the evolution and the observation classes
|
||||
The user should implement classes having the specified meaning
|
||||
|
||||
template <class State, class Numeric, class Observation>
|
||||
struct observer{
|
||||
Observation& observation
|
||||
Numeric observe(const class State&) const;
|
||||
};
|
||||
|
||||
template <class State, class Numeric, class Input>
|
||||
struct evolver{
|
||||
Input& input;
|
||||
State& evolve(const State& s);
|
||||
};
|
||||
*/
|
||||
|
||||
|
||||
template <class Particle, class EvolutionModel>
|
||||
struct evolver{
|
||||
EvolutionModel evolutionModel;
|
||||
void evolve(std::vector<Particle>& particles);
|
||||
void evolve(std::vector<Particle>& dest, const std::vector<Particle>& src);
|
||||
};
|
||||
|
||||
template <class Particle, class EvolutionModel>
|
||||
void evolver<Particle, EvolutionModel>::evolve(std::vector<Particle>& particles){
|
||||
for (typename std::vector<Particle>::iterator it=particles.begin(); it!=particles.end(); ++it){
|
||||
*it=evolutionModel.evolve(*it);
|
||||
}
|
||||
}
|
||||
|
||||
template <class Particle, class EvolutionModel>
|
||||
void evolver<Particle, EvolutionModel>::evolve(std::vector<Particle>& dest, const std::vector<Particle>& src){
|
||||
dest.clear();
|
||||
for (typename std::vector<Particle>::const_iterator it=src.begin(); it!=src.end(); ++it)
|
||||
dest.push_back(evolutionModel.evolve(*it));
|
||||
}
|
||||
|
||||
|
||||
template <class Particle, class Numeric, class QualificationModel, class EvolutionModel, class LikelyhoodModel>
|
||||
struct auxiliary_evolver{
|
||||
EvolutionModel evolutionModel;
|
||||
QualificationModel qualificationModel;
|
||||
LikelyhoodModel likelyhoodModel;
|
||||
void evolve(std::vector<Particle>& particles);
|
||||
void evolve(std::vector<Particle>& dest, const std::vector<Particle>& src);
|
||||
};
|
||||
|
||||
template <class Particle, class Numeric, class QualificationModel, class EvolutionModel, class LikelyhoodModel>
|
||||
void auxiliary_evolver<Particle, Numeric, QualificationModel, EvolutionModel, LikelyhoodModel>::evolve
|
||||
(std::vector<Particle>&particles){
|
||||
std::vector<Numeric> observationWeights(particles.size());
|
||||
unsigned int i=0;
|
||||
for (typename std::vector<Particle>::const_iterator it=particles.begin(); it!=particles.end(); ++it, i++){
|
||||
observationWeights[i]=likelyhoodModel.likelyhood(qualificationModel.evolve(*it));
|
||||
}
|
||||
uniform_resampler<Numeric, Numeric> resampler;
|
||||
std::vector<unsigned int> indexes(resampler.resampleIndexes(observationWeights));
|
||||
for (typename std::vector<unsigned int>::const_iterator it=indexes.begin(); it!=indexes.end(); ++it){
|
||||
Particle & particle=particles[*it];
|
||||
particle=evolutionModel.evolve(particle);
|
||||
particle.setWeight(likelyhoodModel.likelyhood(particle)/observationWeights[*it]);
|
||||
}
|
||||
}
|
||||
|
||||
template <class Particle, class Numeric, class QualificationModel, class EvolutionModel, class LikelyhoodModel>
|
||||
void auxiliary_evolver<Particle, Numeric, QualificationModel, EvolutionModel, LikelyhoodModel>::evolve
|
||||
(std::vector<Particle>& dest, const std::vector<Particle>& src){
|
||||
dest.clear();
|
||||
std::vector<Numeric> observationWeights(src.size());
|
||||
unsigned int i=0;
|
||||
for (typename std::vector<Particle>::const_iterator it=src.begin(); it!=src.end(); ++it, i++){
|
||||
observationWeights[i]=likelyhoodModel.likelyhood(qualificationModel.evolve(*it));
|
||||
}
|
||||
uniform_resampler<Numeric, Numeric> resampler;
|
||||
std::vector<unsigned int> indexes(resampler.resampleIndexes(observationWeights));
|
||||
for (typename std::vector<unsigned int>::const_iterator it=indexes.begin(); it!=indexes.end(); ++it){
|
||||
Particle & particle=src[*it];
|
||||
dest.push_back(evolutionModel.evolve(particle));
|
||||
dest.back().weight*=likelyhoodModel.likelyhood(particle)/observationWeights[*it];
|
||||
}
|
||||
}
|
||||
//END legacy
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,85 @@
|
||||
#ifndef _ICP_H_
|
||||
#define _ICP_H_
|
||||
|
||||
#include <gmapping/utils/point.h>
|
||||
#include <utility>
|
||||
#include <list>
|
||||
#include <vector>
|
||||
|
||||
namespace GMapping{
|
||||
typedef std::pair<Point,Point> PointPair;
|
||||
|
||||
template <typename PointPairContainer>
|
||||
double icpStep(OrientedPoint & retval, const PointPairContainer& container){
|
||||
typedef typename PointPairContainer::const_iterator ContainerIterator;
|
||||
PointPair mean=std::make_pair(Point(0.,0.), Point(0.,0.));
|
||||
int size=0;
|
||||
for (ContainerIterator it=container.begin(); it!=container.end(); it++){
|
||||
mean.first=mean.first+it->first;
|
||||
mean.second=mean.second+it->second;
|
||||
size++;
|
||||
}
|
||||
mean.first=mean.first*(1./size);
|
||||
mean.second=mean.second*(1./size);
|
||||
double sxx=0, sxy=0, syx=0, syy=0;
|
||||
|
||||
for (ContainerIterator it=container.begin(); it!=container.end(); it++){
|
||||
PointPair mf=std::make_pair(it->first-mean.first, it->second-mean.second);
|
||||
sxx+=mf.first.x*mf.second.x;
|
||||
sxy+=mf.first.x*mf.second.y;
|
||||
syx+=mf.first.y*mf.second.x;
|
||||
syy+=mf.first.y*mf.second.y;
|
||||
}
|
||||
retval.theta=atan2(sxy-syx, sxx+sxy);
|
||||
double s=sin(retval.theta), c=cos(retval.theta);
|
||||
retval.x=mean.second.x-(c*mean.first.x-s*mean.first.y);
|
||||
retval.y=mean.second.y-(s*mean.first.x+c*mean.first.y);
|
||||
|
||||
double error=0;
|
||||
for (ContainerIterator it=container.begin(); it!=container.end(); it++){
|
||||
Point delta(
|
||||
c*it->first.x-s*it->first.y+retval.x-it->second.x, s*it->first.x+c*it->first.y+retval.y-it->second.y);
|
||||
error+=delta*delta;
|
||||
}
|
||||
return error;
|
||||
}
|
||||
|
||||
template <typename PointPairContainer>
|
||||
double icpNonlinearStep(OrientedPoint & retval, const PointPairContainer& container){
|
||||
typedef typename PointPairContainer::const_iterator ContainerIterator;
|
||||
PointPair mean=std::make_pair(Point(0.,0.), Point(0.,0.));
|
||||
int size=0;
|
||||
for (ContainerIterator it=container.begin(); it!=container.end(); it++){
|
||||
mean.first=mean.first+it->first;
|
||||
mean.second=mean.second+it->second;
|
||||
size++;
|
||||
}
|
||||
|
||||
mean.first=mean.first*(1./size);
|
||||
mean.second=mean.second*(1./size);
|
||||
|
||||
double ms=0,mc=0;
|
||||
for (ContainerIterator it=container.begin(); it!=container.end(); it++){
|
||||
PointPair mf=std::make_pair(it->first-mean.first, it->second-mean.second);
|
||||
double dalpha=atan2(mf.second.y, mf.second.x) - atan2(mf.first.y, mf.first.x);
|
||||
double gain=sqrt(mean.first*mean.first);
|
||||
ms+=gain*sin(dalpha);
|
||||
mc+=gain*cos(dalpha);
|
||||
}
|
||||
retval.theta=atan2(ms, mc);
|
||||
double s=sin(retval.theta), c=cos(retval.theta);
|
||||
retval.x=mean.second.x-(c*mean.first.x-s*mean.first.y);
|
||||
retval.y=mean.second.y-(s*mean.first.x+c*mean.first.y);
|
||||
|
||||
double error=0;
|
||||
for (ContainerIterator it=container.begin(); it!=container.end(); it++){
|
||||
Point delta(
|
||||
c*it->first.x-s*it->first.y+retval.x-it->second.x, s*it->first.x+c*it->first.y+retval.y-it->second.y);
|
||||
error+=delta*delta;
|
||||
}
|
||||
return error;
|
||||
}
|
||||
|
||||
}//end namespace
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,252 @@
|
||||
#ifndef SCANMATCHER_H
|
||||
#define SCANMATCHER_H
|
||||
|
||||
#include "icp.h"
|
||||
#include "smmap.h"
|
||||
#include <gmapping/utils/macro_params.h>
|
||||
#include <gmapping/utils/stat.h>
|
||||
#include <iostream>
|
||||
#include <gmapping/utils/gvalues.h>
|
||||
#define LASER_MAXBEAMS 2048
|
||||
|
||||
namespace GMapping {
|
||||
|
||||
class ScanMatcher{
|
||||
public:
|
||||
typedef Covariance3 CovarianceMatrix;
|
||||
|
||||
ScanMatcher();
|
||||
~ScanMatcher();
|
||||
double icpOptimize(OrientedPoint& pnew, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const;
|
||||
double optimize(OrientedPoint& pnew, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const;
|
||||
double optimize(OrientedPoint& mean, CovarianceMatrix& cov, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const;
|
||||
|
||||
double registerScan(ScanMatcherMap& map, const OrientedPoint& p, const double* readings);
|
||||
void setLaserParameters
|
||||
(unsigned int beams, double* angles, const OrientedPoint& lpose);
|
||||
void setMatchingParameters
|
||||
(double urange, double range, double sigma, int kernsize, double lopt, double aopt, int iterations, double likelihoodSigma=1, unsigned int likelihoodSkip=0 );
|
||||
void invalidateActiveArea();
|
||||
void computeActiveArea(ScanMatcherMap& map, const OrientedPoint& p, const double* readings);
|
||||
|
||||
inline double icpStep(OrientedPoint & pret, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const;
|
||||
inline double score(const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const;
|
||||
inline unsigned int likelihoodAndScore(double& s, double& l, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const;
|
||||
double likelihood(double& lmax, OrientedPoint& mean, CovarianceMatrix& cov, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings);
|
||||
double likelihood(double& _lmax, OrientedPoint& _mean, CovarianceMatrix& _cov, const ScanMatcherMap& map, const OrientedPoint& p, Gaussian3& odometry, const double* readings, double gain=180.);
|
||||
inline const double* laserAngles() const { return m_laserAngles; }
|
||||
inline unsigned int laserBeams() const { return m_laserBeams; }
|
||||
|
||||
static const double nullLikelihood;
|
||||
protected:
|
||||
//state of the matcher
|
||||
bool m_activeAreaComputed;
|
||||
|
||||
/**laser parameters*/
|
||||
unsigned int m_laserBeams;
|
||||
double m_laserAngles[LASER_MAXBEAMS];
|
||||
//OrientedPoint m_laserPose;
|
||||
PARAM_SET_GET(OrientedPoint, laserPose, protected, public, public)
|
||||
PARAM_SET_GET(double, laserMaxRange, protected, public, public)
|
||||
/**scan_matcher parameters*/
|
||||
PARAM_SET_GET(double, usableRange, protected, public, public)
|
||||
PARAM_SET_GET(double, gaussianSigma, protected, public, public)
|
||||
PARAM_SET_GET(double, likelihoodSigma, protected, public, public)
|
||||
PARAM_SET_GET(int, kernelSize, protected, public, public)
|
||||
PARAM_SET_GET(double, optAngularDelta, protected, public, public)
|
||||
PARAM_SET_GET(double, optLinearDelta, protected, public, public)
|
||||
PARAM_SET_GET(unsigned int, optRecursiveIterations, protected, public, public)
|
||||
PARAM_SET_GET(unsigned int, likelihoodSkip, protected, public, public)
|
||||
PARAM_SET_GET(double, llsamplerange, protected, public, public)
|
||||
PARAM_SET_GET(double, llsamplestep, protected, public, public)
|
||||
PARAM_SET_GET(double, lasamplerange, protected, public, public)
|
||||
PARAM_SET_GET(double, lasamplestep, protected, public, public)
|
||||
PARAM_SET_GET(bool, generateMap, protected, public, public)
|
||||
PARAM_SET_GET(double, enlargeStep, protected, public, public)
|
||||
PARAM_SET_GET(double, fullnessThreshold, protected, public, public)
|
||||
PARAM_SET_GET(double, angularOdometryReliability, protected, public, public)
|
||||
PARAM_SET_GET(double, linearOdometryReliability, protected, public, public)
|
||||
PARAM_SET_GET(double, freeCellRatio, protected, public, public)
|
||||
PARAM_SET_GET(unsigned int, initialBeamsSkip, protected, public, public)
|
||||
|
||||
// allocate this large array only once
|
||||
IntPoint* m_linePoints;
|
||||
};
|
||||
|
||||
inline double ScanMatcher::icpStep(OrientedPoint & pret, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const{
|
||||
const double * angle=m_laserAngles+m_initialBeamsSkip;
|
||||
OrientedPoint lp=p;
|
||||
lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;
|
||||
lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y;
|
||||
lp.theta+=m_laserPose.theta;
|
||||
unsigned int skip=0;
|
||||
double freeDelta=map.getDelta()*m_freeCellRatio;
|
||||
std::list<PointPair> pairs;
|
||||
|
||||
for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++){
|
||||
skip++;
|
||||
skip=skip>m_likelihoodSkip?0:skip;
|
||||
if (*r>m_usableRange||*r==0.0) continue;
|
||||
if (skip) continue;
|
||||
Point phit=lp;
|
||||
phit.x+=*r*cos(lp.theta+*angle);
|
||||
phit.y+=*r*sin(lp.theta+*angle);
|
||||
IntPoint iphit=map.world2map(phit);
|
||||
Point pfree=lp;
|
||||
pfree.x+=(*r-map.getDelta()*freeDelta)*cos(lp.theta+*angle);
|
||||
pfree.y+=(*r-map.getDelta()*freeDelta)*sin(lp.theta+*angle);
|
||||
pfree=pfree-phit;
|
||||
IntPoint ipfree=map.world2map(pfree);
|
||||
bool found=false;
|
||||
Point bestMu(0.,0.);
|
||||
Point bestCell(0.,0.);
|
||||
for (int xx=-m_kernelSize; xx<=m_kernelSize; xx++)
|
||||
for (int yy=-m_kernelSize; yy<=m_kernelSize; yy++){
|
||||
IntPoint pr=iphit+IntPoint(xx,yy);
|
||||
IntPoint pf=pr+ipfree;
|
||||
//AccessibilityState s=map.storage().cellState(pr);
|
||||
//if (s&Inside && s&Allocated){
|
||||
const PointAccumulator& cell=map.cell(pr);
|
||||
const PointAccumulator& fcell=map.cell(pf);
|
||||
if (((double)cell )> m_fullnessThreshold && ((double)fcell )<m_fullnessThreshold){
|
||||
Point mu=phit-cell.mean();
|
||||
if (!found){
|
||||
bestMu=mu;
|
||||
bestCell=cell.mean();
|
||||
found=true;
|
||||
}else
|
||||
if((mu*mu)<(bestMu*bestMu)){
|
||||
bestMu=mu;
|
||||
bestCell=cell.mean();
|
||||
}
|
||||
|
||||
}
|
||||
//}
|
||||
}
|
||||
if (found){
|
||||
pairs.push_back(std::make_pair(phit, bestCell));
|
||||
//std::cerr << "(" << phit.x-bestCell.x << "," << phit.y-bestCell.y << ") ";
|
||||
}
|
||||
//std::cerr << std::endl;
|
||||
}
|
||||
|
||||
OrientedPoint result(0,0,0);
|
||||
//double icpError=icpNonlinearStep(result,pairs);
|
||||
std::cerr << "result(" << pairs.size() << ")=" << result.x << " " << result.y << " " << result.theta << std::endl;
|
||||
pret.x=p.x+result.x;
|
||||
pret.y=p.y+result.y;
|
||||
pret.theta=p.theta+result.theta;
|
||||
pret.theta=atan2(sin(pret.theta), cos(pret.theta));
|
||||
return score(map, p, readings);
|
||||
}
|
||||
|
||||
inline double ScanMatcher::score(const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const{
|
||||
double s=0;
|
||||
const double * angle=m_laserAngles+m_initialBeamsSkip;
|
||||
OrientedPoint lp=p;
|
||||
lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;
|
||||
lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y;
|
||||
lp.theta+=m_laserPose.theta;
|
||||
unsigned int skip=0;
|
||||
double freeDelta=map.getDelta()*m_freeCellRatio;
|
||||
for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++){
|
||||
skip++;
|
||||
skip=skip>m_likelihoodSkip?0:skip;
|
||||
if (skip||*r>m_usableRange||*r==0.0) continue;
|
||||
Point phit=lp;
|
||||
phit.x+=*r*cos(lp.theta+*angle);
|
||||
phit.y+=*r*sin(lp.theta+*angle);
|
||||
IntPoint iphit=map.world2map(phit);
|
||||
Point pfree=lp;
|
||||
pfree.x+=(*r-map.getDelta()*freeDelta)*cos(lp.theta+*angle);
|
||||
pfree.y+=(*r-map.getDelta()*freeDelta)*sin(lp.theta+*angle);
|
||||
pfree=pfree-phit;
|
||||
IntPoint ipfree=map.world2map(pfree);
|
||||
bool found=false;
|
||||
Point bestMu(0.,0.);
|
||||
for (int xx=-m_kernelSize; xx<=m_kernelSize; xx++)
|
||||
for (int yy=-m_kernelSize; yy<=m_kernelSize; yy++){
|
||||
IntPoint pr=iphit+IntPoint(xx,yy);
|
||||
IntPoint pf=pr+ipfree;
|
||||
//AccessibilityState s=map.storage().cellState(pr);
|
||||
//if (s&Inside && s&Allocated){
|
||||
const PointAccumulator& cell=map.cell(pr);
|
||||
const PointAccumulator& fcell=map.cell(pf);
|
||||
if (((double)cell )> m_fullnessThreshold && ((double)fcell )<m_fullnessThreshold){
|
||||
Point mu=phit-cell.mean();
|
||||
if (!found){
|
||||
bestMu=mu;
|
||||
found=true;
|
||||
}else
|
||||
bestMu=(mu*mu)<(bestMu*bestMu)?mu:bestMu;
|
||||
}
|
||||
//}
|
||||
}
|
||||
if (found)
|
||||
s+=exp(-1./m_gaussianSigma*bestMu*bestMu);
|
||||
}
|
||||
return s;
|
||||
}
|
||||
|
||||
inline unsigned int ScanMatcher::likelihoodAndScore(double& s, double& l, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const{
|
||||
using namespace std;
|
||||
l=0;
|
||||
s=0;
|
||||
const double * angle=m_laserAngles+m_initialBeamsSkip;
|
||||
OrientedPoint lp=p;
|
||||
lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;
|
||||
lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y;
|
||||
lp.theta+=m_laserPose.theta;
|
||||
double noHit=nullLikelihood/(m_likelihoodSigma);
|
||||
unsigned int skip=0;
|
||||
unsigned int c=0;
|
||||
double freeDelta=map.getDelta()*m_freeCellRatio;
|
||||
for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++){
|
||||
skip++;
|
||||
skip=skip>m_likelihoodSkip?0:skip;
|
||||
if (*r>m_usableRange) continue;
|
||||
if (skip) continue;
|
||||
Point phit=lp;
|
||||
phit.x+=*r*cos(lp.theta+*angle);
|
||||
phit.y+=*r*sin(lp.theta+*angle);
|
||||
IntPoint iphit=map.world2map(phit);
|
||||
Point pfree=lp;
|
||||
pfree.x+=(*r-freeDelta)*cos(lp.theta+*angle);
|
||||
pfree.y+=(*r-freeDelta)*sin(lp.theta+*angle);
|
||||
pfree=pfree-phit;
|
||||
IntPoint ipfree=map.world2map(pfree);
|
||||
bool found=false;
|
||||
Point bestMu(0.,0.);
|
||||
for (int xx=-m_kernelSize; xx<=m_kernelSize; xx++)
|
||||
for (int yy=-m_kernelSize; yy<=m_kernelSize; yy++){
|
||||
IntPoint pr=iphit+IntPoint(xx,yy);
|
||||
IntPoint pf=pr+ipfree;
|
||||
//AccessibilityState s=map.storage().cellState(pr);
|
||||
//if (s&Inside && s&Allocated){
|
||||
const PointAccumulator& cell=map.cell(pr);
|
||||
const PointAccumulator& fcell=map.cell(pf);
|
||||
if (((double)cell )>m_fullnessThreshold && ((double)fcell )<m_fullnessThreshold){
|
||||
Point mu=phit-cell.mean();
|
||||
if (!found){
|
||||
bestMu=mu;
|
||||
found=true;
|
||||
}else
|
||||
bestMu=(mu*mu)<(bestMu*bestMu)?mu:bestMu;
|
||||
}
|
||||
//}
|
||||
}
|
||||
if (found){
|
||||
s+=exp(-1./m_gaussianSigma*bestMu*bestMu);
|
||||
c++;
|
||||
}
|
||||
if (!skip){
|
||||
double f=(-1./m_likelihoodSigma)*(bestMu*bestMu);
|
||||
l+=(found)?f:noHit;
|
||||
}
|
||||
}
|
||||
return c;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,54 @@
|
||||
#ifndef SMMAP_H
|
||||
#define SMMAP_H
|
||||
#include <gmapping/grid/map.h>
|
||||
#include <gmapping/grid/harray2d.h>
|
||||
#include <gmapping/utils/point.h>
|
||||
#define SIGHT_INC 1
|
||||
|
||||
namespace GMapping {
|
||||
|
||||
struct PointAccumulator{
|
||||
typedef point<float> FloatPoint;
|
||||
/* before
|
||||
PointAccumulator(int i=-1): acc(0,0), n(0), visits(0){assert(i==-1);}
|
||||
*/
|
||||
/*after begin*/
|
||||
PointAccumulator(): acc(0,0), n(0), visits(0){}
|
||||
PointAccumulator(int i): acc(0,0), n(0), visits(0){assert(i==-1);}
|
||||
/*after end*/
|
||||
inline void update(bool value, const Point& p=Point(0,0));
|
||||
inline Point mean() const {return 1./n*Point(acc.x, acc.y);}
|
||||
inline operator double() const { return visits?(double)n*SIGHT_INC/(double)visits:-1; }
|
||||
inline void add(const PointAccumulator& p) {acc=acc+p.acc; n+=p.n; visits+=p.visits; }
|
||||
static const PointAccumulator& Unknown();
|
||||
static PointAccumulator* unknown_ptr;
|
||||
FloatPoint acc;
|
||||
int n, visits;
|
||||
inline double entropy() const;
|
||||
};
|
||||
|
||||
void PointAccumulator::update(bool value, const Point& p){
|
||||
if (value) {
|
||||
acc.x+= static_cast<float>(p.x);
|
||||
acc.y+= static_cast<float>(p.y);
|
||||
n++;
|
||||
visits+=SIGHT_INC;
|
||||
} else
|
||||
visits++;
|
||||
}
|
||||
|
||||
double PointAccumulator::entropy() const{
|
||||
if (!visits)
|
||||
return -log(.5);
|
||||
if (n==visits || n==0)
|
||||
return 0;
|
||||
double x=(double)n*SIGHT_INC/(double)visits;
|
||||
return -( x*log(x)+ (1-x)*log(1-x) );
|
||||
}
|
||||
|
||||
|
||||
typedef Map<PointAccumulator,HierarchicalArray2D<PointAccumulator> > ScanMatcherMap;
|
||||
|
||||
};
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,24 @@
|
||||
#ifndef SENSOR_H
|
||||
#define SENSOR_H
|
||||
|
||||
#include <string>
|
||||
#include <map>
|
||||
|
||||
namespace GMapping{
|
||||
|
||||
class Sensor{
|
||||
public:
|
||||
Sensor(const std::string& name="");
|
||||
virtual ~Sensor();
|
||||
inline std::string getName() const {return m_name;}
|
||||
inline void setName(const std::string& name) {m_name=name;}
|
||||
protected:
|
||||
std::string m_name;
|
||||
};
|
||||
|
||||
typedef std::map<std::string, Sensor*> SensorMap;
|
||||
|
||||
}; //end namespace
|
||||
|
||||
#endif
|
||||
|
||||
@@ -0,0 +1,26 @@
|
||||
#ifndef SENSORREADING_H
|
||||
#define SENSORREADING_H
|
||||
|
||||
#include "sensor.h"
|
||||
namespace GMapping{
|
||||
|
||||
class SensorReading{
|
||||
public:
|
||||
SensorReading(const Sensor* s, double time){
|
||||
m_sensor=s;
|
||||
m_time=time;
|
||||
};
|
||||
~SensorReading(){};
|
||||
inline double getTime() const {return m_time;}
|
||||
inline void setTime(double t) {m_time=t;}
|
||||
inline const Sensor* getSensor() const {return m_sensor;}
|
||||
protected:
|
||||
double m_time;
|
||||
const Sensor* m_sensor;
|
||||
|
||||
};
|
||||
|
||||
}; //end namespace
|
||||
#endif
|
||||
|
||||
|
||||
+29
@@ -0,0 +1,29 @@
|
||||
#ifndef ODOMETRYREADING_H
|
||||
#define ODOMETRYREADING_H
|
||||
|
||||
#include <string.h>
|
||||
#include <gmapping/sensor/sensor_base/sensorreading.h>
|
||||
#include <gmapping/utils/point.h>
|
||||
#include "odometrysensor.h"
|
||||
|
||||
namespace GMapping{
|
||||
|
||||
class OdometryReading: public SensorReading{
|
||||
public:
|
||||
OdometryReading(const OdometrySensor* odo, double time=0);
|
||||
inline const OrientedPoint& getPose() const {return m_pose;}
|
||||
inline const OrientedPoint& getSpeed() const {return m_speed;}
|
||||
inline const OrientedPoint& getAcceleration() const {return m_acceleration;}
|
||||
inline void setPose(const OrientedPoint& pose) {m_pose=pose;}
|
||||
inline void setSpeed(const OrientedPoint& speed) {m_speed=speed;}
|
||||
inline void setAcceleration(const OrientedPoint& acceleration) {m_acceleration=acceleration;}
|
||||
|
||||
protected:
|
||||
OrientedPoint m_pose;
|
||||
OrientedPoint m_speed;
|
||||
OrientedPoint m_acceleration;
|
||||
};
|
||||
|
||||
};
|
||||
#endif
|
||||
|
||||
+20
@@ -0,0 +1,20 @@
|
||||
#ifndef ODOMETRYSENSOR_H
|
||||
#define ODOMETRYSENSOR_H
|
||||
|
||||
#include <string>
|
||||
#include <gmapping/sensor/sensor_base/sensor.h>
|
||||
|
||||
namespace GMapping{
|
||||
|
||||
class OdometrySensor: public Sensor{
|
||||
public:
|
||||
OdometrySensor(const std::string& name, bool ideal=false);
|
||||
inline bool isIdeal() const { return m_ideal; }
|
||||
protected:
|
||||
bool m_ideal;
|
||||
};
|
||||
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
@@ -0,0 +1,26 @@
|
||||
#ifndef RANGEREADING_H
|
||||
#define RANGEREADING_H
|
||||
|
||||
#include <vector>
|
||||
#include <gmapping/sensor/sensor_base/sensorreading.h>
|
||||
#include <gmapping/sensor/sensor_range/rangesensor.h>
|
||||
|
||||
namespace GMapping{
|
||||
|
||||
class RangeReading: public SensorReading, public std::vector<double>{
|
||||
public:
|
||||
RangeReading(const RangeSensor* rs, double time=0);
|
||||
RangeReading(unsigned int n_beams, const double* d, const RangeSensor* rs, double time=0);
|
||||
virtual ~RangeReading();
|
||||
inline const OrientedPoint& getPose() const {return m_pose;}
|
||||
inline void setPose(const OrientedPoint& pose) {m_pose=pose;}
|
||||
unsigned int rawView(double* v, double density=0.) const;
|
||||
std::vector<Point> cartesianForm(double maxRange=1e6) const;
|
||||
unsigned int activeBeams(double density=0.) const;
|
||||
protected:
|
||||
OrientedPoint m_pose;
|
||||
};
|
||||
|
||||
};
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,35 @@
|
||||
#ifndef RANGESENSOR_H
|
||||
#define RANGESENSOR_H
|
||||
|
||||
#include <vector>
|
||||
#include <gmapping/sensor/sensor_base/sensor.h>
|
||||
#include <gmapping/utils/point.h>
|
||||
|
||||
namespace GMapping{
|
||||
|
||||
class RangeSensor: public Sensor{
|
||||
friend class Configuration;
|
||||
friend class CarmenConfiguration;
|
||||
friend class CarmenWrapper;
|
||||
public:
|
||||
struct Beam{
|
||||
OrientedPoint pose; //pose relative to the center of the sensor
|
||||
double span; //spam=0 indicates a line-like beam
|
||||
double maxRange; //maximum range of the sensor
|
||||
double s,c; //sinus and cosinus of the beam (optimization);
|
||||
};
|
||||
RangeSensor(std::string name);
|
||||
RangeSensor(std::string name, unsigned int beams, double res, const OrientedPoint& position=OrientedPoint(0,0,0), double span=0, double maxrange=89.0);
|
||||
inline const std::vector<Beam>& beams() const {return m_beams;}
|
||||
inline std::vector<Beam>& beams() {return m_beams;}
|
||||
inline OrientedPoint getPose() const {return m_pose;}
|
||||
void updateBeamsLookup();
|
||||
bool newFormat;
|
||||
protected:
|
||||
OrientedPoint m_pose;
|
||||
std::vector<Beam> m_beams;
|
||||
};
|
||||
|
||||
};
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,97 @@
|
||||
#ifndef AUTOPTR_H
|
||||
#define AUTOPTR_H
|
||||
#include <assert.h>
|
||||
|
||||
namespace GMapping{
|
||||
|
||||
template <class X>
|
||||
class autoptr{
|
||||
protected:
|
||||
|
||||
public:
|
||||
struct reference{
|
||||
X* data;
|
||||
unsigned int shares;
|
||||
};
|
||||
inline autoptr(X* p=(X*)(0));
|
||||
inline autoptr(const autoptr<X>& ap);
|
||||
inline autoptr& operator=(const autoptr<X>& ap);
|
||||
inline ~autoptr();
|
||||
inline operator int() const;
|
||||
inline X& operator*();
|
||||
inline const X& operator*() const;
|
||||
//p
|
||||
reference * m_reference;
|
||||
protected:
|
||||
};
|
||||
|
||||
template <class X>
|
||||
autoptr<X>::autoptr(X* p){
|
||||
m_reference=0;
|
||||
if (p){
|
||||
m_reference=new reference;
|
||||
m_reference->data=p;
|
||||
m_reference->shares=1;
|
||||
}
|
||||
}
|
||||
|
||||
template <class X>
|
||||
autoptr<X>::autoptr(const autoptr<X>& ap){
|
||||
m_reference=0;
|
||||
reference* ref=ap.m_reference;
|
||||
if (ap.m_reference){
|
||||
m_reference=ref;
|
||||
m_reference->shares++;
|
||||
}
|
||||
}
|
||||
|
||||
template <class X>
|
||||
autoptr<X>& autoptr<X>::operator=(const autoptr<X>& ap){
|
||||
reference* ref=ap.m_reference;
|
||||
if (m_reference==ref){
|
||||
return *this;
|
||||
}
|
||||
if (m_reference && !(--m_reference->shares)){
|
||||
delete m_reference->data;
|
||||
delete m_reference;
|
||||
m_reference=0;
|
||||
}
|
||||
if (ref){
|
||||
m_reference=ref;
|
||||
m_reference->shares++;
|
||||
}
|
||||
//20050802 nasty changes begin
|
||||
else
|
||||
m_reference=0;
|
||||
//20050802 nasty changes end
|
||||
return *this;
|
||||
}
|
||||
|
||||
template <class X>
|
||||
autoptr<X>::~autoptr(){
|
||||
if (m_reference && !(--m_reference->shares)){
|
||||
delete m_reference->data;
|
||||
delete m_reference;
|
||||
m_reference=0;
|
||||
}
|
||||
}
|
||||
|
||||
template <class X>
|
||||
autoptr<X>::operator int() const{
|
||||
return m_reference && m_reference->shares && m_reference->data;
|
||||
}
|
||||
|
||||
template <class X>
|
||||
X& autoptr<X>::operator*(){
|
||||
assert(m_reference && m_reference->shares && m_reference->data);
|
||||
return *(m_reference->data);
|
||||
}
|
||||
|
||||
template <class X>
|
||||
const X& autoptr<X>::operator*() const{
|
||||
assert(m_reference && m_reference->shares && m_reference->data);
|
||||
return *(m_reference->data);
|
||||
}
|
||||
|
||||
};
|
||||
#endif
|
||||
@@ -0,0 +1,115 @@
|
||||
/*****************************************************************
|
||||
*
|
||||
* This file is part of the GMAPPING project
|
||||
*
|
||||
* GMAPPING Copyright (c) 2004 Giorgio Grisetti,
|
||||
* Cyrill Stachniss, and Wolfram Burgard
|
||||
*
|
||||
* This software is licensed under the "Creative Commons
|
||||
* License (Attribution-NonCommercial-ShareAlike 2.0)"
|
||||
* and is copyrighted by Giorgio Grisetti, Cyrill Stachniss,
|
||||
* and Wolfram Burgard.
|
||||
*
|
||||
* Further information on this license can be found at:
|
||||
* http://creativecommons.org/licenses/by-nc-sa/2.0/
|
||||
*
|
||||
* GMAPPING is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied
|
||||
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
|
||||
* PURPOSE.
|
||||
*
|
||||
*****************************************************************/
|
||||
|
||||
|
||||
#ifndef COMMANDLINE_H
|
||||
#define COMMANDLINE_H
|
||||
|
||||
|
||||
#define parseFlag(name,value)\
|
||||
if (!strcmp(argv[c],name)){\
|
||||
value=true;\
|
||||
cout << name << " on"<< endl;\
|
||||
recognized=true;\
|
||||
}\
|
||||
|
||||
#define parseString(name,value)\
|
||||
if (!strcmp(argv[c],name) && c<argc-1){\
|
||||
c++;\
|
||||
value=argv[c];\
|
||||
cout << name << "=" << value << endl;\
|
||||
recognized=true;\
|
||||
}\
|
||||
|
||||
|
||||
#define parseDouble(name,value)\
|
||||
if (!strcmp(argv[c],name) && c<argc-1){\
|
||||
c++;\
|
||||
value=atof(argv[c]);\
|
||||
cout << name << "=" << value << endl;\
|
||||
recognized=true;\
|
||||
}\
|
||||
|
||||
#define parseInt(name,value)\
|
||||
if (!strcmp(argv[c],name) && c<argc-1){\
|
||||
c++;\
|
||||
value=atoi(argv[c]);\
|
||||
cout << name << "=" << value << endl;\
|
||||
recognized=true;\
|
||||
}\
|
||||
|
||||
#define CMD_PARSE_BEGIN(i, count)\
|
||||
{\
|
||||
int c=i;\
|
||||
while (c<count){\
|
||||
bool recognized=false;
|
||||
|
||||
#define CMD_PARSE_END\
|
||||
if (!recognized)\
|
||||
cout << "COMMAND LINE: parameter " << argv[c] << " not recognized" << endl;\
|
||||
c++;\
|
||||
}\
|
||||
}
|
||||
|
||||
#define CMD_PARSE_BEGIN_SILENT(i, count)\
|
||||
{\
|
||||
int c=i;\
|
||||
while (c<count){\
|
||||
bool recognized=false;
|
||||
|
||||
#define CMD_PARSE_END_SILENT\
|
||||
c++;\
|
||||
}\
|
||||
}
|
||||
|
||||
|
||||
#define parseFlagSilent(name,value)\
|
||||
if (!strcmp(argv[c],name)){\
|
||||
value=true;\
|
||||
recognized=true;\
|
||||
}\
|
||||
|
||||
#define parseStringSilent(name,value)\
|
||||
if (!strcmp(argv[c],name) && c<argc-1){\
|
||||
c++;\
|
||||
value=argv[c];\
|
||||
recognized=true;\
|
||||
}\
|
||||
|
||||
|
||||
#define parseDoubleSilent(name,value)\
|
||||
if (!strcmp(argv[c],name) && c<argc-1){\
|
||||
c++;\
|
||||
value=atof(argv[c]);\
|
||||
recognized=true;\
|
||||
}\
|
||||
|
||||
#define parseIntSilent(name,value)\
|
||||
if (!strcmp(argv[c],name) && c<argc-1){\
|
||||
c++;\
|
||||
value=atoi(argv[c]);\
|
||||
recognized=true;\
|
||||
}\
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
@@ -0,0 +1,28 @@
|
||||
#ifndef _GVALUES_H_
|
||||
#define _GVALUES_H_
|
||||
|
||||
#define MAXDOUBLE 1e1000
|
||||
#ifdef LINUX
|
||||
#include <values.h>
|
||||
#endif
|
||||
#ifdef MACOSX
|
||||
#include <limits.h>
|
||||
#include <math.h>
|
||||
//#define isnan(x) (x==FP_NAN)
|
||||
#endif
|
||||
#ifdef _WIN32
|
||||
#include <limits>
|
||||
#ifndef __DRAND48_DEFINED__
|
||||
#define __DRAND48_DEFINED__
|
||||
inline double drand48() { return double(rand()) / RAND_MAX;}
|
||||
#endif
|
||||
#ifndef M_PI
|
||||
#define M_PI 3.1415926535897932384626433832795
|
||||
#endif
|
||||
#define round(d) (floor((d) + 0.5))
|
||||
typedef unsigned int uint;
|
||||
#define isnan(x) (_isnan(x))
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
||||
@@ -0,0 +1,38 @@
|
||||
#ifndef MACRO_PARAMS_H
|
||||
#define MACRO_PARAMS_H
|
||||
|
||||
#define PARAM_SET_GET(type, name, qualifier, setqualifier, getqualifier)\
|
||||
qualifier: type m_##name;\
|
||||
getqualifier: inline type get##name() const {return m_##name;}\
|
||||
setqualifier: inline void set##name(type name) {m_##name=name;}
|
||||
|
||||
#define PARAM_SET(type, name, qualifier, setqualifier)\
|
||||
qualifier: type m_##name;\
|
||||
setqualifier: inline void set##name(type name) {m_##name=name;}
|
||||
|
||||
#define PARAM_GET(type, name, qualifier, getqualifier)\
|
||||
qualifier: type m_##name;\
|
||||
getqualifier: inline type get##name() const {return m_##name;}
|
||||
|
||||
#define MEMBER_PARAM_SET_GET(member, type, name, qualifier, setqualifier, getqualifier)\
|
||||
getqualifier: inline type get##name() const {return member.get##name();}\
|
||||
setqualifier: inline void set##name(type name) { member.set##name(name);}
|
||||
|
||||
#define MEMBER_PARAM_SET(member, type, name, qualifier, setqualifier, getqualifier)\
|
||||
setqualifier: inline void set##name(type name) { member.set##name(name);}
|
||||
|
||||
#define MEMBER_PARAM_GET(member, type, name, qualifier, setqualifier, getqualifier)\
|
||||
getqualifier: inline type get##name() const {return member.get##name();}
|
||||
|
||||
#define STRUCT_PARAM_SET_GET(member, type, name, qualifier, setqualifier, getqualifier)\
|
||||
getqualifier: inline type get##name() const {return member.name;}\
|
||||
setqualifier: inline void set##name(type name) {member.name=name;}
|
||||
|
||||
#define STRUCT_PARAM_SET(member, type, name, qualifier, setqualifier, getqualifier)\
|
||||
setqualifier: inline void set##name(type name) {member.name=name;}
|
||||
|
||||
#define STRUCT_PARAM_GET(member, type, name, qualifier, setqualifier, getqualifier)\
|
||||
getqualifier: inline type get##name() const {return member.name;}\
|
||||
|
||||
#define convertStringArgument(var,val,buf) if (!strcmp(buf,#val)) var=val
|
||||
#endif
|
||||
@@ -0,0 +1,207 @@
|
||||
#ifndef _POINT_H_
|
||||
#define _POINT_H_
|
||||
#include <assert.h>
|
||||
#include <math.h>
|
||||
#include <iostream>
|
||||
#include "gvalues.h"
|
||||
|
||||
#define DEBUG_STREAM cerr << __PRETTY_FUNCTION__ << ":" //FIXME
|
||||
|
||||
namespace GMapping {
|
||||
|
||||
template <class T>
|
||||
struct point{
|
||||
inline point():x(0),y(0) {}
|
||||
inline point(T _x, T _y):x(_x),y(_y){}
|
||||
T x, y;
|
||||
};
|
||||
|
||||
template <class T>
|
||||
inline point<T> operator+(const point<T>& p1, const point<T>& p2){
|
||||
return point<T>(p1.x+p2.x, p1.y+p2.y);
|
||||
}
|
||||
|
||||
template <class T>
|
||||
inline point<T> operator - (const point<T> & p1, const point<T> & p2){
|
||||
return point<T>(p1.x-p2.x, p1.y-p2.y);
|
||||
}
|
||||
|
||||
template <class T>
|
||||
inline point<T> operator * (const point<T>& p, const T& v){
|
||||
return point<T>(p.x*v, p.y*v);
|
||||
}
|
||||
|
||||
template <class T>
|
||||
inline point<T> operator * (const T& v, const point<T>& p){
|
||||
return point<T>(p.x*v, p.y*v);
|
||||
}
|
||||
|
||||
template <class T>
|
||||
inline T operator * (const point<T>& p1, const point<T>& p2){
|
||||
return p1.x*p2.x+p1.y*p2.y;
|
||||
}
|
||||
|
||||
|
||||
template <class T, class A>
|
||||
struct orientedpoint: public point<T>{
|
||||
inline orientedpoint() : point<T>(0,0), theta(0) {};
|
||||
inline orientedpoint(const point<T>& p);
|
||||
inline orientedpoint(T x, T y, A _theta): point<T>(x,y), theta(_theta){}
|
||||
inline void normalize();
|
||||
inline orientedpoint<T,A> rotate(A alpha){
|
||||
T s=sin(alpha), c=cos(alpha);
|
||||
A a=alpha+theta;
|
||||
a=atan2(sin(a),cos(a));
|
||||
return orientedpoint(
|
||||
c*this->x-s*this->y,
|
||||
s*this->x+c*this->y,
|
||||
a);
|
||||
}
|
||||
A theta;
|
||||
};
|
||||
|
||||
|
||||
template <class T, class A>
|
||||
void orientedpoint<T,A>::normalize() {
|
||||
if (theta >= -M_PI && theta < M_PI)
|
||||
return;
|
||||
|
||||
int multiplier = (int)(theta / (2*M_PI));
|
||||
theta = theta - multiplier*2*M_PI;
|
||||
if (theta >= M_PI)
|
||||
theta -= 2*M_PI;
|
||||
if (theta < -M_PI)
|
||||
theta += 2*M_PI;
|
||||
}
|
||||
|
||||
|
||||
template <class T, class A>
|
||||
orientedpoint<T,A>::orientedpoint(const point<T>& p){
|
||||
this->x=p.x;
|
||||
this->y=p.y;
|
||||
this->theta=0.;
|
||||
}
|
||||
|
||||
|
||||
template <class T, class A>
|
||||
orientedpoint<T,A> operator+(const orientedpoint<T,A>& p1, const orientedpoint<T,A>& p2){
|
||||
return orientedpoint<T,A>(p1.x+p2.x, p1.y+p2.y, p1.theta+p2.theta);
|
||||
}
|
||||
|
||||
template <class T, class A>
|
||||
orientedpoint<T,A> operator - (const orientedpoint<T,A> & p1, const orientedpoint<T,A> & p2){
|
||||
return orientedpoint<T,A>(p1.x-p2.x, p1.y-p2.y, p1.theta-p2.theta);
|
||||
}
|
||||
|
||||
template <class T, class A>
|
||||
orientedpoint<T,A> operator * (const orientedpoint<T,A>& p, const T& v){
|
||||
return orientedpoint<T,A>(p.x*v, p.y*v, p.theta*v);
|
||||
}
|
||||
|
||||
template <class T, class A>
|
||||
orientedpoint<T,A> operator * (const T& v, const orientedpoint<T,A>& p){
|
||||
return orientedpoint<T,A>(p.x*v, p.y*v, p.theta*v);
|
||||
}
|
||||
|
||||
template <class T, class A>
|
||||
orientedpoint<T,A> absoluteDifference(const orientedpoint<T,A>& p1,const orientedpoint<T,A>& p2){
|
||||
orientedpoint<T,A> delta=p1-p2;
|
||||
delta.theta=atan2(sin(delta.theta), cos(delta.theta));
|
||||
double s=sin(p2.theta), c=cos(p2.theta);
|
||||
return orientedpoint<T,A>(c*delta.x+s*delta.y,
|
||||
-s*delta.x+c*delta.y, delta.theta);
|
||||
}
|
||||
|
||||
template <class T, class A>
|
||||
orientedpoint<T,A> absoluteSum(const orientedpoint<T,A>& p1,const orientedpoint<T,A>& p2){
|
||||
double s=sin(p1.theta), c=cos(p1.theta);
|
||||
return orientedpoint<T,A>(c*p2.x-s*p2.y,
|
||||
s*p2.x+c*p2.y, p2.theta) + p1;
|
||||
}
|
||||
|
||||
template <class T, class A>
|
||||
point<T> absoluteSum(const orientedpoint<T,A>& p1,const point<T>& p2){
|
||||
double s=sin(p1.theta), c=cos(p1.theta);
|
||||
return point<T>(c*p2.x-s*p2.y, s*p2.x+c*p2.y) + (point<T>) p1;
|
||||
}
|
||||
|
||||
template <class T>
|
||||
struct pointcomparator{
|
||||
bool operator ()(const point<T>& a, const point<T>& b) const {
|
||||
return a.x<b.x || (a.x==b.x && a.y<b.y);
|
||||
}
|
||||
};
|
||||
|
||||
template <class T>
|
||||
struct pointradialcomparator{
|
||||
point<T> origin;
|
||||
bool operator ()(const point<T>& a, const point<T>& b) const {
|
||||
point<T> delta1=a-origin;
|
||||
point<T> delta2=b-origin;
|
||||
return (atan2(delta1.y,delta1.x)<atan2(delta2.y,delta2.x));
|
||||
}
|
||||
};
|
||||
|
||||
template <class T>
|
||||
inline point<T> max(const point<T>& p1, const point<T>& p2){
|
||||
point<T> p=p1;
|
||||
p.x=p.x>p2.x?p.x:p2.x;
|
||||
p.y=p.y>p2.y?p.y:p2.y;
|
||||
return p;
|
||||
}
|
||||
|
||||
template <class T>
|
||||
inline point<T> min(const point<T>& p1, const point<T>& p2){
|
||||
point<T> p=p1;
|
||||
p.x=p.x<p2.x?p.x:p2.x;
|
||||
p.y=p.y<p2.y?p.y:p2.y;
|
||||
return p;
|
||||
}
|
||||
|
||||
template <class T, class F>
|
||||
inline point<T> interpolate(const point<T>& p1, const F& t1, const point<T>& p2, const F& t2, const F& t3){
|
||||
F gain=(t3-t1)/(t2-t1);
|
||||
point<T> p=p1+(p2-p1)*gain;
|
||||
return p;
|
||||
}
|
||||
|
||||
template <class T, class A, class F>
|
||||
inline orientedpoint<T,A>
|
||||
interpolate(const orientedpoint<T,A>& p1, const F& t1, const orientedpoint<T,A>& p2, const F& t2, const F& t3){
|
||||
F gain=(t3-t1)/(t2-t1);
|
||||
orientedpoint<T,A> p;
|
||||
p.x=p1.x+(p2.x-p1.x)*gain;
|
||||
p.y=p1.y+(p2.y-p1.y)*gain;
|
||||
double s=sin(p1.theta)+sin(p2.theta)*gain,
|
||||
c=cos(p1.theta)+cos(p2.theta)*gain;
|
||||
p.theta=atan2(s,c);
|
||||
return p;
|
||||
}
|
||||
|
||||
|
||||
template <class T>
|
||||
inline double euclidianDist(const point<T>& p1, const point<T>& p2){
|
||||
return hypot(p1.x-p2.x, p1.y-p2.y);
|
||||
}
|
||||
template <class T, class A>
|
||||
inline double euclidianDist(const orientedpoint<T,A>& p1, const orientedpoint<T,A>& p2){
|
||||
return hypot(p1.x-p2.x, p1.y-p2.y);
|
||||
}
|
||||
template <class T, class A>
|
||||
inline double euclidianDist(const orientedpoint<T,A>& p1, const point<T>& p2){
|
||||
return hypot(p1.x-p2.x, p1.y-p2.y);
|
||||
}
|
||||
template <class T, class A>
|
||||
inline double euclidianDist(const point<T>& p1, const orientedpoint<T,A>& p2 ){
|
||||
return hypot(p1.x-p2.x, p1.y-p2.y);
|
||||
}
|
||||
|
||||
|
||||
|
||||
typedef point<int> IntPoint;
|
||||
typedef point<double> Point;
|
||||
typedef orientedpoint<double, double> OrientedPoint;
|
||||
|
||||
}; //end namespace
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,147 @@
|
||||
#ifndef STAT_H
|
||||
#define STAT_H
|
||||
#include "point.h"
|
||||
#include <vector>
|
||||
#include "gvalues.h"
|
||||
|
||||
namespace GMapping {
|
||||
|
||||
/**stupid utility function for drawing particles form a zero mean, sigma variance normal distribution
|
||||
probably it should not go there*/
|
||||
double sampleGaussian(double sigma,unsigned int S=0);
|
||||
|
||||
double evalGaussian(double sigmaSquare, double delta);
|
||||
double evalLogGaussian(double sigmaSquare, double delta);
|
||||
int sampleUniformInt(int max);
|
||||
double sampleUniformDouble(double min, double max);
|
||||
|
||||
struct Covariance3{
|
||||
Covariance3 operator + (const Covariance3 & cov) const;
|
||||
static Covariance3 zero;
|
||||
double xx, yy, tt, xy, xt, yt;
|
||||
};
|
||||
|
||||
struct EigenCovariance3{
|
||||
EigenCovariance3();
|
||||
EigenCovariance3(const Covariance3& c);
|
||||
EigenCovariance3 rotate(double angle) const;
|
||||
OrientedPoint sample() const;
|
||||
double eval[3];
|
||||
double evec[3][3];
|
||||
};
|
||||
|
||||
struct Gaussian3{
|
||||
OrientedPoint mean;
|
||||
EigenCovariance3 covariance;
|
||||
Covariance3 cov;
|
||||
double eval(const OrientedPoint& p) const;
|
||||
void computeFromSamples(const std::vector<OrientedPoint> & poses);
|
||||
void computeFromSamples(const std::vector<OrientedPoint> & poses, const std::vector<double>& weights );
|
||||
};
|
||||
|
||||
template<typename PointIterator, typename WeightIterator>
|
||||
Gaussian3 computeGaussianFromSamples(PointIterator& pointBegin, PointIterator& pointEnd, WeightIterator& weightBegin, WeightIterator& weightEnd){
|
||||
Gaussian3 gaussian;
|
||||
OrientedPoint mean=OrientedPoint(0,0,0);
|
||||
double wcum=0;
|
||||
double s=0, c=0;
|
||||
WeightIterator wt=weightBegin;
|
||||
double *w=new double();
|
||||
OrientedPoint *p=new OrientedPoint();
|
||||
for (PointIterator pt=pointBegin; pt!=pointEnd; pt++){
|
||||
*w=*wt;
|
||||
*p=*pt;
|
||||
s+=*w*sin(p->theta);
|
||||
c+=*w*cos(p->theta);
|
||||
mean.x+=*w*p->x;
|
||||
mean.y+=*w*p->y;
|
||||
wcum+=*w;
|
||||
wt++;
|
||||
}
|
||||
mean.x/=wcum;
|
||||
mean.y/=wcum;
|
||||
s/=wcum;
|
||||
c/=wcum;
|
||||
mean.theta=atan2(s,c);
|
||||
|
||||
Covariance3 cov=Covariance3::zero;
|
||||
wt=weightBegin;
|
||||
for (PointIterator pt=pointBegin; pt!=pointEnd; pt++){
|
||||
*w=*wt;
|
||||
*p=*pt;
|
||||
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;
|
||||
wt++;
|
||||
}
|
||||
cov.xx/=wcum;
|
||||
cov.yy/=wcum;
|
||||
cov.tt/=wcum;
|
||||
cov.xy/=wcum;
|
||||
cov.yt/=wcum;
|
||||
cov.xt/=wcum;
|
||||
EigenCovariance3 ecov(cov);
|
||||
gaussian.mean=mean;
|
||||
gaussian.covariance=ecov;
|
||||
gaussian.cov=cov;
|
||||
delete w;
|
||||
delete p;
|
||||
return gaussian;
|
||||
}
|
||||
|
||||
template<typename PointIterator>
|
||||
Gaussian3 computeGaussianFromSamples(PointIterator& pointBegin, PointIterator& pointEnd){
|
||||
Gaussian3 gaussian;
|
||||
OrientedPoint mean=OrientedPoint(0,0,0);
|
||||
double wcum=1;
|
||||
double s=0, c=0;
|
||||
OrientedPoint *p=new OrientedPoint();
|
||||
for (PointIterator pt=pointBegin; pt!=pointEnd; pt++){
|
||||
*p=*pt;
|
||||
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 (PointIterator pt=pointBegin; pt!=pointEnd; pt++){
|
||||
*p=*pt;
|
||||
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);
|
||||
gaussian.mean=mean;
|
||||
gaussian.covariance=ecov;
|
||||
gaussian.cov=cov;
|
||||
delete p;
|
||||
return gaussian;
|
||||
}
|
||||
|
||||
|
||||
}; //end namespace
|
||||
#endif
|
||||
|
||||
Reference in New Issue
Block a user