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

new Pose3 derivatives still don't work :-(

release/4.3a0
Frank Dellaert 2010-03-01 01:35:33 +00:00
parent b943644ddd
commit 20ad08e48f
2 changed files with 15 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
cpp/Pose3.cpp
View File

@ -113,13 +113,14 @@ namespace gtsam {
/* ************************************************************************* */
Matrix Dtransform_from1(const Pose3& pose, const Point3& p) {
#ifdef NEW_EXMAP
Point3 q = transform_from(pose,p);
Matrix DR = skewSymmetric(-q.x(), -q.y(), -q.z());
#ifdef SLOW_BUT_CORRECT_EXPMAP
const Matrix R = pose.rotation().matrix();
Matrix DR = R*skewSymmetric(-p.x(), -p.y(), -p.z());
return collect(2,&DR,&R);
#else
Matrix DR = Drotate1(pose.rotation(), p);
#endif
return collect(2,&DR,&I3);
#endif
}
/* ************************************************************************* */

20
cpp/testPose3.cpp
View File

@ -105,26 +105,26 @@ TEST( Pose3, compose_inverse)
/* ************************************************************************* */
TEST( Pose3, Dtransform_from1_a)
{
Matrix computed = Dtransform_from1(T, P);
Matrix actualDtransform_from1 = Dtransform_from1(T, P);
Matrix numerical = numericalDerivative21(transform_from,T,P);
CHECK(assert_equal(numerical,computed,error));
CHECK(assert_equal(numerical,actualDtransform_from1,error));
}
TEST( Pose3, Dtransform_from1_b)
{
Pose3 origin;
Matrix computed = Dtransform_from1(origin, P);
Matrix actualDtransform_from1 = Dtransform_from1(origin, P);
Matrix numerical = numericalDerivative21(transform_from,origin,P);
CHECK(assert_equal(numerical,computed,error));
CHECK(assert_equal(numerical,actualDtransform_from1,error));
}
TEST( Pose3, Dtransform_from1_c)
{
Point3 origin;
Pose3 T0(R,origin);
Matrix computed = Dtransform_from1(T0, P);
Matrix actualDtransform_from1 = Dtransform_from1(T0, P);
Matrix numerical = numericalDerivative21(transform_from,T0,P);
CHECK(assert_equal(numerical,computed,error));
CHECK(assert_equal(numerical,actualDtransform_from1,error));
}
TEST( Pose3, Dtransform_from1_d)
@ -132,19 +132,19 @@ TEST( Pose3, Dtransform_from1_d)
Rot3 I;
Point3 t0(100,0,0);
Pose3 T0(I,t0);
Matrix computed = Dtransform_from1(T0, P);
Matrix actualDtransform_from1 = Dtransform_from1(T0, P);
//print(computed, "Dtransform_from1_d computed:");
Matrix numerical = numericalDerivative21(transform_from,T0,P);
//print(numerical, "Dtransform_from1_d numerical:");
CHECK(assert_equal(numerical,computed,error));
CHECK(assert_equal(numerical,actualDtransform_from1,error));
}
/* ************************************************************************* */
TEST( Pose3, Dtransform_from2)
{
Matrix computed = Dtransform_from2(T);
Matrix actualDtransform_from2 = Dtransform_from2(T);
Matrix numerical = numericalDerivative22(transform_from,T,P);
CHECK(assert_equal(numerical,computed,error));
CHECK(assert_equal(numerical,actualDtransform_from2,error));
}
/* ************************************************************************* */