00001 #include "solution/registration.h"
00002
00003 #include <vector>
00004 #include <string>
00005 #include <pcl/console/parse.h>
00006 #include <pcl/io/pcd_io.h>
00007 #include <pcl/visualization/pcl_visualizer.h>
00008
00009 #include "load_clouds.h"
00010
00011 int
00012 main (int argc, char ** argv)
00013 {
00014 if (argc < 3)
00015 {
00016 pcl::console::print_info ("Syntax is: %s source target <options>\n", argv[0]);
00017 pcl::console::print_info (" where options are:\n");
00018 pcl::console::print_info (" -i min_sample_dist,max_dist,nr_iters ................ Compute initial alignment\n");
00019 pcl::console::print_info (" -r max_dist,rejection_thresh,tform_eps,max_iters ............. Refine alignment\n");
00020 pcl::console::print_info (" -s output.pcd ........................... Save the registered and merged clouds\n");
00021 pcl::console::print_info ("Note: The inputs (source and target) must be specified without the .pcd extension\n");
00022
00023 return (1);
00024 }
00025
00026
00027 PointCloudPtr src_points = loadPoints (argv[1]);
00028 PointCloudPtr tgt_points = loadPoints (argv[2]);
00029 Eigen::Matrix4f tform = Eigen::Matrix4f::Identity ();
00030
00031
00032 double min_sample_dist, max_correspondence_dist, nr_iters;
00033 bool compute_intial_alignment =
00034 pcl::console::parse_3x_arguments (argc, argv, "-i", min_sample_dist, max_correspondence_dist, nr_iters) > 0;
00035 if (compute_intial_alignment)
00036 {
00037
00038 PointCloudPtr src_keypoints = loadKeypoints (argv[1]);
00039 LocalDescriptorsPtr src_descriptors = loadLocalDescriptors (argv[1]);
00040 PointCloudPtr tgt_keypoints = loadKeypoints (argv[2]);
00041 LocalDescriptorsPtr tgt_descriptors = loadLocalDescriptors (argv[2]);
00042
00043
00044 tform = computeInitialAlignment (src_keypoints, src_descriptors, tgt_keypoints, tgt_descriptors,
00045 min_sample_dist, max_correspondence_dist, nr_iters);
00046
00047 pcl::console::print_info ("Computed initial alignment\n");
00048 }
00049
00050
00051 std::string params_string;
00052 bool refine_alignment = pcl::console::parse_argument (argc, argv, "-r", params_string) > 0;
00053 if (refine_alignment)
00054 {
00055 std::vector<std::string> tokens;
00056 boost::split (tokens, params_string, boost::is_any_of (","), boost::token_compress_on);
00057 assert (tokens.size () == 4);
00058 float max_correspondence_distance = atof(tokens[0].c_str ());
00059 float outlier_rejection_threshold = atof(tokens[1].c_str ());
00060 float transformation_epsilon = atoi(tokens[2].c_str ());
00061 int max_iterations = atoi(tokens[3].c_str ());
00062
00063 tform = refineAlignment (src_points, tgt_points, tform, max_correspondence_distance,
00064 outlier_rejection_threshold, transformation_epsilon, max_iterations);
00065
00066 pcl::console::print_info ("Refined alignment\n");
00067 }
00068
00069
00070 pcl::transformPointCloud (*src_points, *src_points, tform);
00071
00072
00073 std::string filename;
00074 bool save_output = pcl::console::parse_argument (argc, argv, "-s", filename) > 0;
00075 if (save_output)
00076 {
00077
00078 (*src_points) += (*tgt_points);
00079
00080
00081 pcl::io::savePCDFile (filename, *src_points);
00082
00083 pcl::console::print_info ("Saved registered clouds as %s\n", filename.c_str ());
00084 }
00085
00086 else
00087 {
00088 pcl::console::print_info ("Starting visualizer... Close window to exit\n");
00089 pcl::visualization::PCLVisualizer vis;
00090
00091 pcl::visualization::PointCloudColorHandlerCustom<PointT> red (src_points, 255, 0, 0);
00092 vis.addPointCloud (src_points, red, "src_points");
00093
00094 pcl::visualization::PointCloudColorHandlerCustom<PointT> yellow (tgt_points, 255, 255, 0);
00095 vis.addPointCloud (tgt_points, yellow, "tgt_points");
00096
00097 vis.resetCamera ();
00098 vis.spin ();
00099 }
00100
00101 return (0);
00102 }