template <typename PointT> void pcl::registration::CorrespondenceRejectorSampleConsensus<PointT>::getRemainingCorrespondences ( const pcl::Correspondences& original_correspondences, pcl::Correspondences& remaining_correspondences) { if (!input_) { PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] No input cloud dataset was given!\n", getClassName ().c_str ()); return; } if (!target_) { PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] No input target dataset was given!\n", getClassName ().c_str ()); return; } if (save_inliers_) inlier_indices_.clear (); int nr_correspondences = static_cast<int> (original_correspondences.size ()); std::vector<int> source_indices (nr_correspondences); std::vector<int> target_indices (nr_correspondences); // Copy the query-match indices for (size_t i = 0; i < original_correspondences.size (); ++i) { source_indices[i] = original_correspondences[i].index_query; target_indices[i] = original_correspondences[i].index_match; } // from pcl/registration/icp.hpp: std::vector<int> source_indices_good; std::vector<int> target_indices_good; { // From the set of correspondences found, attempt to remove outliers // Create the registration model typedef typename pcl::SampleConsensusModelRegistration<PointT>::Ptr SampleConsensusModelRegistrationPtr; SampleConsensusModelRegistrationPtr model; model.reset (new pcl::SampleConsensusModelRegistration<PointT> (input_, source_indices)); // Pass the target_indices model->setInputTarget (target_, target_indices); // Create a RANSAC model pcl::RandomSampleConsensus<PointT> sac (model, inlier_threshold_); sac.setMaxIterations (max_iterations_); // Compute the set of inliers if (!sac.computeModel ()) { remaining_correspondences = original_correspondences; best_transformation_.setIdentity (); return; } else { if (refine_ && !sac.refineModel ()) { PCL_ERROR ("[pcl::registration::CorrespondenceRejectorSampleConsensus::getRemainingCorrespondences] Could not refine the model! Returning an empty solution.\n"); return; } std::vector<int> inliers; sac.getInliers (inliers); if (inliers.size () < 3) { remaining_correspondences = original_correspondences; best_transformation_.setIdentity (); return; } boost::unordered_map<int, int> index_to_correspondence; for (int i = 0; i < nr_correspondences; ++i) index_to_correspondence[original_correspondences[i].index_query] = i; remaining_correspondences.resize (inliers.size ()); for (size_t i = 0; i < inliers.size (); ++i) remaining_correspondences[i] = original_correspondences[index_to_correspondence[inliers[i]]]; if (save_inliers_) { inlier_indices_.reserve (inliers.size ()); for (const int &inlier : inliers) inlier_indices_.push_back (index_to_correspondence[inlier]); } // get best transformation Eigen::VectorXf model_coefficients; sac.getModelCoefficients (model_coefficients); best_transformation_.row (0) = model_coefficients.segment<4>(0); best_transformation_.row (1) = model_coefficients.segment<4>(4); best_transformation_.row (2) = model_coefficients.segment<4>(8); best_transformation_.row (3) = model_coefficients.segment<4>(12); } } }
template <typename PointSource, typename PointTarget> void pcl::IterativeClosestPoint<PointSource, PointTarget>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess) { // Allocate enough space to hold the results std::vector<int> nn_indices (1); std::vector<float> nn_dists (1); // Point cloud containing the correspondences of each point in <input, indices> PointCloudTarget input_corresp; input_corresp.points.resize (indices_->size ()); nr_iterations_ = 0; converged_ = false; double dist_threshold = corr_dist_threshold_ * corr_dist_threshold_; // If the guessed transformation is non identity if (guess != Eigen::Matrix4f::Identity ()) { // Initialise final transformation to the guessed one final_transformation_ = guess; // Apply guessed transformation prior to search for neighbours transformPointCloud (output, output, guess); } // Resize the vector of distances between correspondences std::vector<float> previous_correspondence_distances (indices_->size ()); correspondence_distances_.resize (indices_->size ()); while (!converged_) // repeat until convergence { // Save the previously estimated transformation previous_transformation_ = transformation_; // And the previous set of distances previous_correspondence_distances = correspondence_distances_; int cnt = 0; std::vector<int> source_indices (indices_->size ()); std::vector<int> target_indices (indices_->size ()); // Iterating over the entire index vector and find all correspondences for (size_t idx = 0; idx < indices_->size (); ++idx) { if (!this->searchForNeighbors (output, (*indices_)[idx], nn_indices, nn_dists)) { PCL_ERROR ("[pcl::%s::computeTransformation] Unable to find a nearest neighbor in the target dataset for point %d in the source!\n", getClassName ().c_str (), (*indices_)[idx]); return; } // Check if the distance to the nearest neighbor is smaller than the user imposed threshold if (nn_dists[0] < dist_threshold) { source_indices[cnt] = (*indices_)[idx]; target_indices[cnt] = nn_indices[0]; cnt++; } // Save the nn_dists[0] to a global vector of distances correspondence_distances_[(*indices_)[idx]] = std::min (nn_dists[0], static_cast<float> (dist_threshold)); } if (cnt < min_number_correspondences_) { PCL_ERROR ("[pcl::%s::computeTransformation] Not enough correspondences found. Relax your threshold parameters.\n", getClassName ().c_str ()); converged_ = false; return; } // Resize to the actual number of valid correspondences source_indices.resize (cnt); target_indices.resize (cnt); std::vector<int> source_indices_good; std::vector<int> target_indices_good; { // From the set of correspondences found, attempt to remove outliers // Create the registration model typedef typename SampleConsensusModelRegistration<PointSource>::Ptr SampleConsensusModelRegistrationPtr; SampleConsensusModelRegistrationPtr model; model.reset (new SampleConsensusModelRegistration<PointSource> (output.makeShared (), source_indices)); // Pass the target_indices model->setInputTarget (target_, target_indices); // Create a RANSAC model RandomSampleConsensus<PointSource> sac (model, inlier_threshold_); sac.setMaxIterations (ransac_iterations_); // Compute the set of inliers if (!sac.computeModel ()) { source_indices_good = source_indices; target_indices_good = target_indices; } else { std::vector<int> inliers; // Get the inliers sac.getInliers (inliers); source_indices_good.resize (inliers.size ()); target_indices_good.resize (inliers.size ()); boost::unordered_map<int, int> source_to_target; for (unsigned int i = 0; i < source_indices.size(); ++i) source_to_target[source_indices[i]] = target_indices[i]; // Copy just the inliers std::copy(inliers.begin(), inliers.end(), source_indices_good.begin()); for (size_t i = 0; i < inliers.size (); ++i) target_indices_good[i] = source_to_target[inliers[i]]; } } // Check whether we have enough correspondences cnt = static_cast<int> (source_indices_good.size ()); if (cnt < min_number_correspondences_) { PCL_ERROR ("[pcl::%s::computeTransformation] Not enough correspondences found. Relax your threshold parameters.\n", getClassName ().c_str ()); converged_ = false; return; } PCL_DEBUG ("[pcl::%s::computeTransformation] Number of correspondences %d [%f%%] out of %zu points [100.0%%], RANSAC rejected: %zu [%f%%].\n", getClassName ().c_str (), cnt, (static_cast<float> (cnt) * 100.0f) / static_cast<float> (indices_->size ()), indices_->size (), source_indices.size () - cnt, static_cast<float> (source_indices.size () - cnt) * 100.0f / static_cast<float> (source_indices.size ())); // Estimate the transform //rigid_transformation_estimation_(output, source_indices_good, *target_, target_indices_good, transformation_); transformation_estimation_->estimateRigidTransformation (output, source_indices_good, *target_, target_indices_good, transformation_); // Tranform the data transformPointCloud (output, output, transformation_); // Obtain the final transformation final_transformation_ = transformation_ * final_transformation_; nr_iterations_++; // Update the vizualization of icp convergence if (update_visualizer_ != 0) update_visualizer_(output, source_indices_good, *target_, target_indices_good ); // Various/Different convergence termination criteria // 1. Number of iterations has reached the maximum user imposed number of iterations (via // setMaximumIterations) // 2. The epsilon (difference) between the previous transformation and the current estimated transformation // is smaller than an user imposed value (via setTransformationEpsilon) // 3. The sum of Euclidean squared errors is smaller than a user defined threshold (via // setEuclideanFitnessEpsilon) if (nr_iterations_ >= max_iterations_ || (transformation_ - previous_transformation_).array ().abs ().sum () < transformation_epsilon_ || fabs (this->getFitnessScore (correspondence_distances_, previous_correspondence_distances)) <= euclidean_fitness_epsilon_ ) { converged_ = true; PCL_DEBUG ("[pcl::%s::computeTransformation] Convergence reached. Number of iterations: %d out of %d. Transformation difference: %f\n", getClassName ().c_str (), nr_iterations_, max_iterations_, (transformation_ - previous_transformation_).array ().abs ().sum ()); PCL_DEBUG ("nr_iterations_ (%d) >= max_iterations_ (%d)\n", nr_iterations_, max_iterations_); PCL_DEBUG ("(transformation_ - previous_transformation_).array ().abs ().sum () (%f) < transformation_epsilon_ (%f)\n", (transformation_ - previous_transformation_).array ().abs ().sum (), transformation_epsilon_); PCL_DEBUG ("fabs (getFitnessScore (correspondence_distances_, previous_correspondence_distances)) (%f) <= euclidean_fitness_epsilon_ (%f)\n", fabs (this->getFitnessScore (correspondence_distances_, previous_correspondence_distances)), euclidean_fitness_epsilon_); } } }
template <typename PointT> void pcl::registration::CorrespondenceRejectorProcrustes<PointT>::getRemainingCorrespondences ( const pcl::Correspondences& original_correspondences, pcl::Correspondences& remaining_correspondences) { int nr_correspondences = (int)original_correspondences.size (); std::vector<int> source_indices (nr_correspondences); std::vector<int> target_indices (nr_correspondences); // Copy the query-match indices for (size_t i = 0; i < original_correspondences.size (); ++i) { source_indices[i] = original_correspondences[i].index_query; target_indices[i] = original_correspondences[i].index_match; } // from pcl/registration/icp.hpp: std::vector<int> source_indices_good; std::vector<int> target_indices_good; { // From the set of correspondences found, attempt to remove outliers // Create the registration model typedef typename pcl::SampleConsensusModelNonRigid<PointT>::Ptr SampleConsensusModelNonRigidPtr; SampleConsensusModelNonRigidPtr model; model.reset (new pcl::SampleConsensusModelNonRigid<PointT> (input_, source_indices)); // Pass the target_indices model->setInputTarget (target_, target_indices); // Create a RANSAC model pcl::RandomSampleConsensus<PointT> sac (model, inlier_threshold_); sac.setMaxIterations (max_iterations_); // Compute the set of inliers if (!sac.computeModel ()) { remaining_correspondences = original_correspondences; best_transformation_.setIdentity (); return; } else { std::vector<int> inliers; sac.getInliers (inliers); if (inliers.size () < 3) { remaining_correspondences = original_correspondences; best_transformation_.setIdentity (); return; } boost::unordered_map<int, int> index_to_correspondence; for (int i = 0; i < nr_correspondences; ++i) index_to_correspondence[original_correspondences[i].index_query] = i; remaining_correspondences.resize (inliers.size ()); for (size_t i = 0; i < inliers.size (); ++i) remaining_correspondences[i] = original_correspondences[index_to_correspondence[inliers[i]]]; // get best transformation Eigen::VectorXf model_coefficients; sac.getModelCoefficients (model_coefficients); best_transformation_.row (0) = model_coefficients.segment<4>(0); best_transformation_.row (1) = model_coefficients.segment<4>(4); best_transformation_.row (2) = model_coefficients.segment<4>(8); best_transformation_.row (3) = model_coefficients.segment<4>(12); } } }
void IterativeClosestPoint<PointSource, PointTarget>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess) { // Allocate enough space to hold the results nr_iterations_ = 0; converged_ = false; output_ = &output; assert(target_->points.size() && output.points.size()); assert(wInput_.size() && wOutput_.size()); float emd_change; int flowSize = wInput_.size() + wOutput_.size() - 1; flow_ = new flow_t[flowSize]; std::vector<int*> samples; // samples.push_back const int RAND_NUM = 3; const int RAND_TOTAL = 6; int a[] = { 0, 1, 2}; int b[] = { 0, 2, 1}; int c[] = { 1, 0, 2}; int d[] = { 1, 2, 0}; int e[] = { 2, 1, 0}; int f[] = { 2, 0, 1}; samples.push_back(a); samples.push_back(b); samples.push_back(c); samples.push_back(d); samples.push_back(e); samples.push_back(f); std::vector<pair<float,int>> vecOutput; std::vector<pair<float,int>> spOutput; std::sort(wInput_.begin(), wInput_.end(), std::greater<float>()); std::sort(wOutput_.begin(), wOutput_.end(), std::greater<float>()); int flag = 0; PointCloudPtr pos(new pcl::PointCloud<PointT>); for (auto &sample: samples) { /// @todo to interate over all possible case /// rather than sample 100 times /// reset condition bool stop = false; nr_iterations_ = 0; emd_change = 10000; /// random sample initial correspondences /// sampled first MINIMUM_CRSP elements in #output myCorrespondence_.clear(); const int MINIMUM_CRSP = 3; // copy the first MINIMUM_CRSP elements in #output for (int i = 0; i < RAND_NUM; i++) { myCorrespondence_.insert(BiValue(i, sample[i])); } assert(myCorrespondence_.size() == MINIMUM_CRSP); // std::cout<<"*************correspondences*****"<<std::endl; // m_util::print_map(myCorrespondence_); // std::cout<<"*********************************"<<std::endl; /// repeat until convergence while (!stop) { /// if converged if (emd_change < 0.001) { stop = true; converged_ = true; break; } if (nr_iterations_ >= max_iterations_ ) { stop = true; break; } /// transform the points int cnt = myCorrespondence_.size(); // ignore this sample if (cnt < min_number_correspondences_) break; std::vector<int> source_indices (cnt); // output std::vector<int> target_indices (cnt); // input int idx = 0; for(auto v : myCorrespondence_.left) { source_indices[idx] = v.second; target_indices[idx] = v.first; idx++; } // Estimate the transform transformation_estimation_->estimateRigidTransformation (output, source_indices, *target_, target_indices, transformation_); // Tranform the data transformPointCloud (output, output, transformation_); // Obtain the final transformation final_transformation_ = transformation_ * final_transformation_; nr_iterations_++; /// calcuate EMD distance for (int i = 0; i < flowSize; i++) { flow_[i].amount = 0; } float e = emd_wapper(target_->points, wInput_, output.points, wOutput_, flow_, dist); emd_change = abs(e - pre_EMD_); pre_EMD_ = e; /// set up the new correspondences // sort the flow according to the weight std::stable_sort(flow_, flow_ + flowSize); // I don't know why #std::sort cause a bug here, // It would modify #from or #to member of flow_t struct // std::sort(flow_, flow_ + flowSize); myCorrespondence_.clear(); for (int i = 0; i < flowSize; i++) { myCorrespondence_.insert(BiValue(flow_[i].from, flow_[i].to)); // maximum number of correspondences /// @todo arguments if(myCorrespondence_.size() == 5) break; } // std::cout<<"*********************************"<<std::endl; // for (int i = 0; i < flowSize; i++) { // std::cout<<flow_[i].from<<" "<<flow_[i].to<<" "<<flow_[i].amount<<std::endl; // } // std::cout<<"***********----------************"<<std::endl; // for(auto p : myCorrespondence_.left){ // std::cout<<p.first<<" "<<p.second<<std::endl; // } // std::cout<<myCorrespondence_.size()<<std::endl; // assert(myCorrespondence_.size() >= MINIMUM_CRSP); } if(pre_EMD_ < best_EMD_) { if (visualize_EMD) pcl::copyPointCloud(output, *pos); best_EMD_ = pre_EMD_; } } if (visualize_EMD && pos->points.size() > 0) pcl::copyPointCloud(*pos, output); }
template <typename PointT> void pcl::registration::CorrespondenceRejectorSampleConsensus2D<PointT>::getRemainingCorrespondences ( const pcl::Correspondences& original_correspondences, pcl::Correspondences& remaining_correspondences) { if (!input_) { PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] No input cloud dataset was given!\n", getClassName ().c_str ()); return; } if (!target_) { PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] No input target dataset was given!\n", getClassName ().c_str ()); return; } if (projection_matrix_ == Eigen::Matrix3f::Identity ()) { PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] Intrinsic camera parameters not given!\n", getClassName ().c_str ()); return; } int nr_correspondences = static_cast<int> (original_correspondences.size ()); std::vector<int> source_indices (nr_correspondences); std::vector<int> target_indices (nr_correspondences); // Copy the query-match indices for (size_t i = 0; i < original_correspondences.size (); ++i) { source_indices[i] = original_correspondences[i].index_query; target_indices[i] = original_correspondences[i].index_match; } // from pcl/registration/icp.hpp: std::vector<int> source_indices_good; std::vector<int> target_indices_good; // From the set of correspondences found, attempt to remove outliers typename pcl::SampleConsensusModelRegistration2D<PointT>::Ptr model (new pcl::SampleConsensusModelRegistration2D<PointT> (input_, source_indices)); // Pass the target_indices model->setInputTarget (target_, target_indices); model->setProjectionMatrix (projection_matrix_); // Create a RANSAC model pcl::RandomSampleConsensus<PointT> sac (model, inlier_threshold_); sac.setMaxIterations (max_iterations_); // Compute the set of inliers if (!sac.computeModel ()) { PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] Error computing model! Returning the original correspondences...\n", getClassName ().c_str ()); remaining_correspondences = original_correspondences; best_transformation_.setIdentity (); return; } else { if (refine_ && !sac.refineModel (2.0)) PCL_WARN ("[pcl::registration::%s::getRemainingCorrespondences] Error refining model!\n", getClassName ().c_str ()); std::vector<int> inliers; sac.getInliers (inliers); if (inliers.size () < 3) { PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] Less than 3 correspondences found!\n", getClassName ().c_str ()); remaining_correspondences = original_correspondences; best_transformation_.setIdentity (); return; } boost::unordered_map<int, int> index_to_correspondence; for (int i = 0; i < nr_correspondences; ++i) index_to_correspondence[original_correspondences[i].index_query] = i; remaining_correspondences.resize (inliers.size ()); for (size_t i = 0; i < inliers.size (); ++i) remaining_correspondences[i] = original_correspondences[index_to_correspondence[inliers[i]]]; // get best transformation Eigen::VectorXf model_coefficients; sac.getModelCoefficients (model_coefficients); best_transformation_.row (0) = model_coefficients.segment<4>(0); best_transformation_.row (1) = model_coefficients.segment<4>(4); best_transformation_.row (2) = model_coefficients.segment<4>(8); best_transformation_.row (3) = model_coefficients.segment<4>(12); } }
typename PointCloud<PointT>::Ptr transform( const typename PointCloud<PointT>::Ptr source, const typename PointCloud<PointT>::Ptr target,boost::shared_ptr<Configuration> configuration) { source_cloud = *source; target_cloud = *target; cout << "[PFHTransformStrategy::transform] Setting source cloud: " << source->size() << " points" << endl; cout << "[PFHTransformStrategy::transform] Setting target cloud: " << target->size() << " points" << endl; typename PointCloud<PointT>::Ptr transformed(new PointCloud<PointT>); typename PointCloud<PointXYZRGB>::Ptr source_points(source); typename PointCloud<PointXYZRGB>::Ptr source_filtered( new PointCloud<PointT>); PointCloud<Normal>::Ptr source_normals(new PointCloud<Normal>); PointCloud<PointWithScale>::Ptr source_keypoints( new PointCloud<PointWithScale>); PointCloud<PFHSignature125>::Ptr source_descriptors( new PointCloud<PFHSignature125>); typename PointCloud<PointT>::Ptr target_points(target); typename PointCloud<PointT>::Ptr target_filtered( new PointCloud<PointT>); PointCloud<Normal>::Ptr target_normals( new PointCloud<Normal>); PointCloud<PointWithScale>::Ptr target_keypoints( new PointCloud<PointWithScale>); PointCloud<PFHSignature125>::Ptr target_descriptors( new PointCloud<PFHSignature125>); cout << "[PFHTransformStrategy::transform] Downsampling source and " << "target clouds..." << endl; //filter(source_points, source_filtered, 0.01f); //filter(target_points, target_filtered, 0.01f); source_filtered=source_points; target_filtered=target_points; cout << "[PFHTransformStrategy::transform] Creating normals for " << "source and target cloud..." << endl; create_normals<Normal>(source_filtered, source_normals); create_normals<Normal>(target_filtered, target_normals); cout << "[PFHTransformStrategy::transform] Finding keypoints in " << "source and target cloud..." << endl; detect_keypoints(source_filtered, source_keypoints); detect_keypoints(target_filtered, target_keypoints); for(PointWithScale p: source_keypoints->points){ cout<<"keypoint "<<p; } vector<int> source_indices(source_keypoints->points.size()); vector<int> target_indices(target_keypoints->points.size()); cout << "[PFHTransformStrategy::transform] Computing PFH features " << "for source and target cloud..." << endl; compute_PFH_features_at_keypoints(source_filtered, source_normals,source_keypoints, source_descriptors, target_indices); compute_PFH_features_at_keypoints(target_filtered, target_normals,target_keypoints, target_descriptors, target_indices); vector<int> correspondences; vector<float> correspondence_scores; find_feature_correspondence(source_descriptors, target_descriptors, correspondences, correspondence_scores); cout << "correspondences: " << correspondences.size() << endl; cout << "c. scores: " << correspondence_scores.size() << endl; cout << "First cloud: Found " << source_keypoints->size() << " keypoints " << "out of " << source_filtered->size() << " total points." << endl; cout << "Second cloud: Found " << target_keypoints->size() << " keypoints" << " out of " << target_filtered->size() << " total points." << endl; // Start with the actual transformation. Yeay :) TransformationFromCorrespondences tfc; tfc.reset(); vector<int> sorted_scores; cv::sortIdx(correspondence_scores, sorted_scores, CV_SORT_EVERY_ROW); vector<float> tmp(correspondence_scores); sort(tmp.begin(), tmp.end()); float median_score = tmp[tmp.size() / 2]; vector<int> fidx; vector<int> fidxt; Eigen::Vector3f source_position(0, 0, 0); Eigen::Vector3f target_position(0, 0, 0); for (size_t i = 0; i < correspondence_scores.size(); i++) { int index = sorted_scores[i]; if (median_score >= correspondence_scores[index]) { source_position[0] = source_keypoints->points[index].x; source_position[1] = source_keypoints->points[index].y; source_position[2] = source_keypoints->points[index].z; target_position[0] = target_keypoints->points[index].x; target_position[1] = target_keypoints->points[index].y; target_position[2] = target_keypoints->points[index].z; if (abs(source_position[1] - target_position[1]) > 0.2) { continue; } // cout<< "abs position difference:"<<abs(source_position[1] - target_position[1])<<"C-Score"<<correspondence_scores[index]<<endl; // cout<<" Source Position " <<source_position<<endl; // cout<<" target position "<<target_position<<endl; tfc.add(source_position, target_position, correspondence_scores[index]); fidx.push_back(source_indices[index]); fidxt.push_back(target_indices[correspondences[index]]); } } cout << "TFC samples: "<<tfc.getNoOfSamples()<<" AccumulatedWeight "<<tfc.getAccumulatedWeight()<<endl; Eigen::Affine3f tr; tr = tfc.getTransformation(); cout << "TFC transformation: " << endl; for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { cout << tr.rotation()(i, i) << "\t"; } cout << endl; } transformPointCloud(*source_filtered, *transformed, tfc.getTransformation()); cout << "transformation finished"; return transformed; };