main.cpp
Go to the documentation of this file.
00001 /*
00002  * main.cpp
00003  *
00004  *  Created on: 11.07.2012
00005  *      Author: ross Kidson
00006  */
00007 
00008 #include <stdio.h>
00009 #include <iostream>
00010 #include <ros/ros.h>
00011 
00012 //local files
00013 #include "rgbd_registration/typedefs.h"
00014 #include "rgbd_registration/rgb_feature_matcher.h"
00015 #include "rgbd_registration/pcl_utils.h"
00016 #include "rgbd_registration/joint_optimize_wrapper.h"
00017 #include "rgbd_registration/parameter_server.h"
00018 
00019 int main (int argc, char** argv)
00020 {
00021   ros::init (argc, argv, "rgbd_registration");
00022   if (!ros::master::check ())
00023   {
00024     ROS_ERROR("roscore not running. stop.");
00025     exit (0);
00026   }
00027 
00028   std::string source_filename, target_filename;
00029   source_filename = ParameterServer::instance ()->get<std::string> ("source_cloud_filename");
00030   target_filename = ParameterServer::instance ()->get<std::string> ("target_cloud_filename");
00031   ROS_INFO_STREAM("[main] Source pointcloud file: " << source_filename);
00032   ROS_INFO_STREAM("[main] Target pointclout file: " << target_filename);
00033 
00034   PointCloudPtr source_cloud_ptr (new PointCloud);
00035   PointCloudPtr target_cloud_ptr (new PointCloud);
00036   pcl::PCDReader reader;
00037   reader.read (source_filename, *source_cloud_ptr);
00038   reader.read (target_filename, *target_cloud_ptr);
00039 
00040   // Extract 2d RGB features, match them between two frames and project them into 3d.  Use Ransac
00041   // to filter out outliers and obtain a transformation between the 2 point clouds
00042   std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >
00043       source_feature_3d_locations, target_feature_3d_locations;
00044   Eigen::Matrix4f ransac_trafo, joint_opt_trafo;
00045   RGBFeatureMatcher point_cloud_matcher (source_cloud_ptr, target_cloud_ptr);
00046   if (!point_cloud_matcher.getMatches (source_feature_3d_locations, target_feature_3d_locations,
00047       ransac_trafo))
00048     exit (0);
00049 
00050   // use the feature points as distinct correspondences in a joint optimization over dense
00051   // clouds and sparse feature points
00052   joint_opt_trafo = performJointOptimization (source_cloud_ptr, target_cloud_ptr,
00053       source_feature_3d_locations, target_feature_3d_locations, ransac_trafo);
00054 
00055   // write the resulting transformed pointcloud to disk
00056   transformAndWriteToFile (source_cloud_ptr, ransac_trafo);
00057 
00058   return 0;
00059 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


rgbd_registration
Author(s): Ross Kidson
autogenerated on Sun Oct 6 2013 12:00:40