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

Merge pull request #499 from Eskilade/add_Rot3_RzRyRx_jacobian 

Added Jacobians for Rot3::RzRyRx and related named constructors
release/4.3a0
Frank Dellaert 2020-08-31 15:14:43 -04:00 committed by GitHub
parent 39aeae657e 63327a7c9d
commit 64a6e90b90
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
4 changed files with 93 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
gtsam/geometry/Rot3.h
View File

@ -154,12 +154,23 @@ namespace gtsam {
static Rot3 Rz(double t);
/// Rotations around Z, Y, then X axes as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis.
static Rot3 RzRyRx(double x, double y, double z);
static Rot3 RzRyRx(double x, double y, double z,
OptionalJacobian<3, 1> Hx = boost::none,
OptionalJacobian<3, 1> Hy = boost::none,
OptionalJacobian<3, 1> Hz = boost::none);
/// Rotations around Z, Y, then X axes as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis.
inline static Rot3 RzRyRx(const Vector& xyz) {
inline static Rot3 RzRyRx(const Vector& xyz,
OptionalJacobian<3, 3> H = boost::none) {
assert(xyz.size() == 3);
return RzRyRx(xyz(0), xyz(1), xyz(2));
Rot3 out;
if (H) {
Vector3 Hx, Hy, Hz;
out = RzRyRx(xyz(0), xyz(1), xyz(2), Hx, Hy, Hz);
(*H) << Hx, Hy, Hz;
} else
out = RzRyRx(xyz(0), xyz(1), xyz(2));
return out;
}
/// Positive yaw is to right (as in aircraft heading). See ypr
@ -185,7 +196,12 @@ namespace gtsam {
* Positive pitch is down (decreasing aircraft altitude).
* Positive roll is to right (decreasing yaw in aircraft).
*/
static Rot3 Ypr(double y, double p, double r) { return RzRyRx(r,p,y);}
static Rot3 Ypr(double y, double p, double r,
OptionalJacobian<3, 1> Hy = boost::none,
OptionalJacobian<3, 1> Hp = boost::none,
OptionalJacobian<3, 1> Hr = boost::none) {
return RzRyRx(r, p, y, Hr, Hp, Hy);
}
/** Create from Quaternion coefficients */
static Rot3 Quaternion(double w, double x, double y, double z) {

6
gtsam/geometry/Rot3M.cpp
View File

@ -82,7 +82,8 @@ Rot3 Rot3::Rz(double t) {
/* ************************************************************************* */
// Considerably faster than composing matrices above !
Rot3 Rot3::RzRyRx(double x, double y, double z) {
Rot3 Rot3::RzRyRx(double x, double y, double z, OptionalJacobian<3, 1> Hx,
OptionalJacobian<3, 1> Hy, OptionalJacobian<3, 1> Hz) {
double cx=cos(x),sx=sin(x);
double cy=cos(y),sy=sin(y);
double cz=cos(z),sz=sin(z);
@ -97,6 +98,9 @@ Rot3 Rot3::RzRyRx(double x, double y, double z) {
double s_c = sx * cz;
double c_c = cx * cz;
double ssc = ss_ * cz, csc = cs_ * cz, sss = ss_ * sz, css = cs_ * sz;
if (Hx) (*Hx) << 1, 0, 0;
if (Hy) (*Hy) << 0, cx, -sx;
if (Hz) (*Hz) << -sy, sc_, cc_;
return Rot3(
_cc,- c_s + ssc, s_s + csc,
_cs, c_c + sss, -s_c + css,

21
gtsam/geometry/Rot3Q.cpp
View File

@ -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())));
}
/* ************************************************************************* */

51
gtsam/geometry/tests/testRot3.cpp
View File

@ -743,6 +743,57 @@ TEST(Rot3, axisAngle) {
CHECK_AXIS_ANGLE(_axis, theta165, Rot3::AxisAngle(axis, theta195))
}
/* ************************************************************************* */
Rot3 RzRyRx_proxy(double const& a, double const& b, double const& c) {
return Rot3::RzRyRx(a, b, c);
}
TEST(Rot3, RzRyRx_scalars_derivative) {
const auto x = 0.1, y = 0.4, z = 0.2;
const auto num_x = numericalDerivative31(RzRyRx_proxy, x, y, z);
const auto num_y = numericalDerivative32(RzRyRx_proxy, x, y, z);
const auto num_z = numericalDerivative33(RzRyRx_proxy, x, y, z);
Vector3 act_x, act_y, act_z;
Rot3::RzRyRx(x, y, z, act_x, act_y, act_z);
CHECK(assert_equal(num_x, act_x));
CHECK(assert_equal(num_y, act_y));
CHECK(assert_equal(num_z, act_z));
}
/* ************************************************************************* */
Rot3 RzRyRx_proxy(Vector3 const& xyz) { return Rot3::RzRyRx(xyz); }
TEST(Rot3, RzRyRx_vector_derivative) {
const auto xyz = Vector3{-0.3, 0.1, 0.7};
const auto num = numericalDerivative11(RzRyRx_proxy, xyz);
Matrix3 act;
Rot3::RzRyRx(xyz, act);
CHECK(assert_equal(num, act));
}
/* ************************************************************************* */
Rot3 Ypr_proxy(double const& y, double const& p, double const& r) {
return Rot3::Ypr(y, p, r);
}
TEST(Rot3, Ypr_derivative) {
const auto y = 0.7, p = -0.3, r = 0.1;
const auto num_y = numericalDerivative31(Ypr_proxy, y, p, r);
const auto num_p = numericalDerivative32(Ypr_proxy, y, p, r);
const auto num_r = numericalDerivative33(Ypr_proxy, y, p, r);
Vector3 act_y, act_p, act_r;
Rot3::Ypr(y, p, r, act_y, act_p, act_r);
CHECK(assert_equal(num_y, act_y));
CHECK(assert_equal(num_p, act_p));
CHECK(assert_equal(num_r, act_r));
}
/* ************************************************************************* */
int main() {
TestResult tr;