From 4008fa8c90689b26b27db8e530d4eecf62f2ec30 Mon Sep 17 00:00:00 2001 From: Tatsuro Sakaguchi Date: Tue, 28 Sep 2021 14:38:32 +0900 Subject: [PATCH] Support high resolution beams --- include/gmapping/scanmatcher/scanmatcher.h | 36 +++++++++++----------- scanmatcher/scanmatcher.cpp | 21 ++++++------- 2 files changed, 28 insertions(+), 29 deletions(-) diff --git a/include/gmapping/scanmatcher/scanmatcher.h b/include/gmapping/scanmatcher/scanmatcher.h index d9d4871..170c99b 100644 --- a/include/gmapping/scanmatcher/scanmatcher.h +++ b/include/gmapping/scanmatcher/scanmatcher.h @@ -8,7 +8,7 @@ #include #include #include -#define LASER_MAXBEAMS 2048 +#include namespace GMapping { @@ -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; @@ -45,7 +45,7 @@ class SCANMATCHER_EXPORT ScanMatcher{ /**laser parameters*/ unsigned int m_laserBeams; - double m_laserAngles[LASER_MAXBEAMS]; + std::vector m_laserAngles; //OrientedPoint m_laserPose; PARAM_SET_GET(OrientedPoint, laserPose, protected, public, public) PARAM_SET_GET(double, laserMaxRange, protected, public, public) @@ -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::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; @@ -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; @@ -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::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; @@ -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; @@ -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::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; @@ -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; diff --git a/scanmatcher/scanmatcher.cpp b/scanmatcher/scanmatcher.cpp index f7fb885..1480108 100644 --- a/scanmatcher/scanmatcher.cpp +++ b/scanmatcher/scanmatcher.cpp @@ -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::const_iterator angle=m_laserAngles.begin()+m_initialBeamsSkip; for (const double* r=readings+m_initialBeamsSkip; rm_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.xmax.x) max.x=phit.x; @@ -161,7 +161,7 @@ void ScanMatcher::computeActiveArea(ScanMatcherMap& map, const OrientedPoint& p, HierarchicalArray2D::PointSet activeArea; /*allocate the active area*/ - angle=m_laserAngles+m_initialBeamsSkip; + angle=m_laserAngles.begin()+m_initialBeamsSkip; for (const double* r=readings+m_initialBeamsSkip; rm_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); @@ -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::const_iterator angle=m_laserAngles.begin()+m_initialBeamsSkip; double esum=0; for (const double* r=readings+m_initialBeamsSkip; rm_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; @@ -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); @@ -560,11 +560,10 @@ void ScanMatcher::setLaserParameters /*if (m_laserAngles) delete [] m_laserAngles; */ - assert(beams(angles, angles+beams); }