Merged in feature/adjoint_operators (pull request #364)
Feature/adjoint operators Approved-by: Mandy Xie <manxie@gatech.edu>release/4.3a0
commit
1d97f86b9d
|
@ -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__":
|
||||
|
|
10
gtsam.h
10
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
|
||||
|
|
|
@ -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
|
||||
|
|
Loading…
Reference in New Issue