From eada1ee505aecc2e6dc4bb4ddf55ae8502c1d257 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 31 Dec 2018 14:48:22 -0500 Subject: [PATCH 1/3] Adding adjoint and adjoint transpose functions --- gtsam/geometry/Pose2.h | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index f03e0852e..079c649f3 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -132,6 +132,8 @@ public: * Ad_pose is 3*3 matrix that when applied to twist xi \f$ [T_x,T_y,\theta] \f$, returns Ad_pose(xi) */ Matrix3 AdjointMap() const; + + /// Apply AdjointMap to twist xi inline Vector3 Adjoint(const Vector3& xi) const { return AdjointMap()*xi; } @@ -141,6 +143,20 @@ public: */ static Matrix3 adjointMap(const Vector3& v); + /** + * Action of the adjointMap on a Lie-algebra vector y, with optional derivatives + */ + 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) { + return adjointMap(xi).transpose() * y; + } + /** * wedge for SE(2): * @param xi 3-dim twist (v,omega) where From 409a0215b88d548524d39cdddbee73de63889bb9 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 31 Dec 2018 15:37:31 -0500 Subject: [PATCH 2/3] Added adjoint operators etc. --- gtsam.h | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/gtsam.h b/gtsam.h index 629fd70cf..d9052f65b 100644 --- a/gtsam.h +++ b/gtsam.h @@ -573,8 +573,13 @@ class Pose2 { // Lie Group static gtsam::Pose2 Expmap(Vector v); static Vector Logmap(const gtsam::Pose2& p); + static Matrix ExpmapDerivative(Vector v); + 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); + Vector adjointTranspose(Vector xi, Vector y); static Matrix wedge(double vx, double vy, double w); // Group Actions on Point2 @@ -623,6 +628,11 @@ 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 Vector adjointTranspose(Vector xi, Vector y); + static Matrix ExpmapDerivative(Vector xi); + static Matrix LogmapDerivative(const gtsam::Pose3& xi); static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz); // Group Action on Point3 From 1999fba7ae06957ec0f941307fcb71e9714cb1e8 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 31 Dec 2018 15:38:12 -0500 Subject: [PATCH 3/3] Cleaned up Pose3 unit test, added unit test for adjoint. --- cython/gtsam/tests/test_Pose3.py | 36 ++++++++++++++++++++++---------- 1 file changed, 25 insertions(+), 11 deletions(-) diff --git a/cython/gtsam/tests/test_Pose3.py b/cython/gtsam/tests/test_Pose3.py index 8fa50b90c..4752a4b02 100644 --- a/cython/gtsam/tests/test_Pose3.py +++ b/cython/gtsam/tests/test_Pose3.py @@ -1,25 +1,32 @@ +"""Pose3 unit tests.""" import math import unittest -from gtsam import Point3, Rot3, Pose3 +import numpy as np + +from gtsam import Point3, Pose3, Rot3 class TestPose3(unittest.TestCase): + """Test selected Pose3 methods.""" - def test__between(self): - T2 = Pose3(Rot3.Rodrigues(0.3,0.2,0.1),Point3(3.5,-8.2,4.2)) + def test_between(self): + """Test between method.""" + T2 = Pose3(Rot3.Rodrigues(0.3, 0.2, 0.1), Point3(3.5, -8.2, 4.2)) T3 = Pose3(Rot3.Rodrigues(-90, 0, 0), Point3(1, 2, 3)) expected = T2.inverse().compose(T3) actual = T2.between(T3) self.assertTrue(actual.equals(expected, 1e-6)) def test_transform_to(self): - transform = Pose3(Rot3.Rodrigues(0,0,-1.570796), Point3(2,4, 0)) - actual = transform.transform_to(Point3(3,2,10)) - expected = Point3 (2,1,10) + """Test transform_to method.""" + transform = Pose3(Rot3.Rodrigues(0, 0, -1.570796), Point3(2, 4, 0)) + actual = transform.transform_to(Point3(3, 2, 10)) + expected = Point3(2, 1, 10) self.assertTrue(actual.equals(expected, 1e-6)) def test_range(self): + """Test range method.""" l1 = Point3(1, 0, 0) l2 = Point3(1, 1, 0) x1 = Pose3() @@ -28,16 +35,23 @@ class TestPose3(unittest.TestCase): xl2 = Pose3(Rot3.Ypr(0.0, 1.0, 0.0), Point3(1, 1, 0)) # establish range is indeed zero - self.assertEqual(1,x1.range(point=l1)) + self.assertEqual(1, x1.range(point=l1)) # establish range is indeed sqrt2 - self.assertEqual(math.sqrt(2.0),x1.range(point=l2)) + self.assertEqual(math.sqrt(2.0), x1.range(point=l2)) # establish range is indeed zero - self.assertEqual(1,x1.range(pose=xl1)) - + self.assertEqual(1, x1.range(pose=xl1)) + # establish range is indeed sqrt2 - self.assertEqual(math.sqrt(2.0),x1.range(pose=xl2)) + self.assertEqual(math.sqrt(2.0), x1.range(pose=xl2)) + + 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) + np.testing.assert_array_equal(actual, expected) if __name__ == "__main__":