diff --git a/gtsam/inference/graph-inl.h b/gtsam/inference/graph-inl.h index a23eda60c5..3ea66405b7 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->template key<1>(); - KEY key2 = factor->template key<2>(); + KEY key1 = factor->key1(); + KEY key2 = factor->key2(); 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->template key<1>(); - KEY key2 = factor2->template key<2>(); + KEY key1 = factor2->key1(); + KEY key2 = factor2->key2(); // 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 1fb44ebf77..1dc397c686 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(this->template key<1>()) << ")" << std::endl; + << keyFormatter(this->key1()) << ")" << 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(this->template key<1>()) << ", " - << keyFormatter(this->template key<2>()) << ")" << std::endl; + << keyFormatter(this->key1()) << ", " + << keyFormatter(this->key2()) << ")" << std::endl; traits::Print(measured_, " measurement: "); std::cout << " noise model sigmas: " << noiseModel_->sigmas().transpose() << std::endl; diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index c58179db35..f6517d2810 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -272,6 +272,64 @@ class GTSAM_EXPORT NoiseModelFactor: public NonlinearFactor { }; // \class NoiseModelFactor +/* ************************************************************************* */ +namespace detail { +/** Convenience base class to add aliases `X1`, `X2`, ..., `X6` -> ValueType. + * Usage example: + * ``` + * class MyFactor : public NoiseModelFactorN, + * public NoiseModelFactorAliases { + * // class implementation ... + * }; + * + * // MyFactor::X1 == Pose3 + * // MyFactor::X2 == Point3 + * ``` + */ +template +struct NoiseModelFactorAliases {}; +template +struct NoiseModelFactorAliases { + using X = T1; + using X1 = T1; +}; +template +struct NoiseModelFactorAliases { + using X1 = T1; + using X2 = T2; +}; +template +struct NoiseModelFactorAliases { + using X1 = T1; + using X2 = T2; + using X3 = T3; +}; +template +struct NoiseModelFactorAliases { + using X1 = T1; + using X2 = T2; + using X3 = T3; + using X4 = T4; +}; +template +struct NoiseModelFactorAliases { + using X1 = T1; + using X2 = T2; + using X3 = T3; + using X4 = T4; + using X5 = T5; +}; +template +struct NoiseModelFactorAliases { + using X1 = T1; + using X2 = T2; + using X3 = T3; + using X4 = T4; + using X5 = T5; + using X6 = T6; +}; +} // namespace detail /* ************************************************************************* */ /** @@ -327,7 +385,9 @@ class GTSAM_EXPORT NoiseModelFactor: public NonlinearFactor { * objects in non-linear manifolds (Lie groups). */ template -class NoiseModelFactorN : public NoiseModelFactor { +class NoiseModelFactorN + : public NoiseModelFactor, + public detail::NoiseModelFactorAliases { public: /// N is the number of variables (N-way factor) enum { N = sizeof...(ValueTypes) }; @@ -377,6 +437,14 @@ class NoiseModelFactorN : public NoiseModelFactor { * // Factor::ValueType<0> // ERROR! Will not compile. * // Factor::ValueType<3> // ERROR! Will not compile. * ``` + * + * You can also use the shortcuts `X1`, ..., `X6` which are the same as + * `ValueType<1>`, ..., `ValueType<6>` respectively (see + * detail::NoiseModelFactorAliases). + * + * Note that, if your class is templated AND you want to use `ValueType<1>` + * inside your class, due to dependent types you need the `template` keyword: + * `typename MyFactor::template ValueType<1>`. */ template > using ValueType = @@ -431,6 +499,10 @@ class NoiseModelFactorN : public NoiseModelFactor { * // key<0>() // ERROR! Will not compile * // key<3>() // ERROR! Will not compile * ``` + * + * Note that, if your class is templated AND you are trying to call `key<1>` + * inside your class, due to dependent types you need the `template` keyword: + * `this->key1()`. */ template inline Key key() const { @@ -555,37 +627,34 @@ class NoiseModelFactorN : public NoiseModelFactor { } public: - /// @name Deprecated methods. Use `key<1>()`, `key<2>()`, ... instead of old - /// `key1()`, `key2()`, ... - /// If your class is templated AND you are trying to call `key<1>` inside your - /// class, due to dependent types you need to do `this->template key<1>()`. + /// @name Shortcut functions `key1()` -> `key<1>()` /// @{ - inline Key GTSAM_DEPRECATED key1() const { + inline Key key1() const { return key<1>(); } template - inline Key GTSAM_DEPRECATED key2() const { + inline Key key2() const { static_assert(I <= N, "Index out of bounds"); return key<2>(); } template - inline Key GTSAM_DEPRECATED key3() const { + inline Key key3() const { static_assert(I <= N, "Index out of bounds"); return key<3>(); } template - inline Key GTSAM_DEPRECATED key4() const { + inline Key key4() const { static_assert(I <= N, "Index out of bounds"); return key<4>(); } template - inline Key GTSAM_DEPRECATED key5() const { + inline Key key5() const { static_assert(I <= N, "Index out of bounds"); return key<5>(); } template - inline Key GTSAM_DEPRECATED key6() const { + inline Key key6() const { static_assert(I <= N, "Index out of bounds"); return key<6>(); } @@ -594,268 +663,11 @@ class NoiseModelFactorN : public NoiseModelFactor { }; // \class NoiseModelFactorN -/* ************************************************************************* */ -/** @deprecated: use NoiseModelFactorN, replacing .key() with .key<1>() and X1 - * with ValueType<1>. - * If your class is templated AND you are trying to call `.key<1>()` or - * `ValueType<1>` inside your class, due to dependent types you need to do - * `this->template key<1>()` or `This::template ValueType<1>`. - * ~~~ - * A convenient base class for creating your own NoiseModelFactor - * with 1 variable. To derive from this class, implement evaluateError(). - */ -template -class GTSAM_DEPRECATED NoiseModelFactor1 : public NoiseModelFactorN { - public: - /** Aliases for value types pulled from keys, for backwards compatibility. - * Note: in your code you can probably just do: - * `using X = ValueType<1>;` - * but this class is uglier due to dependent types. - * See e.g. testNonlinearFactor.cpp:TestFactorN. - */ - using X = typename NoiseModelFactor1::template ValueType<1>; - - protected: - using Base = NoiseModelFactor; // grandparent, for backwards compatibility - using This = NoiseModelFactor1; - - public: - // inherit NoiseModelFactorN's constructors - using NoiseModelFactorN::NoiseModelFactorN; - ~NoiseModelFactor1() override {} - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - ar& boost::serialization::make_nvp( - "NoiseModelFactor", boost::serialization::base_object(*this)); - } -}; // \class NoiseModelFactor1 - -/* ************************************************************************* */ -/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1>() and X1 - * with ValueType<1>. - * If your class is templated AND you are trying to call `.key<1>()` or - * `ValueType<1>` inside your class, due to dependent types you need to do - * `this->template key<1>()` or `This::template ValueType<1>`. - * ~~~ - * A convenient base class for creating your own NoiseModelFactor - * with 2 variables. To derive from this class, implement evaluateError(). - */ -template -class GTSAM_DEPRECATED NoiseModelFactor2 - : public NoiseModelFactorN { - public: - /** Aliases for value types pulled from keys. - * Note: in your code you can probably just do: - * `using X1 = ValueType<1>;` - * but this class is uglier due to dependent types. - * See e.g. testNonlinearFactor.cpp:TestFactorN. - */ - using X1 = typename NoiseModelFactor2::template ValueType<1>; - using X2 = typename NoiseModelFactor2::template ValueType<2>; - - protected: - using Base = NoiseModelFactor; - using This = NoiseModelFactor2; - - public: - // inherit NoiseModelFactorN's constructors - using NoiseModelFactorN::NoiseModelFactorN; - ~NoiseModelFactor2() override {} - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - ar& boost::serialization::make_nvp( - "NoiseModelFactor", boost::serialization::base_object(*this)); - } -}; // \class NoiseModelFactor2 - -/* ************************************************************************* */ -/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1>() and X1 - * with ValueType<1>. - * If your class is templated AND you are trying to call `.key<1>()` or - * `ValueType<1>` inside your class, due to dependent types you need to do - * `this->template key<1>()` or `This::template ValueType<1>`. - * ~~~ - * A convenient base class for creating your own NoiseModelFactor - * with 3 variables. To derive from this class, implement evaluateError(). - */ -template -class GTSAM_DEPRECATED NoiseModelFactor3 - : public NoiseModelFactorN { - public: - /** Aliases for value types pulled from keys. - * Note: in your code you can probably just do: - * `using X1 = ValueType<1>;` - * but this class is uglier due to dependent types. - * See e.g. testNonlinearFactor.cpp:TestFactorN. - */ - using X1 = typename NoiseModelFactor3::template ValueType<1>; - using X2 = typename NoiseModelFactor3::template ValueType<2>; - using X3 = typename NoiseModelFactor3::template ValueType<3>; - - protected: - using Base = NoiseModelFactor; - using This = NoiseModelFactor3; - - public: - // inherit NoiseModelFactorN's constructors - using NoiseModelFactorN::NoiseModelFactorN; - ~NoiseModelFactor3() override {} - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - ar& boost::serialization::make_nvp( - "NoiseModelFactor", boost::serialization::base_object(*this)); - } -}; // \class NoiseModelFactor3 - -/* ************************************************************************* */ -/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1>() and X1 - * with ValueType<1>. - * If your class is templated AND you are trying to call `.key<1>()` or - * `ValueType<1>` inside your class, due to dependent types you need to do - * `this->template key<1>()` or `This::template ValueType<1>`. - * ~~~ - * A convenient base class for creating your own NoiseModelFactor - * with 4 variables. To derive from this class, implement evaluateError(). - */ -template -class GTSAM_DEPRECATED NoiseModelFactor4 - : public NoiseModelFactorN { - public: - /** Aliases for value types pulled from keys. - * Note: in your code you can probably just do: - * `using X1 = ValueType<1>;` - * but this class is uglier due to dependent types. - * See e.g. testNonlinearFactor.cpp:TestFactorN. - */ - using X1 = typename NoiseModelFactor4::template ValueType<1>; - using X2 = typename NoiseModelFactor4::template ValueType<2>; - using X3 = typename NoiseModelFactor4::template ValueType<3>; - using X4 = typename NoiseModelFactor4::template ValueType<4>; - - protected: - using Base = NoiseModelFactor; - using This = NoiseModelFactor4; - - public: - // inherit NoiseModelFactorN's constructors - using NoiseModelFactorN::NoiseModelFactorN; - ~NoiseModelFactor4() override {} - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - ar& boost::serialization::make_nvp( - "NoiseModelFactor", boost::serialization::base_object(*this)); - } -}; // \class NoiseModelFactor4 - -/* ************************************************************************* */ -/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1>() and X1 - * with ValueType<1>. - * If your class is templated AND you are trying to call `.key<1>()` or - * `ValueType<1>` inside your class, due to dependent types you need to do - * `this->template key<1>()` or `This::template ValueType<1>`. - * ~~~ - * A convenient base class for creating your own NoiseModelFactor - * with 5 variables. To derive from this class, implement evaluateError(). - */ -template -class GTSAM_DEPRECATED NoiseModelFactor5 - : public NoiseModelFactorN { - public: - /** Aliases for value types pulled from keys. - * Note: in your code you can probably just do: - * `using X1 = ValueType<1>;` - * but this class is uglier due to dependent types. - * See e.g. testNonlinearFactor.cpp:TestFactorN. - */ - using X1 = typename NoiseModelFactor5::template ValueType<1>; - using X2 = typename NoiseModelFactor5::template ValueType<2>; - using X3 = typename NoiseModelFactor5::template ValueType<3>; - using X4 = typename NoiseModelFactor5::template ValueType<4>; - using X5 = typename NoiseModelFactor5::template ValueType<5>; - - protected: - using Base = NoiseModelFactor; - using This = NoiseModelFactor5; - - public: - // inherit NoiseModelFactorN's constructors - using NoiseModelFactorN::NoiseModelFactorN; - ~NoiseModelFactor5() override {} - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - ar& boost::serialization::make_nvp( - "NoiseModelFactor", boost::serialization::base_object(*this)); - } -}; // \class NoiseModelFactor5 - -/* ************************************************************************* */ -/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1>() and X1 - * with ValueType<1>. - * If your class is templated AND you are trying to call `.key<1>()` or - * `ValueType<1>` inside your class, due to dependent types you need to do - * `this->template key<1>()` or `This::template ValueType<1>`. - * ~~~ - * A convenient base class for creating your own NoiseModelFactor - * with 6 variables. To derive from this class, implement evaluateError(). - */ -template -class GTSAM_DEPRECATED NoiseModelFactor6 - : public NoiseModelFactorN { - public: - /** Aliases for value types pulled from keys. - * Note: in your code you can probably just do: - * `using X1 = ValueType<1>;` - * but this class is uglier due to dependent types. - * See e.g. testNonlinearFactor.cpp:TestFactorN. - */ - using X1 = typename NoiseModelFactor6::template ValueType<1>; - using X2 = typename NoiseModelFactor6::template ValueType<2>; - using X3 = typename NoiseModelFactor6::template ValueType<3>; - using X4 = typename NoiseModelFactor6::template ValueType<4>; - using X5 = typename NoiseModelFactor6::template ValueType<5>; - using X6 = typename NoiseModelFactor6::template ValueType<6>; - - protected: - using Base = NoiseModelFactor; - using This = - NoiseModelFactor6; - - public: - // inherit NoiseModelFactorN's constructors - using NoiseModelFactorN::NoiseModelFactorN; - ~NoiseModelFactor6() override {} - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - ar& boost::serialization::make_nvp( - "NoiseModelFactor", boost::serialization::base_object(*this)); - } -}; // \class NoiseModelFactor6 +#define NoiseModelFactor1 NoiseModelFactorN +#define NoiseModelFactor2 NoiseModelFactorN +#define NoiseModelFactor3 NoiseModelFactorN +#define NoiseModelFactor4 NoiseModelFactorN +#define NoiseModelFactor5 NoiseModelFactorN +#define NoiseModelFactor6 NoiseModelFactorN -} // \namespace gtsam +} // namespace gtsam diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index f2b41e0a1c..d420e2b54d 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(this->template key<1>()) << "," - << keyFormatter(this->template key<2>()) << ")\n"; + << keyFormatter(this->key1()) << "," + << keyFormatter(this->key2()) << ")\n"; traits::Print(measured_, " measured: "); this->noiseModel_->print(" noise model: "); } diff --git a/gtsam/slam/BoundingConstraint.h b/gtsam/slam/BoundingConstraint.h index 79890ec944..bd20668d84 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(this->template key<1>()), c.at(this->template key<2>())); + double x = value(c.at(this->key1()), c.at(this->key2())); return (isGreaterThan_) ? x <= threshold_ : x >= threshold_; } diff --git a/gtsam/slam/FrobeniusFactor.h b/gtsam/slam/FrobeniusFactor.h index 60f880a7a9..2e7e28d033 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(this->template key<1>()) << "," - << keyFormatter(this->template key<2>()) << ")\n"; + << ">(" << keyFormatter(this->key1()) << "," + << keyFormatter(this->key2()) << ")\n"; traits::Print(R12_, " R12: "); this->noiseModel_->print(" noise model: "); } diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index f3089bd71f..37296c0d88 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -140,7 +140,7 @@ class GeneralSFMFactor: public NoiseModelFactorN { // Only linearize if the factor is active if (!this->active(values)) return boost::shared_ptr(); - const Key key1 = this->template key<1>(), key2 = this->template key<2>(); + const Key key1 = this->key1(), key2 = this->key2(); JacobianC H1; JacobianL H2; Vector2 b; @@ -270,8 +270,8 @@ class GeneralSFMFactor2: public NoiseModelFactorN { 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(this->template key<2>()) - << " behind Camera " << DefaultKeyFormatter(this->template key<1>()) << std::endl; + std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) + << " behind Camera " << DefaultKeyFormatter(this->key1()) << std::endl; } return Z_2x1; } diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index add6c86c4c..c90fc80d58 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(this->template key<2>()) << - " moved behind camera " << DefaultKeyFormatter(this->template key<1>()) << std::endl; + std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << + " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; if (throwCheirality_) - throw CheiralityException(this->template key<2>()); + throw CheiralityException(this->key2()); } return Vector2::Constant(2.0 * K_->fx()); } diff --git a/gtsam/slam/ReferenceFrameFactor.h b/gtsam/slam/ReferenceFrameFactor.h index 74e8444046..383c81cc47 100644 --- a/gtsam/slam/ReferenceFrameFactor.h +++ b/gtsam/slam/ReferenceFrameFactor.h @@ -107,16 +107,16 @@ class ReferenceFrameFactor : public NoiseModelFactorN { void print(const std::string& s="", const gtsam::KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { std::cout << s << ": ReferenceFrameFactor(" - << "Global: " << keyFormatter(this->template key<1>()) << "," - << " Transform: " << keyFormatter(this->template key<2>()) << "," - << " Local: " << keyFormatter(this->template key<3>()) << ")\n"; + << "Global: " << keyFormatter(this->key1()) << "," + << " Transform: " << keyFormatter(this->key2()) << "," + << " Local: " << keyFormatter(this->key3()) << ")\n"; this->noiseModel_->print(" noise model"); } // access - convenience functions - 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>(); } + Key global_key() const { return this->key1(); } + Key transform_key() const { return this->key2(); } + Key local_key() const { return this->key3(); } private: /** Serialization function */ diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h index 1d2ef501d6..1013b22b1d 100644 --- a/gtsam/slam/StereoFactor.h +++ b/gtsam/slam/StereoFactor.h @@ -141,10 +141,10 @@ class GenericStereoFactor: public NoiseModelFactorN { if (H1) *H1 = Matrix::Zero(3,6); if (H2) *H2 = Z_3x3; if (verboseCheirality_) - std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->template key<2>()) << - " moved behind camera " << DefaultKeyFormatter(this->template key<1>()) << std::endl; + std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << + " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; if (throwCheirality_) - throw StereoCheiralityException(this->template key<2>()); + throw StereoCheiralityException(this->key2()); } return Vector3::Constant(2.0 * K_->fx()); } diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h index 43001fbc2a..e52837fb4e 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h @@ -136,11 +136,11 @@ class EquivInertialNavFactor_GlobalVel : public NoiseModelFactorNtemplate key<1>()) << "," - << keyFormatter(this->template key<2>()) << "," - << keyFormatter(this->template key<3>()) << "," - << keyFormatter(this->template key<4>()) << "," - << keyFormatter(this->template key<5>()) << "\n"; + << keyFormatter(this->key1()) << "," + << keyFormatter(this->key2()) << "," + << keyFormatter(this->key3()) << "," + << keyFormatter(this->key4()) << "," + << keyFormatter(this->key5()) << "\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 c3682e536d..5d677b82e6 100644 --- a/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h +++ b/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h @@ -73,8 +73,8 @@ class GaussMarkov1stOrderFactor: public NoiseModelFactorN { /** print */ void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { std::cout << s << "GaussMarkov1stOrderFactor(" - << keyFormatter(this->template key<1>()) << "," - << keyFormatter(this->template key<2>()) << ")\n"; + << keyFormatter(this->key1()) << "," + << keyFormatter(this->key2()) << ")\n"; this->noiseModel_->print(" noise model"); } diff --git a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h index efaf71d229..fd3ab729d1 100644 --- a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h +++ b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h @@ -117,11 +117,11 @@ class InertialNavFactor_GlobalVelocity : public NoiseModelFactorNtemplate key<1>()) << "," - << keyFormatter(this->template key<2>()) << "," - << keyFormatter(this->template key<3>()) << "," - << keyFormatter(this->template key<4>()) << "," - << keyFormatter(this->template key<5>()) << "\n"; + << keyFormatter(this->key1()) << "," + << keyFormatter(this->key2()) << "," + << keyFormatter(this->key3()) << "," + << keyFormatter(this->key4()) << "," + << keyFormatter(this->key5()) << "\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 59a834f0b7..2d0d57437f 100644 --- a/gtsam_unstable/slam/InvDepthFactor3.h +++ b/gtsam_unstable/slam/InvDepthFactor3.h @@ -93,8 +93,8 @@ class InvDepthFactor3: public NoiseModelFactorN { 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(this->template key<2>()) << - " moved behind camera " << DefaultKeyFormatter(this->template key<1>()) << std::endl; + std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << + " moved behind camera " << DefaultKeyFormatter(this->key1()) << 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 676e011de2..aaf00b45d1 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(this->template key<1>()) << "," - << keyFormatter(this->template key<2>()) << ")\n"; + << keyFormatter(this->key1()) << "," + << keyFormatter(this->key2()) << ")\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 ad1ba5fbe6..0d295113d6 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(this->template key<1>()) << "," - << keyFormatter(this->template key<2>()) << ")\n" + << keyFormatter(this->key1()) << "," + << keyFormatter(this->key2()) << ")\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 8962b31b2d..ab8cba4816 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(this->template key<2>()) + << DefaultKeyFormatter(this->key2()) << " moved behind camera " - << DefaultKeyFormatter(this->template key<1>()) + << DefaultKeyFormatter(this->key1()) << std::endl; if (throwCheirality_) throw e; diff --git a/gtsam_unstable/slam/ProjectionFactorPPPC.h b/gtsam_unstable/slam/ProjectionFactorPPPC.h index afbf858382..dec893af48 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(this->template key<2>()) << - " moved behind camera " << DefaultKeyFormatter(this->template key<1>()) << std::endl; + std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << + " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; if (throwCheirality_) throw e; } diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index c536a48c3f..c823614502 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -330,13 +330,6 @@ TEST( NonlinearFactor, cloneWithNewNoiseModel ) } /* ************************************************************************* */ -// Suppress deprecation warnings while we are testing backwards compatibility -#define IGNORE_DEPRECATED_PUSH \ - CLANG_DIAGNOSTIC_PUSH_IGNORE("-Wdeprecated-declarations") \ - GCC_DIAGNOSTIC_PUSH_IGNORE("-Wdeprecated-declarations") \ - MSVC_DIAGNOSTIC_PUSH_IGNORE(4996) -/* ************************************************************************* */ -IGNORE_DEPRECATED_PUSH class TestFactor1 : public NoiseModelFactor1 { static_assert(std::is_same::value, "Base type wrong"); static_assert(std::is_same>::value, @@ -358,7 +351,6 @@ class TestFactor1 : public NoiseModelFactor1 { gtsam::NonlinearFactor::shared_ptr(new TestFactor1(*this))); } }; -DIAGNOSTIC_POP() /* ************************************ */ TEST(NonlinearFactor, NoiseModelFactor1) { @@ -388,7 +380,6 @@ TEST(NonlinearFactor, NoiseModelFactor1) { } /* ************************************************************************* */ -IGNORE_DEPRECATED_PUSH class TestFactor4 : public NoiseModelFactor4 { static_assert(std::is_same::value, "Base type wrong"); static_assert( @@ -420,7 +411,6 @@ class TestFactor4 : public NoiseModelFactor4 { return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new TestFactor4(*this))); } }; -DIAGNOSTIC_POP() /* ************************************ */ TEST(NonlinearFactor, NoiseModelFactor4) { @@ -444,7 +434,6 @@ TEST(NonlinearFactor, NoiseModelFactor4) { EXPECT(assert_equal((Vector)(Vector(1) << 0.5 * -30.).finished(), jf.getb())); // Test all functions/types for backwards compatibility - IGNORE_DEPRECATED_PUSH static_assert(std::is_same::value, "X1 type incorrect"); static_assert(std::is_same::value, @@ -463,7 +452,6 @@ TEST(NonlinearFactor, NoiseModelFactor4) { EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 2.).finished(), H.at(1))); EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 3.).finished(), H.at(2))); EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 4.).finished(), H.at(3))); - DIAGNOSTIC_POP() // And test "forward compatibility" using `key` and `ValueType` too static_assert(std::is_same, double>::value, @@ -489,7 +477,6 @@ TEST(NonlinearFactor, NoiseModelFactor4) { } /* ************************************************************************* */ -IGNORE_DEPRECATED_PUSH class TestFactor5 : public NoiseModelFactor5 { public: typedef NoiseModelFactor5 Base; @@ -513,7 +500,6 @@ class TestFactor5 : public NoiseModelFactor5 { public: typedef NoiseModelFactor6 Base; @@ -569,7 +554,6 @@ class TestFactor6 : public NoiseModelFactor6::value, "X1 type incorrect"); EXPECT(assert_equal(tf.key3(), X(3))); - DIAGNOSTIC_POP() + // Test using `key` and `ValueType` static_assert(std::is_same, double>::value,