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::xyzrelease/4.3a0
parent
4566321a99
commit
ceeb1142ec
|
@ -171,6 +171,9 @@ Vector3 Rot3::xyz(OptionalJacobian<3, 3> H) const {
|
|||
|
||||
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());
|
||||
|
@ -266,6 +269,10 @@ pair<Matrix3, Vector3> RQ(const Matrix3& A, OptionalJacobian<3, 9> H) {
|
|||
const Matrix3 R = C * Qz;
|
||||
|
||||
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_d2 = [](double y, double x) { return -y / (x * x + y * y); };
|
||||
|
||||
|
|
|
@ -801,13 +801,35 @@ Vector3 RQ_proxy(Matrix3 const& R) {
|
|||
}
|
||||
|
||||
TEST(Rot3, RQ_derivative) {
|
||||
const auto aa = Vector3{1.0, 0.7, 0.8};
|
||||
const auto R = Rot3::Expmap(aa).matrix();
|
||||
const auto num = numericalDerivative11(RQ_proxy, R);
|
||||
Matrix39 calc;
|
||||
RQ(R, calc);
|
||||
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});
|
||||
|
||||
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));
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
Loading…
Reference in New Issue