[Personalrobots-commit] SF.net SVN: personalrobots:[9204] pkg/trunk/mapping/collision_map/src/ collision_map.cpp

[<ve...@us...>]

Revision: 9204
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9204&view=rev
Author:   veedee
Date:     2009-01-11 10:47:25 +0000 (Sun, 11 Jan 2009)

Log Message:
-----------
fix

Modified Paths:
--------------
    pkg/trunk/mapping/collision_map/src/collision_map.cpp

Modified: pkg/trunk/mapping/collision_map/src/collision_map.cpp
===================================================================
--- pkg/trunk/mapping/collision_map/src/collision_map.cpp	2009-01-11 10:45:41 UTC (rev 9203)
+++ pkg/trunk/mapping/collision_map/src/collision_map.cpp	2009-01-11 10:47:25 UTC (rev 9204)
@@ -144,6 +144,9 @@
       gettimeofday (&t1, NULL);
 
       // Get a set of point indices that respect our bounding limits around the robot
+      vector<int> indices (cloud_.pts.size ());
+      int nr_p = 0;
+
       Point32 minP, maxP;
       minP.x = minP.y = minP.z = FLT_MAX;
       maxP.x = maxP.y = maxP.z = FLT_MIN;
@@ -165,8 +168,11 @@
           maxP.x = (cloud_.pts[i].x > maxP.x) ? cloud_.pts[i].x : maxP.x;
           maxP.y = (cloud_.pts[i].y > maxP.y) ? cloud_.pts[i].y : maxP.y;
           maxP.z = (cloud_.pts[i].z > maxP.z) ? cloud_.pts[i].z : maxP.z;
+          indices[nr_p] = i;
+          nr_p++;
         }
       }
+      indices.resize (nr_p);
 
       // Compute the minimum and maximum bounding box values
       Point32 minB, maxB, divB;
@@ -197,11 +203,11 @@
       }
 
       // First pass: go over all points and count them into the right leaf
-      for (unsigned int cp = 0; cp < cloud_.pts.size (); cp++)
+      for (unsigned int cp = 0; cp < indices.size (); cp++)
       {
-        int i = (int)(floor (cloud_.pts[cp].x / leaf_width_.x));
-        int j = (int)(floor (cloud_.pts[cp].y / leaf_width_.y));
-        int k = (int)(floor (cloud_.pts[cp].z / leaf_width_.z));
+        int i = (int)(floor (cloud_.pts[indices.at (cp)].x / leaf_width_.x));
+        int j = (int)(floor (cloud_.pts[indices.at (cp)].y / leaf_width_.y));
+        int k = (int)(floor (cloud_.pts[indices.at (cp)].z / leaf_width_.z));
 
         int idx = ( (k - minB.z) * divB.y * divB.x ) + ( (j - minB.y) * divB.x ) + (i - minB.x);
         leaves_[idx].i_ = i;


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.