Merge pull request #520 from Eskilade/add_jacobians_Rot3_ypr
Added Jacobians for Rot3::xyz and related conversions to euler anglesrelease/4.3a0
Frank Dellaert
GitHub
commit
c45ba00a83
|
|
@ -158,21 +158,73 @@ Point3 Rot3::column(int index) const{
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector3 Rot3::xyz() const {
|
||||
Vector3 Rot3::xyz(OptionalJacobian<3, 3> H) const {
|
||||
Matrix3 I;Vector3 q;
|
||||
boost::tie(I,q)=RQ(matrix());
|
||||
if (H) {
|
||||
Matrix93 mH;
|
||||
const auto m = matrix();
|
||||
#ifdef GTSAM_USE_QUATERNIONS
|
||||
SO3{m}.vec(mH);
|
||||
#else
|
||||
rot_.vec(mH);
|
||||
#endif
|
||||
|
||||
Matrix39 qHm;
|
||||
boost::tie(I, q) = RQ(m, qHm);
|
||||
|
||||
// TODO : Explore whether this expression can be optimized as both
|
||||
// qHm and mH are super-sparse
|
||||
*H = qHm * mH;
|
||||
} else
|
||||
boost::tie(I, q) = RQ(matrix());
|
||||
return q;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector3 Rot3::ypr() const {
|
||||
Vector3 q = xyz();
|
||||
Vector3 Rot3::ypr(OptionalJacobian<3, 3> H) const {
|
||||
Vector3 q = xyz(H);
|
||||
if (H) H->row(0).swap(H->row(2));
|
||||
|
||||
return Vector3(q(2),q(1),q(0));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector3 Rot3::rpy() const {
|
||||
return xyz();
|
||||
Vector3 Rot3::rpy(OptionalJacobian<3, 3> H) const { return xyz(H); }
|
||||
|
||||
/* ************************************************************************* */
|
||||
double Rot3::roll(OptionalJacobian<1, 3> H) const {
|
||||
double r;
|
||||
if (H) {
|
||||
Matrix3 xyzH;
|
||||
r = xyz(xyzH)(0);
|
||||
*H = xyzH.row(0);
|
||||
} else
|
||||
r = xyz()(0);
|
||||
return r;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double Rot3::pitch(OptionalJacobian<1, 3> H) const {
|
||||
double p;
|
||||
if (H) {
|
||||
Matrix3 xyzH;
|
||||
p = xyz(xyzH)(1);
|
||||
*H = xyzH.row(1);
|
||||
} else
|
||||
p = xyz()(1);
|
||||
return p;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double Rot3::yaw(OptionalJacobian<1, 3> H) const {
|
||||
double y;
|
||||
if (H) {
|
||||
Matrix3 xyzH;
|
||||
y = xyz(xyzH)(2);
|
||||
*H = xyzH.row(2);
|
||||
} else
|
||||
y = xyz()(2);
|
||||
return y;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -203,21 +255,62 @@ Matrix3 Rot3::LogmapDerivative(const Vector3& x) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
pair<Matrix3, Vector3> RQ(const Matrix3& A) {
|
||||
pair<Matrix3, Vector3> RQ(const Matrix3& A, OptionalJacobian<3, 9> H) {
|
||||
const double x = -atan2(-A(2, 1), A(2, 2));
|
||||
const auto Qx = Rot3::Rx(-x).matrix();
|
||||
const Matrix3 B = A * Qx;
|
||||
|
||||
double x = -atan2(-A(2, 1), A(2, 2));
|
||||
Rot3 Qx = Rot3::Rx(-x);
|
||||
Matrix3 B = A * Qx.matrix();
|
||||
const double y = -atan2(B(2, 0), B(2, 2));
|
||||
const auto Qy = Rot3::Ry(-y).matrix();
|
||||
const Matrix3 C = B * Qy;
|
||||
|
||||
double y = -atan2(B(2, 0), B(2, 2));
|
||||
Rot3 Qy = Rot3::Ry(-y);
|
||||
Matrix3 C = B * Qy.matrix();
|
||||
const double z = -atan2(-C(1, 0), C(1, 1));
|
||||
const auto Qz = Rot3::Rz(-z).matrix();
|
||||
const Matrix3 R = C * Qz;
|
||||
|
||||
double z = -atan2(-C(1, 0), C(1, 1));
|
||||
Rot3 Qz = Rot3::Rz(-z);
|
||||
Matrix3 R = C * Qz.matrix();
|
||||
if (H) {
|
||||
if (std::abs(y - M_PI / 2) < 1e-2)
|
||||
throw std::runtime_error(
|
||||
"Rot3::RQ : Derivative undefined at singularity (gimbal lock)");
|
||||
|
||||
Vector xyz = Vector3(x, y, z);
|
||||
auto atan_d1 = [](double y, double x) { return x / (x * x + y * y); };
|
||||
auto atan_d2 = [](double y, double x) { return -y / (x * x + y * y); };
|
||||
|
||||
const auto sx = -Qx(2, 1), cx = Qx(1, 1);
|
||||
const auto sy = -Qy(0, 2), cy = Qy(0, 0);
|
||||
|
||||
*H = Matrix39::Zero();
|
||||
// First, calculate the derivate of x
|
||||
(*H)(0, 5) = atan_d1(A(2, 1), A(2, 2));
|
||||
(*H)(0, 8) = atan_d2(A(2, 1), A(2, 2));
|
||||
|
||||
// Next, calculate the derivate of y. We have
|
||||
// b20 = a20 and b22 = a21 * sx + a22 * cx
|
||||
(*H)(1, 2) = -atan_d1(B(2, 0), B(2, 2));
|
||||
const auto yHb22 = -atan_d2(B(2, 0), B(2, 2));
|
||||
(*H)(1, 5) = yHb22 * sx;
|
||||
(*H)(1, 8) = yHb22 * cx;
|
||||
|
||||
// Next, calculate the derivate of z. We have
|
||||
// c20 = a10 * cy + a11 * sx * sy + a12 * cx * sy
|
||||
// c22 = a11 * cx - a12 * sx
|
||||
const auto c10Hx = (A(1, 1) * cx - A(1, 2) * sx) * sy;
|
||||
const auto c10Hy = A(1, 2) * cx * cy + A(1, 1) * cy * sx - A(1, 0) * sy;
|
||||
Vector9 c10HA = c10Hx * H->row(0) + c10Hy * H->row(1);
|
||||
c10HA[1] = cy;
|
||||
c10HA[4] = sx * sy;
|
||||
c10HA[7] = cx * sy;
|
||||
|
||||
const auto c11Hx = -A(1, 2) * cx - A(1, 1) * sx;
|
||||
Vector9 c11HA = c11Hx * H->row(0);
|
||||
c11HA[4] = cx;
|
||||
c11HA[7] = -sx;
|
||||
|
||||
H->block<1, 9>(2, 0) =
|
||||
atan_d1(C(1, 0), C(1, 1)) * c10HA + atan_d2(C(1, 0), C(1, 1)) * c11HA;
|
||||
}
|
||||
|
||||
const auto xyz = Vector3(x, y, z);
|
||||
return make_pair(R, xyz);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -441,19 +441,19 @@ namespace gtsam {
|
|||
* Use RQ to calculate xyz angle representation
|
||||
* @return a vector containing x,y,z s.t. R = Rot3::RzRyRx(x,y,z)
|
||||
*/
|
||||
Vector3 xyz() const;
|
||||
Vector3 xyz(OptionalJacobian<3, 3> H = boost::none) const;
|
||||
|
||||
/**
|
||||
* Use RQ to calculate yaw-pitch-roll angle representation
|
||||
* @return a vector containing ypr s.t. R = Rot3::Ypr(y,p,r)
|
||||
*/
|
||||
Vector3 ypr() const;
|
||||
Vector3 ypr(OptionalJacobian<3, 3> H = boost::none) const;
|
||||
|
||||
/**
|
||||
* Use RQ to calculate roll-pitch-yaw angle representation
|
||||
* @return a vector containing rpy s.t. R = Rot3::Ypr(y,p,r)
|
||||
*/
|
||||
Vector3 rpy() const;
|
||||
Vector3 rpy(OptionalJacobian<3, 3> H = boost::none) const;
|
||||
|
||||
/**
|
||||
* Accessor to get to component of angle representations
|
||||
|
|
@ -461,7 +461,7 @@ namespace gtsam {
|
|||
* you should instead use xyz() or ypr()
|
||||
* TODO: make this more efficient
|
||||
*/
|
||||
inline double roll() const { return xyz()(0); }
|
||||
double roll(OptionalJacobian<1, 3> H = boost::none) const;
|
||||
|
||||
/**
|
||||
* Accessor to get to component of angle representations
|
||||
|
|
@ -469,7 +469,7 @@ namespace gtsam {
|
|||
* you should instead use xyz() or ypr()
|
||||
* TODO: make this more efficient
|
||||
*/
|
||||
inline double pitch() const { return xyz()(1); }
|
||||
double pitch(OptionalJacobian<1, 3> H = boost::none) const;
|
||||
|
||||
/**
|
||||
* Accessor to get to component of angle representations
|
||||
|
|
@ -477,7 +477,7 @@ namespace gtsam {
|
|||
* you should instead use xyz() or ypr()
|
||||
* TODO: make this more efficient
|
||||
*/
|
||||
inline double yaw() const { return xyz()(2); }
|
||||
double yaw(OptionalJacobian<1, 3> H = boost::none) const;
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Interface
|
||||
|
|
@ -557,7 +557,8 @@ namespace gtsam {
|
|||
* @return an upper triangular matrix R
|
||||
* @return a vector [thetax, thetay, thetaz] in radians.
|
||||
*/
|
||||
GTSAM_EXPORT std::pair<Matrix3,Vector3> RQ(const Matrix3& A);
|
||||
GTSAM_EXPORT std::pair<Matrix3, Vector3> RQ(
|
||||
const Matrix3& A, OptionalJacobian<3, 9> H = boost::none);
|
||||
|
||||
template<>
|
||||
struct traits<Rot3> : public internal::LieGroup<Rot3> {};
|
||||
|
|
|
|||
|
|
@ -794,6 +794,122 @@ TEST(Rot3, Ypr_derivative) {
|
|||
CHECK(assert_equal(num_r, act_r));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector3 RQ_proxy(Matrix3 const& R) {
|
||||
const auto RQ_ypr = RQ(R);
|
||||
return RQ_ypr.second;
|
||||
}
|
||||
|
||||
TEST(Rot3, RQ_derivative) {
|
||||
using VecAndErr = std::pair<Vector3, double>;
|
||||
std::vector<VecAndErr> test_xyz;
|
||||
// Test zeros and a couple of random values
|
||||
test_xyz.push_back(VecAndErr{{0, 0, 0}, error});
|
||||
test_xyz.push_back(VecAndErr{{0, 0.5, -0.5}, error});
|
||||
test_xyz.push_back(VecAndErr{{0.3, 0, 0.2}, error});
|
||||
test_xyz.push_back(VecAndErr{{-0.6, 1.3, 0}, error});
|
||||
test_xyz.push_back(VecAndErr{{1.0, 0.7, 0.8}, error});
|
||||
test_xyz.push_back(VecAndErr{{3.0, 0.7, -0.6}, error});
|
||||
test_xyz.push_back(VecAndErr{{M_PI / 2, 0, 0}, error});
|
||||
test_xyz.push_back(VecAndErr{{0, 0, M_PI / 2}, error});
|
||||
|
||||
// Test close to singularity
|
||||
test_xyz.push_back(VecAndErr{{0, M_PI / 2 - 1e-1, 0}, 1e-8});
|
||||
test_xyz.push_back(VecAndErr{{0, 3 * M_PI / 2 + 1e-1, 0}, 1e-8});
|
||||
test_xyz.push_back(VecAndErr{{0, M_PI / 2 - 1.1e-2, 0}, 1e-4});
|
||||
test_xyz.push_back(VecAndErr{{0, 3 * M_PI / 2 + 1.1e-2, 0}, 1e-4});
|
||||
|
||||
for (auto const& vec_err : test_xyz) {
|
||||
auto const& xyz = vec_err.first;
|
||||
|
||||
const auto R = Rot3::RzRyRx(xyz).matrix();
|
||||
const auto num = numericalDerivative11(RQ_proxy, R);
|
||||
Matrix39 calc;
|
||||
RQ(R, calc).second;
|
||||
|
||||
const auto err = vec_err.second;
|
||||
CHECK(assert_equal(num, calc, err));
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector3 xyz_proxy(Rot3 const& R) { return R.xyz(); }
|
||||
|
||||
TEST(Rot3, xyz_derivative) {
|
||||
const auto aa = Vector3{-0.6, 0.3, 0.2};
|
||||
const auto R = Rot3::Expmap(aa);
|
||||
const auto num = numericalDerivative11(xyz_proxy, R);
|
||||
Matrix3 calc;
|
||||
R.xyz(calc);
|
||||
|
||||
CHECK(assert_equal(num, calc));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector3 ypr_proxy(Rot3 const& R) { return R.ypr(); }
|
||||
|
||||
TEST(Rot3, ypr_derivative) {
|
||||
const auto aa = Vector3{0.1, -0.3, -0.2};
|
||||
const auto R = Rot3::Expmap(aa);
|
||||
const auto num = numericalDerivative11(ypr_proxy, R);
|
||||
Matrix3 calc;
|
||||
R.ypr(calc);
|
||||
|
||||
CHECK(assert_equal(num, calc));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector3 rpy_proxy(Rot3 const& R) { return R.rpy(); }
|
||||
|
||||
TEST(Rot3, rpy_derivative) {
|
||||
const auto aa = Vector3{1.2, 0.3, -0.9};
|
||||
const auto R = Rot3::Expmap(aa);
|
||||
const auto num = numericalDerivative11(rpy_proxy, R);
|
||||
Matrix3 calc;
|
||||
R.rpy(calc);
|
||||
|
||||
CHECK(assert_equal(num, calc));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double roll_proxy(Rot3 const& R) { return R.roll(); }
|
||||
|
||||
TEST(Rot3, roll_derivative) {
|
||||
const auto aa = Vector3{0.8, -0.8, 0.8};
|
||||
const auto R = Rot3::Expmap(aa);
|
||||
const auto num = numericalDerivative11(roll_proxy, R);
|
||||
Matrix13 calc;
|
||||
R.roll(calc);
|
||||
|
||||
CHECK(assert_equal(num, calc));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double pitch_proxy(Rot3 const& R) { return R.pitch(); }
|
||||
|
||||
TEST(Rot3, pitch_derivative) {
|
||||
const auto aa = Vector3{0.01, 0.1, 0.0};
|
||||
const auto R = Rot3::Expmap(aa);
|
||||
const auto num = numericalDerivative11(pitch_proxy, R);
|
||||
Matrix13 calc;
|
||||
R.pitch(calc);
|
||||
|
||||
CHECK(assert_equal(num, calc));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double yaw_proxy(Rot3 const& R) { return R.yaw(); }
|
||||
|
||||
TEST(Rot3, yaw_derivative) {
|
||||
const auto aa = Vector3{0.0, 0.1, 0.6};
|
||||
const auto R = Rot3::Expmap(aa);
|
||||
const auto num = numericalDerivative11(yaw_proxy, R);
|
||||
Matrix13 calc;
|
||||
R.yaw(calc);
|
||||
|
||||
CHECK(assert_equal(num, calc));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
|
|
|
|||
Loading…
Reference in New Issue