Octomap with color

[Ivan Dryanovski]

Hello, 

I'm trying to find out how to add color to Octomaps I produce. My input data are dense pointclouds from an RGB-D camera.

I found the following bit of code allowing me to modify the color of a node:

  point3d endpoint ((float) x*0.05f+0.01f, (float) y*0.05f+0.01f, (float) z*0.05f+0.01f);

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

  n->setColor(z*5+100,x*5+100,y*5+100); // set color to red

 

However, as far as I understand, inserting end points one by one is not the same as inserting an entire scan. Doesn't inserting the entire scan perform special checks to avoid the free space aliasing issues at low incidence angles? 

Is there a method similar to insertScan which allows me to attach color, and if not, can I recreate the insertScan functionality for colored clouds?

Thank you for the help,

Ivan

[Ivan Dryanovski]

Well I figured out a simple way, although probably not the most elegant solution.

1) Insert entire pcl cloud using tree.insertScan

2) Go through all the input pcl cloud.points:

  - find the corresponding node in the tree using tree.search

  - set the color to the node

Cheers,

Ivan

PS. I love the color mode! Thanks Octomap team  

[Armin Hornung]

Hi Ivan,

On 2013-02-15 21:50, Ivan Dryanovski wrote:
> Well I figured out a simple way, although probably not the most
> elegant solution.
>
> 1) Insert entire pcl cloud using tree.insertScan
> 2) Go through all the input pcl cloud.points:
> - find the corresponding node in the tree using tree.search
> - set the color to the node

If you are using ColorOcTree::integrateNodeColor(...) and
ColorOcTree::updateInnerOccupancy() (if needed), then I think that's
the way. Maybe Kai or Felix have another insight how it's done in
RGBD-Slam.

To make it a bit more efficient, you could call computeUpdate(...) in
your implementation, and then updateNode(...) like it's done in
insertScan(...), instead of directly calling insertScan(..). You can
then directly iterate through the updated KeySet for color updates,
saving you the second run through the point cloud and searching for the
key again.


Cheers,
Armin

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

[Armin Hornung]

On 2013-02-18 19:28, Ivan Dryanovski wrote:
> Hi Armin,
>
> Thanks for the reply! I was wondering, which part of the code handles
> the aliasing issues at low angles, when inserting the entire point
> cloud? That seems to be the important piece I want to make sure i use.
That's directly in "computeUpdate", towards the end ("prefer occupied
cells over free ones").


>
> I am doing my mapping offline at the moment, so I'm not worried about
> the efficiency so much (for now). Using the method I posted above, I
> was able to get pretty good results, and I integrated the Octomap
> export with the ccny_rgbd map server. I posted some pictures on the
> ccny_rgbd wiki page [1] if you are interested - it looks like our lab
> built out of Lego blocks, which is pretty awesome.
Wow, that looks really cool!

Cheers,
Armin



>
> Cheers,
>
> Ivan
>
> [1] http://www.ros.org/wiki/ccny_rgbd/keyframe_mapper
>
> --
> Ivan Dryanovski
> The Graduate Center, City University of New York (CUNY)
> CCNY Robotics Lab
> http://robotics.ccny.cuny.edu/People/Dryanovski