From 2c2e43ee5b95547bbe32c59d3aaf70f85e083104 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sun, 7 Nov 2021 18:01:28 -0500 Subject: [PATCH] got rid of second constructor --- gtsam/slam/SmartProjectionRigFactor.h | 23 ----- .../tests/testSmartProjectionRigFactor.cpp | 27 +++--- .../SmartProjectionPoseFactorRollingShutter.h | 22 ----- ...martProjectionPoseFactorRollingShutter.cpp | 91 +++++++++++++------ 4 files changed, 76 insertions(+), 87 deletions(-) diff --git a/gtsam/slam/SmartProjectionRigFactor.h b/gtsam/slam/SmartProjectionRigFactor.h index 26e7b6e97..d2a3140b4 100644 --- a/gtsam/slam/SmartProjectionRigFactor.h +++ b/gtsam/slam/SmartProjectionRigFactor.h @@ -105,29 +105,6 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { "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; diff --git a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp index d6a9b12fe..f518f3dce 100644 --- a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp @@ -29,7 +29,7 @@ #include #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)); - factor3->add(measurement1, x1); // now use default + new SmartRigFactor(model, cameraRig, params)); + 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 cameraIds{0, 0}; + Cameras cameraRig; // single camera in the rig + cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + SmartRigFactor::shared_ptr smartFactor1 = boost::make_shared( - model, Camera(Pose3::identity(), sharedK), params); + model, cameraRig, params); smartFactor1->add(measurements_cam1, views); // we have a single camera so use default cameraIds diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index b770ee7cf..fcec7de28 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/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; diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index 1d32eccca..615f452d8 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -29,7 +29,7 @@ #include #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> /* ************************************************************************* */ 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