add slam_gmapping
This commit is contained in:
X-lanni
@@ -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
|
||||
Reference in New Issue
Block a user