Skip to content

Commit 4008fa8

Browse files
Tacha-STacha-S
committed
Support high resolution beams
1 parent 5e9a6f7 commit 4008fa8

File tree

2 files changed

+28
-29
lines changed

2 files changed

+28
-29
lines changed

‎include/gmapping/scanmatcher/scanmatcher.h‎

Lines changed: 18 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -8,7 +8,7 @@
88
#include <iostream>
99
#include <gmapping/utils/gvalues.h>
1010
#include <gmapping/scanmatcher/scanmatcher_export.h>
11-
#define LASER_MAXBEAMS 2048
11+
#include <vector>
1212

1313
namespace GMapping {
1414

@@ -35,7 +35,7 @@ class SCANMATCHER_EXPORT ScanMatcher{
3535
inline unsigned int likelihoodAndScore(double& s, double& l, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const;
3636
double likelihood(double& lmax, OrientedPoint& mean, CovarianceMatrix& cov, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings);
3737
double likelihood(double& _lmax, OrientedPoint& _mean, CovarianceMatrix& _cov, const ScanMatcherMap& map, const OrientedPoint& p, Gaussian3& odometry, const double* readings, double gain=180.);
38-
inline const double* laserAngles() const { return m_laserAngles; }
38+
inline const double* laserAngles() const { return m_laserAngles.data(); }
3939
inline unsigned int laserBeams() const { return m_laserBeams; }
4040

4141
static const double nullLikelihood;
@@ -45,7 +45,7 @@ class SCANMATCHER_EXPORT ScanMatcher{
4545

4646
/**laser parameters*/
4747
unsigned int m_laserBeams;
48-
double m_laserAngles[LASER_MAXBEAMS];
48+
std::vector<double> m_laserAngles;
4949
//OrientedPoint m_laserPose;
5050
PARAM_SET_GET(OrientedPoint, laserPose, protected, public, public)
5151
PARAM_SET_GET(double, laserMaxRange, protected, public, public)
@@ -75,7 +75,7 @@ class SCANMATCHER_EXPORT ScanMatcher{
7575
};
7676

7777
inline double ScanMatcher::icpStep(OrientedPoint & pret, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const{
78-
const double * angle=m_laserAngles+m_initialBeamsSkip;
78+
std::vector<double>::const_iterator angle=m_laserAngles.begin()+m_initialBeamsSkip;
7979
OrientedPoint lp=p;
8080
lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;
8181
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
9090
if (*r>m_usableRange||*r==0.0) continue;
9191
if (skip) continue;
9292
Point phit=lp;
93-
phit.x+=*r*cos(lp.theta+*angle);
94-
phit.y+=*r*sin(lp.theta+*angle);
93+
phit.x+=*r*cos(lp.theta+(*angle));
94+
phit.y+=*r*sin(lp.theta+(*angle));
9595
IntPoint iphit=map.world2map(phit);
9696
Point pfree=lp;
97-
pfree.x+=(*r-map.getDelta()*freeDelta)*cos(lp.theta+*angle);
98-
pfree.y+=(*r-map.getDelta()*freeDelta)*sin(lp.theta+*angle);
97+
pfree.x+=(*r-map.getDelta()*freeDelta)*cos(lp.theta+(*angle));
98+
pfree.y+=(*r-map.getDelta()*freeDelta)*sin(lp.theta+(*angle));
9999
pfree=pfree-phit;
100100
IntPoint ipfree=map.world2map(pfree);
101101
bool found=false;
@@ -143,7 +143,7 @@ inline double ScanMatcher::icpStep(OrientedPoint & pret, const ScanMatcherMap& m
143143

144144
inline double ScanMatcher::score(const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const{
145145
double s=0;
146-
const double * angle=m_laserAngles+m_initialBeamsSkip;
146+
std::vector<double>::const_iterator angle=m_laserAngles.begin()+m_initialBeamsSkip;
147147
OrientedPoint lp=p;
148148
lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;
149149
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&
155155
skip=skip>m_likelihoodSkip?0:skip;
156156
if (skip||*r>m_usableRange||*r==0.0) continue;
157157
Point phit=lp;
158-
phit.x+=*r*cos(lp.theta+*angle);
159-
phit.y+=*r*sin(lp.theta+*angle);
158+
phit.x+=*r*cos(lp.theta+(*angle));
159+
phit.y+=*r*sin(lp.theta+(*angle));
160160
IntPoint iphit=map.world2map(phit);
161161
Point pfree=lp;
162-
pfree.x+=(*r-map.getDelta()*freeDelta)*cos(lp.theta+*angle);
163-
pfree.y+=(*r-map.getDelta()*freeDelta)*sin(lp.theta+*angle);
162+
pfree.x+=(*r-map.getDelta()*freeDelta)*cos(lp.theta+(*angle));
163+
pfree.y+=(*r-map.getDelta()*freeDelta)*sin(lp.theta+(*angle));
164164
pfree=pfree-phit;
165165
IntPoint ipfree=map.world2map(pfree);
166166
bool found=false;
@@ -193,7 +193,7 @@ inline unsigned int ScanMatcher::likelihoodAndScore(double& s, double& l, const
193193
using namespace std;
194194
l=0;
195195
s=0;
196-
const double * angle=m_laserAngles+m_initialBeamsSkip;
196+
std::vector<double>::const_iterator angle=m_laserAngles.begin()+m_initialBeamsSkip;
197197
OrientedPoint lp=p;
198198
lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;
199199
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
208208
if (*r>m_usableRange) continue;
209209
if (skip) continue;
210210
Point phit=lp;
211-
phit.x+=*r*cos(lp.theta+*angle);
212-
phit.y+=*r*sin(lp.theta+*angle);
211+
phit.x+=*r*cos(lp.theta+(*angle));
212+
phit.y+=*r*sin(lp.theta+(*angle));
213213
IntPoint iphit=map.world2map(phit);
214214
Point pfree=lp;
215-
pfree.x+=(*r-freeDelta)*cos(lp.theta+*angle);
216-
pfree.y+=(*r-freeDelta)*sin(lp.theta+*angle);
215+
pfree.x+=(*r-freeDelta)*cos(lp.theta+(*angle));
216+
pfree.y+=(*r-freeDelta)*sin(lp.theta+(*angle));
217217
pfree=pfree-phit;
218218
IntPoint ipfree=map.world2map(pfree);
219219
bool found=false;

‎scanmatcher/scanmatcher.cpp‎

Lines changed: 10 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -131,13 +131,13 @@ void ScanMatcher::computeActiveArea(ScanMatcherMap& map, const OrientedPoint& p,
131131
if (lp.y>max.y) max.y=lp.y;
132132

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

162162
HierarchicalArray2D<PointAccumulator>::PointSet activeArea;
163163
/*allocate the active area*/
164-
angle=m_laserAngles+m_initialBeamsSkip;
164+
angle=m_laserAngles.begin()+m_initialBeamsSkip;
165165
for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++)
166166
if (m_generateMap){
167167
double d=*r;
168168
if (d>m_laserMaxRange||d==0.0||isnan(d))
169169
continue;
170170
if (d>m_usableRange)
171171
d=m_usableRange;
172-
Point phit=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle));
172+
Point phit=lp+Point(d*cos(lp.theta+(*angle)),d*sin(lp.theta+(*angle)));
173173
IntPoint p0=map.world2map(lp);
174174
IntPoint p1=map.world2map(phit);
175175

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

228228

229-
const double * angle=m_laserAngles+m_initialBeamsSkip;
229+
std::vector<double>::const_iterator angle=m_laserAngles.begin()+m_initialBeamsSkip;
230230
double esum=0;
231231
for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++)
232232
if (m_generateMap){
@@ -235,7 +235,7 @@ double ScanMatcher::registerScan(ScanMatcherMap& map, const OrientedPoint& p, co
235235
continue;
236236
if (d>m_usableRange)
237237
d=m_usableRange;
238-
Point phit=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle));
238+
Point phit=lp+Point(d*cos(lp.theta+(*angle)),d*sin(lp.theta+(*angle)));
239239
IntPoint p1=map.world2map(phit);
240240
//IntPoint linePoints[20000] ;
241241
GridLineTraversalLine line;
@@ -257,8 +257,8 @@ double ScanMatcher::registerScan(ScanMatcherMap& map, const OrientedPoint& p, co
257257
} else {
258258
if (*r>m_laserMaxRange||*r>m_usableRange||*r==0.0||isnan(*r)) continue;
259259
Point phit=lp;
260-
phit.x+=*r*cos(lp.theta+*angle);
261-
phit.y+=*r*sin(lp.theta+*angle);
260+
phit.x+=*r*cos(lp.theta+(*angle));
261+
phit.y+=*r*sin(lp.theta+(*angle));
262262
IntPoint p1=map.world2map(phit);
263263
assert(p1.x>=0 && p1.y>=0);
264264
map.cell(p1).update(true,phit);
@@ -560,11 +560,10 @@ void ScanMatcher::setLaserParameters
560560
/*if (m_laserAngles)
561561
delete [] m_laserAngles;
562562
*/
563-
assert(beams<LASER_MAXBEAMS);
564563
m_laserPose=lpose;
565564
m_laserBeams=beams;
566565
//m_laserAngles=new double[beams];
567-
memcpy(m_laserAngles, angles, sizeof(double)*m_laserBeams);
566+
m_laserAngles = vector<double>(angles, angles+beams);
568567
}
569568

570569

0 commit comments

Comments
 (0)