diff --git a/.cproject b/.cproject index ad66abfa1..abe692394 100644 --- a/.cproject +++ b/.cproject @@ -439,6 +439,14 @@ true true + + make + -j5 + testPoseRTV.run + true + true + true + make -j5 diff --git a/gtsam_unstable/dynamics/PoseRTV.cpp b/gtsam_unstable/dynamics/PoseRTV.cpp index 4ab4bcf1d..e78f5a62d 100644 --- a/gtsam_unstable/dynamics/PoseRTV.cpp +++ b/gtsam_unstable/dynamics/PoseRTV.cpp @@ -258,7 +258,7 @@ PoseRTV PoseRTV::transformed_from(const Pose3& trans, } if (Dtrans) { - *Dtrans = numericalDerivative22(transformed_from_, *this, trans, 1e-6); + *Dtrans = numericalDerivative22(transformed_from_, *this, trans, 1e-8); // // *Dtrans = zeros(9,6); // // directly affecting the pose diff --git a/gtsam_unstable/dynamics/tests/testPoseRTV.cpp b/gtsam_unstable/dynamics/tests/testPoseRTV.cpp index 381971502..c833ea116 100644 --- a/gtsam_unstable/dynamics/tests/testPoseRTV.cpp +++ b/gtsam_unstable/dynamics/tests/testPoseRTV.cpp @@ -232,8 +232,8 @@ TEST( testPoseRTV, transformed_from_1 ) { PoseRTV expected(transform.compose(start.pose()), transform.rotation().rotate(V)); EXPECT(assert_equal(expected, actual, tol)); - Matrix numDGlobal = numericalDerivative21(transformed_from_proxy, start, transform, 1e-8); - Matrix numDTrans = numericalDerivative22(transformed_from_proxy, start, transform, 1e-8); + Matrix numDGlobal = numericalDerivative21(transformed_from_proxy, start, transform, 1e-5); // At 1e-8, fails + Matrix numDTrans = numericalDerivative22(transformed_from_proxy, start, transform, 1e-8); // Sensitive to step size EXPECT(assert_equal(numDGlobal, actDGlobal, tol)); EXPECT(assert_equal(numDTrans, actDTrans, tol)); // FIXME: still needs analytic derivative } @@ -251,8 +251,8 @@ TEST( testPoseRTV, transformed_from_2 ) { PoseRTV expected(transform.compose(start.pose()), transform.rotation().rotate(V)); EXPECT(assert_equal(expected, actual, tol)); - Matrix numDGlobal = numericalDerivative21(transformed_from_proxy, start, transform, 1e-8); - Matrix numDTrans = numericalDerivative22(transformed_from_proxy, start, transform, 1e-8); + Matrix numDGlobal = numericalDerivative21(transformed_from_proxy, start, transform, 1e-5); // At 1e-8, fails + Matrix numDTrans = numericalDerivative22(transformed_from_proxy, start, transform, 1e-8); // Sensitive to step size EXPECT(assert_equal(numDGlobal, actDGlobal, tol)); EXPECT(assert_equal(numDTrans, actDTrans, tol)); // FIXME: still needs analytic derivative }