Ask Your Question

Implementing ColorOcTree in OctoMap using RGBDSLAM

asked 2014-06-10 13:54:24 -0500

xuningy gravatar image

updated 2014-06-11 08:34:48 -0500

I am implementing a ColorOcTree in OctoMap. Since the default in OctoMap is to build a heightmap, given by this piece of code in octomap_server/src/OctomapServer.cpp in OctomapServer::publishAll(const ros::Time& rostime):

        //create marker:
    if (publishMarkerArray){
      unsigned idx = it.getDepth();
      assert(idx < occupiedNodesVis.markers.size());

      geometry_msgs::Point cubeCenter;
      cubeCenter.x = x;
      cubeCenter.y = y;
      cubeCenter.z = z;

      if (m_useHeightMap){
        double minX, minY, minZ, maxX, maxY, maxZ;
        m_octree->getMetricMin(minX, minY, minZ);
        m_octree->getMetricMax(maxX, maxY, maxZ);

        double h = (1.0 - std::min(std::max((cubeCenter.z-minZ)/ (maxZ - minZ), 0.0), 1.0)) *m_colorFactor;

I am seeking to change the height map into a per-voxel color Octomap.

From what is stated in the paper for octomap, I can use the child class of OcTree (ColorOcTree). From what I understand, ColorOcTree contains all the methods for OcTree, plus the average color information. So, To get a colored Octomap, rewrite OctomapServer.cpp from a OcTree based structure to an ColorOcTree structure.

My question is, in order to do this in OctomapServer.cpp, is this the correct method or is there some better way to construct per-voxel colored octomap?

EDIT: I finished changing OctomapServer.cpp and associated nodes to ColorOcTree, however I notice that the color information isn't inputted from the point cloud, as per this code in OctomapServer.cpp

It seems like it is converting a PointCloud2 cloud into a PointCloud.

void OctomapServer::insertCloudCallback(const sensor_msgs::PointCloud2::ConstPtr& cloud){
  ros::WallTime startTime = ros::WallTime::now();
  // ground filtering in base frame
  PCLPointCloud pc; // input cloud for filtering and ground-detection
  pcl::fromROSMsg(*cloud, pc);

The second question becomes, I need to extract RGB data from either PointCloud or PointCloud2, and have it in a float32 representation. Do I use PointXYZRGB instead of PointXYZ in reading the PointCloud (If so how?) Or is there some better way to do it?

EDIT2: I did change PointXYZ to PointXYZRGB and got the associated RGB Data. however, after saving that data into the ColorOcTree and trying to send the ColorRGBA message (also in float32, I did so by dividing each one of the RGB data by 255.0) to the Occupied_cells_vis_array, I get blue color only. Can anyone explain why it is only outputting blue voxels?

edit retag flag offensive close merge delete


Isn't there a version of OctomapServer for colored maps already in RGBD-SLAM?

AHornung gravatar image AHornung  ( 2014-06-11 09:35:04 -0500 )edit

OctomapServer is publishing blue voxels as visualization for occupied nodes by default, unless you enable the parameter for the height map coloring. Only then it will set a different color for each voxel.

AHornung gravatar image AHornung  ( 2014-06-11 09:36:30 -0500 )edit

Hi Armin, There is a OctomapServer for colored maps? I could not find it in the rgbdslam_freiburg directory. Can you be more specific? I realized your second comment is true, but in order to override it, is there any way? I already have extracted data from PC and i believe it is in the tree. How would I use it if I choose to modify OctomapServer.cpp? Thanks!

xuningy gravatar image xuningy  ( 2014-06-11 09:55:14 -0500 )edit

The line in RGBDSLAM package refers me to this repository, which is not the default repository in octomap_server (nor does the ros stack contains the color octomap nodes at all). Do you advise downloading this and using this package instead : This is a deprecated package.

xuningy gravatar image xuningy  ( 2014-06-11 10:01:02 -0500 )edit
AHornung gravatar image AHornung  ( 2014-06-12 02:47:45 -0500 )edit

That code in the branches at alufr-ros-pkg is definitely outdated though.

AHornung gravatar image AHornung  ( 2014-06-12 02:48:23 -0500 )edit

Thanks Armin! I just ended up rewriting OctomapServer overall. Still need to address deprecated packages...

xuningy gravatar image xuningy  ( 2014-06-13 11:08:25 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted

answered 2014-06-11 16:52:59 -0500

xuningy gravatar image

The repository octomap_mapping_color is mostly deprecated, thus the color octomap server wasn't usable in recent version of OctoMap. Wrote my own ColorOctomapServer.cpp and everything works.

edit flag offensive delete link more


Hello I am searching for color Octomap code in ros hydro, you have any suggestions where I can find such code? Regards

Amine gravatar image Amine  ( 2015-02-04 08:05:39 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower


Asked: 2014-06-10 13:54:24 -0500

Seen: 1,169 times

Last updated: Jun 11 '14