is there any way to get the color of each voxel in an Octomap as it is being built in real-time? The package I am looking at, https://github.com/floatlazer/semantic_slam, is a semantic SLAM package which assigns colors to different parts of the Octomap based on what the object is. I would like to extract and use those colors to allow a robot to navigate to a desired type of object.

Is there a way to do this? I saw that Octomap has a conversions.h file, but I am not sure how to use it since I am a bit new to using Octomap.


2 Answers 2


I almost forgot to answer my own question, but here is what I found works. The comments should be helpful in telling what each line does

// use Octomap iterator to find colors, first deserialize
  octomap::AbstractOcTree* tree = octomap_msgs::msgToMap(octomap_msg);
  ROS_INFO("Deserialized Octomap");
  // going from base to derived types (Abstract -> Color)
  octomap::ColorOcTree* octree = dynamic_cast<octomap::ColorOcTree*>(tree);
  ROS_INFO("Dynamic Casted");
  // iterator
  for(octomap::ColorOcTree::leaf_iterator it = octree->begin_leafs(), end = octree->end_leafs(); it!=end; ++it){
    // ROS_INFO("In iterator");
    // get color -> class octomap::ColorOcTreeNode::Color has three fields r, g, b (0 to 255)
    octomap::ColorOcTreeNode::Color currentColor = it->getColor();
    // ROS_INFO("Color extracted");
    // ROS_INFO_STREAM("R is " << currentColor.r << "    G is " << currentColor.g << "    B is " << currentColor.b << '\n');

Sorry for the not-so-useful answer, you can find a semantic_slam issue about your very same question.

Despite it provides no answer, you might try posting there as well. There has been recent activity in the conversation (late 2023), and you have a better chance to come across someone working on your specific case there.

  • $\begingroup$ Thank you for the suggestion! I think I was able to find a way by looking through some similar projects in which they are able to use one of the Octomap iterators (I have yet to actually try it though) to go through individual voxels and extract the data inside $\endgroup$
    – Joshua L
    Commented Feb 15 at 23:36
  • $\begingroup$ I hope this is the solution you were looking for! If you test it successfully, do not forget to post it here as answer. $\endgroup$
    – Luca
    Commented Feb 16 at 8:07

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.