void PosesFromMatches::estimatePosesUsing1Correspondence (const PointCorrespondences6DVector& correspondences, int max_no_of_results, PosesFromMatches::PoseEstimatesVector& pose_estimates) const { //MEASURE_FUNCTION_TIME; //int initial_no_of_pose_estimates = pose_estimates.size (); max_no_of_results = std::min (max_no_of_results, (int)correspondences.size ()); for (int correspondence_idx=0; correspondence_idx<max_no_of_results; ++correspondence_idx) { const PointCorrespondence6D& correspondence = correspondences[correspondence_idx]; PoseEstimate pose_estimate; pose_estimate.transformation = correspondence.transformation; pose_estimate.score = correspondence.score; pose_estimate.correspondence_indices.push_back (correspondence_idx); pose_estimates.push_back (pose_estimate); } // std::cout << "Extracted "<<pose_estimates.size ()-initial_no_of_pose_estimates<<" poses. // Set of pose estimates now has size "<<pose_estimates.size ()<<"\n"; }
void pcl::PosesFromMatches::estimatePosesUsing3Correspondences (const PointCorrespondences6DVector& correspondences, int max_no_of_tested_combinations, int max_no_of_results, PosesFromMatches::PoseEstimatesVector& pose_estimates) const { const Eigen::Vector3f x_direction (1.0f, 0.0f, 0.0f), y_direction (0.0f, 1.0f, 0.0f), z_direction (0.0f, 0.0f, 1.0f); int max_correspondence_idx = static_cast<int> (correspondences.size ()); int counter_for_tested_combinations = 0, counter_for_added_pose_estimates = 0; float max_distance_quotient = 1.0f+parameters_.max_correspondence_distance_error, max_distance_quotient_squared = std::pow (max_distance_quotient, 2), min_distance_quotient = 1.0f / (max_distance_quotient), min_distance_quotient_squared = std::pow (min_distance_quotient, 2); pcl::TransformationFromCorrespondences transformation_from_correspondeces; // The following loop structure goes through the triples in the order 123, 124, 134, 234, 125, 135, 235, ..., // testing the best correspondences triples first, without beeing stuck too long with one specific // (possibly wrong) correspondence. bool done = false; for (int correspondence3_idx = 0; correspondence3_idx < max_correspondence_idx && !done; ++correspondence3_idx) { const pcl::PointCorrespondence6D& correspondence3 = correspondences[correspondence3_idx]; const Eigen::Vector3f& point3 = correspondence3.point1, & corr3 = correspondence3.point2; for (int correspondence2_idx = 0; correspondence2_idx < correspondence3_idx && !done; ++correspondence2_idx) { const pcl::PointCorrespondence6D& correspondence2 = correspondences[correspondence2_idx]; const Eigen::Vector3f& point2 = correspondence2.point1, & corr2 = correspondence2.point2; float distance23_squared = (point3-point2).squaredNorm (), distance23_corr_squared = (corr3-corr2).squaredNorm (), distance23_quotient_squared = distance23_squared/distance23_corr_squared; if ( distance23_quotient_squared < min_distance_quotient_squared || distance23_quotient_squared > max_distance_quotient_squared) continue; for (int correspondence1_idx = 0; correspondence1_idx < correspondence2_idx; ++correspondence1_idx) { if (counter_for_tested_combinations >= max_no_of_tested_combinations) { done = true; break; } ++counter_for_tested_combinations; const pcl::PointCorrespondence6D& correspondence1 = correspondences[correspondence1_idx]; const Eigen::Vector3f& point1 = correspondence1.point1, & corr1 = correspondence1.point2; float distance12_squared = (point2-point1).squaredNorm (), distance12_corr_squared = (corr2-corr1).squaredNorm (), distance12_quotient_squared = distance12_squared/distance12_corr_squared; if ( distance12_quotient_squared < min_distance_quotient_squared || distance12_quotient_squared > max_distance_quotient_squared) continue; float distance13_squared = (point3-point1).squaredNorm (), distance13_corr_squared = (corr3-corr1).squaredNorm (), distance13_quotient_squared = distance13_squared/distance13_corr_squared; if ( distance13_quotient_squared < min_distance_quotient_squared || distance13_quotient_squared > max_distance_quotient_squared) continue; transformation_from_correspondeces.reset (); transformation_from_correspondeces.add (corr1, point1); transformation_from_correspondeces.add (corr2, point2); transformation_from_correspondeces.add (corr3, point3); ++counter_for_added_pose_estimates; PoseEstimate pose_estimate; pose_estimate.transformation = transformation_from_correspondeces.getTransformation (); pose_estimate.score = (correspondence1.distance + correspondence2.distance + correspondence3.distance) / 3.0f; // TODO: based // on the measured distance_errors? pose_estimate.correspondence_indices.push_back (correspondence1_idx); pose_estimate.correspondence_indices.push_back (correspondence2_idx); pose_estimate.correspondence_indices.push_back (correspondence3_idx); pose_estimates.push_back (pose_estimate); if (counter_for_added_pose_estimates >= max_no_of_results) { done = true; break; } } } } }
void PosesFromMatches::estimatePosesUsing2Correspondences (const PointCorrespondences6DVector& correspondences, int max_no_of_tested_combinations, int max_no_of_results, PosesFromMatches::PoseEstimatesVector& pose_estimates) const { const Eigen::Vector3f x_direction (1.0f, 0.0f, 0.0f), y_direction (0.0f, 1.0f, 0.0f), z_direction (0.0f, 0.0f, 1.0f); int max_correspondence_idx = correspondences.size (); int counter_for_tested_combinations = 0, counter_for_added_pose_estimates = 0; float max_distance_quotient = 1.0f+parameters_.max_correspondence_distance_error, max_distance_quotient_squared=powf (max_distance_quotient, 2), min_distance_quotient = 1.0f / (max_distance_quotient), min_distance_quotient_squared=powf (min_distance_quotient, 2); TransformationFromCorrespondences transformation_from_correspondeces; // The following loop structure goes through the pairs in the order 12, 13, 23, 14, 24, 34, ..., // testing the best correspondences pairs first, without beeing stuck too long with one specific // (possibly wrong) correspondence. bool done = false; for (int correspondence2_idx=0; correspondence2_idx<max_correspondence_idx && !done; ++correspondence2_idx) { const PointCorrespondence6D& correspondence2 = correspondences[correspondence2_idx]; for (int correspondence1_idx=0; correspondence1_idx<correspondence2_idx; ++correspondence1_idx) { if (counter_for_tested_combinations >= max_no_of_tested_combinations) { done = true; break; } const PointCorrespondence6D& correspondence1 = correspondences[correspondence1_idx]; ++counter_for_tested_combinations; const Eigen::Vector3f& point1 = correspondence1.point1, & point2 = correspondence2.point1, & corr1 = correspondence1.point2, & corr2 = correspondence2.point2; float distance_squared = (point2-point1).squaredNorm (), distance_corr_squared = (corr2-corr1).squaredNorm (), distance_quotient_squared = distance_squared/distance_corr_squared; if ( distance_quotient_squared < min_distance_quotient_squared || distance_quotient_squared > max_distance_quotient_squared) { //std::cout << "Skipping because of mismatching distances "<<sqrtf (distance1_squared) // << " and "<<sqrtf (distance1_corr_squared)<<".\n"; continue; } float distance = sqrtf (distance_squared); Eigen::Vector3f corr3=corr1, corr4=corr2; corr3[0]+=distance; corr4[0]+=distance; Eigen::Vector3f point3=correspondence1.transformation*corr3, point4=correspondence2.transformation*corr4; distance_squared = (point4-point3).squaredNorm (), distance_corr_squared = (corr4-corr3).squaredNorm (), distance_quotient_squared = distance_squared/distance_corr_squared; if ( distance_quotient_squared < min_distance_quotient_squared || distance_quotient_squared > max_distance_quotient_squared) continue; Eigen::Vector3f corr5=corr1, corr6=corr2; corr5[1]+=distance; corr6[1]+=distance; Eigen::Vector3f point5=correspondence1.transformation*corr5, point6=correspondence2.transformation*corr6; distance_squared = (point6-point5).squaredNorm (), distance_corr_squared = (corr6-corr5).squaredNorm (), distance_quotient_squared = distance_squared/distance_corr_squared; if ( distance_quotient_squared < min_distance_quotient_squared || distance_quotient_squared > max_distance_quotient_squared) continue; Eigen::Vector3f corr7=corr1, corr8=corr2; corr7[2]+=distance; corr8[2]+=distance; Eigen::Vector3f point7=correspondence1.transformation*corr7, point8=correspondence2.transformation*corr8; distance_squared = (point8-point7).squaredNorm (), distance_corr_squared = (corr8-corr7).squaredNorm (), distance_quotient_squared = distance_squared/distance_corr_squared; if ( distance_quotient_squared < min_distance_quotient_squared || distance_quotient_squared > max_distance_quotient_squared) continue; transformation_from_correspondeces.reset (); transformation_from_correspondeces.add (corr1, point1); transformation_from_correspondeces.add (corr2, point2); transformation_from_correspondeces.add (corr3, point3); transformation_from_correspondeces.add (corr4, point4); transformation_from_correspondeces.add (corr5, point5); transformation_from_correspondeces.add (corr6, point6); transformation_from_correspondeces.add (corr7, point7); transformation_from_correspondeces.add (corr8, point8); ++counter_for_added_pose_estimates; PoseEstimate pose_estimate; pose_estimate.transformation = transformation_from_correspondeces.getTransformation (); pose_estimate.score = 0.5f*(correspondence1.score+correspondence2.score); // TODO: based // on the measured distance_errors? pose_estimate.correspondence_indices.push_back (correspondence1_idx); pose_estimate.correspondence_indices.push_back (correspondence2_idx); pose_estimates.push_back (pose_estimate); if (counter_for_added_pose_estimates >= max_no_of_results) { done = true; break; } } } //std::cout << "Tested "<<counter_for_tested_combinations<<" combinations. Extracted " // << counter_for_added_pose_estimates //<< " poses. Set of pose estimates now has size "<<pose_estimates.size ()<<"\n"; }