Implementing ColorOcTree in OctoMap using RGBDSLAM

edit

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;

      occupiedNodesVis.markers[idx].points.push_back(cubeCenter);
      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;
        occupiedNodesVis.markers[idx].colors.push_back(heightMapColor(h));
      }

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?