Merge branch 'develop' into feature/heterogeneousSmartFactorNoise

# Conflicts:
#	gtsam_unstable/slam/SmartStereoProjectionFactor.h

gtsam/geometry/tests/testTriangulation.cpp

@@ -246,6 +246,59 @@ TEST( triangulation, fourPoses_distinct_Ks) {
 #endif
 }
 
+//******************************************************************************
+TEST( triangulation, outliersAndFarLandmarks) {
+  Cal3_S2 K1(1500, 1200, 0, 640, 480);
+  // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
+  SimpleCamera camera1(pose1, K1);
+
+  // create second camera 1 meter to the right of first camera
+  Cal3_S2 K2(1600, 1300, 0, 650, 440);
+  SimpleCamera camera2(pose2, K2);
+
+  // 1. Project two landmarks into two cameras and triangulate
+  Point2 z1 = camera1.project(landmark);
+  Point2 z2 = camera2.project(landmark);
+
+  vector<SimpleCamera> cameras;
+  vector<Point2> measurements;
+
+  cameras += camera1, camera2;
+  measurements += z1, z2;
+
+  double landmarkDistanceThreshold = 10; // landmark is closer than that
+  TriangulationParameters params(1.0, false, landmarkDistanceThreshold); // all default except landmarkDistanceThreshold
+  TriangulationResult actual = triangulateSafe(cameras,measurements,params);
+  EXPECT(assert_equal(landmark, *actual, 1e-2));
+  EXPECT(actual.valid());
+
+  landmarkDistanceThreshold = 4; // landmark is farther than that
+  TriangulationParameters params2(1.0, false, landmarkDistanceThreshold); // all default except landmarkDistanceThreshold
+  actual = triangulateSafe(cameras,measurements,params2);
+  EXPECT(actual.farPoint());
+
+  // 3. Add a slightly rotated third camera above with a wrong measurement (OUTLIER)
+  Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
+  Cal3_S2 K3(700, 500, 0, 640, 480);
+  SimpleCamera camera3(pose3, K3);
+  Point2 z3 = camera3.project(landmark);
+
+  cameras += camera3;
+  measurements += z3 + Point2(10, -10);
+
+  landmarkDistanceThreshold = 10; // landmark is closer than that
+  double outlierThreshold = 100; // loose, the outlier is going to pass
+  TriangulationParameters params3(1.0, false, landmarkDistanceThreshold,outlierThreshold);
+  actual = triangulateSafe(cameras,measurements,params3);
+  EXPECT(actual.valid());
+
+  // now set stricter threshold for outlier rejection
+  outlierThreshold = 5; // tighter, the outlier is not going to pass
+  TriangulationParameters params4(1.0, false, landmarkDistanceThreshold,outlierThreshold);
+  actual = triangulateSafe(cameras,measurements,params4);
+  EXPECT(actual.outlier());
+}
+
 //******************************************************************************
 TEST( triangulation, twoIdenticalPoses) {
   // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)

gtsam/geometry/triangulation.h

@@ -322,6 +322,7 @@ Point3 triangulatePoint3(
 struct TriangulationParameters {
 
   double rankTolerance; ///< threshold to decide whether triangulation is result.degenerate
+  ///< (the rank is the number of singular values of the triangulation matrix which are larger than rankTolerance)
   bool enableEPI; ///< if set to true, will refine triangulation using LM
 
   /**
@@ -383,7 +384,7 @@ private:
  */
 class TriangulationResult: public boost::optional<Point3> {
   enum Status {
-    VALID, DEGENERATE, BEHIND_CAMERA
+    VALID, DEGENERATE, BEHIND_CAMERA, OUTLIER, FAR_POINT
   };
   Status status_;
   TriangulationResult(Status s) :
@@ -406,12 +407,27 @@ public:
   static TriangulationResult Degenerate() {
     return TriangulationResult(DEGENERATE);
   }
+  static TriangulationResult Outlier() {
+    return TriangulationResult(OUTLIER);
+  }
+  static TriangulationResult FarPoint() {
+    return TriangulationResult(FAR_POINT);
+  }
   static TriangulationResult BehindCamera() {
     return TriangulationResult(BEHIND_CAMERA);
   }
+  bool valid() const {
+    return status_ == VALID;
+  }
   bool degenerate() const {
     return status_ == DEGENERATE;
   }
+  bool outlier() const {
+    return status_ == OUTLIER;
+  }
+  bool farPoint() const {
+    return status_ == FAR_POINT;
+  }
   bool behindCamera() const {
     return status_ == BEHIND_CAMERA;
   }
@@ -454,13 +470,13 @@ TriangulationResult triangulateSafe(const std::vector<CAMERA>& cameras,
 
       // Check landmark distance and re-projection errors to avoid outliers
       size_t i = 0;
-      double totalReprojError = 0.0;
+      double maxReprojError = 0.0;
       for(const CAMERA& camera: cameras) {
         const Pose3& pose = camera.pose();
         if (params.landmarkDistanceThreshold > 0
             && distance3(pose.translation(), point)
                 > params.landmarkDistanceThreshold)
-          return TriangulationResult::Degenerate();
+          return TriangulationResult::FarPoint();
 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
         // verify that the triangulated point lies in front of all cameras
         // Only needed if this was not yet handled by exception
@@ -472,14 +488,14 @@ TriangulationResult triangulateSafe(const std::vector<CAMERA>& cameras,
         if (params.dynamicOutlierRejectionThreshold > 0) {
           const Point2& zi = measured.at(i);
           Point2 reprojectionError(camera.project(point) - zi);
-          totalReprojError += reprojectionError.norm();
+          maxReprojError = std::max(maxReprojError, reprojectionError.norm());
         }
         i += 1;
       }
       // Flag as degenerate if average reprojection error is too large
       if (params.dynamicOutlierRejectionThreshold > 0
-          && totalReprojError / m > params.dynamicOutlierRejectionThreshold)
-        return TriangulationResult::Degenerate();
+          && maxReprojError > params.dynamicOutlierRejectionThreshold)
+        return TriangulationResult::Outlier();
 
       // all good!
       return TriangulationResult(point);

gtsam/slam/SmartProjectionFactor.h

@@ -54,9 +54,9 @@ struct GTSAM_EXPORT SmartProjectionParams {
   // Constructor
   SmartProjectionParams(LinearizationMode linMode = HESSIAN,
       DegeneracyMode degMode = IGNORE_DEGENERACY, bool throwCheirality = false,
-      bool verboseCheirality = false) :
+      bool verboseCheirality = false, double retriangulationTh = 1e-5) :
       linearizationMode(linMode), degeneracyMode(degMode), retriangulationThreshold(
-          1e-5), throwCheirality(throwCheirality), verboseCheirality(
+    		  retriangulationTh), throwCheirality(throwCheirality), verboseCheirality(
           verboseCheirality) {
   }
 
@@ -84,9 +84,15 @@ struct GTSAM_EXPORT SmartProjectionParams {
   bool getThrowCheirality() const {
     return throwCheirality;
   }
+  double getRetriangulationThreshold() const {
+	return retriangulationThreshold;
+  }
   void setLinearizationMode(LinearizationMode linMode) {
     linearizationMode = linMode;
   }
+  void setRetriangulationThreshold(double retriangulationTh) {
+	retriangulationThreshold = retriangulationTh;
+  }
   void setDegeneracyMode(DegeneracyMode degMode) {
     degeneracyMode = degMode;
   }
@@ -518,19 +524,19 @@ public:
   }
 
   /// Is result valid?
-  bool isValid() const {
-    return result_;
-  }
+  bool isValid() const { return result_.valid(); }
 
   /** return the degenerate state */
-  bool isDegenerate() const {
-    return result_.degenerate();
-  }
+  bool isDegenerate() const { return result_.degenerate(); }
 
   /** return the cheirality status flag */
-  bool isPointBehindCamera() const {
-    return result_.behindCamera();
-  }
+  bool isPointBehindCamera() const { return result_.behindCamera(); }
+
+  /** return the outlier state */
+  bool isOutlier() const { return result_.outlier(); }
+
+  /** return the farPoint state */
+  bool isFarPoint() const { return result_.farPoint(); }
 
 private:
 

gtsam/slam/tests/testSmartProjectionPoseFactor.cpp

@@ -81,6 +81,17 @@ TEST( SmartProjectionPoseFactor, Constructor4) {
   factor1.add(measurement1, x1);
 }
 
+/* ************************************************************************* */
+TEST( SmartProjectionPoseFactor, params) {
+  using namespace vanillaPose;
+  SmartProjectionParams params;
+  double rt = params.getRetriangulationThreshold();
+  EXPECT_DOUBLES_EQUAL(1e-5, rt, 1e-7);
+  params.setRetriangulationThreshold(1e-3);
+  rt = params.getRetriangulationThreshold();
+  EXPECT_DOUBLES_EQUAL(1e-3, rt, 1e-7);
+}
+
 /* ************************************************************************* */
 TEST( SmartProjectionPoseFactor, Equals ) {
   using namespace vanillaPose;

gtsam_unstable/slam/SmartStereoProjectionFactor.h

@@ -527,19 +527,19 @@ public:
     }
 
     /// Is result valid?
-    bool isValid() const {
-      return bool(result_);
-    }
+    bool isValid() const { return result_.valid(); }
 
     /** return the degenerate state */
-    bool isDegenerate() const {
-      return result_.degenerate();
-    }
+    bool isDegenerate() const { return result_.degenerate(); }
 
     /** return the cheirality status flag */
-    bool isPointBehindCamera() const {
-      return result_.behindCamera();
-    }
+    bool isPointBehindCamera() const { return result_.behindCamera(); }
+
+    /** return the outlier state */
+    bool isOutlier() const { return result_.outlier(); }
+
+    /** return the farPoint state */
+    bool isFarPoint() const { return result_.farPoint(); }
 
 private:
 

gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp

@@ -877,7 +877,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
   EXPECT(smartFactor1->point());
   EXPECT(smartFactor2->point());
   EXPECT(smartFactor3->point());
-  EXPECT(smartFactor4->point().degenerate());
+  EXPECT(smartFactor4->point().outlier());
   EXPECT(smartFactor4b->point());
 
   // Factor 4 is disabled, pose 3 stays put