Performing ICP between Pointclouds in C++

edit

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 ...