Added more unit tests that expose compilation issue

gtsam/geometry/Point3.cpp

@@ -111,6 +111,18 @@ double Point3::norm(boost::optional<Matrix&> H) const {
   return r;
 }
 
+/* ************************************************************************* */
+double Point3::norm(boost::optional<Eigen::Matrix<double,1,3>&> H) const {
+  double r = norm();
+  if (H) {
+    if (fabs(r) > 1e-10)
+      *H << x_ / r, y_ / r, z_ / r;
+    else
+      *H << 1, 1, 1; // really infinity, why 1 ?
+  }
+  return r;
+}
+
 /* ************************************************************************* */
 Point3 Point3::normalize(boost::optional<Matrix&> H) const {
   Point3 normalized = *this / norm();

gtsam/geometry/Point3.h

@@ -186,6 +186,9 @@ namespace gtsam {
     /** Distance of the point from the origin, with Jacobian */
     double norm(boost::optional<Matrix&> H) const;
 
+    /** Distance of the point from the origin, with Jacobian */
+    double norm(boost::optional<Eigen::Matrix<double,1,3>&> H) const;
+
     /** normalize, with optional Jacobian */
     Point3 normalize(boost::optional<Matrix&> H = boost::none) const;
 

gtsam_unstable/nonlinear/tests/testExpression.cpp

@@ -73,27 +73,84 @@ TEST(Expression, Leaves) {
 }
 
 /* ************************************************************************* */
+// Unary(Leaf)
+namespace unary {
+Point2 f0(const Point3& p, boost::optional<Matrix23&> H) {
+  return Point2();
+}
+LieScalar f1(const Point3& p, boost::optional<Eigen::Matrix<double, 1, 3>&> H) {
+  return LieScalar(0.0);
+}
+double f2(const Point3& p, boost::optional<Eigen::Matrix<double, 1, 3>&> H) {
+  return 0.0;
+}
+Expression<Point3> p(1);
+set<Key> expected = list_of(1);
+}
+TEST(Expression, Unary0) {
+  using namespace unary;
+  Expression<Point2> e(f0, p);
+  EXPECT(expected == e.keys());
+}
+TEST(Expression, Unary1) {
+  using namespace unary;
+  Expression<double> e(f1, p);
+  EXPECT(expected == e.keys());
+}
+TEST(Expression, Unary2) {
+  using namespace unary;
+  Expression<double> e(f2, p);
+  EXPECT(expected == e.keys());
+}
+/* ************************************************************************* */
+//Nullary Method
+TEST(Expression, NullaryMethod) {
 
-//TEST(Expression, NullaryMethod) {
+  // Create expression
-//  Expression<Point3> p(67);
+  Expression<Point3> p(67);
-//  Expression<LieScalar> norm(p, &Point3::norm);
+  Expression<double> norm(p, &Point3::norm);
-//  Values values;
+
-//  values.insert(67,Point3(3,4,5));
+  // Create Values
-//  Augmented<LieScalar> a = norm.augmented(values);
+  Values values;
-//  EXPECT(a.value() == sqrt(50));
+  values.insert(67, Point3(3, 4, 5));
-//  JacobianMap expected;
+
-//  expected[67] = (Matrix(1,3) << 3/sqrt(50),4/sqrt(50),5/sqrt(50));
+  // Pre-allocate JacobianMap
-//  EXPECT(assert_equal(expected.at(67),a.jacobians().at(67)));
+  FastVector<Key> keys;
-//}
+  keys.push_back(67);
+  FastVector<int> dims;
+  dims.push_back(3);
+  VerticalBlockMatrix Ab(dims, 1);
+  JacobianMap map(keys, Ab);
+
+  // Get value and Jacobian
+  double actual = norm.value(values, map);
+
+  // Check all
+  EXPECT(actual == sqrt(50));
+  Matrix expected(1, 3);
+  expected << 3.0 / sqrt(50.0), 4.0 / sqrt(50.0), 5.0 / sqrt(50.0);
+  EXPECT(assert_equal(expected,Ab(0)));
+}
 /* ************************************************************************* */
 // Binary(Leaf,Leaf)
 namespace binary {
 // Create leaves
+double doubleF(const Pose3& pose, const Point3& point,
+    boost::optional<Eigen::Matrix<double, 1, 6>&> H1,
+    boost::optional<Eigen::Matrix<double, 1, 3>&> H2) {
+  return 0.0;
+}
 Expression<Pose3> x(1);
 Expression<Point3> p(2);
 Expression<Point3> p_cam(x, &Pose3::transform_to, p);
 }
 /* ************************************************************************* */
+// Check that creating an expression to double compiles
+TEST(Expression, BinaryToDouble) {
+  using namespace binary;
+  Expression<double> p_cam(doubleF, x, p);
+}
+/* ************************************************************************* */
 // keys
 TEST(Expression, BinaryKeys) {
   set<Key> expected = list_of(1)(2);