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

Use translation method to get jacobian for pose in pose coordinates

release/4.3a0
peterQFR 2021-12-21 08:33:09 +10:00
parent cf0830084d
commit 66c8ca4af0
1 changed files with 4 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/navigation/BarometricFactor.cpp
View File

@ -45,9 +45,11 @@ bool BarometricFactor::equals(const NonlinearFactor& expected,
Vector BarometricFactor::evaluateError(const Pose3& p, const double& bias, Vector BarometricFactor::evaluateError(const Pose3& p, const double& bias,
boost::optional<Matrix&> H, boost::optional<Matrix&> H,
boost::optional<Matrix&> H2) const { boost::optional<Matrix&> H2) const {
Matrix tH;
Vector ret =(Vector(1) << (p.translation(tH).z() + bias - nT_)).finished();
if (H) (*H) = tH.block<1,6>(2,0);
if (H2) (*H2) = (Matrix(1, 1) << 1.0).finished(); if (H2) (*H2) = (Matrix(1, 1) << 1.0).finished();
if (H) (*H) = (Matrix(1, 6) << 0., 0., 0., 0., 0., 1.).finished(); return ret;
return (Vector(1) << (p.translation().z() + bias - nT_)).finished();
} }
} // namespace gtsam } // namespace gtsam