├── .gitignore ├── CMakeLists.txt ├── README ├── align_exe.cxx ├── alignment.cxx ├── alignment.h ├── bbox.cxx ├── compare_correspondences.cxx ├── correspondence_procrustes.h ├── correspondence_procrustes.hpp ├── cropper.cxx ├── distance.cxx ├── sac_non_rigid.h ├── sac_non_rigid.hpp ├── save_correspondences.cxx ├── util.cxx ├── util.h └── visualize.cxx /.gitignore: -------------------------------------------------------------------------------- 1 | *.swp 2 | tags 3 | archive/ 4 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.6 FATAL_ERROR) 2 | project(3D_REGISTRATION) 3 | #find pcl libraries 4 | find_package(PCL 1.4 REQUIRED) 5 | #include files 6 | include_directories(${PCL_INCLUDE_DIRS}) 7 | #lib files 8 | link_directories(${PCL_LIBRARY_DIRS}) 9 | #definitions 10 | add_definitions(${PCL_DEFINITIONS}) 11 | 12 | #necessary libs 13 | set(pcl_libs 14 | ${PCL_COMMON_LIBRARIES} 15 | ${PCL_IO_LIBRARIES} 16 | ${PCL_KD_TREE_LIBRARY} 17 | ${PCL_REGISTRATION_LIBRARY} 18 | ${PCL_SEARCH_LIBRARY} 19 | ${PCL_FEATURES_LIBRARY} 20 | ${PCL_VISUALIZATION_LIBRARY} 21 | ) 22 | 23 | 24 | ################################ 25 | #Alignment library 26 | set(alignment_sources 27 | alignment.h alignment.cxx 28 | correspondence_procrustes.h correspondence_procrustes.hpp 29 | sac_non_rigid.h sac_non_rigid.hpp 30 | util.h util.cxx 31 | ) 32 | add_library(pcl_alignment ${alignment_sources}) 33 | target_link_libraries(pcl_alignment ${pcl_libs}) 34 | 35 | ################################ 36 | #Executables 37 | add_executable(align align_exe.cxx) 38 | target_link_libraries(align pcl_alignment ${pcl_libs}) 39 | 40 | add_executable(corrs save_correspondences.cxx) 41 | target_link_libraries(corrs pcl_alignment ${pcl_libs}) 42 | 43 | add_executable(visualize visualize.cxx) 44 | target_link_libraries(visualize pcl_alignment ${pcl_libs}) 45 | 46 | add_executable(compare compare_correspondences.cxx) 47 | target_link_libraries(compare pcl_alignment ${pcl_libs}) 48 | 49 | add_executable(bbox bbox.cxx) 50 | target_link_libraries(bbox pcl_alignment ${pcl_libs}) 51 | 52 | add_executable(crop cropper.cxx) 53 | target_link_libraries(crop pcl_alignment ${pcl_libs}) 54 | 55 | add_executable(distance distance.cxx) 56 | target_link_libraries(distance pcl_alignment ${pcl_libs}) 57 | 58 | 59 | -------------------------------------------------------------------------------- /README: -------------------------------------------------------------------------------- 1 | Feature Based Alignment 2 | 3 | Some programs to roughly align two point clouds representing the same underlying geometry. 4 | The goal is to accurately register two point clouds created from different image spectrums 5 | (e.g. visual to infrared), in different coordinate systems of different scales. 6 | 7 | This depends on: 8 | - PCL (http://pointclouds.org/) 9 | - CMake and some c++ compiler 10 | Also used the pcl visualization library, which depends on some toolkit (I use QT4). 11 | 12 | Current approach is: 13 | - Take two point clouds 14 | - Estimate normals for each point 15 | - Find FPFH Features for each point 16 | - Find likely correspondences between two models (based on FPFH) 17 | - Use top features to estimate scale, 18 | - Run RANSAC to estimate transformation, and eliminate correspondences 19 | - Use remaining correspondences to run ICP for finer alignment 20 | 21 | 22 | Usage Example: 23 | #estimate correspondences - saves a corrs.txt file 24 | ./corrs cloudA.ply cloudB.ply AtoBCorrs.txt 25 | 26 | #run rough alignment using corrs 27 | ./align cloudA.ply cloudB.ply AtoBCorrs.txt 28 | 29 | #saves files: 30 | irRough.pcd - cloud A after RANSAC alignment 31 | eoRough.pcd - cloud B after RANSAC alignment 32 | irFinal.pcd - cloud A after ICP alignment 33 | eoFinal.pcd - cloud B after ICP alignment 34 | 35 | 36 | other tools include bbox and circular cropping 37 | ./bbox cloud.pcd 38 | 39 | ./crop cloud.pcd centerX centerY centerZ radius out.pcd 40 | -------------------------------------------------------------------------------- /align_exe.cxx: -------------------------------------------------------------------------------- 1 | #include "util.h" 2 | #include "alignment.h" 3 | 4 | //------------------------- 5 | //--- Alignment Main ------ 6 | //------------------------- 7 | int main(int argc, char* argv[]) 8 | { 9 | if(argc < 3) { 10 | cout<<"usage: ./icp cloud0.ply cloud1.ply [corrs.txt]"<(0,0) = scale*R; 33 | trans0.block<4,1>(0,3) = T; 34 | cout<<"Initial Transformation:\n"<size()<(Atrim, Btrim, finalTrans); 78 | cout<<"ICP Transform:\n"< 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include "correspondence_procrustes.h" 10 | #include 11 | #include 12 | 13 | //--- Main Public Method. Computes rough alignment between two point clouds ----- 14 | Eigen::Matrix4f RoughFeatureAlignment::align(PointCloud::Ptr irCloud, 15 | PointCloud::Ptr eoCloud, 16 | pcl::CorrespondencesPtr corrs) 17 | { 18 | if(corrs.get() == NULL) { 19 | NormalCloud::Ptr irNormals = getPointNormals(irCloud); 20 | NormalCloud::Ptr eoNormals = getPointNormals(eoCloud); 21 | 22 | //----PFH Features ----- 23 | PFHCloud::Ptr irPFH = getPFHFeatures(irCloud, irNormals); 24 | PFHCloud::Ptr eoPFH = getPFHFeatures(eoCloud, eoNormals); 25 | 26 | //---- Get PFH Corrs ---- 27 | cout<<"Getting PFH Correspondences"<(irPFH, eoPFH); 29 | cout<<"Number of correspondences found: "<size()<begin(), corrs->end(), pcl::isBetterCorrespondence); 34 | reverse(corrs->begin(), corrs->end()); 35 | int numFeats = (int) (.1 * corrs->size()); 36 | vector indices_src(numFeats), indices_tgt(numFeats); 37 | for(int i=0; iat(i).index_query; 39 | indices_tgt[i] = corrs->at(i).index_match; 40 | } 41 | 42 | int topHalf = (int) (.15 * corrs->size()); 43 | pcl::CorrespondencesPtr topCorrs(new pcl::Correspondences()); 44 | for(int i=0; ipush_back( corrs->at(i) ); 46 | corrs = topCorrs; 47 | 48 | // Estimate the centroids of source, target 49 | Eigen::Vector4f centroid_src, centroid_tgt; 50 | compute3DCentroid (*irCloud, indices_src, centroid_src); 51 | compute3DCentroid (*eoCloud, indices_tgt, centroid_tgt); 52 | 53 | // Compute RMSD for each centroid for top features 54 | PointXYZ srcCent(centroid_src[0], centroid_src[1], centroid_src[2]); 55 | double scale_src = util::computeRMSD(*irCloud, indices_src, srcCent); 56 | PointXYZ tgtCent(centroid_tgt[0], centroid_tgt[1], centroid_tgt[2]); 57 | double scale_tgt = util::computeRMSD(*eoCloud, indices_tgt, tgtCent); 58 | 59 | //calculate rough target scaling 60 | double roughScale = scale_tgt / scale_src; 61 | cout<<"Rough initial scaling: "< sac; 71 | sac.setInputCloud(scaledIR); 72 | sac.setTargetCloud(eoCloud); 73 | sac.setMaxIterations(1000); 74 | sac.setInlierThreshold(20);//5.0 for geo downtown, .5 for hemenways.... 75 | sac.setInputCorrespondences(corrs); 76 | pcl::CorrespondencesPtr newCorrs(new pcl::Correspondences()); 77 | sac.getCorrespondences(*newCorrs); 78 | Eigen::Matrix4f rSacTrans = sac.getBestTransformation(); 79 | trimmedCorrs_= newCorrs; 80 | cout<<"Number of correspondences left: "<size()< svd; 86 | svd.estimateRigidTransformation(*scaledIR, *eoCloud, *newCorrs, transform); 87 | Eigen::Matrix3f R = roughScale*transform.block<3,3>(0,0); 88 | transform.block<3,3>(0,0) = R; 89 | return transform; 90 | #if 0 91 | pcl::registration::TransformationEstimationLM lev; 92 | lev.estimateRigidTransformation(*irCloud, *eoCloud, *corrs, transform); 93 | cout<<"Levenberg Marcquat est: \n"<( irCloud, irPFH, eoCloud, eoPFH, trans ); 98 | cout<<"SAC Transform:\n"< pfh; 110 | pfh.setInputCloud (cloud); 111 | pfh.setInputNormals (normals); 112 | // alternatively, if cloud is of tpe PointNormal, do pfh.setInputNormals (cloud); 113 | 114 | // Create an empty kdtree representation, and pass it to the PFH estimation object. 115 | // Its content will be filled inside the object, based on the given input dataset (as no other search surface is given). 116 | pcl::search::KdTree::Ptr tree (new pcl::search::KdTree ()); 117 | pfh.setSearchMethod (tree); 118 | 119 | // Output datasets 120 | PFHCloud::Ptr pfhs (new PFHCloud()); 121 | 122 | // Use all neighbors in a sphere of radius 5cm 123 | // IMPORTANT: the radius used here has to be larger than the radius used to estimate the surface normals!!! 124 | //pfh.setRadiusSearch (radius); 125 | pfh.setKSearch(K); 126 | 127 | // Compute the features 128 | pfh.compute (*pfhs); 129 | 130 | // pfhs->points.size () should have the same size as the input cloud->points.size ()* 131 | return pfhs; 132 | } 133 | 134 | 135 | //---------Estimates point normals using points within radius------------ 136 | NormalCloud::Ptr 137 | RoughFeatureAlignment::getPointNormals(PointCloud::Ptr cloud, int K) 138 | { 139 | // Create the normal estimation class, and pass the input dataset to it 140 | pcl::NormalEstimation ne; 141 | ne.setInputCloud (cloud); 142 | 143 | // Create an empty kdtree representation, and pass it to the normal estimation object. 144 | // Its content will be filled inside the object, based on the given input dataset (as no other search surface is given). 145 | pcl::search::KdTree::Ptr tree (new pcl::search::KdTree ()); 146 | ne.setSearchMethod (tree); 147 | 148 | #if 0 149 | //test search method 150 | tree->setInputCloud(cloud); 151 | int index = 10000; 152 | double radius = 1.25; 153 | vector k_indices; 154 | vector k_sqr_distances; 155 | tree->radiusSearch(index, radius, k_indices, k_sqr_distances); 156 | cout<<"Tree found "<::Ptr cloud_normals (new pcl::PointCloud()); 161 | 162 | // Use all neighbors in a sphere of radius 3cm 163 | //ne.setRadiusSearch (radius); 164 | ne.setKSearch(K); 165 | 166 | // Compute the features 167 | ne.compute (*cloud_normals); 168 | 169 | //output cloud norms 170 | return cloud_normals; 171 | } 172 | 173 | -------------------------------------------------------------------------------- /alignment.h: -------------------------------------------------------------------------------- 1 | #ifndef alignment_h_ 2 | #define alignment_h_ 3 | 4 | #include "util.h" 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | using namespace std; 16 | //point types 17 | typedef pcl::PointXYZ PointXYZ; 18 | typedef pcl::PointXYZRGB PointXYZRGB; 19 | typedef pcl::Normal Normal; 20 | typedef pcl::PointNormal PointNormal; 21 | typedef pcl::FPFHSignature33 PointPFH; 22 | 23 | //cloud types 24 | typedef pcl::PointCloud PointCloud; 25 | typedef pcl::PointCloud ColorCloud; 26 | typedef pcl::PointCloud NormalCloud; 27 | typedef pcl::PointCloud PointNormalCloud; 28 | typedef pcl::PointCloud PFHCloud; 29 | 30 | 31 | class RoughFeatureAlignment { 32 | public: 33 | //---------- rough alignment step (non intersecting clouds) ------ 34 | Eigen::Matrix4f align(PointCloud::Ptr A, 35 | PointCloud::Ptr B, 36 | pcl::CorrespondencesPtr corrs = pcl::CorrespondencesPtr()); 37 | 38 | //get and return correspondences 39 | pcl::CorrespondencesPtr getCorrespondences() { return corrs_; } 40 | pcl::CorrespondencesPtr getTrimmedCorrespondences() { return trimmedCorrs_; } 41 | 42 | //----- Takes in an xyz point cloud and normal cloud and spits out PFH Feature cloud -------- 43 | PFHCloud::Ptr getPFHFeatures(PointCloud::Ptr cloud, NormalCloud::Ptr normals, int K=60); 44 | 45 | //---------Estimates point normals using points within radius------------ 46 | NormalCloud::Ptr getPointNormals(PointCloud::Ptr cloud, int K=45); 47 | 48 | //--------- estimate correspondances ---------------- 49 | template 50 | pcl::CorrespondencesPtr estimateCorrespondances(typename pcl::PointCloud::Ptr& sourcePts, 51 | typename pcl::PointCloud::Ptr& targetPts, 52 | const unsigned K=10000); 53 | 54 | //--------- More exact feature alignment --------------- 55 | template 56 | typename pcl::PointCloud::Ptr runICP(typename pcl::PointCloud::Ptr irCloud, 57 | typename pcl::PointCloud::Ptr eoCloud, 58 | Eigen::Matrix4f& transform) 59 | { 60 | pcl::IterativeClosestPoint icp; 61 | // Set the input source and target 62 | icp.setInputCloud(irCloud); 63 | icp.setInputTarget(eoCloud); 64 | 65 | // Set the max correspondence distance to 5cm (e.g., correspondences with higher distances will be ignored) 66 | icp.setMaxCorrespondenceDistance (200); 67 | // Set the maximum number of iterations (criterion 1) 68 | icp.setMaximumIterations (300); 69 | // Set the transformation epsilon (criterion 2) 70 | icp.setTransformationEpsilon (1e-16); 71 | // Set the euclidean distance difference epsilon (criterion 3) 72 | icp.setEuclideanFitnessEpsilon (.5); 73 | 74 | // Perform the alignment 75 | PointCloud::Ptr registered(new pcl::PointCloud()); 76 | icp.align (*registered, transform); 77 | cout<<"ICP has converged:"< 94 | pcl::CorrespondencesPtr 95 | RoughFeatureAlignment::estimateCorrespondances(typename pcl::PointCloud::Ptr& sourcePts, 96 | typename pcl::PointCloud::Ptr& targetPts, 97 | const unsigned K) 98 | { 99 | //---- grab correspondences for each point in the source cloud ----- 100 | cout<<"Getting correspondences"< ce; 104 | ce.setInputTarget(targetPts); 105 | ce.setInputCloud(sourcePts); 106 | pcl::CorrespondencesPtr corr(new pcl::Correspondences()); 107 | ce.determineCorrespondences(*corr); 108 | 109 | sort(corr->begin(), corr->end(), pcl::isBetterCorrespondence); 110 | //reverse(corr->begin(), corr->end()); 111 | corrs_ = corr; 112 | 113 | //corrs 114 | //vector corrs; 115 | //for(int i=0; isize(); ++i) 116 | // corrs.push_back( (*corr)[i]) ; 117 | 118 | //find range of correspondences 119 | //sort(corrs.begin(), corrs.end(), pcl::isBetterCorrespondence); 120 | //reverse(corrs.begin(), corrs.end()); 121 | 122 | //get trimmed 123 | //reject manually.... 124 | //pcl::CorrespondencesPtr trimmed(new pcl::Correspondences()); 125 | //for(int i=0; ipush_back(corrs[ corrs.size() - i - 1 ]); 127 | //} 128 | //trimmedCorrs_ = trimmed; 129 | return corrs_; 130 | } 131 | 132 | #endif //alignment_h_ 133 | 134 | 135 | -------------------------------------------------------------------------------- /bbox.cxx: -------------------------------------------------------------------------------- 1 | #include "util.h" 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | using namespace std; 8 | int main(int argc, char* argv[]) 9 | { 10 | if(argc < 2) { 11 | cout<<"usage: ./bbox cloud.pcd"<::Ptr irCloud = util::loadCloud(argv[1]); 17 | pcl::PointXYZ minPt, maxPt; 18 | pcl::getMinMax3D(*irCloud, minPt, maxPt); 19 | 20 | cout<<"Bounding box min,max: "< 3 | #include 4 | #include 5 | using namespace std; 6 | int main(int argc, char* argv[]) 7 | { 8 | if(argc < 3) { 9 | cout<<"usage: ./compare corr1.txt corr2.txt"< corrsA; 23 | for(int i=0; isize(); ++i){ 24 | pcl::Correspondence corrA = A->at(i); 25 | corrsA[corrA.index_query] = corrA; 26 | } 27 | 28 | //zip through B for comparison 29 | for(int i=0; isize(); ++i) { 30 | pcl::Correspondence corrB = B->at(i); 31 | map::iterator iter = corrsA.find(corrB.index_query); 32 | if(iter == corrsA.end()) 33 | continue; 34 | 35 | pcl::Correspondence corrA = iter->second; 36 | if(corrA.index_match == corrB.index_match) { 37 | ssd = (corrA.distance - corrB.distance) * (corrA.distance - corrB.distance); 38 | goodMatches++; 39 | meanDist += (corrA.distance + corrB.distance)/2.0; 40 | } 41 | else { 42 | badMatches++; 43 | } 44 | } 45 | 46 | cout<<"Number of good matches: "< 5 | 6 | #include 7 | #include "sac_non_rigid.h" 8 | //#include 9 | 10 | namespace pcl 11 | { 12 | namespace registration 13 | { 14 | /** \brief CorrespondenceRejectorProcrustes implements a correspondence rejection 15 | * using Random Sample Consensus to identify inliers (and reject outliers), using 16 | * procrustes to estimate goodness of fit 17 | * \author Andy Miller 18 | * \ingroup registration 19 | */ 20 | template 21 | class CorrespondenceRejectorProcrustes: public CorrespondenceRejector 22 | { 23 | using CorrespondenceRejector::input_correspondences_; 24 | using CorrespondenceRejector::rejection_name_; 25 | using CorrespondenceRejector::getClassName; 26 | 27 | typedef pcl::PointCloud PointCloud; 28 | typedef typename PointCloud::Ptr PointCloudPtr; 29 | typedef typename PointCloud::ConstPtr PointCloudConstPtr; 30 | 31 | public: 32 | 33 | /** \brief Empty constructor. */ 34 | CorrespondenceRejectorProcrustes () 35 | { 36 | rejection_name_ = "CorrespondenceRejectorProcrustes"; 37 | inlier_threshold_ = 0.05; 38 | } 39 | 40 | /** \brief Get a list of valid correspondences after rejection from the original set of correspondences. 41 | * \param[in] original_correspondences the set of initial correspondences given 42 | * \param[out] remaining_correspondences the resultant filtered set of remaining correspondences 43 | */ 44 | inline void 45 | getRemainingCorrespondences (const pcl::Correspondences& original_correspondences, 46 | pcl::Correspondences& remaining_correspondences); 47 | 48 | /** \brief Provide a source point cloud dataset (must contain XYZ data!) 49 | * \param[in] cloud a cloud containing XYZ data 50 | */ 51 | virtual inline void 52 | setInputCloud (const PointCloudConstPtr &cloud) { input_ = cloud; } 53 | 54 | /** \brief Provide a target point cloud dataset (must contain XYZ data!) 55 | * \param[in] cloud a cloud containing XYZ data 56 | */ 57 | virtual inline void 58 | setTargetCloud (const PointCloudConstPtr &cloud) { target_ = cloud; } 59 | 60 | /** \brief Set the maximum distance between corresponding points. 61 | * Correspondences with distances below the threshold are considered as inliers. 62 | * \param[in] threshold Distance threshold in the same dimension as source and target data sets. 63 | */ 64 | inline void 65 | setInlierThreshold (double threshold) { inlier_threshold_ = threshold; }; 66 | 67 | /** \brief Get the maximum distance between corresponding points. 68 | * \return Distance threshold in the same dimension as source and target data sets. 69 | */ 70 | inline double 71 | getInlierThreshold() { return inlier_threshold_; }; 72 | 73 | /** \brief Set the maximum number of iterations. 74 | * \param[in] max_iterations Maximum number if iterations to run 75 | */ 76 | inline void 77 | setMaxIterations (int max_iterations) {max_iterations_ = std::max(max_iterations, 0); }; 78 | 79 | /** \brief Get the maximum number of iterations. 80 | * \return max_iterations Maximum number if iterations to run 81 | */ 82 | inline int 83 | getMaxIterations () { return max_iterations_; }; 84 | 85 | /** \brief Get the best transformation after RANSAC rejection. 86 | * \return The homogeneous 4x4 transformation yielding the largest number of inliers. 87 | */ 88 | inline Eigen::Matrix4f 89 | getBestTransformation () { return best_transformation_; }; 90 | 91 | protected: 92 | 93 | /** \brief Apply the rejection algorithm. 94 | * \param[out] correspondences the set of resultant correspondences. 95 | */ 96 | inline void 97 | applyRejection (pcl::Correspondences &correspondences) 98 | { 99 | getRemainingCorrespondences (*input_correspondences_, correspondences); 100 | } 101 | 102 | double inlier_threshold_; 103 | 104 | int max_iterations_; 105 | 106 | PointCloudConstPtr input_; 107 | PointCloudConstPtr target_; 108 | 109 | Eigen::Matrix4f best_transformation_; 110 | }; 111 | } 112 | } 113 | 114 | #include "correspondence_procrustes.hpp" 115 | 116 | #endif /* correspondence_procrustes_ */ 117 | 118 | 119 | -------------------------------------------------------------------------------- /correspondence_procrustes.hpp: -------------------------------------------------------------------------------- 1 | #ifndef correspondence_procrustes_hpp_ 2 | #define correspondence_procrustes_hpp_ 3 | #include 4 | 5 | ////////////////////////////////////////////////////////////////////////////////////////////// 6 | template void 7 | pcl::registration::CorrespondenceRejectorProcrustes::getRemainingCorrespondences ( 8 | const pcl::Correspondences& original_correspondences, 9 | pcl::Correspondences& remaining_correspondences) 10 | { 11 | int nr_correspondences = (int)original_correspondences.size (); 12 | std::vector source_indices (nr_correspondences); 13 | std::vector target_indices (nr_correspondences); 14 | 15 | // Copy the query-match indices 16 | for (size_t i = 0; i < original_correspondences.size (); ++i) 17 | { 18 | source_indices[i] = original_correspondences[i].index_query; 19 | target_indices[i] = original_correspondences[i].index_match; 20 | } 21 | 22 | // from pcl/registration/icp.hpp: 23 | std::vector source_indices_good; 24 | std::vector target_indices_good; 25 | { 26 | // From the set of correspondences found, attempt to remove outliers 27 | // Create the registration model 28 | typedef typename pcl::SampleConsensusModelNonRigid::Ptr SampleConsensusModelNonRigidPtr; 29 | SampleConsensusModelNonRigidPtr model; 30 | model.reset (new pcl::SampleConsensusModelNonRigid (input_, source_indices)); 31 | // Pass the target_indices 32 | model->setInputTarget (target_, target_indices); 33 | // Create a RANSAC model 34 | pcl::RandomSampleConsensus sac (model, inlier_threshold_); 35 | sac.setMaxIterations (max_iterations_); 36 | 37 | // Compute the set of inliers 38 | if (!sac.computeModel ()) 39 | { 40 | remaining_correspondences = original_correspondences; 41 | best_transformation_.setIdentity (); 42 | return; 43 | } 44 | else 45 | { 46 | std::vector inliers; 47 | sac.getInliers (inliers); 48 | 49 | if (inliers.size () < 3) 50 | { 51 | remaining_correspondences = original_correspondences; 52 | best_transformation_.setIdentity (); 53 | return; 54 | } 55 | boost::unordered_map index_to_correspondence; 56 | for (int i = 0; i < nr_correspondences; ++i) 57 | index_to_correspondence[original_correspondences[i].index_query] = i; 58 | 59 | remaining_correspondences.resize (inliers.size ()); 60 | for (size_t i = 0; i < inliers.size (); ++i) 61 | remaining_correspondences[i] = original_correspondences[index_to_correspondence[inliers[i]]]; 62 | 63 | // get best transformation 64 | Eigen::VectorXf model_coefficients; 65 | sac.getModelCoefficients (model_coefficients); 66 | best_transformation_.row (0) = model_coefficients.segment<4>(0); 67 | best_transformation_.row (1) = model_coefficients.segment<4>(4); 68 | best_transformation_.row (2) = model_coefficients.segment<4>(8); 69 | best_transformation_.row (3) = model_coefficients.segment<4>(12); 70 | } 71 | } 72 | } 73 | 74 | #endif //correspondence_procrustes_hpp_ 75 | 76 | -------------------------------------------------------------------------------- /cropper.cxx: -------------------------------------------------------------------------------- 1 | #include "util.h" 2 | #include 3 | #include 4 | #include 5 | using namespace std; 6 | int main(int argc, char* argv[]) 7 | { 8 | if(argc < 7) { 9 | cout<<"usage: ./crop cloud.pcd centerX centerY centerZ radius cloud_out.pcd"< PointCloud; 14 | 15 | //--- grab center argument --- 16 | //create center point 17 | PointXYZ center(util::fromString(argv[2]), 18 | util::fromString(argv[3]), 19 | util::fromString(argv[4])); 20 | PointXYZ centerZ(center.x, center.y, 0.0); 21 | 22 | //radius 23 | double radius = util::fromString(argv[5]); 24 | double rad2 = radius*radius; 25 | cout<<"Cropping cloud around point: "<size(); ++i) { 33 | PointXYZ pt = cloud->at(i); 34 | PointXYZ pt2d(pt.x, pt.y, 0.0); 35 | if(util::distance2(pt2d, centerZ) < rad2) 36 | out->push_back(pt); 37 | } 38 | 39 | //save cloud 40 | cout<<"Saving cloud of size: "<size()< 3 | #include 4 | #include 5 | using namespace std; 6 | int main(int argc, char* argv[]) 7 | { 8 | if(argc < 2) { 9 | cout<<"usage: ./distance cloud.pcd"< PointCloud; 14 | 15 | //load cloud 16 | int argI=1; 17 | PointCloud::Ptr cloud = util::loadCloud(argv[argI++]); 18 | cout<<"running distances on "<<*cloud< tree; 22 | tree.setInputCloud(cloud); 23 | 24 | double minDist = 10e6, maxDist = -1.0; 25 | for(int i=0; isize(); ++i) { 26 | int k = 2; 27 | vector indices(k); 28 | vector sqrDist(k); 29 | tree.nearestKSearch(i, k, indices, sqrDist); 30 | if(sqrDist[1] < minDist) 31 | minDist = sqrDist[1]; 32 | 33 | } 34 | 35 | 36 | //find min/max distance 37 | // double minDist = 10e6, maxDist = -1.0; 38 | // for(int i=0; isize(); ++i) { 39 | // PointXYZ pta = cloud->at(i); 40 | // for(int j=0; jsize(); ++j) { 41 | // if(j > i) 42 | // continue; 43 | // PointXYZ ptb = cloud->at(j); 44 | // double dist2 = util::distance2(pta, ptb); 45 | // if(dist2 > maxDist) 46 | // maxDist = dist2; 47 | // if(dist2 < minDist) 48 | // minDist = dist2; 49 | // } 50 | // } 51 | minDist = sqrt(minDist); 52 | maxDist = sqrt(maxDist); 53 | 54 | //report the distances 55 | cout<<"Min distance between points "< 5 | #include 6 | #include "pcl/sample_consensus/sac_model.h" 7 | #include "pcl/sample_consensus/model_types.h" 8 | #include 9 | #include 10 | 11 | namespace pcl 12 | { 13 | /** \brief SampleConsensusModelNonRigid defines a model for Point-To-Point registration outlier rejection. 14 | * \author Radu Bogdan Rusu 15 | * \ingroup sample_consensus 16 | */ 17 | template 18 | class SampleConsensusModelNonRigid : public SampleConsensusModel 19 | { 20 | using SampleConsensusModel::input_; 21 | using SampleConsensusModel::indices_; 22 | 23 | public: 24 | typedef typename SampleConsensusModel::PointCloud PointCloud; 25 | typedef typename SampleConsensusModel::PointCloudPtr PointCloudPtr; 26 | typedef typename SampleConsensusModel::PointCloudConstPtr PointCloudConstPtr; 27 | 28 | typedef boost::shared_ptr Ptr; 29 | 30 | /** \brief Constructor for base SampleConsensusModelNonRigid. 31 | * \param[in] cloud the input point cloud dataset 32 | */ 33 | SampleConsensusModelNonRigid (const PointCloudConstPtr &cloud) : SampleConsensusModel (cloud) 34 | { 35 | // Call our own setInputCloud 36 | setInputCloud (cloud); 37 | } 38 | 39 | /** \brief Constructor for base SampleConsensusModelNonRigid. 40 | * \param[in] cloud the input point cloud dataset 41 | * \param[in] indices a vector of point indices to be used from \a cloud 42 | */ 43 | SampleConsensusModelNonRigid (const PointCloudConstPtr &cloud, 44 | const std::vector &indices) : 45 | SampleConsensusModel (cloud, indices) 46 | { 47 | computeOriginalIndexMapping (); 48 | computeSampleDistanceThreshold (cloud, indices); 49 | } 50 | 51 | /** \brief Provide a pointer to the input dataset 52 | * \param[in] cloud the const boost shared pointer to a PointCloud message 53 | */ 54 | inline virtual void 55 | setInputCloud (const PointCloudConstPtr &cloud) 56 | { 57 | SampleConsensusModel::setInputCloud (cloud); 58 | computeOriginalIndexMapping (); 59 | computeSampleDistanceThreshold (cloud); 60 | } 61 | 62 | /** \brief Set the input point cloud target. 63 | * \param target the input point cloud target 64 | */ 65 | inline void 66 | setInputTarget (const PointCloudConstPtr &target) 67 | { 68 | target_ = target; 69 | indices_tgt_.reset (new std::vector); 70 | // Cache the size and fill the target indices 71 | unsigned int target_size = target->size (); 72 | indices_tgt_->resize (target_size); 73 | 74 | for (unsigned int i = 0; i < target_size; ++i) 75 | (*indices_tgt_)[i] = i; 76 | computeOriginalIndexMapping (); 77 | } 78 | 79 | /** \brief Set the input point cloud target. 80 | * \param[in] target the input point cloud target 81 | * \param[in] indices_tgt a vector of point indices to be used from \a target 82 | */ 83 | inline void 84 | setInputTarget (const PointCloudConstPtr &target, const std::vector &indices_tgt) 85 | { 86 | target_ = target; 87 | indices_tgt_.reset (new std::vector (indices_tgt)); 88 | computeOriginalIndexMapping (); 89 | } 90 | 91 | /** \brief Compute a 4x4 rigid transformation matrix from the samples given 92 | * \param[in] samples the indices found as good candidates for creating a valid model 93 | * \param[out] model_coefficients the resultant model coefficients 94 | */ 95 | bool 96 | computeModelCoefficients (const std::vector &samples, 97 | Eigen::VectorXf &model_coefficients); 98 | 99 | /** \brief Compute all distances from the transformed points to their correspondences 100 | * \param[in] model_coefficients the 4x4 transformation matrix 101 | * \param[out] distances the resultant estimated distances 102 | */ 103 | void 104 | getDistancesToModel (const Eigen::VectorXf &model_coefficients, 105 | std::vector &distances); 106 | 107 | /** \brief Select all the points which respect the given model coefficients as inliers. 108 | * \param[in] model_coefficients the 4x4 transformation matrix 109 | * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers 110 | * \param[out] inliers the resultant model inliers 111 | */ 112 | void 113 | selectWithinDistance (const Eigen::VectorXf &model_coefficients, 114 | const double threshold, 115 | std::vector &inliers); 116 | 117 | /** \brief Count all the points which respect the given model coefficients as inliers. 118 | * 119 | * \param[in] model_coefficients the coefficients of a model that we need to compute distances to 120 | * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers 121 | * \return the resultant number of inliers 122 | */ 123 | virtual int 124 | countWithinDistance (const Eigen::VectorXf &model_coefficients, 125 | const double threshold); 126 | 127 | /** \brief Recompute the 4x4 transformation using the given inlier set 128 | * \param[in] inliers the data inliers found as supporting the model 129 | * \param[in] model_coefficients the initial guess for the optimization 130 | * \param[out] optimized_coefficients the resultant recomputed transformation 131 | */ 132 | void 133 | optimizeModelCoefficients (const std::vector &inliers, 134 | const Eigen::VectorXf &model_coefficients, 135 | Eigen::VectorXf &optimized_coefficients); 136 | 137 | void 138 | projectPoints (const std::vector &inliers, 139 | const Eigen::VectorXf &model_coefficients, 140 | PointCloud &projected_points, bool copy_data_fields = true) 141 | {}; 142 | 143 | bool 144 | doSamplesVerifyModel (const std::set &indices, 145 | const Eigen::VectorXf &model_coefficients, 146 | const double threshold) 147 | { 148 | //PCL_ERROR ("[pcl::SampleConsensusModelNonRigid::doSamplesVerifyModel] called!\n"); 149 | return (false); 150 | } 151 | 152 | /** \brief Return an unique id for this model (SACMODEL_REGISTRATION). */ 153 | inline pcl::SacModel 154 | getModelType () const { return (SACMODEL_REGISTRATION); } 155 | 156 | protected: 157 | /** \brief Check whether a model is valid given the user constraints. 158 | * \param[in] model_coefficients the set of model coefficients 159 | */ 160 | inline bool 161 | isModelValid (const Eigen::VectorXf &model_coefficients) 162 | { 163 | // Needs a valid model coefficients 164 | if (model_coefficients.size () != 16) 165 | return (false); 166 | 167 | return (true); 168 | } 169 | 170 | /** \brief Check if a sample of indices results in a good sample of points 171 | * indices. 172 | * \param[in] samples the resultant index samples 173 | */ 174 | bool 175 | isSampleGood (const std::vector &samples) const; 176 | 177 | /** \brief Computes an "optimal" sample distance threshold based on the 178 | * principal directions of the input cloud. 179 | * \param[in] cloud the const boost shared pointer to a PointCloud message 180 | */ 181 | inline void 182 | computeSampleDistanceThreshold (const PointCloudConstPtr &cloud) 183 | { 184 | // Compute the principal directions via PCA 185 | Eigen::Vector4f xyz_centroid; 186 | compute3DCentroid (*cloud, xyz_centroid); 187 | EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix; 188 | computeCovarianceMatrixNormalized (*cloud, xyz_centroid, covariance_matrix); 189 | EIGEN_ALIGN16 Eigen::Vector3f eigen_values; 190 | EIGEN_ALIGN16 Eigen::Matrix3f eigen_vectors; 191 | pcl::eigen33 (covariance_matrix, eigen_vectors, eigen_values); 192 | 193 | // Compute the distance threshold for sample selection 194 | sample_dist_thresh_ = eigen_values.array ().sqrt ().sum () / 3.0; 195 | sample_dist_thresh_ *= sample_dist_thresh_; 196 | cout<<"SAC Sample distance threshold="< &indices) 207 | { 208 | // Compute the principal directions via PCA 209 | Eigen::Vector4f xyz_centroid; 210 | compute3DCentroid (*cloud, indices, xyz_centroid); 211 | EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix; 212 | computeCovarianceMatrixNormalized (*cloud, indices, xyz_centroid, covariance_matrix); 213 | EIGEN_ALIGN16 Eigen::Vector3f eigen_values; 214 | EIGEN_ALIGN16 Eigen::Matrix3f eigen_vectors; 215 | pcl::eigen33 (covariance_matrix, eigen_vectors, eigen_values); 216 | 217 | // Compute the distance threshold for sample selection 218 | sample_dist_thresh_ = eigen_values.array ().sqrt ().sum () / 3.0; 219 | sample_dist_thresh_ *= sample_dist_thresh_; 220 | cout<<"SAC Sample distance threshold="< &cloud_src, 239 | const std::vector &indices_src, 240 | const pcl::PointCloud &cloud_tgt, 241 | const std::vector &indices_tgt, 242 | Eigen::VectorXf &transform); 243 | 244 | /** \brief Compute mappings between original indices of the input_/target_ clouds. */ 245 | void 246 | computeOriginalIndexMapping () 247 | { 248 | if (!indices_tgt_ || !indices_ || indices_->empty () || indices_->size () != indices_tgt_->size ()) 249 | return; 250 | for (size_t i = 0; i < indices_->size (); ++i) 251 | correspondences_[(*indices_)[i]] = (*indices_tgt_)[i]; 252 | } 253 | 254 | /** \brief A boost shared pointer to the target point cloud data array. */ 255 | PointCloudConstPtr target_; 256 | 257 | /** \brief A pointer to the vector of target point indices to use. */ 258 | boost::shared_ptr > indices_tgt_; 259 | 260 | /** \brief Given the index in the original point cloud, give the matching original index in the target cloud */ 261 | boost::unordered_map correspondences_; 262 | 263 | /** \brief Internal distance threshold used for the sample selection step. */ 264 | double sample_dist_thresh_; 265 | public: 266 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 267 | }; 268 | } 269 | 270 | #include "sac_non_rigid.hpp" 271 | 272 | #endif //#ifndef sac_non_rigid_h_ 273 | 274 | -------------------------------------------------------------------------------- /sac_non_rigid.hpp: -------------------------------------------------------------------------------- 1 | #ifndef sac_non_rigid_hpp_ 2 | #define sac_non_rigid_hpp_ 3 | 4 | //#include "pcl/sample_consensus/sac_model_registration.h" 5 | #include "sac_non_rigid.h" 6 | #include "util.h" 7 | 8 | ////////////////////////////////////////////////////////////////////////// 9 | template bool 10 | pcl::SampleConsensusModelNonRigid::isSampleGood (const std::vector &samples) const 11 | { 12 | return ((input_->points[samples[1]].getArray4fMap () - input_->points[samples[0]].getArray4fMap ()).matrix ().squaredNorm () > sample_dist_thresh_ && 13 | (input_->points[samples[2]].getArray4fMap () - input_->points[samples[0]].getArray4fMap ()).matrix ().squaredNorm () > sample_dist_thresh_ && 14 | (input_->points[samples[2]].getArray4fMap () - input_->points[samples[1]].getArray4fMap ()).matrix ().squaredNorm () > sample_dist_thresh_); 15 | } 16 | 17 | ////////////////////////////////////////////////////////////////////////// 18 | template bool 19 | pcl::SampleConsensusModelNonRigid::computeModelCoefficients (const std::vector &samples, Eigen::VectorXf &model_coefficients) 20 | { 21 | if (!target_) 22 | { 23 | PCL_ERROR ("[pcl::SampleConsensusModelNonRigid::computeModelCoefficients] No target dataset given!\n"); 24 | return (false); 25 | } 26 | // Need 3 samples 27 | if (samples.size () != 3) 28 | return (false); 29 | 30 | std::vector indices_tgt (3); 31 | for (int i = 0; i < 3; ++i) 32 | indices_tgt[i] = correspondences_[samples[i]]; 33 | 34 | estimateNonRigidTransformationSVD (*input_, samples, *target_, indices_tgt, model_coefficients); 35 | return (true); 36 | } 37 | 38 | ////////////////////////////////////////////////////////////////////////// 39 | template void 40 | pcl::SampleConsensusModelNonRigid::getDistancesToModel (const Eigen::VectorXf &model_coefficients, std::vector &distances) 41 | { 42 | if (indices_->size () != indices_tgt_->size ()) 43 | { 44 | PCL_ERROR ("[pcl::SampleConsensusModelNonRigid::getDistancesToModel] Number of source indices (%lu) differs than number of target indices (%lu)!\n", (unsigned long)indices_->size (), (unsigned long)indices_tgt_->size ()); 45 | distances.clear (); 46 | return; 47 | } 48 | if (!target_) 49 | { 50 | PCL_ERROR ("[pcl::SampleConsensusModelNonRigid::getDistanceToModel] No target dataset given!\n"); 51 | return; 52 | } 53 | // Check if the model is valid given the user constraints 54 | if (!isModelValid (model_coefficients)) 55 | { 56 | distances.clear (); 57 | return; 58 | } 59 | distances.resize (indices_->size ()); 60 | 61 | // Get the 4x4 transformation 62 | Eigen::Matrix4f transform; 63 | transform.row (0) = model_coefficients.segment<4>(0); 64 | transform.row (1) = model_coefficients.segment<4>(4); 65 | transform.row (2) = model_coefficients.segment<4>(8); 66 | transform.row (3) = model_coefficients.segment<4>(12); 67 | 68 | for (size_t i = 0; i < indices_->size (); ++i) 69 | { 70 | Eigen::Vector4f pt_src = input_->points[(*indices_)[i]].getVector4fMap (); 71 | pt_src[3] = 1; 72 | Eigen::Vector4f pt_tgt = target_->points[(*indices_tgt_)[i]].getVector4fMap (); 73 | pt_tgt[3] = 1; 74 | 75 | Eigen::Vector4f p_tr = transform * pt_src; 76 | // Calculate the distance from the transformed point to its correspondence 77 | // need to compute the real norm here to keep MSAC and friends general 78 | distances[i] = (p_tr - pt_tgt).norm (); 79 | } 80 | } 81 | 82 | ////////////////////////////////////////////////////////////////////////// 83 | template void 84 | pcl::SampleConsensusModelNonRigid::selectWithinDistance (const Eigen::VectorXf &model_coefficients, const double threshold, std::vector &inliers) 85 | { 86 | if (indices_->size () != indices_tgt_->size ()) 87 | { 88 | PCL_ERROR ("[pcl::SampleConsensusModelNonRigid::selectWithinDistance] Number of source indices (%lu) differs than number of target indices (%lu)!\n", (unsigned long)indices_->size (), (unsigned long)indices_tgt_->size ()); 89 | inliers.clear (); 90 | return; 91 | } 92 | if (!target_) 93 | { 94 | PCL_ERROR ("[pcl::SampleConsensusModelNonRigid::selectWithinDistance] No target dataset given!\n"); 95 | return; 96 | } 97 | 98 | double thresh = threshold * threshold; 99 | 100 | // Check if the model is valid given the user constraints 101 | if (!isModelValid (model_coefficients)) 102 | { 103 | inliers.clear (); 104 | return; 105 | } 106 | 107 | inliers.resize (indices_->size ()); 108 | 109 | Eigen::Matrix4f transform; 110 | transform.row (0) = model_coefficients.segment<4>(0); 111 | transform.row (1) = model_coefficients.segment<4>(4); 112 | transform.row (2) = model_coefficients.segment<4>(8); 113 | transform.row (3) = model_coefficients.segment<4>(12); 114 | 115 | int nr_p = 0; 116 | for (size_t i = 0; i < indices_->size (); ++i) 117 | { 118 | Eigen::Vector4f pt_src = input_->points[(*indices_)[i]].getVector4fMap (); 119 | pt_src[3] = 1; 120 | Eigen::Vector4f pt_tgt = target_->points[(*indices_tgt_)[i]].getVector4fMap (); 121 | pt_tgt[3] = 1; 122 | 123 | Eigen::Vector4f p_tr = transform * pt_src; 124 | // Calculate the distance from the transformed point to its correspondence 125 | if ((p_tr - pt_tgt).squaredNorm () < thresh) 126 | inliers[nr_p++] = (*indices_)[i]; 127 | } 128 | inliers.resize (nr_p); 129 | } 130 | 131 | ////////////////////////////////////////////////////////////////////////// 132 | template int 133 | pcl::SampleConsensusModelNonRigid::countWithinDistance ( 134 | const Eigen::VectorXf &model_coefficients, const double threshold) 135 | { 136 | if (indices_->size () != indices_tgt_->size ()) 137 | { 138 | PCL_ERROR ("[pcl::SampleConsensusModelNonRigid::countWithinDistance] Number of source indices (%lu) differs than number of target indices (%lu)!\n", (unsigned long)indices_->size (), (unsigned long)indices_tgt_->size ()); 139 | return (0); 140 | } 141 | if (!target_) 142 | { 143 | PCL_ERROR ("[pcl::SampleConsensusModelNonRigid::countWithinDistance] No target dataset given!\n"); 144 | return (0); 145 | } 146 | 147 | double thresh = threshold * threshold; 148 | 149 | // Check if the model is valid given the user constraints 150 | if (!isModelValid (model_coefficients)) 151 | return (0); 152 | 153 | Eigen::Matrix4f transform; 154 | transform.row (0) = model_coefficients.segment<4>(0); 155 | transform.row (1) = model_coefficients.segment<4>(4); 156 | transform.row (2) = model_coefficients.segment<4>(8); 157 | transform.row (3) = model_coefficients.segment<4>(12); 158 | 159 | int nr_p = 0; 160 | for (size_t i = 0; i < indices_->size (); ++i) 161 | { 162 | Eigen::Vector4f pt_src = input_->points[(*indices_)[i]].getVector4fMap (); 163 | pt_src[3] = 1; 164 | Eigen::Vector4f pt_tgt = target_->points[(*indices_tgt_)[i]].getVector4fMap (); 165 | pt_tgt[3] = 1; 166 | 167 | Eigen::Vector4f p_tr = transform * pt_src; 168 | // Calculate the distance from the transformed point to its correspondence 169 | if ((p_tr - pt_tgt).squaredNorm () < thresh) 170 | nr_p++; 171 | } 172 | return (nr_p); 173 | } 174 | 175 | ////////////////////////////////////////////////////////////////////////// 176 | template void 177 | pcl::SampleConsensusModelNonRigid::optimizeModelCoefficients (const std::vector &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) 178 | { 179 | if (indices_->size () != indices_tgt_->size ()) 180 | { 181 | PCL_ERROR ("[pcl::SampleConsensusModelNonRigid::optimizeModelCoefficients] Number of source indices (%lu) differs than number of target indices (%lu)!\n", (unsigned long)indices_->size (), (unsigned long)indices_tgt_->size ()); 182 | optimized_coefficients = model_coefficients; 183 | return; 184 | } 185 | 186 | // Check if the model is valid given the user constraints 187 | if (!isModelValid (model_coefficients) || !target_) 188 | { 189 | optimized_coefficients = model_coefficients; 190 | return; 191 | } 192 | 193 | std::vector indices_src (inliers.size ()); 194 | std::vector indices_tgt (inliers.size ()); 195 | for (size_t i = 0; i < inliers.size (); ++i) 196 | { 197 | // NOTE: not tested! 198 | indices_src[i] = (*indices_)[inliers[i]]; 199 | indices_tgt[i] = (*indices_tgt_)[inliers[i]]; 200 | } 201 | 202 | estimateNonRigidTransformationSVD (*input_, indices_src, *target_, indices_tgt, optimized_coefficients); 203 | } 204 | 205 | ////////////////////////////////////////////////////////////////////////// 206 | template void 207 | pcl::SampleConsensusModelNonRigid::estimateNonRigidTransformationSVD ( 208 | const pcl::PointCloud &cloud_src, 209 | const std::vector &indices_src, 210 | const pcl::PointCloud &cloud_tgt, 211 | const std::vector &indices_tgt, 212 | Eigen::VectorXf &transform) 213 | { 214 | transform.resize (16); 215 | Eigen::Vector4f centroid_src, centroid_tgt; 216 | // Estimate the centroids of source, target 217 | compute3DCentroid (cloud_src, indices_src, centroid_src); 218 | compute3DCentroid (cloud_tgt, indices_tgt, centroid_tgt); 219 | 220 | // Subtract the centroids from source, target 221 | Eigen::MatrixXf cloud_src_demean; 222 | demeanPointCloud (cloud_src, indices_src, centroid_src, cloud_src_demean); 223 | 224 | Eigen::MatrixXf cloud_tgt_demean; 225 | demeanPointCloud (cloud_tgt, indices_tgt, centroid_tgt, cloud_tgt_demean); 226 | 227 | //Scale points such that the root mean square distance from points to origin is 1 228 | Eigen::Vector4f origin(0,0,0,0); 229 | double scale_src = util::computeRMSD(cloud_src_demean, origin); 230 | double scale_tgt = util::computeRMSD(cloud_tgt_demean, origin); 231 | cloud_src_demean *= (1.0/scale_src); 232 | cloud_tgt_demean *= (1.0/scale_tgt); 233 | 234 | // Assemble the correlation matrix H = source * target' 235 | Eigen::Matrix3f H = (cloud_src_demean * cloud_tgt_demean.transpose ()).topLeftCorner<3, 3>(); 236 | 237 | // Compute the Singular Value Decomposition 238 | Eigen::JacobiSVD svd (H, Eigen::ComputeFullU | Eigen::ComputeFullV); 239 | Eigen::Matrix3f u = svd.matrixU (); 240 | Eigen::Matrix3f v = svd.matrixV (); 241 | 242 | // Compute R = V * U' 243 | if (u.determinant () * v.determinant () < 0) 244 | { 245 | for (int x = 0; x < 3; ++x) 246 | v (x, 2) *= -1; 247 | } 248 | Eigen::Matrix3f R = v * u.transpose (); 249 | 250 | // Return the correct transformation 251 | transform.segment<3> (0) = R.row (0); transform[12] = 0; 252 | transform.segment<3> (4) = R.row (1); transform[13] = 0; 253 | transform.segment<3> (8) = R.row (2); transform[14] = 0; 254 | 255 | //correct the scale 256 | transform *= (scale_tgt/scale_src); 257 | 258 | //set the correct translation 259 | Eigen::Vector3f t = centroid_tgt.head<3> () - R * centroid_src.head<3> (); 260 | transform[3] = t[0]; 261 | transform[7] = t[1]; 262 | transform[11] = t[2]; 263 | transform[15] = 1.0; 264 | } 265 | 266 | 267 | #define PCL_INSTANTIATE_SampleConsensusModelNonRigid(T) template class PCL_EXPORTS pcl::SampleConsensusModelNonRigid; 268 | 269 | #endif // sac_non_rigid_hpp_ 270 | 271 | -------------------------------------------------------------------------------- /save_correspondences.cxx: -------------------------------------------------------------------------------- 1 | #include "util.h" 2 | #include "alignment.h" 3 | using namespace std; 4 | //------------------------------------ 5 | //--- Save Correspondences Main ------ 6 | //------------------------------------ 7 | int main(int argc, char* argv[]) 8 | { 9 | if(argc < 4) { 10 | cout<<"usage: ./icp cloud0.ply cloud1.ply corrFile.txt"<(srcPFH, tgtPFH); 26 | cout<<"Saving "<size()<<" correspondences"< 4 | #include 5 | #include 6 | #include 7 | 8 | 9 | //compute RMSD for eigen matrix represented point clouds 10 | double util::computeRMSD(const Eigen::MatrixXf& cloud, const Eigen::Vector4f& pt) 11 | { 12 | size_t npts = cloud.cols(); 13 | if(cloud.rows() != 4) { 14 | cout<<"Cloud must be 4xNumPoints!"<(0,i); 21 | double distSqr = (pt[0] - cpt[0])*(pt[0] - cpt[0]) + 22 | (pt[1] - cpt[1])*(pt[1] - cpt[1]) + 23 | (pt[2] - cpt[2])*(pt[2] - cpt[2]); 24 | distSqrSum+=distSqr; 25 | } 26 | return sqrt(distSqrSum/npts); 27 | } 28 | 29 | //Trim clouds down to the correspondences 30 | void util::trimClouds(PointCloud::Ptr A, PointCloud::Ptr B, pcl::CorrespondencesPtr corrs, PointCloud::Ptr Atrim, PointCloud::Ptr Btrim) 31 | { 32 | for(int i=0; isize(); ++i) { 33 | PointXYZ& ptA = A->at( corrs->at(i).index_query ); 34 | PointXYZ& ptB = B->at( corrs->at(i).index_match ); 35 | Atrim->push_back(ptA); 36 | Btrim->push_back(ptB); 37 | } 38 | } 39 | 40 | 41 | //load cloud into pointer - does PCD and PLY files 42 | util::PointCloud::Ptr util::loadCloud(const string& name) 43 | { 44 | string ext = name.substr(name.find_last_of(".")+1); 45 | PointCloud::Ptr cloud(new PointCloud()); 46 | if(ext == "ply") { 47 | pcl::PLYReader reader; 48 | reader.read(name, *cloud); 49 | } 50 | else if(ext=="pcd"){ 51 | pcl::io::loadPCDFile(name, *cloud); 52 | } 53 | return cloud; 54 | } 55 | 56 | //save ply or pcd files 57 | void util::saveCloud(const string& name, const util::PointCloud& cloud) 58 | { 59 | string ext = name.substr(name.find_last_of(".")+1); 60 | if(ext == "ply") 61 | pcl::io::savePLYFile(name, cloud); 62 | else if(ext=="pcd") 63 | pcl::io::savePCDFile(name, cloud); 64 | } 65 | 66 | //show two clouds 67 | void util::visualizeClouds(PointCloud::Ptr A, PointCloud::Ptr B, pcl::CorrespondencesPtr corrs, int numShow) 68 | { 69 | //--- visualize transformed cloud. 70 | ColorCloud::Ptr Acolor(new ColorCloud()); 71 | PointCloud::const_iterator aIter = A->begin(); 72 | for(aIter = A->begin(); aIter != A->end(); ++aIter){ 73 | PointXYZRGB pt; 74 | pt.x = aIter->x; 75 | pt.y = aIter->y; 76 | pt.z = aIter->z; 77 | uint8_t r = 255, g = 0, b = 0; // Example: Red color 78 | uint32_t rgb = ((uint32_t)r << 16 | (uint32_t)g << 8 | (uint32_t)b); 79 | pt.rgb = *reinterpret_cast(&rgb); 80 | Acolor->push_back(pt); 81 | } 82 | 83 | //b cloud 84 | ColorCloud::Ptr Bcolor(new ColorCloud()); 85 | PointCloud::const_iterator bIter = B->begin(); 86 | for(bIter = B->begin(); bIter != B->end(); ++bIter){ 87 | PointXYZRGB pt; 88 | pt.x = bIter->x; 89 | pt.y = bIter->y; 90 | pt.z = bIter->z; 91 | uint8_t r =0, g = 0, b = 255; // Example:Blue color 92 | uint32_t rgb = ((uint32_t)r << 16 | (uint32_t)g << 8 | (uint32_t)b); 93 | pt.rgb = *reinterpret_cast(&rgb); 94 | Bcolor->push_back(pt); 95 | } 96 | 97 | // -------------------------------------------- 98 | // -----Open 3D viewer and add point cloud----- 99 | // -------------------------------------------- 100 | pcl::visualization::PCLVisualizer viewer("3D Viewer"); 101 | viewer.setBackgroundColor (0, 0, 0); 102 | pcl::visualization::PointCloudColorHandlerRGBField rgb(Acolor), rgb2(Bcolor); 103 | viewer.addPointCloud (Acolor, rgb, "sample cloud"); 104 | viewer.addPointCloud (Bcolor, rgb2, "cloud 2"); 105 | viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud"); 106 | viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "cloud 2"); 107 | viewer.addCoordinateSystem (1.0); 108 | viewer.initCameraParameters (); 109 | 110 | //------------------------------------ 111 | //-----Add shapes at cloud points----- 112 | //------------------------------------ 113 | //paint the first few corrs 114 | if(corrs.get() != NULL && numShow > 0) { 115 | numShow = min((int) corrs->size(), numShow); 116 | int count=0; 117 | for(int i=0; isize(); ++i) { 118 | pcl::Correspondence c = (*corrs)[i]; 119 | if(c.distance < .01) 120 | continue; 121 | 122 | uint8_t r = 0, g = 255, b = 0; 123 | uint32_t rgb = ((uint32_t)r << 16 | (uint32_t)g << 8 | (uint32_t)b); 124 | PointXYZRGB& ptA = Acolor->at(c.index_query); 125 | PointXYZRGB& ptB = Bcolor->at(c.index_match); 126 | ptA.rgb = *reinterpret_cast(&rgb); 127 | ptB.rgb = *reinterpret_cast(&rgb); 128 | viewer.addLine (ptA, ptB, "line"+count); 129 | 130 | count++; 131 | if(count > numShow) break; 132 | } 133 | } 134 | 135 | //pcl::visualization::CloudViewer viewer("Simple Cloud Viewer"); 136 | //viewer.showCloud(Acolor, "cloud A"); 137 | //viewer.showCloud(Bcolor, "cloud B"); 138 | //-------------------- 139 | // -----Main loop----- 140 | //-------------------- 141 | while (!viewer.wasStopped ()) 142 | { 143 | viewer.spinOnce (100); 144 | //boost::this_thread::sleep (boost::posix_time::microseconds (100000)); 145 | } 146 | } 147 | 148 | 149 | 150 | //String split helper methods.... 151 | vector& util::split(const string &s, char delim, vector &elems) { 152 | stringstream ss(s); 153 | string item; 154 | while(getline(ss, item, delim)) { 155 | elems.push_back(item); 156 | } 157 | return elems; 158 | } 159 | vector util::split(const string &s, char delim) { 160 | vector elems; 161 | return split(s, delim, elems); 162 | } 163 | 164 | //load correspondences from file - comma separated 165 | pcl::CorrespondencesPtr util::loadCorrespondences(const string& name) 166 | { 167 | pcl::CorrespondencesPtr corrs(new pcl::Correspondences); 168 | 169 | //load from file 170 | ifstream file(name.c_str()); 171 | if (file.is_open()) 172 | { 173 | string line; 174 | while ( file.good() ) { 175 | getline (file,line); 176 | vector strNums = util::split(line); 177 | if(strNums.size()<3) 178 | continue; 179 | int index_query = util::fromString(strNums[0]); 180 | int index_match = util::fromString(strNums[1]); 181 | float distance = util::fromString(strNums[2]); 182 | pcl::Correspondence corr(index_query, index_match, distance); 183 | corrs->push_back(corr); 184 | } 185 | file.close(); 186 | } 187 | else { 188 | cout << "Unable to open correspondence file "<size(); ++i) { 200 | file << corrs->at(i) << '\n'; 201 | } 202 | file.close(); 203 | } 204 | -------------------------------------------------------------------------------- /util.h: -------------------------------------------------------------------------------- 1 | #ifndef util_h_ 2 | #define util_h_ 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | using namespace std; 9 | 10 | //--- Utility functions ----- 11 | namespace util { 12 | 13 | //xyz point and xyz cloud 14 | typedef pcl::PointXYZ PointXYZ; 15 | typedef pcl::PointXYZRGB PointXYZRGB; 16 | typedef pcl::PointCloud PointCloud; 17 | typedef pcl::PointCloud ColorCloud; 18 | 19 | //trim clouds down to just the correspondences given 20 | void trimClouds(PointCloud::Ptr A, 21 | PointCloud::Ptr B, 22 | pcl::CorrespondencesPtr corrs, 23 | PointCloud::Ptr Atrim, 24 | PointCloud::Ptr Btrim); 25 | 26 | //cloud IO wrappers. Visualizer wrapper 27 | PointCloud::Ptr loadCloud(const string& name); 28 | void saveCloud(const string& name, const PointCloud& cloud); 29 | void visualizeClouds(PointCloud::Ptr A, 30 | PointCloud::Ptr B, 31 | pcl::CorrespondencesPtr corrs=pcl::CorrespondencesPtr(), 32 | int numShow = 5); 33 | 34 | //correspondences IO wrapper 35 | pcl::CorrespondencesPtr loadCorrespondences(const string& name); 36 | void saveCorrespondences(const string& name, pcl::CorrespondencesPtr); 37 | 38 | //computer RMSD of cloud 39 | template 40 | double computeRMSD(const pcl::PointCloud& cloud, const PointT& pt); 41 | template 42 | double computeRMSD(const pcl::PointCloud& cloud, const vector& indices, const PointT& pt); 43 | double computeRMSD(const Eigen::MatrixXf& cloud, const Eigen::Vector4f& pt); 44 | 45 | //pcl point distances - these definitely exist somewhere 46 | inline double distance2(const PointXYZ& a, const PointXYZ& b) { 47 | return (a.x-b.x)*(a.x-b.x)+(a.y-b.y)*(a.y-b.y)+(a.z-b.z)*(a.z-b.z); 48 | } 49 | inline double distance(const PointXYZ& a, const PointXYZ& b) { return sqrt(distance2(a,b)); } 50 | 51 | //---- String methods that I probably should not have had to implement ---- 52 | //convert type from string 53 | template 54 | T fromString(const string& s); 55 | 56 | //split string into vector of strings 57 | vector split(const string& s, char delim=' '); 58 | vector& split(const string &s, char delim, vector &elems); 59 | }; 60 | 61 | template 62 | double util::computeRMSD(const pcl::PointCloud& cloud, const PointT& pt) 63 | { 64 | double distSqrSum = 0.0; 65 | for(int i=0; i 71 | double util::computeRMSD(const pcl::PointCloud& cloud, const vector& indices, const PointT& pt) 72 | { 73 | double distSqrSum = 0.0; 74 | for(int i=0; i 83 | T util::fromString(const std::string& s) 84 | { 85 | try { 86 | istringstream stream (s); 87 | T t; 88 | stream >> t; 89 | return t; 90 | } 91 | catch(int e){ 92 | cout<<"Cannot convert string "< 3 | #include 4 | #include 5 | #include 6 | using namespace std; 7 | int main(int argc, char* argv[]) 8 | { 9 | if(argc < 3) { 10 | cout<<"usage: ./show_clouds cloud1.pcd cloud2.pcd"<= 4) { 17 | corrs = util::loadCorrespondences(argv[3]); 18 | sort(corrs->begin(), corrs->end(), pcl::isBetterCorrespondence); 19 | reverse(corrs->begin(), corrs->end()); 20 | } 21 | //load num to show if applicable 22 | int numShow = 0; 23 | if(argc >= 5) 24 | numShow = util::fromString(argv[4]); 25 | 26 | //visualize with correspondences 27 | pcl::PointCloud::Ptr irCloud = util::loadCloud(argv[1]); 28 | pcl::PointCloud::Ptr eoCloud = util::loadCloud(argv[2]); 29 | cout<<"red Cloud: "<