got rid of second constructor

gtsam/slam/SmartProjectionRigFactor.h

@@ -105,29 +105,6 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
           "linearizationMode must be set to HESSIAN");
   }
 
-  /**
-   * Constructor
-   * @param sharedNoiseModel isotropic noise model for the 2D feature
-   * measurements
-   * @param camera single camera (fixed poses wrt body and intrinsics)
-   * @param params parameters for the smart projection factors
-   */
-  SmartProjectionRigFactor(
-      const SharedNoiseModel& sharedNoiseModel, const Camera& camera,
-      const SmartProjectionParams& params = SmartProjectionParams())
-      : Base(sharedNoiseModel, params) {
-    // throw exception if configuration is not supported by this factor
-    if (Base::params_.degeneracyMode != gtsam::ZERO_ON_DEGENERACY)
-      throw std::runtime_error(
-          "SmartProjectionRigFactor: "
-          "degeneracyMode must be set to ZERO_ON_DEGENERACY");
-    if (Base::params_.linearizationMode != gtsam::HESSIAN)
-      throw std::runtime_error(
-          "SmartProjectionRigFactor: "
-          "linearizationMode must be set to HESSIAN");
-    cameraRig_.push_back(camera);
-  }
-
   /** Virtual destructor */
   ~SmartProjectionRigFactor() override = default;
 

gtsam/slam/tests/testSmartProjectionRigFactor.cpp

@@ -29,7 +29,7 @@
 #include <iostream>
 
 #include "smartFactorScenarios.h"
-#define DISABLE_TIMING
+//#define DISABLE_TIMING
 
 using namespace boost::assign;
 using namespace std::placeholders;
@@ -110,17 +110,6 @@ TEST(SmartProjectionRigFactor, Constructor4) {
   factor1.add(measurement1, x1, cameraId1);
 }
 
-/* ************************************************************************* */
-TEST(SmartProjectionRigFactor, Constructor5) {
-  using namespace vanillaRig;
-  SmartProjectionParams params2(
-      gtsam::HESSIAN,
-      gtsam::ZERO_ON_DEGENERACY);  // only config that works with rig factors
-  params2.setRankTolerance(rankTol);
-  SmartRigFactor factor1(model, Camera(Pose3::identity(), sharedK), params2);
-  factor1.add(measurement1, x1, cameraId1);
-}
-
 /* ************************************************************************* */
 TEST(SmartProjectionRigFactor, Equals) {
   using namespace vanillaRig;
@@ -138,8 +127,8 @@ TEST(SmartProjectionRigFactor, Equals) {
   CHECK(assert_equal(*factor1, *factor2));
 
   SmartRigFactor::shared_ptr factor3(
-      new SmartRigFactor(model, Camera(Pose3::identity(), sharedK), params));
+      new SmartRigFactor(model, cameraRig, params));
-  factor3->add(measurement1, x1);  // now use default
+  factor3->add(measurement1, x1);  // now use default camera ID
 
   CHECK(assert_equal(*factor1, *factor3));
 }
@@ -152,7 +141,10 @@ TEST(SmartProjectionRigFactor, noiseless) {
   Point2 level_uv = level_camera.project(landmark1);
   Point2 level_uv_right = level_camera_right.project(landmark1);
 
-  SmartRigFactor factor(model, Camera(Pose3::identity(), sharedK), params);
+  Cameras cameraRig;  // single camera in the rig
+  cameraRig.push_back(Camera(Pose3::identity(), sharedK));
+
+  SmartRigFactor factor(model, cameraRig, params);
   factor.add(level_uv, x1);  // both taken from the same camera
   factor.add(level_uv_right, x2);
 
@@ -504,8 +496,11 @@ TEST(SmartProjectionRigFactor, Factors) {
   KeyVector views{x1, x2};
   FastVector<size_t> cameraIds{0, 0};
 
+  Cameras cameraRig;  // single camera in the rig
+  cameraRig.push_back(Camera(Pose3::identity(), sharedK));
+
   SmartRigFactor::shared_ptr smartFactor1 = boost::make_shared<SmartRigFactor>(
-      model, Camera(Pose3::identity(), sharedK), params);
+      model, cameraRig, params);
   smartFactor1->add(measurements_cam1,
                     views);  // we have a single camera so use default cameraIds
 

gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h

@@ -109,28 +109,6 @@ class SmartProjectionPoseFactorRollingShutter
           "linearizationMode must be set to HESSIAN");
   }
 
-  /**
-   * Constructor
-   * @param Isotropic measurement noise
-   * @param camera single camera (fixed poses wrt body and intrinsics)
-   * @param params internal parameters of the smart factors
-   */
-  SmartProjectionPoseFactorRollingShutter(
-      const SharedNoiseModel& sharedNoiseModel, const Camera& camera,
-      const SmartProjectionParams& params = SmartProjectionParams())
-      : Base(sharedNoiseModel, params) {
-    // throw exception if configuration is not supported by this factor
-    if (Base::params_.degeneracyMode != gtsam::ZERO_ON_DEGENERACY)
-      throw std::runtime_error(
-          "SmartProjectionRigFactor: "
-          "degeneracyMode must be set to ZERO_ON_DEGENERACY");
-    if (Base::params_.linearizationMode != gtsam::HESSIAN)
-      throw std::runtime_error(
-          "SmartProjectionRigFactor: "
-          "linearizationMode must be set to HESSIAN");
-    cameraRig_.push_back(camera);
-  }
-
   /** Virtual destructor */
   ~SmartProjectionPoseFactorRollingShutter() override = default;
 

gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp

@@ -29,7 +29,7 @@
 #include <iostream>
 
 #include "gtsam/slam/tests/smartFactorScenarios.h"
-#define DISABLE_TIMING
+//#define DISABLE_TIMING
 
 using namespace gtsam;
 using namespace boost::assign;
@@ -87,22 +87,28 @@ typedef SmartProjectionPoseFactorRollingShutter<PinholePose<Cal3_S2>>
 /* ************************************************************************* */
 TEST(SmartProjectionPoseFactorRollingShutter, Constructor) {
   using namespace vanillaPoseRS;
+  Cameras cameraRig;
+  cameraRig.push_back(Camera(Pose3::identity(), sharedK));
   SmartFactorRS::shared_ptr factor1(
-      new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params));
+      new SmartFactorRS(model, cameraRig, params));
 }
 
 /* ************************************************************************* */
 TEST(SmartProjectionPoseFactorRollingShutter, Constructor2) {
   using namespace vanillaPoseRS;
+  Cameras cameraRig;
+  cameraRig.push_back(Camera(Pose3::identity(), sharedK));
   params.setRankTolerance(rankTol);
-  SmartFactorRS factor1(model, Camera(Pose3::identity(), sharedK), params);
+  SmartFactorRS factor1(model, cameraRig, params);
 }
 
 /* ************************************************************************* */
 TEST(SmartProjectionPoseFactorRollingShutter, add) {
   using namespace vanillaPoseRS;
+  Cameras cameraRig;
+  cameraRig.push_back(Camera(Pose3::identity(), sharedK));
   SmartFactorRS::shared_ptr factor1(
-      new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params));
+      new SmartFactorRS(model, cameraRig, params));
   factor1->add(measurement1, x1, x2, interp_factor);
 }
 
@@ -226,7 +232,10 @@ TEST(SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians) {
   Point2 level_uv_right = cam2.project(landmark1);
   Pose3 body_P_sensorId = Pose3::identity();
 
-  SmartFactorRS factor(model, Camera(body_P_sensorId, sharedK), params);
+  Cameras cameraRig;
+  cameraRig.push_back(Camera(body_P_sensorId, sharedK));
+
+  SmartFactorRS factor(model, cameraRig, params);
   factor.add(level_uv, x1, x2, interp_factor1);
   factor.add(level_uv_right, x2, x3, interp_factor2);
 
@@ -301,7 +310,10 @@ TEST(SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians) {
   Point2 level_uv_right = cam2.project(landmark1);
   Pose3 body_P_sensorNonId = body_P_sensor;
 
-  SmartFactorRS factor(model, Camera(body_P_sensorNonId, sharedK), params);
+  Cameras cameraRig;
+  cameraRig.push_back(Camera(body_P_sensorNonId, sharedK));
+
+  SmartFactorRS factor(model, cameraRig, params);
   factor.add(level_uv, x1, x2, interp_factor1);
   factor.add(level_uv_right, x2, x3, interp_factor2);
 
@@ -392,16 +404,19 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses) {
   interp_factors.push_back(interp_factor2);
   interp_factors.push_back(interp_factor3);
 
+  Cameras cameraRig;
+  cameraRig.push_back(Camera(Pose3::identity(), sharedK));
+
   SmartFactorRS::shared_ptr smartFactor1(
-      new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params));
+      new SmartFactorRS(model, cameraRig, params));
   smartFactor1->add(measurements_lmk1, key_pairs, interp_factors);
 
   SmartFactorRS::shared_ptr smartFactor2(
-      new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params));
+      new SmartFactorRS(model, cameraRig, params));
   smartFactor2->add(measurements_lmk2, key_pairs, interp_factors);
 
   SmartFactorRS::shared_ptr smartFactor3(
-      new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params));
+      new SmartFactorRS(model, cameraRig, params));
   smartFactor3->add(measurements_lmk3, key_pairs, interp_factors);
 
   const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@@ -633,8 +648,11 @@ TEST(SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses) {
   measurements_lmk1.push_back(cam1.project(landmark1));
   measurements_lmk1.push_back(cam2.project(landmark1));
 
+  Cameras cameraRig;
+  cameraRig.push_back(Camera(body_P_sensorId, sharedKSimple));
+
   SmartFactorRS::shared_ptr smartFactor1(
-      new SmartFactorRS(model, Camera(body_P_sensorId, sharedKSimple), params));
+      new SmartFactorRS(model, cameraRig, params));
   double interp_factor = 0;  // equivalent to measurement taken at pose 1
   smartFactor1->add(measurements_lmk1[0], x1, x2, interp_factor);
   interp_factor = 1;  // equivalent to measurement taken at pose 2
@@ -728,13 +746,16 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI) {
   params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist);
   params.setEnableEPI(true);
 
-  SmartFactorRS smartFactor1(model, Camera(Pose3::identity(), sharedK), params);
+  Cameras cameraRig;
+  cameraRig.push_back(Camera(Pose3::identity(), sharedK));
+
+  SmartFactorRS smartFactor1(model, cameraRig, params);
   smartFactor1.add(measurements_lmk1, key_pairs, interp_factors);
 
-  SmartFactorRS smartFactor2(model, Camera(Pose3::identity(), sharedK), params);
+  SmartFactorRS smartFactor2(model, cameraRig, params);
   smartFactor2.add(measurements_lmk2, key_pairs, interp_factors);
 
-  SmartFactorRS smartFactor3(model, Camera(Pose3::identity(), sharedK), params);
+  SmartFactorRS smartFactor3(model, cameraRig, params);
   smartFactor3.add(measurements_lmk3, key_pairs, interp_factors);
 
   const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@@ -794,13 +815,16 @@ TEST(SmartProjectionPoseFactorRollingShutter,
   params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist);
   params.setEnableEPI(false);
 
-  SmartFactorRS smartFactor1(model, Camera(Pose3::identity(), sharedK), params);
+  Cameras cameraRig;
+  cameraRig.push_back(Camera(Pose3::identity(), sharedK));
+
+  SmartFactorRS smartFactor1(model, cameraRig, params);
   smartFactor1.add(measurements_lmk1, key_pairs, interp_factors);
 
-  SmartFactorRS smartFactor2(model, Camera(Pose3::identity(), sharedK), params);
+  SmartFactorRS smartFactor2(model, cameraRig, params);
   smartFactor2.add(measurements_lmk2, key_pairs, interp_factors);
 
-  SmartFactorRS smartFactor3(model, Camera(Pose3::identity(), sharedK), params);
+  SmartFactorRS smartFactor3(model, cameraRig, params);
   smartFactor3.add(measurements_lmk3, key_pairs, interp_factors);
 
   const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@@ -869,20 +893,23 @@ TEST(SmartProjectionPoseFactorRollingShutter,
   params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold);
   params.setEnableEPI(false);
 
+  Cameras cameraRig;
+  cameraRig.push_back(Camera(Pose3::identity(), sharedK));
+
   SmartFactorRS::shared_ptr smartFactor1(
-      new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params));
+      new SmartFactorRS(model, cameraRig, params));
   smartFactor1->add(measurements_lmk1, key_pairs, interp_factors);
 
   SmartFactorRS::shared_ptr smartFactor2(
-      new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params));
+      new SmartFactorRS(model, cameraRig, params));
   smartFactor2->add(measurements_lmk2, key_pairs, interp_factors);
 
   SmartFactorRS::shared_ptr smartFactor3(
-      new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params));
+      new SmartFactorRS(model, cameraRig, params));
   smartFactor3->add(measurements_lmk3, key_pairs, interp_factors);
 
   SmartFactorRS::shared_ptr smartFactor4(
-      new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params));
+      new SmartFactorRS(model, cameraRig, params));
   smartFactor4->add(measurements_lmk4, key_pairs, interp_factors);
 
   const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@@ -933,8 +960,11 @@ TEST(SmartProjectionPoseFactorRollingShutter,
   interp_factors.push_back(interp_factor2);
   interp_factors.push_back(interp_factor3);
 
+  Cameras cameraRig;
+  cameraRig.push_back(Camera(Pose3::identity(), sharedK));
+
   SmartFactorRS::shared_ptr smartFactor1(
-      new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params));
+      new SmartFactorRS(model, cameraRig, params));
   smartFactor1->add(measurements_lmk1, key_pairs, interp_factors);
 
   Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
@@ -1071,8 +1101,11 @@ TEST(SmartProjectionPoseFactorRollingShutter,
   interp_factors.push_back(interp_factor3);
   interp_factors.push_back(interp_factor1);
 
+  Cameras cameraRig;
+  cameraRig.push_back(Camera(Pose3::identity(), sharedK));
+
   SmartFactorRS::shared_ptr smartFactor1(
-      new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params));
+      new SmartFactorRS(model, cameraRig, params));
   smartFactor1->add(measurements_lmk1_redundant, key_pairs, interp_factors);
 
   Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
@@ -1227,17 +1260,20 @@ TEST(SmartProjectionPoseFactorRollingShutter,
   interp_factors_redundant.push_back(
       interp_factors.at(0));  // we readd the first interp factor
 
+  Cameras cameraRig;
+  cameraRig.push_back(Camera(Pose3::identity(), sharedK));
+
   SmartFactorRS::shared_ptr smartFactor1(
-      new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params));
+      new SmartFactorRS(model, cameraRig, params));
   smartFactor1->add(measurements_lmk1_redundant, key_pairs_redundant,
                     interp_factors_redundant);
 
   SmartFactorRS::shared_ptr smartFactor2(
-      new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params));
+      new SmartFactorRS(model, cameraRig, params));
   smartFactor2->add(measurements_lmk2, key_pairs, interp_factors);
 
   SmartFactorRS::shared_ptr smartFactor3(
-      new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params));
+      new SmartFactorRS(model, cameraRig, params));
   smartFactor3->add(measurements_lmk3, key_pairs, interp_factors);
 
   const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@@ -1313,8 +1349,11 @@ TEST(SmartProjectionPoseFactorRollingShutter, timing) {
   size_t nrTests = 10000;
 
   for (size_t i = 0; i < nrTests; i++) {
+    Cameras cameraRig;
+    cameraRig.push_back(Camera(body_P_sensorId, sharedKSimple));
+
     SmartFactorRS::shared_ptr smartFactorRS(new SmartFactorRS(
-        model, Camera(body_P_sensorId, sharedKSimple), params));
+        model, cameraRig, params));
     double interp_factor = 0;  // equivalent to measurement taken at pose 1
     smartFactorRS->add(measurements_lmk1[0], x1, x2, interp_factor);
     interp_factor = 1;  // equivalent to measurement taken at pose 2