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

Cleaner Jacobian.

release/4.3a0
Frank Dellaert 2021-11-06 13:51:15 -04:00
parent 4bd80357f5
commit 238563f0e5
1 changed files with 2 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
gtsam/geometry/Pose3.cpp
View File

@ -93,9 +93,8 @@ Vector6 Pose3::AdjointTranspose(const Vector6& x, OptionalJacobian<6, 6> H_pose,
if (H_pose) { if (H_pose) {
const auto w_T_hat = skewSymmetric(AdTx.head<3>()), const auto w_T_hat = skewSymmetric(AdTx.head<3>()),
v_T_hat = skewSymmetric(AdTx.tail<3>()); v_T_hat = skewSymmetric(AdTx.tail<3>());
*H_pose = (Matrix6() << w_T_hat, v_T_hat, // *H_pose << w_T_hat, v_T_hat, //
/* */ v_T_hat, Z_3x3) /* */ v_T_hat, Z_3x3;
.finished();
} }
if (H_x) { if (H_x) {
*H_x = Ad.transpose(); *H_x = Ad.transpose();