From 0ef12f2f2012f08fb6bb46dbf57dd4a863e05738 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Wed, 19 Sep 2012 02:23:43 +0000 Subject: [PATCH] Moved in reference frame factor from MastSLAM --- .cproject | 342 +++++++++--------- gtsam_unstable/slam/ReferenceFrameFactor.h | 133 +++++++ .../slam/tests/testReferenceFrameFactor.cpp | 248 +++++++++++++ 3 files changed, 560 insertions(+), 163 deletions(-) create mode 100644 gtsam_unstable/slam/ReferenceFrameFactor.h create mode 100644 gtsam_unstable/slam/tests/testReferenceFrameFactor.cpp diff --git a/.cproject b/.cproject index 52375afab..2bc98db3c 100644 --- a/.cproject +++ b/.cproject @@ -1,7 +1,5 @@ - - - + @@ -309,14 +307,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -343,6 +333,7 @@ make + tests/testBayesTree.run true false @@ -350,6 +341,7 @@ make + testBinaryBayesNet.run true false @@ -397,6 +389,7 @@ make + testSymbolicBayesNet.run true false @@ -404,6 +397,7 @@ make + tests/testSymbolicFactor.run true false @@ -411,6 +405,7 @@ make + testSymbolicFactorGraph.run true false @@ -426,11 +421,20 @@ make + tests/testBayesTree true false true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j5 @@ -519,22 +523,6 @@ false true - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - make -j2 @@ -551,6 +539,22 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + make -j2 @@ -575,26 +579,26 @@ true true - + make - -j5 - testValues.run + -j2 + all true true true - + make - -j5 - testOrdering.run + -j2 + check true true true - + make - -j5 - testKey.run + -j2 + clean true true true @@ -679,26 +683,26 @@ true true - + make - -j2 - all + -j5 + testValues.run true true true - + make - -j2 - check + -j5 + testOrdering.run true true true - + make - -j2 - clean + -j5 + testKey.run true true true @@ -799,6 +803,14 @@ true true + + make + -j5 + testReferenceFrameFactor.run + true + true + true + make -j5 @@ -1033,6 +1045,7 @@ make + testGraph.run true false @@ -1040,6 +1053,7 @@ make + testJunctionTree.run true false @@ -1047,6 +1061,7 @@ make + testSymbolicBayesNetB.run true false @@ -1206,6 +1221,7 @@ make + testErrors.run true false @@ -1251,10 +1267,10 @@ true true - + make - -j2 - testGaussianFactor.run + -j5 + testLinearContainerFactor.run true true true @@ -1339,10 +1355,10 @@ true true - + make - -j5 - testLinearContainerFactor.run + -j2 + testGaussianFactor.run true true true @@ -1677,7 +1693,6 @@ make - testSimulated2DOriented.run true false @@ -1717,7 +1732,6 @@ make - testSimulated2D.run true false @@ -1725,7 +1739,6 @@ make - testSimulated3D.run true false @@ -1917,7 +1930,6 @@ make - tests/testGaussianISAM2 true false @@ -1939,102 +1951,6 @@ true true - - make - -j2 - testRot3.run - true - true - true - - - make - -j2 - testRot2.run - true - true - true - - - make - -j2 - testPose3.run - true - true - true - - - make - -j2 - timeRot3.run - true - true - true - - - make - -j2 - testPose2.run - true - true - true - - - make - -j2 - testCal3_S2.run - true - true - true - - - make - -j2 - testSimpleCamera.run - true - true - true - - - make - -j2 - testHomography2.run - true - true - true - - - make - -j2 - testCalibratedCamera.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - testPoint2.run - true - true - true - make -j3 @@ -2236,6 +2152,7 @@ cpack + -G DEB true false @@ -2243,6 +2160,7 @@ cpack + -G RPM true false @@ -2250,6 +2168,7 @@ cpack + -G TGZ true false @@ -2257,6 +2176,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -2390,34 +2310,98 @@ true true - + make - -j5 - testSpirit.run + -j2 + testRot3.run true true true - + make - -j5 - testWrap.run + -j2 + testRot2.run true true true - + make - -j5 - check.wrap + -j2 + testPose3.run true true true - + make - -j5 - wrap + -j2 + timeRot3.run + true + true + true + + + make + -j2 + testPose2.run + true + true + true + + + make + -j2 + testCal3_S2.run + true + true + true + + + make + -j2 + testSimpleCamera.run + true + true + true + + + make + -j2 + testHomography2.run + true + true + true + + + make + -j2 + testCalibratedCamera.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + testPoint2.run true true true @@ -2461,6 +2445,38 @@ false true + + make + -j5 + testSpirit.run + true + true + true + + + make + -j5 + testWrap.run + true + true + true + + + make + -j5 + check.wrap + true + true + true + + + make + -j5 + wrap + true + true + true + diff --git a/gtsam_unstable/slam/ReferenceFrameFactor.h b/gtsam_unstable/slam/ReferenceFrameFactor.h new file mode 100644 index 000000000..f0ea66b36 --- /dev/null +++ b/gtsam_unstable/slam/ReferenceFrameFactor.h @@ -0,0 +1,133 @@ +/* ---------------------------------------------------------------------------- + + * 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 ReferenceFrameFactor.h + * @brief A constraint for combining graphs by common landmarks and a transform node + * @author Alex Cunningham + */ + +#pragma once + +#include + +namespace gtsam { + +/** + * Transform function that must be specialized specific domains + * @tparam T is a Transform type + * @tparam P is a point type + */ +template +P transform_point( + const T& trans, const P& global, + boost::optional Dtrans, + boost::optional Dglobal) { + return trans.transform_from(global, Dtrans, Dglobal); +} + +/** + * A constraint between two landmarks in separate maps + * Templated on: + * Point : Type of landmark + * Transform : Transform variable class + * + * The transform is defined as transforming global to local: + * l = lTg * g + * + * The Point and Transform concepts must be Lie types, and the transform + * relationship "Point = transform_from(Transform, Point)" must exist. + * + * To implement this function in new domains, specialize a new version of + * Point transform_point(transform, global, Dtrans, Dglobal) + * to use the correct point and transform types. + * + * This base class should be specialized to implement the cost function for + * specific classes of landmarks + */ +template +class ReferenceFrameFactor : public NoiseModelFactor3 { +protected: + /** default constructor for serialization only */ + ReferenceFrameFactor() {} + +public: + typedef NoiseModelFactor3 Base; + typedef ReferenceFrameFactor This; + + typedef POINT Point; + typedef TRANSFORM Transform; + + /** + * General constructor with arbitrary noise model (constrained or otherwise) + */ + ReferenceFrameFactor(Key globalKey, Key transKey, Key localKey, const noiseModel::Base::shared_ptr& model) + : Base(model,globalKey, transKey, localKey) {} + + /** + * Construct a hard frame of reference reference constraint with equal mu values for + * each degree of freedom. + */ + ReferenceFrameFactor(double mu, Key globalKey, Key transKey, Key localKey) + : Base(globalKey, transKey, localKey, Point().dim(), mu) {} + + /** + * Simple soft constraint constructor for frame of reference, with equal weighting for + * each degree of freedom. + */ + ReferenceFrameFactor(Key globalKey, Key transKey, Key localKey, double sigma = 1e-2) + : Base(noiseModel::Isotropic::Sigma(POINT().dim(), sigma), + globalKey, transKey, localKey) {} + + virtual ~ReferenceFrameFactor(){} + + virtual NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + NonlinearFactor::shared_ptr(new This(*this))); } + + /** Combined cost and derivative function using boost::optional */ + virtual Vector evaluateError(const Point& global, const Transform& trans, const Point& local, + boost::optional Dforeign = boost::none, + boost::optional Dtrans = boost::none, + boost::optional Dlocal = boost::none) const { + Point newlocal = transform_point(trans, global, Dtrans, Dforeign); + if (Dlocal) { + Point dummy; + *Dlocal = -1* gtsam::eye(dummy.dim()); + } + return local.localCoordinates(newlocal); + } + + virtual void print(const std::string& s="", + const gtsam::KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + std::cout << s << ": ReferenceFrameFactor(" + << "Global: " << keyFormatter(this->key1()) << "," + << " Transform: " << keyFormatter(this->key2()) << "," + << " Local: " << keyFormatter(this->key3()) << ")\n"; + this->noiseModel_->print(" noise model"); + } + + // access - convenience functions + Key global_key() const { return this->key1(); } + Key transform_key() const { return this->key2(); } + Key local_key() const { return this->key3(); } + +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NonlinearFactor3", + boost::serialization::base_object(*this)); + } +}; + +} // \namespace gtsam diff --git a/gtsam_unstable/slam/tests/testReferenceFrameFactor.cpp b/gtsam_unstable/slam/tests/testReferenceFrameFactor.cpp new file mode 100644 index 000000000..4e7c9e455 --- /dev/null +++ b/gtsam_unstable/slam/tests/testReferenceFrameFactor.cpp @@ -0,0 +1,248 @@ +/* ---------------------------------------------------------------------------- + + * 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 testReferenceFrameFactor.cpp + * @author Alex Cunningham + */ + +#include + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +using namespace std; +using namespace boost; +using namespace gtsam; + +typedef gtsam::ReferenceFrameFactor PointReferenceFrameFactor; +typedef gtsam::ReferenceFrameFactor PoseReferenceFrameFactor; + +Key lA1 = symbol_shorthand::L(1), lA2 = symbol_shorthand::L(2), lB1 = symbol_shorthand::L(11), lB2 = symbol_shorthand::L(12); +Key tA1 = symbol_shorthand::T(1), tB1 = symbol_shorthand::T(2); + +/* ************************************************************************* */ +TEST( ReferenceFrameFactor, equals ) { + PointReferenceFrameFactor + c1(lB1, tA1, lA1), + c2(lB1, tA1, lA1), + c3(lB1, tA1, lA2); + + EXPECT(assert_equal(c1, c1)); + EXPECT(assert_equal(c1, c2)); + EXPECT(!c1.equals(c3)); +} + +/* ************************************************************************* */ +LieVector evaluateError_(const PointReferenceFrameFactor& c, + const Point2& global, const Pose2& trans, const Point2& local) { + return LieVector(c.evaluateError(global, trans, local)); +} +TEST( ReferenceFrameFactor, jacobians ) { + + // from examples below + Point2 local(2.0, 3.0), global(-1.0, 2.0); + Pose2 trans(1.5, 2.5, 0.3); + + PointReferenceFrameFactor tc(lA1, tA1, lB1); + Matrix actualDT, actualDL, actualDF; + tc.evaluateError(global, trans, local, actualDF, actualDT, actualDL); + + Matrix numericalDT, numericalDL, numericalDF; + numericalDF = numericalDerivative31( + boost::bind(evaluateError_, tc, _1, _2, _3), + global, trans, local, 1e-5); + numericalDT = numericalDerivative32( + boost::bind(evaluateError_, tc, _1, _2, _3), + global, trans, local, 1e-5); + numericalDL = numericalDerivative33( + boost::bind(evaluateError_, tc, _1, _2, _3), + global, trans, local, 1e-5); + + EXPECT(assert_equal(numericalDF, actualDF)); + EXPECT(assert_equal(numericalDL, actualDL)); + EXPECT(assert_equal(numericalDT, actualDT)); +} + +/* ************************************************************************* */ +TEST( ReferenceFrameFactor, jacobians_zero ) { + + // get values that are ideal + Pose2 trans(2.0, 3.0, 0.0); + Point2 global(5.0, 6.0); + Point2 local = trans.transform_from(global); + + PointReferenceFrameFactor tc(lA1, tA1, lB1); + Vector actCost = tc.evaluateError(global, trans, local), + expCost = zero(2); + EXPECT(assert_equal(expCost, actCost, 1e-5)); + + Matrix actualDT, actualDL, actualDF; + tc.evaluateError(global, trans, local, actualDF, actualDT, actualDL); + + Matrix numericalDT, numericalDL, numericalDF; + numericalDF = numericalDerivative31( + boost::bind(evaluateError_, tc, _1, _2, _3), + global, trans, local, 1e-5); + numericalDT = numericalDerivative32( + boost::bind(evaluateError_, tc, _1, _2, _3), + global, trans, local, 1e-5); + numericalDL = numericalDerivative33( + boost::bind(evaluateError_, tc, _1, _2, _3), + global, trans, local, 1e-5); + + EXPECT(assert_equal(numericalDF, actualDF)); + EXPECT(assert_equal(numericalDL, actualDL)); + EXPECT(assert_equal(numericalDT, actualDT)); +} + +/* ************************************************************************* */ +TEST_UNSAFE( ReferenceFrameFactor, converge_trans ) { + + // initial points + Point2 local1(2.0, 2.0), local2(4.0, 5.0), + global1(-1.0, 5.0), global2(2.0, 3.0); + Pose2 transIdeal(7.0, 3.0, M_PI/2); + + // verify direction + EXPECT(assert_equal(local1, transIdeal.transform_from(global1))); + EXPECT(assert_equal(local2, transIdeal.transform_from(global2))); + + // choose transform + // Pose2 trans = transIdeal; // ideal - works + // Pose2 trans = transIdeal * Pose2(0.1, 1.0, 0.00); // translation - works + // Pose2 trans = transIdeal * Pose2(10.1, 1.0, 0.00); // large translation - works + // Pose2 trans = transIdeal * Pose2(0.0, 0.0, 0.1); // small rotation - works + Pose2 trans = transIdeal * Pose2(-200.0, 100.0, 1.3); // combined - works + // Pose2 trans = transIdeal * Pose2(-200.0, 100.0, 2.0); // beyond pi/2 - fails + + NonlinearFactorGraph graph; + graph.add(PointReferenceFrameFactor(lB1, tA1, lA1)); + graph.add(PointReferenceFrameFactor(lB2, tA1, lA2)); + + // hard constraints on points + double error_gain = 1000.0; + graph.add(NonlinearEquality(lA1, local1, error_gain)); + graph.add(NonlinearEquality(lA2, local2, error_gain)); + graph.add(NonlinearEquality(lB1, global1, error_gain)); + graph.add(NonlinearEquality(lB2, global2, error_gain)); + + // create initial estimate + Values init; + init.insert(lA1, local1); + init.insert(lA2, local2); + init.insert(lB1, global1); + init.insert(lB2, global2); + init.insert(tA1, trans); + + // optimize + LevenbergMarquardtOptimizer solver(graph, init); + Values actual = solver.optimize(); + + Values expected; + expected.insert(lA1, local1); + expected.insert(lA2, local2); + expected.insert(lB1, global1); + expected.insert(lB2, global2); + expected.insert(tA1, transIdeal); + + EXPECT(assert_equal(expected, actual, 1e-4)); +} + +/* ************************************************************************* */ +TEST( ReferenceFrameFactor, converge_local ) { + + // initial points + Point2 global(-1.0, 2.0); + // Pose2 trans(1.5, 2.5, 0.3); // original + // Pose2 trans(1.5, 2.5, 1.0); // larger rotation + Pose2 trans(1.5, 2.5, 3.1); // significant rotation + + Point2 idealLocal = trans.transform_from(global); + + // perturb the initial estimate + // Point2 local = idealLocal; // Ideal case - works + // Point2 local = idealLocal + Point2(1.0, 0.0); // works + Point2 local = idealLocal + Point2(-10.0, 10.0); // works + + NonlinearFactorGraph graph; + double error_gain = 1000.0; + graph.add(PointReferenceFrameFactor(lB1, tA1, lA1)); + graph.add(NonlinearEquality(lB1, global, error_gain)); + graph.add(NonlinearEquality(tA1, trans, error_gain)); + + // create initial estimate + Values init; + init.insert(lA1, local); + init.insert(lB1, global); + init.insert(tA1, trans); + + // optimize + LevenbergMarquardtOptimizer solver(graph, init); + Values actual = solver.optimize(); + + CHECK(actual.exists(lA1)); + EXPECT(assert_equal(idealLocal, actual.at(lA1), 1e-5)); +} + +/* ************************************************************************* */ +TEST( ReferenceFrameFactor, converge_global ) { + + // initial points + Point2 local(2.0, 3.0); + // Pose2 trans(1.5, 2.5, 0.3); // original + // Pose2 trans(1.5, 2.5, 1.0); // larger rotation + Pose2 trans(1.5, 2.5, 3.1); // significant rotation + + Point2 idealForeign = trans.inverse().transform_from(local); + + // perturb the initial estimate + // Point2 global = idealForeign; // Ideal - works + // Point2 global = idealForeign + Point2(1.0, 0.0); // simple - works + Point2 global = idealForeign + Point2(10.0, -10.0); // larger - works + + NonlinearFactorGraph graph; + double error_gain = 1000.0; + graph.add(PointReferenceFrameFactor(lB1, tA1, lA1)); + graph.add(NonlinearEquality(lA1, local, error_gain)); + graph.add(NonlinearEquality(tA1, trans, error_gain)); + + // create initial estimate + Values init; + init.insert(lA1, local); + init.insert(lB1, global); + init.insert(tA1, trans); + + // optimize + LevenbergMarquardtOptimizer solver(graph, init); + Values actual = solver.optimize(); + + // verify + CHECK(actual.exists(lB1)); + EXPECT(assert_equal(idealForeign, actual.at(lB1), 1e-5)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ + +