Friday, October 25, 2013

Let's integrate pcl17 with ROS Fuerte. Using Point Cloud Libary to perform object recognition

We are going to add 3D object recognition to our robot so I have been trying to implement PCL 1.7 on ubuntu 12.04 these two weeks, where the ROS version is fuerte. As we know, even we only want to have a small function in ROS, we need to look into a lot of documents and it is a pity that there is very few step by step tutorial. I simply record what I have done for the past two weeks for my own reference and I hope it can help some one who want to do the same thing.

If you are using Groovy or Hydro, this article may not be helpful on some technical details. However, the pipeline should be the same.

I. PCL17 

Currently PCL is independent from ROS. And the original ros wiki http://wiki.ros.org/pcl  does not provide up-to-date info for PCL installation. We need to refer to wiki.ros.org/pcl17 and you can get basic ideas how to do that through reading the following two posts.

http://answers.ros.org/question/58409/problems-with-repository-pcl17/
http://answers.ros.org/question/44821/updating-to-pcl-17-in-ros-fuerte/

For me, I have downloaded pcl17, but for pcl_ros I am using the original one with fuerte. The package pcl_ros provides functions to convert different data types between PCL and ROS and transformation for point cloud.

It takes quite a looooooong time to compile pcl17. Over 3000s on my Thinkpad T410. So please be patient or just AFK..

II. pcl/Tutorial

Though the compilation of the pcl17 itself is not a problem, it is not that straight forward to include PCL in your own ROS code. Firstly, we try to go through the pcl/Tutorial.  This is a very important tutorial and you should read the whole page line by line and try not to miss a single word!

http://wiki.ros.org/pcl/Tutorials

This section is just following and explaining the ros tutorial,  solving the problems during compilation.

1.Code skeleton

I am not familiar with catkin so that I am using rosbuild. Because in this version of PCL, the used namespace is pcl17. That means we need to change all the pcl:: to pcl17::, and use #include<pcl17/***> instead of #include<pcl/***>. Also, remember to change the manifest.xml

<depend package="pcl17"/>

I stick to the dependency on pcl_ros package because I am not using pcl17_ros.

<depend package="pcl_ros"/> 

You should be able to compile the example.cpp successfully. 

2.sensor_msgs/PointCloud2

Proceed on to ros pcl tutorial section 4.1, simply add the 5 lines of codes into function cloud_cb(), pcl::VoxelGrid . Now you have a function which can down sample the point cloud.

rosrun my_pcl_tutorial example input:= <PointCloud2>
 
The input must be a PointCloud2 type. If you have an RGB-D device and openni installed, you can try

/camera/depth/points
/camera/depth_registered/points 

The results can be visualized in rviz.
But, if you do not have an RGB-D device at present? You can download some .pcd file, the extension means point cloud document.  For example

http://svn.pointclouds.org/data/tutorials/correspondence_grouping/milk_cartoon_all_small_clorox.pcd

After you have the .pcd file, we can use the function in pcl_ros to publish a point cloud. Refer to http://wiki.ros.org/pcl_ros 4.4.3 Usage. For example

rosrun pcl_ros pcd_to_pointcloud milk_cartoon_all_small_clorox.pcd 0.5

So the pcd_to_pointcloud function can read the .pcd file and publish it as point cloud every o.5 sec. The topic is

/cloud_pcd

Finally, using

rosrun my_pcl_tutorial example input:=/cloud_pcd

Now we can see some results in rviz.

3. pcl/PointCloud<T>

I did not really compile and execute this tutorial.  But the data type conversion mentioned here is very important. The cloud using in ROS is sensor_msgs::PointCloud2, but in PCL it is pcl17::PointCloud. We need to perform conversions when needed. The functions are fromROSMsg and toROSMsg, can be checked in

http://docs.pointclouds.org/1.0.0/namespacepcl.html

Pay attention to the format whether your input argument is a pointer or not? We will give detailed examples in the next section.

III. Correspondence Grouping 

After trying out some examples, now we are going to integrate the object recognition with ROS. Please refer to the following link. Now let's try to realize the function in ROS.

http://pointclouds.org/documentation/tutorials/correspondence_grouping.php#correspondence-grouping

1. Code Modification

Read through the PCL tutorial, get familiar with the functions. As we already have the ROS skeleton for PCL, what we need to do is to embed the main() function into cloud_cb(). It is not necessary to have showHelp() and parseCommandLine(). We simply modify the parameters in the program. If you want use command line to control the parameters, ROS provides powerful tools to do that..

Similarly, change all the pcl:: to pcl17::. Pay attention to PCL_VISUALIZER_POINT_SIZE, this parameter in visualization:: class need to change to PCL17_VISUALIZER_POINT_SIZE.

To use the .pcd file provided by the tutorial, you can simply read the .pcd file, or using pcl_ros::fromROSMsg, take in the parameters are arguments for cloud_cb(). Here, we read the model from the file, and takes in the scene from ROS topic. In this way, it is easier to change the scene from .pcd file.

To read the .pcd file, we can use the loadPCDFile() provided by PCL. However, it seems that the loaded point cloud does not contain the transformation/reference information, we need to define it in either camera_link or base_link. Or else it cannot be shown in rviz because rviz cannot perform a transformation.
// The model is loaded from pcd file, the scene is directly obtained from the camera, rostopic, which is an input of the cloud_cb.
  std::string   model_path=ros::package::getPath("my_pcl_tutorial");
  model_path+="/pcd/milk.pcd";
  if (pcl17::io::loadPCDFile (model_path, *model) < 0)
  {
    std::cout << "Error loading model cloud." << std::endl;
    return;
  }

  model->header.frame_id= "camera_link";
  model->header.stamp = ros::Time::now ();


for the scene cloud
void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
  pcl17::fromROSMsg (*input, *scene);
}

 And we publish the rotated_model
  pcl17::toROSMsg(*rotated_model,cloud_filtered);
  pub.publish (cloud_filtered);


If you are still not clear how to modify the code, leave me a message with your email.

Finally, we are not going to use the visualization module. Because the visualization will spin itself and the call back function stops there...Instead, we need to publish the topic and visualize the results in rviz. So, we can comment out all the visualization:: class related code. The specific lines which need to be commented are listed below.

  //pcl17::visualization::PCLVisualizer viewer ("Correspondence Grouping");
  //viewer.addPointCloud (scene, "scene_cloud");


    //viewer.addPointCloud (off_scene_model, off_scene_model_color_handler, "off_scene_model");

    //viewer.addPointCloud (scene_keypoints, scene_keypoints_color_handler, "scene_keypoints");
    //viewer.setPointCloudRenderingProperties (pcl17::visualization::PCL17_VISUALIZER_POINT_SIZE, 5, "scene_keypoints");


    //viewer.addPointCloud (off_scene_model_keypoints, off_scene_model_keypoints_color_handler, "off_scene_model_keypoints");
    //viewer.setPointCloudRenderingProperties (pcl17::visualization::PCL17_VISUALIZER_POINT_SIZE, 5, "off_scene_model_keypoints");


    //viewer.addPointCloud (rotated_model, rotated_model_color_handler, ss_cloud.str ());

        //viewer.addLine<PointType, PointType> (model_point, scene_point, 0, 255, 0, ss_line.str ());

//  while (!viewer.wasStopped ())
//  {
//    viewer.spinOnce ();
//  }

2. Compilation 

The executable for our program is correspondence_grouping. Here several errors are listed. However, some errors may not be a problem if you have already changed your code according to my suggestion.

fatal error: vtkMatrix4x4.h: No such file or directory
Add in two lines into CMakelists.txt

include_directories(/usr/include/vtk-5.8)
target_link_libraries(correspondence_grouping ${PCL_LIBRARIES} libvtkCommon.so libvtkFiltering.so libvtkRendering.so)

Please refer to http://answers.ros.org/question/39444/pcl-visualizer-error/?answer=39744#post-id-39744


libboost error

/usr/bin/ld: note: 'boost::system::system_category()' is defined in DSO /usr/lib/libboost_system.so.1.46.1 so try adding it to the linker command line
  /usr/lib/libboost_system.so.1.46.1: could not read symbols: Invalid operation


You need to add in one line into CMakelist.txt

rosbuild_link_boost(correspondence_grouping system)

PCL_VISUALIZER_POINT_SIZE not declared in common.h
This is introduced in the previous section. Change it to PCL17_VISUALIZER_POINT_SIZE
Please refer to
http://www.pcl-users.org/Correspondence-Grouping-Tutorial-ROS-td4021994.html

3. Show results 

Hopefully you can compile you code successfully. 

First, publish the point cloud from a .pcd file. Pay attention to the file path

rosrun pcl_ros pcd_to_pointcloud pcd/milk_cartoon_all_small_clorox.pcd 0.2

So the reference for the cloud is by default /base_link.

rosrun my_pcl_tutorial correspondence_grouping input:=/cloud_pcd

And now we can have a look at the results in rviz.


13 comments:

  1. Hi, I can't understand how to modify the code from your tutorial, could you send me your edited codes or guide me along please? My email is liewchaochien@hotmail.com. Thanks.

    ReplyDelete
    Replies
    1. btw, this is the error message i get without editing the codes

      /home/chao/abc/src/wall_turning/src/correspondence_grouping.cpp: In function ‘int main(int, char**)’:
      /home/chao/abc/src/wall_turning/src/correspondence_grouping.cpp:297:76: error: cannot declare variable ‘rf_est’ to be of abstract type ‘pcl::BOARDLocalReferenceFrameEstimation’
      /usr/include/pcl-1.7/pcl/features/board.h:59:9: note: because the following virtual functions are pure within ‘pcl::BOARDLocalReferenceFrameEstimation’:
      /opt/ros/groovy/include/pcl-1.6/pcl/features/feature.h:309:7: note: void pcl::Feature::computeFeatureEigen(pcl::PointCloud >&) [with PointInT = pcl::PointXYZRGBA, PointOutT = pcl::ReferenceFrame]
      make[2]: *** [wall_turning/CMakeFiles/correspondence_grouping.dir/src/correspondence_grouping.cpp.o] Error 1
      make[1]: *** [wall_turning/CMakeFiles/correspondence_grouping.dir/all] Error 2
      make: *** [all] Error 2

      Delete
    2. You need to change it to
      pcl17::BOARDLocalReferenceFrameEstimation rf_est;

      I have sent a copy to you :)

      Delete
    3. This comment has been removed by the author.

      Delete
  2. May I have the copy of the code too? thank
    email: soliderdare2013@gmail.com

    ReplyDelete
    Replies
    1. This comment has been removed by the author.

      Delete
  3. Can I have a copy? thanks a lot
    Hussein-mahmoud@live.com

    ReplyDelete
  4. This comment has been removed by the author.

    ReplyDelete
  5. This comment has been removed by the author.

    ReplyDelete
  6. Hello can i also have that code? i tried to do it my self but became tired but couldn't exactly run it.
    My email: dinesh99981@gmail.com
    Thank's for helping us.

    ReplyDelete
  7. Hi! thanks for the post. i implemented the above and it compiles without errors, but i keep getting segmentation fault (core dump) error during runtime. Particularly on these two lines of code:
    rf_est.compute (*model_rf);
    clusterer.recognize (rototranslations, clustered_corrs);

    Do you have any idea about this issue?
    Thanks in advance

    ReplyDelete
    Replies
    1. It has really been a very long time.. sorry that I cannot give any help ..

      Delete