From 620f9cb99fb5b5dd5bda1296eaf93f933b11e73b Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sun, 7 Nov 2021 18:32:43 -0500 Subject: [PATCH] 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));