diff --git a/nonlinear/NonlinearConstraint.h b/nonlinear/NonlinearConstraint.h index 2bc65922a..14cdc47a1 100644 --- a/nonlinear/NonlinearConstraint.h +++ b/nonlinear/NonlinearConstraint.h @@ -45,6 +45,7 @@ public: */ NonlinearConstraint(size_t dim, double mu = 1000.0): Base(noiseModel::Constrained::All(dim)), mu_(fabs(mu)), dim_(dim) {} + virtual ~NonlinearConstraint() {} /** returns the gain mu */ double mu() const { return mu_; } @@ -116,6 +117,7 @@ public: : Base(dim, mu), key_(key) { this->keys_.push_back(key); } + virtual ~NonlinearConstraint1() {} /* print */ void print(const std::string& s = "") const { @@ -173,6 +175,7 @@ protected: public: NonlinearEqualityConstraint1(const Key& key, size_t dim, double mu = 1000.0) : Base(key, dim, mu) {} + virtual ~NonlinearEqualityConstraint1() {} /** Always active, so fixed value for active() */ virtual bool active(const Config& c) const { return true; } @@ -206,6 +209,7 @@ public: this->keys_.push_back(key1); this->keys_.push_back(key2); } + virtual ~NonlinearConstraint2() {} /* print */ void print(const std::string& s = "") const { @@ -267,11 +271,119 @@ protected: public: NonlinearEqualityConstraint2(const Key1& key1, const Key2& key2, size_t dim, double mu = 1000.0) : Base(key1, key2, dim, mu) {} + virtual ~NonlinearEqualityConstraint2() {} + /** Always active, so fixed value for active() */ virtual bool active(const Config& c) const { return true; } }; +/** + * A ternary constraint + */ +template +class NonlinearConstraint3 : public NonlinearConstraint { + +protected: + typedef NonlinearConstraint3 This; + typedef NonlinearConstraint Base; + + /** keys for the constrained variables */ + Key1 key1_; + Key2 key2_; + Key3 key3_; + +public: + + /** + * Basic constructor + * @param key1 is the identifier for the first variable constrained + * @param key2 is the identifier for the second variable constrained + * @param key3 is the identifier for the second variable constrained + * @param dim is the size of the constraint (p) + * @param mu is the gain for the factor + */ + NonlinearConstraint3(const Key1& key1, const Key2& key2, const Key3& key3, + size_t dim, double mu = 1000.0) : + Base(dim, mu), key1_(key1), key2_(key2), key3_(key3) { + this->keys_.push_back(key1); + this->keys_.push_back(key2); + this->keys_.push_back(key3); + } + virtual ~NonlinearConstraint3() {} + + /* print */ + void print(const std::string& s = "") const { + std::cout << "NonlinearConstraint3 " << s << std::endl; + std::cout << "key1: " << (std::string) key1_ << std::endl; + std::cout << "key2: " << (std::string) key2_ << std::endl; + std::cout << "key3: " << (std::string) key3_ << std::endl; + std::cout << "mu: " << this->mu_ << std::endl; + } + + /** Check if two factors are equal. Note type is Factor and needs cast. */ + virtual bool equals(const Factor& f, double tol = 1e-9) const { + const This* p = dynamic_cast (&f); + if (p == NULL) return false; + return Base::equals(*p, tol) && (key1_ == p->key1_) && (key2_ == p->key2_) && (key3_ == p->key3_); + } + + /** error function g(x), switched depending on whether the constraint is active */ + inline Vector unwhitenedError(const Config& x) const { + if (!active(x)) { + return zero(this->dim()); + } + const Key1& j1 = key1_; + const Key2& j2 = key2_; + const Key3& j3 = key3_; + const X1& xj1 = x[j1]; + const X2& xj2 = x[j2]; + const X3& xj3 = x[j3]; + return evaluateError(xj1, xj2, xj3); + } + + /** Linearize from config */ + boost::shared_ptr linearize(const Config& c) const { + if (!active(c)) { + boost::shared_ptr factor; + return factor; + } + const Key1& j1 = key1_; const Key2& j2 = key2_; const Key3& j3 = key3_; + const X1& x1 = c[j1]; const X2& x2 = c[j2]; const X3& x3 = c[j3]; + Matrix grad1, grad2, grad3; + Vector g = -1.0 * evaluateError(x1, x2, x3, grad1, grad2, grad3); + SharedDiagonal model = noiseModel::Constrained::All(this->dim()); + return GaussianFactor::shared_ptr(new GaussianFactor(j1, grad1, j2, grad2, j3, grad3, g, model)); + } + + /** g(x) with optional derivative3 - does not depend on active */ + virtual Vector evaluateError(const X1& x1, const X2& x2, const X3& x3, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none, + boost::optional H3 = boost::none) const = 0; +}; + +/** + * Ternary Equality constraint - simply forces the value of active() to true + */ +template +class NonlinearEqualityConstraint3 : public NonlinearConstraint3 { + +protected: + typedef NonlinearEqualityConstraint3 This; + typedef NonlinearConstraint3 Base; + +public: + NonlinearEqualityConstraint3(const Key1& key1, const Key2& key2, const Key3& key3, + size_t dim, double mu = 1000.0) + : Base(key1, key2, key3, dim, mu) {} + virtual ~NonlinearEqualityConstraint3() {} + + /** Always active, so fixed value for active() */ + virtual bool active(const Config& c) const { return true; } +}; + + /** * Simple unary equality constraint - fixes a value for a variable */ @@ -288,6 +400,7 @@ public: NonlinearEquality1(const X& value, const Key& key1, double mu = 1000.0) : Base(key1, X::dim(), mu), value_(value) {} + virtual ~NonlinearEquality1() {} /** g(x) with optional derivative */ Vector evaluateError(const X& x1, boost::optional H1 = boost::none) const { @@ -313,6 +426,7 @@ public: NonlinearEquality2(const Key& key1, const Key& key2, double mu = 1000.0) : Base(key1, key2, X::dim(), mu) {} + virtual ~NonlinearEquality2() {} /** g(x) with optional derivative2 */ Vector evaluateError(const X& x1, const X& x2, diff --git a/nonlinear/NonlinearFactor.h b/nonlinear/NonlinearFactor.h index cc7cf745f..b6d8d26ee 100644 --- a/nonlinear/NonlinearFactor.h +++ b/nonlinear/NonlinearFactor.h @@ -388,7 +388,7 @@ namespace gtsam { /** Print */ void print(const std::string& s = "") const { - std::cout << "NonlinearFactor2 " << s << std::endl; + std::cout << "NonlinearFactor3 " << s << std::endl; std::cout << "key1: " << (std::string) key1_ << std::endl; std::cout << "key2: " << (std::string) key2_ << std::endl; std::cout << "key3: " << (std::string) key3_ << std::endl; @@ -419,7 +419,7 @@ namespace gtsam { boost::shared_ptr linearize(const Config& c) const { const X1& x1 = c[key1_]; const X2& x2 = c[key2_]; - const X2& x3 = c[key3_]; + const X3& x3 = c[key3_]; Matrix A1, A2, A3; Vector b = -evaluateError(x1, x2, x3, A1, A2, A3); // TODO pass unwhitened + noise model to Gaussian factor diff --git a/slam/BetweenConstraint.h b/slam/BetweenConstraint.h index d009c520c..8f77a1253 100644 --- a/slam/BetweenConstraint.h +++ b/slam/BetweenConstraint.h @@ -6,7 +6,7 @@ #pragma once -#include +#include "NonlinearConstraint.h" namespace gtsam { diff --git a/slam/BoundingConstraint.h b/slam/BoundingConstraint.h index ab1dc25d7..c438ec510 100644 --- a/slam/BoundingConstraint.h +++ b/slam/BoundingConstraint.h @@ -6,8 +6,8 @@ #pragma once -#include -#include +#include "Lie.h" +#include "NonlinearConstraint.h" namespace gtsam { diff --git a/slam/Makefile.am b/slam/Makefile.am index fd65d3505..23dec1188 100644 --- a/slam/Makefile.am +++ b/slam/Makefile.am @@ -28,7 +28,7 @@ check_PROGRAMS += tests/testSimulated3D headers += BetweenFactor.h PriorFactor.h # General constraints -headers += BetweenConstraint.h BoundingConstraint.h +headers += BetweenConstraint.h BoundingConstraint.h TransformConstraint.h # 2D Pose SLAM sources += pose2SLAM.cpp Pose2SLAMOptimizer.cpp dataset.cpp diff --git a/slam/TransformConstraint.h b/slam/TransformConstraint.h new file mode 100644 index 000000000..6dde14166 --- /dev/null +++ b/slam/TransformConstraint.h @@ -0,0 +1,66 @@ +/* + * @file TransformConstraint.h + * @brief A constraint for combining graphs by common landmarks and a transform node + * @author Alex Cunningham + */ + +#pragma once + +#include "NonlinearConstraint.h" + +namespace gtsam { + +/** + * A constraint between two landmarks in separate maps + * Templated on: + * Config : The overall config + * PKey : Key of landmark being constrained + * Point : Type of landmark + * TKey : Key of transform used + * Transform : Transform variable class + * + * The Point and Transform concepts must be Lie types, and the transform + * relationship "Point = transform_from(Transform, Point)" must exist. + * + * This base class should be specialized to implement the cost function for + * specific classes of landmarks + */ +template +class TransformConstraint : public NonlinearEqualityConstraint3 { + +public: + typedef NonlinearEqualityConstraint3 Base; + typedef TransformConstraint This; + + /** + * General constructor with all of the keys to variables in the map + * Extracts everything necessary from the key types + */ + TransformConstraint(const PKey& foreignKey, const TKey& transKey, const PKey& localKey, double mu = 1000.0) + : Base(foreignKey, transKey, localKey, Point().dim(), mu) {} + + virtual ~TransformConstraint(){} + + /** Combined cost and derivative function using boost::optional */ + virtual Vector evaluateError(const Point& foreign, const Transform& trans, const Point& local, + boost::optional Dforeign = boost::none, + boost::optional Dtrans = boost::none, + boost::optional Dlocal = boost::none) const { + if (Dforeign) { + Matrix Af; + transform_from(trans, foreign, boost::none, Af); + *Dforeign = Af; + } + if (Dtrans) { + Matrix At; + transform_from(trans, foreign, At, boost::none); + *Dtrans = At; + } + if (Dlocal) { + Point dummy; + *Dlocal = -1* eye(dummy.dim()); + } + return gtsam::logmap(gtsam::between(local, transform_from(trans, foreign))); + } +}; +} // \namespace gtsam diff --git a/slam/simulated2DConstraints.h b/slam/simulated2DConstraints.h index 2ab247981..4e7d9ca76 100644 --- a/slam/simulated2DConstraints.h +++ b/slam/simulated2DConstraints.h @@ -10,10 +10,10 @@ #include // TODO: remove -#include -#include -#include -#include +#include "NonlinearConstraint.h" +#include "BetweenConstraint.h" +#include "BoundingConstraint.h" +#include "simulated2D.h" // \namespace diff --git a/tests/Makefile.am b/tests/Makefile.am index 996627194..5553dd82c 100644 --- a/tests/Makefile.am +++ b/tests/Makefile.am @@ -11,6 +11,7 @@ check_PROGRAMS += testNonlinearEquality testNonlinearFactor testNonlinearFactorG check_PROGRAMS += testNonlinearOptimizer testSubgraphPreconditioner check_PROGRAMS += testSymbolicBayesNet testSymbolicFactorGraph testTupleConfig check_PROGRAMS += testNonlinearEqualityConstraint testBoundingConstraint +check_PROGRAMS += testTransformConstraint if USE_LDL check_PROGRAMS += testConstraintOptimizer diff --git a/tests/testTransformConstraint.cpp b/tests/testTransformConstraint.cpp new file mode 100644 index 000000000..0cd1d7500 --- /dev/null +++ b/tests/testTransformConstraint.cpp @@ -0,0 +1,263 @@ +/* + * @file testTransformConstraint.cpp + * @author Alex Cunningham + */ + +#include + +#include + +#include + +#include +#include +#include + +#include +#include + +#include +#include + +// implementations +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; +using namespace boost; + +typedef TypedSymbol PoseKey; +typedef TypedSymbol PointKey; +typedef TypedSymbol TransformKey; + +typedef LieConfig PoseConfig; +typedef LieConfig PointConfig; +typedef LieConfig TransformConfig; + +typedef TupleConfig3< PoseConfig, PointConfig, TransformConfig > DDFConfig; +typedef NonlinearFactorGraph DDFGraph; +typedef NonlinearOptimizer Optimizer; + +typedef NonlinearEquality PoseConstraint; +typedef NonlinearEquality PointConstraint; +typedef NonlinearEquality TransformPriorConstraint; + +typedef TransformConstraint PointTransformConstraint; + +PointKey lA1(1), lA2(2), lB1(3); +TransformKey t1(1); + +/* ************************************************************************* */ +TEST( TransformConstraint, equals ) { + PointTransformConstraint c1(lB1, t1, lA1), + c2(lB1, t1, lA1), + c3(lB1, t1, lA2); + + CHECK(assert_equal(c1, c1)); + CHECK(assert_equal(c1, c2)); + CHECK(!c1.equals(c3)); +} + +/* ************************************************************************* */ +TEST( TransformConstraint, jacobians ) { + + // from examples below + Point2 local(2.0, 3.0), global(-1.0, 2.0); + Pose2 trans(1.5, 2.5, 0.3); + + PointTransformConstraint tc(lA1, t1, lB1); + Matrix actualDT, actualDL, actualDF; + tc.evaluateError(global, trans, local, actualDF, actualDT, actualDL); + + Matrix numericalDT, numericalDL, numericalDF; + numericalDF = numericalDerivative31( + boost::bind(&PointTransformConstraint::evaluateError, ref(tc), _1, _2, _3, boost::none, boost::none, boost::none), + global, trans, local, 1e-5); + numericalDT = numericalDerivative32( + boost::bind(&PointTransformConstraint::evaluateError, ref(tc), _1, _2, _3, boost::none, boost::none, boost::none), + global, trans, local, 1e-5); + numericalDL = numericalDerivative33( + boost::bind(&PointTransformConstraint::evaluateError, ref(tc), _1, _2, _3, boost::none, boost::none, boost::none), + global, trans, local, 1e-5); + + CHECK(assert_equal(numericalDF, actualDF)); + CHECK(assert_equal(numericalDL, actualDL)); + CHECK(assert_equal(numericalDT, actualDT)); +} + +/* ************************************************************************* */ +TEST( TransformConstraint, jacobians_zero ) { + + // get values that are ideal + Pose2 trans(2.0, 3.0, 0.0); + Point2 global(5.0, 6.0); + Point2 local = transform_from(trans, global); + + PointTransformConstraint tc(lA1, t1, lB1); + Vector actCost = tc.evaluateError(global, trans, local), + expCost = zero(2); + CHECK(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(&PointTransformConstraint::evaluateError, ref(tc), _1, _2, _3, boost::none, boost::none, boost::none), + global, trans, local, 1e-5); + numericalDT = numericalDerivative32( + boost::bind(&PointTransformConstraint::evaluateError, ref(tc), _1, _2, _3, boost::none, boost::none, boost::none), + global, trans, local, 1e-5); + numericalDL = numericalDerivative33( + boost::bind(&PointTransformConstraint::evaluateError, ref(tc), _1, _2, _3, boost::none, boost::none, boost::none), + global, trans, local, 1e-5); + + CHECK(assert_equal(numericalDF, actualDF)); + CHECK(assert_equal(numericalDL, actualDL)); + CHECK(assert_equal(numericalDT, actualDT)); +} + +/* ************************************************************************* */ +TEST( TransformConstraint, 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 + CHECK(assert_equal(local1, transform_from(transIdeal, global1))); + CHECK(assert_equal(local2, transform_from(transIdeal, 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 + + // keys + PointKey localK1(1), localK2(2), + globalK1(3), globalK2(4); + TransformKey transK(1); + + DDFGraph graph; + + graph.add(PointTransformConstraint(globalK1, transK, localK1)); + graph.add(PointTransformConstraint(globalK2, transK, localK2)); + + // hard constraints on points + double error_gain = 1000.0; + graph.add(PointConstraint(localK1, local1, error_gain)); + graph.add(PointConstraint(localK2, local2, error_gain)); + graph.add(PointConstraint(globalK1, global1, error_gain)); + graph.add(PointConstraint(globalK2, global2, error_gain)); + + // create initial estimate + DDFConfig init; + init.insert(localK1, local1); + init.insert(localK2, local2); + init.insert(globalK1, global1); + init.insert(globalK2, global2); + init.insert(transK, trans); + + // optimize + Optimizer::shared_config actual = Optimizer::optimizeLM(graph, init); + + DDFConfig expected; + expected.insert(localK1, local1); + expected.insert(localK2, local2); + expected.insert(globalK1, global1); + expected.insert(globalK2, global2); + expected.insert(transK, transIdeal); + + CHECK(assert_equal(expected, *actual, 1e-4)); +} + +/* ************************************************************************* */ +TEST( TransformConstraint, 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 = transform_from(trans, 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 + + + // keys + PointKey localK(1), globalK(2); + TransformKey transK(1); + + DDFGraph graph; + double error_gain = 1000.0; + graph.add(PointTransformConstraint(globalK, transK, localK)); + graph.add(PointConstraint(globalK, global, error_gain)); + graph.add(TransformPriorConstraint(transK, trans, error_gain)); + + // create initial estimate + DDFConfig init; + init.insert(localK, local); + init.insert(globalK, global); + init.insert(transK, trans); + + // optimize + Optimizer::shared_config actual = Optimizer::optimizeLM(graph, init); + + CHECK(assert_equal(idealLocal, actual->at(localK), 1e-5)); +} + +/* ************************************************************************* */ +TEST( TransformConstraint, 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 = transform_from(inverse(trans), 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 + + // keys + PointKey localK(1), globalK(2); + TransformKey transK(1); + + DDFGraph graph; + double error_gain = 1000.0; + graph.add(PointTransformConstraint(globalK, transK, localK)); + graph.add(PointConstraint(localK, local, error_gain)); + graph.add(TransformPriorConstraint(transK, trans, error_gain)); + + // create initial estimate + DDFConfig init; + init.insert(localK, local); + init.insert(globalK, global); + init.insert(transK, trans); + + // optimize + Optimizer::shared_config actual = Optimizer::optimizeLM(graph, init); + + // verify + CHECK(assert_equal(idealForeign, actual->at(globalK), 1e-5)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ + +