Skip to content

Commit

Permalink
Support high resolution beams
Browse files Browse the repository at this point in the history
  • Loading branch information
@Tacha-S
Tacha-S committed Sep 30, 2021
1 parent 5e9a6f7 commit 4008fa8
Show file tree
Hide file tree
Showing 2 changed files with 28 additions and 29 deletions.
36 changes: 18 additions & 18 deletions include/gmapping/scanmatcher/scanmatcher.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,7 @@
#include <iostream>
#include <gmapping/utils/gvalues.h>
#include <gmapping/scanmatcher/scanmatcher_export.h>
#define LASER_MAXBEAMS 2048
#include <vector>

namespace GMapping {

Expand All @@ -35,7 +35,7 @@ class SCANMATCHER_EXPORT ScanMatcher{
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 const double* laserAngles() const { return m_laserAngles.data(); }
inline unsigned int laserBeams() const { return m_laserBeams; }

static const double nullLikelihood;
Expand All @@ -45,7 +45,7 @@ class SCANMATCHER_EXPORT ScanMatcher{

/**laser parameters*/
unsigned int m_laserBeams;
double m_laserAngles[LASER_MAXBEAMS];
std::vector<double> m_laserAngles;
//OrientedPoint m_laserPose;
PARAM_SET_GET(OrientedPoint, laserPose, protected, public, public)
PARAM_SET_GET(double, laserMaxRange, protected, public, public)
Expand Down Expand Up @@ -75,7 +75,7 @@ class SCANMATCHER_EXPORT ScanMatcher{
};

inline double ScanMatcher::icpStep(OrientedPoint & pret, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const{
const double * angle=m_laserAngles+m_initialBeamsSkip;
std::vector<double>::const_iterator angle=m_laserAngles.begin()+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;
Expand All @@ -90,12 +90,12 @@ inline double ScanMatcher::icpStep(OrientedPoint & pret, const ScanMatcherMap& m
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);
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.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;
Expand Down Expand Up @@ -143,7 +143,7 @@ inline double ScanMatcher::icpStep(OrientedPoint & pret, const ScanMatcherMap& m

inline double ScanMatcher::score(const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const{
double s=0;
const double * angle=m_laserAngles+m_initialBeamsSkip;
std::vector<double>::const_iterator angle=m_laserAngles.begin()+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;
Expand All @@ -155,12 +155,12 @@ inline double ScanMatcher::score(const ScanMatcherMap& map, const OrientedPoint&
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);
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.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;
Expand Down Expand Up @@ -193,7 +193,7 @@ inline unsigned int ScanMatcher::likelihoodAndScore(double& s, double& l, const
using namespace std;
l=0;
s=0;
const double * angle=m_laserAngles+m_initialBeamsSkip;
std::vector<double>::const_iterator angle=m_laserAngles.begin()+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;
Expand All @@ -208,12 +208,12 @@ inline unsigned int ScanMatcher::likelihoodAndScore(double& s, double& l, const
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);
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.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;
Expand Down
21 changes: 10 additions & 11 deletions scanmatcher/scanmatcher.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -131,13 +131,13 @@ void ScanMatcher::computeActiveArea(ScanMatcherMap& map, const OrientedPoint& p,
if (lp.y>max.y) max.y=lp.y;

/*determine the size of the area*/
const double * angle=m_laserAngles+m_initialBeamsSkip;
std::vector<double>::const_iterator angle=m_laserAngles.begin()+m_initialBeamsSkip;
for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++){
if (*r>m_laserMaxRange||*r==0.0||isnan(*r)) continue;
double d=*r>m_usableRange?m_usableRange:*r;
Point phit=lp;
phit.x+=d*cos(lp.theta+*angle);
phit.y+=d*sin(lp.theta+*angle);
phit.x+=d*cos(lp.theta+(*angle));
phit.y+=d*sin(lp.theta+(*angle));
if (phit.x<min.x) min.x=phit.x;
if (phit.y<min.y) min.y=phit.y;
if (phit.x>max.x) max.x=phit.x;
Expand All @@ -161,15 +161,15 @@ void ScanMatcher::computeActiveArea(ScanMatcherMap& map, const OrientedPoint& p,

HierarchicalArray2D<PointAccumulator>::PointSet activeArea;
/*allocate the active area*/
angle=m_laserAngles+m_initialBeamsSkip;
angle=m_laserAngles.begin()+m_initialBeamsSkip;
for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++)
if (m_generateMap){
double d=*r;
if (d>m_laserMaxRange||d==0.0||isnan(d))
continue;
if (d>m_usableRange)
d=m_usableRange;
Point phit=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle));
Point phit=lp+Point(d*cos(lp.theta+(*angle)),d*sin(lp.theta+(*angle)));
IntPoint p0=map.world2map(lp);
IntPoint p1=map.world2map(phit);

Expand Down Expand Up @@ -226,7 +226,7 @@ double ScanMatcher::registerScan(ScanMatcherMap& map, const OrientedPoint& p, co
IntPoint p0=map.world2map(lp);


const double * angle=m_laserAngles+m_initialBeamsSkip;
std::vector<double>::const_iterator angle=m_laserAngles.begin()+m_initialBeamsSkip;
double esum=0;
for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++)
if (m_generateMap){
Expand All @@ -235,7 +235,7 @@ double ScanMatcher::registerScan(ScanMatcherMap& map, const OrientedPoint& p, co
continue;
if (d>m_usableRange)
d=m_usableRange;
Point phit=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle));
Point phit=lp+Point(d*cos(lp.theta+(*angle)),d*sin(lp.theta+(*angle)));
IntPoint p1=map.world2map(phit);
//IntPoint linePoints[20000] ;
GridLineTraversalLine line;
Expand All @@ -257,8 +257,8 @@ double ScanMatcher::registerScan(ScanMatcherMap& map, const OrientedPoint& p, co
} else {
if (*r>m_laserMaxRange||*r>m_usableRange||*r==0.0||isnan(*r)) continue;
Point phit=lp;
phit.x+=*r*cos(lp.theta+*angle);
phit.y+=*r*sin(lp.theta+*angle);
phit.x+=*r*cos(lp.theta+(*angle));
phit.y+=*r*sin(lp.theta+(*angle));
IntPoint p1=map.world2map(phit);
assert(p1.x>=0 && p1.y>=0);
map.cell(p1).update(true,phit);
Expand Down Expand Up @@ -560,11 +560,10 @@ void ScanMatcher::setLaserParameters
/*if (m_laserAngles)
delete [] m_laserAngles;
*/
assert(beams<LASER_MAXBEAMS);
m_laserPose=lpose;
m_laserBeams=beams;
//m_laserAngles=new double[beams];
memcpy(m_laserAngles, angles, sizeof(double)*m_laserBeams);
m_laserAngles = vector<double>(angles, angles+beams);
}


Expand Down

0 comments on commit 4008fa8

Please sign in to comment.