use ransac changes

maps_server/src/libmaps/RansacGeneric.hpp

@@ -20,6 +20,7 @@ class RansacGeneric {
 public:
   RansacGeneric() {
     setMaximumIterations(5000);
+    setSkippedIterationFactor(1);
     setGoodSolutionProbability(1-1e-8);
     setRefineUsingInliers(false);
     setMaximumError(-1);
@@ -28,6 +29,9 @@ class RansacGeneric {
   virtual ~RansacGeneric() {}
 
   void setMaximumIterations(const int iIters) { mMaximumIterations = iIters; }
+  void setSkippedIterationFactor(const double iFactor) {
+    mSkippedIterationFactor = iFactor;
+  }
   void setGoodSolutionProbability(const double iProb) {
     mGoodSolutionProbability = iProb;
   }
@@ -50,12 +54,13 @@ class RansacGeneric {
     const double epsilon = 1e-10;
 
     // best results are currently invalid
-    double bestScore = -1;
+    int bestScore = 0;
     bool success = false;
 
     // start number of iterations as infinite, then reduce as we go
     double numIterationsNeeded = 1e10;
     int iterationCount = 0;
+    int skippedSampleCount = 0;
 
     // for random sample index generation
     std::vector<int> allIndices(n);
@@ -80,6 +85,17 @@ class RansacGeneric {
       // compute errors over all data points
       std::vector<double> errors2 = iProblem.computeSquaredErrors(solution);
 
+      // check whether this is a valid sample
+      // TODO: this should be done via a method in Problem class, but would
+      // require changing all existing usages to include that method
+      if (errors2.size() == 0) {
+        ++skippedSampleCount;
+        if (skippedSampleCount >=
+            mMaximumIterations*mSkippedIterationFactor) break;
+        continue;
+      }
+      skippedSampleCount = 0;
+
       // compute error threshold to be applied to each term
       double thresh = mMaximumError;
       if (thresh < 0) {
@@ -100,7 +116,7 @@ class RansacGeneric {
       }
 
       // if this is the best score, update solution and convergence criteria
-      double score = inliers.size();
+      int score = inliers.size();
       if (score > bestScore) {
         bestScore = score;
         result.mInliers = inliers;
@@ -126,7 +142,7 @@ class RansacGeneric {
     result.mNumIterations = iterationCount;
 
     // refine result using all inliers if specified
-    if (mRefineUsingInliers) {
+    if (result.mSuccess && mRefineUsingInliers) {
       result.mSolution = iProblem.estimate(result.mInliers);
     }
 
@@ -137,6 +153,7 @@ class RansacGeneric {
 protected:
   bool mRefineUsingInliers;
   int mMaximumIterations;
+  double mSkippedIterationFactor;
   double mGoodSolutionProbability;
   double mMaximumError;
 };

plane-seg/src/PlaneFitter.cpp

@@ -2,7 +2,7 @@
 
 #include <limits>
 
-#include <maps_utils/RansacGeneric.hpp>
+#include <maps/RansacGeneric.hpp>
 
 using namespace planeseg;