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

Fix tolerance for ubuntu quaternion cases

release/4.3a0
Frank Dellaert 2024-08-26 15:08:04 -07:00
parent bfa967c2bc
commit 475b37f7d6
2 changed files with 7 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/geometry/tests/testPose3.cpp
View File

@ -1223,7 +1223,7 @@ TEST(Pose3, ExpmapChainRule) {
// Test the derivatives at zero
const Matrix6 expected = numericalDerivative11<Pose3, Vector6>(g, Z_6x1);
EXPECT(assert_equal<Matrix6>(expected, M)); // Pose3::ExpmapDerivative(Z_6x1) is identity
EXPECT(assert_equal<Matrix6>(expected, M, 1e-5)); // Pose3::ExpmapDerivative(Z_6x1) is identity
// Test the derivatives at another value
const Vector6 delta{0.1, 0.2, 0.3, 0.4, 0.5, 0.6};

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

@ -958,7 +958,7 @@ TEST(Rot3, determinant) {
/* ************************************************************************* */
TEST(Rot3, ExpmapChainRule) {
// Muliply with an arbitrary matrix and exponentiate
// Multiply with an arbitrary matrix and exponentiate
Matrix3 M;
M << 1, 2, 3, 4, 5, 6, 7, 8, 9;
auto g = [&](const Vector3& omega) {
@ -967,17 +967,17 @@ TEST(Rot3, ExpmapChainRule) {
// Test the derivatives at zero
const Matrix3 expected = numericalDerivative11<Rot3, Vector3>(g, Z_3x1);
EXPECT(assert_equal<Matrix3>(expected, M)); // SO3::ExpmapDerivative(Z_3x1) is identity
EXPECT(assert_equal<Matrix3>(expected, M, 1e-5)); // SO3::ExpmapDerivative(Z_3x1) is identity
// Test the derivatives at another value
const Vector3 delta{0.1,0.2,0.3};
const Matrix3 expected2 = numericalDerivative11<Rot3, Vector3>(g, delta);
EXPECT(assert_equal<Matrix3>(expected2, SO3::ExpmapDerivative(M*delta) * M));
EXPECT(assert_equal<Matrix3>(expected2, SO3::ExpmapDerivative(M*delta) * M, 1e-5));
}
/* ************************************************************************* */
TEST(Rot3, expmapChainRule) {
// Muliply an arbitrary rotation with exp(M*x)
// Multiply an arbitrary rotation with exp(M*x)
// Perhaps counter-intuitively, this has the same derivatives as above
Matrix3 M;
M << 1, 2, 3, 4, 5, 6, 7, 8, 9;
@ -988,12 +988,12 @@ TEST(Rot3, expmapChainRule) {
// Test the derivatives at zero
const Matrix3 expected = numericalDerivative11<Rot3, Vector3>(g, Z_3x1);
EXPECT(assert_equal<Matrix3>(expected, M));
EXPECT(assert_equal<Matrix3>(expected, M, 1e-5));
// Test the derivatives at another value
const Vector3 delta{0.1,0.2,0.3};
const Matrix3 expected2 = numericalDerivative11<Rot3, Vector3>(g, delta);
EXPECT(assert_equal<Matrix3>(expected2, SO3::ExpmapDerivative(M*delta) * M));
EXPECT(assert_equal<Matrix3>(expected2, SO3::ExpmapDerivative(M*delta) * M, 1e-5));
}
/* ************************************************************************* */