added dynamic outlier rejection (with unit tests) for smart pose factors

gtsam_unstable/slam/SmartProjectionFactor.h

@@ -95,6 +95,13 @@ protected:
   /// shorthand for base class type
   typedef SmartFactorBase<POSE, CALIBRATION, D> Base;
 
+  double landmarkDistanceThreshold_; // if the landmark is triangulated at a
+  // distance larger than that the factor is considered degenerate
+
+  double dynamicOutlierRejectionThreshold_; // if this is nonnegative the factor will check if the
+  // average reprojection error is smaller than this threshold after triangulation,
+  // and the factor is disregarded if the error is large
+
   /// shorthand for this class
   typedef SmartProjectionFactor<POSE, LANDMARK, CALIBRATION, D> This;
 
@@ -119,12 +126,15 @@ public:
   SmartProjectionFactor(const double rankTol, const double linThreshold,
       const bool manageDegeneracy, const bool enableEPI,
       boost::optional<POSE> body_P_sensor = boost::none,
-      SmartFactorStatePtr state = SmartFactorStatePtr(
+      double landmarkDistanceThreshold = 1e10,
-          new SmartProjectionFactorState())) :
+      double dynamicOutlierRejectionThreshold = -1,
+      SmartFactorStatePtr state = SmartFactorStatePtr(new SmartProjectionFactorState())) :
       Base(body_P_sensor), rankTolerance_(rankTol), retriangulationThreshold_(
           1e-5), manageDegeneracy_(manageDegeneracy), enableEPI_(enableEPI), linearizationThreshold_(
           linThreshold), degenerate_(false), cheiralityException_(false), throwCheirality_(
-          false), verboseCheirality_(false), state_(state) {
+          false), verboseCheirality_(false), state_(state),
+          landmarkDistanceThreshold_(landmarkDistanceThreshold),
+          dynamicOutlierRejectionThreshold_(dynamicOutlierRejectionThreshold) {
   }
 
   /** Virtual destructor */
@@ -238,6 +248,31 @@ public:
             rankTolerance_, enableEPI_);
         degenerate_ = false;
         cheiralityException_ = false;
+
+        // Check landmark distance and reprojection errors to avoid outliers
+        double totalReprojError = 0.0;
+        size_t i=0;
+        BOOST_FOREACH(const Camera& camera, cameras) {
+          Point3 cameraTranslation = camera.pose().translation();
+          // we discard smart factors corresponding to points that are far away
+          if(cameraTranslation.distance(point_) > landmarkDistanceThreshold_){
+            degenerate_ = true;
+            break;
+          }
+          const Point2& zi = this->measured_.at(i);
+          try {
+            Point2 reprojectionError(camera.project(point_) - zi);
+            totalReprojError += reprojectionError.vector().norm();
+          } catch (CheiralityException& e) {
+            cheiralityException_ = true;
+          }
+          i += 1;
+        }
+        // we discard smart factors that have large reprojection error
+        if(dynamicOutlierRejectionThreshold_ > 0 &&
+            totalReprojError/m > dynamicOutlierRejectionThreshold_)
+          degenerate_ = true;
+
       } catch (TriangulationUnderconstrainedException&) {
         // if TriangulationUnderconstrainedException can be
         // 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before

gtsam_unstable/slam/SmartProjectionPoseFactor.h

@@ -59,8 +59,10 @@ public:
   SmartProjectionPoseFactor(const double rankTol = 1,
       const double linThreshold = -1, const bool manageDegeneracy = false,
       const bool enableEPI = false, boost::optional<POSE> body_P_sensor = boost::none,
-      linearizationType linearizeTo = HESSIAN) :
+      linearizationType linearizeTo = HESSIAN, double landmarkDistanceThreshold = 1e10,
-        Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor), linearizeTo_(linearizeTo) {}
+      double dynamicOutlierRejectionThreshold = -1) :
+        Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor,
+        landmarkDistanceThreshold, dynamicOutlierRejectionThreshold), linearizeTo_(linearizeTo) {}
 
   /** Virtual destructor */
   virtual ~SmartProjectionPoseFactor() {}

gtsam_unstable/slam/tests/testSmartProjectionPoseFactor.cpp

@@ -431,6 +431,147 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ){
   EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
 }
 
+/* *************************************************************************/
+TEST( SmartProjectionPoseFactor, landmarkDistance ){
+
+  double excludeLandmarksFutherThanDist = 2;
+
+  std::vector<Key> views;
+  views.push_back(x1);
+  views.push_back(x2);
+  views.push_back(x3);
+
+  // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
+  Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
+  SimpleCamera cam1(pose1, *K);
+  // create second camera 1 meter to the right of first camera
+  Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0));
+  SimpleCamera cam2(pose2, *K);
+  // create third camera 1 meter above the first camera
+  Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0));
+  SimpleCamera cam3(pose3, *K);
+
+  // three landmarks ~5 meters infront of camera
+  Point3 landmark1(5, 0.5, 1.2);
+  Point3 landmark2(5, -0.5, 1.2);
+  Point3 landmark3(3, 0, 3.0);
+
+  vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;
+
+  // 1. Project three landmarks into three cameras and triangulate
+  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
+  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
+  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
+
+  SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist));
+  smartFactor1->add(measurements_cam1, views, model, K);
+
+  SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist));
+  smartFactor2->add(measurements_cam2, views, model, K);
+
+  SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist));
+  smartFactor3->add(measurements_cam3, views, model, K);
+
+  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
+
+  NonlinearFactorGraph graph;
+  graph.push_back(smartFactor1);
+  graph.push_back(smartFactor2);
+  graph.push_back(smartFactor3);
+  graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
+  graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));
+
+  //  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
+  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise
+  Values values;
+  values.insert(x1, pose1);
+  values.insert(x2, pose2);
+  values.insert(x3, pose3*noise_pose);
+
+  // All factors are disabled and pose should remain where it is
+  LevenbergMarquardtParams params;
+  Values result;
+  LevenbergMarquardtOptimizer optimizer(graph, values, params);
+  result = optimizer.optimize();
+  EXPECT(assert_equal(values.at<Pose3>(x3),result.at<Pose3>(x3)));
+}
+
+/* *************************************************************************/
+TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ){
+
+  double excludeLandmarksFutherThanDist = 1e10;
+  double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error
+
+  std::vector<Key> views;
+  views.push_back(x1);
+  views.push_back(x2);
+  views.push_back(x3);
+
+  // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
+  Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
+  SimpleCamera cam1(pose1, *K);
+  // create second camera 1 meter to the right of first camera
+  Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0));
+  SimpleCamera cam2(pose2, *K);
+  // create third camera 1 meter above the first camera
+  Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0));
+  SimpleCamera cam3(pose3, *K);
+
+  // three landmarks ~5 meters infront of camera
+  Point3 landmark1(5, 0.5, 1.2);
+  Point3 landmark2(5, -0.5, 1.2);
+  Point3 landmark3(3, 0, 3.0);
+  Point3 landmark4(5, -0.5, 1);
+
+  vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3, measurements_cam4;
+
+  // 1. Project three landmarks into three cameras and triangulate
+  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
+  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
+  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
+  projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4);
+  measurements_cam4.at(0) = measurements_cam4.at(0) + Point2(10,10); // add outlier
+
+  SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none,
+      JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
+  smartFactor1->add(measurements_cam1, views, model, K);
+
+  SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD,
+      excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
+  smartFactor2->add(measurements_cam2, views, model, K);
+
+  SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD,
+      excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
+  smartFactor3->add(measurements_cam3, views, model, K);
+
+  SmartFactor::shared_ptr smartFactor4(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD,
+      excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
+  smartFactor4->add(measurements_cam4, views, model, K);
+
+  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
+
+  NonlinearFactorGraph graph;
+  graph.push_back(smartFactor1);
+  graph.push_back(smartFactor2);
+  graph.push_back(smartFactor3);
+  graph.push_back(smartFactor4);
+  graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
+  graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));
+
+  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise
+  Values values;
+  values.insert(x1, pose1);
+  values.insert(x2, pose2);
+  values.insert(x3, pose3);
+
+  // All factors are disabled and pose should remain where it is
+  LevenbergMarquardtParams params;
+  Values result;
+  LevenbergMarquardtOptimizer optimizer(graph, values, params);
+  result = optimizer.optimize();
+  EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
+}
+
 /* *************************************************************************/
 TEST( SmartProjectionPoseFactor, jacobianQ ){