From 37ed46cee1d8e4cf2a68eb4f6d61f1815aa82307 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 11 Feb 2019 10:44:24 -0500 Subject: [PATCH] Added underscore to case conflicts until wrap can handle this. --- cython/gtsam/tests/test_Pose2.py | 21 +++++++++++++++++++++ cython/gtsam/tests/test_Pose3.py | 4 ++-- gtsam.h | 8 ++++---- gtsam/geometry/Pose2.h | 8 ++++++-- gtsam/geometry/Pose3.h | 4 ++++ 5 files changed, 37 insertions(+), 8 deletions(-) create mode 100644 cython/gtsam/tests/test_Pose2.py diff --git a/cython/gtsam/tests/test_Pose2.py b/cython/gtsam/tests/test_Pose2.py new file mode 100644 index 000000000..f43ec0683 --- /dev/null +++ b/cython/gtsam/tests/test_Pose2.py @@ -0,0 +1,21 @@ +"""Pose2 unit tests.""" +import unittest + +import numpy as np + +from gtsam import Pose2 + + +class TestPose2(unittest.TestCase): + """Test selected Pose2 methods.""" + + def test_adjoint(self): + """Test adjoint method.""" + xi = np.array([1, 2, 3]) + expected = np.dot(Pose2.adjointMap(xi), xi) + actual = Pose2.adjoint(xi, xi) + np.testing.assert_array_equal(actual, expected) + + +if __name__ == "__main__": + unittest.main() diff --git a/cython/gtsam/tests/test_Pose3.py b/cython/gtsam/tests/test_Pose3.py index 4752a4b02..b878a9551 100644 --- a/cython/gtsam/tests/test_Pose3.py +++ b/cython/gtsam/tests/test_Pose3.py @@ -49,8 +49,8 @@ class TestPose3(unittest.TestCase): def test_adjoint(self): """Test adjoint method.""" xi = np.array([1, 2, 3, 4, 5, 6]) - expected = np.dot(Pose3.adjointMap(xi), xi) - actual = Pose3.adjoint(xi, xi) + expected = np.dot(Pose3.adjointMap_(xi), xi) + actual = Pose3.adjoint_(xi, xi) np.testing.assert_array_equal(actual, expected) diff --git a/gtsam.h b/gtsam.h index 3d4ee5c29..4720d9d6b 100644 --- a/gtsam.h +++ b/gtsam.h @@ -577,8 +577,8 @@ class Pose2 { static Matrix LogmapDerivative(const gtsam::Pose2& v); Matrix AdjointMap() const; Vector Adjoint(Vector xi) const; - static Matrix adjointMap(Vector v); - Vector adjoint(Vector xi, Vector y); + static Matrix adjointMap_(Vector v); + Vector adjoint_(Vector xi, Vector y); Vector adjointTranspose(Vector xi, Vector y); static Matrix wedge(double vx, double vy, double w); @@ -628,8 +628,8 @@ class Pose3 { static Vector Logmap(const gtsam::Pose3& pose); Matrix AdjointMap() const; Vector Adjoint(Vector xi) const; - static Matrix adjointMap(Vector xi); - static Vector adjoint(Vector xi, Vector y); + static Matrix adjointMap_(Vector xi); + static Vector adjoint_(Vector xi, Vector y); static Vector adjointTranspose(Vector xi, Vector y); static Matrix ExpmapDerivative(Vector xi); static Matrix LogmapDerivative(const gtsam::Pose3& xi); diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 603c3a421..6937ff78d 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -146,17 +146,21 @@ public: /** * Action of the adjointMap on a Lie-algebra vector y, with optional derivatives */ - Vector3 adjoint(const Vector3& xi, const Vector3& y) { + static Vector3 adjoint(const Vector3& xi, const Vector3& y) { return adjointMap(xi) * y; } /** * The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space. */ - Vector3 adjointTranspose(const Vector3& xi, const Vector3& y) { + static Vector3 adjointTranspose(const Vector3& xi, const Vector3& y) { return adjointMap(xi).transpose() * y; } + // temporary fix for wrappers until case issue is resolved + static Matrix3 adjointMap_(const Vector3 &xi) { return adjointMap(xi);} + static Vector3 adjoint_(const Vector3 &xi, const Vector3 &y) { return adjoint(xi, y);} + /** * wedge for SE(2): * @param xi 3-dim twist (v,omega) where diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index ca0fdff10..757c4fdd4 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -160,6 +160,10 @@ public: static Vector6 adjoint(const Vector6 &xi, const Vector6 &y, OptionalJacobian<6, 6> = boost::none); + // temporary fix for wrappers until case issue is resolved + static Matrix6 adjointMap_(const Vector6 &xi) { return adjointMap(xi);} + static Vector6 adjoint_(const Vector6 &xi, const Vector6 &y) { return adjoint(xi, y);} + /** * The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space. */