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";
}
