Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008 #include <stdio.h>
00009 #include <iostream>
00010 #include <ros/ros.h>
00011
00012
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
00041
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
00051
00052 joint_opt_trafo = performJointOptimization (source_cloud_ptr, target_cloud_ptr,
00053 source_feature_3d_locations, target_feature_3d_locations, ransac_trafo);
00054
00055
00056 transformAndWriteToFile (source_cloud_ptr, ransac_trafo);
00057
00058 return 0;
00059 }