From 401ee703647a575483a8ab115fa65c388d3818c4 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 28 Dec 2014 17:14:04 +0100 Subject: [PATCH] Fixed overload resoltion error in template matching by removing overloads in traits --- gtsam/base/Lie.h | 26 ++++++------------- gtsam/nonlinear/expressions.h | 4 +-- .../examples/Pose2SLAMExampleExpressions.cpp | 6 +++-- 3 files changed, 14 insertions(+), 22 deletions(-) diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index 4a42bccf8..9c410ac25 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -144,9 +144,6 @@ struct LieGroupTraits : Testable { /// @{ typedef multiplicative_group_tag group_flavor; static Class Identity() { return Class::identity();} - static Class Compose(const Class& m1, const Class& m2) { return m1 * m2;} - static Class Between(const Class& m1, const Class& m2) { return m1.inverse() * m2;} - static Class Inverse(const Class& m) { return m.inverse();} /// @} /// @name Manifold @@ -161,21 +158,13 @@ struct LieGroupTraits : Testable { static int GetDimension(const Class&) {return dimension;} - static TangentVector Local(const Class& origin, const Class& other) { - return origin.localCoordinates(other); - } - - static Class Retract(const Class& origin, const TangentVector& v) { - return origin.retract(v); - } - static TangentVector Local(const Class& origin, const Class& other, - ChartJacobian Horigin, ChartJacobian Hother = boost::none) { + ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) { return origin.localCoordinates(other, Horigin, Hother); } static Class Retract(const Class& origin, const TangentVector& v, - ChartJacobian Horigin, ChartJacobian Hv = boost::none) { + ChartJacobian Horigin = boost::none, ChartJacobian Hv = boost::none) { return origin.retract(v, Horigin, Hv); } @@ -192,17 +181,18 @@ struct LieGroupTraits : Testable { return Class::Expmap(v, Hv); } - static Class Compose(const Class& m1, const Class& m2, ChartJacobian H1, - ChartJacobian H2 = boost::none) { + static Class Compose(const Class& m1, const Class& m2, // + ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { return m1.compose(m2, H1, H2); } - static Class Between(const Class& m1, const Class& m2, ChartJacobian H1, - ChartJacobian H2 = boost::none) { + static Class Between(const Class& m1, const Class& m2, // + ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { return m1.between(m2, H1, H2); } - static Class Inverse(const Class& m, ChartJacobian H) { + static Class Inverse(const Class& m, // + ChartJacobian H = boost::none) { return m.inverse(H); } diff --git a/gtsam/nonlinear/expressions.h b/gtsam/nonlinear/expressions.h index 18d0d5d8f..a549517e5 100644 --- a/gtsam/nonlinear/expressions.h +++ b/gtsam/nonlinear/expressions.h @@ -8,14 +8,14 @@ #pragma once #include -#include +#include namespace gtsam { // Generics template Expression between(const Expression& t1, const Expression& t2) { - return Expression(t1, &T::between, t2); + return Expression(traits::Between, t1, t2); } typedef Expression double_; diff --git a/gtsam_unstable/examples/Pose2SLAMExampleExpressions.cpp b/gtsam_unstable/examples/Pose2SLAMExampleExpressions.cpp index 3a3b97b77..e9ccbb2cb 100644 --- a/gtsam_unstable/examples/Pose2SLAMExampleExpressions.cpp +++ b/gtsam_unstable/examples/Pose2SLAMExampleExpressions.cpp @@ -18,8 +18,8 @@ */ // The two new headers that allow using our Automatic Differentiation Expression framework -#include -#include +#include +#include // Header order is close to far #include @@ -48,6 +48,8 @@ int main(int argc, char** argv) { noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); // 2b. Add odometry factors + BinaryExpression::Function f = traits::Between; + Expression(traits::Between, x1, x2); graph.push_back(ExpressionFactor(model, Pose2(2, 0, 0 ), between(x1,x2))); graph.push_back(ExpressionFactor(model, Pose2(2, 0, M_PI_2), between(x2,x3))); graph.push_back(ExpressionFactor(model, Pose2(2, 0, M_PI_2), between(x3,x4)));