From 2c2e43ee5b95547bbe32c59d3aaf70f85e083104 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sun, 7 Nov 2021 18:01:28 -0500 Subject: [PATCH 1/4] 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 From ac5875671f06d8ca56f532f284b36180e7b60e13 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sun, 7 Nov 2021 18:12:19 -0500 Subject: [PATCH 2/4] further cleanup before moving to sharedPtrs --- gtsam/slam/SmartProjectionRigFactor.h | 3 +- .../tests/testSmartProjectionRigFactor.cpp | 59 ------------------- 2 files changed, 1 insertion(+), 61 deletions(-) diff --git a/gtsam/slam/SmartProjectionRigFactor.h b/gtsam/slam/SmartProjectionRigFactor.h index d2a3140b4..adce44c15 100644 --- a/gtsam/slam/SmartProjectionRigFactor.h +++ b/gtsam/slam/SmartProjectionRigFactor.h @@ -62,8 +62,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { /// vector of keys (one for each observation) with potentially repeated keys KeyVector nonUniqueKeys_; - /// cameras in the rig (fixed poses wrt body + fixed intrinsics, for each - /// camera) + /// cameras in the rig (fixed poses wrt body and intrinsics, for each camera) typename Base::Cameras cameraRig_; /// vector of camera Ids (one for each observation, in the same order), diff --git a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp index f518f3dce..519850d98 100644 --- a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp @@ -578,11 +578,6 @@ TEST(SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor) { projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - std::vector> sharedKs; - sharedKs.push_back(sharedK); - sharedKs.push_back(sharedK); - sharedKs.push_back(sharedK); - // create smart factor Cameras cameraRig; // single camera in the rig cameraRig.push_back(Camera(Pose3::identity(), sharedK)); @@ -1254,60 +1249,6 @@ TEST(SmartProjectionRigFactor, timing) { } #endif -///* **************************************************************************/ -// BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, -// "gtsam_noiseModel_Constrained"); -// BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, -// "gtsam_noiseModel_Diagonal"); -// BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, -// "gtsam_noiseModel_Gaussian"); -// BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); -// BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, -// "gtsam_noiseModel_Isotropic"); -// BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); -// BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); -// -// TEST(SmartProjectionRigFactor, serialize) { -// using namespace vanillaRig; -// using namespace gtsam::serializationTestHelpers; -// SmartProjectionParams params( -// gtsam::HESSIAN, -// gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors -// params.setRankTolerance(rankTol); -// -// Cameras cameraRig; // single camera in the rig -// cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); -// -// SmartRigFactor factor(model, cameraRig, params); -// -// EXPECT(equalsObj(factor)); -// EXPECT(equalsXML(factor)); -// EXPECT(equalsBinary(factor)); -//} -// -//// SERIALIZATION TEST FAILS: to be fixed -////TEST(SmartProjectionRigFactor, serialize2) { -//// using namespace vanillaRig; -//// using namespace gtsam::serializationTestHelpers; -//// SmartProjectionParams params; -//// params.setRankTolerance(rankTol); -//// SmartRigFactor factor(model, params); -//// -//// // insert some measurements -//// KeyVector key_view; -//// Point2Vector meas_view; -//// std::vector> sharedKs; -//// -//// key_view.push_back(Symbol('x', 1)); -//// meas_view.push_back(Point2(10, 10)); -//// sharedKs.push_back(sharedK); -//// factor.add(meas_view, key_view, sharedKs); -//// -//// EXPECT(equalsObj(factor)); -//// EXPECT(equalsXML(factor)); -//// EXPECT(equalsBinary(factor)); -////} - /* ************************************************************************* */ int main() { TestResult tr; From 4ba93738edde14256c37729504fa37fa0b5a84a8 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sun, 7 Nov 2021 18:25:35 -0500 Subject: [PATCH 3/4] moved rig to use shared ptrs --- gtsam/slam/SmartProjectionRigFactor.h | 21 ++--- .../tests/testSmartProjectionRigFactor.cpp | 86 +++++++++---------- 2 files changed, 54 insertions(+), 53 deletions(-) diff --git a/gtsam/slam/SmartProjectionRigFactor.h b/gtsam/slam/SmartProjectionRigFactor.h index adce44c15..87125020a 100644 --- a/gtsam/slam/SmartProjectionRigFactor.h +++ b/gtsam/slam/SmartProjectionRigFactor.h @@ -63,7 +63,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { KeyVector nonUniqueKeys_; /// cameras in the rig (fixed poses wrt body and intrinsics, for each camera) - typename Base::Cameras cameraRig_; + boost::shared_ptr cameraRig_; /// vector of camera Ids (one for each observation, in the same order), /// identifying which camera took the measurement @@ -90,7 +90,8 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { * @param params parameters for the smart projection factors */ SmartProjectionRigFactor( - const SharedNoiseModel& sharedNoiseModel, const Cameras& cameraRig, + const SharedNoiseModel& sharedNoiseModel, + const boost::shared_ptr& cameraRig, const SmartProjectionParams& params = SmartProjectionParams()) : Base(sharedNoiseModel, params), cameraRig_(cameraRig) { // throw exception if configuration is not supported by this factor @@ -151,7 +152,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { "SmartProjectionRigFactor: " "trying to add inconsistent inputs"); } - if (cameraIds.size() == 0 && cameraRig_.size() > 1) { + if (cameraIds.size() == 0 && cameraRig_->size() > 1) { throw std::runtime_error( "SmartProjectionRigFactor: " "camera rig includes multiple camera " @@ -168,7 +169,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { const KeyVector& nonUniqueKeys() const { return nonUniqueKeys_; } /// return the calibration object - const Cameras& cameraRig() const { return cameraRig_; } + const boost::shared_ptr& cameraRig() const { return cameraRig_; } /// return the calibration object const FastVector& cameraIds() const { return cameraIds_; } @@ -186,7 +187,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { std::cout << "-- Measurement nr " << i << std::endl; std::cout << "key: " << keyFormatter(nonUniqueKeys_[i]) << std::endl; std::cout << "cameraId: " << cameraIds_[i] << std::endl; - cameraRig_[cameraIds_[i]].print("camera in rig:\n"); + (*cameraRig_)[cameraIds_[i]].print("camera in rig:\n"); } Base::print("", keyFormatter); } @@ -195,7 +196,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { bool equals(const NonlinearFactor& p, double tol = 1e-9) const override { const This* e = dynamic_cast(&p); return e && Base::equals(p, tol) && nonUniqueKeys_ == e->nonUniqueKeys() && - cameraRig_.equals(e->cameraRig()) && + (*cameraRig_).equals(*(e->cameraRig())) && std::equal(cameraIds_.begin(), cameraIds_.end(), e->cameraIds().begin()); } @@ -210,7 +211,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { typename Base::Cameras cameras; cameras.reserve(nonUniqueKeys_.size()); // preallocate for (size_t i = 0; i < nonUniqueKeys_.size(); i++) { - const typename Base::Camera& camera_i = cameraRig_[cameraIds_[i]]; + const typename Base::Camera& camera_i = (*cameraRig_)[cameraIds_[i]]; const Pose3 world_P_sensor_i = values.at(nonUniqueKeys_[i]) // = world_P_body * camera_i.pose(); // = body_P_cam_i @@ -249,7 +250,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { } else { // valid result: compute jacobians b = -cameras.reprojectionError(*this->result_, this->measured_, Fs, E); for (size_t i = 0; i < Fs.size(); i++) { - const Pose3& body_P_sensor = cameraRig_[cameraIds_[i]].pose(); + const Pose3& body_P_sensor = (*cameraRig_)[cameraIds_[i]].pose(); const Pose3 world_P_body = cameras[i].pose() * body_P_sensor.inverse(); Eigen::Matrix H; world_P_body.compose(body_P_sensor, H); @@ -354,9 +355,9 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - //ar& BOOST_SERIALIZATION_NVP(nonUniqueKeys_); + // ar& BOOST_SERIALIZATION_NVP(nonUniqueKeys_); // ar& BOOST_SERIALIZATION_NVP(cameraRig_); - //ar& BOOST_SERIALIZATION_NVP(cameraIds_); + // ar& BOOST_SERIALIZATION_NVP(cameraIds_); } }; // end of class declaration diff --git a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp index 519850d98..b111ee572 100644 --- a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp @@ -70,8 +70,8 @@ SmartProjectionParams params( /* ************************************************************************* */ TEST(SmartProjectionRigFactor, Constructor) { using namespace vanillaRig; - Cameras cameraRig; - cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + boost::shared_ptr cameraRig(new Cameras()); + cameraRig->push_back(Camera(Pose3::identity(), sharedK)); SmartRigFactor::shared_ptr factor1( new SmartRigFactor(model, cameraRig, params)); } @@ -79,7 +79,7 @@ TEST(SmartProjectionRigFactor, Constructor) { /* ************************************************************************* */ TEST(SmartProjectionRigFactor, Constructor2) { using namespace vanillaRig; - Cameras cameraRig; + boost::shared_ptr cameraRig(new Cameras()); SmartProjectionParams params2( gtsam::HESSIAN, gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors @@ -90,8 +90,8 @@ TEST(SmartProjectionRigFactor, Constructor2) { /* ************************************************************************* */ TEST(SmartProjectionRigFactor, Constructor3) { using namespace vanillaRig; - Cameras cameraRig; - cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + boost::shared_ptr cameraRig(new Cameras()); + cameraRig->push_back(Camera(Pose3::identity(), sharedK)); SmartRigFactor::shared_ptr factor1( new SmartRigFactor(model, cameraRig, params)); factor1->add(measurement1, x1, cameraId1); @@ -100,8 +100,8 @@ TEST(SmartProjectionRigFactor, Constructor3) { /* ************************************************************************* */ TEST(SmartProjectionRigFactor, Constructor4) { using namespace vanillaRig; - Cameras cameraRig; - cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + boost::shared_ptr cameraRig(new Cameras()); + cameraRig->push_back(Camera(Pose3::identity(), sharedK)); SmartProjectionParams params2( gtsam::HESSIAN, gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors @@ -113,8 +113,8 @@ TEST(SmartProjectionRigFactor, Constructor4) { /* ************************************************************************* */ TEST(SmartProjectionRigFactor, Equals) { using namespace vanillaRig; - Cameras cameraRig; // single camera in the rig - cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig + cameraRig->push_back(Camera(Pose3::identity(), sharedK)); SmartRigFactor::shared_ptr factor1( new SmartRigFactor(model, cameraRig, params)); @@ -141,8 +141,8 @@ TEST(SmartProjectionRigFactor, noiseless) { Point2 level_uv = level_camera.project(landmark1); Point2 level_uv_right = level_camera_right.project(landmark1); - Cameras cameraRig; // single camera in the rig - cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + boost::shared_ptr cameraRig(new Cameras()); // 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 @@ -198,8 +198,8 @@ TEST(SmartProjectionRigFactor, noiseless) { TEST(SmartProjectionRigFactor, noisy) { using namespace vanillaRig; - Cameras cameraRig; // single camera in the rig - cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig + cameraRig->push_back(Camera(Pose3::identity(), sharedK)); // Project two landmarks into two cameras Point2 pixelError(0.2, 0.2); @@ -242,8 +242,8 @@ TEST(SmartProjectionRigFactor, smartFactorWithSensorBodyTransform) { // create arbitrary body_T_sensor (transforms from sensor to body) Pose3 body_T_sensor = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(1, 1, 1)); - Cameras cameraRig; // single camera in the rig - cameraRig.push_back(Camera(body_T_sensor, sharedK)); + boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig + cameraRig->push_back(Camera(body_T_sensor, sharedK)); // These are the poses we want to estimate, from camera measurements const Pose3 sensor_T_body = body_T_sensor.inverse(); @@ -327,10 +327,10 @@ TEST(SmartProjectionRigFactor, smartFactorWithMultipleCameras) { Pose3(Rot3::Ypr(-M_PI / 5, 0., -M_PI / 2), Point3(0, 0, 1)); Pose3 body_T_sensor3 = Pose3::identity(); - Cameras cameraRig; // single camera in the rig - cameraRig.push_back(Camera(body_T_sensor1, sharedK)); - cameraRig.push_back(Camera(body_T_sensor2, sharedK)); - cameraRig.push_back(Camera(body_T_sensor3, sharedK)); + boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig + cameraRig->push_back(Camera(body_T_sensor1, sharedK)); + cameraRig->push_back(Camera(body_T_sensor2, sharedK)); + cameraRig->push_back(Camera(body_T_sensor3, sharedK)); // These are the poses we want to estimate, from camera measurements const Pose3 sensor_T_body1 = body_T_sensor1.inverse(); @@ -408,8 +408,8 @@ TEST(SmartProjectionRigFactor, 3poses_smart_projection_factor) { using namespace vanillaPose2; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; - Cameras cameraRig; // single camera in the rig - cameraRig.push_back(Camera(Pose3::identity(), sharedK2)); + boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig + cameraRig->push_back(Camera(Pose3::identity(), sharedK2)); // Project three landmarks into three cameras projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); @@ -496,8 +496,8 @@ 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)); + boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig + cameraRig->push_back(Camera(Pose3::identity(), sharedK)); SmartRigFactor::shared_ptr smartFactor1 = boost::make_shared( model, cameraRig, params); @@ -579,8 +579,8 @@ TEST(SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor) { projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); // create smart factor - Cameras cameraRig; // single camera in the rig - cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig + cameraRig->push_back(Camera(Pose3::identity(), sharedK)); FastVector cameraIds{0, 0, 0}; SmartRigFactor::shared_ptr smartFactor1( new SmartRigFactor(model, cameraRig, params)); @@ -647,8 +647,8 @@ TEST(SmartProjectionRigFactor, landmarkDistance) { params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setEnableEPI(false); - Cameras cameraRig; // single camera in the rig - cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig + cameraRig->push_back(Camera(Pose3::identity(), sharedK)); FastVector cameraIds{0, 0, 0}; SmartRigFactor::shared_ptr smartFactor1( @@ -718,8 +718,8 @@ TEST(SmartProjectionRigFactor, dynamicOutlierRejection) { params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); - Cameras cameraRig; // single camera in the rig - cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig + cameraRig->push_back(Camera(Pose3::identity(), sharedK)); FastVector cameraIds{0, 0, 0}; SmartRigFactor::shared_ptr smartFactor1( @@ -784,8 +784,8 @@ TEST(SmartProjectionRigFactor, CheckHessian) { params.setRankTolerance(10); params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); - Cameras cameraRig; // single camera in the rig - cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig + cameraRig->push_back(Camera(Pose3::identity(), sharedK)); FastVector cameraIds{0, 0, 0}; SmartRigFactor::shared_ptr smartFactor1( @@ -860,8 +860,8 @@ TEST(SmartProjectionRigFactor, Hessian) { measurements_cam1.push_back(cam1_uv1); measurements_cam1.push_back(cam2_uv1); - Cameras cameraRig; // single camera in the rig - cameraRig.push_back(Camera(Pose3::identity(), sharedK2)); + boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig + cameraRig->push_back(Camera(Pose3::identity(), sharedK2)); FastVector cameraIds{0, 0}; SmartProjectionParams params( @@ -890,8 +890,8 @@ TEST(SmartProjectionRigFactor, Hessian) { /* ************************************************************************* */ TEST(SmartProjectionRigFactor, ConstructorWithCal3Bundler) { using namespace bundlerPose; - Cameras cameraRig; // single camera in the rig - cameraRig.push_back(Camera(Pose3::identity(), sharedBundlerK)); + boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig + cameraRig->push_back(Camera(Pose3::identity(), sharedBundlerK)); SmartProjectionParams params; params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); @@ -918,8 +918,8 @@ TEST(SmartProjectionRigFactor, Cal3Bundler) { KeyVector views{x1, x2, x3}; - Cameras cameraRig; // single camera in the rig - cameraRig.push_back(Camera(Pose3::identity(), sharedBundlerK)); + boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig + cameraRig->push_back(Camera(Pose3::identity(), sharedBundlerK)); FastVector cameraIds{0, 0, 0}; SmartRigFactor::shared_ptr smartFactor1( @@ -989,8 +989,8 @@ TEST(SmartProjectionRigFactor, // create inputs KeyVector keys{x1, x2, x3, x1}; - Cameras cameraRig; // single camera in the rig - cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig + cameraRig->push_back(Camera(Pose3::identity(), sharedK)); FastVector cameraIds{0, 0, 0, 0}; SmartRigFactor::shared_ptr smartFactor1( @@ -1117,8 +1117,8 @@ TEST(SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose) { // create inputs KeyVector keys{x1, x2, x3}; - Cameras cameraRig; // single camera in the rig - cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig + cameraRig->push_back(Camera(Pose3::identity(), sharedK)); FastVector cameraIds{0, 0, 0}; FastVector cameraIdsRedundant{0, 0, 0, 0}; @@ -1204,8 +1204,8 @@ TEST(SmartProjectionRigFactor, timing) { Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple); Pose3 body_P_sensorId = Pose3::identity(); - Cameras cameraRig; // single camera in the rig - cameraRig.push_back(Camera(body_P_sensorId, sharedKSimple)); + boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig + cameraRig->push_back(Camera(body_P_sensorId, sharedKSimple)); // one landmarks 1m in front of camera Point3 landmark1(0, 0, 10); From 620f9cb99fb5b5dd5bda1296eaf93f933b11e73b Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sun, 7 Nov 2021 18:32:43 -0500 Subject: [PATCH 4/4] now using shared ptrs --- gtsam/slam/SmartProjectionRigFactor.h | 2 +- .../tests/testSmartProjectionRigFactor.cpp | 2 +- .../SmartProjectionPoseFactorRollingShutter.h | 17 ++-- ...martProjectionPoseFactorRollingShutter.cpp | 80 +++++++++---------- 4 files changed, 51 insertions(+), 50 deletions(-) diff --git a/gtsam/slam/SmartProjectionRigFactor.h b/gtsam/slam/SmartProjectionRigFactor.h index 87125020a..8d6918b3e 100644 --- a/gtsam/slam/SmartProjectionRigFactor.h +++ b/gtsam/slam/SmartProjectionRigFactor.h @@ -196,7 +196,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { bool equals(const NonlinearFactor& p, double tol = 1e-9) const override { const This* e = dynamic_cast(&p); return e && Base::equals(p, tol) && nonUniqueKeys_ == e->nonUniqueKeys() && - (*cameraRig_).equals(*(e->cameraRig())) && + cameraRig_->equals(*(e->cameraRig())) && std::equal(cameraIds_.begin(), cameraIds_.end(), e->cameraIds().begin()); } diff --git a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp index b111ee572..b8150a1aa 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; diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index fcec7de28..23203be67 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -59,7 +59,7 @@ class SmartProjectionPoseFactorRollingShutter /// one or more cameras taking observations (fixed poses wrt body + fixed /// intrinsics) - typename Base::Cameras cameraRig_; + boost::shared_ptr cameraRig_; /// vector of camera Ids (one for each observation, in the same order), /// identifying which camera took the measurement @@ -95,7 +95,8 @@ class SmartProjectionPoseFactorRollingShutter * @param params internal parameters of the smart factors */ SmartProjectionPoseFactorRollingShutter( - const SharedNoiseModel& sharedNoiseModel, const Cameras& cameraRig, + const SharedNoiseModel& sharedNoiseModel, + const boost::shared_ptr& cameraRig, const SmartProjectionParams& params = SmartProjectionParams()) : Base(sharedNoiseModel, params), cameraRig_(cameraRig) { // throw exception if configuration is not supported by this factor @@ -175,7 +176,7 @@ class SmartProjectionPoseFactorRollingShutter "SmartProjectionPoseFactorRollingShutter: " "trying to add inconsistent inputs"); } - if (cameraIds.size() == 0 && cameraRig_.size() > 1) { + if (cameraIds.size() == 0 && cameraRig_->size() > 1) { throw std::runtime_error( "SmartProjectionPoseFactorRollingShutter: " "camera rig includes multiple camera " @@ -200,7 +201,7 @@ class SmartProjectionPoseFactorRollingShutter const std::vector& alphas() const { return alphas_; } /// return the calibration object - const Cameras& cameraRig() const { return cameraRig_; } + const boost::shared_ptr& cameraRig() const { return cameraRig_; } /// return the calibration object const FastVector& cameraIds() const { return cameraIds_; } @@ -222,7 +223,7 @@ class SmartProjectionPoseFactorRollingShutter << keyFormatter(world_P_body_key_pairs_[i].second) << std::endl; std::cout << " alpha: " << alphas_[i] << std::endl; std::cout << "cameraId: " << cameraIds_[i] << std::endl; - cameraRig_[cameraIds_[i]].print("camera in rig:\n"); + (*cameraRig_)[cameraIds_[i]].print("camera in rig:\n"); } Base::print("", keyFormatter); } @@ -251,7 +252,7 @@ class SmartProjectionPoseFactorRollingShutter } return e && Base::equals(p, tol) && alphas_ == e->alphas() && - keyPairsEqual && cameraRig_.equals(e->cameraRig()) && + keyPairsEqual && cameraRig_->equals(*(e->cameraRig())) && std::equal(cameraIds_.begin(), cameraIds_.end(), e->cameraIds().begin()); } @@ -273,7 +274,7 @@ class SmartProjectionPoseFactorRollingShutter double interpolationFactor = alphas_[i]; const Pose3& w_P_body = interpolate(w_P_body1, w_P_body2, interpolationFactor); - const typename Base::Camera& camera_i = cameraRig_[cameraIds_[i]]; + const typename Base::Camera& camera_i = (*cameraRig_)[cameraIds_[i]]; const Pose3& body_P_cam = camera_i.pose(); const Pose3& w_P_cam = w_P_body.compose(body_P_cam); cameras.emplace_back(w_P_cam, @@ -326,7 +327,7 @@ class SmartProjectionPoseFactorRollingShutter auto w_P_body = interpolate(w_P_body1, w_P_body2, interpolationFactor, dInterpPose_dPoseBody1, dInterpPose_dPoseBody2); - const typename Base::Camera& camera_i = cameraRig_[cameraIds_[i]]; + const typename Base::Camera& camera_i = (*cameraRig_)[cameraIds_[i]]; auto body_P_cam = camera_i.pose(); auto w_P_cam = w_P_body.compose(body_P_cam, dPoseCam_dInterpPose); PinholeCamera camera(w_P_cam, camera_i.calibration()); diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index 615f452d8..c17ad7e1c 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,8 +87,8 @@ typedef SmartProjectionPoseFactorRollingShutter> /* ************************************************************************* */ TEST(SmartProjectionPoseFactorRollingShutter, Constructor) { using namespace vanillaPoseRS; - Cameras cameraRig; - cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + boost::shared_ptr cameraRig(new Cameras()); + cameraRig->push_back(Camera(Pose3::identity(), sharedK)); SmartFactorRS::shared_ptr factor1( new SmartFactorRS(model, cameraRig, params)); } @@ -96,8 +96,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, Constructor) { /* ************************************************************************* */ TEST(SmartProjectionPoseFactorRollingShutter, Constructor2) { using namespace vanillaPoseRS; - Cameras cameraRig; - cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + boost::shared_ptr cameraRig(new Cameras()); + cameraRig->push_back(Camera(Pose3::identity(), sharedK)); params.setRankTolerance(rankTol); SmartFactorRS factor1(model, cameraRig, params); } @@ -105,8 +105,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, Constructor2) { /* ************************************************************************* */ TEST(SmartProjectionPoseFactorRollingShutter, add) { using namespace vanillaPoseRS; - Cameras cameraRig; - cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + boost::shared_ptr cameraRig(new Cameras()); + cameraRig->push_back(Camera(Pose3::identity(), sharedK)); SmartFactorRS::shared_ptr factor1( new SmartFactorRS(model, cameraRig, params)); factor1->add(measurement1, x1, x2, interp_factor); @@ -134,8 +134,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) { FastVector cameraIds{0, 0, 0}; - Cameras cameraRig; - cameraRig.push_back(Camera(body_P_sensor, sharedK)); + boost::shared_ptr cameraRig(new Cameras()); + cameraRig->push_back(Camera(body_P_sensor, sharedK)); // create by adding a batch of measurements with a bunch of calibrations SmartFactorRS::shared_ptr factor2( @@ -189,8 +189,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) { } { // create slightly different factors (different extrinsics) and show equal // returns false - Cameras cameraRig2; - cameraRig2.push_back(Camera(body_P_sensor * body_P_sensor, sharedK)); + boost::shared_ptr cameraRig2(new Cameras()); + cameraRig2->push_back(Camera(body_P_sensor * body_P_sensor, sharedK)); SmartFactorRS::shared_ptr factor1( new SmartFactorRS(model, cameraRig2, params)); factor1->add(measurement1, x1, x2, interp_factor1, cameraId1); @@ -232,8 +232,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians) { Point2 level_uv_right = cam2.project(landmark1); Pose3 body_P_sensorId = Pose3::identity(); - Cameras cameraRig; - cameraRig.push_back(Camera(body_P_sensorId, sharedK)); + boost::shared_ptr cameraRig(new Cameras()); + cameraRig->push_back(Camera(body_P_sensorId, sharedK)); SmartFactorRS factor(model, cameraRig, params); factor.add(level_uv, x1, x2, interp_factor1); @@ -310,8 +310,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians) { Point2 level_uv_right = cam2.project(landmark1); Pose3 body_P_sensorNonId = body_P_sensor; - Cameras cameraRig; - cameraRig.push_back(Camera(body_P_sensorNonId, sharedK)); + boost::shared_ptr cameraRig(new Cameras()); + cameraRig->push_back(Camera(body_P_sensorNonId, sharedK)); SmartFactorRS factor(model, cameraRig, params); factor.add(level_uv, x1, x2, interp_factor1); @@ -404,8 +404,8 @@ 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)); + boost::shared_ptr cameraRig(new Cameras()); + cameraRig->push_back(Camera(Pose3::identity(), sharedK)); SmartFactorRS::shared_ptr smartFactor1( new SmartFactorRS(model, cameraRig, params)); @@ -478,9 +478,9 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_multiCam) { interp_factors.push_back(interp_factor2); interp_factors.push_back(interp_factor3); - Cameras cameraRig; - cameraRig.push_back(Camera(body_P_sensor, sharedK)); - cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + boost::shared_ptr cameraRig(new Cameras()); + cameraRig->push_back(Camera(body_P_sensor, sharedK)); + cameraRig->push_back(Camera(Pose3::identity(), sharedK)); SmartFactorRS::shared_ptr smartFactor1( new SmartFactorRS(model, cameraRig, params)); @@ -566,10 +566,10 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_multiCam2) { interp_factors.push_back(interp_factor2); interp_factors.push_back(interp_factor3); - Cameras cameraRig; - cameraRig.push_back(Camera(body_T_sensor1, sharedK)); - cameraRig.push_back(Camera(body_T_sensor2, sharedK)); - cameraRig.push_back(Camera(body_T_sensor3, sharedK)); + boost::shared_ptr cameraRig(new Cameras()); + cameraRig->push_back(Camera(body_T_sensor1, sharedK)); + cameraRig->push_back(Camera(body_T_sensor2, sharedK)); + cameraRig->push_back(Camera(body_T_sensor3, sharedK)); SmartFactorRS::shared_ptr smartFactor1( new SmartFactorRS(model, cameraRig, params)); @@ -648,8 +648,8 @@ 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)); + boost::shared_ptr cameraRig(new Cameras()); + cameraRig->push_back(Camera(body_P_sensorId, sharedKSimple)); SmartFactorRS::shared_ptr smartFactor1( new SmartFactorRS(model, cameraRig, params)); @@ -746,8 +746,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI) { params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setEnableEPI(true); - Cameras cameraRig; - cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + boost::shared_ptr cameraRig(new Cameras()); + cameraRig->push_back(Camera(Pose3::identity(), sharedK)); SmartFactorRS smartFactor1(model, cameraRig, params); smartFactor1.add(measurements_lmk1, key_pairs, interp_factors); @@ -815,8 +815,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setEnableEPI(false); - Cameras cameraRig; - cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + boost::shared_ptr cameraRig(new Cameras()); + cameraRig->push_back(Camera(Pose3::identity(), sharedK)); SmartFactorRS smartFactor1(model, cameraRig, params); smartFactor1.add(measurements_lmk1, key_pairs, interp_factors); @@ -893,8 +893,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); params.setEnableEPI(false); - Cameras cameraRig; - cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + boost::shared_ptr cameraRig(new Cameras()); + cameraRig->push_back(Camera(Pose3::identity(), sharedK)); SmartFactorRS::shared_ptr smartFactor1( new SmartFactorRS(model, cameraRig, params)); @@ -960,8 +960,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, interp_factors.push_back(interp_factor2); interp_factors.push_back(interp_factor3); - Cameras cameraRig; - cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + boost::shared_ptr cameraRig(new Cameras()); + cameraRig->push_back(Camera(Pose3::identity(), sharedK)); SmartFactorRS::shared_ptr smartFactor1( new SmartFactorRS(model, cameraRig, params)); @@ -1101,8 +1101,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, interp_factors.push_back(interp_factor3); interp_factors.push_back(interp_factor1); - Cameras cameraRig; - cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + boost::shared_ptr cameraRig(new Cameras()); + cameraRig->push_back(Camera(Pose3::identity(), sharedK)); SmartFactorRS::shared_ptr smartFactor1( new SmartFactorRS(model, cameraRig, params)); @@ -1260,8 +1260,8 @@ 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)); + boost::shared_ptr cameraRig(new Cameras()); + cameraRig->push_back(Camera(Pose3::identity(), sharedK)); SmartFactorRS::shared_ptr smartFactor1( new SmartFactorRS(model, cameraRig, params)); @@ -1349,8 +1349,8 @@ 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)); + boost::shared_ptr cameraRig(new Cameras()); + cameraRig->push_back(Camera(body_P_sensorId, sharedKSimple)); SmartFactorRS::shared_ptr smartFactorRS(new SmartFactorRS( model, cameraRig, params));