-----------------------------------------------------------
Data structures
Point.h #ifndef _POINT_H_
#define _POINT_H_
#include <assert.h>
#include <math.h>
#include <iostream>
#include <sstream>
#include <fstream>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#define DEBUG_STREAM cerr << __PRETTY_FUNCTION__ << ":" //FIXME
using namespace boost::archive;
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;
friend class boost::serialization::access;
template <class Archive>
void serialize(Archive& ar, const unsigned int version){
ar & x & y;
}
};
...
Structure to serialize (originally, I want to serialize a Particle ...)
Struct Particle{
ScanMatcherMap map;
OrientedPoint pose;
double weight;
double weightSum;
double gweight;
int previousIndex;
TNode* node;
};
typedef Map<PointAccumulator, HierarchicalArray2D<PointAccumulator>> ScanMatcherMap;
template <class Cell, class Storage, const bool isClass=true> class Map{
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;
};
struct PointAccumulator{
typedef point<float> FloatPoint;
PointAccumulator(): acc(0,0), n(0), visits(0){}
PointAccumulator(int i): acc(0,0), n(0), visits(0){assert(i==-1);}
static PointAccumulator* unknown_ptr;
FloatPoint acc;
int n, visits;
};
template <class Cell> class HierarchicalArray2D: public Array2D<autoptr< Array2D<Cell> > >{
typedef std::set< point<int>, pointcomparator<int> > PointSet;
HierarchicalArray2D(int xsize, int ysize, int patchMagnitude=5);
PointSet m_activeArea;
int m_patchMagnitude;
int m_patchSize;
};
template<class Cell, const bool debug=false> class Array2D{
Cell ** m_cells;
int m_xsize, m_ysize;
};
template <class X> class autoptr{
struct reference{
X* data;
unsigned int shares;
};
reference * m_reference;
};
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);
OrientedPoint pose;
double weight;
double accWeight;
double gweight;
TNode* parent;
const RangeReading* reading;
unsigned int childs;
mutable unsigned int visitCounter;
mutable bool flag;
};
class RangeReading: public SensorReading, public std::vector<double>{
std::vector<Point> cartesianForm(double maxRange=1e6) const;
unsigned int activeBeams(double density=0.) const;
OrientedPoint m_pose;
};