From edcf94591df94596ff6c134225377a13178a153e Mon Sep 17 00:00:00 2001
From: lcarlone <lcarlone@mit.edu>
Date: Sat, 6 Aug 2016 23:27:54 -0400
Subject: [PATCH] added functions to get/set smart stereo parameters with unit
 test

---
 .../slam/SmartStereoProjectionFactor.h        |  8 +++++
 .../testSmartStereoProjectionPoseFactor.cpp   | 29 +++++++++++++++++++
 2 files changed, 37 insertions(+)

diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h
index 56b5d85de..9b9d3c935 100644
--- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h
+++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h
@@ -74,6 +74,7 @@ namespace gtsam {
      std::cout << triangulation << std::endl;
    }
 
+   // get class variables
    LinearizationMode getLinearizationMode() const {
      return linearizationMode;
    }
@@ -89,12 +90,19 @@ namespace gtsam {
    bool getThrowCheirality() const {
      return throwCheirality;
    }
+   double getRetriangulationThreshold() const {
+     return retriangulationThreshold;
+   }
+   // set class variables
    void setLinearizationMode(LinearizationMode linMode) {
      linearizationMode = linMode;
    }
    void setDegeneracyMode(DegeneracyMode degMode) {
      degeneracyMode = degMode;
    }
+   void setRetriangulationThreshold(double retriangulationTh) {
+     retriangulationThreshold = retriangulationTh;
+   }
    void setRankTolerance(double rankTol) {
      triangulation.rankTolerance = rankTol;
    }
diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp
index b17347edb..c01ee98f4 100644
--- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp
+++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp
@@ -79,6 +79,35 @@ vector<StereoPoint2> stereo_projectToMultipleCameras(const StereoCamera& cam1,
 
 LevenbergMarquardtParams lm_params;
 
+/* ************************************************************************* */
+TEST( SmartStereoProjectionPoseFactor, params) {
+  SmartStereoProjectionParams p;
+
+  // check default values and "get"
+  EXPECT(p.getLinearizationMode() == HESSIAN);
+  EXPECT(p.getDegeneracyMode() == IGNORE_DEGENERACY);
+  EXPECT_DOUBLES_EQUAL(p.getRetriangulationThreshold(), 1e-5, 1e-9);
+  EXPECT(p.getVerboseCheirality() == false);
+  EXPECT(p.getThrowCheirality() == false);
+
+  // check "set"
+  p.setLinearizationMode(JACOBIAN_SVD);
+  p.setDegeneracyMode(ZERO_ON_DEGENERACY);
+  p.setRankTolerance(100);
+  p.setEnableEPI(true);
+  p.setLandmarkDistanceThreshold(200);
+  p.setDynamicOutlierRejectionThreshold(3);
+  p.setRetriangulationThreshold(1e-2);
+
+  EXPECT(p.getLinearizationMode() == JACOBIAN_SVD);
+  EXPECT(p.getDegeneracyMode() == ZERO_ON_DEGENERACY);
+  EXPECT_DOUBLES_EQUAL(p.getTriangulationParameters().rankTolerance, 100, 1e-5);
+  EXPECT(p.getTriangulationParameters().enableEPI == true);
+  EXPECT_DOUBLES_EQUAL(p.getTriangulationParameters().landmarkDistanceThreshold, 200, 1e-5);
+  EXPECT_DOUBLES_EQUAL(p.getTriangulationParameters().dynamicOutlierRejectionThreshold, 3, 1e-5);
+  EXPECT_DOUBLES_EQUAL(p.getRetriangulationThreshold(), 1e-2, 1e-5);
+}
+
 /* ************************************************************************* */
 TEST( SmartStereoProjectionPoseFactor, Constructor) {
   SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor(model));