From 86774e8e1d54757e96ff0ea9e4f09931ca959f7e Mon Sep 17 00:00:00 2001 From: thduynguyen Date: Fri, 26 Sep 2014 17:21:19 -0400 Subject: [PATCH] factor to constrain distance between two points --- gtsam/slam/DistanceFactor.h | 88 +++++++++++++++++++++++++ gtsam/slam/tests/testDistanceFactor.cpp | 82 +++++++++++++++++++++++ 2 files changed, 170 insertions(+) create mode 100644 gtsam/slam/DistanceFactor.h create mode 100644 gtsam/slam/tests/testDistanceFactor.cpp diff --git a/gtsam/slam/DistanceFactor.h b/gtsam/slam/DistanceFactor.h new file mode 100644 index 000000000..acecd41a2 --- /dev/null +++ b/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 + +namespace gtsam { + +/** + * Factor to constrain known measured distance between two points + */ +template +class DistanceFactor: public NoiseModelFactor2 { + + double measured_; /// measured distance + + typedef NoiseModelFactor2 Base; + typedef DistanceFactor 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::shared_ptr(new This(*this))); } + + /// h(x)-z + Vector evaluateError(const POINT& p1, const POINT& p2, + boost::optional H1 = boost::none, boost::optional 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 (&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 + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NoiseModelFactor2", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(measured_); + } +}; + +} /* namespace gtsam */ diff --git a/gtsam/slam/tests/testDistanceFactor.cpp b/gtsam/slam/tests/testDistanceFactor.cpp new file mode 100644 index 000000000..b16519715 --- /dev/null +++ b/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 +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +typedef DistanceFactor DistanceFactor2D; +typedef DistanceFactor 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 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 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); +} +/* ************************************************************************* */ +