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

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; }

geometry/Point3.cpp

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

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) {
+  inline Point3 compose(const Point3& p1, const Point3& p2, boost::optional<Matrix&> H1, boost::optional<Matrix&> H2=boost::none) {
-    return Matrix_(3,3,
+	  if (H1) *H1 = eye(3);
-        1.0, 0.0, 0.0,
+	  if (H2) *H2 = eye(3);
-        0.0, 1.0, 0.0,
+	  return p1.compose(p2);
-        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);
   }
 
   /** 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);
+  Point3   add (const Point3 &p, const Point3 &q,
-  Matrix Dadd2(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);
+  Point3   sub (const Point3 &p, const Point3 &q,
-  Matrix Dsub2(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); 

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) {
+  Pose2 inverse(const Pose2& pose, boost::optional<Matrix&> H1) {
-	  return -AdjointMap(pose);
+	  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,

geometry/Pose2.h

@@ -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

geometry/Pose3.cpp

@@ -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 (H1) {
-	  if (H2) *H2 = Dtransform_from2(pose);
-	  return transform_from(pose, p);
-  }
-
-  /* ************************************************************************* */
-  Matrix Dtransform_from1(const Pose3& pose, const Point3& p) {
 #ifdef CORRECT_POSE3_EXMAP
-		const Matrix R = pose.rotation().matrix();
+			const Matrix R = pose.rotation().matrix();
-    Matrix DR = R*skewSymmetric(-p.x(), -p.y(), -p.z());
+			Matrix DR = R*skewSymmetric(-p.x(), -p.y(), -p.z());
-    return collect(2,&DR,&R);
+			*H1 = collect(2,&DR,&R);
 #else
-    Matrix DR = Drotate1(pose.rotation(), p);
+			Matrix DR;
-    return collect(2,&DR,&I3);
+			rotate(pose.rotation(), p, DR, boost::none);
+			*H1 = collect(2,&DR,&I3);
 #endif
-  }
+	  }
-
+	  if (H2) *H2 = pose.rotation().matrix();
-  /* ************************************************************************* */
+	  return transform_from(pose, p);
-  Matrix Dtransform_from2(const Pose3& pose) {
-    return pose.rotation().matrix();
   }
 
   /* ************************************************************************* */
@@ -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 (H1) { // *H1 = Dtransform_to1(pose, p);
-	  if (H2) *H2 = Dtransform_to2(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
+  Pose3 compose(const Pose3& p1, const Pose3& p2,
-  Matrix Dtransform_to1(const Pose3& pose, const Point3& p) {
+		  	boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
-    Point3 q = transform_to(pose,p);
+	  if (H1) {
-    Matrix DR = skewSymmetric(q.x(), q.y(), q.z());
 #ifdef CORRECT_POSE3_EXMAP
-		return collect(2, &DR, &_I3);
+		*H1 = AdjointMap(inverse(p2));
-#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));
 #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);
+		Matrix Dt;
-		return gtsam::stack(2, &DR, &Dt);
+		transform_from(p1,t2, Dt, boost::none);
+		*H1 = gtsam::stack(2, &DR, &Dt);
 #endif
-	}
+	  }
-
+	  if (H2) {
-	Matrix Dcompose2(const Pose3& p1, const Pose3& p2) {
 #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));
+  Pose3 inverse(const Pose3& p, boost::optional<Matrix&> H1) {
-	// TODO: combined function will save !
+	  if (H1)
-	Matrix Dinverse(const Pose3& p) {
 #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);
+	  	Matrix invH;
-		Pose3 result = compose(invp1, p2);
+		Pose3 invp1 = inverse(p1, invH);
-		if (H1) *H1 = Dcompose1(invp1, p2) * Dinverse(p1);
+		Matrix composeH1;
-		if (H2) *H2 = Dcompose2(invp1, p2);
+		Pose3 result = compose(invp1, p2, composeH1, H2);
+		if (H1) *H1 = composeH1 * invH;
 		return result;
 	}
 

geometry/Pose3.h

@@ -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);
+  Pose3 compose(const Pose3& p1, const Pose3& p2,
-  Matrix Dcompose2(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

geometry/Rot3.cpp

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

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<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);
+  Point3 rotate(const Rot3& R, const Point3& p,
-  Matrix Drotate2(const Rot3& R); // does not depend on 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);
+  Rot3 compose (const Rot3& R1, const Rot3& R2,
-  Matrix Dcompose1(const Rot3& R1, const Rot3& R2);
+	boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
-  Matrix Dcompose2(const Rot3& R1, const Rot3& R2);
 
   /**
    * Return relative rotation D s.t. R2=D*R1, i.e. D=R2*R1'
    */
-  //Rot3    between (const Rot3& R1, const Rot3& R2);
+  Rot3 between (const Rot3& R1, const Rot3& R2,
-  Matrix Dbetween1(const Rot3& R1, const Rot3& R2);
+	boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
-  Matrix Dbetween2(const Rot3& R1, const Rot3& R2);
 
   /**
    * [RQ] receives a 3 by 3 matrix and returns an upper triangular matrix R

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;
-	  Matrix D_cameraPoint_pose = Dtransform_to1(camera.pose(), point);  // 3x6
+	  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;
-		Matrix D_cameraPoint_point = Dtransform_to2(camera.pose(), 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

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;
-  Matrix actualDcompose1 = Dcompose1(pose1, pose2);
+  Matrix actualDcompose2;
-  Matrix actualDcompose2 = Dcompose2(pose1, pose2);
+  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, actualDcompose2;
-  Matrix actualDcompose1 = Dcompose1(pose1, pose2);
+  Pose2 pose_actual_fcn = compose(pose1, pose2, actualDcompose1, actualDcompose2);
-  Matrix actualDcompose2 = Dcompose2(pose1, pose2);
 
   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, actualDcompose2;
-  Matrix actualDcompose1 = Dcompose1(pose1, pose2);
+  Pose2 pose_actual_fcn = compose(pose1,pose2, actualDcompose1, actualDcompose2);
-  Matrix actualDcompose2 = Dcompose2(pose1, pose2);
 
   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));
 }
 

geometry/tests/testPose3.cpp

@@ -150,15 +150,16 @@ TEST(Pose3, Adjoint_compose)
 /* ************************************************************************* */
 TEST( Pose3, compose )
 {
-  Matrix actual = (T2*T2).matrix();
+	Matrix actual = (T2*T2).matrix();
-  Matrix expected = T2.matrix()*T2.matrix();
+	Matrix expected = T2.matrix()*T2.matrix();
-  CHECK(assert_equal(actual,expected,1e-8));
+	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 actual = (T1*T2).matrix();
-  Matrix expected = T1.matrix()*T2.matrix();
+	Matrix expected = T1.matrix()*T2.matrix();
-  CHECK(assert_equal(actual,expected,1e-8));
+	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 actualDinverse;
-  Matrix expected = inverse(T.matrix());
+	Matrix actual = inverse(T, actualDinverse).matrix();
-  CHECK(assert_equal(actual,expected,1e-8));
+	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 actual = (T*inverse(T)).matrix();
-  Matrix expected = eye(4,4);
+	Matrix expected = eye(4,4);
-  CHECK(assert_equal(actual,expected,1e-8));
+	CHECK(assert_equal(actual,expected,1e-8));
 }
 
 /* ************************************************************************* */
 TEST( Pose3, Dtransform_from1_a)
 {
-  Matrix actualDtransform_from1 = Dtransform_from1(T, P);
+	Matrix actualDtransform_from1;
-  Matrix numerical = numericalDerivative21(transform_from,T,P);
+	transform_from(T, P, actualDtransform_from1, boost::none);
-  CHECK(assert_equal(numerical,actualDtransform_from1,1e-8));
+	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 actualDtransform_from1;
-  Matrix numerical = numericalDerivative21(transform_from,origin,P);
+	transform_from(origin, P, actualDtransform_from1, boost::none);
-  CHECK(assert_equal(numerical,actualDtransform_from1,1e-8));
+	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 actualDtransform_from1;
-  Matrix numerical = numericalDerivative21(transform_from,T0,P);
+	transform_from(T0, P, actualDtransform_from1, boost::none);
-  CHECK(assert_equal(numerical,actualDtransform_from1,1e-8));
+	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);
+	Matrix actualDtransform_from1;
-  //print(computed, "Dtransform_from1_d computed:");
+	transform_from(T0, P, actualDtransform_from1, boost::none);
-  Matrix numerical = numericalDerivative21(transform_from,T0,P);
+	//print(computed, "Dtransform_from1_d computed:");
-  //print(numerical, "Dtransform_from1_d numerical:");
+	Matrix numerical = numericalDerivative21(transform_from,T0,P);
-  CHECK(assert_equal(numerical,actualDtransform_from1,1e-8));
+	//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 actualDtransform_from2;
-  Matrix numerical = numericalDerivative22(transform_from,T,P);
+	transform_from(T,P, boost::none, actualDtransform_from2);
-  CHECK(assert_equal(numerical,actualDtransform_from2,1e-8));
+	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 computed;
-  Matrix numerical = numericalDerivative21(transform_to,T,P);
+	transform_to(T, P, computed, boost::none);
-  CHECK(assert_equal(numerical,computed,1e-8));
+	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 computed;
-  Matrix numerical = numericalDerivative22(transform_to,T,P);
+	transform_to(T,P, boost::none, computed);
-  CHECK(assert_equal(numerical,computed,1e-8));
+	Matrix numerical = numericalDerivative22(transform_to,T,P);
+	CHECK(assert_equal(numerical,computed,1e-8));
 }
 
 /* ************************************************************************* */

geometry/tests/testRot3.cpp

@@ -224,27 +224,17 @@ TEST(Rot3, BCH)
 }
 
 /* ************************************************************************* */
-// rotate derivatives
+TEST( Rot3, rotate_derivatives)
-
-TEST( Rot3, Drotate1)
 {
-	Matrix actualDrotate1 = Drotate1(R, P);
+	Matrix actualDrotate1a, actualDrotate1b, actualDrotate2;
-	Matrix numerical = numericalDerivative21(rotate, R, P);
+	rotate(R, P, actualDrotate1a, actualDrotate2);
-	CHECK(assert_equal(numerical,actualDrotate1,error));
+	rotate(R.inverse(), P, actualDrotate1b, boost::none);
-}
+	Matrix numerical1 = numericalDerivative21(rotate, R, P);
-
+	Matrix numerical2 = numericalDerivative21(rotate, R.inverse(), P);
-TEST( Rot3, Drotate1_)
+	Matrix numerical3 = numericalDerivative22(rotate, R, P);
-{
+	EXPECT(assert_equal(numerical1,actualDrotate1a,error));
-	Matrix actualDrotate1 = Drotate1(inverse(R), P);
+	EXPECT(assert_equal(numerical2,actualDrotate1b,error));
-	Matrix numerical = numericalDerivative21(rotate, inverse(R), P);
+	EXPECT(assert_equal(numerical3,actualDrotate2, error));
-	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));
 }
 
 /* ************************************************************************* */
@@ -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));
 }