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

testPoseRTV no longer fails when using quaternions - still should add full analytical derivatives, however.

release/4.3a0
Alex Cunningham 2012-06-06 12:44:06 +00:00
parent 3729b322fd
commit 74138787e8
3 changed files with 13 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
.cproject
View File

@ -439,6 +439,14 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testPoseRTV.run" path="build/gtsam_unstable/dynamics" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testPoseRTV.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testBTree.run" path="build/gtsam_unstable/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testBTree.run" path="build/gtsam_unstable/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>

2
gtsam_unstable/dynamics/PoseRTV.cpp
View File

@ -258,7 +258,7 @@ PoseRTV PoseRTV::transformed_from(const Pose3& trans,
} }
if (Dtrans) { if (Dtrans) {
*Dtrans = numericalDerivative22(transformed_from_, *this, trans, 1e-6); *Dtrans = numericalDerivative22(transformed_from_, *this, trans, 1e-8);
// //
// *Dtrans = zeros(9,6); // *Dtrans = zeros(9,6);
// // directly affecting the pose // // directly affecting the pose

8
gtsam_unstable/dynamics/tests/testPoseRTV.cpp
View File

@ -232,8 +232,8 @@ TEST( testPoseRTV, transformed_from_1 ) {
PoseRTV expected(transform.compose(start.pose()), transform.rotation().rotate(V)); PoseRTV expected(transform.compose(start.pose()), transform.rotation().rotate(V));
EXPECT(assert_equal(expected, actual, tol)); EXPECT(assert_equal(expected, actual, tol));
Matrix numDGlobal = numericalDerivative21(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); Matrix numDTrans = numericalDerivative22(transformed_from_proxy, start, transform, 1e-8); // Sensitive to step size
EXPECT(assert_equal(numDGlobal, actDGlobal, tol)); EXPECT(assert_equal(numDGlobal, actDGlobal, tol));
EXPECT(assert_equal(numDTrans, actDTrans, tol)); // FIXME: still needs analytic derivative 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)); PoseRTV expected(transform.compose(start.pose()), transform.rotation().rotate(V));
EXPECT(assert_equal(expected, actual, tol)); EXPECT(assert_equal(expected, actual, tol));
Matrix numDGlobal = numericalDerivative21(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); Matrix numDTrans = numericalDerivative22(transformed_from_proxy, start, transform, 1e-8); // Sensitive to step size
EXPECT(assert_equal(numDGlobal, actDGlobal, tol)); EXPECT(assert_equal(numDGlobal, actDGlobal, tol));
EXPECT(assert_equal(numDTrans, actDTrans, tol)); // FIXME: still needs analytic derivative EXPECT(assert_equal(numDTrans, actDTrans, tol)); // FIXME: still needs analytic derivative
} }