poses_from_matches.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2010-2012, Willow Garage, Inc.
00006  *
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of the copyright holder(s) nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  */
00037 
00038 #include <cstddef>
00039 #include <iostream>
00040 #include <pcl/common/eigen.h>
00041 #include <pcl/common/poses_from_matches.h>
00042 #include <pcl/common/transformation_from_correspondences.h>
00043 
00045 pcl::PosesFromMatches::PosesFromMatches () : parameters_ () 
00046 {
00047 }
00048 
00050 pcl::PosesFromMatches::~PosesFromMatches ()
00051 {
00052 }
00053 
00055 void 
00056 pcl::PosesFromMatches::estimatePosesUsing1Correspondence (const pcl::PointCorrespondences6DVector& correspondences,
00057                                                           int max_no_of_results,
00058                                                           pcl::PosesFromMatches::PoseEstimatesVector& pose_estimates) const
00059 {
00060   if (max_no_of_results < 0)
00061     max_no_of_results = static_cast<int> (correspondences.size ());
00062   else
00063     max_no_of_results = std::min (max_no_of_results, static_cast<int> (correspondences.size ()));
00064   
00065   for (int correspondence_idx = 0; correspondence_idx < max_no_of_results; ++correspondence_idx)
00066   {
00067     const pcl::PointCorrespondence6D& correspondence = correspondences[correspondence_idx];
00068     PoseEstimate pose_estimate;
00069     pose_estimate.transformation = correspondence.transformation;
00070     pose_estimate.score = correspondence.distance;
00071     pose_estimate.correspondence_indices.push_back (correspondence_idx);
00072     pose_estimates.push_back (pose_estimate);
00073   }
00074 }
00075 
00076 
00078 void 
00079 pcl::PosesFromMatches::estimatePosesUsing2Correspondences (const pcl::PointCorrespondences6DVector& correspondences,
00080                                                            int max_no_of_tested_combinations, int max_no_of_results,
00081                                                            pcl::PosesFromMatches::PoseEstimatesVector& pose_estimates) const
00082 {
00083   const Eigen::Vector3f x_direction (1.0f, 0.0f, 0.0f),
00084                         y_direction (0.0f, 1.0f, 0.0f),
00085                         z_direction (0.0f, 0.0f, 1.0f);
00086   
00087   int max_correspondence_idx = static_cast<int> (correspondences.size ());
00088   int counter_for_tested_combinations = 0,
00089       counter_for_added_pose_estimates = 0;
00090   float max_distance_quotient = 1.0f+parameters_.max_correspondence_distance_error,
00091         max_distance_quotient_squared=powf (max_distance_quotient, 2),
00092         min_distance_quotient = 1.0f / (max_distance_quotient),
00093         min_distance_quotient_squared = std::pow (min_distance_quotient, 2);
00094 
00095   pcl::TransformationFromCorrespondences transformation_from_correspondeces;
00096   
00097   // The following loop structure goes through the pairs in the order 12, 13, 23, 14, 24, 34, ...,
00098   // testing the best correspondences pairs first, without beeing stuck too long with one specific
00099   // (possibly wrong) correspondence.
00100   bool done = false;
00101   for (int correspondence2_idx = 0; correspondence2_idx < max_correspondence_idx && !done; ++correspondence2_idx)
00102   {
00103     const pcl::PointCorrespondence6D& correspondence2 = correspondences[correspondence2_idx];
00104     for (int correspondence1_idx = 0; correspondence1_idx < correspondence2_idx; ++correspondence1_idx)
00105     {
00106       if (counter_for_tested_combinations >= max_no_of_tested_combinations)
00107       {
00108         done = true;
00109         break;
00110       }
00111       
00112       const pcl::PointCorrespondence6D& correspondence1 = correspondences[correspondence1_idx];
00113       ++counter_for_tested_combinations;
00114       
00115       const Eigen::Vector3f& point1 = correspondence1.point1, & point2 = correspondence2.point1,
00116                            & corr1  = correspondence1.point2, & corr2  = correspondence2.point2;
00117       
00118       float distance_squared = (point2-point1).squaredNorm (),
00119             distance_corr_squared = (corr2-corr1).squaredNorm (),
00120             distance_quotient_squared = distance_squared/distance_corr_squared;
00121       if (   distance_quotient_squared < min_distance_quotient_squared
00122           || distance_quotient_squared > max_distance_quotient_squared)
00123       {
00124         //std::cout << "Skipping because of mismatching distances "<<sqrtf (distance1_squared)
00125         //          << " and "<<sqrtf (distance1_corr_squared)<<".\n";
00126         continue;
00127       }
00128       
00129       float distance = sqrtf (distance_squared);
00130       
00131       Eigen::Vector3f corr3=corr1, corr4=corr2;
00132       corr3[0]+=distance; corr4[0]+=distance;
00133       Eigen::Vector3f point3=correspondence1.transformation*corr3, point4=correspondence2.transformation*corr4;
00134       
00135       distance_squared = (point4-point3).squaredNorm (),
00136       distance_corr_squared = (corr4-corr3).squaredNorm (),
00137       distance_quotient_squared = distance_squared/distance_corr_squared;
00138       if (   distance_quotient_squared < min_distance_quotient_squared
00139           || distance_quotient_squared > max_distance_quotient_squared)
00140         continue;
00141       
00142       Eigen::Vector3f corr5=corr1, corr6=corr2;
00143       corr5[1]+=distance; corr6[1]+=distance;
00144       Eigen::Vector3f point5=correspondence1.transformation*corr5, point6=correspondence2.transformation*corr6;
00145       
00146       distance_squared = (point6-point5).squaredNorm (),
00147       distance_corr_squared = (corr6-corr5).squaredNorm (),
00148       distance_quotient_squared = distance_squared/distance_corr_squared;
00149       if (   distance_quotient_squared < min_distance_quotient_squared
00150           || distance_quotient_squared > max_distance_quotient_squared)
00151         continue;
00152       
00153       Eigen::Vector3f corr7=corr1, corr8=corr2;
00154       corr7[2]+=distance; corr8[2]+=distance;
00155       Eigen::Vector3f point7=correspondence1.transformation*corr7, point8=correspondence2.transformation*corr8;
00156       
00157       distance_squared = (point8-point7).squaredNorm (),
00158       distance_corr_squared = (corr8-corr7).squaredNorm (),
00159       distance_quotient_squared = distance_squared/distance_corr_squared;
00160       if (   distance_quotient_squared < min_distance_quotient_squared
00161           || distance_quotient_squared > max_distance_quotient_squared)
00162         continue;
00163       
00164       transformation_from_correspondeces.reset ();
00165       transformation_from_correspondeces.add (corr1, point1);
00166       transformation_from_correspondeces.add (corr2, point2);
00167       transformation_from_correspondeces.add (corr3, point3);
00168       transformation_from_correspondeces.add (corr4, point4);
00169       transformation_from_correspondeces.add (corr5, point5);
00170       transformation_from_correspondeces.add (corr6, point6);
00171       transformation_from_correspondeces.add (corr7, point7);
00172       transformation_from_correspondeces.add (corr8, point8);
00173       
00174       ++counter_for_added_pose_estimates;
00175       PoseEstimate pose_estimate;
00176       pose_estimate.transformation = transformation_from_correspondeces.getTransformation ();
00177       pose_estimate.score = 0.5f * (correspondence1.distance + correspondence2.distance); // TODO: based
00178                                                                                 // on the measured distance_errors?
00179       pose_estimate.correspondence_indices.push_back (correspondence1_idx);
00180       pose_estimate.correspondence_indices.push_back (correspondence2_idx);
00181       pose_estimates.push_back (pose_estimate);
00182       if (counter_for_added_pose_estimates >= max_no_of_results)
00183       {
00184         done = true;
00185         break;
00186       }
00187     }
00188   }
00189 }
00190 
00192 void
00193 pcl::PosesFromMatches::estimatePosesUsing3Correspondences (const PointCorrespondences6DVector& correspondences,
00194                                                            int max_no_of_tested_combinations, int max_no_of_results,
00195                                                            PosesFromMatches::PoseEstimatesVector& pose_estimates) const
00196 {
00197   const Eigen::Vector3f x_direction (1.0f, 0.0f, 0.0f),
00198                         y_direction (0.0f, 1.0f, 0.0f),
00199                         z_direction (0.0f, 0.0f, 1.0f);
00200   
00201   int max_correspondence_idx = static_cast<int> (correspondences.size ());
00202   int counter_for_tested_combinations = 0,
00203       counter_for_added_pose_estimates = 0;
00204   float max_distance_quotient = 1.0f+parameters_.max_correspondence_distance_error,
00205         max_distance_quotient_squared = std::pow (max_distance_quotient, 2),
00206         min_distance_quotient = 1.0f / (max_distance_quotient),
00207         min_distance_quotient_squared = std::pow (min_distance_quotient, 2);
00208 
00209   pcl::TransformationFromCorrespondences transformation_from_correspondeces;
00210   
00211   // The following loop structure goes through the triples in the order 123, 124, 134, 234, 125, 135, 235, ...,
00212   // testing the best correspondences triples first, without beeing stuck too long with one specific
00213   // (possibly wrong) correspondence.
00214   bool done = false;
00215   for (int correspondence3_idx = 0; correspondence3_idx < max_correspondence_idx && !done; ++correspondence3_idx)
00216   {
00217     const pcl::PointCorrespondence6D& correspondence3 = correspondences[correspondence3_idx];
00218     const Eigen::Vector3f& point3 = correspondence3.point1,
00219                   & corr3  = correspondence3.point2;
00220     for (int correspondence2_idx = 0; correspondence2_idx < correspondence3_idx && !done; ++correspondence2_idx)
00221     {
00222       const pcl::PointCorrespondence6D& correspondence2 = correspondences[correspondence2_idx];
00223       const Eigen::Vector3f& point2 = correspondence2.point1,
00224                     & corr2  = correspondence2.point2;
00225       
00226       float distance23_squared = (point3-point2).squaredNorm (),
00227             distance23_corr_squared = (corr3-corr2).squaredNorm (),
00228             distance23_quotient_squared = distance23_squared/distance23_corr_squared;
00229       if (   distance23_quotient_squared < min_distance_quotient_squared 
00230           || distance23_quotient_squared > max_distance_quotient_squared)
00231         continue;
00232       
00233       for (int correspondence1_idx = 0; correspondence1_idx < correspondence2_idx; ++correspondence1_idx)
00234       {
00235         if (counter_for_tested_combinations >= max_no_of_tested_combinations)
00236         {
00237           done = true;
00238           break;
00239         }
00240         ++counter_for_tested_combinations;
00241         const pcl::PointCorrespondence6D& correspondence1 = correspondences[correspondence1_idx];
00242         const Eigen::Vector3f& point1 = correspondence1.point1,
00243                              & corr1  = correspondence1.point2;
00244         float distance12_squared = (point2-point1).squaredNorm (),
00245               distance12_corr_squared = (corr2-corr1).squaredNorm (),
00246               distance12_quotient_squared = distance12_squared/distance12_corr_squared;
00247         if (   distance12_quotient_squared < min_distance_quotient_squared
00248             || distance12_quotient_squared > max_distance_quotient_squared)
00249           continue;
00250         float distance13_squared = (point3-point1).squaredNorm (),
00251               distance13_corr_squared = (corr3-corr1).squaredNorm (),
00252               distance13_quotient_squared = distance13_squared/distance13_corr_squared;
00253         if (   distance13_quotient_squared < min_distance_quotient_squared
00254             || distance13_quotient_squared > max_distance_quotient_squared)
00255           continue;
00256         
00257         transformation_from_correspondeces.reset ();
00258         transformation_from_correspondeces.add (corr1, point1);
00259         transformation_from_correspondeces.add (corr2, point2);
00260         transformation_from_correspondeces.add (corr3, point3);
00261         
00262         ++counter_for_added_pose_estimates;
00263         PoseEstimate pose_estimate;
00264         pose_estimate.transformation = transformation_from_correspondeces.getTransformation ();
00265         pose_estimate.score = (correspondence1.distance + correspondence2.distance + correspondence3.distance) / 3.0f; // TODO: based
00266                                                                                   // on the measured distance_errors?
00267         pose_estimate.correspondence_indices.push_back (correspondence1_idx);
00268         pose_estimate.correspondence_indices.push_back (correspondence2_idx);
00269         pose_estimate.correspondence_indices.push_back (correspondence3_idx);
00270         pose_estimates.push_back (pose_estimate);
00271         if (counter_for_added_pose_estimates >= max_no_of_results)
00272         {
00273           done = true;
00274           break;
00275         }
00276       }
00277     }
00278   }
00279 }
00280 


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:31:11