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

Revert "test for stability of RQ due to atan2 as well as fix" 

This reverts commit 1d5239dd25.
release/4.3a0
Varun Agrawal 2020-03-18 19:14:19 -04:00
parent 1d5239dd25
commit 7b6a80eba2
2 changed files with 4 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
gtsam/geometry/Rot3.cpp
View File

@ -197,27 +197,15 @@ Matrix3 Rot3::LogmapDerivative(const Vector3& x) {
/* ************************************************************************* */ /* ************************************************************************* */
pair<Matrix3, Vector3> RQ(const Matrix3& A) { pair<Matrix3, Vector3> RQ(const Matrix3& A) {
// atan2 has curious/unstable behavior near 0. double x = -atan2(-A(2, 1), A(2, 2));
// If either x or y is close to zero, the results can vary depending on
// what the sign of either x or y is.
// E.g. for x = 1e-15, y = -0.08, atan2(x, y) != atan2(-x, y),
// even though arithmetically, they are both atan2(0, y).
// Suppressing small numbers to 0.0 doesn't work since the floating point
// representation still causes the issue due to a delta difference.
// The only fix is to convert to an int so we are guaranteed the value is 0.
// This lambda function checks if a number is close to 0.
// If yes, then return the integer representation, else do nothing.
auto suppress = [](auto x) { return abs(x) > 1e-15 ? x : int(x); };
double x = -atan2(-suppress(A(2, 1)), suppress(A(2, 2)));
Rot3 Qx = Rot3::Rx(-x); Rot3 Qx = Rot3::Rx(-x);
Matrix3 B = A * Qx.matrix(); Matrix3 B = A * Qx.matrix();
double y = -atan2(suppress(B(2, 0)), suppress(B(2, 2))); double y = -atan2(B(2, 0), B(2, 2));
Rot3 Qy = Rot3::Ry(-y); Rot3 Qy = Rot3::Ry(-y);
Matrix3 C = B * Qy.matrix(); Matrix3 C = B * Qy.matrix();
double z = -atan2(-suppress(C(1, 0)), suppress(C(1, 1))); double z = -atan2(-C(1, 0), C(1, 1));
Rot3 Qz = Rot3::Rz(-z); Rot3 Qz = Rot3::Rz(-z);
Matrix3 R = C * Qz.matrix(); Matrix3 R = C * Qz.matrix();

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

@ -14,7 +14,6 @@
* @brief Unit tests for Rot3 class - common between Matrix and Quaternion * @brief Unit tests for Rot3 class - common between Matrix and Quaternion
* @author Alireza Fathi * @author Alireza Fathi
* @author Frank Dellaert * @author Frank Dellaert
* @author Varun Agrawal
*/ */
#include <gtsam/geometry/Point3.h> #include <gtsam/geometry/Point3.h>
@ -381,7 +380,7 @@ TEST( Rot3, inverse )
Rot3 actual = R.inverse(actualH); Rot3 actual = R.inverse(actualH);
CHECK(assert_equal(I,R*actual)); CHECK(assert_equal(I,R*actual));
CHECK(assert_equal(I,actual*R)); CHECK(assert_equal(I,actual*R));
CHECK(assert_equal(actual, Rot3(R.transpose()))); CHECK(assert_equal((Matrix)actual.matrix(), R.transpose()));
Matrix numericalH = numericalDerivative11(testing::inverse<Rot3>, R); Matrix numericalH = numericalDerivative11(testing::inverse<Rot3>, R);
CHECK(assert_equal(numericalH,actualH)); CHECK(assert_equal(numericalH,actualH));
@ -503,29 +502,6 @@ TEST( Rot3, RQ)
CHECK(assert_equal(expected,actual,1e-6)); CHECK(assert_equal(expected,actual,1e-6));
} }
/* ************************************************************************* */
TEST( Rot3, RQStability)
{
// Test case to check if xyz() is computed correctly when using quaternions.
Matrix actualR;
Vector actualXyz;
// A is equivalent to the below
double kDegree = M_PI / 180;
const Rot3 nRy = Rot3::Yaw(315 * kDegree);
const Rot3 yRp = Rot3::Pitch(275 * kDegree);
const Rot3 pRb = Rot3::Roll(180 * kDegree);
const Rot3 nRb = nRy * yRp * pRb;
// A(2, 1) ~= -0.0
// Since A(2, 2) < 0, x-angle should be positive
Matrix A = nRb.matrix();
boost::tie(actualR, actualXyz) = RQ(A);
Vector3 expectedXyz(3.14159, -1.48353, -0.785398);
CHECK(assert_equal(expectedXyz,actualXyz,1e-6));
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Rot3, expmapStability ) { TEST( Rot3, expmapStability ) {
Vector w = Vector3(78e-9, 5e-8, 97e-7); Vector w = Vector3(78e-9, 5e-8, 97e-7);