From b1baf6c8b3f347006f897df2014e70a50ab3e164 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sun, 7 Nov 2021 14:50:56 -0500 Subject: [PATCH] final cosmetics --- gtsam/slam/SmartProjectionRigFactor.h | 14 +-- .../tests/testSmartProjectionRigFactor.cpp | 8 +- .../SmartProjectionPoseFactorRollingShutter.h | 85 ++++++++++--------- 3 files changed, 56 insertions(+), 51 deletions(-) diff --git a/gtsam/slam/SmartProjectionRigFactor.h b/gtsam/slam/SmartProjectionRigFactor.h index f4a72694c..26e7b6e97 100644 --- a/gtsam/slam/SmartProjectionRigFactor.h +++ b/gtsam/slam/SmartProjectionRigFactor.h @@ -71,6 +71,8 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { FastVector cameraIds_; public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + typedef CAMERA Camera; typedef CameraSet Cameras; @@ -127,7 +129,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { } /** Virtual destructor */ - ~SmartProjectionRigFactor() override {} + ~SmartProjectionRigFactor() override = default; /** * add a new measurement, corresponding to an observation from pose "poseKey" @@ -176,8 +178,8 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { if (cameraIds.size() == 0 && cameraRig_.size() > 1) { throw std::runtime_error( "SmartProjectionRigFactor: " - "camera rig includes multiple camera but add did not input " - "cameraIds"); + "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], @@ -376,9 +378,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(cameraRig_); - ar& BOOST_SERIALIZATION_NVP(cameraIds_); + //ar& BOOST_SERIALIZATION_NVP(nonUniqueKeys_); + // ar& BOOST_SERIALIZATION_NVP(cameraRig_); + //ar& BOOST_SERIALIZATION_NVP(cameraIds_); } }; // end of class declaration diff --git a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp index c652f9b41..d6a9b12fe 100644 --- a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp @@ -1229,16 +1229,16 @@ TEST(SmartProjectionRigFactor, timing) { size_t nrTests = 10000; for (size_t i = 0; i < nrTests; i++) { - SmartRigFactor::shared_ptr smartFactorP( + SmartRigFactor::shared_ptr smartRigFactor( new SmartRigFactor(model, cameraRig, params)); - smartFactorP->add(measurements_lmk1[0], x1, cameraId1); - smartFactorP->add(measurements_lmk1[1], x1, cameraId1); + smartRigFactor->add(measurements_lmk1[0], x1, cameraId1); + smartRigFactor->add(measurements_lmk1[1], x1, cameraId1); Values values; values.insert(x1, pose1); values.insert(x2, pose2); gttic_(SmartRigFactor_LINEARIZE); - smartFactorP->linearize(values); + smartRigFactor->linearize(values); gttoc_(SmartRigFactor_LINEARIZE); } diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index 4a9481d6b..e69767ad7 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -84,6 +84,9 @@ class SmartProjectionPoseFactorRollingShutter typedef std::vector> FBlocks; // vector of F blocks + /// Default constructor, only for serialization + SmartProjectionPoseFactorRollingShutter() {} + /** * Constructor * @param Isotropic measurement noise @@ -197,8 +200,8 @@ class SmartProjectionPoseFactorRollingShutter if (cameraIds.size() == 0 && cameraRig_.size() > 1) { throw std::runtime_error( "SmartProjectionPoseFactorRollingShutter: " - "camera rig includes multiple camera but add did not input " - "cameraIds"); + "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, @@ -275,6 +278,43 @@ class SmartProjectionPoseFactorRollingShutter e->cameraIds().begin()); } + /** + * Collect all cameras involved in this factor + * @param values Values structure which must contain camera poses + * corresponding to keys involved in this factor + * @return Cameras + */ + typename Base::Cameras cameras(const Values& values) const override { + typename Base::Cameras cameras; + for (size_t i = 0; i < this->measured_.size(); + i++) { // for each measurement + const Pose3& w_P_body1 = + values.at(world_P_body_key_pairs_[i].first); + const Pose3& w_P_body2 = + values.at(world_P_body_key_pairs_[i].second); + double interpolationFactor = alphas_[i]; + const Pose3& w_P_body = + interpolate(w_P_body1, w_P_body2, interpolationFactor); + const Pose3& body_P_cam = cameraRig_[cameraIds_[i]].pose(); + const Pose3& w_P_cam = w_P_body.compose(body_P_cam); + cameras.emplace_back(w_P_cam, + make_shared( + cameraRig_[cameraIds_[i]].calibration())); + } + return cameras; + } + + /** + * error calculates the error of the factor. + */ + double error(const Values& values) const override { + if (this->active(values)) { + return this->totalReprojectionError(this->cameras(values)); + } else { // else of active flag + return 0.0; + } + } + /** * Compute jacobian F, E and error vector at a given linearization point * @param values Values structure which must contain camera poses @@ -404,43 +444,6 @@ class SmartProjectionPoseFactorRollingShutter this->keys_, augmentedHessianUniqueKeys); } - /** - * error calculates the error of the factor. - */ - double error(const Values& values) const override { - if (this->active(values)) { - return this->totalReprojectionError(this->cameras(values)); - } else { // else of active flag - return 0.0; - } - } - - /** - * Collect all cameras involved in this factor - * @param values Values structure which must contain camera poses - * corresponding to keys involved in this factor - * @return Cameras - */ - typename Base::Cameras cameras(const Values& values) const override { - typename Base::Cameras cameras; - for (size_t i = 0; i < this->measured_.size(); - i++) { // for each measurement - const Pose3& w_P_body1 = - values.at(world_P_body_key_pairs_[i].first); - const Pose3& w_P_body2 = - values.at(world_P_body_key_pairs_[i].second); - double interpolationFactor = alphas_[i]; - const Pose3& w_P_body = - interpolate(w_P_body1, w_P_body2, interpolationFactor); - const Pose3& body_P_cam = cameraRig_[cameraIds_[i]].pose(); - const Pose3& w_P_cam = w_P_body.compose(body_P_cam); - cameras.emplace_back(w_P_cam, - make_shared( - cameraRig_[cameraIds_[i]].calibration())); - } - return cameras; - } - /** * Linearize to Gaussian Factor (possibly adding a damping factor Lambda for * LM) @@ -457,8 +460,8 @@ class SmartProjectionPoseFactorRollingShutter return this->createHessianFactor(values, lambda); default: throw std::runtime_error( - "SmartProjectionPoseFactorRollingShutter: unknown linearization " - "mode"); + "SmartProjectionPoseFactorRollingShutter: " + "unknown linearization mode"); } }