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

Merge pull request #2039 from DanMcGann/bugfix/pose3_bearing_range_jacobians 

Bugfix Bearing/Range Pose3toPose3 Jacobians
release/4.3a0
Varun Agrawal 2025-03-05 10:17:45 -05:00 committed by GitHub
parent a89b88c2a0 68327a1936
commit 922d50592b
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
2 changed files with 14 additions and 20 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;
} }
/* ************************************************************************* */ /* ************************************************************************* */

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

@ -769,18 +769,9 @@ TEST(Pose3, PoseToPoseBearing) {
EXPECT(assert_equal(Unit3(0,1,0), xl1.bearing(xl2, actualH1, actualH2), 1e-9)); EXPECT(assert_equal(Unit3(0,1,0), xl1.bearing(xl2, actualH1, actualH2), 1e-9));
// Check numerical derivatives // Check numerical derivatives
expectedH1 = numericalDerivative21(bearing_proxy, xl1, l2); std::function<Unit3(const Pose3&, const Pose3&)> f = [](const Pose3& a, const Pose3& b) { return a.bearing(b); };
expectedH1 = numericalDerivative21(f, xl1, xl2);
// Since the second pose is treated as a point, the value calculated by expectedH2 = numericalDerivative22(f, xl1, xl2);
// 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);
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));
} }