From 8c33168fb3650ff216e60e6ad31dceae4f8d351b Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Fri, 20 Aug 2010 15:17:13 +0000 Subject: [PATCH] Made all core geometry functions that have derivatives use the combined derivative form instead of separate derivative functions. --- geometry/Point2.h | 8 --- geometry/Point3.cpp | 22 ++++--- geometry/Point3.h | 23 +++----- geometry/Pose2.cpp | 13 +---- geometry/Pose2.h | 4 +- geometry/Pose3.cpp | 107 ++++++++++++++++------------------- geometry/Pose3.h | 13 +---- geometry/Rot3.cpp | 36 +++++------- geometry/Rot3.h | 21 +++---- geometry/StereoCamera.cpp | 8 +-- geometry/tests/testPose2.cpp | 19 +++---- geometry/tests/testPose3.cpp | 92 ++++++++++++++++-------------- geometry/tests/testRot3.cpp | 44 ++++++-------- 13 files changed, 180 insertions(+), 230 deletions(-) diff --git a/geometry/Point2.h b/geometry/Point2.h index 074b8e8fc..0910c9062 100644 --- a/geometry/Point2.h +++ b/geometry/Point2.h @@ -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; } diff --git a/geometry/Point3.cpp b/geometry/Point3.cpp index 644a66ac8..1609ce764 100644 --- a/geometry/Point3.cpp +++ b/geometry/Point3.cpp @@ -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 H1, boost::optional 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 H1, boost::optional 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) diff --git a/geometry/Point3.h b/geometry/Point3.h index eab52ce5a..67023be93 100644 --- a/geometry/Point3.h +++ b/geometry/Point3.h @@ -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 H1, boost::optional 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 H1, boost::optional 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 H1, boost::optional H2=boost::none); /** cross product */ Point3 cross(const Point3 &p, const Point3 &q); diff --git a/geometry/Pose2.cpp b/geometry/Pose2.cpp index cc0cd556b..3461f3feb 100644 --- a/geometry/Pose2.cpp +++ b/geometry/Pose2.cpp @@ -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 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, diff --git a/geometry/Pose2.h b/geometry/Pose2.h index 031f60508..f1e7a22f0 100644 --- a/geometry/Pose2.h +++ b/geometry/Pose2.h @@ -141,7 +141,7 @@ namespace gtsam { /** * inverse transformation */ - Matrix Dinverse(const Pose2& pose); + Pose2 inverse(const Pose2& pose, boost::optional 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 H1, boost::optional 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 diff --git a/geometry/Pose3.cpp b/geometry/Pose3.cpp index b7ac732a4..87afbe9f8 100644 --- a/geometry/Pose3.cpp +++ b/geometry/Pose3.cpp @@ -146,26 +146,19 @@ namespace gtsam { /* ************************************************************************* */ Point3 transform_from(const Pose3& pose, const Point3& p, boost::optional H1, boost::optional 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 H1, boost::optional 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 H1, boost::optional 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 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 H1, boost::optional 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; } diff --git a/geometry/Pose3.h b/geometry/Pose3.h index e2df0e873..394c36f56 100644 --- a/geometry/Pose3.h +++ b/geometry/Pose3.h @@ -116,27 +116,20 @@ namespace gtsam { Point3 transform_from(const Pose3& pose, const Point3& p, boost::optional H1, boost::optional 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 H1, boost::optional 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 H1, boost::optional H2); /** * Derivative of inverse */ - Matrix Dinverse(const Pose3& p); + Pose3 inverse(const Pose3& p, boost::optional H1); /** * Return relative pose between p1 and p2, in p1 coordinate frame diff --git a/geometry/Rot3.cpp b/geometry/Rot3.cpp index 3210cd196..11334aed1 100644 --- a/geometry/Rot3.cpp +++ b/geometry/Rot3.cpp @@ -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 H1, boost::optional 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 H1, boost::optional 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 H1, boost::optional H2) { + if (H1) *H1 = -(R2.transpose()*R1.matrix()); + if (H2) *H2 = I3; + return between(R1, R2); } /* ************************************************************************* */ diff --git a/geometry/Rot3.h b/geometry/Rot3.h index 85abbe2ae..1e76b7714 100644 --- a/geometry/Rot3.h +++ b/geometry/Rot3.h @@ -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 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 H1, boost::optional 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 H1, boost::optional 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 H1, boost::optional H2); /** * [RQ] receives a 3 by 3 matrix and returns an upper triangular matrix R diff --git a/geometry/StereoCamera.cpp b/geometry/StereoCamera.cpp index 38da76e0e..5aeab0e6b 100644 --- a/geometry/StereoCamera.cpp +++ b/geometry/StereoCamera.cpp @@ -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 diff --git a/geometry/tests/testPose2.cpp b/geometry/tests/testPose2.cpp index 53de49fa9..5099c7689 100644 --- a/geometry/tests/testPose2.cpp +++ b/geometry/tests/testPose2.cpp @@ -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(compose, pose1, pose2, 1e-5); Matrix numericalH2 = numericalDerivative22(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(compose, pose1, pose2, 1e-5); Matrix numericalH2 = numericalDerivative22(compose, pose1, pose2, 1e-5); @@ -265,7 +263,8 @@ TEST(Pose2, inverse ) // Check derivative Matrix numericalH = numericalDerivative11(inverse, lTg, 1e-5); - Matrix actualDinverse = Dinverse(lTg); + Matrix actualDinverse; + inverse(lTg, actualDinverse); CHECK(assert_equal(numericalH,actualDinverse)); } diff --git a/geometry/tests/testPose3.cpp b/geometry/tests/testPose3.cpp index f2d5b61ba..eaa2f1d2c 100644 --- a/geometry/tests/testPose3.cpp +++ b/geometry/tests/testPose3.cpp @@ -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(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(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(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(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(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(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)); } /* ************************************************************************* */ diff --git a/geometry/tests/testRot3.cpp b/geometry/tests/testRot3.cpp index 3cb834f03..1e1ddc74a 100644 --- a/geometry/tests/testRot3.cpp +++ b/geometry/tests/testRot3.cpp @@ -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 (compose, R1, R2, 1e-5); - Matrix actualH1 = Dcompose1(R1, R2); CHECK(assert_equal(numericalH1,actualH1)); - Matrix actualH2 = Dcompose2(R1, R2); Matrix numericalH2 = numericalDerivative22 (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 (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 , R1, R2, 1e-5); - Matrix actualH1 = Dbetween1(R1, R2); CHECK(assert_equal(numericalH1,actualH1)); - Matrix actualH2 = Dbetween2(R1, R2); Matrix numericalH2 = numericalDerivative22(between , R1, R2, 1e-5); CHECK(assert_equal(numericalH2,actualH2)); }