rgbd_registration: joint_optimize_wrapper.cpp Source File

Go to the documentation of this file.
00001 /*
00002  * joint_optimize_wrapper.cpp
00003  *
00004  *  Created on: Sep 26, 2012
00005  *      Author: kidson
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   //ICP cannot handle points with NaN values
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   //pointcloud normals are required for icp point to plane
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   // the indices of features are required by icp joint optimization
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   // Please see rgbd_registration.launch for an explanation of the following parameters
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")); //1
00059   // Set TransformationEstimationWDF as ICP transform estimator
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 }