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

Also add jacobians to Rot3::RzRyRx for when quaternions are selected

release/4.3a0
Christian Berg 2020-08-31 18:49:18 +02:00
parent 62098fb584
commit 63327a7c9d
1 changed files with 17 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
gtsam/geometry/Rot3Q.cpp
View File

@ -67,7 +67,20 @@ namespace gtsam {
}
/* ************************************************************************* */
Rot3 Rot3::RzRyRx(double x, double y, double z) { return Rot3(
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())));