00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
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
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
00057 if ((p0[2] - p1[2]) < maxZdiff)
00058 {
00059
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
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));
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
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
00100 Map<Vector3f> tp (&(transformed[cp][0]));
00101 tp = (transform * sp).start<3> ();
00102
00103
00104
00105
00106
00107
00108
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
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
00121
00122 if (pos_match - cp/step < limit_match ||
00123 pos_inliers - cp/step < limit_inliers)
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;
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
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
00236
00237
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
00299
00300
00301
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
00325
00326
00327
00328
00329
00330 int nr_check = (int)(model->header.nr_points * percent/100.0);
00331
00332
00333 int step = model->header.nr_points / nr_check;
00334 nr_check = (model->header.nr_points-1) / step + 1;
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
00354 int target = 0;
00355 while (header.nr_points <= (target = rand() / (RAND_MAX/header.nr_points)));
00356
00357
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
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
00383 int source = 0;
00384 while (model->header.nr_points <= (source = rand() / (RAND_MAX/model->header.nr_points)));
00385
00386
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
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
00404
00405
00406
00407
00408
00409
00410
00411
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
00422 double w = match_idx.size () / (double)nr_check;
00423 double pNoOutliers = 1 - w;
00424 pNoOutliers = max (std::numeric_limits<double>::epsilon (), pNoOutliers);
00425 pNoOutliers = min (1 - std::numeric_limits<double>::epsilon (), pNoOutliers);
00426 k2 = log (1 - p2success) / log (pNoOutliers);
00427
00428
00429 }
00430 else
00431 annDeallocPts (transformed);
00432
00433 iterations2++;
00434
00435
00436
00437 if (iterations2 >= maxInnerIterations)
00438 {
00439
00440 break;
00441 }
00442 }
00443
00444 sum_iter += iterations2;
00445 }
00446 else
00447 {
00448 for (int source = 0; source < model->header.nr_points; source++)
00449 {
00450
00451 if (!potentialMatch (points[target], model->points[source], use_rotation))
00452 continue;
00453 found_correspondence = true;
00454
00455
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
00461
00462
00463
00464
00465
00466
00467
00468
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
00477 best_source = source;
00478 }
00479 else
00480 annDeallocPts (transformed);
00481 }
00482 sum_iter += model->header.nr_points;
00483 }
00484
00485
00486 if (!found_correspondence)
00487 {
00488 trial++;
00489 if (trial < maxIterations)
00490 continue;
00491 else
00492 break;
00493 }
00494 trial = 0;
00495
00496
00497 if (best_match_idx.size () > 0)
00498 {
00499
00500
00501
00502
00506
00508 inliers.clear ();
00509 inliers.reserve (model->header.nr_points);
00510
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
00522
00523
00524
00525
00526
00527 int nr_nn = kd_tree->annkFRSearch (result[cp], sqr_threshold, 0, NULL, NULL, 0.0);
00528
00529 if (nr_nn > 0)
00530 {
00531
00532
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
00539 pos_inliers++;
00540 }
00541
00542 if (
00543 pos_inliers - cp < (int)best_model_inliers.size ())
00544 break;
00545 }
00546
00548
00549
00550 sort (inliers.begin (), inliers.end ());
00551 inliers.erase (unique (inliers.begin (), inliers.end ()), inliers.end ());
00552
00553
00554
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
00562
00563
00564 double w = inliers.size () / (double)header.nr_points;
00565 double pNoOutliers = 1 - w;
00566 pNoOutliers = max (std::numeric_limits<double>::epsilon (), pNoOutliers);
00567 pNoOutliers = min (1 - std::numeric_limits<double>::epsilon (), pNoOutliers);
00568 k = log (1 - p_success) / log (pNoOutliers);
00569 }
00570 else
00571 annDeallocPts (result);
00572 }
00573
00574
00575
00576 iterations++;
00577
00578
00579
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
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
00627
00628
00629 best_model->header.dimID.resize (3);
00630 print_info (stderr, "Resulting dimensions: %s\n", getAvailableDimensions (best_model->header).c_str ());
00631
00632
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)]);
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