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

Fixed Dbetween1

release/4.3a0
Frank Dellaert 2010-01-08 03:06:03 +00:00
parent 19a3e228d7
commit f684becf1f
2 changed files with 6 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cpp/Pose3.cpp
View File

@ -174,7 +174,7 @@ namespace gtsam {
Matrix Dbetween1(const Pose3& p1, const Pose3& p2){
Pose3 invp1 = inverse(p1);
return Dinverse(p1) * Dcompose2(p2,invp1);
return Dcompose2(p2,invp1) * Dinverse(p1);
}
Matrix Dbetween2(const Pose3& p1, const Pose3& p2){

6
cpp/testPose3.cpp
View File

@ -283,13 +283,17 @@ TEST(Pose3, manifold) {
/* ************************************************************************* */
TEST( Pose3, between )
{
Rot3 R = rodriguez(0.3,0.2,0.1);
Point3 t(3.5,-8.2,4.2);
Pose3 T(R,t);
Pose3 expected = pose1 * inverse(T);
Pose3 actual = between(T, pose1);
CHECK(assert_equal(expected,actual));
Matrix numericalH1 = numericalDerivative21(between<Pose3> , T, pose1, 1e-5);
Matrix actualH1 = Dbetween1(T, pose1);
// CHECK(assert_equal(numericalH1,actualH1)); // chain rule does not work ??
CHECK(assert_equal(numericalH1,actualH1)); // chain rule does not work ??
Matrix actualH2 = Dbetween2(T, pose1);
Matrix numericalH2 = numericalDerivative22(between<Pose3> , T, pose1, 1e-5);