factor to constrain distance between two points

gtsam/slam/DistanceFactor.h

@@ -0,0 +1,88 @@
+/* ----------------------------------------------------------------------------
+
+ * 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
+
+ * -------------------------------------------------------------------------- */
+
+/*
+ * @file DistanceFactor.h
+ * @author Duy-Nguyen Ta
+ * @date Sep 26, 2014
+ * 
+ */
+
+#pragma once
+
+#include <gtsam/nonlinear/NonlinearFactor.h>
+
+namespace gtsam {
+
+/**
+ * Factor to constrain known measured distance between two points
+ */
+template<class POINT>
+class DistanceFactor: public NoiseModelFactor2<POINT, POINT> {
+
+  double measured_; /// measured distance
+
+  typedef NoiseModelFactor2<POINT, POINT> Base;
+  typedef DistanceFactor<POINT> This;
+
+public:
+  /// Default constructor
+  DistanceFactor() {
+  }
+
+  /// Constructor with keys and known measured distance
+  DistanceFactor(Key p1, Key p2, double measured, const SharedNoiseModel& model) :
+    Base(model, p1, p2), measured_(measured) {
+  }
+
+  /// @return a deep copy of this factor
+  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+    return boost::static_pointer_cast<gtsam::NonlinearFactor>(
+        gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
+
+  /// h(x)-z
+  Vector evaluateError(const POINT& p1, const POINT& p2,
+      boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
+          boost::none) const {
+    double distance = p1.distance(p2, H1, H2);
+    return (Vector(1) << distance - measured_);
+  }
+
+  /** return the measured */
+  double measured() const {
+    return measured_;
+  }
+
+  /** equals specialized to this factor */
+  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) && fabs(this->measured_ - e->measured_) < tol;
+  }
+
+  /** print contents */
+  void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+    std::cout << s << "DistanceFactor, distance = " << measured_ << std::endl;
+    Base::print("", keyFormatter);
+  }
+
+private:
+
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template<class ARCHIVE>
+  void serialize(ARCHIVE & ar, const unsigned int version) {
+    ar & boost::serialization::make_nvp("NoiseModelFactor2",
+        boost::serialization::base_object<Base>(*this));
+    ar & BOOST_SERIALIZATION_NVP(measured_);
+  }
+};
+
+} /* namespace gtsam */

gtsam/slam/tests/testDistanceFactor.cpp

@@ -0,0 +1,82 @@
+/* ----------------------------------------------------------------------------
+
+ * 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
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ *  @file  testDistanceFactor.cpp
+ *  @brief Unit tests for DistanceFactor Class
+ *  @author Duy-Nguyen Ta
+ *  @date Oct 2012
+ */
+
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/TestableAssertions.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/geometry/Point2.h>
+#include <gtsam/geometry/Point3.h>
+#include <gtsam/slam/DistanceFactor.h>
+
+using namespace std;
+using namespace gtsam;
+
+typedef DistanceFactor<Point2> DistanceFactor2D;
+typedef DistanceFactor<Point3> DistanceFactor3D;
+
+SharedDiagonal noise = noiseModel::Unit::Create(1);
+Point3 P(0., 1., 2.5), Q(10., -81., 7.);
+Point2 p(1., 2.5), q(-81., 7.);
+
+/* ************************************************************************* */
+TEST(DistanceFactor, Point3) {
+  DistanceFactor3D distanceFactor(0, 1, P.distance(Q), noise);
+  Matrix H1, H2;
+  Vector error = distanceFactor.evaluateError(P, Q, H1, H2);
+
+  Vector expectedError = zero(1);
+  EXPECT(assert_equal(expectedError, error, 1e-10));
+
+  boost::function<Vector(const Point3&, const Point3&)> testEvaluateError(
+      boost::bind(&DistanceFactor3D::evaluateError, distanceFactor, _1, _2,
+          boost::none, boost::none));
+  Matrix numericalH1 = numericalDerivative21(testEvaluateError, P, Q, 1e-5);
+  Matrix numericalH2 = numericalDerivative22(testEvaluateError, P, Q, 1e-5);
+
+  EXPECT(assert_equal(numericalH1, H1, 1e-8));
+  EXPECT(assert_equal(numericalH2, H2, 1e-8));
+
+}
+
+/* ************************************************************************* */
+TEST(DistanceFactor, Point2) {
+  DistanceFactor2D distanceFactor(0, 1, p.distance(q), noise);
+  Matrix H1, H2;
+  Vector error = distanceFactor.evaluateError(p, q, H1, H2);
+
+  Vector expectedError = zero(1);
+  EXPECT(assert_equal(expectedError, error, 1e-10));
+
+  boost::function<Vector(const Point2&, const Point2&)> testEvaluateError(
+      boost::bind(&DistanceFactor2D::evaluateError, distanceFactor, _1, _2,
+          boost::none, boost::none));
+  Matrix numericalH1 = numericalDerivative21(testEvaluateError, p, q, 1e-5);
+  Matrix numericalH2 = numericalDerivative22(testEvaluateError, p, q, 1e-5);
+
+  EXPECT(assert_equal(numericalH1, H1, 1e-8));
+  EXPECT(assert_equal(numericalH2, H2, 1e-8));
+
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */
+