[Documentation] [TitleIndex] [WordIndex

Package Summary

Enables matching a mesh model file (e.g. STL) to a point cloud using ROS.


The intended use for rail_mesh_icp is to allow RGBD sensing platforms using ROS to match a mesh model (e.g. an STL file) to point clouds. This is useful in cases where an accurate estimate of the model's 6-DoF pose in the point cloud is needed (e.g. grasping/manipulation).

ROS Service Types

ROS Service Types



template_matcher_node matches a model (source) point cloud to a target point cloud using service calls to the icp_matcher_node and an estimate of the object's 6-DoF pose. This 6-DoF pose estimate can come from an external detector via service calls for dynamic objects or predefined in a launch file for static objects. The target point clouds can either be raw point clouds from a sensor_msgs/PointCloud2 topic or pre-processed point clouds included in the service calls.

Subscribed Topics

<~/pcl_topic parameter> (sensor_msgs/PointCloud2)

Published Topics

~/template_points (sensor_msgs/PointCloud2) ~/target_points (sensor_msgs/PointCloud2) ~/matched_template_points (sensor_msgs/PointCloud2)


~/match_template (rail_mesh_icp/TemplateMatch)


~/debug (bool, default: true) ~/visualize (bool, default: true) ~/pcl_topic (string, default: "/head_camera/depth_registered/points") ~/pre_processed_cloud (bool, default: false) ~/template_file (string, default: "corner.pcd") ~/matching_frame (string, default: "/map") ~/initial_estimate_string (string, default: "0 0 0 0 0 0") ~/latch_initial (bool, default: true) ~/template_offset_string (string, default: "0 0 0 0 0 0") ~/template_frame (string, default: "template_pose")


icp_matcher_node matches a template point cloud to a target point cloud using PCL's Iterative Closest Point (ICP) class template pcl::IterativeClosestPoint.


/icp_match_clouds (rail_mesh_icp/ICPMatch)


/icp_matcher_node/iterations (int, default: 50) /icp_matcher_node/max_distance (float, default: 1.0) /icp_matcher_node/trans_epsilon (float, default: 1e-8) /icp_matcher_node/fit_epsilon (float, default: 1e-8)


mesh_sampler_node uniformly samples a polygon mesh in PLY format to create a point cloud of the mesh file. Inputs are the mesh file and outputs are the sampled point cloud. Sampling resolution and number of samples can be changed, use -h when running the node for more information.


To install the rail_mesh_icp package, you can install from source with the following commands:

Making the Mesh (Source) Cloud

  1. After you have mesh model for your object, convert it to a PLY (i.e. .ply) file format so the mesh sampling tool

can read the file format and convert it to a point cloud. In Ubuntu, [Mesh Lab](http://meshlab.sourceforge.net) allows you to import many mesh model formats (e.g. STL, PLY, DAE) and convert them to PLY via the Import Mesh and Export Mesh options.

  1. With the mesh model in PLY format, you can call the mesh sampling tool via `rosrun rail_mesh_icp mesh_sampler_node

[INPUT_MESH_MODEL].ply [OUTPUT_POINT_CLOUD].pcd. Optional arguments, like sampling density, are detailed using -h`.

  1. If the sampling tool was successful, there should now be a .pcd file in the cad_models folder.

Running a demo using the Fetch platform

  1. An example launch file to match the corner.pcd template in the cad_models folder to a static environment object is template_match_demo.launch. It's intended use is with the Fetchit Challenge environment. Please follow tutorials found here to install the environment and robot simulator.

  2. Launch a simulation of the fetch and simulation environment using roslaunch fetchit_challenge main.launch.

  3. Launch the navigation stack to enable localization via roslaunch fetch_navigation build_map.launch.

  4. AFTER launching navigation, teleop the Fetch to face the front corver of the large recangular machine using keyboard teleop via rosrun teleop_twist_keyboard teleop_twist_keyboard.py making sure the Fetch's published point cloud under topic /head_camera/depth_registered/points clearly captures the corner of the machine as shown below:


  1. Start the template matching and icp nodes using the demo launch file via roslaunch rail_mesh_icp template_match_demo.launch. This should launch a version of the generic template matching node (i.e. template_matcher_node) specific to matching the corner template along with an ICP matching node (i.e. icp_matcher_node) automatically.

  2. Verify that the new initial_estimate frame in tf is near the corner of the schunk machine. This initial guess is used to initialize the template_matcher_node for ICP. If the initial_estimate is off by more than 0.1 m and/or 20 degrees along each axis, ICP might not find a registration between the template and viewing point cloud. If the estimate is too far, adjust it in the launch file (i.e. adjust variable initial_estimate inside template_match_demo.launch). This can also be done faster using static_transform_publisher directly.

  3. A service call to detect the corner via rosservice call /template_matcher_demo_node/match_template will start the template matching. Once matched, an estimated frame named template_pose should appear in tf with the x-axis aligned to the center of the cylindrical part of the machine as shown below:


  1. Note that this template pose can be changed arbitrarily with respect to the matched template using the template_offset launch argument. Additionally, if debug is enabled you can view the target, template, and matched template point clouds in rviz by subscribing to target_points, template_points, matched_template_points, respectively.

Matching to other templates (bin, handle, etc.)

  1. To do this, simply create another launch similar to the template_match_demo.launch which is currently set up specifically for the machine matched in the demo. The main changes will be updating initial_estimate, template_offset, provide_processed_cloud, latch_initial_estimate, and template_filename.

  2. Note that for moveable objects in the environment, the initial_estimate can be given along with each ~match_template service call.

2020-01-25 13:02