ransac_transformation.cpp
Go to the documentation of this file.
00001 /*
00002  * RansacTransformation.cpp
00003  *
00004  *  Created on: 13.07.2012
00005  *      Author: ross
00006  */
00007 
00008 #include "rgbd_registration/ransac_transformation.h"
00009 #include "rgbd_registration/parameter_server.h"
00010 
00011 #include <ros/console.h>
00012 
00013 //pcl
00014 #include <Eigen/Geometry>
00015 #include <pcl/common/transformation_from_correspondences.h>
00016 #include <iostream>
00017 
00018 RansacTransformation::RansacTransformation ()
00019 {
00020   // TODO Auto-generated constructor stub
00021 
00022 }
00023 
00024 RansacTransformation::~RansacTransformation ()
00025 {
00026   // TODO Auto-generated destructor stub
00027 }
00028 
00029 void RansacTransformation::computeInliersAndError (
00030     const std::vector<cv::DMatch>& matches,
00031     const Eigen::Matrix4f& transformation,
00032     const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& origins,
00033     const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& earlier,
00034     std::vector<cv::DMatch>& inliers, //output var
00035     double& mean_error, std::vector<double>& errors, double squaredMaxInlierDistInM)
00036 { //output var
00037 
00038 
00039   inliers.clear ();
00040   errors.clear ();
00041 
00042   std::vector<std::pair<float, int> > dists;
00043   std::vector<cv::DMatch> inliers_temp;
00044 
00045   assert(matches.size() > 0);
00046   mean_error = 0.0;
00047   for (unsigned int j = 0; j < matches.size (); j++)
00048   { //compute new error and inliers
00049 
00050     unsigned int this_id = matches[j].queryIdx;
00051     unsigned int earlier_id = matches[j].trainIdx;
00052 
00053     Eigen::Vector4f vec = (transformation * origins[this_id]) - earlier[earlier_id];
00054 
00055     double error = vec.dot (vec);
00056 
00057     if (error > squaredMaxInlierDistInM)
00058       continue; //ignore outliers
00059     if (! (error >= 0.0))
00060     {
00061       ROS_DEBUG_STREAM("Transformation for error !> 0: " << transformation);
00062       ROS_DEBUG_STREAM(error << " " << matches.size());
00063     }
00064     error = sqrt (error);
00065     dists.push_back (std::pair<float, int> (error, j));
00066     inliers_temp.push_back (matches[j]); //include inlier
00067 
00068     mean_error += error;
00069     errors.push_back (error);
00070   }
00071 
00072   if (inliers_temp.size () < 3)
00073   { //at least the samples should be inliers
00074     ROS_WARN_COND(inliers_temp.size() > 3, "No inliers at all in %d matches!", (int)matches.size()); // only warn if this checks for all initial matches
00075     mean_error = 1e9;
00076   }
00077   else
00078   {
00079     mean_error /= inliers_temp.size ();
00080 
00081     // sort inlier ascending according to their error
00082     sort (dists.begin (), dists.end ());
00083 
00084     inliers.resize (inliers_temp.size ());
00085     for (unsigned int i = 0; i < inliers_temp.size (); i++)
00086     {
00087       inliers[i] = matches[dists[i].second];
00088     }
00089   }
00090   if (! (mean_error > 0))
00091     ROS_DEBUG_STREAM("Transformation for mean error !> 0: " << transformation);
00092   if (! (mean_error > 0))
00093     ROS_DEBUG_STREAM(mean_error << " " << inliers_temp.size());
00094 
00095 }
00096 
00097 template<class InputIterator>
00098 Eigen::Matrix4f RansacTransformation::getTransformFromMatches (const std::vector<Eigen::Vector4f,
00099     Eigen::aligned_allocator<Eigen::Vector4f> > & source_feature_locations_3d, const std::vector<
00100     Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > & target_feature_locations_3d,
00101     InputIterator iter_begin, InputIterator iter_end, bool& valid, float max_dist_m)
00102 {
00103   pcl::TransformationFromCorrespondences tfc;
00104   valid = true;
00105   std::vector<Eigen::Vector3f> t, f;
00106 
00107   for (; iter_begin != iter_end; iter_begin++)
00108   {
00109     int this_id = iter_begin->queryIdx;
00110     int earlier_id = iter_begin->trainIdx;
00111 
00112     Eigen::Vector3f from (source_feature_locations_3d[this_id][0],
00113         source_feature_locations_3d[this_id][1], source_feature_locations_3d[this_id][2]);
00114     Eigen::Vector3f to (target_feature_locations_3d[earlier_id][0],
00115         target_feature_locations_3d[earlier_id][1], target_feature_locations_3d[earlier_id][2]);
00116     if (max_dist_m > 0)
00117     { //storing is only necessary, if max_dist is given
00118       f.push_back (from);
00119       t.push_back (to);
00120     }
00121     tfc.add (from, to, 1.0 / to (2));//the further, the less weight b/c of accuracy decay
00122   }
00123 
00124   // find smallest distance between a point and its neighbour in the same cloud
00125   // je groesser das dreieck aufgespannt ist, desto weniger fallen kleine positionsfehler der einzelnen
00126   // Punkte ist Gewicht!
00127 
00128   if (max_dist_m > 0)
00129   {
00130     //float min_neighbour_dist = 1e6;
00131     Eigen::Matrix4f foo;
00132 
00133     valid = true;
00134     for (uint i = 0; i < f.size (); i++)
00135     {
00136       float d_f = (f.at ( (i + 1) % f.size ()) - f.at (i)).norm ();
00137       float d_t = (t.at ( (i + 1) % t.size ()) - t.at (i)).norm ();
00138 
00139       if (abs (d_f - d_t) > max_dist_m)
00140       {
00141         valid = false;
00142         return Eigen::Matrix4f ();
00143       }
00144     }
00145     //here one could signal that some samples are very close, but as the transformation is validated elsewhere we don't
00146     //if (min_neighbour_dist < 0.5) { ROS_INFO...}
00147   }
00148   // get relative movement from samples
00149   return tfc.getTransformation ().matrix ();
00150 }
00151 
00154 bool RansacTransformation::getRelativeTransformationTo (const std::vector<Eigen::Vector4f,
00155     Eigen::aligned_allocator<Eigen::Vector4f> > & source_feature_locations_3d, const std::vector<
00156     Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > & target_feature_locations_3d,
00157     std::vector<cv::DMatch>* initial_matches, Eigen::Matrix4f& resulting_transformation,
00158     float& rmse, std::vector<cv::DMatch>& matches, uint min_matches)
00159 {
00160 
00161   assert(initial_matches != NULL);
00162   matches.clear ();
00163 
00164   if (initial_matches->size () <= min_matches)
00165   {
00166     ROS_INFO("[RansacTransformation] Only %d feature matches between source and target (minimal: %i)",(int)initial_matches->size() , min_matches);
00167     return false;
00168   }
00169   else
00170   {
00171     ROS_INFO("[RansacTransformation] %d feature matches between source and target (minimal: %i)",(int)initial_matches->size() , min_matches);
00172   }
00173 
00174   //unsigned int min_inlier_threshold = int(initial_matches->size()*0.2);
00175   unsigned int min_inlier_threshold = (unsigned int) min_matches;
00176   std::vector<cv::DMatch> inlier; //holds those feature correspondences that support the transformation
00177   double inlier_error; //all squared errors
00178   srand ((long) std::clock ());
00179 
00180   // a point is an inlier if it's no more than max_dist_m m from its partner apart
00181   const float max_dist_m = ParameterServer::instance ()->get<double> ("max_dist_for_inliers");
00182   const int ransac_iterations = ParameterServer::instance ()->get<int> ("ransac_iterations");
00183   std::vector<double> dummy;
00184 
00185   // best values of all iterations (including invalids)
00186   double best_error = 1e6, best_error_invalid = 1e6;
00187   unsigned int best_inlier_invalid = 0, best_inlier_cnt = 0, valid_iterations = 0;
00188 
00189   Eigen::Matrix4f transformation;
00190 
00191   const unsigned int sample_size = 3;// chose this many randomly from the correspondences:
00192   for (int n_iter = 0; n_iter < ransac_iterations; n_iter++)
00193   {
00194     //generate a map of samples. Using a map solves the problem of drawing a sample more than once
00195     std::set<cv::DMatch> sample_matches;
00196     std::vector<cv::DMatch> sample_matches_vector;
00197     while (sample_matches.size () < sample_size)
00198     {
00199       int id = rand () % initial_matches->size ();
00200       sample_matches.insert (initial_matches->at (id));
00201       sample_matches_vector.push_back (initial_matches->at (id));
00202     }
00203 
00204     bool valid; // valid is false iff the sampled points clearly aren't inliers themself
00205     transformation = getTransformFromMatches (source_feature_locations_3d,
00206         target_feature_locations_3d, sample_matches.begin (), sample_matches.end (), valid,
00207         max_dist_m);
00208     if (!valid)
00209       continue; // valid is false iff the sampled points aren't inliers themself
00210     if (transformation != transformation)
00211       continue; //Contains NaN
00212 
00213     //test whether samples are inliers (more strict than before)
00214     computeInliersAndError (sample_matches_vector, transformation, source_feature_locations_3d,
00215         target_feature_locations_3d, inlier, inlier_error, /*output*/
00216         dummy, max_dist_m * max_dist_m);
00217     ROS_DEBUG("Transformation from and for %u samples results in an error of %f and %i inliers.", sample_size, inlier_error, (int)inlier.size());
00218     if (inlier_error > 1000)
00219       continue; //most possibly a false match in the samples
00220     computeInliersAndError (*initial_matches, transformation, source_feature_locations_3d,
00221         target_feature_locations_3d, inlier, inlier_error, /*output*/
00222         dummy, max_dist_m * max_dist_m);
00223     ROS_DEBUG("Transformation from %u samples results in an error of %f and %i inliers for all matches (%i).", sample_size, inlier_error, (int)inlier.size(), (int)initial_matches->size());
00224 
00225     // check also invalid iterations
00226     if (inlier.size () > best_inlier_invalid)
00227     {
00228       best_inlier_invalid = inlier.size ();
00229       best_error_invalid = inlier_error;
00230     }
00231     ROS_DEBUG("iteration %d  cnt: %lu, best: %u,  error: %.2f",n_iter, inlier.size(), best_inlier_cnt, inlier_error*100);
00232 
00233     if (inlier.size () < min_inlier_threshold || inlier_error > max_dist_m)
00234     {
00235       ROS_DEBUG("Skipped iteration: inliers: %i (min %i), inlier_error: %.2f (max %.2f)", (int)inlier.size(), (int) min_inlier_threshold, inlier_error*100, max_dist_m*100);
00236       continue;
00237     }
00238     // ROS_INFO("[RansacTransformation] Refining iteration from %i samples: all matches: %i, inliers: %i, inlier_error: %f", (int)sample_size, (int)initial_matches->size(), (int)inlier.size(), inlier_error);
00239     valid_iterations++;
00240     //if (inlier_error > 0) ROS_ERROR("size: %i", (int)dummy.size());
00241     assert(inlier_error>=0);
00242 
00243     //Performance hacks:
00245     if (inlier.size () > initial_matches->size () * 0.5)
00246       n_iter += 10;
00248     if (inlier.size () > initial_matches->size () * 0.8)
00249       n_iter += 20;
00250 
00251     if (inlier_error < best_error)
00252     { //copy this to the result
00253       resulting_transformation = transformation;
00254       matches = inlier;
00255       assert(matches.size()>= min_inlier_threshold);
00256       best_inlier_cnt = inlier.size ();
00257       //assert(matches.size()>= ((float)initial_matches->size())*min_inlier_ratio);
00258       rmse = inlier_error;
00259       best_error = inlier_error;
00260       // ROS_INFO("[RansacTransformation]   new best iteration %d  cnt: %d, best_inlier: %d,  error: %.4f, bestError: %.4f",n_iter, inlier.size(), best_inlier_cnt, inlier_error, best_error);
00261 
00262     }
00263     else
00264     {
00265       // ROS_INFO("[RansacTransformation] NO new best iteration %d  cnt: %d, best_inlier: %d,  error: %.4f, bestError: %.4f",n_iter, inlier.size(), best_inlier_cnt, inlier_error, best_error);
00266     }
00267 
00268     //int max_ndx = min((int) min_inlier_threshold,30); //? What is this 30?
00269     double new_inlier_error;
00270 
00271     transformation = getTransformFromMatches (source_feature_locations_3d,
00272         target_feature_locations_3d, matches.begin (), matches.end (), valid, max_dist_m); // compute new trafo from all inliers:
00273     if (transformation != transformation)
00274       continue; //Contains NaN
00275     computeInliersAndError (*initial_matches, transformation, source_feature_locations_3d,
00276         target_feature_locations_3d, inlier, new_inlier_error, dummy, max_dist_m * max_dist_m);
00277     ROS_DEBUG("Refined Transformation from all matches (%i) results in an error of %f and %i inliers for all matches.", (int)initial_matches->size(), inlier_error, (int)inlier.size());
00278     // ROS_INFO("[RansacTransformation] asd recomputed: inliersize: %i, inlier error: %f", (int) inlier.size(),100*new_inlier_error);
00279 
00280 
00281     // check also invalid iterations
00282     if (inlier.size () > best_inlier_invalid)
00283     {
00284       best_inlier_invalid = inlier.size ();
00285       best_error_invalid = inlier_error;
00286     }
00287 
00288     if (inlier.size () < min_inlier_threshold || new_inlier_error > max_dist_m)
00289     {
00290       //inlier.size() < ((float)initial_matches->size())*min_inlier_ratio ||
00291       // ROS_INFO("[RansacTransformation] Skipped iteration: inliers: %i (min %i), inlier_error: %.2f (max %.2f)", (int)inlier.size(), (int) min_inlier_threshold,  inlier_error*100, max_dist_m*100);
00292       continue;
00293     }
00294     // ROS_INFO("[RansacTransformation] Refined iteration from %i samples: all matches %i, inliers: %i, new_inlier_error: %f", (int)sample_size, (int)initial_matches->size(), (int)inlier.size(), new_inlier_error);
00295 
00296     assert(new_inlier_error>=0);
00297 
00298     if (new_inlier_error < best_error)
00299     {
00300       resulting_transformation = transformation;
00301       matches = inlier;
00302       assert(matches.size()>= min_inlier_threshold);
00303       //assert(matches.size()>= ((float)initial_matches->size())*min_inlier_ratio);
00304       rmse = new_inlier_error;
00305       best_error = new_inlier_error;
00306       // ROS_INFO("[RansacTransformation]   improved: new best iteration %d  cnt: %d, best_inlier: %d,  error: %.2f, bestError: %.2f",n_iter, inlier.size(), best_inlier_cnt, inlier_error*100, best_error*100);
00307     }
00308     else
00309     {
00310       // ROS_INFO("[RansacTransformation] improved: NO new best iteration %d  cnt: %d, best_inlier: %d,  error: %.2f, bestError: %.2f",n_iter, inlier.size(), best_inlier_cnt, inlier_error*100, best_error*100);
00311     }
00312   } //iterations
00313   ROS_INFO("[RansacTransformation] %i good iterations (from %i), inlier pct %i, inlier cnt: %i, error: %.2f cm",valid_iterations, ransac_iterations, (int) (matches.size()*1.0/initial_matches->size()*100),(int) matches.size(),rmse*100);
00314   ROS_DEBUG("best overall: inlier: %i, error: %.2f",best_inlier_invalid, best_error_invalid*100);
00315 
00316   return matches.size () >= min_inlier_threshold;
00317 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


rgbd_registration
Author(s): Ross Kidson
autogenerated on Thu May 23 2013 15:36:53