register_views.cpp
Go to the documentation of this file.
00001 /*
00002  *  Experiment with efficent view registration/idetification methods.
00003  *
00004  *  Copywrong (K) 2010 Z.
00005  *
00006  *  This program is free software; you can redistribute it and/or modify
00007  *  it under the terms of the GNU General Public License as published by
00008  *  the Free Software Foundation; either version 2 of the License, or
00009  *  (at your option) any later version.
00010  *
00011  *  This program is distributed in the hope that it will be useful,
00012  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00013  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014  *  GNU General Public License for more details.
00015  *
00016  *  You should have received a copy of the GNU General Public License
00017  *  along with this program; if not, write to the Free Software
00018  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00019  *
00020  * $Id: RegisterViews.cc,v 1.0 2010/30/10 12:00:00 zoli Exp $
00021  */
00022 #include "../common/CommonTerminalRoutines.h"
00023 #include "../common/CommonVTKRoutines.h"
00024 #include "../common/CommonIORoutines.h"
00025 #include "../common/CommonANNRoutines.h"
00026 #include "../common/ANN-VTK.h"
00027 
00028 #include <Eigen/Array>
00029 #include <Eigen/Geometry>
00030 #include <Eigen/SVD>
00031 
00032 // for srand init
00033 #include <time.h>
00034 
00035 #define BUCKET_SIZE 30
00036 
00037 using namespace std;
00038 using namespace Eigen;
00039 
00040 ANNkd_tree*  kd_tree;
00041 ANNidxArray  nnIdx;
00042 ANNdistArray sqrDists;
00043 
00044 struct PCD
00045 {
00046   PCD_Header header;
00047   ANNpointArray points;
00048 };
00049 
00050 double maxZdiff = 0.03;
00051 double minAdiff = DEG2RAD(30);
00052 int nxIdx, model_nxIdx;
00053 
00054 inline bool potentialMatch (const ANNpoint p0, const ANNpoint p1, const bool check_angle)
00055 {
00056   // if hight is ok
00057   if ((p0[2] - p1[2]) < maxZdiff)
00058   {
00059     // if angle to Z has to be checked
00060     if (check_angle)
00061       return (acos (p1[model_nxIdx+2]) > minAdiff);
00062     else
00063       return true;
00064   }
00065   return false;
00066 }
00067 
00068 inline Matrix4f computeTransformation (const ANNpoint target, const ANNpoint source, const bool rotation)
00069 {
00070   // compute transformation
00071   Matrix4f transform = Matrix4f::Identity ();
00072   transform (0,3) = target[0] - source[0];
00073   transform (1,3) = target[1] - source[1];
00074   double angle2D = 0;
00075   if (rotation)
00076     angle2D = atan2 (target[nxIdx+1], target[nxIdx+0]) - atan2 (source[model_nxIdx+1], source[model_nxIdx+0]);
00077   else
00078     angle2D = rand () / ((RAND_MAX + 1.0) / (2*M_PI)); // random rotation
00079   transform (0,0) = +cos (angle2D);
00080   transform (0,1) = -sin (angle2D);
00081   transform (1,0) = -transform (0,1);
00082   transform (1,1) = +transform (0,0);
00083   return transform;
00084 }
00085 
00086 ANNpointArray evaluateTransformation (Matrix4f transform, vector<PCD>::iterator model, double sqr_threshold, int step, int limit_match, int limit_inliers, vector<int> &match_idx, vector<int> &tmp_inliers)
00087 {
00088   int nr_check = model->header.nr_points / step;
00089   
00090   // evaluate transformation through exhaustive search
00091   match_idx.clear (); match_idx.reserve (nr_check);
00092   tmp_inliers.clear (); tmp_inliers.reserve (nr_check);
00093   ANNpointArray transformed = annAllocPts (model->header.nr_points, 3);
00094   int pos_match = nr_check;
00095   int pos_inliers = nr_check;
00096   for (int cp = 0; cp < model->header.nr_points; cp += step)
00097   {
00098     Vector4f sp (model->points[cp][0], model->points[cp][1], model->points[cp][2], 1);
00099     //ANNpoint point = annAllocPt (3);
00100     Map<Vector3f> tp (&(transformed[cp][0]));
00101     tp = (transform * sp).start<3> ();
00102     /*kd_tree->annkSearch (point, 1, nnIdx, sqrDists, 0.0);
00103       if (sqrDists[0] < sqr_threshold)
00104       {
00105       match_idx.push_back (cp);
00106       tmp_inliers.push_back (nnIdx[0]);
00107       pos_match++;
00108       pos_inliers++;
00109       }*/
00110     int nr_nn = kd_tree->annkFRSearch (transformed[cp], sqr_threshold, 0, NULL, NULL, 0.0);
00111     if (nr_nn > 0)
00112     {
00113       match_idx.push_back (cp);
00114       //kd_tree->annkSearch (point, 1, nnIdx, sqrDists, 0.0);
00115       kd_tree->annkFRSearch (transformed[cp], sqr_threshold, 1, nnIdx, sqrDists, 0.0);
00116       tmp_inliers.push_back (nnIdx[0]);
00117       pos_match++;
00118       pos_inliers++;
00119     }
00120     //annDeallocPt (point);
00121     // skip checking if there is no chance of producing a better match
00122     if (pos_match - cp/step < limit_match ||
00123         pos_inliers - cp/step < limit_inliers) // this will not exit early enough as tmp_inliers may contains duplicates as well, but it helps
00124       break;
00125   }
00126   return transformed;
00127 }
00128 
00129 /* ---[ */
00130 int
00131 main (int argc, char** argv)
00132 {
00133   srand (time (NULL));
00134   
00136   cANN::tictoc tt, t_global;
00137   double threshold = 0.03; // 3 cm
00138   double p_success = 0.999;
00139   double p2success = 0.999;
00140   int maxIterations = 100;
00141   int maxInnerIterations = 50;
00142   int minInnerIterations = 25;
00143   double percent = 50.0;
00144   int max_nr_nn = 1;
00145   double center_x, center_y;
00146   double center_th = -1;
00147   
00149   if (argc < 3)
00150   {
00151     print_error (stderr, "Syntax is: %s <scan>.pcd <result>.pcd <model1-N>.pcd <options>\n", argv[0]);
00152     fprintf (stderr, "  where options are: -threshold X = specify the inlier threshold (default "); print_value (stderr, "%g", threshold); fprintf (stderr, ")\n");
00153     fprintf (stderr, "                     -maxZdiff X  = specify the maximum height difference between corespoinding points (default "); print_value (stderr, "%g", maxZdiff); fprintf (stderr, ")\n");
00154     fprintf (stderr, "                     -max_iter N  = maximum number of outer RANSAC iteration (default "); print_value (stderr, "%d", maxIterations); fprintf (stderr, ")\n");
00155     fprintf (stderr, "                     -max2iter N  = maximum number of inner RANSAC iteration (default "); print_value (stderr, "%d", maxInnerIterations); fprintf (stderr, ")\n");
00156     fprintf (stderr, "                     -min2iter N  = minimum number of inner RANSAC iteration (default "); print_value (stderr, "%d", minInnerIterations); fprintf (stderr, ")\n");
00157     fprintf (stderr, "                     -p_success X = expected probability for a successful RANSAC match (default "); print_value (stderr, "%g", p_success); fprintf (stderr, ")\n");
00158     fprintf (stderr, "                     -p2success X = expected probability for finding the best transformation - exhaustive search if 1 (default "); print_value (stderr, "%g", p2success); fprintf (stderr, ")\n");
00159     fprintf (stderr, "                     -percent N   = use only this percent of model points for a pre-check (default "); print_value (stderr, "%g", percent); fprintf (stderr, ")\n");
00160     fprintf (stderr, "                     -max_nr_nn N = use at most this many neighbors when getting the final number of inliers (default "); print_value (stderr, "%g", max_nr_nn); fprintf (stderr, ")\n");
00161     fprintf (stderr, "                     -center_th X = check deviation in 2D of model center to specified x,y and reject if less than X -- disabled if < 0 (default "); print_value (stderr, "%g", center_th); fprintf (stderr, ")\n");
00162     fprintf (stderr, "                     -center X,Y  = if center_th > 0, the model's center should be closer to these coordinates in 2D than the threshold\n");
00163     fprintf (stderr, "\n");
00164     fprintf (stderr, "                     -message S   = message string to be saved as comment (default "); print_value (stderr, "\"\" - none"); fprintf (stderr, ")\n");
00165     fprintf (stderr, "                     -params 0/1  = disable/enable saving of parameters as comment (default "); print_value (stderr, "disabled"); fprintf (stderr, ")\n");
00166     fprintf (stderr, "                     -precision N = number of digits after decimal point (default "); print_value (stderr, "5"); fprintf (stderr, ")\n");
00167     return (-1);
00168   }
00169   
00171   ParseArgument (argc, argv, "-threshold", threshold); double sqr_threshold = threshold * threshold;
00172   ParseArgument (argc, argv, "-maxZdiff", maxZdiff);
00173   ParseArgument (argc, argv, "-p_success", p_success);
00174   ParseArgument (argc, argv, "-p2success", p2success);
00175   ParseArgument (argc, argv, "-max_iter", maxIterations);
00176   ParseArgument (argc, argv, "-max2iter", maxInnerIterations);
00177   ParseArgument (argc, argv, "-min2iter", minInnerIterations);
00178   ParseArgument (argc, argv, "-percent", percent);
00179   ParseArgument (argc, argv, "-center_th", center_th); double sqr_center_th = center_th * center_th;
00180   ParseRangeArguments (argc, argv, "-center", center_x, center_y);
00181   int precision = 5; ParseArgument (argc, argv, "-precision", precision); cout.precision (precision);
00182   bool binary_output = true; ParseArgument (argc, argv, "-binary", binary_output);
00183   string message; ParseArgument (argc, argv, "-message", message); 
00184   bool params = false; ParseArgument (argc, argv, "-params", params);
00185   ParseArgument (argc, argv, "-max_nr_nn", max_nr_nn);
00186   
00187   print_info (stderr, "Probability of failure: %g (global), %g (local)\n", 1-p_success, 1-p2success);
00188 
00190   if (max_nr_nn < 1)
00191     max_nr_nn = 1;
00192   nnIdx    = new ANNidx[max_nr_nn];
00193   sqrDists = new ANNdist[max_nr_nn];
00194 
00196   std::vector<int> pPCDFileIndices = ParseFileExtensionArgument (argc, argv, ".pcd");
00197   if (pPCDFileIndices.size () < 3)
00198   {
00199     print_error (stderr, "Need a scan, a result and at elast one model .PCD file!\n");
00200     return (-1);
00201   }
00202 
00204   PCD_Header header;
00205   print_info (stderr, "Loading ");
00206   print_value (stderr, "%s... ", argv[pPCDFileIndices.at (0)]);
00207   ANNpointArray points = LoadPCDFile (argv[pPCDFileIndices.at (0)], header);
00208   if (points == NULL)
00209     return (-1);
00210   fprintf (stderr, "[done : "); print_value (stderr, "%d %d", header.nr_points, header.dimID.size ()); fprintf (stderr, "D points]\n");
00211   print_info (stderr, "Available dimensions: "); print_value (stderr, "%s\n", getAvailableDimensions (header).c_str ());
00212   if (header.comments.size () != 0)
00213   {
00214     print_value (stderr, "Header comments:\n");
00215     fprintf (stderr, "%s", header.comments.c_str ());
00216   }
00217   //unsigned nr_dims = header.dimID.size ();
00218 
00220   int xIdx = getIndex (header, "x");
00221   if (xIdx != 0)
00222   {
00223     print_error (stderr, "XYZ coordinates not first dimensions!\n");
00224     return (-1);
00225   }
00226   nxIdx = getIndex (header, "nx");
00227   if (nxIdx == -1)
00228   {
00229     print_error (stderr, "Missing normal information!\n");
00230     return (-1);
00231   }
00232 
00234   kd_tree  = new ANNkd_tree (points, header.nr_points, 3, BUCKET_SIZE);
00235   //ANNkd_tree*  kd_tree  = new ANNkd_tree (points, header.nr_points, 3, BUCKET_SIZE);
00236   //ANNidxArray  nnIdx    = new ANNidx[1];
00237   //ANNdistArray sqrDists = new ANNdist[1];
00238   
00240   vector<PCD> models (pPCDFileIndices.size () - 2);
00259 
00267 
00277 
00278   t_global.tic ();
00279   
00281   vector<PCD>::iterator best_model;
00282   std::vector<int> best_model_inliers;
00283   ANNpointArray best_model_result = NULL;
00284   Matrix4f best_model_transform;
00285   int best_i = -1;
00286   int sum_pts = 0;
00288   for (unsigned i = 2; i < pPCDFileIndices.size (); i++)
00289   {
00290     PCD_Header header2;
00291     print_info (stderr, "\nLoading model ");
00292     print_value (stderr, "%s... ", argv[pPCDFileIndices.at (i)]);
00293     ANNpointArray points2 = LoadPCDFile (argv[pPCDFileIndices.at (i)], header2);
00294     if (points == NULL)
00295       return (-1);
00296     fprintf (stderr, "[done : "); print_value (stderr, "%d %d", header2.nr_points, header2.dimID.size ()); fprintf (stderr, "D points]\n");
00297     print_info (stderr, "Available dimensions: "); print_value (stderr, "%s\n", getAvailableDimensions (header2).c_str ());
00298     //if (header2.comments.size () != 0)
00299     //{
00300     //  print_value (stderr, "Header comments:\n");
00301     //  fprintf (stderr, "%s", header2.comments.c_str ());
00302     //}
00303   
00305     int xIdx = getIndex (header2, "x");
00306     if (xIdx != 0)
00307     {
00308       print_error (stderr, "XYZ coordinates not first dimensions!\n");
00309       return (-1);
00310     }
00311     model_nxIdx = getIndex (header2, "nx");
00312     if (nxIdx == -1)
00313     {
00314       print_error (stderr, "Missing normal information!\n");
00315       return (-1);
00316     }
00317   
00319     models[i-2].points = points2;
00320     models[i-2].header = header2;
00321     vector<PCD>::iterator model = models.begin () + i - 2;
00322     sum_pts += model->header.nr_points;
00323     
00324     //cerr << model->points << endl;
00325     //fprintf (stderr, "Loaded %s header: ", model->header.data_type?"ASCII":"BINARY"); print_value (stderr, "%d %d", model->header.nr_points, model->header.dimID.size ());
00326     //fprintf (stderr, "D "); print_value (stderr, "%s\n", getAvailableDimensions (model->header).c_str ());
00327     //if (model->header.comments.size () != 0)
00328     //  fprintf (stderr, "%s", model->header.comments.c_str ());
00329     
00330     int nr_check = (int)(model->header.nr_points * percent/100.0); // can introduce numeric errors, so we'll re-set it
00331     //int max_check = 1500;
00332     //if (nr_check > max_check) nr_check = max_check;
00333     int step = model->header.nr_points / nr_check;
00334     nr_check = (model->header.nr_points-1) / step + 1; // more accurate than nr_points/step?
00335     print_info (stderr, "Using %g%%: ", percent); print_value (stderr, "%d/%d", nr_check, model->header.nr_points); fprintf (stderr, " points for pre-checking, with a step of "); print_value (stderr, "%d\n", step);
00336     
00337     int iterations = 0;
00338     double k = (double)maxIterations; 
00339   
00340     std::vector<int> best_inliers, inliers;
00341     ANNpointArray best_result = NULL;
00342     Matrix4f best_transform;
00343   
00345     tt.tic ();
00346     
00348     int used_rot = 0;
00349     int trial = 0;
00350     long sum_iter = 0;
00351     while (iterations < k)
00352     {
00353       // Get a point from the scan
00354       int target = 0;
00355       while (header.nr_points <= (target = rand() / (RAND_MAX/header.nr_points)));
00356 
00357       // if normal not parallel to Z, use it to align model with the PCD
00358       bool use_rotation = false;
00359       if (acos (fabs (points[target][nxIdx+2])) > minAdiff)
00360       {
00361         used_rot++;
00362         use_rotation = true;
00363       }
00364   
00365       // Search for the best matching model point
00366       ANNpointArray result = NULL;
00367       Matrix4f match_transform;
00368       
00370       vector<int> best_match_idx;
00371       int best_source;
00372       
00374       bool found_correspondence = false;
00375       if (p2success < 1.0)
00376       {
00377         int iterations2 = 0;
00378         int trial2 = 0;
00379         double k2 = (double)maxInnerIterations; 
00380         while (iterations2 < k2 || iterations2 < minInnerIterations)
00381         {
00382           // get a suitable point from the model
00383           int source = 0;
00384           while (model->header.nr_points <= (source = rand() / (RAND_MAX/model->header.nr_points)));
00385 
00386           // skip points that have no correspondence, but give up if too many
00387           if (!potentialMatch (points[target], model->points[source], use_rotation))
00388           {
00389             trial2++;
00390             if (trial2 < maxInnerIterations)
00391               continue;
00392             else
00393               break;
00394           }
00395           trial2 = 0;
00396           found_correspondence = true;
00397           
00398           // get transform and evaluate
00399           Matrix4f transform = computeTransformation (points[target], model->points[source], use_rotation);
00400           vector<int> match_idx, tmp_inliers;
00401           ANNpointArray transformed = evaluateTransformation (transform, model, sqr_threshold, step, best_match_idx.size (), best_model_inliers.size (), match_idx, tmp_inliers);
00402           
00403 //          // check centroid
00404 //          if (SQR(transform(0,3)-center_x) + SQR(transform(1,3)-center_y) > sqr_center_th)
00405 //          {
00406 //            nr_skipped++;
00407 //            annDeallocPts (transformed);
00408 //            continue;
00409 //          }
00410 
00411           // save best transformation
00412           if (best_match_idx.size () < match_idx.size ())
00413           {
00414             if (result != NULL) annDeallocPts (result);
00415             match_transform = transform;
00416             best_match_idx = match_idx;
00417             result = transformed;
00418             inliers = tmp_inliers;
00419             best_source = source;
00420             
00421             // compute the k2 parameter (k2=log(z)/log(1-w^n))
00422             double w = match_idx.size () / (double)nr_check;
00423             double pNoOutliers = 1 - w;
00424             pNoOutliers = max (std::numeric_limits<double>::epsilon (), pNoOutliers);       // Avoid division by -Inf
00425             pNoOutliers = min (1 - std::numeric_limits<double>::epsilon (), pNoOutliers);   // Avoid division by 0.
00426             k2 = log (1 - p2success) / log (pNoOutliers);
00427             
00428             //print_info (stderr, "[MODEL] Trial %d out of %g: best is: %d/%d so far.\n" , iterations2, ceil (k2), best_match_idx.size (), nr_check);
00429           }
00430           else
00431             annDeallocPts (transformed);
00432           
00433           iterations2++;
00434           //#if DEBUG
00435           //print_info (stderr, "[MODEL] Trial %d out of %g: %d inliers (best is: %d so far).\n" , iterations2, ceil (k2), match_idx.size (), best_match_idx.size ());
00436           //#endif
00437           if (iterations2 >= maxInnerIterations)
00438           {
00439             //print_warning (stderr, "[MODEL-RANSAC] reached the maximum number of trials: %d\n", maxInnerIterations);
00440             break;
00441           }
00442         }
00443         //print_info (stderr, "[MODEL-RANSAC] done %d iterations\n", iterations2);// in ", iterations); print_value (stderr, "%g", tt.toc ()); fprintf (stderr, " seconds\n");
00444         sum_iter += iterations2;
00445       }
00446       else
00447       {
00448         for (int source = 0; source < model->header.nr_points; source++)
00449         {
00450           // check only point correspondences that have a high chance of producing a good transform
00451           if (!potentialMatch (points[target], model->points[source], use_rotation))
00452             continue;
00453           found_correspondence = true;
00454 
00455           // get transform and evaluate
00456           Matrix4f transform = computeTransformation (points[target], model->points[source], use_rotation);
00457           vector<int> match_idx, tmp_inliers;
00458           ANNpointArray transformed = evaluateTransformation (transform, model, sqr_threshold, step, best_match_idx.size (), best_model_inliers.size (), match_idx, tmp_inliers);
00459           
00460 //          // check centroid
00461 //          if (SQR(transform(0,3)-center_x) + SQR(transform(1,3)-center_y) > sqr_center_th)
00462 //          {
00463 //            nr_skipped++;
00464 //            annDeallocPts (transformed);
00465 //            continue;
00466 //          }
00467 
00468           // save best transformation
00469           if (best_match_idx.size () < match_idx.size ())
00470           {
00471             if (result != NULL) annDeallocPts (result);
00472             match_transform = transform;
00473             best_match_idx = match_idx;
00474             result = transformed;
00475             inliers = tmp_inliers;
00476             //print_info (stderr, "[MODEL] Trial %d out of %ld: best is: %d so far.\n" , source, model->header.nr_points, best_match_idx.size ());
00477             best_source = source;
00478           }
00479           else
00480             annDeallocPts (transformed);
00481         }
00482         sum_iter += model->header.nr_points;
00483       }
00484       
00485       // skip points that have no correspondence, but give up if too many
00486       if (!found_correspondence)
00487       {
00488         trial++;
00489         if (trial < maxIterations)
00490           continue;
00491         else
00492           break;
00493       }
00494       trial = 0;
00495 
00496       // get the final number of inliers
00497       if (best_match_idx.size () > 0)
00498       {
00499         //print_info (stderr, "[MODEL-%d] Best match (%d) has ", target, best_source);
00500         //fprintf (stderr, "%d/%g%%", best_match_idx.size (), 100*best_match_idx.size () / (double)nr_check);
00501         //fprintf (stderr, " inliers\n");
00502         
00506         //cerr << "before: " << inliers.size () << endl;
00508         inliers.clear ();
00509         inliers.reserve (model->header.nr_points);
00510         //int pos_match = model->header.nr_points;
00511         int pos_inliers = model->header.nr_points;
00512         for (int cp = 0; cp < model->header.nr_points; cp++)
00513         {
00515           Vector4f sp (model->points[cp][0], model->points[cp][1], model->points[cp][2], 1);
00516           Map<Vector3f> tp (&(result[cp][0]));
00517           tp = (match_transform * sp).start<3> ();
00518           // skip points that were already checked
00522           //if (cp == 4)
00523           //{
00524           //  cerr << sp.transpose () << " - " << tp.transpose () << endl;
00525           //  cerr << match_transform << endl;
00526           //}
00527           int nr_nn = kd_tree->annkFRSearch (result[cp], sqr_threshold, 0, NULL, NULL, 0.0);
00528           //cerr << nr_nn << "/";
00529           if (nr_nn > 0)
00530           {
00531             //cerr << nnIdx[0] << " ";
00532             //match_idx.push_back (cp);
00533             if (nr_nn > max_nr_nn)
00534               nr_nn = max_nr_nn;
00535             kd_tree->annkFRSearch (result[cp], sqr_threshold, nr_nn, nnIdx, sqrDists, 0.0);
00536             for (int nn = 0; nn < nr_nn; nn++)
00537               inliers.push_back (nnIdx[nn]);
00538             //pos_match++;
00539             pos_inliers++;
00540           }
00541           // skip checking if there is no chance of producing a better match
00542           if (//pos_match - cp < best_match_idx.size () ||
00543             pos_inliers - cp < (int)best_model_inliers.size ()) // this will not exit early enough as tmp_inliers may contains duplicates as well, but it helps
00544             break;
00545         }
00546         //cerr << "after: " << inliers.size () << endl;
00548       
00549         // Remove doubles from inliers list
00550         sort (inliers.begin (), inliers.end ());
00551         inliers.erase (unique (inliers.begin (), inliers.end ()), inliers.end ());
00552         //cerr << "unique: " << inliers.size () << endl;
00553         
00554         // Better match?
00555         if (best_inliers.size () < inliers.size ())
00556         {
00557           if (best_result != NULL) annDeallocPts (best_result);
00558           best_inliers = inliers;
00559           best_result = result;
00560           best_transform = match_transform;
00561           //inliers.clear ();
00562     
00563           // compute the k parameter (k=log(z)/log(1-w^n))
00564           double w = inliers.size () / (double)header.nr_points;
00565           double pNoOutliers = 1 - w;
00566           pNoOutliers = max (std::numeric_limits<double>::epsilon (), pNoOutliers);       // Avoid division by -Inf
00567           pNoOutliers = min (1 - std::numeric_limits<double>::epsilon (), pNoOutliers);   // Avoid division by 0.
00568           k = log (1 - p_success) / log (pNoOutliers);
00569         }
00570         else
00571           annDeallocPts (result);
00572       }
00573       //else
00574       //  print_error (stderr, "[MODEL] Unable to find a solution!\n");
00575   
00576       iterations++;
00577       //#if DEBUG
00578       //print_info (stderr, "Trial %d out of %g: %d inliers (best is: %d so far).\n" , iterations, ceil (k), inliers.size (), best_inliers.size ());
00579       //#endif
00580       if (iterations >= maxIterations)
00581       {
00582         print_warning (stderr, "RANSAC reached the maximum number of trials\n");
00583         break;
00584       }
00585     }
00586     print_info (stderr, "[RANSAC] done %d iterations (%d with rotations, %ld internal loops) in ", iterations, used_rot, sum_iter); print_value (stderr, "%g", tt.toc ()); fprintf (stderr, " seconds using %s search\n", p2success<1?"sac":"exchaustive");
00587   
00588     if (best_inliers.size () > 0)
00589     {
00590       print_info (stderr, "[RANSAC] Best match has ");
00591       print_value (stderr, "%d/%g%%", best_inliers.size (), 100*best_inliers.size () / (double)header.nr_points);
00592       fprintf (stderr, " inliers\n");
00593       
00594       // check centroid
00595       print_info(stderr, "Distance to (%g,%g) is %g\n", center_x, center_y, sqrt (SQR(best_transform(0,3)-center_x) + SQR(best_transform(1,3)-center_y)));
00596 
00597       if (best_model_inliers.size () < best_inliers.size ())
00598       {
00599         if (best_model_result != NULL) annDeallocPts (best_model_result);
00600         best_model_transform = best_transform;
00601         best_model = model;
00602         best_model_inliers = best_inliers;
00603         best_model_result = best_result;
00604         best_i = i;
00605       }
00606       else
00607         annDeallocPts (best_result);
00608     }
00609     else
00610       print_error (stderr, "[RANSAC] Unable to find a solution!\n");
00611   }
00612   
00614   if (best_model_inliers.size () > 0)
00615   {
00616     print_info (stderr, "\nBest model was loaded from model file ");
00617     print_value (stderr, "%d: %s\n", best_i-1, argv[pPCDFileIndices.at (best_i)]);
00618     print_info (stderr, "Best model out of ");
00619     print_value (stderr, "%d", pPCDFileIndices.size () - 2);
00620     fprintf (stderr, " has ");
00621     print_value (stderr, "%d/%g%%", best_model_inliers.size (), 100*best_model_inliers.size () / (double)header.nr_points);
00622     fprintf (stderr, " inliers\n");
00623     print_info (stderr, "Transformation between best model and scan:\n");
00624     cerr << best_model_transform << endl;
00625     
00626     // precaution in case not all points are saved
00627     //best_model->header.nr_points = best_model_result.size ();
00628     // (int)(model->header.nr_points * percent/100.0)
00629     best_model->header.dimID.resize (3);
00630     print_info (stderr, "Resulting dimensions: %s\n", getAvailableDimensions (best_model->header).c_str ());
00631     
00632     // Save
00633     if (message.size ())
00634       addCommentToHeader (best_model->header, "%s\n", message.c_str ());
00635     if (params)
00636     {
00637       string parameters (argv[0]);
00638       for (int i = 1; i < argc; i++)
00639       {
00640         parameters.append (" ");
00641         parameters.append (argv[i]);
00642       }
00643       addCommentToHeader (best_model->header, "%s\n", parameters.c_str ());
00644     }
00645     print_info (stderr, "Writing resulting "); print_value (stderr, "%d", best_model->header.nr_points); fprintf (stderr, " points to "); print_value (stderr, "%s ", argv[pPCDFileIndices.at (1)]);// fprintf (stderr, "... ");
00646     best_model->header.data_type = binary_output ? PCD_BINARY : PCD_ASCII;
00647     SavePCDFile (argv[pPCDFileIndices.at (1)], best_model_result, best_model->header, precision);
00648     fprintf (stderr, "[done]\n");
00649   }
00650   else
00651     print_error (stderr, "Did not find a suitable solution!\n");
00652   
00654   print_info (stderr, "[done searching through "); print_value (stderr, "%d", sum_pts); fprintf (stderr, " model points in "); print_value (stderr, "%g", t_global.toc ()); fprintf (stderr, " seconds]\n");
00655   return 0;
00656 }
00657 /* ]--- */
00658 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


pcl_cloud_tools
Author(s): Nico Blodow, Zoltan-Csaba Marton, Dejan Pangercic
autogenerated on Sun Oct 6 2013 11:58:08