Ask Your Question
0

Performing ICP between Pointclouds in C++

asked 2013-04-09 07:43:52 -0500

mdegges gravatar image

updated 2014-01-28 17:16:07 -0500

ngrennan gravatar image

Hi-

I'm trying to create a script that takes in 2 bag files (fixed_frame and camera_frame), converts the ROS sensor_msgs/PointCloud2 data to pcl/PointCloud data, performs ICP between the pointcloud in fixed_frame and all the pointclouds in camera_frame, and publishes the results. Here is my code so far:

// Includes listed here

ros::Publisher _pub;
pcl::PointCloud<pcl::PointXYZ> cloud_in; 

void fixed_frame_cb (const sensor_msgs::PointCloud2ConstPtr& pc2_msg) {

  // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(
                           new pcl::PointCloud<pcl::PointXYZ>
                           );
  pcl::fromROSMsg ( *pc2_msg, *cloud_in );


}

void camera_frame_cb (const sensor_msgs::PointCloud2ConstPtr& next_pc2_msg) {

  // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2_in(
                           new pcl::PointCloud<pcl::PointXYZ>
                           );
  pcl::fromROSMsg ( *next_pc2_msg, *cloud2_in );

  // Perform ICP
  pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
  icp.setInputCloud(cloud2_in);
  icp.setInputTarget(cloud_in);
  pcl::PointCloud<pcl::PointXYZ> Final;
  icp.align(Final);

  // Convert the pcl/PointCloud to sensor_msgs/PointCloud2
  sensor_msgs::PointCloud2 output;
  pcl::toROSMsg( *cloud2_in, output );
  // Publish the results
  _pub.publish( output );

}

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

  // Initialize ROS
  ros::init (argc, argv, "my_script");
  ros::NodeHandle n( "~" );

  // Create ROS subscriber for fixed_frame topic
  ros::Subscriber sub = n.subscribe(
                    "/sr/pointcloud2/raw",
                    1,
                    fixed_frame_cb
                    );

  // Create ROS subscriber for camera_frame topic
  ros::Subscriber sub2 = n.subscribe(
                     "/sr/pointcloud2/raw",
                     1,
                     camera_frame_cb
                     );

  // Create ROS publisher for transformed pointcloud
  _pub = n.advertise<sensor_msgs::PointCloud2>(
                           "/iml_transform_point_cloud/transformed_point_cloud",
                           1
                           );

  // Spin
  ros::spin ();
}

I'm getting a lot of errors when I run 'make'. New to ros and c++, so I think these might just be syntax problems. Can anyone point me in the right direction? Thanks in advance.

Make errors:

mdegges@ubuntu:~/ros_workspace/iml_ros/my_script$ make mkdir -p bin cd build && cmake -Wdev -DCMAKE_TOOLCHAIN_FILE=/opt/ros/groovy/share/ros/core/rosbuild/rostoolchain.cmake .. [rosbuild] Building package my_script -- Using CATKIN_DEVEL_PREFIX: /home/mdegges/ros_workspace/iml_ros/my_script/build/devel -- Using CMAKE_PREFIX_PATH: /opt/ros/groovy -- This workspace overlays: /opt/ros/groovy -- Found gtest sources under '/usr/src/gtest': gtests will be built -- catkin 0.5.63 [rosbuild] Including /opt/ros/groovy/share/roscpp/rosbuild/roscpp.cmake [rosbuild] Including /opt/ros/groovy/share/rospy/rosbuild/rospy.cmake -- Configuring done -- Generating done CMake Warning: Manually-specified variables were not used by the project:

CMAKE_TOOLCHAIN_FILE

-- Build files have been written to: /home/mdegges/ros_workspace/iml_ros/my_script/build cd build && make make[1]: Entering directory /home/mdegges/ros_workspace/iml_ros/my_script/build' make[2]: Entering directory /home/mdegges/ros_workspace/iml_ros/my_script/build' make[3]: Entering directory /home/mdegges/ros_workspace/iml_ros/my_script/build' make[3]: Leaving directory /home/mdegges/ros_workspace/iml_ros/my_script/build' [ 0%] Built target rospack_genmsg_libexe make[3]: Entering directory /home/mdegges/ros_workspace/iml_ros/my_script/build' make[3]: Leaving directory /home/mdegges/ros_workspace/iml_ros/my_script/build' [ 0%] Built target rosbuild_precompile make[3]: Entering directory /home/mdegges/ros_workspace/iml_ros/my_script/build' make[3]: Leaving directory /home/mdegges/ros_workspace/iml_ros/my_script/build' make[3]: Entering directory `/home/mdegges/ros_workspace/iml_ros/my_script/build' [100%] Building CXX object CMakeFiles/my_script.dir/src/example.cpp.o /home/mdegges/ros_workspace/iml_ros/my_script/src/example.cpp: In function ‘void camera_frame_cb(const PointCloud2ConstPtr ...

(more)
edit retag flag offensive close merge delete

Comments

Please edit your question with the errors you're seeing. It is very hard to help you without seeing what the problem is.

Dan Lazewatsky gravatar imageDan Lazewatsky ( 2013-04-09 08:28:38 -0500 )edit

Ok- I posted the make errors on pastie: http://pastie.org/7397787

mdegges gravatar imagemdegges ( 2013-04-09 15:57:06 -0500 )edit

cannot open the page on pastie. why don't you just paste the error here? and you should also provide you CMakeLists.txt file

yangyangcv gravatar imageyangyangcv ( 2013-04-09 16:00:13 -0500 )edit

Sorry- edited the question with the errors included. The CMakeLists.txt file is the default, except I added a line at the bottom to make the script executable.

mdegges gravatar imagemdegges ( 2013-04-09 18:58:21 -0500 )edit

1 Answer

Sort by » oldest newest most voted
0

answered 2013-04-11 12:29:00 -0500

mdegges gravatar image

Figured it out. I just had some syntax errors- for ex, when I defined the global variable 'cloud_in', I forgot to make it a pointer.

edit flag offensive delete link more

Comments

How did you get the two .bag files into your program? I tried rosbag play -l 2017-06-07-13-49-48.bag 2017-06-07-14-05-40.bag /camera/depth/points:=icp/input/raw But that doesn't work.

jrgen gravatar imagejrgen ( 2017-09-29 03:27:11 -0500 )edit

Your Answer

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

Add Answer

Question Tools

1 follower

Stats

Asked: 2013-04-09 07:43:52 -0500

Seen: 1,388 times

Last updated: Apr 09 '13