Octomap for Dynamic scenes

[Sunando Sengupta]

Hi,

I am trying to use octomap to model dynamic scenes. I have created a tree and use stereo images to build the tree. I generate 3D points and insert it in the tree, and continue for next pair. However, for moving objects I an getting a trail. I guess once I have updated the voxel as occupied, it remains. I am not able to modify the log-odds to show it was unoccupied after the object has moved. My code snippet is

ColorOcTree treeCol (res);

for all scans

{

   for all points in the current_scan

   {

      point3d endpoint ((float) (P.at<float>(0)), -(float) (P.at<float>(1)), (float) (P.at<float>(2))); 

      ColorOcTreeNode* n = treeCol.updateNode(endpoint, true); 

      n->setColor(r,g,b);

   }

   treeCol.updateInnerOccupancy();

}

I tried to change the  setProbHit(hit) and setProbMiss(miss) but was not able to solve it.

I think I might be missing something. Any help will be very much appreciated. 

Thanks a Lot,

Sunando

[Armin Hornung]

On 2013-04-06 09:36, Sunando Sengupta wrote:
> Hi,
>
> I am trying to use octomap to model dynamic scenes. I have created a
> tree and use stereo images to build the tree. I generate 3D points and
> insert it in the tree, and continue for next pair. However, for moving
> objects I an getting a trail. I guess once I have updated the voxel as
> occupied, it remains. I am not able to modify the log-odds to show it
> was unoccupied after the object has moved. My code snippet is
>
> ColorOcTree treeCol (res);
>
> for all scans
> {
> for all points in the current_scan
> {
> point3d endpoint ((float) (P.at<float>(0)), -(float)
> (P.at<float>(1)), (float) (P.at<float>(2)));
> ColorOcTreeNode* n = treeCol.updateNode(endpoint, true);
> n->setColor(r,g,b);
> }
>
> treeCol.updateInnerOccupancy();
> }
>

You are only updating a single node corresponding to the endpoint. If
you don't clear nodes along the ray to the endpoint, you won't be able
to remove any occupied nodes.

So, just use the functions insertScan (insertPointCloud in 1.6) instead,
or insertRay (preferably the first). If you don't want to implement it
yourself for your octree, use them from OccupancyOcTreeBase and then
loop through all your endpoints again to set the color.

Best regards,
Armin

--
Armin Hornung
Humanoid Robots Lab, Albert-Ludwigs-Universit�t Freiburg
Contact: http://www.informatik.uni-freiburg.de/~hornunga