From f0745a9c286c4104647302779ac4442ded799703 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Tue, 5 Oct 2021 23:10:45 -0400 Subject: [PATCH] now I only need to fix comments in rolling shutter factor --- gtsam/slam/SmartProjectionRigFactor.h | 35 ++++++++++++++---- .../tests/testSmartProjectionRigFactor.cpp | 36 +++++++++++-------- .../SmartProjectionPoseFactorRollingShutter.h | 5 +++ 3 files changed, 56 insertions(+), 20 deletions(-) diff --git a/gtsam/slam/SmartProjectionRigFactor.h b/gtsam/slam/SmartProjectionRigFactor.h index 0246d6327..3cae1ea64 100644 --- a/gtsam/slam/SmartProjectionRigFactor.h +++ b/gtsam/slam/SmartProjectionRigFactor.h @@ -94,6 +94,23 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { Base::params_.linearizationMode = gtsam::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) { + // use only configuration that works with this factor + Base::params_.degeneracyMode = gtsam::ZERO_ON_DEGENERACY; + Base::params_.linearizationMode = gtsam::HESSIAN; + cameraRig_.push_back(camera); + } + /** Virtual destructor */ ~SmartProjectionRigFactor() override { } @@ -104,9 +121,9 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { * @param measured 2-dimensional location of the projection of a * single landmark in a single view (the measurement) * @param poseKey key corresponding to the body pose of the camera taking the measurement - * @param cameraId ID of the camera in the rig taking the measurement + * @param cameraId ID of the camera in the rig taking the measurement (default 0) */ - void add(const Point2& measured, const Key& poseKey, const size_t cameraId) { + void add(const Point2& measured, const Key& poseKey, const size_t cameraId = 0) { // store measurement and key this->measured_.push_back(measured); this->nonUniqueKeys_.push_back(poseKey); @@ -127,13 +144,19 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { * @param cameraIds IDs of the cameras in the rig taking each measurement (same order as the measurements) */ void add(const Point2Vector& measurements, const KeyVector& poseKeys, - const FastVector& cameraIds) { - if(poseKeys.size() != measurements.size() || poseKeys.size() != cameraIds.size()){ + const FastVector& cameraIds = FastVector()) { + if (poseKeys.size() != measurements.size() + || (poseKeys.size() != cameraIds.size() && cameraIds.size() != 0)) { throw std::runtime_error("SmartProjectionRigFactor: " - "trying to add inconsistent inputs"); + "trying to add inconsistent inputs"); + } + if (cameraIds.size() == 0 && cameraRig_.size() > 1) { + throw std::runtime_error( + "SmartProjectionRigFactor: " + "camera rig includes multiple camera but add did not input cameraIds"); } for (size_t i = 0; i < measurements.size(); i++) { - add(measurements[i], poseKeys[i], cameraIds[i]); + add(measurements[i], poseKeys[i], cameraIds.size() == 0 ? 0 : cameraIds[i]); } } diff --git a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp index fb3f3c461..a2a30e89a 100644 --- a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp @@ -92,6 +92,15 @@ TEST( SmartProjectionRigFactor, Constructor4) { factor1.add(measurement1, x1, cameraId1); } +/* ************************************************************************* */ +TEST( SmartProjectionRigFactor, Constructor5) { + using namespace vanillaPose; + SmartProjectionParams params; + params.setRankTolerance(rankTol); + SmartRigFactor factor1(model, Camera(Pose3::identity(), sharedK), params); + factor1.add(measurement1, x1, cameraId1); +} + /* ************************************************************************* */ TEST( SmartProjectionRigFactor, Equals ) { using namespace vanillaPose; @@ -105,6 +114,11 @@ TEST( SmartProjectionRigFactor, Equals ) { factor2->add(measurement1, x1, cameraId1); CHECK(assert_equal(*factor1, *factor2)); + + SmartRigFactor::shared_ptr factor3(new SmartRigFactor(model, Camera(Pose3::identity(), sharedK))); + factor3->add(measurement1, x1); // now use default + + CHECK(assert_equal(*factor1, *factor3)); } /* *************************************************************************/ @@ -112,16 +126,13 @@ TEST( SmartProjectionRigFactor, noiseless ) { using namespace vanillaPose; - Cameras cameraRig; // single camera in the rig - cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); - // Project two landmarks into two cameras Point2 level_uv = level_camera.project(landmark1); Point2 level_uv_right = level_camera_right.project(landmark1); - SmartRigFactor factor(model, cameraRig); - factor.add(level_uv, x1, cameraId1); // both taken from the same camera - factor.add(level_uv_right, x2, cameraId1); + SmartRigFactor factor(model, Camera(Pose3::identity(), sharedK) ); + factor.add(level_uv, x1); // both taken from the same camera + factor.add(level_uv_right, x2); Values values; // it's a pose factor, hence these are poses values.insert(x1, cam1.pose()); @@ -245,13 +256,13 @@ TEST(SmartProjectionRigFactor, smartFactorWithSensorBodyTransform) { params.setEnableEPI(false); SmartRigFactor smartFactor1(model, cameraRig, params); - smartFactor1.add(measurements_cam1, views, cameraIds); + smartFactor1.add(measurements_cam1, views); // use default CameraIds since we have a single camera SmartRigFactor smartFactor2(model, cameraRig, params); - smartFactor2.add(measurements_cam2, views, cameraIds); + smartFactor2.add(measurements_cam2, views); SmartRigFactor smartFactor3(model, cameraRig, params); - smartFactor3.add(measurements_cam3, views, cameraIds); + smartFactor3.add(measurements_cam3, views); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -459,15 +470,12 @@ TEST( SmartProjectionRigFactor, Factors ) { measurements_cam1.push_back(cam2.project(landmark1)); // Create smart factors - Cameras cameraRig; // single camera in the rig - cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); - KeyVector views { x1, x2 }; FastVector cameraIds { 0, 0 }; SmartRigFactor::shared_ptr smartFactor1 = boost::make_shared < SmartRigFactor - > (model,cameraRig); - smartFactor1->add(measurements_cam1, views, cameraIds); + > (model, Camera(Pose3::identity(), sharedK)); + smartFactor1->add(measurements_cam1, views); // we have a single camera so use default cameraIds SmartRigFactor::Cameras cameras; cameras.push_back(cam1); diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index 09d13f0e5..f3f8900f5 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -183,6 +183,11 @@ class SmartProjectionPoseFactorRollingShutter throw std::runtime_error("SmartProjectionPoseFactorRollingShutter: " "trying to add inconsistent inputs"); } + if (cameraIds.size() == 0 && cameraRig_.size() > 1) { + throw std::runtime_error( + "SmartProjectionPoseFactorRollingShutter: " + "camera rig includes multiple camera but add did not input cameraIds"); + } for (size_t i = 0; i < measurements.size(); i++) { add(measurements[i], world_P_body_key_pairs[i].first, world_P_body_key_pairs[i].second, alphas[i],