Merged in fix/PriorFactorWithCharts (pull request #57)

Fixed the prior factor to use charts and traits

gtsam/slam/BetweenFactor.h

@@ -74,25 +74,26 @@ namespace gtsam {
       std::cout << s << "BetweenFactor("
           << keyFormatter(this->key1()) << ","
           << keyFormatter(this->key2()) << ")\n";
-      measured_.print("  measured: ");
+      traits::print<T>()(measured_, "  measured: ");
       this->noiseModel_->print("  noise model: ");
     }
 
     /** equals */
     virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
       const This *e =  dynamic_cast<const This*> (&expected);
-      return e != NULL && Base::equals(*e, tol) && this->measured_.equals(e->measured_, tol);
+      return e != NULL && Base::equals(*e, tol) && traits::equals<T>()(this->measured_, e->measured_, tol);
     }
 
     /** implement functions needed to derive from Factor */
 
     /** vector of errors */
     Vector evaluateError(const T& p1, const T& p2,
-        boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
+        boost::optional<Matrix&> H1 = boost::none,boost::optional<Matrix&> H2 =
             boost::none) const {
       T hx = p1.between(p2, H1, H2); // h(x)
+      DefaultChart<T> chart;
       // manifold equivalent of h(x)-z -> log(z,h(x))
-      return measured_.localCoordinates(hx);
+      return chart.local(measured_, hx);
     }
 
     /** return the measured */
@@ -129,7 +130,9 @@ namespace gtsam {
 
     /** Syntactic sugar for constrained version */
     BetweenConstraint(const VALUE& measured, Key key1, Key key2, double mu = 1000.0) :
-      BetweenFactor<VALUE>(key1, key2, measured, noiseModel::Constrained::All(VALUE::Dim(), fabs(mu))) {}
+      BetweenFactor<VALUE>(key1, key2, measured,
+                           noiseModel::Constrained::All(DefaultChart<VALUE>::getDimension(measured), fabs(mu)))
+    {}
 
   private:
 

gtsam/slam/PriorFactor.h

@@ -67,23 +67,24 @@ namespace gtsam {
     /** print */
     virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
       std::cout << s << "PriorFactor on " << keyFormatter(this->key()) << "\n";
-      prior_.print("  prior mean: ");
+      traits::print<T>()(prior_, "  prior mean: ");
       this->noiseModel_->print("  noise model: ");
     }
 
     /** equals */
     virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
       const This* e = dynamic_cast<const This*> (&expected);
-      return e != NULL && Base::equals(*e, tol) && this->prior_.equals(e->prior_, tol);
+      return e != NULL && Base::equals(*e, tol) && traits::equals<T>()(prior_, e->prior_, tol);
     }
 
     /** implement functions needed to derive from Factor */
 
     /** vector of errors */
     Vector evaluateError(const T& p, boost::optional<Matrix&> H = boost::none) const {
-      if (H) (*H) = eye(p.dim());
+      DefaultChart<T> chart;
+      if (H) (*H) = eye(chart.getDimension(p));
       // manifold equivalent of h(x)-z -> log(z,h(x))
-      return prior_.localCoordinates(p);
+      return chart.local(prior_,p);
     }
 
     const VALUE & prior() const { return prior_; }

gtsam/slam/tests/testBetweenFactor.cpp

@@ -44,6 +44,30 @@ TEST(BetweenFactor, Rot3) {
   EXPECT(assert_equal(numericalH2,actualH2, 1E-5));
 }
 
+/* ************************************************************************* */
+/*
+// Constructor scalar
+TEST(BetweenFactor, ConstructorScalar) {
+  SharedNoiseModel model;
+  double measured_value = 0.0;
+  BetweenFactor<double> factor(1, 2, measured_value, model);
+}
+
+// Constructor vector3
+TEST(BetweenFactor, ConstructorVector3) {
+  SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0);
+  Vector3 measured_value(1, 2, 3);
+  BetweenFactor<Vector3> factor(1, 2, measured_value, model);
+}
+
+// Constructor dynamic sized vector
+TEST(BetweenFactor, ConstructorDynamicSizeVector) {
+  SharedNoiseModel model = noiseModel::Isotropic::Sigma(5, 1.0);
+  Vector measured_value(5); measured_value << 1, 2, 3, 4, 5;
+  BetweenFactor<Vector> factor(1, 2, measured_value, model);
+}
+*/
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

gtsam/slam/tests/testPriorFactor.cpp

@@ -5,8 +5,8 @@
  * @date   Nov 4, 2014
  */
 
+#include <gtsam/base/Vector.h>
 #include <gtsam/slam/PriorFactor.h>
-#include <gtsam/base/LieScalar.h>
 #include <CppUnitLite/TestHarness.h>
 
 using namespace std;
@@ -14,10 +14,23 @@ using namespace gtsam;
 
 /* ************************************************************************* */
 
-// Constructor
-TEST(PriorFactor, Constructor) {
+// Constructor scalar
+TEST(PriorFactor, ConstructorScalar) {
   SharedNoiseModel model;
-  PriorFactor<LieScalar> factor(1, LieScalar(1.0), model);
+  PriorFactor<double> factor(1, 1.0, model);
+}
+
+// Constructor vector3
+TEST(PriorFactor, ConstructorVector3) {
+  SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0);
+  PriorFactor<Vector3> factor(1, Vector3(1,2,3), model);
+}
+
+// Constructor dynamic sized vector
+TEST(PriorFactor, ConstructorDynamicSizeVector) {
+  Vector v(5); v << 1, 2, 3, 4, 5;
+  SharedNoiseModel model = noiseModel::Isotropic::Sigma(5, 1.0);
+  PriorFactor<Vector> factor(1, v, model);
 }
 
 /* ************************************************************************* */