12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #1234 from shteren1/pose3_jacobians

release/4.3a0
Varun Agrawal 2022-07-07 10:53:29 -04:00 committed by GitHub
parent af04094a38 7c077f8536
commit b91c43e691
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
4 changed files with 162 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
gtsam/geometry/Pose3.cpp
View File

@ -489,6 +489,11 @@ boost::optional<Pose3> align(const Point3Pairs &baPointPairs) {
}
#endif
/* ************************************************************************* */
Pose3 Pose3::slerp(double t, const Pose3& other, OptionalJacobian<6, 6> Hx, OptionalJacobian<6, 6> Hy) const {
return interpolate(*this, other, t, Hx, Hy);
}
/* ************************************************************************* */
std::ostream &operator<<(std::ostream &os, const Pose3& pose) {
// Both Rot3 and Point3 have ostream definitions so we use them.

8
gtsam/geometry/Pose3.h
View File

@ -379,6 +379,14 @@ public:
return std::make_pair(0, 2);
}
/**
* @brief Spherical Linear interpolation between *this and other
* @param s a value between 0 and 1.5
* @param other final point of interpolation geodesic on manifold
*/
Pose3 slerp(double t, const Pose3& other, OptionalJacobian<6, 6> Hx = boost::none,
OptionalJacobian<6, 6> Hy = boost::none) const;
/// Output stream operator
GTSAM_EXPORT
friend std::ostream &operator<<(std::ostream &os, const Pose3& p);

33
gtsam/geometry/geometry.i
View File

@ -446,24 +446,43 @@ class Pose3 {
// Group
static gtsam::Pose3 identity();
gtsam::Pose3 inverse() const;
gtsam::Pose3 inverse(Eigen::Ref<Eigen::MatrixXd> H) const;
gtsam::Pose3 compose(const gtsam::Pose3& pose) const;
gtsam::Pose3 compose(const gtsam::Pose3& pose,
Eigen::Ref<Eigen::MatrixXd> H1,
Eigen::Ref<Eigen::MatrixXd> H2) const;
gtsam::Pose3 between(const gtsam::Pose3& pose) const;
gtsam::Pose3 between(const gtsam::Pose3& pose,
Eigen::Ref<Eigen::MatrixXd> H1,
Eigen::Ref<Eigen::MatrixXd> H2) const;
gtsam::Pose3 slerp(double t, const gtsam::Pose3& pose) const;
gtsam::Pose3 slerp(double t, const gtsam::Pose3& pose,
Eigen::Ref<Eigen::MatrixXd> Hx,
Eigen::Ref<Eigen::MatrixXd> Hy) const;
// Operator Overloads
gtsam::Pose3 operator*(const gtsam::Pose3& pose) const;
// Manifold
gtsam::Pose3 retract(Vector v) const;
gtsam::Pose3 retract(Vector v, Eigen::Ref<Eigen::MatrixXd> Hxi) const;
Vector localCoordinates(const gtsam::Pose3& pose) const;
Vector localCoordinates(const gtsam::Pose3& pose, Eigen::Ref<Eigen::MatrixXd> Hxi) const;
// Lie Group
static gtsam::Pose3 Expmap(Vector v);
static gtsam::Pose3 Expmap(Vector v, Eigen::Ref<Eigen::MatrixXd> Hxi);
static Vector Logmap(const gtsam::Pose3& pose);
static Vector Logmap(const gtsam::Pose3& pose, Eigen::Ref<Eigen::MatrixXd> Hpose);
gtsam::Pose3 expmap(Vector v);
Vector logmap(const gtsam::Pose3& pose);
Matrix AdjointMap() const;
Vector Adjoint(Vector xi) const;
Vector Adjoint(Vector xi, Eigen::Ref<Eigen::MatrixXd> H_this,
Eigen::Ref<Eigen::MatrixXd> H_xib) const;
Vector AdjointTranspose(Vector xi) const;
Vector AdjointTranspose(Vector xi, Eigen::Ref<Eigen::MatrixXd> H_this,
Eigen::Ref<Eigen::MatrixXd> H_x) const;
static Matrix adjointMap(Vector xi);
static Vector adjoint(Vector xi, Vector y);
static Matrix adjointMap_(Vector xi);
@ -476,7 +495,11 @@ class Pose3 {
// Group Action on Point3
gtsam::Point3 transformFrom(const gtsam::Point3& point) const;
gtsam::Point3 transformFrom(const gtsam::Point3& point, Eigen::Ref<Eigen::MatrixXd> Hself,
Eigen::Ref<Eigen::MatrixXd> Hpoint) const;
gtsam::Point3 transformTo(const gtsam::Point3& point) const;
gtsam::Point3 transformTo(const gtsam::Point3& point, Eigen::Ref<Eigen::MatrixXd> Hself,
Eigen::Ref<Eigen::MatrixXd> Hpoint) const;
// Matrix versions
Matrix transformFrom(const Matrix& points) const;
@ -484,15 +507,25 @@ class Pose3 {
// Standard Interface
gtsam::Rot3 rotation() const;
gtsam::Rot3 rotation(Eigen::Ref<Eigen::MatrixXd> Hself) const;
gtsam::Point3 translation() const;
gtsam::Point3 translation(Eigen::Ref<Eigen::MatrixXd> Hself) const;
double x() const;
double y() const;
double z() const;
Matrix matrix() const;
gtsam::Pose3 transformPoseFrom(const gtsam::Pose3& pose) const;
gtsam::Pose3 transformPoseFrom(const gtsam::Pose3& pose, Eigen::Ref<Eigen::MatrixXd> Hself,
Eigen::Ref<Eigen::MatrixXd> HaTb) const;
gtsam::Pose3 transformPoseTo(const gtsam::Pose3& pose) const;
gtsam::Pose3 transformPoseTo(const gtsam::Pose3& pose, Eigen::Ref<Eigen::MatrixXd> Hself,
Eigen::Ref<Eigen::MatrixXd> HwTb) const;
double range(const gtsam::Point3& point);
double range(const gtsam::Point3& point, Eigen::Ref<Eigen::MatrixXd> Hself,
Eigen::Ref<Eigen::MatrixXd> Hpoint);
double range(const gtsam::Pose3& pose);
double range(const gtsam::Pose3& pose, Eigen::Ref<Eigen::MatrixXd> Hself,
Eigen::Ref<Eigen::MatrixXd> Hpose);
// enabling serialization functionality
void serialize() const;

117
python/gtsam/tests/test_Pose3.py
View File

@ -19,6 +19,62 @@ from gtsam import Point3, Pose3, Rot3, Point3Pairs
from gtsam.utils.test_case import GtsamTestCase
def numerical_derivative_pose(pose, method, delta=1e-5):
jacobian = np.zeros((6, 6))
for idx in range(6):
xplus = np.zeros(6)
xplus[idx] = delta
xminus = np.zeros(6)
xminus[idx] = -delta
pose_plus = pose.retract(xplus).__getattribute__(method)()
pose_minus = pose.retract(xminus).__getattribute__(method)()
jacobian[:, idx] = pose_minus.localCoordinates(pose_plus) / (2 * delta)
return jacobian
def numerical_derivative_2_poses(pose, other_pose, method, delta=1e-5, inputs=()):
jacobian = np.zeros((6, 6))
other_jacobian = np.zeros((6, 6))
for idx in range(6):
xplus = np.zeros(6)
xplus[idx] = delta
xminus = np.zeros(6)
xminus[idx] = -delta
pose_plus = pose.retract(xplus).__getattribute__(method)(*inputs, other_pose)
pose_minus = pose.retract(xminus).__getattribute__(method)(*inputs, other_pose)
jacobian[:, idx] = pose_minus.localCoordinates(pose_plus) / (2 * delta)
other_pose_plus = pose.__getattribute__(method)(*inputs, other_pose.retract(xplus))
other_pose_minus = pose.__getattribute__(method)(*inputs, other_pose.retract(xminus))
other_jacobian[:, idx] = other_pose_minus.localCoordinates(other_pose_plus) / (2 * delta)
return jacobian, other_jacobian
def numerical_derivative_pose_point(pose, point, method, delta=1e-5):
jacobian = np.zeros((3, 6))
point_jacobian = np.zeros((3, 3))
for idx in range(6):
xplus = np.zeros(6)
xplus[idx] = delta
xminus = np.zeros(6)
xminus[idx] = -delta
point_plus = pose.retract(xplus).__getattribute__(method)(point)
point_minus = pose.retract(xminus).__getattribute__(method)(point)
jacobian[:, idx] = (point_plus - point_minus) / (2 * delta)
if idx < 3:
xplus = np.zeros(3)
xplus[idx] = delta
xminus = np.zeros(3)
xminus[idx] = -delta
point_plus = pose.__getattribute__(method)(point + xplus)
point_minus = pose.__getattribute__(method)(point + xminus)
point_jacobian[:, idx] = (point_plus - point_minus) / (2 * delta)
return jacobian, point_jacobian
class TestPose3(GtsamTestCase):
"""Test selected Pose3 methods."""
@ -30,6 +86,47 @@ class TestPose3(GtsamTestCase):
actual = T2.between(T3)
self.gtsamAssertEquals(actual, expected, 1e-6)
#test jacobians
jacobian = np.zeros((6, 6), order='F')
jacobian_other = np.zeros((6, 6), order='F')
T2.between(T3, jacobian, jacobian_other)
jacobian_numerical, jacobian_numerical_other = numerical_derivative_2_poses(T2, T3, 'between')
self.gtsamAssertEquals(jacobian, jacobian_numerical)
self.gtsamAssertEquals(jacobian_other, jacobian_numerical_other)
def test_inverse(self):
"""Test between method."""
pose = Pose3(Rot3.Rodrigues(0, 0, -math.pi/2), Point3(2, 4, 0))
expected = Pose3(Rot3.Rodrigues(0, 0, math.pi/2), Point3(4, -2, 0))
actual = pose.inverse()
self.gtsamAssertEquals(actual, expected, 1e-6)
#test jacobians
jacobian = np.zeros((6, 6), order='F')
pose.inverse(jacobian)
jacobian_numerical = numerical_derivative_pose(pose, 'inverse')
self.gtsamAssertEquals(jacobian, jacobian_numerical)
def test_slerp(self):
"""Test slerp method."""
pose0 = gtsam.Pose3()
pose1 = Pose3(Rot3.Rodrigues(0, 0, -math.pi/2), Point3(2, 4, 0))
actual_alpha_0 = pose0.slerp(0, pose1)
self.gtsamAssertEquals(actual_alpha_0, pose0)
actual_alpha_1 = pose0.slerp(1, pose1)
self.gtsamAssertEquals(actual_alpha_1, pose1)
actual_alpha_half = pose0.slerp(0.5, pose1)
expected_alpha_half = Pose3(Rot3.Rodrigues(0, 0, -math.pi/4), Point3(0.17157288, 2.41421356, 0))
self.gtsamAssertEquals(actual_alpha_half, expected_alpha_half, tol=1e-6)
# test jacobians
jacobian = np.zeros((6, 6), order='F')
jacobian_other = np.zeros((6, 6), order='F')
pose0.slerp(0.5, pose1, jacobian, jacobian_other)
jacobian_numerical, jacobian_numerical_other = numerical_derivative_2_poses(pose0, pose1, 'slerp', inputs=[0.5])
self.gtsamAssertEquals(jacobian, jacobian_numerical)
self.gtsamAssertEquals(jacobian_other, jacobian_numerical_other)
def test_transformTo(self):
"""Test transformTo method."""
pose = Pose3(Rot3.Rodrigues(0, 0, -math.pi/2), Point3(2, 4, 0))
@ -37,6 +134,15 @@ class TestPose3(GtsamTestCase):
expected = Point3(2, 1, 10)
self.gtsamAssertEquals(actual, expected, 1e-6)
#test jacobians
point = Point3(3, 2, 10)
jacobian_pose = np.zeros((3, 6), order='F')
jacobian_point = np.zeros((3, 3), order='F')
pose.transformTo(point, jacobian_pose, jacobian_point)
jacobian_numerical_pose, jacobian_numerical_point = numerical_derivative_pose_point(pose, point, 'transformTo')
self.gtsamAssertEquals(jacobian_pose, jacobian_numerical_pose)
self.gtsamAssertEquals(jacobian_point, jacobian_numerical_point)
# multi-point version
points = np.stack([Point3(3, 2, 10), Point3(3, 2, 10)]).T
actual_array = pose.transformTo(points)
@ -51,6 +157,15 @@ class TestPose3(GtsamTestCase):
expected = Point3(3, 2, 10)
self.gtsamAssertEquals(actual, expected, 1e-6)
#test jacobians
point = Point3(3, 2, 10)
jacobian_pose = np.zeros((3, 6), order='F')
jacobian_point = np.zeros((3, 3), order='F')
pose.transformFrom(point, jacobian_pose, jacobian_point)
jacobian_numerical_pose, jacobian_numerical_point = numerical_derivative_pose_point(pose, point, 'transformFrom')
self.gtsamAssertEquals(jacobian_pose, jacobian_numerical_pose)
self.gtsamAssertEquals(jacobian_point, jacobian_numerical_point)
# multi-point version
points = np.stack([Point3(2, 1, 10), Point3(2, 1, 10)]).T
actual_array = pose.transformFrom(points)
@ -122,4 +237,4 @@ class TestPose3(GtsamTestCase):
if __name__ == "__main__":
unittest.main()
unittest.main()