Merge pull request #196 from mtlazaro/master

Considering min range as laser param. Now points with range under min…
RainerKuemmerle · Aug 3, 2017 · dd0b67b · dd0b67b
2 parents 17fce87 + cb5d5cc
commit dd0b67b
Show file tree

Hide file tree

Showing 3 changed files with 8 additions and 5 deletions.
diff --git a/g2o/types/data/laser_parameters.cpp b/g2o/types/data/laser_parameters.cpp
@@ -28,24 +28,26 @@
 
 namespace g2o {
 
-  LaserParameters::LaserParameters(int t, int nbeams, double _firstBeamAngle, double _angularStep, double _maxRange, double _accuracy, int _remissionMode)
+  LaserParameters::LaserParameters(int t, int nbeams, double _firstBeamAngle, double _angularStep, double _maxRange, double _accuracy, int _remissionMode, double _minRange)
   {
     type           = t;
     firstBeamAngle = _firstBeamAngle;
     angularStep    = _angularStep;
     maxRange       = _maxRange;
+    minRange       = _minRange;
     laserPose      = SE2(0., 0., 0.);
     accuracy       = _accuracy;
     remissionMode  = _remissionMode;
     fov            = angularStep * nbeams;
   }
 
-  LaserParameters::LaserParameters(int nbeams, double _firstBeamAngle, double _angularStep, double _maxRange)
+  LaserParameters::LaserParameters(int nbeams, double _firstBeamAngle, double _angularStep, double _maxRange, double _minRange)
   {
     type           = 0;
     firstBeamAngle = _firstBeamAngle;
     angularStep    = _angularStep;
     maxRange       = _maxRange;
+    minRange       = _minRange;
     laserPose      = SE2(0., 0., 0.);
     accuracy       = 0.1;
     remissionMode  = 0;

diff --git a/g2o/types/data/laser_parameters.h b/g2o/types/data/laser_parameters.h
@@ -38,8 +38,8 @@ namespace g2o {
   struct G2O_TYPES_DATA_API LaserParameters
   {
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
-    LaserParameters(int type, int beams, double firstBeamAngle, double angularStep, double maxRange, double accuracy, int remissionMode);
-    LaserParameters(int beams, double firstBeamAngle, double angularStep, double maxRange);
+    LaserParameters(int type, int beams, double firstBeamAngle, double angularStep, double maxRange, double accuracy, int remissionMode, double minRange = 0.0);
+    LaserParameters(int beams, double firstBeamAngle, double angularStep, double maxRange, double minRange = 0.0);
     SE2 laserPose;
     int type;
     double firstBeamAngle;
@@ -48,6 +48,7 @@ namespace g2o {
     double accuracy;
     int remissionMode;
     double maxRange;
+    double minRange;
   };
 
 }

diff --git a/g2o/types/data/raw_laser.cpp b/g2o/types/data/raw_laser.cpp
@@ -95,7 +95,7 @@ namespace g2o {
     Point2DVector points;
     for (size_t i = 0; i < _ranges.size(); ++i) {
       const double& r = _ranges[i];
-      if (r < _laserParams.maxRange) {
+      if (r < _laserParams.maxRange && r > _laserParams.minRange) {
         double alpha = _laserParams.firstBeamAngle + i * _laserParams.angularStep;
         points.push_back(Vector2D(cos(alpha) * r, sin(alpha) * r));
       }