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

Made all core geometry functions that have derivatives use the combined derivative form instead of separate derivative functions.

release/4.3a0
Alex Cunningham 2010-08-20 15:17:13 +00:00
parent 9367170fe5
commit 8c33168fb3
13 changed files with 180 additions and 230 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
geometry/Point2.h
View File

@ -101,14 +101,6 @@ namespace gtsam {
if(H2) *H2 = eye(2);
return compose(p1, p2);
}
inline Matrix Dcompose1(const Point2& p1, const Point2& p0) {
return Matrix_(2,2,
1.0, 0.0,
0.0, 1.0); }
inline Matrix Dcompose2(const Point2& p1, const Point2& p0) {
return Matrix_(2,2,
1.0, 0.0,
0.0, 1.0); }
/** "Between", subtracts point coordinates */
inline Point2 between(const Point2& p1, const Point2& p2) { return p2-p1; }

22
geometry/Point3.cpp
View File

@ -52,24 +52,22 @@ namespace gtsam {
return p+q;
}
/* ************************************************************************* */
Matrix Dadd1(const Point3 &p, const Point3 &q) {
return eye(3,3);
}
/* ************************************************************************* */
Matrix Dadd2(const Point3 &p, const Point3 &q) {
return eye(3,3);
Point3 add(const Point3 &p, const Point3 &q,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
if (H1) *H1 = eye(3,3);
if (H2) *H2 = eye(3,3);
return add(p,q);
}
/* ************************************************************************* */
Point3 sub(const Point3 &p, const Point3 &q) {
return p+q;
}
/* ************************************************************************* */
Matrix Dsub1(const Point3 &p, const Point3 &q) {
return eye(3,3);
}
/* ************************************************************************* */
Matrix Dsub2(const Point3 &p, const Point3 &q) {
return -eye(3,3);
Point3 sub(const Point3 &p, const Point3 &q,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
if (H1) *H1 = eye(3,3);
if (H2) *H2 = -eye(3,3);
return sub(p,q);
}
/* ************************************************************************* */
Point3 cross(const Point3 &p, const Point3 &q)

23
geometry/Point3.h
View File

@ -102,17 +102,10 @@ namespace gtsam {
};
/** "Compose" - just adds coordinates of two points */
inline Matrix Dcompose1(const Point3& p1, const Point3& p0) {
return Matrix_(3,3,
1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0);
}
inline Matrix Dcompose2(const Point3& p1, const Point3& p0) {
return Matrix_(3,3,
1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0);
inline Point3 compose(const Point3& p1, const Point3& p2, boost::optional<Matrix&> H1, boost::optional<Matrix&> H2=boost::none) {
if (H1) *H1 = eye(3);
if (H2) *H2 = eye(3);
return p1.compose(p2);
}
/** Syntactic sugar for multiplying coordinates by a scalar s*p */
@ -120,13 +113,13 @@ namespace gtsam {
/** add two points, add(p,q) is same as p+q */
Point3 add (const Point3 &p, const Point3 &q);
Matrix Dadd1(const Point3 &p, const Point3 &q);
Matrix Dadd2(const Point3 &p, const Point3 &q);
Point3 add (const Point3 &p, const Point3 &q,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2=boost::none);
/** subtract two points, sub(p,q) is same as p-q */
Point3 sub (const Point3 &p, const Point3 &q);
Matrix Dsub1(const Point3 &p, const Point3 &q);
Matrix Dsub2(const Point3 &p, const Point3 &q);
Point3 sub (const Point3 &p, const Point3 &q,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2=boost::none);
/** cross product */
Point3 cross(const Point3 &p, const Point3 &q);

13
geometry/Pose2.cpp
View File

@ -101,8 +101,9 @@ namespace gtsam {
return Pose2(R.inverse(), R.unrotate(Point2(-t.x(), -t.y())));
}
Matrix Dinverse(const Pose2& pose) {
return -AdjointMap(pose);
Pose2 inverse(const Pose2& pose, boost::optional<Matrix&> H1) {
if (H1) *H1 = -AdjointMap(pose);
return pose.inverse();
}
/* ************************************************************************* */
@ -131,14 +132,6 @@ namespace gtsam {
return p1*p2;
}
Matrix Dcompose1(const Pose2& p1, const Pose2& p2) {
return AdjointMap(inverse(p2));
}
Matrix Dcompose2(const Pose2& p1, const Pose2& p2) {
return I3;
}
/* ************************************************************************* */
// see doc/math.lyx, SE(2) section
Point2 transform_from(const Pose2& pose, const Point2& p,

4
geometry/Pose2.h
View File

@ -141,7 +141,7 @@ namespace gtsam {
/**
* inverse transformation
*/
Matrix Dinverse(const Pose2& pose);
Pose2 inverse(const Pose2& pose, boost::optional<Matrix&> H1);
/**
* compose this transformation onto another (first p1 and then p2)
@ -149,8 +149,6 @@ namespace gtsam {
Pose2 compose(const Pose2& p1, const Pose2& p2,
boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2 = boost::none);
Matrix Dcompose1(const Pose2& p1, const Pose2& p2);
Matrix Dcompose2(const Pose2& p1, const Pose2& p2);
/**
* Return point coordinates in pose coordinate frame

107
geometry/Pose3.cpp
View File

@ -146,26 +146,19 @@ namespace gtsam {
/* ************************************************************************* */
Point3 transform_from(const Pose3& pose, const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
if (H1) *H1 = Dtransform_from1(pose, p);
if (H2) *H2 = Dtransform_from2(pose);
return transform_from(pose, p);
}
/* ************************************************************************* */
Matrix Dtransform_from1(const Pose3& pose, const Point3& p) {
if (H1) {
#ifdef CORRECT_POSE3_EXMAP
const Matrix R = pose.rotation().matrix();
Matrix DR = R*skewSymmetric(-p.x(), -p.y(), -p.z());
return collect(2,&DR,&R);
const Matrix R = pose.rotation().matrix();
Matrix DR = R*skewSymmetric(-p.x(), -p.y(), -p.z());
*H1 = collect(2,&DR,&R);
#else
Matrix DR = Drotate1(pose.rotation(), p);
return collect(2,&DR,&I3);
Matrix DR;
rotate(pose.rotation(), p, DR, boost::none);
*H1 = collect(2,&DR,&I3);
#endif
}
/* ************************************************************************* */
Matrix Dtransform_from2(const Pose3& pose) {
return pose.rotation().matrix();
}
if (H2) *H2 = pose.rotation().matrix();
return transform_from(pose, p);
}
/* ************************************************************************* */
@ -177,63 +170,58 @@ namespace gtsam {
/* ************************************************************************* */
Point3 transform_to(const Pose3& pose, const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
if (H1) *H1 = Dtransform_to1(pose, p);
if (H2) *H2 = Dtransform_to2(pose, p);
if (H1) { // *H1 = Dtransform_to1(pose, p);
Point3 q = transform_to(pose,p);
Matrix DR = skewSymmetric(q.x(), q.y(), q.z());
#ifdef CORRECT_POSE3_EXMAP
*H1 = collect(2, &DR, &_I3);
#else
Matrix DT = - pose.rotation().transpose(); // negative because of sub
*H1 = collect(2,&DR,&DT);
#endif
}
if (H2) *H2 = pose.rotation().transpose();
return transform_to(pose, p);
}
/* ************************************************************************* */
// see math.lyx, derivative of action
Matrix Dtransform_to1(const Pose3& pose, const Point3& p) {
Point3 q = transform_to(pose,p);
Matrix DR = skewSymmetric(q.x(), q.y(), q.z());
Pose3 compose(const Pose3& p1, const Pose3& p2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
if (H1) {
#ifdef CORRECT_POSE3_EXMAP
return collect(2, &DR, &_I3);
#else
Matrix DT = - pose.rotation().transpose(); // negative because of sub
return collect(2,&DR,&DT);
#endif
}
/* ************************************************************************* */
Matrix Dtransform_to2(const Pose3& pose, const Point3& p) {
return pose.rotation().transpose();
}
/* ************************************************************************* */
// compose = Pose3(compose(R1,R2),transform_from(p1,t2)
Matrix Dcompose1(const Pose3& p1, const Pose3& p2) {
#ifdef CORRECT_POSE3_EXMAP
return AdjointMap(inverse(p2));
*H1 = AdjointMap(inverse(p2));
#else
const Rot3& R2 = p2.rotation();
const Point3& t2 = p2.translation();
Matrix DR_R1 = R2.transpose(), DR_t1 = Z3;
Matrix DR_R1 = R2.transpose(), DR_t1 = Z3;
Matrix DR = collect(2, &DR_R1, &DR_t1);
Matrix Dt = Dtransform_from1(p1, t2);
return gtsam::stack(2, &DR, &Dt);
Matrix Dt;
transform_from(p1,t2, Dt, boost::none);
*H1 = gtsam::stack(2, &DR, &Dt);
#endif
}
Matrix Dcompose2(const Pose3& p1, const Pose3& p2) {
}
if (H2) {
#ifdef CORRECT_POSE3_EXMAP
return I6;
*H2 = I6;
#else
Matrix R1 = p1.rotation().matrix();
Matrix DR = collect(2, &I3, &Z3);
Matrix Dt = collect(2, &Z3, &R1);
return gtsam::stack(2, &DR, &Dt);
*H2 = gtsam::stack(2, &DR, &Dt);
#endif
}
}
return p1.compose(p2);
}
/* ************************************************************************* */
// inverse = Pose3(inverse(R),-unrotate(R,t));
// TODO: combined function will save !
Matrix Dinverse(const Pose3& p) {
Pose3 inverse(const Pose3& p, boost::optional<Matrix&> H1) {
if (H1)
#ifdef CORRECT_POSE3_EXMAP
return - AdjointMap(p);
{ *H1 = - AdjointMap(p); }
#else
{
const Rot3& R = p.rotation();
const Point3& t = p.translation();
Matrix Rt = R.transpose();
@ -241,18 +229,21 @@ namespace gtsam {
Matrix Dt_R1 = -skewSymmetric(unrotate(R,t).vector()), Dt_t1 = -Rt;
Matrix DR = collect(2, &DR_R1, &DR_t1);
Matrix Dt = collect(2, &Dt_R1, &Dt_t1);
return gtsam::stack(2, &DR, &Dt);
*H1 = gtsam::stack(2, &DR, &Dt);
}
#endif
return p.inverse();
}
/* ************************************************************************* */
// between = compose(p2,inverse(p1));
Pose3 between(const Pose3& p1, const Pose3& p2, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) {
Pose3 invp1 = inverse(p1);
Pose3 result = compose(invp1, p2);
if (H1) *H1 = Dcompose1(invp1, p2) * Dinverse(p1);
if (H2) *H2 = Dcompose2(invp1, p2);
Matrix invH;
Pose3 invp1 = inverse(p1, invH);
Matrix composeH1;
Pose3 result = compose(invp1, p2, composeH1, H2);
if (H1) *H1 = composeH1 * invH;
return result;
}

13
geometry/Pose3.h
View File

@ -116,27 +116,20 @@ namespace gtsam {
Point3 transform_from(const Pose3& pose, const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
// Older transform functions
Matrix Dtransform_from1(const Pose3& pose, const Point3& p);
Matrix Dtransform_from2(const Pose3& pose); // does not depend on p !
/** receives the point in world coordinates and transforms it to Pose coordinates */
Point3 transform_to(const Pose3& pose, const Point3& p);
Point3 transform_to(const Pose3& pose, const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
Matrix Dtransform_to1(const Pose3& pose, const Point3& p);
Matrix Dtransform_to2(const Pose3& pose, const Point3& p);
/**
* Derivatives of compose
*/
Matrix Dcompose1(const Pose3& p1, const Pose3& p2);
Matrix Dcompose2(const Pose3& p1, const Pose3& p2);
Pose3 compose(const Pose3& p1, const Pose3& p2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
/**
* Derivative of inverse
*/
Matrix Dinverse(const Pose3& p);
Pose3 inverse(const Pose3& p, boost::optional<Matrix&> H1);
/**
* Return relative pose between p1 and p2, in p1 coordinate frame

36
geometry/Rot3.cpp
View File

@ -166,13 +166,11 @@ namespace gtsam {
}
/* ************************************************************************* */
Matrix Drotate1(const Rot3& R, const Point3& p) {
return R.matrix() * skewSymmetric(-p.x(), -p.y(), -p.z());
}
/* ************************************************************************* */
Matrix Drotate2(const Rot3& R) {
return R.matrix();
Point3 rotate(const Rot3& R, const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
if (H1) *H1 = R.matrix() * skewSymmetric(-p.x(), -p.y(), -p.z());
if (H2) *H2 = R.matrix();
return rotate(R,p);
}
/* ************************************************************************* */
@ -193,23 +191,19 @@ namespace gtsam {
}
/* ************************************************************************* */
Matrix Dcompose1(const Rot3& R1, const Rot3& R2){
return R2.transpose();
Rot3 compose (const Rot3& R1, const Rot3& R2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
if (H1) *H1 = R2.transpose();
if (H2) *H2 = I3;
return R1.compose(R2);
}
/* ************************************************************************* */
Matrix Dcompose2(const Rot3& R1, const Rot3& R2){
return I3;
}
/* ************************************************************************* */
Matrix Dbetween1(const Rot3& R1, const Rot3& R2){
return -(R2.transpose()*R1.matrix());
}
/* ************************************************************************* */
Matrix Dbetween2(const Rot3& R1, const Rot3& R2){
return I3;
Rot3 between (const Rot3& R1, const Rot3& R2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
if (H1) *H1 = -(R2.transpose()*R1.matrix());
if (H2) *H2 = I3;
return between(R1, R2);
}
/* ************************************************************************* */

21
geometry/Rot3.h
View File

@ -143,7 +143,7 @@ namespace gtsam {
inline size_t dim() const { return dimension; }
/** Compose two rotations */
inline Rot3 compose(const Rot3& R1) const { return *this * R1;}
inline Rot3 compose(const Rot3& R2) const { return *this * R2;}
/** Exponential map at identity - create a rotation from canonical coordinates
* using Rodriguez' formula
@ -190,15 +190,18 @@ namespace gtsam {
};
// derivative of inverse rotation R^T s.t. inverse(R)*R = Rot3()
inline Matrix Dinverse(Rot3 R) { return -R.matrix();}
inline Rot3 inverse(const Rot3& R, boost::optional<Matrix&> H1) {
if (H1) *H1 = -R.matrix();
return R.inverse();
}
/**
* rotate point from rotated coordinate frame to
* world = R*p
*/
inline Point3 rotate(const Rot3& R, const Point3& p) { return R*p;}
Matrix Drotate1(const Rot3& R, const Point3& p);
Matrix Drotate2(const Rot3& R); // does not depend on p !
Point3 rotate(const Rot3& R, const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
/**
* rotate point from world to rotated
@ -211,16 +214,14 @@ namespace gtsam {
/**
* compose two rotations i.e., R=R1*R2
*/
//Rot3 compose (const Rot3& R1, const Rot3& R2);
Matrix Dcompose1(const Rot3& R1, const Rot3& R2);
Matrix Dcompose2(const Rot3& R1, const Rot3& R2);
Rot3 compose (const Rot3& R1, const Rot3& R2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
/**
* Return relative rotation D s.t. R2=D*R1, i.e. D=R2*R1'
*/
//Rot3 between (const Rot3& R1, const Rot3& R2);
Matrix Dbetween1(const Rot3& R1, const Rot3& R2);
Matrix Dbetween2(const Rot3& R1, const Rot3& R2);
Rot3 between (const Rot3& R1, const Rot3& R2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
/**
* [RQ] receives a 3 by 3 matrix and returns an upper triangular matrix R

8
geometry/StereoCamera.cpp
View File

@ -57,8 +57,8 @@ Matrix Dproject_stereo_pose(const StereoCamera& camera, const Point3& point) {
//Matrix D_intrinsic_pose = Dproject_pose(camera.calibrated_, point);
//**** above function call inlined
Point3 cameraPoint = transform_to(camera.pose(), point);
Matrix D_cameraPoint_pose = Dtransform_to1(camera.pose(), point); // 3x6
Matrix D_cameraPoint_pose;
Point3 cameraPoint = transform_to(camera.pose(), point, D_cameraPoint_pose, boost::none);
//cout << "D_cameraPoint_pose" << endl;
//print(D_cameraPoint_pose);
@ -83,8 +83,8 @@ Matrix Dproject_stereo_point(const StereoCamera& camera, const Point3& point) {
//Matrix D_intrinsic_point = Dproject_point(camera.calibrated_, point);
//**** above function call inlined
Point3 cameraPoint = transform_to(camera.pose(), point);
Matrix D_cameraPoint_point = Dtransform_to2(camera.pose(), point);
Matrix D_cameraPoint_point;
Point3 cameraPoint = transform_to(camera.pose(), point, boost::none, D_cameraPoint_point);
//Point2 intrinsic = project_to_camera(cameraPoint); // unused
Matrix D_intrinsic_cameraPoint = Dproject_to_stereo_camera1(camera, cameraPoint); // 3x3 Jacobian

19
geometry/tests/testPose2.cpp
View File

@ -175,9 +175,9 @@ TEST(Pose2, compose_a)
Pose2 pose1(M_PI/4.0, Point2(sqrt(0.5), sqrt(0.5)));
Pose2 pose2(M_PI/2.0, Point2(0.0, 2.0));
Pose2 actual = compose(pose1, pose2);
Matrix actualDcompose1 = Dcompose1(pose1, pose2);
Matrix actualDcompose2 = Dcompose2(pose1, pose2);
Matrix actualDcompose1;
Matrix actualDcompose2;
Pose2 actual = compose(pose1, pose2, actualDcompose1, actualDcompose2);
Pose2 expected(3.0*M_PI/4.0, Point2(-sqrt(0.5), 3.0*sqrt(0.5)));
CHECK(assert_equal(expected, actual));
@ -213,9 +213,8 @@ TEST(Pose2, compose_b)
Pose2 pose_expected(Rot2::fromAngle(M_PI/4.0), Point2(1.0, 2.0));
Pose2 pose_actual_op = pose1 * pose2;
Pose2 pose_actual_fcn = compose(pose1, pose2);
Matrix actualDcompose1 = Dcompose1(pose1, pose2);
Matrix actualDcompose2 = Dcompose2(pose1, pose2);
Matrix actualDcompose1, actualDcompose2;
Pose2 pose_actual_fcn = compose(pose1, pose2, actualDcompose1, actualDcompose2);
Matrix numericalH1 = numericalDerivative21<Pose2, Pose2, Pose2>(compose, pose1, pose2, 1e-5);
Matrix numericalH2 = numericalDerivative22<Pose2, Pose2, Pose2>(compose, pose1, pose2, 1e-5);
@ -236,9 +235,8 @@ TEST(Pose2, compose_c)
Pose2 pose_expected(Rot2::fromAngle(M_PI/2.0), Point2(1.0, 2.0));
Pose2 pose_actual_op = pose1 * pose2;
Pose2 pose_actual_fcn = compose(pose1,pose2);
Matrix actualDcompose1 = Dcompose1(pose1, pose2);
Matrix actualDcompose2 = Dcompose2(pose1, pose2);
Matrix actualDcompose1, actualDcompose2;
Pose2 pose_actual_fcn = compose(pose1,pose2, actualDcompose1, actualDcompose2);
Matrix numericalH1 = numericalDerivative21<Pose2, Pose2, Pose2>(compose, pose1, pose2, 1e-5);
Matrix numericalH2 = numericalDerivative22<Pose2, Pose2, Pose2>(compose, pose1, pose2, 1e-5);
@ -265,7 +263,8 @@ TEST(Pose2, inverse )
// Check derivative
Matrix numericalH = numericalDerivative11<Pose2,Pose2>(inverse, lTg, 1e-5);
Matrix actualDinverse = Dinverse(lTg);
Matrix actualDinverse;
inverse(lTg, actualDinverse);
CHECK(assert_equal(numericalH,actualDinverse));
}

92
geometry/tests/testPose3.cpp
View File

@ -150,15 +150,16 @@ TEST(Pose3, Adjoint_compose)
/* ************************************************************************* */
TEST( Pose3, compose )
{
Matrix actual = (T2*T2).matrix();
Matrix expected = T2.matrix()*T2.matrix();
CHECK(assert_equal(actual,expected,1e-8));
Matrix actual = (T2*T2).matrix();
Matrix expected = T2.matrix()*T2.matrix();
CHECK(assert_equal(actual,expected,1e-8));
Matrix actualDcompose1, actualDcompose2;
compose(T2, T2, actualDcompose1, actualDcompose2);
Matrix numericalH1 = numericalDerivative21<Pose3,Pose3,Pose3>(compose, T2, T2, 1e-5);
Matrix actualDcompose1 = Dcompose1(T2, T2);
CHECK(assert_equal(numericalH1,actualDcompose1,5e-5));
Matrix actualDcompose2 = Dcompose2(T2, T2);
Matrix numericalH2 = numericalDerivative22<Pose3,Pose3,Pose3>(compose, T2, T2, 1e-5);
CHECK(assert_equal(numericalH2,actualDcompose2));
}
@ -167,15 +168,16 @@ TEST( Pose3, compose )
TEST( Pose3, compose2 )
{
const Pose3& T1 = T;
Matrix actual = (T1*T2).matrix();
Matrix expected = T1.matrix()*T2.matrix();
CHECK(assert_equal(actual,expected,1e-8));
Matrix actual = (T1*T2).matrix();
Matrix expected = T1.matrix()*T2.matrix();
CHECK(assert_equal(actual,expected,1e-8));
Matrix actualDcompose1, actualDcompose2;
compose(T1, T2, actualDcompose1, actualDcompose2);
Matrix numericalH1 = numericalDerivative21<Pose3,Pose3,Pose3>(compose, T1, T2, 1e-5);
Matrix actualDcompose1 = Dcompose1(T1, T2);
CHECK(assert_equal(numericalH1,actualDcompose1,5e-5));
Matrix actualDcompose2 = Dcompose2(T1, T2);
Matrix numericalH2 = numericalDerivative22<Pose3,Pose3,Pose3>(compose, T1, T2, 1e-5);
CHECK(assert_equal(numericalH2,actualDcompose2));
}
@ -183,12 +185,12 @@ TEST( Pose3, compose2 )
/* ************************************************************************* */
TEST( Pose3, inverse)
{
Matrix actual = inverse(T).matrix();
Matrix expected = inverse(T.matrix());
CHECK(assert_equal(actual,expected,1e-8));
Matrix actualDinverse;
Matrix actual = inverse(T, actualDinverse).matrix();
Matrix expected = inverse(T.matrix());
CHECK(assert_equal(actual,expected,1e-8));
Matrix numericalH = numericalDerivative11<Pose3,Pose3>(inverse, T, 1e-5);
Matrix actualDinverse = Dinverse(T);
CHECK(assert_equal(numericalH,actualDinverse));
}
@ -200,41 +202,45 @@ TEST( Pose3, inverseDerivatives2)
Pose3 T(R,t);
Matrix numericalH = numericalDerivative11<Pose3,Pose3>(inverse, T, 1e-5);
Matrix actualDinverse = Dinverse(T);
Matrix actualDinverse;
inverse(T, actualDinverse);
CHECK(assert_equal(numericalH,actualDinverse,5e-5));
}
/* ************************************************************************* */
TEST( Pose3, compose_inverse)
{
Matrix actual = (T*inverse(T)).matrix();
Matrix expected = eye(4,4);
CHECK(assert_equal(actual,expected,1e-8));
Matrix actual = (T*inverse(T)).matrix();
Matrix expected = eye(4,4);
CHECK(assert_equal(actual,expected,1e-8));
}
/* ************************************************************************* */
TEST( Pose3, Dtransform_from1_a)
{
Matrix actualDtransform_from1 = Dtransform_from1(T, P);
Matrix numerical = numericalDerivative21(transform_from,T,P);
CHECK(assert_equal(numerical,actualDtransform_from1,1e-8));
Matrix actualDtransform_from1;
transform_from(T, P, actualDtransform_from1, boost::none);
Matrix numerical = numericalDerivative21(transform_from,T,P);
CHECK(assert_equal(numerical,actualDtransform_from1,1e-8));
}
TEST( Pose3, Dtransform_from1_b)
{
Pose3 origin;
Matrix actualDtransform_from1 = Dtransform_from1(origin, P);
Matrix numerical = numericalDerivative21(transform_from,origin,P);
CHECK(assert_equal(numerical,actualDtransform_from1,1e-8));
Matrix actualDtransform_from1;
transform_from(origin, P, actualDtransform_from1, boost::none);
Matrix numerical = numericalDerivative21(transform_from,origin,P);
CHECK(assert_equal(numerical,actualDtransform_from1,1e-8));
}
TEST( Pose3, Dtransform_from1_c)
{
Point3 origin;
Pose3 T0(R,origin);
Matrix actualDtransform_from1 = Dtransform_from1(T0, P);
Matrix numerical = numericalDerivative21(transform_from,T0,P);
CHECK(assert_equal(numerical,actualDtransform_from1,1e-8));
Matrix actualDtransform_from1;
transform_from(T0, P, actualDtransform_from1, boost::none);
Matrix numerical = numericalDerivative21(transform_from,T0,P);
CHECK(assert_equal(numerical,actualDtransform_from1,1e-8));
}
TEST( Pose3, Dtransform_from1_d)
@ -242,35 +248,39 @@ TEST( Pose3, Dtransform_from1_d)
Rot3 I;
Point3 t0(100,0,0);
Pose3 T0(I,t0);
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,actualDtransform_from1,1e-8));
Matrix actualDtransform_from1;
transform_from(T0, P, actualDtransform_from1, boost::none);
//print(computed, "Dtransform_from1_d computed:");
Matrix numerical = numericalDerivative21(transform_from,T0,P);
//print(numerical, "Dtransform_from1_d numerical:");
CHECK(assert_equal(numerical,actualDtransform_from1,1e-8));
}
/* ************************************************************************* */
TEST( Pose3, Dtransform_from2)
{
Matrix actualDtransform_from2 = Dtransform_from2(T);
Matrix numerical = numericalDerivative22(transform_from,T,P);
CHECK(assert_equal(numerical,actualDtransform_from2,1e-8));
Matrix actualDtransform_from2;
transform_from(T,P, boost::none, actualDtransform_from2);
Matrix numerical = numericalDerivative22(transform_from,T,P);
CHECK(assert_equal(numerical,actualDtransform_from2,1e-8));
}
/* ************************************************************************* */
TEST( Pose3, Dtransform_to1)
{
Matrix computed = Dtransform_to1(T, P);
Matrix numerical = numericalDerivative21(transform_to,T,P);
CHECK(assert_equal(numerical,computed,1e-8));
Matrix computed;
transform_to(T, P, computed, boost::none);
Matrix numerical = numericalDerivative21(transform_to,T,P);
CHECK(assert_equal(numerical,computed,1e-8));
}
/* ************************************************************************* */
TEST( Pose3, Dtransform_to2)
{
Matrix computed = Dtransform_to2(T,P);
Matrix numerical = numericalDerivative22(transform_to,T,P);
CHECK(assert_equal(numerical,computed,1e-8));
Matrix computed;
transform_to(T,P, boost::none, computed);
Matrix numerical = numericalDerivative22(transform_to,T,P);
CHECK(assert_equal(numerical,computed,1e-8));
}
/* ************************************************************************* */

44
geometry/tests/testRot3.cpp
View File

@ -224,27 +224,17 @@ TEST(Rot3, BCH)
}
/* ************************************************************************* */
// rotate derivatives
TEST( Rot3, Drotate1)
TEST( Rot3, rotate_derivatives)
{
Matrix actualDrotate1 = Drotate1(R, P);
Matrix numerical = numericalDerivative21(rotate, R, P);
CHECK(assert_equal(numerical,actualDrotate1,error));
}
TEST( Rot3, Drotate1_)
{
Matrix actualDrotate1 = Drotate1(inverse(R), P);
Matrix numerical = numericalDerivative21(rotate, inverse(R), P);
CHECK(assert_equal(numerical,actualDrotate1,error));
}
TEST( Rot3, Drotate2_DNrotate2)
{
Matrix actualDrotate2 = Drotate2(R);
Matrix numerical = numericalDerivative22(rotate, R, P);
CHECK(assert_equal(numerical,actualDrotate2,error));
Matrix actualDrotate1a, actualDrotate1b, actualDrotate2;
rotate(R, P, actualDrotate1a, actualDrotate2);
rotate(R.inverse(), P, actualDrotate1b, boost::none);
Matrix numerical1 = numericalDerivative21(rotate, R, P);
Matrix numerical2 = numericalDerivative21(rotate, R.inverse(), P);
Matrix numerical3 = numericalDerivative22(rotate, R, P);
EXPECT(assert_equal(numerical1,actualDrotate1a,error));
EXPECT(assert_equal(numerical2,actualDrotate1b,error));
EXPECT(assert_equal(numerical3,actualDrotate2, error));
}
/* ************************************************************************* */
@ -269,15 +259,14 @@ TEST( Rot3, compose )
Rot3 R2 = Rot3::rodriguez(0.2, 0.3, 0.5);
Rot3 expected = R1 * R2;
Rot3 actual = compose(R1, R2);
Matrix actualH1, actualH2;
Rot3 actual = compose(R1, R2, actualH1, actualH2);
CHECK(assert_equal(expected,actual));
Matrix numericalH1 = numericalDerivative21<Rot3, Rot3, Rot3> (compose, R1,
R2, 1e-5);
Matrix actualH1 = Dcompose1(R1, R2);
CHECK(assert_equal(numericalH1,actualH1));
Matrix actualH2 = Dcompose2(R1, R2);
Matrix numericalH2 = numericalDerivative22<Rot3, Rot3, Rot3> (compose, R1,
R2, 1e-5);
CHECK(assert_equal(numericalH2,actualH2));
@ -290,11 +279,11 @@ TEST( Rot3, inverse )
Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3);
Rot3 I;
CHECK(assert_equal(I,R*inverse(R)));
Matrix actualH;
CHECK(assert_equal(I,R*inverse(R, actualH)));
CHECK(assert_equal(I,inverse(R)*R));
Matrix numericalH = numericalDerivative11<Rot3, Rot3> (inverse, R, 1e-5);
Matrix actualH = Dinverse(R);
CHECK(assert_equal(numericalH,actualH));
}
@ -310,14 +299,13 @@ TEST( Rot3, between )
Rot3 R2 = Rot3::rodriguez(0.2, 0.3, 0.5);
Rot3 expected = inverse(R1) * R2;
Rot3 actual = between(R1, R2);
Matrix actualH1, actualH2;
Rot3 actual = between(R1, R2, actualH1, actualH2);
CHECK(assert_equal(expected,actual));
Matrix numericalH1 = numericalDerivative21(between<Rot3> , R1, R2, 1e-5);
Matrix actualH1 = Dbetween1(R1, R2);
CHECK(assert_equal(numericalH1,actualH1));
Matrix actualH2 = Dbetween2(R1, R2);
Matrix numericalH2 = numericalDerivative22(between<Rot3> , R1, R2, 1e-5);
CHECK(assert_equal(numericalH2,actualH2));
}