Also add jacobians to Rot3::RzRyRx for when quaternions are selected
parent
62098fb584
commit
63327a7c9d
|
@ -67,10 +67,23 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 Rot3::RzRyRx(double x, double y, double z) { return Rot3(
|
||||
gtsam::Quaternion(Eigen::AngleAxisd(z, Eigen::Vector3d::UnitZ())) *
|
||||
gtsam::Quaternion(Eigen::AngleAxisd(y, Eigen::Vector3d::UnitY())) *
|
||||
gtsam::Quaternion(Eigen::AngleAxisd(x, Eigen::Vector3d::UnitX())));
|
||||
Rot3 Rot3::RzRyRx(double x, double y, double z, OptionalJacobian<3, 1> Hx,
|
||||
OptionalJacobian<3, 1> Hy, OptionalJacobian<3, 1> Hz) {
|
||||
if (Hx) (*Hx) << 1, 0, 0;
|
||||
|
||||
if (Hy or Hz) {
|
||||
const auto cx = cos(x), sx = sin(x);
|
||||
if (Hy) (*Hy) << 0, cx, -sx;
|
||||
if (Hz) {
|
||||
const auto cy = cos(y), sy = sin(y);
|
||||
(*Hz) << -sy, sx * cy, cx * cy;
|
||||
}
|
||||
}
|
||||
|
||||
return Rot3(
|
||||
gtsam::Quaternion(Eigen::AngleAxisd(z, Eigen::Vector3d::UnitZ())) *
|
||||
gtsam::Quaternion(Eigen::AngleAxisd(y, Eigen::Vector3d::UnitY())) *
|
||||
gtsam::Quaternion(Eigen::AngleAxisd(x, Eigen::Vector3d::UnitX())));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
Loading…
Reference in New Issue