Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008 #include "rgbd_registration/joint_optimize_wrapper.h"
00009 #include "rgbd_registration/transformation_estimation_wdf.h"
00010 #include "rgbd_registration/pcl_utils.h"
00011 #include "rgbd_registration/parameter_server.h"
00012
00013 #include <ros/console.h>
00014
00015 #include <pcl/registration/icp.h>
00016 #include <pcl/filters/filter.h>
00017
00018 Eigen::Matrix4f performJointOptimization (PointCloudConstPtr source_cloud_ptr,
00019 PointCloudConstPtr target_cloud_ptr, std::vector<Eigen::Vector4f, Eigen::aligned_allocator<
00020 Eigen::Vector4f> >& source_feature_3d_locations, std::vector<Eigen::Vector4f,
00021 Eigen::aligned_allocator<Eigen::Vector4f> >& target_feature_3d_locations,
00022 Eigen::Matrix4f& initial_transformation)
00023 {
00024
00025 std::vector<int> removed_points;
00026 PointCloudPtr source_cloud_noNaN_ptr (new PointCloud);
00027 PointCloudPtr target_cloud_noNaN_ptr (new PointCloud);
00028 pcl::removeNaNFromPointCloud (*source_cloud_ptr, *source_cloud_noNaN_ptr, removed_points);
00029 pcl::removeNaNFromPointCloud (*target_cloud_ptr, *target_cloud_noNaN_ptr, removed_points);
00030
00031
00032 ROS_INFO("[performJointOptimization] Calculating point cloud normals...");
00033 PointCloudNormalsPtr source_cloud_normals_ptr (new PointCloudNormals);
00034 PointCloudNormalsPtr target_cloud_normals_ptr (new PointCloudNormals);
00035 calculatePointCloudNormals (source_cloud_noNaN_ptr, source_cloud_normals_ptr);
00036 calculatePointCloudNormals (target_cloud_noNaN_ptr, target_cloud_normals_ptr);
00037
00038
00039 std::vector<int> source_indices, target_indices;
00040 getIndicesFromMatches<PointNormal> (source_cloud_normals_ptr, source_feature_3d_locations,
00041 source_indices);
00042 getIndicesFromMatches<PointNormal> (target_cloud_normals_ptr, target_feature_3d_locations,
00043 target_indices);
00044
00045 boost::shared_ptr<TransformationEstimationWDF<PointNormal, PointNormal> > initial_transform_WDF (
00046 new TransformationEstimationWDF<PointNormal, PointNormal> ());
00047
00048
00049 ParameterServer* ps = ParameterServer::instance ();
00050
00051 initial_transform_WDF->setAlpha (ps->get<double> ("alpha"));
00052 initial_transform_WDF->setCorrespondecesDFP (source_indices, target_indices);
00053
00054 pcl::IterativeClosestPoint<PointNormal, PointNormal> icp_wdf;
00055 icp_wdf.setMaxCorrespondenceDistance (ps->get<double> ("max_correspondence_dist"));
00056 icp_wdf.setMaximumIterations (ps->get<int> ("max_iterations"));
00057 icp_wdf.setTransformationEpsilon (ps->get<double> ("transformation_epsilon"));
00058 icp_wdf.setEuclideanFitnessEpsilon (ps->get<double> ("euclidean_fitness_epsilon"));
00059
00060 icp_wdf.setTransformationEstimation (initial_transform_WDF);
00061
00062 checkforNaNs (source_cloud_normals_ptr);
00063 checkforNaNs (target_cloud_normals_ptr);
00064 icp_wdf.setInputCloud (source_cloud_normals_ptr);
00065 icp_wdf.setInputTarget (target_cloud_normals_ptr);
00066
00067 if (ps->get<bool> ("enable_pcl_debug_verbosity"))
00068 pcl::console::setVerbosityLevel (pcl::console::L_DEBUG);
00069 else
00070 ROS_INFO(
00071 "[performJointOptimization] Now Performing Joint Optimization. This could take up to several minutes.....");
00072
00073 PointCloudNormalsPtr cloud_transformed (new PointCloudNormals);
00074 if (ParameterServer::instance ()->get<bool> ("use_ransac_to_initialize_icp"))
00075 icp_wdf.align (*cloud_transformed, initial_transformation);
00076 else
00077 icp_wdf.align (*cloud_transformed);
00078
00079 ROS_INFO_STREAM(
00080 "[performJointOptimization] Has converged? = " << icp_wdf.hasConverged () << std::endl << " fitness score (SSD): " << icp_wdf.getFitnessScore (1000) << " \n Final Transformation: \n" << icp_wdf.getFinalTransformation());
00081
00082 if (ps->get<bool> ("save_all_pointclouds"))
00083 {
00084 writePCDToFile ("source_cloud.pcd", source_cloud_normals_ptr);
00085 writePCDToFile ("target_cloud.pcd", target_cloud_normals_ptr);
00086 writePCDToFile ("source_cloud_features.pcd", source_cloud_normals_ptr, source_indices);
00087 writePCDToFile ("target_cloud_features.pcd", target_cloud_normals_ptr, target_indices);
00088 transformAndWriteToFile (source_cloud_normals_ptr, source_indices,
00089 icp_wdf.getFinalTransformation ());
00090 }
00091
00092 return icp_wdf.getFinalTransformation ();
00093 }