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

Throw exception in Rot3::RQ for derivatives close to singularity 

Also added more tests and a TODO to investigate a more efficient method of
getting the jacobian of Rot3::xyz
release/4.3a0
Christian Berg 2020-09-15 20:53:59 +02:00
parent 4566321a99
commit ceeb1142ec
2 changed files with 35 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
gtsam/geometry/Rot3.cpp
View File

@ -171,6 +171,9 @@ Vector3 Rot3::xyz(OptionalJacobian<3, 3> H) const {
Matrix39 qHm; Matrix39 qHm;
boost::tie(I, q) = RQ(m, 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; *H = qHm * mH;
} else } else
boost::tie(I, q) = RQ(matrix()); boost::tie(I, q) = RQ(matrix());
@ -266,6 +269,10 @@ pair<Matrix3, Vector3> RQ(const Matrix3& A, OptionalJacobian<3, 9> H) {
const Matrix3 R = C * Qz; const Matrix3 R = C * Qz;
if (H) { if (H) {
if (std::abs(y - M_PI / 2) < 1e-2)
throw std::runtime_error(
"Rot3::RQ : Derivative undefined at singularity (gimbal lock)");
auto atan_d1 = [](double y, double x) { return x / (x * x + y * y); }; 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); }; auto atan_d2 = [](double y, double x) { return -y / (x * x + y * y); };

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

@ -801,13 +801,35 @@ Vector3 RQ_proxy(Matrix3 const& R) {
} }
TEST(Rot3, RQ_derivative) { TEST(Rot3, RQ_derivative) {
const auto aa = Vector3{1.0, 0.7, 0.8}; using VecAndErr = std::pair<Vector3, double>;
const auto R = Rot3::Expmap(aa).matrix(); std::vector<VecAndErr> test_xyz;
const auto num = numericalDerivative11(RQ_proxy, R); // Test zeros and a couple of random values
Matrix39 calc; test_xyz.push_back(VecAndErr{{0, 0, 0}, error});
RQ(R, calc); 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});
CHECK(assert_equal(num, calc)); // 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));
}
} }
/* ************************************************************************* */ /* ************************************************************************* */