Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Support high resolution beams #39

Open
wants to merge 2 commits into
base: melodic-devel
Choose a base branch
from
Open

Support high resolution beams #39

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
4008fa8
Support high resolution beams
Tacha-S Sep 28, 2021
b92f48a
Merge branch 'melodic-devel' into remove-beams-limit
Tacha-S Oct 20, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
36 changes: 18 additions & 18 deletions include/gmapping/scanmatcher/scanmatcher.h
View file
Open in desktop
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
Open in desktop
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