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

Bugfix Bearing/Range Pose3toPose3 Jacobians 

Previously the jacobians for bearing between two Pose3 objects were
computed incorrectly as it neglected the jacobian of a pose's
translation wrt its tangent vector. The jacobian for range betwen two
Pose3 objects was computed correctly but in a non-canonical way. This
commit fixes the bearing jacobian, and provides a canonical
implementation for both Pose3-Pose3 bearing and range jacobians.
release/4.3a0
Dan McGann 2025-03-04 15:30:13 -05:00
parent c6a22301b7
commit ead1271f33
2 changed files with 11 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
gtsam/geometry/Pose3.cpp
View File

@ -442,9 +442,11 @@ double Pose3::range(const Point3& point, OptionalJacobian<1, 6> Hself,
/* ************************************************************************* */ /* ************************************************************************* */
double Pose3::range(const Pose3& pose, OptionalJacobian<1, 6> Hself, double Pose3::range(const Pose3& pose, OptionalJacobian<1, 6> Hself,
OptionalJacobian<1, 6> Hpose) const { OptionalJacobian<1, 6> Hpose) const {
Matrix36 D_point_pose;
Matrix13 D_local_point; Matrix13 D_local_point;
double r = range(pose.translation(), Hself, Hpose ? &D_local_point : 0); Point3 point = pose.translation(Hpose ? &D_point_pose : 0);
if (Hpose) *Hpose << Matrix13::Zero(), D_local_point * pose.rotation().matrix(); double r = range(point, Hself, Hpose ? &D_local_point : 0);
if (Hpose) *Hpose = D_local_point * D_point_pose;
return r; return r;
} }
@ -467,12 +469,13 @@ Unit3 Pose3::bearing(const Point3& point, OptionalJacobian<2, 6> Hself,
/* ************************************************************************* */ /* ************************************************************************* */
Unit3 Pose3::bearing(const Pose3& pose, OptionalJacobian<2, 6> Hself, Unit3 Pose3::bearing(const Pose3& pose, OptionalJacobian<2, 6> Hself,
OptionalJacobian<2, 6> Hpose) const { OptionalJacobian<2, 6> Hpose) const {
if (Hpose) { Matrix36 D_point_pose;
Hpose->setZero(); Matrix23 D_local_point;
return bearing(pose.translation(), Hself, Hpose.cols<3>(3)); Point3 point = pose.translation(Hpose ? &D_point_pose : 0);
} Unit3 b = bearing(point, Hself, Hpose ? &D_local_point : 0);
return bearing(pose.translation(), Hself, {}); if (Hpose) *Hpose = D_local_point * D_point_pose;
return b;
} }
/* ************************************************************************* */ /* ************************************************************************* */

10
gtsam/geometry/tests/testPose3.cpp
View File

@ -770,17 +770,7 @@ TEST(Pose3, PoseToPoseBearing) {
// Check numerical derivatives // Check numerical derivatives
expectedH1 = numericalDerivative21(bearing_proxy, xl1, l2); expectedH1 = numericalDerivative21(bearing_proxy, xl1, l2);
// Since the second pose is treated as a point, the value calculated by
// numericalDerivative22 only depends on the position of the pose. Here, we
// calculate the Jacobian w.r.t. the second pose's position, and then augment
// that with zeroes in the block that is w.r.t. the second pose's
// orientation.
H2block = numericalDerivative22(bearing_proxy, xl1, l2); H2block = numericalDerivative22(bearing_proxy, xl1, l2);
expectedH2 = Matrix(2, 6);
expectedH2.setZero();
expectedH2.block<2, 3>(0, 3) = H2block;
EXPECT(assert_equal(expectedH1, actualH1, 1e-5)); EXPECT(assert_equal(expectedH1, actualH1, 1e-5));
EXPECT(assert_equal(expectedH2, actualH2, 1e-5)); EXPECT(assert_equal(expectedH2, actualH2, 1e-5));
} }