Extract color from Octomap

Question

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.

Answer 1

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');
```

Answer 2

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.