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