A failing unit test for a custom chart

gtsam/nonlinear/Values-inl.h

@@ -302,8 +302,8 @@ namespace gtsam {
    }
 
   // overloaded insert with chart initializer
-  template<typename ValueType, typename Chart, typename ChartInit>
-  void Values::insert(Key j, const ValueType& val, ChartInit chart) {
+  template<typename ValueType, typename Chart>
+  void Values::insert(Key j, const ValueType& val, Chart chart) {
     insert(j, static_cast<const Value&>(ChartValue<ValueType, Chart>(val, chart)));
   }
 
@@ -320,8 +320,8 @@ namespace gtsam {
   }
 
   // update with chart initializer, /todo: perhaps there is a way to init chart from old value...
-  template<typename ValueType, typename Chart, typename ChartInit>
-  void Values::update(Key j, const ValueType& val, ChartInit chart) {
+  template<typename ValueType, typename Chart>
+  void Values::update(Key j, const ValueType& val, Chart chart) {
     update(j, static_cast<const Value&>(ChartValue<ValueType, Chart>(val, chart)));
   }
 

gtsam/nonlinear/Values.h

@@ -263,8 +263,8 @@ namespace gtsam {
     void insert(Key j, const ValueType& val);
 
     /// overloaded insert version that also specifies a chart initializer
-    template <typename ValueType, typename Chart, typename ChartInit>
-    void insert(Key j, const ValueType& val, ChartInit chart);
+    template <typename ValueType, typename Chart>
+    void insert(Key j, const ValueType& val, Chart chart);
 
 
     /** insert that mimics the STL map insert - if the value already exists, the map is not modified
@@ -288,8 +288,8 @@ namespace gtsam {
     void update(Key j, const T& val);
 
     /// overloaded insert version that also specifies a chart initializer
-    template <typename T, typename Chart, typename ChartInit>
-    void update(Key j, const T& val, ChartInit chart);
+    template <typename T, typename Chart>
+    void update(Key j, const T& val, Chart chart);
 
     /** update the current available values without adding new ones */
     void update(const Values& values);

gtsam_unstable/nonlinear/ExpressionFactor.h

@@ -55,7 +55,7 @@ public:
     noiseModel_ = noiseModel;
 
     // Get dimensions of Jacobian matrices
-    // An Expression is assumed unmutable, so we do this now
+    // An Expression is assumed immutable, so we do this now
     std::map<Key, size_t> map;
     expression_.dims(map);
     size_t n = map.size();
@@ -83,12 +83,12 @@ public:
    */
   virtual Vector unwhitenedError(const Values& x,
       boost::optional<std::vector<Matrix>&> H = boost::none) const {
-    // TODO(PTF) Is this a place for custom charts?
     DefaultChart<T> chart;
     if (H) {
       // H should be pre-allocated
       assert(H->size()==size());
 
+      // todo...fix for custom charts.
       VerticalBlockMatrix Ab(dimensions_, Dim);
 
       // Wrap keys and VerticalBlockMatrix into structure passed to expression_
@@ -116,11 +116,20 @@ public:
     if (!active(x))
       return boost::shared_ptr<JacobianFactor>();
 
+    // For custom charts, we would have to update the dimensions here
+    // based on the values that have been passed in.
+    FastVector<Key>::const_iterator kit = keys_.begin();
+    std::vector<size_t> dims;
+    dims.reserve(keys_.size());
+    for(; kit != keys_.end(); ++kit) {
+      dims.push_back(x.at(*kit).dim());
+    }
+
     // Create a writeable JacobianFactor in advance
     // In case noise model is constrained, we need to provide a noise model
     bool constrained = noiseModel_->is_constrained();
     boost::shared_ptr<JacobianFactor> factor(
-        constrained ? new JacobianFactor(keys_, dimensions_, Dim,
+        constrained ? new JacobianFactor(keys_, dims, Dim,
             boost::static_pointer_cast<noiseModel::Constrained>(noiseModel_)->unit()) :
             new JacobianFactor(keys_, dimensions_, Dim));
 

gtsam_unstable/nonlinear/tests/testCustomChartExpression.cpp

@@ -0,0 +1,119 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------1------------------------------------------- */
+
+/**
+ * @file testExpression.cpp
+ * @date September 18, 2014
+ * @author Frank Dellaert
+ * @author Paul Furgale
+ * @brief unit tests for Block Automatic Differentiation
+ */
+
+#include <gtsam_unstable/nonlinear/Expression.h>
+#include <gtsam_unstable/nonlinear/ExpressionFactor.h>
+#include <gtsam_unstable/nonlinear/expressionTesting.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/linear/VectorValues.h>
+
+#include <CppUnitLite/TestHarness.h>
+
+#include <boost/assign/list_of.hpp>
+using boost::assign::list_of;
+using boost::assign::map_list_of;
+
+using namespace std;
+using namespace gtsam;
+
+// Dynamically sized Vector
+
+struct ProjectionChart {
+  typedef Eigen::Vector3d type;
+  typedef Eigen::Vector2d vector;
+  enum { dimension = 2 };
+  static vector local(const type& origin, const type& other) {
+    return (other - origin).head<2>();
+  }
+  static type retract(const type& origin, const vector& d) {
+    return origin + Eigen::Vector3d(2.0 * d[0], 3.0 * d[1], 0.0);
+  }
+  static int getDimension(const type& origin) {
+    return 2;
+  }
+};
+
+namespace gtsam {
+namespace traits {
+template<> struct is_chart<ProjectionChart> : public boost::true_type {};
+template<> struct dimension<ProjectionChart> : public boost::integral_constant<int, 2> {};
+}  // namespace traits
+}  // namespace gtsam
+
+TEST(ExpressionCustomChart, projection) {
+  std::cout << "Hello!\n";
+  // Create expression
+  Expression<Eigen::Vector3d> point(1);
+
+  Eigen::Vector3d pval(1.0, 2.0, 3.0);
+  Values standard;
+  standard.insert(1, pval);
+
+  Values custom;
+  custom.insert(1, pval, ProjectionChart());
+
+  Eigen::Vector3d pstandard = point.value(standard);
+  Eigen::Vector3d pcustom = point.value(custom);
+
+  // The values should be the same.
+  EXPECT(assert_equal(Matrix(pstandard), Matrix(pcustom), 1e-10));
+
+
+  EXPECT_LONGS_EQUAL(3, standard.at(1).dim());
+  VectorValues vstandard = standard.zeroVectors();
+  EXPECT_LONGS_EQUAL(3, vstandard.at(1).size());
+
+
+  EXPECT_LONGS_EQUAL(2, custom.at(1).dim());
+  VectorValues vcustom = custom.zeroVectors();
+  EXPECT_LONGS_EQUAL(2, vcustom.at(1).size());
+
+  ExpressionFactor<Eigen::Vector3d> f(noiseModel::Unit::Create(pval.size()), pval, point);
+
+  boost::shared_ptr<GaussianFactor> gfstandard = f.linearize(standard);
+  boost::shared_ptr<JacobianFactor> jfstandard = //
+      boost::dynamic_pointer_cast<JacobianFactor>(gfstandard);
+
+  typedef std::pair<Eigen::MatrixXd, Eigen::VectorXd> Jacobian;
+  Jacobian Jstandard = jfstandard->jacobianUnweighted();
+  EXPECT(assert_equal(Eigen::Matrix3d::Identity(), Jstandard.first, 1e-10));
+
+  boost::shared_ptr<GaussianFactor> gfcustom = f.linearize(custom);
+  boost::shared_ptr<JacobianFactor> jfcustom = //
+      boost::dynamic_pointer_cast<JacobianFactor>(gfcustom);
+
+  Eigen::MatrixXd expectedJacobian = Eigen::MatrixXd::Zero(3,2);
+  expectedJacobian(0,0) = 2.0;
+  expectedJacobian(1,1) = 3.0;
+  Jacobian Jcustom = jfcustom->jacobianUnweighted();
+  EXPECT(assert_equal(expectedJacobian, Jcustom.first, 1e-10));
+
+  // Amazingly, the finite differences get the expected Jacobian right.
+  const double fd_step = 1e-5;
+  const double tolerance = 1e-5;
+  EXPECT_CORRECT_EXPRESSION_JACOBIANS(point, custom, fd_step, tolerance);
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */
+