How to correctly build an OctoMap using a series of registered point clouds

edit

Hi Guys, I hope you can help me with this one, i'm quite new with ROS/PCL/OctoMap. It just look like a long question, but it is really simple question... I just give some extra details. So I have a RGBD recording of a camera mounted on a robot, traversing through a corridor inside a building, and I want to build from the RGBD point-clouds I record an OctoMap of the traversed path. In order to do so, I have built with PCL a point cloud registration pipeline. Now, I want to use this pipeline in order to build an OctoMap. Here is what I tried to do:

Iterate through the point-cloud array of the recorded point-clouds, and for each adjacent point clouds, do the following:

 1. Find the registration transformation matrix (using my pipeline) between the two adjacent point clouds, where point cloud X is used as the source point cloud for registration, and point cloud X-1 is used as the target point cloud for registration.
 2. Transform the source point cloud using the transformation matrix from the last step.
 3. Add the transformed source point cloud to octomap.

However, it doesn't work, I suspect that I am doing something wrong in step 3. here an example source code of what I am trying to do:

octomap::OcTree octree_map(0.05);
for(int i = start; i < end; i += 1)
{
    // PART 1 - Process the source point cloud (noise removal, down sampling, etc...)
    point_cloud_processing_pipline_source = inference_factory.createPointCloudProcessingPipeline(inference_configuration_file, point_cloud_processing_pipeline_configuration_file);
    point_cloud_processing_pipline_source->setInputCloud(pointCloud2ConstPtrVector[i]);

    // PART 2 - Process the target point cloud (noise removal, down sampling, etc...)
    point_cloud_processing_pipline_target = inference_factory.createPointCloudProcessingPipeline(inference_configuration_file, point_cloud_processing_pipeline_configuration_file);
    point_cloud_processing_pipline_target->setInputCloud(pointCloud2ConstPtrVector[i - 1]);

    // PART 3 - Create point cloud registration pipeline
    auto point_cloud_registration_pipeline = inference_factory.createPointCloudRegistrationPipeline(inference_configuration_file, point_cloud_registration_pipeline_configuration_file);

    // PART 4 - Set the source and target point clouds of the registration pipeline
    point_cloud_registration_pipeline->setSourcePointCloudProcessingPipeline(point_cloud_processing_pipline_source);
    point_cloud_registration_pipeline->setTargetPointCloudProcessingPipeline(point_cloud_processing_pipline_target);

    // PART 5 - Find the trasformation matrix that transforms source into target (matrix type is Eigen::Matrix4f)
    auto transformation_matrix = point_cloud_registration_pipeline->estimateTransformation();

    // PART 6 - Transform the source point cloud with the registration's transformation matrix
    auto point_cloud_2 = boost::make_shared<sensor_msgs::PointCloud2>();
    auto source_point_cloud_transformed = boost::make_shared<pcl::PointCloud<PointType>>();
    pcl::transformPointCloud (*point_cloud_processing_pipline_source->getPoints(), *source_point_cloud_transformed, transformation_matrix);
    pcl::toROSMsg(*source_point_cloud_transformed, *point_cloud_2); 

    // PART 7 - Add the transformed source point cloud to octomap
    octomap::point3d sensor_origin = octomap::point3d(0, 0, 0);
    double maxrange = 30;
    bool lazy_eval = false;
    bool discretize = false;
    octomap::Pointcloud octomap_scan;
    octomap::pointCloud2ToOctomap(*point_cloud_2, octomap_scan);
    octree_map.insertPointCloud(octomap_scan, sensor_origin, maxrange, lazy_eval, discretize);
    octree_map.write("test.ot");
}

There is no need to understand the full code, let's just assume that the registration matrix transformation_matrix that I get from my registration pipeline is correct, and let's concentrate only on PART 7 in the code. Am I adding the transformed source point cloud correctly to octomap? Am I passing all the other parameters correctly? For some reason It looks like that the octomap I get (when i visualize it with octovis), has all the point clouds I added placed at the same point (it looks like they are ... (more)