Ask Your Question
2

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

asked 2017-01-22 07:29:38 -0500

xenomarz gravatar image

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)

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2017-03-23 06:48:14 -0500

LostInTheWoods gravatar image

updated 2018-04-05 00:17:34 -0500

jayess gravatar image

Hi, might be a bit too late for a reply but if that's the case I'll leave this comment for future reference. I've been working a bit with this combo (PCL+ROS+Octomap) and it really isn't intuitive some of the functions.

Your problem seems to originate in that you transform the point clouds by the transform between two sequential point clouds (e.g T: PC[k]->PC[k+1]). If you are going to add them in the way you uses now you'll have to calculate the absolute transform from the first point cloud to the current (e.g recursively T[k+1] = T_(k+1|k) * T[k], where T_(k+1|k) is the transform you currently calculate.

Alternative approach: You can also skip the transformation step in PCL and just calculate the absolute transform, this can be used in the following function call.

void octomap::OccupancyOcTreeBase< NODE >::insertPointCloud(const Pointcloud & scan,
        const point3d & sensor_origin, const pose6d & frame_origin,
        double maxrange, bool lazy_eval, bool discretize)

Note that the sensor origin actually is set to (0,0,0) in most applications, unsure why though..

One more note is that the maxrange sets how far from the sensor origin the nodes can be added (e.g if you use this in a SLAM system, the biggest map you can get will be a sphere with 30m radius.

Hope the explanation makes sense, if anyone has a clearer grasp of the purpose of sensor origin and maxrange please add a comment.

edit flag offensive delete link more

Your Answer

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

Add Answer

Question Tools

2 followers

Stats

Asked: 2017-01-22 07:29:38 -0500

Seen: 1,017 times

Last updated: Apr 05 '18