Skip to content

Commit 9ff4e95

Browse files
Tacha-STacha-S
committed
Support high resolution beams
1 parent 49feb77 commit 9ff4e95

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
@@ -7,7 +7,7 @@
77
#include <gmapping/utils/stat.h>
88
#include <iostream>
99
#include <gmapping/utils/gvalues.h>
10-
#define LASER_MAXBEAMS 2048
10+
#include <vector>
1111

1212
namespace GMapping {
1313

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

4040
static const double nullLikelihood;
@@ -44,7 +44,7 @@ class ScanMatcher{
4444

4545
/**laser parameters*/
4646
unsigned int m_laserBeams;
47-
double m_laserAngles[LASER_MAXBEAMS];
47+
std::vector<double> m_laserAngles;
4848
//OrientedPoint m_laserPose;
4949
PARAM_SET_GET(OrientedPoint, laserPose, protected, public, public)
5050
PARAM_SET_GET(double, laserMaxRange, protected, public, public)
@@ -74,7 +74,7 @@ class ScanMatcher{
7474
};
7575

7676
inline double ScanMatcher::icpStep(OrientedPoint & pret, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const{
77-
const double * angle=m_laserAngles+m_initialBeamsSkip;
77+
std::vector<double>::const_iterator angle=m_laserAngles.begin()+m_initialBeamsSkip;
7878
OrientedPoint lp=p;
7979
lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;
8080
lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y;
@@ -89,12 +89,12 @@ inline double ScanMatcher::icpStep(OrientedPoint & pret, const ScanMatcherMap& m
8989
if (*r>m_usableRange||*r==0.0) continue;
9090
if (skip) continue;
9191
Point phit=lp;
92-
phit.x+=*r*cos(lp.theta+*angle);
93-
phit.y+=*r*sin(lp.theta+*angle);
92+
phit.x+=*r*cos(lp.theta+(*angle));
93+
phit.y+=*r*sin(lp.theta+(*angle));
9494
IntPoint iphit=map.world2map(phit);
9595
Point pfree=lp;
96-
pfree.x+=(*r-map.getDelta()*freeDelta)*cos(lp.theta+*angle);
97-
pfree.y+=(*r-map.getDelta()*freeDelta)*sin(lp.theta+*angle);
96+
pfree.x+=(*r-map.getDelta()*freeDelta)*cos(lp.theta+(*angle));
97+
pfree.y+=(*r-map.getDelta()*freeDelta)*sin(lp.theta+(*angle));
9898
pfree=pfree-phit;
9999
IntPoint ipfree=map.world2map(pfree);
100100
bool found=false;
@@ -142,7 +142,7 @@ inline double ScanMatcher::icpStep(OrientedPoint & pret, const ScanMatcherMap& m
142142

143143
inline double ScanMatcher::score(const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const{
144144
double s=0;
145-
const double * angle=m_laserAngles+m_initialBeamsSkip;
145+
std::vector<double>::const_iterator angle=m_laserAngles.begin()+m_initialBeamsSkip;
146146
OrientedPoint lp=p;
147147
lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;
148148
lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y;
@@ -154,12 +154,12 @@ inline double ScanMatcher::score(const ScanMatcherMap& map, const OrientedPoint&
154154
skip=skip>m_likelihoodSkip?0:skip;
155155
if (skip||*r>m_usableRange||*r==0.0) continue;
156156
Point phit=lp;
157-
phit.x+=*r*cos(lp.theta+*angle);
158-
phit.y+=*r*sin(lp.theta+*angle);
157+
phit.x+=*r*cos(lp.theta+(*angle));
158+
phit.y+=*r*sin(lp.theta+(*angle));
159159
IntPoint iphit=map.world2map(phit);
160160
Point pfree=lp;
161-
pfree.x+=(*r-map.getDelta()*freeDelta)*cos(lp.theta+*angle);
162-
pfree.y+=(*r-map.getDelta()*freeDelta)*sin(lp.theta+*angle);
161+
pfree.x+=(*r-map.getDelta()*freeDelta)*cos(lp.theta+(*angle));
162+
pfree.y+=(*r-map.getDelta()*freeDelta)*sin(lp.theta+(*angle));
163163
pfree=pfree-phit;
164164
IntPoint ipfree=map.world2map(pfree);
165165
bool found=false;
@@ -192,7 +192,7 @@ inline unsigned int ScanMatcher::likelihoodAndScore(double& s, double& l, const
192192
using namespace std;
193193
l=0;
194194
s=0;
195-
const double * angle=m_laserAngles+m_initialBeamsSkip;
195+
std::vector<double>::const_iterator angle=m_laserAngles.begin()+m_initialBeamsSkip;
196196
OrientedPoint lp=p;
197197
lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;
198198
lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y;
@@ -207,12 +207,12 @@ inline unsigned int ScanMatcher::likelihoodAndScore(double& s, double& l, const
207207
if (*r>m_usableRange) continue;
208208
if (skip) continue;
209209
Point phit=lp;
210-
phit.x+=*r*cos(lp.theta+*angle);
211-
phit.y+=*r*sin(lp.theta+*angle);
210+
phit.x+=*r*cos(lp.theta+(*angle));
211+
phit.y+=*r*sin(lp.theta+(*angle));
212212
IntPoint iphit=map.world2map(phit);
213213
Point pfree=lp;
214-
pfree.x+=(*r-freeDelta)*cos(lp.theta+*angle);
215-
pfree.y+=(*r-freeDelta)*sin(lp.theta+*angle);
214+
pfree.x+=(*r-freeDelta)*cos(lp.theta+(*angle));
215+
pfree.y+=(*r-freeDelta)*sin(lp.theta+(*angle));
216216
pfree=pfree-phit;
217217
IntPoint ipfree=map.world2map(pfree);
218218
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)