From b46fd77c4a062ca44e24e2f438ffa59a34b3fa29 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Wed, 4 Jan 2023 23:21:50 -0500 Subject: [PATCH] Revert "revert confusing `->template key()` notations" This reverts commit 97fcb409b99dd0963ef5e88836c066b3bad993df. --- gtsam/inference/graph-inl.h | 8 ++++---- gtsam/nonlinear/FunctorizedFactor.h | 6 +++--- gtsam/slam/BetweenFactor.h | 4 ++-- gtsam/slam/BoundingConstraint.h | 2 +- gtsam/slam/FrobeniusFactor.h | 4 ++-- gtsam/slam/GeneralSFMFactor.h | 6 +++--- gtsam/slam/ProjectionFactor.h | 6 +++--- gtsam/slam/ReferenceFrameFactor.h | 12 ++++++------ gtsam/slam/StereoFactor.h | 6 +++--- .../slam/EquivInertialNavFactor_GlobalVel.h | 10 +++++----- gtsam_unstable/slam/GaussMarkov1stOrderFactor.h | 4 ++-- .../slam/InertialNavFactor_GlobalVelocity.h | 10 +++++----- gtsam_unstable/slam/InvDepthFactor3.h | 4 ++-- gtsam_unstable/slam/PoseBetweenFactor.h | 4 ++-- gtsam_unstable/slam/PoseToPointFactor.h | 4 ++-- gtsam_unstable/slam/ProjectionFactorPPP.h | 4 ++-- gtsam_unstable/slam/ProjectionFactorPPPC.h | 4 ++-- 17 files changed, 49 insertions(+), 49 deletions(-) diff --git a/gtsam/inference/graph-inl.h b/gtsam/inference/graph-inl.h index 3ea66405b..a23eda60c 100644 --- a/gtsam/inference/graph-inl.h +++ b/gtsam/inference/graph-inl.h @@ -200,8 +200,8 @@ boost::shared_ptr composePoses(const G& graph, const PredecessorMap boost::shared_ptr factor = boost::dynamic_pointer_cast(nl_factor); if (!factor) continue; - KEY key1 = factor->key1(); - KEY key2 = factor->key2(); + KEY key1 = factor->template key<1>(); + KEY key2 = factor->template key<2>(); PoseVertex v1 = key2vertex.find(key1)->second; PoseVertex v2 = key2vertex.find(key2)->second; @@ -270,8 +270,8 @@ void split(const G& g, const PredecessorMap& tree, G& Ab1, G& Ab2) { FACTOR2>(factor); if (!factor2) continue; - KEY key1 = factor2->key1(); - KEY key2 = factor2->key2(); + KEY key1 = factor2->template key<1>(); + KEY key2 = factor2->template key<2>(); // if the tree contains the key if ((tree.find(key1) != tree.end() && tree.find(key1)->second.compare(key2) == 0) || diff --git a/gtsam/nonlinear/FunctorizedFactor.h b/gtsam/nonlinear/FunctorizedFactor.h index 1eeb6ff1f..1fb44ebf7 100644 --- a/gtsam/nonlinear/FunctorizedFactor.h +++ b/gtsam/nonlinear/FunctorizedFactor.h @@ -101,7 +101,7 @@ class FunctorizedFactor : public NoiseModelFactorN { const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override { Base::print(s, keyFormatter); std::cout << s << (s != "" ? " " : "") << "FunctorizedFactor(" - << keyFormatter(key1()) << ")" << std::endl; + << keyFormatter(this->template key<1>()) << ")" << std::endl; traits::Print(measured_, " measurement: "); std::cout << " noise model sigmas: " << noiseModel_->sigmas().transpose() << std::endl; @@ -208,8 +208,8 @@ class FunctorizedFactor2 : public NoiseModelFactorN { const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override { Base::print(s, keyFormatter); std::cout << s << (s != "" ? " " : "") << "FunctorizedFactor2(" - << keyFormatter(key1()) << ", " - << keyFormatter(key2()) << ")" << std::endl; + << keyFormatter(this->template key<1>()) << ", " + << keyFormatter(this->template key<2>()) << ")" << std::endl; traits::Print(measured_, " measurement: "); std::cout << " noise model sigmas: " << noiseModel_->sigmas().transpose() << std::endl; diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index 5bc632909..f2b41e0a1 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -88,8 +88,8 @@ namespace gtsam { const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { std::cout << s << "BetweenFactor(" - << keyFormatter(key1()) << "," - << keyFormatter(key2()) << ")\n"; + << keyFormatter(this->template key<1>()) << "," + << keyFormatter(this->template key<2>()) << ")\n"; traits::Print(measured_, " measured: "); this->noiseModel_->print(" noise model: "); } diff --git a/gtsam/slam/BoundingConstraint.h b/gtsam/slam/BoundingConstraint.h index 62dfca120..79890ec94 100644 --- a/gtsam/slam/BoundingConstraint.h +++ b/gtsam/slam/BoundingConstraint.h @@ -129,7 +129,7 @@ struct BoundingConstraint2: public NoiseModelFactorN { /** active when constraint *NOT* met */ bool active(const Values& c) const override { // note: still active at equality to avoid zigzagging - double x = value(c.at(key1()), c.at(key2())); + double x = value(c.at(this->template key<1>()), c.at(this->template key<2>())); return (isGreaterThan_) ? x <= threshold_ : x >= threshold_; } diff --git a/gtsam/slam/FrobeniusFactor.h b/gtsam/slam/FrobeniusFactor.h index c309522ea..60f880a7a 100644 --- a/gtsam/slam/FrobeniusFactor.h +++ b/gtsam/slam/FrobeniusFactor.h @@ -130,8 +130,8 @@ class FrobeniusBetweenFactor : public NoiseModelFactorN { print(const std::string &s, const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override { std::cout << s << "FrobeniusBetweenFactor<" << demangle(typeid(Rot).name()) - << ">(" << keyFormatter(key1()) << "," - << keyFormatter(key2()) << ")\n"; + << ">(" << keyFormatter(this->template key<1>()) << "," + << keyFormatter(this->template key<2>()) << ")\n"; traits::Print(R12_, " R12: "); this->noiseModel_->print(" noise model: "); } diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index e1544b524..f3089bd71 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -140,7 +140,7 @@ public: // Only linearize if the factor is active if (!this->active(values)) return boost::shared_ptr(); - const Key key1 = key1(), key2 = key2(); + const Key key1 = this->template key<1>(), key2 = this->template key<2>(); JacobianC H1; JacobianL H2; Vector2 b; @@ -270,8 +270,8 @@ public: if (H1) *H1 = Matrix::Zero(2, 6); if (H2) *H2 = Matrix::Zero(2, 3); if (H3) *H3 = Matrix::Zero(2, DimK); - std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(key2()) - << " behind Camera " << DefaultKeyFormatter(key1()) << std::endl; + std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->template key<2>()) + << " behind Camera " << DefaultKeyFormatter(this->template key<1>()) << std::endl; } return Z_2x1; } diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index 6a9d71ace..add6c86c4 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -154,10 +154,10 @@ namespace gtsam { if (H1) *H1 = Matrix::Zero(2,6); if (H2) *H2 = Matrix::Zero(2,3); if (verboseCheirality_) - std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(key2()) << - " moved behind camera " << DefaultKeyFormatter(key1()) << std::endl; + std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->template key<2>()) << + " moved behind camera " << DefaultKeyFormatter(this->template key<1>()) << std::endl; if (throwCheirality_) - throw CheiralityException(key2()); + throw CheiralityException(this->template key<2>()); } return Vector2::Constant(2.0 * K_->fx()); } diff --git a/gtsam/slam/ReferenceFrameFactor.h b/gtsam/slam/ReferenceFrameFactor.h index 47d63ef46..74e844404 100644 --- a/gtsam/slam/ReferenceFrameFactor.h +++ b/gtsam/slam/ReferenceFrameFactor.h @@ -107,16 +107,16 @@ public: void print(const std::string& s="", const gtsam::KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { std::cout << s << ": ReferenceFrameFactor(" - << "Global: " << keyFormatter(key1()) << "," - << " Transform: " << keyFormatter(key2()) << "," - << " Local: " << keyFormatter(key3()) << ")\n"; + << "Global: " << keyFormatter(this->template key<1>()) << "," + << " Transform: " << keyFormatter(this->template key<2>()) << "," + << " Local: " << keyFormatter(this->template key<3>()) << ")\n"; this->noiseModel_->print(" noise model"); } // access - convenience functions - Key global_key() const { return key1(); } - Key transform_key() const { return key2(); } - Key local_key() const { return key3(); } + Key global_key() const { return this->template key<1>(); } + Key transform_key() const { return this->template key<2>(); } + Key local_key() const { return this->template key<3>(); } private: /** Serialization function */ diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h index 102ffd759..1d2ef501d 100644 --- a/gtsam/slam/StereoFactor.h +++ b/gtsam/slam/StereoFactor.h @@ -141,10 +141,10 @@ public: if (H1) *H1 = Matrix::Zero(3,6); if (H2) *H2 = Z_3x3; if (verboseCheirality_) - std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(key2()) << - " moved behind camera " << DefaultKeyFormatter(key1()) << std::endl; + std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->template key<2>()) << + " moved behind camera " << DefaultKeyFormatter(this->template key<1>()) << std::endl; if (throwCheirality_) - throw StereoCheiralityException(key2()); + throw StereoCheiralityException(this->template key<2>()); } return Vector3::Constant(2.0 * K_->fx()); } diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h index f4b7942e3..43001fbc2 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h @@ -136,11 +136,11 @@ public: /** print */ void print(const std::string& s = "EquivInertialNavFactor_GlobalVel", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { std::cout << s << "(" - << keyFormatter(key1()) << "," - << keyFormatter(key2()) << "," - << keyFormatter(key3()) << "," - << keyFormatter(key4()) << "," - << keyFormatter(key5()) << "\n"; + << keyFormatter(this->template key<1>()) << "," + << keyFormatter(this->template key<2>()) << "," + << keyFormatter(this->template key<3>()) << "," + << keyFormatter(this->template key<4>()) << "," + << keyFormatter(this->template key<5>()) << "\n"; std::cout << "delta_pos_in_t0: " << this->delta_pos_in_t0_.transpose() << std::endl; std::cout << "delta_vel_in_t0: " << this->delta_vel_in_t0_.transpose() << std::endl; std::cout << "delta_angles: " << this->delta_angles_ << std::endl; diff --git a/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h b/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h index c0dc5d8e9..c3682e536 100644 --- a/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h +++ b/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h @@ -73,8 +73,8 @@ public: /** print */ void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { std::cout << s << "GaussMarkov1stOrderFactor(" - << keyFormatter(key1()) << "," - << keyFormatter(key2()) << ")\n"; + << keyFormatter(this->template key<1>()) << "," + << keyFormatter(this->template key<2>()) << ")\n"; this->noiseModel_->print(" noise model"); } diff --git a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h index d056ed1b1..efaf71d22 100644 --- a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h +++ b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h @@ -117,11 +117,11 @@ public: /** print */ void print(const std::string& s = "InertialNavFactor_GlobalVelocity", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { std::cout << s << "(" - << keyFormatter(key1()) << "," - << keyFormatter(key2()) << "," - << keyFormatter(key3()) << "," - << keyFormatter(key4()) << "," - << keyFormatter(key5()) << "\n"; + << keyFormatter(this->template key<1>()) << "," + << keyFormatter(this->template key<2>()) << "," + << keyFormatter(this->template key<3>()) << "," + << keyFormatter(this->template key<4>()) << "," + << keyFormatter(this->template key<5>()) << "\n"; std::cout << "acc measurement: " << this->measurement_acc_.transpose() << std::endl; std::cout << "gyro measurement: " << this->measurement_gyro_.transpose() << std::endl; std::cout << "dt: " << this->dt_ << std::endl; diff --git a/gtsam_unstable/slam/InvDepthFactor3.h b/gtsam_unstable/slam/InvDepthFactor3.h index 596ab5802..59a834f0b 100644 --- a/gtsam_unstable/slam/InvDepthFactor3.h +++ b/gtsam_unstable/slam/InvDepthFactor3.h @@ -93,8 +93,8 @@ public: if (H1) *H1 = Matrix::Zero(2,6); if (H2) *H2 = Matrix::Zero(2,5); if (H3) *H3 = Matrix::Zero(2,1); - std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(key2()) << - " moved behind camera " << DefaultKeyFormatter(key1()) << std::endl; + std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->template key<2>()) << + " moved behind camera " << DefaultKeyFormatter(this->template key<1>()) << std::endl; return Vector::Ones(2) * 2.0 * K_->fx(); } return (Vector(1) << 0.0).finished(); diff --git a/gtsam_unstable/slam/PoseBetweenFactor.h b/gtsam_unstable/slam/PoseBetweenFactor.h index 4b96d5808..676e011de 100644 --- a/gtsam_unstable/slam/PoseBetweenFactor.h +++ b/gtsam_unstable/slam/PoseBetweenFactor.h @@ -68,8 +68,8 @@ namespace gtsam { /** print */ void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { std::cout << s << "BetweenFactor(" - << keyFormatter(key1()) << "," - << keyFormatter(key2()) << ")\n"; + << keyFormatter(this->template key<1>()) << "," + << keyFormatter(this->template key<2>()) << ")\n"; measured_.print(" measured: "); if(this->body_P_sensor_) this->body_P_sensor_->print(" sensor pose in body frame: "); diff --git a/gtsam_unstable/slam/PoseToPointFactor.h b/gtsam_unstable/slam/PoseToPointFactor.h index 2e1ef79c1..ad1ba5fbe 100644 --- a/gtsam_unstable/slam/PoseToPointFactor.h +++ b/gtsam_unstable/slam/PoseToPointFactor.h @@ -48,8 +48,8 @@ class PoseToPointFactor : public NoiseModelFactorN { void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { std::cout << s << "PoseToPointFactor(" - << keyFormatter(key1()) << "," - << keyFormatter(key2()) << ")\n" + << keyFormatter(this->template key<1>()) << "," + << keyFormatter(this->template key<2>()) << ")\n" << " measured: " << measured_.transpose() << std::endl; this->noiseModel_->print(" noise model: "); } diff --git a/gtsam_unstable/slam/ProjectionFactorPPP.h b/gtsam_unstable/slam/ProjectionFactorPPP.h index cb0646cdd..8962b31b2 100644 --- a/gtsam_unstable/slam/ProjectionFactorPPP.h +++ b/gtsam_unstable/slam/ProjectionFactorPPP.h @@ -143,9 +143,9 @@ namespace gtsam { if (H3) *H3 = Matrix::Zero(2,3); if (verboseCheirality_) std::cout << e.what() << ": Landmark " - << DefaultKeyFormatter(key2()) + << DefaultKeyFormatter(this->template key<2>()) << " moved behind camera " - << DefaultKeyFormatter(key1()) + << DefaultKeyFormatter(this->template key<1>()) << std::endl; if (throwCheirality_) throw e; diff --git a/gtsam_unstable/slam/ProjectionFactorPPPC.h b/gtsam_unstable/slam/ProjectionFactorPPPC.h index f5fdc1066..afbf85838 100644 --- a/gtsam_unstable/slam/ProjectionFactorPPPC.h +++ b/gtsam_unstable/slam/ProjectionFactorPPPC.h @@ -130,8 +130,8 @@ class GTSAM_UNSTABLE_EXPORT ProjectionFactorPPPC if (H3) *H3 = Matrix::Zero(2,3); if (H4) *H4 = Matrix::Zero(2,CALIBRATION::Dim()); if (verboseCheirality_) - std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(key2()) << - " moved behind camera " << DefaultKeyFormatter(key1()) << std::endl; + std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->template key<2>()) << + " moved behind camera " << DefaultKeyFormatter(this->template key<1>()) << std::endl; if (throwCheirality_) throw e; }