From e5017984a1ed98ca2b759772a2f492b336b43ce4 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 26 Dec 2014 16:47:51 +0100 Subject: [PATCH] traits_x -> traits --- gtsam/base/ChartValue.h | 8 +- gtsam/base/GenericValue.h | 81 ++----------------- gtsam/base/Group.h | 24 +++--- gtsam/base/Lie.h | 22 ++--- gtsam/base/LieMatrix.h | 2 +- gtsam/base/LieScalar.h | 2 +- gtsam/base/LieVector.h | 2 +- gtsam/base/Manifold.h | 24 +++--- gtsam/base/Testable.h | 24 +++--- gtsam/base/VectorSpace.h | 16 ++-- gtsam/base/concepts.h | 2 +- gtsam/base/numericalDerivative.h | 47 ++++++----- gtsam/base/testLie.h | 4 +- gtsam/base/tests/testLieMatrix.cpp | 8 +- gtsam/base/tests/testLieScalar.cpp | 4 +- gtsam/discrete/DecisionTreeFactor.h | 2 +- gtsam/discrete/DiscreteBayesNet.h | 2 +- gtsam/discrete/DiscreteConditional.h | 2 +- gtsam/discrete/DiscreteFactor.h | 4 +- gtsam/discrete/DiscreteFactorGraph.h | 2 +- gtsam/discrete/Potentials.h | 4 +- .../tests/testAlgebraicDecisionTree.cpp | 2 +- gtsam/discrete/tests/testDecisionTree.cpp | 4 +- gtsam/geometry/Cal3Bundler.h | 2 +- gtsam/geometry/Cal3DS2.h | 2 +- gtsam/geometry/Cal3Unified.h | 2 +- gtsam/geometry/Cal3_S2.h | 2 +- gtsam/geometry/Cal3_S2Stereo.h | 2 +- gtsam/geometry/CalibratedCamera.h | 2 +- gtsam/geometry/Cyclic.h | 2 +- gtsam/geometry/EssentialMatrix.h | 2 +- gtsam/geometry/PinholeCamera.h | 2 +- gtsam/geometry/Point2.h | 2 +- gtsam/geometry/Point3.h | 2 +- gtsam/geometry/Pose2.h | 2 +- gtsam/geometry/Pose3.h | 2 +- gtsam/geometry/Quaternion.h | 2 +- gtsam/geometry/Rot2.h | 2 +- gtsam/geometry/Rot3.h | 2 +- gtsam/geometry/SO3.h | 2 +- gtsam/geometry/StereoCamera.h | 2 +- gtsam/geometry/StereoPoint2.h | 2 +- gtsam/geometry/Unit3.h | 2 +- gtsam/geometry/tests/testCyclic.cpp | 32 ++++---- gtsam/geometry/tests/testPoint2.cpp | 10 +-- gtsam/geometry/tests/testPoint3.cpp | 8 +- gtsam/geometry/tests/testQuaternion.cpp | 28 +++---- gtsam/geometry/tests/testSO3.cpp | 26 +++--- gtsam/inference/LabeledSymbol.h | 2 +- gtsam/inference/Ordering.h | 2 +- gtsam/inference/Symbol.h | 2 +- gtsam/inference/VariableIndex.h | 2 +- gtsam/inference/VariableSlots.h | 2 +- gtsam/linear/Errors.h | 2 +- gtsam/linear/GaussianBayesNet.h | 2 +- gtsam/linear/GaussianBayesTree.h | 2 +- gtsam/linear/GaussianConditional.h | 2 +- gtsam/linear/GaussianFactor.h | 2 +- gtsam/linear/GaussianFactorGraph.h | 2 +- gtsam/linear/HessianFactor.h | 2 +- gtsam/linear/JacobianFactor.h | 2 +- gtsam/linear/NoiseModel.h | 10 +-- gtsam/linear/VectorValues.h | 2 +- gtsam/navigation/AttitudeFactor.h | 4 +- gtsam/navigation/ImuBias.h | 2 +- gtsam/nonlinear/AdaptAutoDiff.h | 10 +-- gtsam/nonlinear/Expression-inl.h | 14 ++-- gtsam/nonlinear/Expression.h | 4 +- gtsam/nonlinear/ExtendedKalmanFilter-inl.h | 4 +- gtsam/nonlinear/ISAM2-inl.h | 2 +- gtsam/nonlinear/ISAM2.h | 2 +- gtsam/nonlinear/LinearContainerFactor.h | 2 +- gtsam/nonlinear/NonlinearEquality.h | 30 +++---- gtsam/nonlinear/NonlinearFactor.h | 2 +- gtsam/nonlinear/NonlinearFactorGraph.h | 2 +- gtsam/nonlinear/Values.h | 2 +- gtsam/nonlinear/tests/testAdaptAutoDiff.cpp | 2 +- gtsam/nonlinear/tests/testValues.cpp | 2 +- gtsam/slam/BetweenFactor.h | 18 ++--- gtsam/slam/GeneralSFMFactor.h | 4 +- gtsam/slam/PoseTranslationPrior.h | 4 +- gtsam/slam/PriorFactor.h | 8 +- gtsam/slam/ProjectionFactor.h | 2 +- gtsam/slam/RangeFactor.h | 4 +- gtsam/slam/ReferenceFrameFactor.h | 4 +- gtsam/slam/SmartFactorBase.h | 2 +- gtsam/slam/SmartProjectionPoseFactor.h | 2 +- gtsam/slam/StereoFactor.h | 2 +- gtsam/symbolic/SymbolicBayesNet.h | 2 +- gtsam/symbolic/SymbolicBayesTree.h | 4 +- gtsam/symbolic/SymbolicConditional.h | 2 +- gtsam/symbolic/SymbolicEliminationTree.h | 2 +- gtsam/symbolic/SymbolicFactor.h | 2 +- gtsam/symbolic/SymbolicFactorGraph.h | 2 +- gtsam_unstable/dynamics/PoseRTV.h | 2 +- gtsam_unstable/geometry/BearingS2.h | 2 +- gtsam_unstable/geometry/Pose3Upright.h | 2 +- gtsam_unstable/geometry/SimWall2D.h | 2 +- gtsam_unstable/linear/LinearEquality.h | 2 +- .../linear/LinearEqualityFactorGraph.h | 2 +- gtsam_unstable/linear/LinearInequality.h | 2 +- .../linear/LinearInequalityFactorGraph.h | 2 +- .../nonlinear/BatchFixedLagSmoother.h | 2 +- .../nonlinear/ConcurrentBatchFilter.h | 2 +- .../nonlinear/ConcurrentBatchSmoother.h | 2 +- .../nonlinear/ConcurrentIncrementalFilter.h | 2 +- .../nonlinear/ConcurrentIncrementalSmoother.h | 2 +- gtsam_unstable/nonlinear/ExpressionFactor.h | 8 +- gtsam_unstable/nonlinear/LinearizedFactor.h | 4 +- gtsam_unstable/nonlinear/expressionTesting.h | 2 +- .../nonlinear/tests/testCallRecord.cpp | 2 +- .../slam/GaussMarkov1stOrderFactor.h | 6 +- .../slam/InertialNavFactor_GlobalVelocity.h | 2 +- gtsam_unstable/slam/ProjectionFactorPPP.h | 2 +- gtsam_unstable/slam/ProjectionFactorPPPC.h | 2 +- .../slam/SmartStereoProjectionFactor.h | 2 +- .../slam/SmartStereoProjectionPoseFactor.h | 2 +- .../slam/TransformBtwRobotsUnaryFactor.h | 2 +- .../slam/TransformBtwRobotsUnaryFactorEM.h | 2 +- .../slam/tests/testPoseBetweenFactor.cpp | 2 +- .../slam/tests/testPosePriorFactor.cpp | 2 +- .../slam/tests/testProjectionFactorPPP.cpp | 2 +- tests/simulated2D.h | 4 +- tests/simulated2DConstraints.h | 4 +- tests/testManifold.cpp | 48 +++++------ 125 files changed, 340 insertions(+), 406 deletions(-) diff --git a/gtsam/base/ChartValue.h b/gtsam/base/ChartValue.h index f6e472d23..7c95592e1 100644 --- a/gtsam/base/ChartValue.h +++ b/gtsam/base/ChartValue.h @@ -210,11 +210,11 @@ //template //GenericValue convertToChartValue(const T& value, // boost::optional< -// Eigen::Matrix::dimension, -// traits_x::dimension>&> H = boost::none) { +// Eigen::Matrix::dimension, +// traits::dimension>&> H = boost::none) { // if (H) { -// *H = Eigen::Matrix::dimension, -// traits_x::dimension>::Identity(); +// *H = Eigen::Matrix::dimension, +// traits::dimension>::Identity(); // } // return GenericValue(value); //} diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h index 9c846f739..ad2ea05a2 100644 --- a/gtsam/base/GenericValue.h +++ b/gtsam/base/GenericValue.h @@ -19,7 +19,7 @@ #pragma once -#include +#include #include #include @@ -30,71 +30,6 @@ namespace gtsam { -// To play as a GenericValue, we need the following traits -namespace traits { - -// trait to wrap the default equals of types -template -struct equals { - typedef T type; - typedef bool result_type; - bool operator()(const T& a, const T& b, double tol) { - return a.equals(b, tol); - } -}; - -// trait to wrap the default print of types -template -struct print { - typedef T type; - typedef void result_type; - void operator()(const T& obj, const std::string& str) { - obj.print(str); - } -}; - -// equals for scalars -template<> -struct equals { - typedef double type; - typedef bool result_type; - bool operator()(double a, double b, double tol) { - return std::abs(a - b) <= tol; - } -}; - -// print for scalars -template<> -struct print { - typedef double type; - typedef void result_type; - void operator()(double a, const std::string& str) { - std::cout << str << ": " << a << std::endl; - } -}; - -// equals for Matrix types -template -struct equals > { - typedef Eigen::Matrix type; - typedef bool result_type; - bool operator()(const type& A, const type& B, double tol) { - return equal_with_abs_tol(A, B, tol); - } -}; - -// print for Matrix types -template -struct print > { - typedef Eigen::Matrix type; - typedef void result_type; - void operator()(const type& A, const std::string& str) { - std::cout << str << ": " << A << std::endl; - } -}; - -} - /** * Wraps any type T so it can play as a Value */ @@ -137,17 +72,17 @@ public: // Cast the base class Value pointer to a templated generic class pointer const GenericValue& genericValue2 = static_cast(p); // Return the result of using the equals traits for the derived class - return traits::equals()(this->value_, genericValue2.value_, tol); + return traits::Equals(this->value_, genericValue2.value_, tol); } /// non virtual equals function, uses traits bool equals(const GenericValue &other, double tol = 1e-9) const { - return traits::equals()(this->value(), other.value(), tol); + return traits::Equals(this->value(), other.value(), tol); } /// Virtual print function, uses traits virtual void print(const std::string& str) const { - traits::print()(value_, str); + traits::Print(value_, str); } /** @@ -179,7 +114,7 @@ public: /// Generic Value interface version of retract virtual Value* retract_(const Vector& delta) const { // Call retract on the derived class using the retract trait function - const T retractResult = traits_x::Retract(GenericValue::value(), delta); + const T retractResult = traits::Retract(GenericValue::value(), delta); // Create a Value pointer copy of the result void* resultAsValuePlace = @@ -197,12 +132,12 @@ public: static_cast&>(value2); // Return the result of calling localCoordinates trait on the derived class - return traits_x::Local(GenericValue::value(), genericValue2.value()); + return traits::Local(GenericValue::value(), genericValue2.value()); } /// Non-virtual version of retract GenericValue retract(const Vector& delta) const { - return GenericValue(traits_x::Retract(GenericValue::value(), delta)); + return GenericValue(traits::Retract(GenericValue::value(), delta)); } /// Non-virtual version of localCoordinates @@ -212,7 +147,7 @@ public: /// Return run-time dimensionality virtual size_t dim() const { - return traits_x::GetDimension(value_); + return traits::GetDimension(value_); } /// Assignment operator diff --git a/gtsam/base/Group.h b/gtsam/base/Group.h index de33f6225..aec4b5f1c 100644 --- a/gtsam/base/Group.h +++ b/gtsam/base/Group.h @@ -33,7 +33,7 @@ struct group_tag {}; struct multiplicative_group_tag {}; struct additive_group_tag {}; -template struct traits_x; +template struct traits; /** * Group Concept @@ -41,18 +41,18 @@ template struct traits_x; template class IsGroup { public: - typedef typename traits_x::structure_category structure_category_tag; - typedef typename traits_x::group_flavor flavor_tag; - //typedef typename traits_x::identity::value_type identity_value_type; + typedef typename traits::structure_category structure_category_tag; + typedef typename traits::group_flavor flavor_tag; + //typedef typename traits::identity::value_type identity_value_type; BOOST_CONCEPT_USAGE(IsGroup) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::value), "This type's structure_category trait does not assert it as a group (or derived)"); - e = traits_x::Identity(); - e = traits_x::Compose(g, h); - e = traits_x::Between(g, h); - e = traits_x::Inverse(g); + e = traits::Identity(); + e = traits::Compose(g, h); + e = traits::Between(g, h); + e = traits::Inverse(g); operator_usage(flavor); // todo: how do we test the act concept? or do we even need to? } @@ -77,10 +77,10 @@ private: template BOOST_CONCEPT_REQUIRES(((IsGroup)),(bool)) // check_group_invariants(const G& a, const G& b, double tol = 1e-9) { - G e = traits_x::Identity(); - return traits_x::Equals(traits_x::Compose(a, traits_x::Inverse(a)), e, tol) - && traits_x::Equals(traits_x::Between(a, b), traits_x::Compose(traits_x::Inverse(a), b), tol) - && traits_x::Equals(traits_x::Compose(a, traits_x::Between(a, b)), b, tol); + G e = traits::Identity(); + return traits::Equals(traits::Compose(a, traits::Inverse(a)), e, tol) + && traits::Equals(traits::Between(a, b), traits::Compose(traits::Inverse(a), b), tol) + && traits::Equals(traits::Compose(a, traits::Between(a, b)), b, tol); } /// Macro to add group traits, additive flavor diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index ec276b2de..a1206ebce 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -238,10 +238,10 @@ inline Class expmap_default(const Class& t, const Vector& d) { template class IsLieGroup: public IsGroup, public IsManifold { public: - typedef typename traits_x::structure_category structure_category_tag; - typedef typename traits_x::ManifoldType ManifoldType; - typedef typename traits_x::TangentVector TangentVector; - typedef typename traits_x::ChartJacobian ChartJacobian; + typedef typename traits::structure_category structure_category_tag; + typedef typename traits::ManifoldType ManifoldType; + typedef typename traits::TangentVector TangentVector; + typedef typename traits::ChartJacobian ChartJacobian; BOOST_CONCEPT_USAGE(IsLieGroup) { BOOST_STATIC_ASSERT_MSG( @@ -249,15 +249,15 @@ public: "This type's trait does not assert it is a Lie group (or derived)"); // group opertations with Jacobians - g = traits_x::Compose(g, h, Hg, Hh); - g = traits_x::Between(g, h, Hg, Hh); - g = traits_x::Inverse(g, Hg); + g = traits::Compose(g, h, Hg, Hh); + g = traits::Between(g, h, Hg, Hh); + g = traits::Inverse(g, Hg); // log and exp map without Jacobians - g = traits_x::Expmap(v); - v = traits_x::Logmap(g); + g = traits::Expmap(v); + v = traits::Logmap(g); // log and expnential map with Jacobians - g = traits_x::Expmap(v, Hg); - v = traits_x::Logmap(g, Hg); + g = traits::Expmap(v, Hg); + v = traits::Logmap(g, Hg); } private: T g, h; diff --git a/gtsam/base/LieMatrix.h b/gtsam/base/LieMatrix.h index 1a5cf32c2..7c167a46b 100644 --- a/gtsam/base/LieMatrix.h +++ b/gtsam/base/LieMatrix.h @@ -120,7 +120,7 @@ private: template<> -struct traits_x : public internal::VectorSpace { +struct traits : public internal::VectorSpace { // Override Retract, as the default version does not know how to initialize static LieMatrix Retract(const LieMatrix& origin, const TangentVector& v, diff --git a/gtsam/base/LieScalar.h b/gtsam/base/LieScalar.h index 631b88553..2e4be834f 100644 --- a/gtsam/base/LieScalar.h +++ b/gtsam/base/LieScalar.h @@ -67,6 +67,6 @@ namespace gtsam { }; template<> - struct traits_x : public internal::ScalarTraits {}; + struct traits : public internal::ScalarTraits {}; } // \namespace gtsam diff --git a/gtsam/base/LieVector.h b/gtsam/base/LieVector.h index 0e06efdb2..360c376ec 100644 --- a/gtsam/base/LieVector.h +++ b/gtsam/base/LieVector.h @@ -102,6 +102,6 @@ private: template<> -struct traits_x : public internal::VectorSpace {}; +struct traits : public internal::VectorSpace {}; } // \namespace gtsam diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index 358f21a37..b07bd3575 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -50,7 +50,7 @@ struct manifold_tag {}; * */ -template struct traits_x; +template struct traits; namespace internal { @@ -122,10 +122,10 @@ struct Manifold: Testable, ManifoldImpl { template BOOST_CONCEPT_REQUIRES(((IsTestable)),(bool)) // check_manifold_invariants(const T& a, const T& b, double tol=1e-9) { - typename traits_x::TangentVector v0 = traits_x::Local(a,a); - typename traits_x::TangentVector v = traits_x::Local(a,b); - T c = traits_x::Retract(a,v); - return v0.norm() < tol && traits_x::Equals(b,c,tol); + typename traits::TangentVector v0 = traits::Local(a,a); + typename traits::TangentVector v = traits::Local(a,b); + T c = traits::Retract(a,v); + return v0.norm() < tol && traits::Equals(b,c,tol); } /// Manifold concept @@ -134,10 +134,10 @@ class IsManifold { public: - typedef typename traits_x::structure_category structure_category_tag; - static const size_t dim = traits_x::dimension; - typedef typename traits_x::ManifoldType ManifoldType; - typedef typename traits_x::TangentVector TangentVector; + typedef typename traits::structure_category structure_category_tag; + static const size_t dim = traits::dimension; + typedef typename traits::ManifoldType ManifoldType; + typedef typename traits::TangentVector TangentVector; BOOST_CONCEPT_USAGE(IsManifold) { BOOST_STATIC_ASSERT_MSG( @@ -146,8 +146,8 @@ public: BOOST_STATIC_ASSERT(TangentVector::SizeAtCompileTime == dim); // make sure Chart methods are defined - v = traits_x::Local(p, q); - q = traits_x::Retract(p, v); + v = traits::Local(p, q); + q = traits::Retract(p, v); } private: @@ -160,7 +160,7 @@ private: template struct FixedDimension { typedef const int value_type; - static const int value = traits_x::dimension; + static const int value = traits::dimension; BOOST_STATIC_ASSERT_MSG(value != Eigen::Dynamic, "FixedDimension instantiated for dymanically-sized type."); }; diff --git a/gtsam/base/Testable.h b/gtsam/base/Testable.h index 9bf1a8aa2..4790530ab 100644 --- a/gtsam/base/Testable.h +++ b/gtsam/base/Testable.h @@ -43,7 +43,7 @@ namespace gtsam { // Forward declaration - template struct traits_x; + template struct traits; /** * A testable concept check that should be placed in applicable unit @@ -61,13 +61,13 @@ namespace gtsam { BOOST_CONCEPT_USAGE(IsTestable) { // check print function, with optional string - traits_x::Print(t, std::string()); - traits_x::Print(t); + traits::Print(t, std::string()); + traits::Print(t); // check print, with optional threshold double tol = 1.0; - r1 = traits_x::Equals(t,t,tol); - r2 = traits_x::Equals(t,t); + r1 = traits::Equals(t,t,tol); + r2 = traits::Equals(t,t); } }; // \ Testable @@ -81,13 +81,13 @@ namespace gtsam { /** Call equal on the object */ template inline bool equal(const T& obj1, const T& obj2, double tol) { - return traits_x::Equals(obj1,obj2, tol); + return traits::Equals(obj1,obj2, tol); } /** Call equal without tolerance (use default tolerance) */ template inline bool equal(const T& obj1, const T& obj2) { - return traits_x::Equals(obj1,obj2); + return traits::Equals(obj1,obj2); } /** @@ -95,11 +95,11 @@ namespace gtsam { */ template bool assert_equal(const V& expected, const V& actual, double tol = 1e-9) { - if (traits_x::Equals(actual,expected, tol)) + if (traits::Equals(actual,expected, tol)) return true; printf("Not equal:\n"); - traits_x::Print(expected,"expected:\n"); - traits_x::Print(actual,"actual:\n"); + traits::Print(expected,"expected:\n"); + traits::Print(actual,"actual:\n"); return false; } @@ -111,7 +111,7 @@ namespace gtsam { double tol_; equals(double tol = 1e-9) : tol_(tol) {} bool operator()(const V& expected, const V& actual) { - return (traits_x::Equals(actual, expected, tol_)); + return (traits::Equals(actual, expected, tol_)); } }; @@ -124,7 +124,7 @@ namespace gtsam { equals_star(double tol = 1e-9) : tol_(tol) {} bool operator()(const boost::shared_ptr& expected, const boost::shared_ptr& actual) { if (!actual || !expected) return false; - return (traits_x::Equals(*actual,*expected, tol_)); + return (traits::Equals(*actual,*expected, tol_)); } }; diff --git a/gtsam/base/VectorSpace.h b/gtsam/base/VectorSpace.h index 2e0b93fc0..094e6f162 100644 --- a/gtsam/base/VectorSpace.h +++ b/gtsam/base/VectorSpace.h @@ -16,7 +16,7 @@ namespace gtsam { struct vector_space_tag: public lie_group_tag { }; -template struct traits_x; +template struct traits; namespace internal { @@ -254,16 +254,16 @@ struct ScalarTraits : VectorSpaceImpl { } // namespace internal /// double -template<> struct traits_x : public internal::ScalarTraits { +template<> struct traits : public internal::ScalarTraits { }; /// float -template<> struct traits_x : public internal::ScalarTraits { +template<> struct traits : public internal::ScalarTraits { }; // traits for any fixed double Eigen matrix template -struct traits_x > : +struct traits > : internal::VectorSpaceImpl< Eigen::Matrix, M * N> { @@ -431,19 +431,19 @@ struct DynamicTraits { // traits for fully dynamic matrix template -struct traits_x > : +struct traits > : public internal::DynamicTraits<-1, -1, Options, MaxRows, MaxCols> { }; // traits for dynamic column vector template -struct traits_x > : +struct traits > : public internal::DynamicTraits<-1, 1, Options, MaxRows, MaxCols> { }; // traits for dynamic row vector template -struct traits_x > : +struct traits > : public internal::DynamicTraits<1, -1, Options, MaxRows, MaxCols> { }; @@ -452,7 +452,7 @@ template class IsVectorSpace: public IsLieGroup { public: - typedef typename traits_x::structure_category structure_category_tag; + typedef typename traits::structure_category structure_category_tag; BOOST_CONCEPT_USAGE(IsVectorSpace) { BOOST_STATIC_ASSERT_MSG( diff --git a/gtsam/base/concepts.h b/gtsam/base/concepts.h index 210eee3aa..da872d006 100644 --- a/gtsam/base/concepts.h +++ b/gtsam/base/concepts.h @@ -27,6 +27,6 @@ namespace gtsam { -template struct traits_x; +template struct traits; } diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index cda220ec0..e5cfbdd3b 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -62,7 +62,7 @@ namespace gtsam { namespace internal { template struct FixedSizeMatrix { - typedef Eigen::Matrix::dimension, traits_x::dimension> type; + typedef Eigen::Matrix::dimension, traits::dimension> type; }; } @@ -77,12 +77,12 @@ typename internal::FixedSizeMatrix::type numericalGradient(boost::function::structure_category>::value), + (boost::is_base_of::structure_category>::value), "Template argument X must be a manifold type."); - static const int N = traits_x::dimension; + static const int N = traits::dimension; BOOST_STATIC_ASSERT_MSG(N>0, "Template argument X must be fixed-size type."); - typedef typename traits_x::TangentVector TangentX; + typedef typename traits::TangentVector TangentX; // Prepare a tangent vector to perturb x with, only works for fixed size TangentX d; @@ -91,9 +91,9 @@ typename internal::FixedSizeMatrix::type numericalGradient(boost::function::Retract(x, d)); + double hxplus = h(traits::Retract(x, d)); d(j) = -delta; - double hxmin = h(traits_x::Retract(x, d)); + double hxmin = h(traits::Retract(x, d)); d(j) = 0; g(j) = (hxplus - hxmin) * factor; } @@ -115,20 +115,19 @@ template // TODO Should compute fixed-size matrix typename internal::FixedSizeMatrix::type numericalDerivative11(boost::function h, const X& x, double delta = 1e-5) { - using namespace traits; typedef typename internal::FixedSizeMatrix::type Matrix; - BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); - typedef traits_x TraitsY; + typedef traits TraitsY; typedef typename TraitsY::TangentVector TangentY; - BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X must be a manifold type."); - static const int N = traits_x::dimension; + static const int N = traits::dimension; BOOST_STATIC_ASSERT_MSG(N>0, "Template argument X must be fixed-size type."); - typedef traits_x TraitsX; + typedef traits TraitsX; typedef typename TraitsX::TangentVector TangentX; // get value at x, and corresponding chart @@ -174,9 +173,9 @@ typename internal::FixedSizeMatrix::type numericalDerivative11(Y (*h)(const template typename internal::FixedSizeMatrix::type numericalDerivative21(const boost::function& h, const X1& x1, const X2& x2, double delta = 1e-5) { - BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); - BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); return numericalDerivative11(boost::bind(h, _1, x2), x1, delta); } @@ -199,9 +198,9 @@ typename internal::FixedSizeMatrix::type numericalDerivative21(Y (*h)(cons template typename internal::FixedSizeMatrix::type numericalDerivative22(boost::function h, const X1& x1, const X2& x2, double delta = 1e-5) { -// BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), +// BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), // "Template argument X1 must be a manifold type."); - BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X2 must be a manifold type."); return numericalDerivative11(boost::bind(h, x1, _1), x2, delta); } @@ -227,9 +226,9 @@ template typename internal::FixedSizeMatrix::type numericalDerivative31( boost::function h, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { - BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); - BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); return numericalDerivative11(boost::bind(h, _1, x2, x3), x1, delta); } @@ -255,9 +254,9 @@ template typename internal::FixedSizeMatrix::type numericalDerivative32( boost::function h, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { - BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); - BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X2 must be a manifold type."); return numericalDerivative11(boost::bind(h, x1, _1, x3), x2, delta); } @@ -283,9 +282,9 @@ template typename internal::FixedSizeMatrix::type numericalDerivative33( boost::function h, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { - BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); - BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X3 must be a manifold type."); return numericalDerivative11(boost::bind(h, x1, x2, _1), x3, delta); } @@ -308,9 +307,9 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative33(Y (* template inline typename internal::FixedSizeMatrix::type numericalHessian(boost::function f, const X& x, double delta = 1e-5) { - BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X must be a manifold type."); - typedef Eigen::Matrix::dimension, 1> VectorD; + typedef Eigen::Matrix::dimension, 1> VectorD; typedef boost::function F; typedef boost::function G; G ng = static_cast(numericalGradient ); diff --git a/gtsam/base/testLie.h b/gtsam/base/testLie.h index 62eda0c68..4bbca85ca 100644 --- a/gtsam/base/testLie.h +++ b/gtsam/base/testLie.h @@ -33,7 +33,7 @@ void testLieGroupDerivatives(TestResult& result_, const std::string& name_, const G& t1, const G& t2) { Matrix H1, H2; - typedef traits_x T; + typedef traits T; // Inverse EXPECT(assert_equal(t1.inverse(),T::Inverse(t1, H1))); @@ -58,7 +58,7 @@ void testChartDerivatives(TestResult& result_, const std::string& name_, const G& t1, const G& t2) { Matrix H1, H2; - typedef traits_x T; + typedef traits T; // Retract typename G::TangentVector w12 = T::Local(t1, t2); diff --git a/gtsam/base/tests/testLieMatrix.cpp b/gtsam/base/tests/testLieMatrix.cpp index 1d386c0f3..7cc649e66 100644 --- a/gtsam/base/tests/testLieMatrix.cpp +++ b/gtsam/base/tests/testLieMatrix.cpp @@ -30,7 +30,7 @@ TEST( LieMatrix, construction ) { Matrix m = (Matrix(2,2) << 1.0,2.0, 3.0,4.0).finished(); LieMatrix lie1(m), lie2(m); - EXPECT(traits_x::GetDimension(m) == 4); + EXPECT(traits::GetDimension(m) == 4); EXPECT(assert_equal(m, lie1.matrix())); EXPECT(assert_equal(lie1, lie2)); } @@ -50,17 +50,17 @@ TEST(LieMatrix, retract) { Vector update = (Vector(4) << 3.0, 4.0, 6.0, 7.0).finished(); LieMatrix expected((Matrix(2,2) << 4.0, 6.0, 9.0, 11.0).finished()); - LieMatrix actual = traits_x::Retract(init,update); + LieMatrix actual = traits::Retract(init,update); EXPECT(assert_equal(expected, actual)); Vector expectedUpdate = update; - Vector actualUpdate = traits_x::Local(init,actual); + Vector actualUpdate = traits::Local(init,actual); EXPECT(assert_equal(expectedUpdate, actualUpdate)); Vector expectedLogmap = (Vector(4) << 1, 2, 3, 4).finished(); - Vector actualLogmap = traits_x::Logmap(LieMatrix((Matrix(2,2) << 1.0, 2.0, 3.0, 4.0).finished())); + Vector actualLogmap = traits::Logmap(LieMatrix((Matrix(2,2) << 1.0, 2.0, 3.0, 4.0).finished())); EXPECT(assert_equal(expectedLogmap, actualLogmap)); } diff --git a/gtsam/base/tests/testLieScalar.cpp b/gtsam/base/tests/testLieScalar.cpp index 9055298e5..060087c2a 100644 --- a/gtsam/base/tests/testLieScalar.cpp +++ b/gtsam/base/tests/testLieScalar.cpp @@ -48,7 +48,7 @@ TEST( testLieScalar, construction ) { EXPECT_DOUBLES_EQUAL(2., lie1.value(),tol); EXPECT_DOUBLES_EQUAL(2., lie2.value(),tol); - EXPECT(traits_x::dimension == 1); + EXPECT(traits::dimension == 1); EXPECT(assert_equal(lie1, lie2)); } @@ -56,7 +56,7 @@ TEST( testLieScalar, construction ) { TEST( testLieScalar, localCoordinates ) { LieScalar lie1(1.), lie2(3.); - Vector1 actual = traits_x::Local(lie1, lie2); + Vector1 actual = traits::Local(lie1, lie2); EXPECT( assert_equal((Vector)(Vector(1) << 2).finished(), actual)); } diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h index e1883e22b..f7253ec24 100644 --- a/gtsam/discrete/DecisionTreeFactor.h +++ b/gtsam/discrete/DecisionTreeFactor.h @@ -168,6 +168,6 @@ namespace gtsam { // DecisionTreeFactor // traits -template<> struct traits_x : public Testable {}; +template<> struct traits : public Testable {}; }// namespace gtsam diff --git a/gtsam/discrete/DiscreteBayesNet.h b/gtsam/discrete/DiscreteBayesNet.h index ca0de378b..de5ec8042 100644 --- a/gtsam/discrete/DiscreteBayesNet.h +++ b/gtsam/discrete/DiscreteBayesNet.h @@ -96,7 +96,7 @@ namespace gtsam { }; // traits -template<> struct traits_x : public Testable {}; +template<> struct traits : public Testable {}; } // \ namespace gtsam diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h index 80ce51fd7..88c3e5620 100644 --- a/gtsam/discrete/DiscreteConditional.h +++ b/gtsam/discrete/DiscreteConditional.h @@ -131,7 +131,7 @@ public: // DiscreteConditional // traits -template<> struct traits_x : public Testable {}; +template<> struct traits : public Testable {}; /* ************************************************************************* */ template diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h index 0bf01df05..e24dfdf2a 100644 --- a/gtsam/discrete/DiscreteFactor.h +++ b/gtsam/discrete/DiscreteFactor.h @@ -103,7 +103,7 @@ public: // DiscreteFactor // traits -template<> struct traits_x : public Testable {}; -template<> struct traits_x : public Testable {}; +template<> struct traits : public Testable {}; +template<> struct traits : public Testable {}; }// namespace gtsam diff --git a/gtsam/discrete/DiscreteFactorGraph.h b/gtsam/discrete/DiscreteFactorGraph.h index 0c1ae1ff2..c5b11adf1 100644 --- a/gtsam/discrete/DiscreteFactorGraph.h +++ b/gtsam/discrete/DiscreteFactorGraph.h @@ -148,6 +148,6 @@ public: }; // \ DiscreteFactorGraph /// traits -template<> struct traits_x : public Testable {}; +template<> struct traits : public Testable {}; } // \ namespace gtsam diff --git a/gtsam/discrete/Potentials.h b/gtsam/discrete/Potentials.h index a98067c72..978781d63 100644 --- a/gtsam/discrete/Potentials.h +++ b/gtsam/discrete/Potentials.h @@ -89,8 +89,8 @@ namespace gtsam { }; // Potentials // traits -template<> struct traits_x : public Testable {}; -template<> struct traits_x : public Testable {}; +template<> struct traits : public Testable {}; +template<> struct traits : public Testable {}; } // namespace gtsam diff --git a/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp b/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp index 0cfae1f2b..970a18b42 100644 --- a/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp +++ b/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp @@ -42,7 +42,7 @@ typedef AlgebraicDecisionTree ADT; // traits namespace gtsam { -template<> struct traits_x : public Testable {}; +template<> struct traits : public Testable {}; } #define DISABLE_DOT diff --git a/gtsam/discrete/tests/testDecisionTree.cpp b/gtsam/discrete/tests/testDecisionTree.cpp index 5c32278a8..05223c67a 100644 --- a/gtsam/discrete/tests/testDecisionTree.cpp +++ b/gtsam/discrete/tests/testDecisionTree.cpp @@ -46,7 +46,7 @@ typedef DecisionTree CrazyDecisionTree; // check that DecisionTree // traits namespace gtsam { -template<> struct traits_x : public Testable {}; +template<> struct traits : public Testable {}; } /* ******************************************************************************** */ @@ -57,7 +57,7 @@ typedef DecisionTree DT; // traits namespace gtsam { -template<> struct traits_x
: public Testable
{}; +template<> struct traits
: public Testable
{}; } struct Ring { diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index 8adbce06c..ce9f94c48 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -172,6 +172,6 @@ private: }; template<> -struct traits_x : public internal::Manifold {}; +struct traits : public internal::Manifold {}; } // namespace gtsam diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index 6ad9706de..0c77eebbc 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -110,7 +110,7 @@ private: }; template<> -struct traits_x : public internal::Manifold {}; +struct traits : public internal::Manifold {}; } diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h index aea50208a..d0e0f3891 100644 --- a/gtsam/geometry/Cal3Unified.h +++ b/gtsam/geometry/Cal3Unified.h @@ -140,7 +140,7 @@ private: }; template<> -struct traits_x : public internal::Manifold {}; +struct traits : public internal::Manifold {}; } diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index 7684bb088..0c5386822 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -224,6 +224,6 @@ private: }; template<> -struct traits_x : public internal::Manifold {}; +struct traits : public internal::Manifold {}; } // \ namespace gtsam diff --git a/gtsam/geometry/Cal3_S2Stereo.h b/gtsam/geometry/Cal3_S2Stereo.h index 6ca778dcc..3585fb156 100644 --- a/gtsam/geometry/Cal3_S2Stereo.h +++ b/gtsam/geometry/Cal3_S2Stereo.h @@ -155,7 +155,7 @@ namespace gtsam { // Define GTSAM traits template<> - struct traits_x : public internal::Manifold { + struct traits : public internal::Manifold { }; } // \ namespace gtsam diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index 6fbb31197..cda01b600 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -202,7 +202,7 @@ private: }; template<> -struct traits_x : public internal::Manifold {}; +struct traits : public internal::Manifold {}; } diff --git a/gtsam/geometry/Cyclic.h b/gtsam/geometry/Cyclic.h index c89390c04..2c883707f 100644 --- a/gtsam/geometry/Cyclic.h +++ b/gtsam/geometry/Cyclic.h @@ -65,7 +65,7 @@ public: /// Define cyclic group traits to be a model of the Group concept template -struct traits_x > { +struct traits > { typedef group_tag structure_category;GTSAM_ADDITIVE_GROUP(Cyclic) static Cyclic Identity() { return Cyclic::Identity(); diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index 375122191..c9e702078 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -199,7 +199,7 @@ private: }; template<> -struct traits_x : public internal::Manifold {}; +struct traits : public internal::Manifold {}; } // gtsam diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 33bf9dec3..b409c956d 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -497,6 +497,6 @@ private: template -struct traits_x< PinholeCamera > : public internal::Manifold > {}; +struct traits< PinholeCamera > : public internal::Manifold > {}; } // \ gtsam diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index 08129ab75..0a2735861 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -191,7 +191,7 @@ private: inline Point2 operator*(double s, const Point2& p) {return p*s;} template<> -struct traits_x : public internal::VectorSpace {}; +struct traits : public internal::VectorSpace {}; } // \ namespace gtsam diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 2a8c7852a..468a08c33 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -185,5 +185,5 @@ namespace gtsam { inline Point3 operator*(double s, const Point3& p) { return p*s;} template<> - struct traits_x : public internal::VectorSpace {}; + struct traits : public internal::VectorSpace {}; } diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index ba54d51c0..be860107e 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -291,7 +291,7 @@ typedef std::pair Point2Pair; GTSAM_EXPORT boost::optional align(const std::vector& pairs); template<> -struct traits_x : public internal::LieGroupTraits {}; +struct traits : public internal::LieGroupTraits {}; } // namespace gtsam diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 20babaae8..d930c815e 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -323,7 +323,7 @@ typedef std::pair Point3Pair; GTSAM_EXPORT boost::optional align(const std::vector& pairs); template<> -struct traits_x : public internal::LieGroupTraits { +struct traits : public internal::LieGroupTraits { }; } // namespace gtsam diff --git a/gtsam/geometry/Quaternion.h b/gtsam/geometry/Quaternion.h index 742aeeeb5..534b4ab42 100644 --- a/gtsam/geometry/Quaternion.h +++ b/gtsam/geometry/Quaternion.h @@ -24,7 +24,7 @@ namespace gtsam { // Define traits template -struct traits_x { +struct traits { typedef QUATERNION_TYPE ManifoldType; typedef QUATERNION_TYPE Q; diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index 3d647cb96..95f025622 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -208,6 +208,6 @@ namespace gtsam { }; template<> - struct traits_x : public internal::LieGroupTraits {}; + struct traits : public internal::LieGroupTraits {}; } // gtsam diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index b70c3f44f..e9568fb26 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -462,6 +462,6 @@ namespace gtsam { GTSAM_EXPORT std::pair RQ(const Matrix3& A); template<> - struct traits_x : public internal::LieGroupTraits {}; + struct traits : public internal::LieGroupTraits {}; } diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index 4a23df99f..cafdc6bf7 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -63,7 +63,7 @@ public: }; template<> -struct traits_x : public internal::LieGroupTraits {}; +struct traits : public internal::LieGroupTraits {}; } // end namespace gtsam diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index dd7adbb04..49abcf36b 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -147,6 +147,6 @@ private: }; template<> -struct traits_x : public internal::Manifold {}; +struct traits : public internal::Manifold {}; } diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h index 2f37fea3f..2ec745fd8 100644 --- a/gtsam/geometry/StereoPoint2.h +++ b/gtsam/geometry/StereoPoint2.h @@ -175,5 +175,5 @@ namespace gtsam { }; template<> - struct traits_x : public internal::Manifold {}; + struct traits : public internal::Manifold {}; } diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index fd151746b..79c3977cd 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -163,7 +163,7 @@ private: }; // Define GTSAM traits -template <> struct traits_x : public internal::Manifold {}; +template <> struct traits : public internal::Manifold {}; } // namespace gtsam diff --git a/gtsam/geometry/tests/testCyclic.cpp b/gtsam/geometry/tests/testCyclic.cpp index 78bf5b015..a15d7e2c2 100644 --- a/gtsam/geometry/tests/testCyclic.cpp +++ b/gtsam/geometry/tests/testCyclic.cpp @@ -27,7 +27,7 @@ typedef Cyclic<3> G; // Let's use the cyclic group of order 3 //****************************************************************************** TEST(Cyclic, Concept) { BOOST_CONCEPT_ASSERT((IsGroup)); - EXPECT_LONGS_EQUAL(0, traits_x::Identity()); + EXPECT_LONGS_EQUAL(0, traits::Identity()); } //****************************************************************************** @@ -37,31 +37,31 @@ TEST(Cyclic, Constructor) { //****************************************************************************** TEST(Cyclic, Compose) { - EXPECT_LONGS_EQUAL(0, traits_x::Compose(G(0),G(0))); - EXPECT_LONGS_EQUAL(1, traits_x::Compose(G(0),G(1))); - EXPECT_LONGS_EQUAL(2, traits_x::Compose(G(0),G(2))); + EXPECT_LONGS_EQUAL(0, traits::Compose(G(0),G(0))); + EXPECT_LONGS_EQUAL(1, traits::Compose(G(0),G(1))); + EXPECT_LONGS_EQUAL(2, traits::Compose(G(0),G(2))); - EXPECT_LONGS_EQUAL(2, traits_x::Compose(G(2),G(0))); - EXPECT_LONGS_EQUAL(0, traits_x::Compose(G(2),G(1))); - EXPECT_LONGS_EQUAL(1, traits_x::Compose(G(2),G(2))); + EXPECT_LONGS_EQUAL(2, traits::Compose(G(2),G(0))); + EXPECT_LONGS_EQUAL(0, traits::Compose(G(2),G(1))); + EXPECT_LONGS_EQUAL(1, traits::Compose(G(2),G(2))); } //****************************************************************************** TEST(Cyclic, Between) { - EXPECT_LONGS_EQUAL(0, traits_x::Between(G(0),G(0))); - EXPECT_LONGS_EQUAL(1, traits_x::Between(G(0),G(1))); - EXPECT_LONGS_EQUAL(2, traits_x::Between(G(0),G(2))); + EXPECT_LONGS_EQUAL(0, traits::Between(G(0),G(0))); + EXPECT_LONGS_EQUAL(1, traits::Between(G(0),G(1))); + EXPECT_LONGS_EQUAL(2, traits::Between(G(0),G(2))); - EXPECT_LONGS_EQUAL(1, traits_x::Between(G(2),G(0))); - EXPECT_LONGS_EQUAL(2, traits_x::Between(G(2),G(1))); - EXPECT_LONGS_EQUAL(0, traits_x::Between(G(2),G(2))); + EXPECT_LONGS_EQUAL(1, traits::Between(G(2),G(0))); + EXPECT_LONGS_EQUAL(2, traits::Between(G(2),G(1))); + EXPECT_LONGS_EQUAL(0, traits::Between(G(2),G(2))); } //****************************************************************************** TEST(Cyclic, Ivnverse) { - EXPECT_LONGS_EQUAL(0, traits_x::Inverse(G(0))); - EXPECT_LONGS_EQUAL(2, traits_x::Inverse(G(1))); - EXPECT_LONGS_EQUAL(1, traits_x::Inverse(G(2))); + EXPECT_LONGS_EQUAL(0, traits::Inverse(G(0))); + EXPECT_LONGS_EQUAL(2, traits::Inverse(G(1))); + EXPECT_LONGS_EQUAL(1, traits::Inverse(G(2))); } //****************************************************************************** diff --git a/gtsam/geometry/tests/testPoint2.cpp b/gtsam/geometry/tests/testPoint2.cpp index 71524d25d..fce7955e9 100644 --- a/gtsam/geometry/tests/testPoint2.cpp +++ b/gtsam/geometry/tests/testPoint2.cpp @@ -66,16 +66,16 @@ TEST(Point2, Lie) { Point2 p1(1, 2), p2(4, 5); Matrix H1, H2; - EXPECT(assert_equal(Point2(5,7), traits_x::Compose(p1, p2, H1, H2))); + EXPECT(assert_equal(Point2(5,7), traits::Compose(p1, p2, H1, H2))); EXPECT(assert_equal(eye(2), H1)); EXPECT(assert_equal(eye(2), H2)); - EXPECT(assert_equal(Point2(3,3), traits_x::Between(p1, p2, H1, H2))); + EXPECT(assert_equal(Point2(3,3), traits::Between(p1, p2, H1, H2))); EXPECT(assert_equal(-eye(2), H1)); EXPECT(assert_equal(eye(2), H2)); - EXPECT(assert_equal(Point2(5,7), traits_x::Retract(p1, Vector2(4., 5.)))); - EXPECT(assert_equal(Vector2(3.,3.), traits_x::Local(p1,p2))); + EXPECT(assert_equal(Point2(5,7), traits::Retract(p1, Vector2(4., 5.)))); + EXPECT(assert_equal(Vector2(3.,3.), traits::Local(p1,p2))); } /* ************************************************************************* */ @@ -83,7 +83,7 @@ TEST( Point2, expmap) { Vector d(2); d(0) = 1; d(1) = -1; - Point2 a(4, 5), b = traits_x::Retract(a,d), c(5, 4); + Point2 a(4, 5), b = traits::Retract(a,d), c(5, 4); EXPECT(assert_equal(b,c)); } diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index f81e2f7b2..5c6b32dca 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -46,16 +46,16 @@ TEST(Point3, Lie) { Point3 p2(4, 5, 6); Matrix H1, H2; - EXPECT(assert_equal(Point3(5, 7, 9), traits_x::Compose(p1, p2, H1, H2))); + EXPECT(assert_equal(Point3(5, 7, 9), traits::Compose(p1, p2, H1, H2))); EXPECT(assert_equal(eye(3), H1)); EXPECT(assert_equal(eye(3), H2)); - EXPECT(assert_equal(Point3(3, 3, 3), traits_x::Between(p1, p2, H1, H2))); + EXPECT(assert_equal(Point3(3, 3, 3), traits::Between(p1, p2, H1, H2))); EXPECT(assert_equal(-eye(3), H1)); EXPECT(assert_equal(eye(3), H2)); - EXPECT(assert_equal(Point3(5, 7, 9), traits_x::Retract(p1, Vector3(4,5,6)))); - EXPECT(assert_equal(Vector3(3, 3, 3), traits_x::Local(p1,p2))); + EXPECT(assert_equal(Point3(5, 7, 9), traits::Retract(p1, Vector3(4,5,6)))); + EXPECT(assert_equal(Vector3(3, 3, 3), traits::Local(p1,p2))); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testQuaternion.cpp b/gtsam/geometry/tests/testQuaternion.cpp index a3d18ba20..7302754d7 100644 --- a/gtsam/geometry/tests/testQuaternion.cpp +++ b/gtsam/geometry/tests/testQuaternion.cpp @@ -23,7 +23,7 @@ using namespace std; using namespace gtsam; typedef Quaternion Q; // Typedef -typedef traits_x::ChartJacobian QuaternionJacobian; +typedef traits::ChartJacobian QuaternionJacobian; //****************************************************************************** TEST(Quaternion , Concept) { @@ -52,7 +52,7 @@ TEST(Quaternion , Local) { Q q2(Eigen::AngleAxisd(0.1, z_axis)); QuaternionJacobian H1, H2; Vector3 expected(0, 0, 0.1); - Vector3 actual = traits_x::Local(q1, q2, H1, H2); + Vector3 actual = traits::Local(q1, q2, H1, H2); EXPECT(assert_equal((Vector)expected,actual)); } @@ -63,7 +63,7 @@ TEST(Quaternion , Retract) { Q expected(Eigen::AngleAxisd(0.1, z_axis)); Vector3 v(0, 0, 0.1); QuaternionJacobian Hq, Hv; - Q actual = traits_x::Retract(q, v, Hq, Hv); + Q actual = traits::Retract(q, v, Hq, Hv); EXPECT(actual.isApprox(expected)); } @@ -75,13 +75,13 @@ TEST(Quaternion , Compose) { Q expected = q1 * q2; Matrix actualH1, actualH2; - Q actual = traits_x::Compose(q1, q2, actualH1, actualH2); - EXPECT(traits_x::Equals(expected,actual)); + Q actual = traits::Compose(q1, q2, actualH1, actualH2); + EXPECT(traits::Equals(expected,actual)); - Matrix numericalH1 = numericalDerivative21(traits_x::Compose, q1, q2); + Matrix numericalH1 = numericalDerivative21(traits::Compose, q1, q2); EXPECT(assert_equal(numericalH1,actualH1)); - Matrix numericalH2 = numericalDerivative22(traits_x::Compose, q1, q2); + Matrix numericalH2 = numericalDerivative22(traits::Compose, q1, q2); EXPECT(assert_equal(numericalH2,actualH2)); } @@ -93,13 +93,13 @@ TEST(Quaternion , Between) { Q expected = q1.inverse() * q2; Matrix actualH1, actualH2; - Q actual = traits_x::Between(q1, q2, actualH1, actualH2); - EXPECT(traits_x::Equals(expected,actual)); + Q actual = traits::Between(q1, q2, actualH1, actualH2); + EXPECT(traits::Equals(expected,actual)); - Matrix numericalH1 = numericalDerivative21(traits_x::Between, q1, q2); + Matrix numericalH1 = numericalDerivative21(traits::Between, q1, q2); EXPECT(assert_equal(numericalH1,actualH1)); - Matrix numericalH2 = numericalDerivative22(traits_x::Between, q1, q2); + Matrix numericalH2 = numericalDerivative22(traits::Between, q1, q2); EXPECT(assert_equal(numericalH2,actualH2)); } @@ -110,10 +110,10 @@ TEST(Quaternion , Inverse) { Q expected(Eigen::AngleAxisd(-0.1, z_axis)); Matrix actualH; - Q actual = traits_x::Inverse(q1, actualH); - EXPECT(traits_x::Equals(expected,actual)); + Q actual = traits::Inverse(q1, actualH); + EXPECT(traits::Equals(expected,actual)); - Matrix numericalH = numericalDerivative11(traits_x::Inverse, q1); + Matrix numericalH = numericalDerivative11(traits::Inverse, q1); EXPECT(assert_equal(numericalH,actualH)); } diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index 0d493cb76..a36d30194 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -51,7 +51,7 @@ TEST(SO3 , Local) { SO3 q2(Eigen::AngleAxisd(0.1, z_axis)); SO3Jacobian H1,H2; Vector3 expected(0, 0, 0.1); - Vector3 actual = traits_x::Local(q1, q2, H1, H2); + Vector3 actual = traits::Local(q1, q2, H1, H2); EXPECT(assert_equal((Vector)expected,actual)); } @@ -61,7 +61,7 @@ TEST(SO3 , Retract) { SO3 q(Eigen::AngleAxisd(0, z_axis)); SO3 expected(Eigen::AngleAxisd(0.1, z_axis)); Vector3 v(0, 0, 0.1); - SO3 actual = traits_x::Retract(q, v); + SO3 actual = traits::Retract(q, v); EXPECT(actual.isApprox(expected)); } @@ -73,13 +73,13 @@ TEST(SO3 , Compose) { SO3 expected = q1 * q2; Matrix actualH1, actualH2; - SO3 actual = traits_x::Compose(q1, q2, actualH1, actualH2); - EXPECT(traits_x::Equals(expected,actual)); + SO3 actual = traits::Compose(q1, q2, actualH1, actualH2); + EXPECT(traits::Equals(expected,actual)); - Matrix numericalH1 = numericalDerivative21(traits_x::Compose, q1, q2); + Matrix numericalH1 = numericalDerivative21(traits::Compose, q1, q2); EXPECT(assert_equal(numericalH1,actualH1)); - Matrix numericalH2 = numericalDerivative22(traits_x::Compose, q1, q2); + Matrix numericalH2 = numericalDerivative22(traits::Compose, q1, q2); EXPECT(assert_equal(numericalH2,actualH2)); } @@ -91,13 +91,13 @@ TEST(SO3 , Between) { SO3 expected = q1.inverse() * q2; Matrix actualH1, actualH2; - SO3 actual = traits_x::Between(q1, q2, actualH1, actualH2); - EXPECT(traits_x::Equals(expected,actual)); + SO3 actual = traits::Between(q1, q2, actualH1, actualH2); + EXPECT(traits::Equals(expected,actual)); - Matrix numericalH1 = numericalDerivative21(traits_x::Between, q1, q2); + Matrix numericalH1 = numericalDerivative21(traits::Between, q1, q2); EXPECT(assert_equal(numericalH1,actualH1)); - Matrix numericalH2 = numericalDerivative22(traits_x::Between, q1, q2); + Matrix numericalH2 = numericalDerivative22(traits::Between, q1, q2); EXPECT(assert_equal(numericalH2,actualH2)); } @@ -108,10 +108,10 @@ TEST(SO3 , Inverse) { SO3 expected(Eigen::AngleAxisd(-0.1, z_axis)); Matrix actualH; - SO3 actual = traits_x::Inverse(q1, actualH); - EXPECT(traits_x::Equals(expected,actual)); + SO3 actual = traits::Inverse(q1, actualH); + EXPECT(traits::Equals(expected,actual)); - Matrix numericalH = numericalDerivative11(traits_x::Inverse, q1); + Matrix numericalH = numericalDerivative11(traits::Inverse, q1); EXPECT(assert_equal(numericalH,actualH)); } #endif diff --git a/gtsam/inference/LabeledSymbol.h b/gtsam/inference/LabeledSymbol.h index 11370fe76..23936afed 100644 --- a/gtsam/inference/LabeledSymbol.h +++ b/gtsam/inference/LabeledSymbol.h @@ -131,7 +131,7 @@ inline unsigned char mrsymbolLabel(Key key) { return LabeledSymbol(key).label(); inline size_t mrsymbolIndex(Key key) { return LabeledSymbol(key).index(); } /// traits -template<> struct traits_x : public Testable {}; +template<> struct traits : public Testable {}; } // \namespace gtsam diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h index bfeb4435f..e25590578 100644 --- a/gtsam/inference/Ordering.h +++ b/gtsam/inference/Ordering.h @@ -193,7 +193,7 @@ namespace gtsam { }; /// traits - template<> struct traits_x : public Testable {}; + template<> struct traits : public Testable {}; } diff --git a/gtsam/inference/Symbol.h b/gtsam/inference/Symbol.h index 63b0f3261..4f9734639 100644 --- a/gtsam/inference/Symbol.h +++ b/gtsam/inference/Symbol.h @@ -162,7 +162,7 @@ inline Key Z(size_t j) { return Symbol('z', j); } } /// traits -template<> struct traits_x : public Testable {}; +template<> struct traits : public Testable {}; } // \ namespace gtsam diff --git a/gtsam/inference/VariableIndex.h b/gtsam/inference/VariableIndex.h index f66d6a165..bcaec6ee4 100644 --- a/gtsam/inference/VariableIndex.h +++ b/gtsam/inference/VariableIndex.h @@ -179,7 +179,7 @@ protected: /// traits template<> -struct traits_x : public Testable { +struct traits : public Testable { }; } //\ namespace gtsam diff --git a/gtsam/inference/VariableSlots.h b/gtsam/inference/VariableSlots.h index 3a7685f60..fad789436 100644 --- a/gtsam/inference/VariableSlots.h +++ b/gtsam/inference/VariableSlots.h @@ -83,7 +83,7 @@ public: }; /// traits -template<> struct traits_x : public Testable {}; +template<> struct traits : public Testable {}; /* ************************************************************************* */ template diff --git a/gtsam/linear/Errors.h b/gtsam/linear/Errors.h index f01624c20..44d68be83 100644 --- a/gtsam/linear/Errors.h +++ b/gtsam/linear/Errors.h @@ -73,7 +73,7 @@ namespace gtsam { /// traits template<> - struct traits_x : public Testable { + struct traits : public Testable { }; } //\ namespace gtsam diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index d6f53012f..a89e7e21c 100644 --- a/gtsam/linear/GaussianBayesNet.h +++ b/gtsam/linear/GaussianBayesNet.h @@ -173,7 +173,7 @@ namespace gtsam { /// traits template<> - struct traits_x : public Testable { + struct traits : public Testable { }; } //\ namespace gtsam diff --git a/gtsam/linear/GaussianBayesTree.h b/gtsam/linear/GaussianBayesTree.h index c4b314f4c..1b2ad47e0 100644 --- a/gtsam/linear/GaussianBayesTree.h +++ b/gtsam/linear/GaussianBayesTree.h @@ -130,7 +130,7 @@ namespace gtsam { /// traits template<> - struct traits_x : public Testable { + struct traits : public Testable { }; } //\ namespace gtsam diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index 181aec1f5..e0a820819 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -143,7 +143,7 @@ namespace gtsam { /// traits template<> -struct traits_x : public Testable {}; +struct traits : public Testable {}; } // \ namespace gtsam diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index 19b28cccd..16b1f759a 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -149,7 +149,7 @@ namespace gtsam { /// traits template<> -struct traits_x : public Testable { +struct traits : public Testable { }; } // \ namespace gtsam diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 5c8840065..8653e3629 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -346,7 +346,7 @@ namespace gtsam { /// traits template<> -struct traits_x : public Testable { +struct traits : public Testable { }; } // \ namespace gtsam diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index bae14e621..ec4710dd7 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -445,7 +445,7 @@ namespace gtsam { /// traits template<> -struct traits_x : public Testable {}; +struct traits : public Testable {}; } // \ namespace gtsam diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 0ff851ee7..4b44197c6 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -362,7 +362,7 @@ namespace gtsam { /// traits template<> -struct traits_x : public Testable { +struct traits : public Testable { }; } // \ namespace gtsam diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 9870eae2e..7aa210dad 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -888,11 +888,11 @@ namespace gtsam { typedef noiseModel::Constrained::shared_ptr SharedConstrained; /// traits - template<> struct traits_x : public Testable {}; - template<> struct traits_x : public Testable {}; - template<> struct traits_x : public Testable {}; - template<> struct traits_x : public Testable {}; - template<> struct traits_x : public Testable {}; + template<> struct traits : public Testable {}; + template<> struct traits : public Testable {}; + template<> struct traits : public Testable {}; + template<> struct traits : public Testable {}; + template<> struct traits : public Testable {}; } //\ namespace gtsam diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index b75001490..ce33116ab 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -383,7 +383,7 @@ namespace gtsam { /// traits template<> - struct traits_x : public Testable { + struct traits : public Testable { }; } // \namespace gtsam diff --git a/gtsam/navigation/AttitudeFactor.h b/gtsam/navigation/AttitudeFactor.h index 805dc8d19..08b75c00d 100644 --- a/gtsam/navigation/AttitudeFactor.h +++ b/gtsam/navigation/AttitudeFactor.h @@ -132,7 +132,7 @@ private: }; /// traits -template<> struct traits_x : public Testable {}; +template<> struct traits : public Testable {}; /** * Version of AttitudeFactor for Pose3 @@ -216,7 +216,7 @@ private: }; /// traits -template<> struct traits_x : public Testable {}; +template<> struct traits : public Testable {}; } /// namespace gtsam diff --git a/gtsam/navigation/ImuBias.h b/gtsam/navigation/ImuBias.h index d61bcfe9f..da01af613 100644 --- a/gtsam/navigation/ImuBias.h +++ b/gtsam/navigation/ImuBias.h @@ -180,7 +180,7 @@ namespace imuBias { } // namespace imuBias template<> -struct traits_x : public internal::VectorSpace< +struct traits : public internal::VectorSpace< imuBias::ConstantBias> { }; diff --git a/gtsam/nonlinear/AdaptAutoDiff.h b/gtsam/nonlinear/AdaptAutoDiff.h index 2aa151a14..7295f3160 100644 --- a/gtsam/nonlinear/AdaptAutoDiff.h +++ b/gtsam/nonlinear/AdaptAutoDiff.h @@ -31,7 +31,7 @@ namespace detail { // By default, we assume an Identity element template -struct Origin { T operator()() { return traits_x::Identity();} }; +struct Origin { T operator()() { return traits::Identity();} }; // but dimple manifolds don't have one, so we just use the default constructor template @@ -50,7 +50,7 @@ struct Canonical { GTSAM_CONCEPT_MANIFOLD_TYPE(T) - typedef traits_x Traits; + typedef traits Traits; enum { dimension = Traits::dimension }; typedef typename Traits::TangentVector TangentVector; typedef typename Traits::structure_category Category; @@ -69,9 +69,9 @@ struct Canonical { template class AdaptAutoDiff { - static const int N = traits_x::dimension; - static const int M1 = traits_x::dimension; - static const int M2 = traits_x::dimension; + static const int N = traits::dimension; + static const int M1 = traits::dimension; + static const int M2 = traits::dimension; typedef Eigen::Matrix RowMajor1; typedef Eigen::Matrix RowMajor2; diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index a4008ad98..190457673 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -141,7 +141,7 @@ void handleLeafCase(const Eigen::MatrixBase& dTdA, */ template class ExecutionTrace { - static const int Dim = traits_x::dimension; + static const int Dim = traits::dimension; enum { Constant, Leaf, Function } kind; @@ -326,7 +326,7 @@ public: /// Return dimensions for each argument virtual void dims(std::map& map) const { - map[key_] = traits_x::dimension; + map[key_] = traits::dimension; } /// Return value @@ -402,15 +402,15 @@ public: /// meta-function to generate fixed-size JacobianTA type template struct Jacobian { - typedef Eigen::Matrix::dimension, - traits_x::dimension> type; + typedef Eigen::Matrix::dimension, + traits::dimension> type; }; /// meta-function to generate JacobianTA optional reference template struct MakeOptionalJacobian { - typedef OptionalJacobian::dimension, - traits_x::dimension> type; + typedef OptionalJacobian::dimension, + traits::dimension> type; }; /** @@ -576,7 +576,7 @@ struct FunctionalNode { /// Provide convenience access to Record storage and implement /// the virtual function based interface of CallRecord using the CallRecordImplementor struct Record: public internal::CallRecordImplementor::dimension>, public Base::Record { + traits::dimension>, public Base::Record { using Base::Record::print; using Base::Record::startReverseAD4; using Base::Record::reverseAD4; diff --git a/gtsam/nonlinear/Expression.h b/gtsam/nonlinear/Expression.h index 526ef1478..2e0b6c01e 100644 --- a/gtsam/nonlinear/Expression.h +++ b/gtsam/nonlinear/Expression.h @@ -173,7 +173,7 @@ private: assert(H.size()==keys.size()); // Pre-allocate and zero VerticalBlockMatrix - static const int Dim = traits_x::dimension; + static const int Dim = traits::dimension; VerticalBlockMatrix Ab(dims, Dim); Ab.matrix().setZero(); JacobianMap jacobianMap(keys, Ab); @@ -237,7 +237,7 @@ private: template struct apply_compose { typedef T result_type; - static const int Dim = traits_x::dimension; + static const int Dim = traits::dimension; T operator()(const T& x, const T& y, OptionalJacobian H1 = boost::none, OptionalJacobian H2 = boost::none) const { return x.compose(y, H1, H2); diff --git a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h index 5e5cd3752..9251aac07 100644 --- a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h +++ b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h @@ -43,7 +43,7 @@ namespace gtsam { // Extract the current estimate of x1,P1 VectorValues result = marginal->solve(VectorValues()); const T& current = linearizationPoints.at(lastKey); - T x = traits_x::Retract(current, result[lastKey]); + T x = traits::Retract(current, result[lastKey]); // Create a Jacobian Factor from the root node of the produced Bayes Net. // This will act as a prior for the next iteration. @@ -71,7 +71,7 @@ namespace gtsam { // Create a Jacobian Prior Factor directly P_initial. // Since x0 is set to the provided mean, the b vector in the prior will be zero // TODO Frank asks: is there a reason why noiseModel is not simply P_initial ? - int n = traits_x::GetDimension(x_initial); + int n = traits::GetDimension(x_initial); priorFactor_ = JacobianFactor::shared_ptr( new JacobianFactor(key_initial, P_initial->R(), Vector::Zero(n), noiseModel::Unit::Create(n))); diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h index 94cf98e10..f7c6d0345 100644 --- a/gtsam/nonlinear/ISAM2-inl.h +++ b/gtsam/nonlinear/ISAM2-inl.h @@ -29,7 +29,7 @@ namespace gtsam { template VALUE ISAM2::calculateEstimate(Key key) const { const Vector& delta = getDelta()[key]; - return traits_x::Retract(theta_.at(key), delta); + return traits::Retract(theta_.at(key), delta); } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index ad0371cc9..9adceee6a 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -634,7 +634,7 @@ protected: }; // ISAM2 /// traits -template<> struct traits_x : public Testable {}; +template<> struct traits : public Testable {}; /// Optimize the BayesTree, starting from the root. /// @param replaced Needs to contain diff --git a/gtsam/nonlinear/LinearContainerFactor.h b/gtsam/nonlinear/LinearContainerFactor.h index 004ab0db1..676b3fb85 100644 --- a/gtsam/nonlinear/LinearContainerFactor.h +++ b/gtsam/nonlinear/LinearContainerFactor.h @@ -155,7 +155,7 @@ private: }; // \class LinearContainerFactor -template<> struct traits_x : public Testable {}; +template<> struct traits : public Testable {}; } // \namespace gtsam diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index 2c3eadcfc..6e348fb58 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -93,7 +93,7 @@ public: */ NonlinearEquality(Key j, const T& feasible, bool (*_compare)(const T&, const T&) = compare) : - Base(noiseModel::Constrained::All(traits_x::GetDimension(feasible)), + Base(noiseModel::Constrained::All(traits::GetDimension(feasible)), j), feasible_(feasible), allow_error_(false), error_gain_(0.0), // compare_(_compare) { } @@ -103,7 +103,7 @@ public: */ NonlinearEquality(Key j, const T& feasible, double error_gain, bool (*_compare)(const T&, const T&) = compare) : - Base(noiseModel::Constrained::All(traits_x::GetDimension(feasible)), + Base(noiseModel::Constrained::All(traits::GetDimension(feasible)), j), feasible_(feasible), allow_error_(true), error_gain_(error_gain), // compare_(_compare) { } @@ -115,8 +115,8 @@ public: virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { std::cout << s << "Constraint: on [" << keyFormatter(this->key()) << "]\n"; - traits_x::Print(feasible_, "Feasible Point:\n"); - std::cout << "Variable Dimension: " << traits_x::GetDimension(feasible_) << std::endl; + traits::Print(feasible_, "Feasible Point:\n"); + std::cout << "Variable Dimension: " << traits::GetDimension(feasible_) << std::endl; } /** Check if two factors are equal */ @@ -144,11 +144,11 @@ public: /** error function */ Vector evaluateError(const T& xj, boost::optional H = boost::none) const { - const size_t nj = traits_x::GetDimension(feasible_); + const size_t nj = traits::GetDimension(feasible_); if (allow_error_) { if (H) *H = eye(nj); // FIXME: this is not the right linearization for nonlinear compare - return traits_x::Local(xj,feasible_); + return traits::Local(xj,feasible_); } else if (compare_(feasible_, xj)) { if (H) *H = eye(nj); @@ -198,7 +198,7 @@ private: // \class NonlinearEquality template -struct traits_x > : Testable > { +struct traits > : Testable > { }; /* ************************************************************************* */ @@ -237,7 +237,7 @@ public: * */ NonlinearEquality1(const X& value, Key key, double mu = 1000.0) : - Base( noiseModel::Constrained::All(traits_x::GetDimension(value), + Base( noiseModel::Constrained::All(traits::GetDimension(value), std::abs(mu)), key), value_(value) { } @@ -254,9 +254,9 @@ public: Vector evaluateError(const X& x1, boost::optional H = boost::none) const { if (H) - (*H) = eye(traits_x::GetDimension(x1)); + (*H) = eye(traits::GetDimension(x1)); // manifold equivalent of h(x)-z -> log(z,h(x)) - return traits_x::Local(value_,x1); + return traits::Local(value_,x1); } /** Print */ @@ -283,7 +283,7 @@ private: // \NonlinearEquality1 template -struct traits_x > : Testable > { +struct traits > : Testable > { }; /* ************************************************************************* */ @@ -312,7 +312,7 @@ public: ///TODO: comment NonlinearEquality2(Key key1, Key key2, double mu = 1000.0) : - Base(noiseModel::Constrained::All(traits_x::dimension, std::abs(mu)), key1, key2) { + Base(noiseModel::Constrained::All(traits::dimension, std::abs(mu)), key1, key2) { } virtual ~NonlinearEquality2() { } @@ -326,10 +326,10 @@ public: /** g(x) with optional derivative2 */ Vector evaluateError(const X& x1, const X& x2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { - static const size_t p = traits_x::dimension; + static const size_t p = traits::dimension; if (H1) *H1 = -eye(p); if (H2) *H2 = eye(p); - return traits_x::Local(x1,x2); + return traits::Local(x1,x2); } private: @@ -346,7 +346,7 @@ private: // \NonlinearEquality2 template -struct traits_x > : Testable > { +struct traits > : Testable > { }; diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index f04e05284..5d9f176b3 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -145,7 +145,7 @@ public: }; // \class NonlinearFactor /// traits -template<> struct traits_x : public Testable { +template<> struct traits : public Testable { }; /* ************************************************************************* */ diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index 18b8f38d3..28f72d57d 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -163,7 +163,7 @@ namespace gtsam { /// traits template<> -struct traits_x : public Testable { +struct traits : public Testable { }; } //\ namespace gtsam diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index d3ba93d40..05f39ed4e 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -525,7 +525,7 @@ namespace gtsam { /// traits template<> - struct traits_x : public Testable { + struct traits : public Testable { }; } //\ namespace gtsam diff --git a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp index 0286e9845..f3858c818 100644 --- a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp +++ b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp @@ -49,7 +49,7 @@ struct Cal: public Cal3Bundler { }; template<> -struct traits_x : public internal::Manifold {}; +struct traits : public internal::Manifold {}; // With that, camera below behaves like Snavely's 9-dim vector typedef PinholeCamera Camera; diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index b798c6321..faa285cd5 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -72,7 +72,7 @@ public: }; namespace gtsam { -template <> struct traits_x : public internal::Manifold {}; +template <> struct traits : public internal::Manifold {}; } diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index 09e1d310a..319c74cd5 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -74,14 +74,14 @@ namespace gtsam { std::cout << s << "BetweenFactor(" << keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) << ")\n"; - traits_x::Print(measured_, " measured: "); + traits::Print(measured_, " measured: "); this->noiseModel_->print(" noise model: "); } /** equals */ virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) && traits_x::Equals(this->measured_, e->measured_, tol); + return e != NULL && Base::equals(*e, tol) && traits::Equals(this->measured_, e->measured_, tol); } /** implement functions needed to derive from Factor */ @@ -89,16 +89,16 @@ namespace gtsam { /** vector of errors */ Vector evaluateError(const T& p1, const T& p2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { - T hx = traits_x::Between(p1, p2, H1, H2); // h(x) + T hx = traits::Between(p1, p2, H1, H2); // h(x) // manifold equivalent of h(x)-z -> log(z,h(x)) #ifdef SLOW_BUT_CORRECT_BETWEENFACTOR - typename traits_x::ChartJacobian::Fixed Hlocal; - Vector rval = traits_x::Local(measured_, hx, boost::none, (H1 || H2) ? &Hlocal : 0); + typename traits::ChartJacobian::Fixed Hlocal; + Vector rval = traits::Local(measured_, hx, boost::none, (H1 || H2) ? &Hlocal : 0); if (H1) *H1 = Hlocal * (*H1); if (H1) *H2 = Hlocal * (*H2); return rval; #else - return traits_x::Local(measured_, hx); + return traits::Local(measured_, hx); #endif } @@ -126,7 +126,7 @@ namespace gtsam { /// traits template - struct traits_x > : public Testable > {}; + struct traits > : public Testable > {}; /** * Binary between constraint - forces between to a given value @@ -141,7 +141,7 @@ namespace gtsam { /** Syntactic sugar for constrained version */ BetweenConstraint(const VALUE& measured, Key key1, Key key2, double mu = 1000.0) : BetweenFactor(key1, key2, measured, - noiseModel::Constrained::All(traits_x::GetDimension(measured), fabs(mu))) + noiseModel::Constrained::All(traits::GetDimension(measured), fabs(mu))) {} private: @@ -157,6 +157,6 @@ namespace gtsam { /// traits template - struct traits_x > : public Testable > {}; + struct traits > : public Testable > {}; } /// namespace gtsam diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index 3b8d65275..2f8dd601f 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -128,7 +128,7 @@ namespace gtsam { }; template - struct traits_x > : Testable< + struct traits > : Testable< GeneralSFMFactor > { }; @@ -230,7 +230,7 @@ namespace gtsam { }; template - struct traits_x > : Testable< + struct traits > : Testable< GeneralSFMFactor2 > { }; diff --git a/gtsam/slam/PoseTranslationPrior.h b/gtsam/slam/PoseTranslationPrior.h index 6e8952887..6689653aa 100644 --- a/gtsam/slam/PoseTranslationPrior.h +++ b/gtsam/slam/PoseTranslationPrior.h @@ -62,8 +62,8 @@ public: Vector evaluateError(const Pose& pose, boost::optional H = boost::none) const { const Translation& newTrans = pose.translation(); const Rotation& R = pose.rotation(); - const int tDim = traits_x::GetDimension(newTrans); - const int xDim = traits_x::GetDimension(pose); + const int tDim = traits::GetDimension(newTrans); + const int xDim = traits::GetDimension(pose); if (H) { *H = gtsam::zeros(tDim, xDim); std::pair transInterval = POSE::translationInterval(); diff --git a/gtsam/slam/PriorFactor.h b/gtsam/slam/PriorFactor.h index 9b7aa5513..eee14f9f2 100644 --- a/gtsam/slam/PriorFactor.h +++ b/gtsam/slam/PriorFactor.h @@ -67,24 +67,24 @@ namespace gtsam { /** print */ virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { std::cout << s << "PriorFactor on " << keyFormatter(this->key()) << "\n"; - traits_x::Print(prior_, " prior mean: "); + traits::Print(prior_, " prior mean: "); this->noiseModel_->print(" noise model: "); } /** equals */ virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This* e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) && traits_x::Equals(prior_, e->prior_, tol); + return e != NULL && Base::equals(*e, tol) && traits::Equals(prior_, e->prior_, tol); } /** implement functions needed to derive from Factor */ /** vector of errors */ Vector evaluateError(const T& p, boost::optional H = boost::none) const { - if (H) (*H) = eye(traits_x::GetDimension(p)); + if (H) (*H) = eye(traits::GetDimension(p)); // manifold equivalent of h(x)-z -> log(z,h(x)) // TODO(ASL) Add Jacobians. - return traits_x::Local(prior_,p); + return traits::Local(prior_,p); } const VALUE & prior() const { return prior_; } diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index a04037328..52e28beaf 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -190,7 +190,7 @@ namespace gtsam { /// traits template - struct traits_x > : + struct traits > : public Testable > { }; diff --git a/gtsam/slam/RangeFactor.h b/gtsam/slam/RangeFactor.h index a050ff123..216b9320e 100644 --- a/gtsam/slam/RangeFactor.h +++ b/gtsam/slam/RangeFactor.h @@ -100,7 +100,7 @@ private: /// traits template -struct traits_x > : public Testable > {}; +struct traits > : public Testable > {}; /** * Binary factor for a range measurement, with a transform applied @@ -192,6 +192,6 @@ private: /// traits template -struct traits_x > : public Testable > {}; +struct traits > : public Testable > {}; } // \ namespace gtsam diff --git a/gtsam/slam/ReferenceFrameFactor.h b/gtsam/slam/ReferenceFrameFactor.h index 9a195c9ac..6a70d58b4 100644 --- a/gtsam/slam/ReferenceFrameFactor.h +++ b/gtsam/slam/ReferenceFrameFactor.h @@ -101,7 +101,7 @@ public: Point newlocal = transform_point(trans, global, Dtrans, Dforeign); if (Dlocal) *Dlocal = -1* gtsam::eye(Point::dimension); - return traits_x::Local(local,newlocal); + return traits::Local(local,newlocal); } virtual void print(const std::string& s="", @@ -130,6 +130,6 @@ private: /// traits template -struct traits_x > : public Testable > {}; +struct traits > : public Testable > {}; } // \namespace gtsam diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 77eb4d288..44d6902fa 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -48,7 +48,7 @@ protected: boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame (one for all cameras) - static const int ZDim = traits_x::dimension; ///< Measurement dimension + static const int ZDim = traits::dimension; ///< Measurement dimension /// Definitions for blocks of F typedef Eigen::Matrix Matrix2D; // F diff --git a/gtsam/slam/SmartProjectionPoseFactor.h b/gtsam/slam/SmartProjectionPoseFactor.h index 715f34c36..0801d141f 100644 --- a/gtsam/slam/SmartProjectionPoseFactor.h +++ b/gtsam/slam/SmartProjectionPoseFactor.h @@ -217,6 +217,6 @@ private: /// traits template -struct traits_x > : public Testable > {}; +struct traits > : public Testable > {}; } // \ namespace gtsam diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h index 7fb4e1f6c..7b21e044f 100644 --- a/gtsam/slam/StereoFactor.h +++ b/gtsam/slam/StereoFactor.h @@ -182,6 +182,6 @@ private: /// traits template -struct traits_x > : public Testable > {}; +struct traits > : public Testable > {}; } // \ namespace gtsam diff --git a/gtsam/symbolic/SymbolicBayesNet.h b/gtsam/symbolic/SymbolicBayesNet.h index a48ca2522..5dda02350 100644 --- a/gtsam/symbolic/SymbolicBayesNet.h +++ b/gtsam/symbolic/SymbolicBayesNet.h @@ -83,7 +83,7 @@ namespace gtsam { /// traits template<> - struct traits_x : public Testable { + struct traits : public Testable { }; } //\ namespace gtsam diff --git a/gtsam/symbolic/SymbolicBayesTree.h b/gtsam/symbolic/SymbolicBayesTree.h index ff9109218..cac711043 100644 --- a/gtsam/symbolic/SymbolicBayesTree.h +++ b/gtsam/symbolic/SymbolicBayesTree.h @@ -71,8 +71,8 @@ namespace gtsam { }; /// traits -template<> struct traits_x : public Testable {}; -template<> struct traits_x : public Testable {}; +template<> struct traits : public Testable {}; +template<> struct traits : public Testable {}; } //\ namespace gtsam diff --git a/gtsam/symbolic/SymbolicConditional.h b/gtsam/symbolic/SymbolicConditional.h index d779a4d57..4b89a4b60 100644 --- a/gtsam/symbolic/SymbolicConditional.h +++ b/gtsam/symbolic/SymbolicConditional.h @@ -124,7 +124,7 @@ namespace gtsam { /// traits template<> -struct traits_x : public Testable { +struct traits : public Testable { }; } //\ namespace gtsam diff --git a/gtsam/symbolic/SymbolicEliminationTree.h b/gtsam/symbolic/SymbolicEliminationTree.h index 0fa01eb29..e20fb67bd 100644 --- a/gtsam/symbolic/SymbolicEliminationTree.h +++ b/gtsam/symbolic/SymbolicEliminationTree.h @@ -61,7 +61,7 @@ namespace gtsam { /// traits template<> -struct traits_x : public Testable { +struct traits : public Testable { }; } //\ namespace gtsam diff --git a/gtsam/symbolic/SymbolicFactor.h b/gtsam/symbolic/SymbolicFactor.h index 8da80c4d0..f30f88935 100644 --- a/gtsam/symbolic/SymbolicFactor.h +++ b/gtsam/symbolic/SymbolicFactor.h @@ -162,7 +162,7 @@ namespace gtsam { /// traits template<> - struct traits_x : public Testable { + struct traits : public Testable { }; } //\ namespace gtsam diff --git a/gtsam/symbolic/SymbolicFactorGraph.h b/gtsam/symbolic/SymbolicFactorGraph.h index ecb7c04b0..9813df331 100644 --- a/gtsam/symbolic/SymbolicFactorGraph.h +++ b/gtsam/symbolic/SymbolicFactorGraph.h @@ -118,7 +118,7 @@ namespace gtsam { /// traits template<> -struct traits_x : public Testable { +struct traits : public Testable { }; } //\ namespace gtsam diff --git a/gtsam_unstable/dynamics/PoseRTV.h b/gtsam_unstable/dynamics/PoseRTV.h index baea15e6e..ea7a2c9ff 100644 --- a/gtsam_unstable/dynamics/PoseRTV.h +++ b/gtsam_unstable/dynamics/PoseRTV.h @@ -190,6 +190,6 @@ private: template<> -struct traits_x : public internal::LieGroupTraits {}; +struct traits : public internal::LieGroupTraits {}; } // \namespace gtsam diff --git a/gtsam_unstable/geometry/BearingS2.h b/gtsam_unstable/geometry/BearingS2.h index fa45d48c4..3c9247048 100644 --- a/gtsam_unstable/geometry/BearingS2.h +++ b/gtsam_unstable/geometry/BearingS2.h @@ -100,6 +100,6 @@ private: }; /// traits -template<> struct traits_x : public Testable {}; +template<> struct traits : public Testable {}; } // \namespace gtsam diff --git a/gtsam_unstable/geometry/Pose3Upright.h b/gtsam_unstable/geometry/Pose3Upright.h index e50426c90..c48508fa0 100644 --- a/gtsam_unstable/geometry/Pose3Upright.h +++ b/gtsam_unstable/geometry/Pose3Upright.h @@ -141,7 +141,7 @@ private: }; // \class Pose3Upright template<> -struct traits_x : public internal::Manifold {}; +struct traits : public internal::Manifold {}; } // \namespace gtsam diff --git a/gtsam_unstable/geometry/SimWall2D.h b/gtsam_unstable/geometry/SimWall2D.h index 78544e62d..38bba2ee3 100644 --- a/gtsam_unstable/geometry/SimWall2D.h +++ b/gtsam_unstable/geometry/SimWall2D.h @@ -69,7 +69,7 @@ namespace gtsam { typedef std::vector SimWall2DVector; /// traits - template<> struct traits_x : public Testable {}; + template<> struct traits : public Testable {}; /** * Calculates the next pose in a trajectory constrained by walls, with noise on diff --git a/gtsam_unstable/linear/LinearEquality.h b/gtsam_unstable/linear/LinearEquality.h index c5efa1055..bc1b2bc12 100644 --- a/gtsam_unstable/linear/LinearEquality.h +++ b/gtsam_unstable/linear/LinearEquality.h @@ -117,7 +117,7 @@ public: /// traits -template<> struct traits_x : public Testable {}; +template<> struct traits : public Testable {}; } // \ namespace gtsam diff --git a/gtsam_unstable/linear/LinearEqualityFactorGraph.h b/gtsam_unstable/linear/LinearEqualityFactorGraph.h index 9ddab9102..9c067ae3d 100644 --- a/gtsam_unstable/linear/LinearEqualityFactorGraph.h +++ b/gtsam_unstable/linear/LinearEqualityFactorGraph.h @@ -29,7 +29,7 @@ public: }; /// traits -template<> struct traits_x : public Testable< +template<> struct traits : public Testable< LinearEqualityFactorGraph> { }; diff --git a/gtsam_unstable/linear/LinearInequality.h b/gtsam_unstable/linear/LinearInequality.h index 5900e2980..7c62c3d54 100644 --- a/gtsam_unstable/linear/LinearInequality.h +++ b/gtsam_unstable/linear/LinearInequality.h @@ -139,7 +139,7 @@ public: }; // \ LinearInequality /// traits -template<> struct traits_x : public Testable {}; +template<> struct traits : public Testable {}; } // \ namespace gtsam diff --git a/gtsam_unstable/linear/LinearInequalityFactorGraph.h b/gtsam_unstable/linear/LinearInequalityFactorGraph.h index a32c10b5f..eca271941 100644 --- a/gtsam_unstable/linear/LinearInequalityFactorGraph.h +++ b/gtsam_unstable/linear/LinearInequalityFactorGraph.h @@ -44,7 +44,7 @@ public: }; /// traits -template<> struct traits_x : public Testable< +template<> struct traits : public Testable< LinearInequalityFactorGraph> { }; diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h index dc699b150..5c66d411f 100644 --- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h @@ -67,7 +67,7 @@ public: template VALUE calculateEstimate(Key key) const { const Vector delta = delta_.at(key); - return traits_x::Retract(theta_.at(key), delta); + return traits::Retract(theta_.at(key), delta); } /** read the current set of optimizer parameters */ diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h index f1a0b8ef2..9a458ee5a 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h @@ -255,7 +255,7 @@ typedef ConcurrentBatchFilter::Result ConcurrentBatchFilterResult; /// traits template<> -struct traits_x : public Testable { +struct traits : public Testable { }; } // \ namespace gtsam diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h index 62618f362..f3808686c 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h +++ b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h @@ -206,7 +206,7 @@ typedef ConcurrentBatchSmoother::Result ConcurrentBatchSmootherResult; /// traits template<> -struct traits_x : public Testable { +struct traits : public Testable { }; } //\ namespace gtsam diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h index 2dcb23204..11012674e 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h @@ -200,7 +200,7 @@ typedef ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilterResult; /// traits template<> -struct traits_x : public Testable { +struct traits : public Testable { }; } //\ namespace gtsam diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h index edfc8f09a..78bab5079 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h @@ -170,7 +170,7 @@ typedef ConcurrentIncrementalSmoother::Result ConcurrentIncrementalSmootherResul /// traits template<> -struct traits_x : public Testable { +struct traits : public Testable { }; } // \ namespace gtsam diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h index a74a212c3..e75add618 100644 --- a/gtsam_unstable/nonlinear/ExpressionFactor.h +++ b/gtsam_unstable/nonlinear/ExpressionFactor.h @@ -38,7 +38,7 @@ protected: Expression expression_; ///< the expression that is AD enabled FastVector dims_; ///< dimensions of the Jacobian matrices - static const int Dim = traits_x::dimension; + static const int Dim = traits::dimension; public: @@ -67,10 +67,10 @@ public: boost::optional&> H = boost::none) const { if (H) { const T value = expression_.value(x, keys_, dims_, *H); - return traits_x::Local(measurement_, value); + return traits::Local(measurement_, value); } else { const T value = expression_.value(x); - return traits_x::Local(measurement_, value); + return traits::Local(measurement_, value); } } @@ -98,7 +98,7 @@ public: T value = expression_.value(x, jacobianMap); // <<< Reverse AD happens here ! // Evaluate error and set RHS vector b - Ab(size()).col(0) = -traits_x::Local(measurement_, value); + Ab(size()).col(0) = -traits::Local(measurement_, value); // Whiten the corresponding system, Ab already contains RHS Vector dummy(Dim); diff --git a/gtsam_unstable/nonlinear/LinearizedFactor.h b/gtsam_unstable/nonlinear/LinearizedFactor.h index 2ffdd159e..72c7c66f5 100644 --- a/gtsam_unstable/nonlinear/LinearizedFactor.h +++ b/gtsam_unstable/nonlinear/LinearizedFactor.h @@ -158,7 +158,7 @@ private: /// traits template<> -struct traits_x : public Testable { +struct traits : public Testable { }; /** @@ -273,7 +273,7 @@ private: /// traits template<> -struct traits_x : public Testable { +struct traits : public Testable { }; } // \namespace aspn diff --git a/gtsam_unstable/nonlinear/expressionTesting.h b/gtsam_unstable/nonlinear/expressionTesting.h index 125ebc5d7..bcca51d07 100644 --- a/gtsam_unstable/nonlinear/expressionTesting.h +++ b/gtsam_unstable/nonlinear/expressionTesting.h @@ -107,7 +107,7 @@ void testExpressionJacobians(TestResult& result_, const std::string& name_, const gtsam::Expression& expression, const gtsam::Values& values, double nd_step, double tolerance) { // Create factor - size_t size = traits_x::dimension; + size_t size = traits::dimension; ExpressionFactor f(noiseModel::Unit::Create(size), expression.value(values), expression); testFactorJacobians(result_, name_, f, values, nd_step, tolerance); diff --git a/gtsam_unstable/nonlinear/tests/testCallRecord.cpp b/gtsam_unstable/nonlinear/tests/testCallRecord.cpp index f4bd7323e..15a0cc494 100644 --- a/gtsam_unstable/nonlinear/tests/testCallRecord.cpp +++ b/gtsam_unstable/nonlinear/tests/testCallRecord.cpp @@ -71,7 +71,7 @@ struct CallConfig { /// traits namespace gtsam { -template<> struct traits_x : public Testable {}; +template<> struct traits : public Testable {}; } struct Record: public internal::CallRecordImplementor { diff --git a/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h b/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h index 67d559108..857c07761 100644 --- a/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h +++ b/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h @@ -91,8 +91,8 @@ public: boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { - Vector v1( traits_x::Logmap(p1) ); - Vector v2( traits_x::Logmap(p2) ); + Vector v1( traits::Logmap(p1) ); + Vector v2( traits::Logmap(p2) ); Vector alpha(tau_.size()); Vector alpha_v1(tau_.size()); @@ -133,7 +133,7 @@ private: }; // \class GaussMarkov1stOrderFactor /// traits -template struct traits_x > : +template struct traits > : public Testable > { }; diff --git a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h index 0da1c3187..6b111b759 100644 --- a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h +++ b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h @@ -395,7 +395,7 @@ private: /// traits template -struct traits_x > : +struct traits > : public Testable > { }; diff --git a/gtsam_unstable/slam/ProjectionFactorPPP.h b/gtsam_unstable/slam/ProjectionFactorPPP.h index 5f6f2636b..0426c3ba4 100644 --- a/gtsam_unstable/slam/ProjectionFactorPPP.h +++ b/gtsam_unstable/slam/ProjectionFactorPPP.h @@ -181,7 +181,7 @@ namespace gtsam { /// traits template - struct traits_x > : + struct traits > : public Testable > { }; diff --git a/gtsam_unstable/slam/ProjectionFactorPPPC.h b/gtsam_unstable/slam/ProjectionFactorPPPC.h index 9b97b59a3..5906a6664 100644 --- a/gtsam_unstable/slam/ProjectionFactorPPPC.h +++ b/gtsam_unstable/slam/ProjectionFactorPPPC.h @@ -171,7 +171,7 @@ namespace gtsam { /// traits template - struct traits_x > : + struct traits > : public Testable > { }; diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index 548bc98a0..5d2ba3323 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -747,7 +747,7 @@ private: /// traits template -struct traits_x > : +struct traits > : public Testable > { }; diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h index bb4d5b42d..2e3bc866b 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h @@ -217,7 +217,7 @@ private: /// traits template -struct traits_x > : +struct traits > : public Testable > { }; diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h index f25e84b8d..2abc49fa1 100644 --- a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h +++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h @@ -226,7 +226,7 @@ namespace gtsam { /// traits template - struct traits_x > : + struct traits > : public Testable > { }; diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h index b0059ecc9..adfb9ae36 100644 --- a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h +++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h @@ -424,7 +424,7 @@ namespace gtsam { /// traits template - struct traits_x > : + struct traits > : public Testable > { }; diff --git a/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp b/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp index 35ed6d2ae..b468a2b6e 100644 --- a/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp +++ b/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp @@ -31,7 +31,7 @@ typedef PoseBetweenFactor TestPoseBetweenFactor; /// traits namespace gtsam { template<> -struct traits_x : public Testable { +struct traits : public Testable { }; } diff --git a/gtsam_unstable/slam/tests/testPosePriorFactor.cpp b/gtsam_unstable/slam/tests/testPosePriorFactor.cpp index 4dd836937..ddb5cf7a2 100644 --- a/gtsam_unstable/slam/tests/testPosePriorFactor.cpp +++ b/gtsam_unstable/slam/tests/testPosePriorFactor.cpp @@ -31,7 +31,7 @@ typedef PosePriorFactor TestPosePriorFactor; /// traits namespace gtsam { template<> -struct traits_x : public Testable { +struct traits : public Testable { }; } diff --git a/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp b/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp index 84a1c13f6..573352e87 100644 --- a/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp +++ b/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp @@ -51,7 +51,7 @@ typedef ProjectionFactorPPP TestProjectionFactor; /// traits namespace gtsam { template<> -struct traits_x : public Testable { +struct traits : public Testable { }; } diff --git a/tests/simulated2D.h b/tests/simulated2D.h index 4b67c9874..37d2455eb 100644 --- a/tests/simulated2D.h +++ b/tests/simulated2D.h @@ -274,12 +274,12 @@ namespace simulated2D { /// traits namespace gtsam { template -struct traits_x > : Testable< +struct traits > : Testable< simulated2D::GenericMeasurement > { }; template<> -struct traits_x : public Testable { +struct traits : public Testable { }; } diff --git a/tests/simulated2DConstraints.h b/tests/simulated2DConstraints.h index 0d4d309c3..f593ab23a 100644 --- a/tests/simulated2DConstraints.h +++ b/tests/simulated2DConstraints.h @@ -90,11 +90,11 @@ namespace simulated2D { virtual double value(const Point& x, boost::optional H = boost::none) const { if (H) { - Matrix D = zeros(1, traits_x::GetDimension(x)); + Matrix D = zeros(1, traits::GetDimension(x)); D(0, IDX) = 1.0; *H = D; } - return traits_x::Logmap(x)(IDX); + return traits::Logmap(x)(IDX); } }; diff --git a/tests/testManifold.cpp b/tests/testManifold.cpp index 6d27a250d..f6180fb73 100644 --- a/tests/testManifold.cpp +++ b/tests/testManifold.cpp @@ -68,17 +68,17 @@ TEST(Manifold, SomeVectorSpacesGTSAM) { // dimension TEST(Manifold, _dimension) { //using namespace traits; - EXPECT_LONGS_EQUAL(2, traits_x::dimension); - EXPECT_LONGS_EQUAL(8, traits_x::dimension); - EXPECT_LONGS_EQUAL(1, traits_x::dimension); + EXPECT_LONGS_EQUAL(2, traits::dimension); + EXPECT_LONGS_EQUAL(8, traits::dimension); + EXPECT_LONGS_EQUAL(1, traits::dimension); } //****************************************************************************** TEST(Manifold, Identity) { - EXPECT_DOUBLES_EQUAL(0.0, traits_x::Identity(), 0.0); - EXPECT(assert_equal(Matrix(Matrix24::Zero()), Matrix(traits_x::Identity()))); - EXPECT(assert_equal(Pose3(), traits_x::Identity())); - EXPECT(assert_equal(Point2(), traits_x::Identity())); + EXPECT_DOUBLES_EQUAL(0.0, traits::Identity(), 0.0); + EXPECT(assert_equal(Matrix(Matrix24::Zero()), Matrix(traits::Identity()))); + EXPECT(assert_equal(Pose3(), traits::Identity())); + EXPECT(assert_equal(Point2(), traits::Identity())); } //****************************************************************************** @@ -86,14 +86,14 @@ TEST(Manifold, Identity) { TEST(Manifold, DefaultChart) { //DefaultChart chart1; - EXPECT(traits_x::Local(Point2(0, 0), Point2(1, 0)) == Vector2(1, 0)); - EXPECT(traits_x::Retract(Point2(0, 0), Vector2(1, 0)) == Point2(1, 0)); + EXPECT(traits::Local(Point2(0, 0), Point2(1, 0)) == Vector2(1, 0)); + EXPECT(traits::Retract(Point2(0, 0), Vector2(1, 0)) == Point2(1, 0)); Vector v2(2); v2 << 1, 0; //DefaultChart chart2; - EXPECT(assert_equal(v2, traits_x::Local(Vector2(0, 0), Vector2(1, 0)))); - EXPECT(traits_x::Retract(Vector2(0, 0), v2) == Vector2(1, 0)); + EXPECT(assert_equal(v2, traits::Local(Vector2(0, 0), Vector2(1, 0)))); + EXPECT(traits::Retract(Vector2(0, 0), v2) == Vector2(1, 0)); { typedef Matrix2 ManifoldPoint; @@ -102,8 +102,8 @@ TEST(Manifold, DefaultChart) { m << 1, 3, 2, 4; // m as per default is in column-major storage mode. So this yields a linear representation of (1, 2, 3, 4)! - EXPECT(assert_equal(Vector(Vector4(1, 2, 3, 4)), Vector(traits_x::Local(ManifoldPoint::Zero(), m)))); - EXPECT(traits_x::Retract(m, Vector4(1, 2, 3, 4)) == 2 * m); + EXPECT(assert_equal(Vector(Vector4(1, 2, 3, 4)), Vector(traits::Local(ManifoldPoint::Zero(), m)))); + EXPECT(traits::Retract(m, Vector4(1, 2, 3, 4)) == 2 * m); } { @@ -111,8 +111,8 @@ TEST(Manifold, DefaultChart) { ManifoldPoint m; //DefaultChart chart; m << 1, 2; - EXPECT(assert_equal(Vector(Vector2(1, 2)), Vector(traits_x::Local(ManifoldPoint::Zero(), m)))); - EXPECT(traits_x::Retract(m, Vector2(1, 2)) == 2 * m); + EXPECT(assert_equal(Vector(Vector2(1, 2)), Vector(traits::Local(ManifoldPoint::Zero(), m)))); + EXPECT(traits::Retract(m, Vector2(1, 2)) == 2 * m); } { @@ -120,33 +120,33 @@ TEST(Manifold, DefaultChart) { ManifoldPoint m; //DefaultChart chart; m << 1; - EXPECT(assert_equal(Vector(ManifoldPoint::Ones()), Vector(traits_x::Local(ManifoldPoint::Zero(), m)))); - EXPECT(traits_x::Retract(m, ManifoldPoint::Ones()) == 2 * m); + EXPECT(assert_equal(Vector(ManifoldPoint::Ones()), Vector(traits::Local(ManifoldPoint::Zero(), m)))); + EXPECT(traits::Retract(m, ManifoldPoint::Ones()) == 2 * m); } //DefaultChart chart3; Vector v1(1); v1 << 1; - EXPECT(assert_equal(v1, traits_x::Local(0, 1))); - EXPECT_DOUBLES_EQUAL(traits_x::Retract(0, v1), 1, 1e-9); + EXPECT(assert_equal(v1, traits::Local(0, 1))); + EXPECT_DOUBLES_EQUAL(traits::Retract(0, v1), 1, 1e-9); // Dynamic does not work yet ! Vector z = zero(2), v(2); v << 1, 0; //DefaultChart chart4; -// EXPECT(assert_equal(traits_x::Local(z, v), v)); -// EXPECT(assert_equal(traits_x::Retract(z, v), v)); +// EXPECT(assert_equal(traits::Local(z, v), v)); +// EXPECT(assert_equal(traits::Retract(z, v), v)); Vector v3(3); v3 << 1, 1, 1; Rot3 I = Rot3::identity(); Rot3 R = I.retract(v3); //DefaultChart chart5; - EXPECT(assert_equal(v3, traits_x::Local(I, R))); - EXPECT(assert_equal(traits_x::Retract(I, v3), R)); + EXPECT(assert_equal(v3, traits::Local(I, R))); + EXPECT(assert_equal(traits::Retract(I, v3), R)); // Check zero vector //DefaultChart chart6; - EXPECT(assert_equal(zero(3), traits_x::Local(R, R))); + EXPECT(assert_equal(zero(3), traits::Local(R, R))); } //******************************************************************************