Ternary now fixed

gtsam_unstable/nonlinear/Expression-inl.h

@@ -476,9 +476,12 @@ class TernaryExpression: public ExpressionNode<T> {
 
 public:
 
+  typedef Eigen::Matrix<double, T::dimension, A1::dimension> JacobianTA1;
+  typedef Eigen::Matrix<double, T::dimension, A2::dimension> JacobianTA2;
+  typedef Eigen::Matrix<double, T::dimension, A3::dimension> JacobianTA3;
   typedef boost::function<
-      T(const A1&, const A2&, const A3&, boost::optional<Matrix&>,
-          boost::optional<Matrix&>, boost::optional<Matrix&>)> Function;
+      T(const A1&, const A2&, const A3&, boost::optional<JacobianTA1&>,
+          boost::optional<JacobianTA2&>, boost::optional<JacobianTA3&>)> Function;
 
 private:
 
@@ -528,11 +531,13 @@ public:
     Augmented<A1> a1 = this->expressionA1_->forward(values);
     Augmented<A2> a2 = this->expressionA2_->forward(values);
     Augmented<A3> a3 = this->expressionA3_->forward(values);
-    Matrix dTdA1, dTdA2, dTdA3;
+    JacobianTA1 dTdA1;
+    JacobianTA2 dTdA2;
+    JacobianTA3 dTdA3;
     T t = function_(a1.value(), a2.value(), a3.value(),
-        a1.constant() ? none : boost::optional<Matrix&>(dTdA1),
-        a2.constant() ? none : boost::optional<Matrix&>(dTdA2),
-        a3.constant() ? none : boost::optional<Matrix&>(dTdA3));
+        a1.constant() ? none : boost::optional<JacobianTA1&>(dTdA1),
+        a2.constant() ? none : boost::optional<JacobianTA2&>(dTdA2),
+        a3.constant() ? none : boost::optional<JacobianTA3&>(dTdA3));
     return Augmented<T>(t, dTdA1, a1.jacobians(), dTdA2, a2.jacobians(), dTdA3,
         a3.jacobians());
   }
@@ -542,7 +547,9 @@ public:
     boost::shared_ptr<JacobianTrace<A1> > trace1;
     boost::shared_ptr<JacobianTrace<A2> > trace2;
     boost::shared_ptr<JacobianTrace<A3> > trace3;
-    Matrix dTdA1, dTdA2, dTdA3;
+    JacobianTA1 dTdA1;
+    JacobianTA2 dTdA2;
+    JacobianTA3 dTdA3;
     /// Start the reverse AD process
     virtual void reverseAD(JacobianMap& jacobians) const {
       trace1->reverseAD(dTdA1, jacobians);

gtsam_unstable/nonlinear/tests/testBADFactor.cpp

@@ -123,6 +123,15 @@ TEST(BADFactor, test) {
   EXPECT_LONGS_EQUAL(2, f2.dim());
   boost::shared_ptr<GaussianFactor> gf2 = f2.linearize(values);
   EXPECT( assert_equal(*expected, *gf2, 1e-9));
+
+  TernaryExpression<Point2,Pose3,Point3,Cal3_S2>::Function fff = project6;
+
+  // Try ternary version
+  BADFactor<Point2> f3(model, measured, project3(x, p, K));
+  EXPECT_DOUBLES_EQUAL(expected_error, f3.error(values), 1e-9);
+  EXPECT_LONGS_EQUAL(2, f3.dim());
+  boost::shared_ptr<GaussianFactor> gf3 = f3.linearize(values);
+  EXPECT( assert_equal(*expected, *gf3, 1e-9));
 }
 
 /* ************************************************************************* */
@@ -213,6 +222,52 @@ TEST(BADFactor, test) {
   EXPECT( assert_equal(expected, *jf,1e-9));
 }
 
+/* ************************************************************************* */
+// Test compose with three arguments
+Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3,
+    boost::optional<Matrix3&> H1, boost::optional<Matrix3&> H2,
+    boost::optional<Matrix3&> H3) {
+  // return dummy derivatives (not correct, but that's ok for testing here)
+  if (H1)
+    *H1 = eye(3);
+  if (H2)
+    *H2 = eye(3);
+  if (H3)
+    *H3 = eye(3);
+  return R1 * (R2 * R3);
+}
+
+TEST(BADFactor, composeTernary) {
+
+  // Create expression
+  Rot3_ A(1), B(2), C(3);
+  Rot3_ ABC(composeThree, A, B, C);
+
+  // Create factor
+  BADFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), ABC);
+
+  // Create some values
+  Values values;
+  values.insert(1, Rot3());
+  values.insert(2, Rot3());
+  values.insert(3, Rot3());
+
+  // Check unwhitenedError
+  std::vector<Matrix> H(3);
+  Vector actual = f.unwhitenedError(values, H);
+  EXPECT_LONGS_EQUAL(3, H.size());
+  EXPECT( assert_equal(eye(3), H[0],1e-9));
+  EXPECT( assert_equal(eye(3), H[1],1e-9));
+  EXPECT( assert_equal(eye(3), H[2],1e-9));
+
+  // Check linearization
+  JacobianFactor expected(1, eye(3), 2, eye(3), 3, eye(3), zero(3));
+  boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
+  boost::shared_ptr<JacobianFactor> jf = //
+      boost::dynamic_pointer_cast<JacobianFactor>(gf);
+  EXPECT( assert_equal(expected, *jf,1e-9));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

gtsam_unstable/slam/expressions.h

@@ -10,6 +10,7 @@
 #include <gtsam_unstable/nonlinear/Expression.h>
 #include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/geometry/PinholeCamera.h>
+#include <boost/bind.hpp>
 
 namespace gtsam {
 
@@ -48,6 +49,16 @@ Point2_ project(const Point3_& p_cam) {
   return Point2_(PinholeCamera<Cal3_S2>::project_to_camera, p_cam);
 }
 
+Point2 project6(const Pose3& x, const Point3& p, const Cal3_S2& K,
+    boost::optional<Matrix26&> Dpose, boost::optional<Matrix23&> Dpoint,
+    boost::optional<Matrix25&> Dcal) {
+  return PinholeCamera<Cal3_S2>(x, K).project(p, Dpose, Dpoint, Dcal);
+}
+
+Point2_ project3(const Pose3_& x, const Point3_& p, const Cal3_S2_& K) {
+  return Point2_(project6, x, p, K);
+}
+
 template<class CAL>
 Point2_ uncalibrate(const Expression<CAL>& K, const Point2_& xy_hat) {
   return Point2_(K, &CAL::uncalibrate, xy_hat);

gtsam_unstable/timing/timeCameraExpression.cpp

@@ -26,7 +26,6 @@
 #include <time.h>
 #include <iostream>
 #include <iomanip>      // std::setprecision
-
 using namespace std;
 using namespace gtsam;
 
@@ -74,37 +73,43 @@ int main() {
   // Dedicated factor
   // Oct 3, 2014, Macbook Air
   // 4.2 musecs/call
-  GeneralSFMFactor2<Cal3_S2> oldFactor2(z, model, 1, 2, 3);
-  time(oldFactor2, values);
+  GeneralSFMFactor2<Cal3_S2> f1(z, model, 1, 2, 3);
+  time(f1, values);
 
   // BADFactor
   // Oct 3, 2014, Macbook Air
   // 20.3 musecs/call
-  BADFactor<Point2> newFactor2(model, z,
+  BADFactor<Point2> f2(model, z,
       uncalibrate(K, project(transform_to(x, p))));
-  time(newFactor2, values);
+  time(f2, values);
+
+  // BADFactor ternary
+  // Oct 3, 2014, Macbook Air
+  // 20.3 musecs/call
+  BADFactor<Point2> f3(model, z, project3(x, p, K));
+  time(f3, values);
 
   // CALIBRATED
 
   // Dedicated factor
   // Oct 3, 2014, Macbook Air
   // 3.4 musecs/call
-  GenericProjectionFactor<Pose3, Point3> oldFactor1(z, model, 1, 2, fixedK);
-  time(oldFactor1, values);
+  GenericProjectionFactor<Pose3, Point3> g1(z, model, 1, 2, fixedK);
+  time(g1, values);
 
   // BADFactor
   // Oct 3, 2014, Macbook Air
   // 16.0 musecs/call
-  BADFactor<Point2> newFactor1(model, z,
+  BADFactor<Point2> g2(model, z,
       uncalibrate(Cal3_S2_(*fixedK), project(transform_to(x, p))));
-  time(newFactor1, values);
+  time(g2, values);
 
   // BADFactor, optimized
   // Oct 3, 2014, Macbook Air
   // 9.0 musecs/call
   typedef PinholeCamera<Cal3_S2> Camera;
   typedef Expression<Camera> Camera_;
-  BADFactor<Point2> newFactor3(model, z, Point2_(myProject, x, p));
-  time(newFactor3, values);
+  BADFactor<Point2> g3(model, z, Point2_(myProject, x, p));
+  time(g3, values);
   return 0;
 }