Re: [ros-users] slam_gmapping: extracting pose history

Hi all,

On Thu, 2010-05-13 at 21:08 -0700, Brian Gerkey wrote:
> I've also noticed that the time required to produce the occupancy grid
> grows over time. I don't find this too surprising, because presumably
> there's more raytracing to be done with each added pose / scan.

Online SLAM algorithms try to avoid a situation where this is necessary
since a time/space complexity that grows with the distance traveled
would be prohibitive.

The algorithm behind GMapping [1] only requires the current map, robot
pose and weight for each particle. By default, the map does not model
free space, but this can be changed through an appropriate
configuration.

The attached patch instructs GMapping to build the complete map
for each particle as part of the regular SLAM process and extracts the
map without any additional processing and without any unnecessary
copying. This should also avoid the need to patch GMapping to store past
laser scans, but I have left that part of the ROS package untouched for
now.

We used GMapping with a similar configuration at the RoboLudens and
RoboCup Rescue competitions in 2006 [2] and it worked well for online
operation.

Let me know if you have any questions regarding my changes. I'd
appreciate it if they could be incorporated into ROS.

Cheers,

Rene

[1]
http://www.informatik.uni-freiburg.de/~stachnis/pdf/grisetti06tro.pdf
[2] Thanks to Christoph Hertzberg for digging out the code and sending
    it my way.
-- 
------------------------------------------------------------
Dipl.-Inf. René Wagner                     Junior Researcher
DFKI Bremen                           Enrique-Schmidt-Str. 5
Safe and Secure Cognitive Systems             D-28359 Bremen

Phone: (+49) 421-218-64224       Fax: (+49) 421-218-98-64224
Email: Rene.Wagner@dfki.de       Web: http://www.dfki.de/sks
--- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
Deutsches Forschungszentrum für Künstliche Intelligenz  GmbH
Firmensitz: Trippstadter Strasse 122, D-67663 Kaiserslautern

Geschäftsführung:
   Prof. Dr. Dr. h.c. mult. Wolfgang Wahlster (Vorsitzender)
                                          Dr. Walter Olthoff
Vorsitzender des Aufsichtsrats:
                                Prof. Dr. h.c. Hans A. Aukes
Amtsgericht Kaiserslautern                          HRB 2313
------------------------------------------------------------

Index: src/slam_gmapping.cpp

===================================================================
--- src/slam_gmapping.cpp    (revision 29585)
+++ src/slam_gmapping.cpp    (working copy)
@@ -378,7 +378,7 @@
   gsp_->setMotionModelParameters(srr_, srt_, str_, stt_);
   gsp_->setUpdateDistances(linearUpdate_, angularUpdate_, resampleThreshold_);
   gsp_->setUpdatePeriod(temporalUpdate_);
-  gsp_->setgenerateMap(false);
+  gsp_->setgenerateMap(true);
   gsp_->GridSlamProcessor::init(particles_, xmin_, ymin_, xmax_, ymax_,
                                 delta_, initialPose);
   gsp_->setllsamplerange(llsamplerange_);
@@ -548,7 +548,7 @@
   matcher.setusableRange(maxUrange_);
   matcher.setgenerateMap(true);

-  GMapping::GridSlamProcessor::Particle best =
+  const GMapping::GridSlamProcessor::Particle &best =
           gsp_->getParticles()[gsp_->getBestParticleIndex()];
   std_msgs::Float64 entropy;
   entropy.data = computePoseEntropy();
@@ -566,6 +566,7 @@
     map_.map.info.origin.orientation.w = 1.0;
   } 

+  /*
   GMapping::Point center;
   center.x=(xmin_ + ymax_) / 2.0;
   center.y=(ymin_ + ymax_) / 2.0;
@@ -591,7 +592,10 @@
     matcher.computeActiveArea(smap, n->pose, &((*n->reading)[0]));
     matcher.registerScan(smap, n->pose, &((*n->reading)[0]));
   }
+  */

+ const GMapping::ScanMatcherMap &smap = best.map;
+
// the map may have expanded, so resize ros message as well
if(map_.map.info.width != (unsigned int) smap.getMapSizeX() || map_.map.info.height != (unsigned int) smap.getMapSizeY()) {