Revision history [back]

I recommend using the octomap_server http://wiki.ros.org/octomap_server. This will filter redundant points by binning them into a 3d occupancy grid. You can watch the results in rViz as the map generates too which is handy for debugging. I have been using it process sonar point clouds from an ROV with decent results.

Octomap has a nice c++ api if you prefer to do things manually. https://github.com/OctoMap/octomap

I included an example launch file to help you out, you will probably need to remap your point cloud topic to cloud_in:

<launch>
    <node name="octomap" pkg="octomap_server" type="octomap_server_node">
        <param name="frame_id" value="/odom"/>
        <param name="resolution" value=".4"/>
    </node>
</launch>

I hope that helps.

I recommend using the octomap_server http://wiki.ros.org/octomap_server. This will filter redundant points by binning them into a 3d occupancy grid. You can watch the results in rViz as the map generates too which is handy for debugging. I have been using it process sonar point clouds from an ROV with decent results.

Octomap has a nice c++ api if you prefer to do things manually. https://github.com/OctoMap/octomap

octmap_server

1. Start the octomap_sever ros node
2. play your rosbag file back http://wiki.ros.org/rosbag/Tutorials/Recording%20and%20playing%20back%20data
3. open rViz to see whats going on. Subscribe to the occupied_cells_vis_array topic.

I included an example launch file to help you out, you will probably need to remap your point cloud topic to cloud_in:

<launch>
    <node name="octomap" pkg="octomap_server" type="octomap_server_node">
        <param name="frame_id" value="/odom"/>
        <param name="resolution" value=".4"/>
    </node>
</launch>

C++

if you want to go the c++ route with rosbag or want to read in a .pcd file you probably want to do something like this. You can then use octovis http://wiki.ros.org/octovis to open the octomap and view it.

#include <octomap/octomap.h>
#include <octomap/OcTree.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_types.h>
#include <pcl/PCLPointCloud2.h>
#include <pcl/conversions.h>


int main(int argc, char *argv[])
{

    ros::init(argc, argv, "pcl2octo");

    octomap::OcTree tree (0.1);   // declare your octomap at 0.1 resolution


    //\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\//\
    // read in your data here either rosbag or pcl
    // http://wiki.ros.org/rosbag/Code%20API#cpp_api
    // http://pointclouds.org/documentation/tutorials/pcd_file_format.php
    //\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/

    for(/* enter conditoins here */){  // crate a loop for each sensor measurement taken

        pcl::PointCloud<pcl::PointXYZ> pt; // populate this with the point cloud from one sensor ping
        octomap::point3d sensorOrigin(x,y,z); // where x,y,z are the location of the sensor for that measurement

        for (pcl::PointCloud<pcl::PointXYZ>::const_iterator it = pt.begin(); it != pt.end(); ++it){
          cloud.push_back(it->x, it->y, it->z);
        }

        tree.insertPointCloud(cloud,sensorOrigin);
    }
    tree.writeBinary("simple_tree.bt");

    return 0;


}

I hope that helps.