diff --git a/.cproject b/.cproject index bcb1b93d9..a35bd2fed 100644 --- a/.cproject +++ b/.cproject @@ -375,14 +375,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -409,6 +401,7 @@ make + tests/testBayesTree.run true false @@ -416,6 +409,7 @@ make + testBinaryBayesNet.run true false @@ -463,6 +457,7 @@ make + testSymbolicBayesNet.run true false @@ -470,6 +465,7 @@ make + tests/testSymbolicFactor.run true false @@ -477,6 +473,7 @@ make + testSymbolicFactorGraph.run true false @@ -492,11 +489,20 @@ make + tests/testBayesTree true false true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -523,7 +529,6 @@ make - testGraph.run true false @@ -595,7 +600,6 @@ make - testInference.run true false @@ -603,7 +607,6 @@ make - testGaussianFactor.run true false @@ -611,7 +614,6 @@ make - testJunctionTree.run true false @@ -619,7 +621,6 @@ make - testSymbolicBayesNet.run true false @@ -627,7 +628,6 @@ make - testSymbolicFactorGraph.run true false @@ -681,22 +681,6 @@ true true - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - make -j2 @@ -713,6 +697,22 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + make -j2 @@ -737,7 +737,15 @@ true true - + + make + -j2 + all + true + true + true + + make -j2 check @@ -745,6 +753,14 @@ true true + + make + -j2 + clean + true + true + true + make -j2 @@ -785,15 +801,7 @@ true true - - make - -j2 - all - true - true - true - - + make -j2 check @@ -801,14 +809,6 @@ true true - - make - -j2 - clean - true - true - true - make -j2 @@ -1033,14 +1033,6 @@ true true - - make - -j2 - testNonlinearEqualityConstraint.run - true - true - true - make -j2 @@ -1139,6 +1131,7 @@ make + testErrors.run true false @@ -1546,7 +1539,6 @@ make - testSimulated2DOriented.run true false @@ -1586,7 +1578,6 @@ make - testSimulated2D.run true false @@ -1594,7 +1585,6 @@ make - testSimulated3D.run true false @@ -1842,7 +1832,6 @@ make - tests/testGaussianISAM2 true false @@ -1864,6 +1853,46 @@ true true + + make + -j2 + install + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + dist + true + true + true + make -j2 @@ -1960,62 +1989,6 @@ true true - - make - -j2 - install - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - dist - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - install - true - true - true - make -j2 @@ -2056,6 +2029,22 @@ true true + + make + -j2 + check + true + true + true + + + make + -j2 + install + true + true + true + diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 44a2f1417..fd93d39fc 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -296,8 +296,6 @@ void Constrained::print(const std::string& name) const { /* ************************************************************************* */ Vector Constrained::whiten(const Vector& v) const { - // ediv_ does the right thing with the errors -// return ediv_(v, sigmas_); const Vector& a = v; const Vector& b = sigmas_; size_t n = a.size(); @@ -305,7 +303,7 @@ Vector Constrained::whiten(const Vector& v) const { Vector c(n); for( size_t i = 0; i < n; i++ ) { const double& ai = a(i), &bi = b(i); - c(i) = (bi==0.0) ? ai : ai/bi; + c(i) = (bi==0.0) ? ai : ai/bi; // NOTE: not ediv_() } return c; } @@ -313,6 +311,7 @@ Vector Constrained::whiten(const Vector& v) const { /* ************************************************************************* */ double Constrained::distance(const Vector& v) const { Vector w = Diagonal::whiten(v); // get noisemodel for constrained elements + // TODO Find a better way of doing these checks for (size_t i=0; i - -namespace gtsam { - -/* ************************************************************************* */ -/** - * Simple unary equality constraint - fixes a value for a variable - */ -template -class NonlinearEquality1 : public NonlinearFactor1 { - -public: - typedef typename KEY::Value X; - -protected: - typedef NonlinearFactor1 Base; - - /** default constructor to allow for serialization */ - NonlinearEquality1() {} - - X value_; /// fixed value for variable - - GTSAM_CONCEPT_MANIFOLD_TYPE(X); - GTSAM_CONCEPT_TESTABLE_TYPE(X); - -public: - - typedef boost::shared_ptr > shared_ptr; - - NonlinearEquality1(const X& value, const KEY& key1, double mu = 1000.0) - : Base(noiseModel::Constrained::All(value.dim(), fabs(mu)), key1), value_(value) {} - - virtual ~NonlinearEquality1() {} - - /** g(x) with optional derivative */ - Vector evaluateError(const X& x1, boost::optional H = boost::none) const { - if (H) (*H) = eye(x1.dim()); - // manifold equivalent of h(x)-z -> log(z,h(x)) - return value_.localCoordinates(x1); - } - - /** Print */ - virtual void print(const std::string& s = "") const { - std::cout << s << ": NonlinearEquality1(" - << (std::string) this->key_ << "),"<< "\n"; - this->noiseModel_->print(); - value_.print("Value"); - } - -private: - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NonlinearFactor1", - boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(value_); - } -}; - -/* ************************************************************************* */ -/** - * Simple binary equality constraint - this constraint forces two factors to - * be the same. - */ -template -class NonlinearEquality2 : public NonlinearFactor2 { -public: - typedef typename KEY::Value X; - -protected: - typedef NonlinearFactor2 Base; - - GTSAM_CONCEPT_MANIFOLD_TYPE(X); - - /** default constructor to allow for serialization */ - NonlinearEquality2() {} - -public: - - typedef boost::shared_ptr > shared_ptr; - - NonlinearEquality2(const KEY& key1, const KEY& key2, double mu = 1000.0) - : Base(noiseModel::Constrained::All(X::Dim(), fabs(mu)), key1, key2) {} - virtual ~NonlinearEquality2() {} - - /** g(x) with optional derivative2 */ - Vector evaluateError(const X& x1, const X& x2, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const { - const size_t p = X::Dim(); - if (H1) *H1 = -eye(p); - if (H2) *H2 = eye(p); - return x1.localCoordinates(x2); - } - -private: - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NonlinearFactor2", - boost::serialization::base_object(*this)); - } -}; - -} diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index 84fdb94a5..8f2db79fa 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -154,7 +154,111 @@ namespace gtsam { ar & BOOST_SERIALIZATION_NVP(error_gain_); } - }; // NonlinearEquality + }; // \class NonlinearEquality + + /* ************************************************************************* */ + /** + * Simple unary equality constraint - fixes a value for a variable + */ + template + class NonlinearEquality1 : public NonlinearFactor1 { + + public: + typedef typename KEY::Value X; + + protected: + typedef NonlinearFactor1 Base; + + /** default constructor to allow for serialization */ + NonlinearEquality1() {} + + X value_; /// fixed value for variable + + GTSAM_CONCEPT_MANIFOLD_TYPE(X); + GTSAM_CONCEPT_TESTABLE_TYPE(X); + + public: + + typedef boost::shared_ptr > shared_ptr; + + NonlinearEquality1(const X& value, const KEY& key1, double mu = 1000.0) + : Base(noiseModel::Constrained::All(value.dim(), fabs(mu)), key1), value_(value) {} + + virtual ~NonlinearEquality1() {} + + /** g(x) with optional derivative */ + Vector evaluateError(const X& x1, boost::optional H = boost::none) const { + if (H) (*H) = eye(x1.dim()); + // manifold equivalent of h(x)-z -> log(z,h(x)) + return value_.localCoordinates(x1); + } + + /** Print */ + virtual void print(const std::string& s = "") const { + std::cout << s << ": NonlinearEquality1(" + << (std::string) this->key_ << "),"<< "\n"; + this->noiseModel_->print(); + value_.print("Value"); + } + + private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NonlinearFactor1", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(value_); + } + }; // \NonlinearEquality1 + + /* ************************************************************************* */ + /** + * Simple binary equality constraint - this constraint forces two factors to + * be the same. + */ + template + class NonlinearEquality2 : public NonlinearFactor2 { + public: + typedef typename KEY::Value X; + + protected: + typedef NonlinearFactor2 Base; + + GTSAM_CONCEPT_MANIFOLD_TYPE(X); + + /** default constructor to allow for serialization */ + NonlinearEquality2() {} + + public: + + typedef boost::shared_ptr > shared_ptr; + + NonlinearEquality2(const KEY& key1, const KEY& key2, double mu = 1000.0) + : Base(noiseModel::Constrained::All(X::Dim(), fabs(mu)), key1, key2) {} + virtual ~NonlinearEquality2() {} + + /** g(x) with optional derivative2 */ + Vector evaluateError(const X& x1, const X& x2, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const { + const size_t p = X::Dim(); + if (H1) *H1 = -eye(p); + if (H2) *H2 = eye(p); + return x1.localCoordinates(x2); + } + + private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NonlinearFactor2", + boost::serialization::base_object(*this)); + } + }; // \NonlinearEquality2 } // namespace gtsam diff --git a/gtsam/slam/BoundingConstraint.h b/gtsam/slam/BoundingConstraint.h index 289f0667c..e8f59cb2d 100644 --- a/gtsam/slam/BoundingConstraint.h +++ b/gtsam/slam/BoundingConstraint.h @@ -18,7 +18,7 @@ #pragma once #include -#include +#include namespace gtsam { diff --git a/gtsam/slam/simulated2DConstraints.h b/gtsam/slam/simulated2DConstraints.h index 04dbd0231..b09a5c638 100644 --- a/gtsam/slam/simulated2DConstraints.h +++ b/gtsam/slam/simulated2DConstraints.h @@ -21,7 +21,7 @@ #include -#include +#include #include #include #include diff --git a/tests/Makefile.am b/tests/Makefile.am index fb3de57f7..2807e1fd8 100644 --- a/tests/Makefile.am +++ b/tests/Makefile.am @@ -19,7 +19,6 @@ check_PROGRAMS += testSymbolicBayesNet testSymbolicFactorGraph check_PROGRAMS += testTupleValues check_PROGRAMS += testNonlinearISAM check_PROGRAMS += testBoundingConstraint -check_PROGRAMS += testNonlinearEqualityConstraint check_PROGRAMS += testPose2SLAMwSPCG check_PROGRAMS += testGaussianISAM2 check_PROGRAMS += testExtendedKalmanFilter diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index 5e988f2f3..31e870f00 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -18,6 +18,8 @@ #include #include +#include +#include #include #include #include @@ -27,6 +29,10 @@ using namespace std; using namespace gtsam; +namespace eq2D = gtsam::simulated2D::equality_constraints; + +static const double tol = 1e-5; + typedef TypedSymbol PoseKey; typedef Values PoseValues; typedef PriorFactor PosePrior; @@ -243,6 +249,346 @@ TEST ( NonlinearEquality, allow_error_optimize_with_factors ) { EXPECT(assert_equal(expected, *result.values())); } +/* ************************************************************************* */ +SharedDiagonal hard_model = noiseModel::Constrained::All(2); +SharedDiagonal soft_model = noiseModel::Isotropic::Sigma(2, 1.0); + +typedef NonlinearFactorGraph Graph; +typedef boost::shared_ptr shared_graph; +typedef boost::shared_ptr shared_values; +typedef NonlinearOptimizer Optimizer; + +/* ************************************************************************* */ +TEST( testNonlinearEqualityConstraint, unary_basics ) { + Point2 pt(1.0, 2.0); + simulated2D::PoseKey key(1); + double mu = 1000.0; + eq2D::UnaryEqualityConstraint constraint(pt, key, mu); + + simulated2D::Values config1; + config1.insert(key, pt); + EXPECT(constraint.active(config1)); + EXPECT(assert_equal(zero(2), constraint.evaluateError(pt), tol)); + EXPECT(assert_equal(zero(2), constraint.unwhitenedError(config1), tol)); + EXPECT_DOUBLES_EQUAL(0.0, constraint.error(config1), tol); + + simulated2D::Values config2; + Point2 ptBad1(2.0, 2.0); + config2.insert(key, ptBad1); + EXPECT(constraint.active(config2)); + EXPECT(assert_equal(Vector_(2, 1.0, 0.0), constraint.evaluateError(ptBad1), tol)); + EXPECT(assert_equal(Vector_(2, 1.0, 0.0), constraint.unwhitenedError(config2), tol)); + EXPECT_DOUBLES_EQUAL(500.0, constraint.error(config2), tol); +} + +/* ************************************************************************* */ +TEST( testNonlinearEqualityConstraint, unary_linearization ) { + Point2 pt(1.0, 2.0); + simulated2D::PoseKey key(1); + double mu = 1000.0; + Ordering ordering; + ordering += key; + eq2D::UnaryEqualityConstraint constraint(pt, key, mu); + + simulated2D::Values config1; + config1.insert(key, pt); + GaussianFactor::shared_ptr actual1 = constraint.linearize(config1, ordering); + GaussianFactor::shared_ptr expected1(new JacobianFactor(ordering[key], eye(2,2), zero(2), hard_model)); + EXPECT(assert_equal(*expected1, *actual1, tol)); + + simulated2D::Values config2; + Point2 ptBad(2.0, 2.0); + config2.insert(key, ptBad); + GaussianFactor::shared_ptr actual2 = constraint.linearize(config2, ordering); + GaussianFactor::shared_ptr expected2(new JacobianFactor(ordering[key], eye(2,2), Vector_(2,-1.0,0.0), hard_model)); + EXPECT(assert_equal(*expected2, *actual2, tol)); +} + +/* ************************************************************************* */ +TEST( testNonlinearEqualityConstraint, unary_simple_optimization ) { + // create a single-node graph with a soft and hard constraint to + // ensure that the hard constraint overrides the soft constraint + Point2 truth_pt(1.0, 2.0); + simulated2D::PoseKey key(1); + double mu = 10.0; + eq2D::UnaryEqualityConstraint::shared_ptr constraint( + new eq2D::UnaryEqualityConstraint(truth_pt, key, mu)); + + Point2 badPt(100.0, -200.0); + simulated2D::Prior::shared_ptr factor( + new simulated2D::Prior(badPt, soft_model, key)); + + shared_graph graph(new Graph()); + graph->push_back(constraint); + graph->push_back(factor); + + shared_values initValues(new simulated2D::Values()); + initValues->insert(key, badPt); + + // verify error values + EXPECT(constraint->active(*initValues)); + + simulated2D::Values expected; + expected.insert(key, truth_pt); + EXPECT(constraint->active(expected)); + EXPECT_DOUBLES_EQUAL(0.0, constraint->error(expected), tol); + + Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initValues); + EXPECT(assert_equal(expected, *actual, tol)); +} + +/* ************************************************************************* */ +TEST( testNonlinearEqualityConstraint, odo_basics ) { + Point2 x1(1.0, 2.0), x2(2.0, 3.0), odom(1.0, 1.0); + simulated2D::PoseKey key1(1), key2(2); + double mu = 1000.0; + eq2D::OdoEqualityConstraint constraint(odom, key1, key2, mu); + + simulated2D::Values config1; + config1.insert(key1, x1); + config1.insert(key2, x2); + EXPECT(constraint.active(config1)); + EXPECT(assert_equal(zero(2), constraint.evaluateError(x1, x2), tol)); + EXPECT(assert_equal(zero(2), constraint.unwhitenedError(config1), tol)); + EXPECT_DOUBLES_EQUAL(0.0, constraint.error(config1), tol); + + simulated2D::Values config2; + Point2 x1bad(2.0, 2.0); + Point2 x2bad(2.0, 2.0); + config2.insert(key1, x1bad); + config2.insert(key2, x2bad); + EXPECT(constraint.active(config2)); + EXPECT(assert_equal(Vector_(2, -1.0, -1.0), constraint.evaluateError(x1bad, x2bad), tol)); + EXPECT(assert_equal(Vector_(2, -1.0, -1.0), constraint.unwhitenedError(config2), tol)); + EXPECT_DOUBLES_EQUAL(1000.0, constraint.error(config2), tol); +} + +/* ************************************************************************* */ +TEST( testNonlinearEqualityConstraint, odo_linearization ) { + Point2 x1(1.0, 2.0), x2(2.0, 3.0), odom(1.0, 1.0); + simulated2D::PoseKey key1(1), key2(2); + double mu = 1000.0; + Ordering ordering; + ordering += key1, key2; + eq2D::OdoEqualityConstraint constraint(odom, key1, key2, mu); + + simulated2D::Values config1; + config1.insert(key1, x1); + config1.insert(key2, x2); + GaussianFactor::shared_ptr actual1 = constraint.linearize(config1, ordering); + GaussianFactor::shared_ptr expected1( + new JacobianFactor(ordering[key1], -eye(2,2), ordering[key2], + eye(2,2), zero(2), hard_model)); + EXPECT(assert_equal(*expected1, *actual1, tol)); + + simulated2D::Values config2; + Point2 x1bad(2.0, 2.0); + Point2 x2bad(2.0, 2.0); + config2.insert(key1, x1bad); + config2.insert(key2, x2bad); + GaussianFactor::shared_ptr actual2 = constraint.linearize(config2, ordering); + GaussianFactor::shared_ptr expected2( + new JacobianFactor(ordering[key1], -eye(2,2), ordering[key2], + eye(2,2), Vector_(2, 1.0, 1.0), hard_model)); + EXPECT(assert_equal(*expected2, *actual2, tol)); +} + +/* ************************************************************************* */ +TEST( testNonlinearEqualityConstraint, odo_simple_optimize ) { + // create a two-node graph, connected by an odometry constraint, with + // a hard prior on one variable, and a conflicting soft prior + // on the other variable - the constraints should override the soft constraint + Point2 truth_pt1(1.0, 2.0), truth_pt2(3.0, 2.0); + simulated2D::PoseKey key1(1), key2(2); + + // hard prior on x1 + eq2D::UnaryEqualityConstraint::shared_ptr constraint1( + new eq2D::UnaryEqualityConstraint(truth_pt1, key1)); + + // soft prior on x2 + Point2 badPt(100.0, -200.0); + simulated2D::Prior::shared_ptr factor( + new simulated2D::Prior(badPt, soft_model, key2)); + + // odometry constraint + eq2D::OdoEqualityConstraint::shared_ptr constraint2( + new eq2D::OdoEqualityConstraint( + truth_pt1.between(truth_pt2), key1, key2)); + + shared_graph graph(new Graph()); + graph->push_back(constraint1); + graph->push_back(constraint2); + graph->push_back(factor); + + shared_values initValues(new simulated2D::Values()); + initValues->insert(key1, Point2()); + initValues->insert(key2, badPt); + + Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initValues); + simulated2D::Values expected; + expected.insert(key1, truth_pt1); + expected.insert(key2, truth_pt2); + CHECK(assert_equal(expected, *actual, tol)); +} + +/* ********************************************************************* */ +TEST (testNonlinearEqualityConstraint, two_pose ) { + /* + * Determining a ground truth linear system + * with two poses seeing one landmark, with each pose + * constrained to a particular value + */ + + shared_graph graph(new Graph()); + + simulated2D::PoseKey x1(1), x2(2); + simulated2D::PointKey l1(1), l2(2); + Point2 pt_x1(1.0, 1.0), + pt_x2(5.0, 6.0); + graph->add(eq2D::UnaryEqualityConstraint(pt_x1, x1)); + graph->add(eq2D::UnaryEqualityConstraint(pt_x2, x2)); + + Point2 z1(0.0, 5.0); + SharedNoiseModel sigma(noiseModel::Isotropic::Sigma(2, 0.1)); + graph->add(simulated2D::Measurement(z1, sigma, x1,l1)); + + Point2 z2(-4.0, 0.0); + graph->add(simulated2D::Measurement(z2, sigma, x2,l2)); + + graph->add(eq2D::PointEqualityConstraint(l1, l2)); + + shared_values initialEstimate(new simulated2D::Values()); + initialEstimate->insert(x1, pt_x1); + initialEstimate->insert(x2, Point2()); + initialEstimate->insert(l1, Point2(1.0, 6.0)); // ground truth + initialEstimate->insert(l2, Point2(-4.0, 0.0)); // starting with a separate reference frame + + Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initialEstimate); + + simulated2D::Values expected; + expected.insert(x1, pt_x1); + expected.insert(l1, Point2(1.0, 6.0)); + expected.insert(l2, Point2(1.0, 6.0)); + expected.insert(x2, Point2(5.0, 6.0)); + CHECK(assert_equal(expected, *actual, 1e-5)); +} + +/* ********************************************************************* */ +TEST (testNonlinearEqualityConstraint, map_warp ) { + // get a graph + shared_graph graph(new Graph()); + + // keys + simulated2D::PoseKey x1(1), x2(2); + simulated2D::PointKey l1(1), l2(2); + + // constant constraint on x1 + Point2 pose1(1.0, 1.0); + graph->add(eq2D::UnaryEqualityConstraint(pose1, x1)); + + SharedDiagonal sigma = noiseModel::Isotropic::Sigma(1,0.1); + + // measurement from x1 to l1 + Point2 z1(0.0, 5.0); + graph->add(simulated2D::Measurement(z1, sigma, x1, l1)); + + // measurement from x2 to l2 + Point2 z2(-4.0, 0.0); + graph->add(simulated2D::Measurement(z2, sigma, x2, l2)); + + // equality constraint between l1 and l2 + graph->add(eq2D::PointEqualityConstraint(l1, l2)); + + // create an initial estimate + shared_values initialEstimate(new simulated2D::Values()); + initialEstimate->insert(x1, Point2( 1.0, 1.0)); + initialEstimate->insert(l1, Point2( 1.0, 6.0)); + initialEstimate->insert(l2, Point2(-4.0, 0.0)); // starting with a separate reference frame + initialEstimate->insert(x2, Point2( 0.0, 0.0)); // other pose starts at origin + + // optimize + Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initialEstimate); + + simulated2D::Values expected; + expected.insert(x1, Point2(1.0, 1.0)); + expected.insert(l1, Point2(1.0, 6.0)); + expected.insert(l2, Point2(1.0, 6.0)); + expected.insert(x2, Point2(5.0, 6.0)); + CHECK(assert_equal(expected, *actual, tol)); +} + +// make a realistic calibration matrix +double fov = 60; // degrees +size_t w=640,h=480; +Cal3_S2 K(fov,w,h); +boost::shared_ptr shK(new Cal3_S2(K)); + +// typedefs for visual SLAM example +typedef visualSLAM::Values VValues; +typedef boost::shared_ptr shared_vconfig; +typedef visualSLAM::Graph VGraph; +typedef NonlinearOptimizer VOptimizer; + +// factors for visual slam +typedef NonlinearEquality2 Point3Equality; + +/* ********************************************************************* */ +TEST (testNonlinearEqualityConstraint, stereo_constrained ) { + + // create initial estimates + Rot3 faceDownY(Matrix_(3,3, + 1.0, 0.0, 0.0, + 0.0, 0.0, 1.0, + 0.0, -1.0, 0.0)); + Pose3 pose1(faceDownY, Point3()); // origin, left camera + SimpleCamera camera1(K, pose1); + Pose3 pose2(faceDownY, Point3(2.0, 0.0, 0.0)); // 2 units to the left + SimpleCamera camera2(K, pose2); + Point3 landmark(1.0, 5.0, 0.0); //centered between the cameras, 5 units away + + // keys + visualSLAM::PoseKey x1(1), x2(2); + visualSLAM::PointKey l1(1), l2(2); + + // create graph + VGraph::shared_graph graph(new VGraph()); + + // create equality constraints for poses + graph->addPoseConstraint(1, camera1.pose()); + graph->addPoseConstraint(2, camera2.pose()); + + // create factors + SharedDiagonal vmodel = noiseModel::Unit::Create(3); + graph->addMeasurement(camera1.project(landmark), vmodel, 1, 1, shK); + graph->addMeasurement(camera2.project(landmark), vmodel, 2, 2, shK); + + // add equality constraint + graph->add(Point3Equality(l1, l2)); + + // create initial data + Point3 landmark1(0.5, 5.0, 0.0); + Point3 landmark2(1.5, 5.0, 0.0); + + shared_vconfig initValues(new VValues()); + initValues->insert(x1, pose1); + initValues->insert(x2, pose2); + initValues->insert(l1, landmark1); + initValues->insert(l2, landmark2); + + // optimize + VOptimizer::shared_values actual = VOptimizer::optimizeLM(graph, initValues); + + // create config + VValues truthValues; + truthValues.insert(x1, camera1.pose()); + truthValues.insert(x2, camera2.pose()); + truthValues.insert(l1, landmark); + truthValues.insert(l2, landmark); + + // check if correct + CHECK(assert_equal(truthValues, *actual, 1e-5)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/tests/testNonlinearEqualityConstraint.cpp b/tests/testNonlinearEqualityConstraint.cpp deleted file mode 100644 index f9966695b..000000000 --- a/tests/testNonlinearEqualityConstraint.cpp +++ /dev/null @@ -1,375 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testNonlinearEqualityConstraint.cpp - * @author Alex Cunningham - */ - -#include - -#include -#include -#include -#include - -namespace eq2D = gtsam::simulated2D::equality_constraints; - -using namespace std; -using namespace gtsam; - -static const double tol = 1e-5; - -SharedDiagonal hard_model = noiseModel::Constrained::All(2); -SharedDiagonal soft_model = noiseModel::Isotropic::Sigma(2, 1.0); - -typedef NonlinearFactorGraph Graph; -typedef boost::shared_ptr shared_graph; -typedef boost::shared_ptr shared_values; -typedef NonlinearOptimizer Optimizer; - -/* ************************************************************************* */ -TEST( testNonlinearEqualityConstraint, unary_basics ) { - Point2 pt(1.0, 2.0); - simulated2D::PoseKey key(1); - double mu = 1000.0; - eq2D::UnaryEqualityConstraint constraint(pt, key, mu); - - simulated2D::Values config1; - config1.insert(key, pt); - EXPECT(constraint.active(config1)); - EXPECT(assert_equal(zero(2), constraint.evaluateError(pt), tol)); - EXPECT(assert_equal(zero(2), constraint.unwhitenedError(config1), tol)); - EXPECT_DOUBLES_EQUAL(0.0, constraint.error(config1), tol); - - simulated2D::Values config2; - Point2 ptBad1(2.0, 2.0); - config2.insert(key, ptBad1); - EXPECT(constraint.active(config2)); - EXPECT(assert_equal(Vector_(2, 1.0, 0.0), constraint.evaluateError(ptBad1), tol)); - EXPECT(assert_equal(Vector_(2, 1.0, 0.0), constraint.unwhitenedError(config2), tol)); - EXPECT_DOUBLES_EQUAL(500.0, constraint.error(config2), tol); -} - -/* ************************************************************************* */ -TEST( testNonlinearEqualityConstraint, unary_linearization ) { - Point2 pt(1.0, 2.0); - simulated2D::PoseKey key(1); - double mu = 1000.0; - Ordering ordering; - ordering += key; - eq2D::UnaryEqualityConstraint constraint(pt, key, mu); - - simulated2D::Values config1; - config1.insert(key, pt); - GaussianFactor::shared_ptr actual1 = constraint.linearize(config1, ordering); - GaussianFactor::shared_ptr expected1(new JacobianFactor(ordering[key], eye(2,2), zero(2), hard_model)); - EXPECT(assert_equal(*expected1, *actual1, tol)); - - simulated2D::Values config2; - Point2 ptBad(2.0, 2.0); - config2.insert(key, ptBad); - GaussianFactor::shared_ptr actual2 = constraint.linearize(config2, ordering); - GaussianFactor::shared_ptr expected2(new JacobianFactor(ordering[key], eye(2,2), Vector_(2,-1.0,0.0), hard_model)); - EXPECT(assert_equal(*expected2, *actual2, tol)); -} - -/* ************************************************************************* */ -TEST( testNonlinearEqualityConstraint, unary_simple_optimization ) { - // create a single-node graph with a soft and hard constraint to - // ensure that the hard constraint overrides the soft constraint - Point2 truth_pt(1.0, 2.0); - simulated2D::PoseKey key(1); - double mu = 10.0; - eq2D::UnaryEqualityConstraint::shared_ptr constraint( - new eq2D::UnaryEqualityConstraint(truth_pt, key, mu)); - - Point2 badPt(100.0, -200.0); - simulated2D::Prior::shared_ptr factor( - new simulated2D::Prior(badPt, soft_model, key)); - - shared_graph graph(new Graph()); - graph->push_back(constraint); - graph->push_back(factor); - - shared_values initValues(new simulated2D::Values()); - initValues->insert(key, badPt); - - // verify error values - EXPECT(constraint->active(*initValues)); - - simulated2D::Values expected; - expected.insert(key, truth_pt); - EXPECT(constraint->active(expected)); - EXPECT_DOUBLES_EQUAL(0.0, constraint->error(expected), tol); - - Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initValues); - EXPECT(assert_equal(expected, *actual, tol)); -} - -/* ************************************************************************* */ -TEST( testNonlinearEqualityConstraint, odo_basics ) { - Point2 x1(1.0, 2.0), x2(2.0, 3.0), odom(1.0, 1.0); - simulated2D::PoseKey key1(1), key2(2); - double mu = 1000.0; - eq2D::OdoEqualityConstraint constraint(odom, key1, key2, mu); - - simulated2D::Values config1; - config1.insert(key1, x1); - config1.insert(key2, x2); - EXPECT(constraint.active(config1)); - EXPECT(assert_equal(zero(2), constraint.evaluateError(x1, x2), tol)); - EXPECT(assert_equal(zero(2), constraint.unwhitenedError(config1), tol)); - EXPECT_DOUBLES_EQUAL(0.0, constraint.error(config1), tol); - - simulated2D::Values config2; - Point2 x1bad(2.0, 2.0); - Point2 x2bad(2.0, 2.0); - config2.insert(key1, x1bad); - config2.insert(key2, x2bad); - EXPECT(constraint.active(config2)); - EXPECT(assert_equal(Vector_(2, -1.0, -1.0), constraint.evaluateError(x1bad, x2bad), tol)); - EXPECT(assert_equal(Vector_(2, -1.0, -1.0), constraint.unwhitenedError(config2), tol)); - EXPECT_DOUBLES_EQUAL(1000.0, constraint.error(config2), tol); -} - -/* ************************************************************************* */ -TEST( testNonlinearEqualityConstraint, odo_linearization ) { - Point2 x1(1.0, 2.0), x2(2.0, 3.0), odom(1.0, 1.0); - simulated2D::PoseKey key1(1), key2(2); - double mu = 1000.0; - Ordering ordering; - ordering += key1, key2; - eq2D::OdoEqualityConstraint constraint(odom, key1, key2, mu); - - simulated2D::Values config1; - config1.insert(key1, x1); - config1.insert(key2, x2); - GaussianFactor::shared_ptr actual1 = constraint.linearize(config1, ordering); - GaussianFactor::shared_ptr expected1( - new JacobianFactor(ordering[key1], -eye(2,2), ordering[key2], - eye(2,2), zero(2), hard_model)); - EXPECT(assert_equal(*expected1, *actual1, tol)); - - simulated2D::Values config2; - Point2 x1bad(2.0, 2.0); - Point2 x2bad(2.0, 2.0); - config2.insert(key1, x1bad); - config2.insert(key2, x2bad); - GaussianFactor::shared_ptr actual2 = constraint.linearize(config2, ordering); - GaussianFactor::shared_ptr expected2( - new JacobianFactor(ordering[key1], -eye(2,2), ordering[key2], - eye(2,2), Vector_(2, 1.0, 1.0), hard_model)); - EXPECT(assert_equal(*expected2, *actual2, tol)); -} - -/* ************************************************************************* */ -TEST( testNonlinearEqualityConstraint, odo_simple_optimize ) { - // create a two-node graph, connected by an odometry constraint, with - // a hard prior on one variable, and a conflicting soft prior - // on the other variable - the constraints should override the soft constraint - Point2 truth_pt1(1.0, 2.0), truth_pt2(3.0, 2.0); - simulated2D::PoseKey key1(1), key2(2); - - // hard prior on x1 - eq2D::UnaryEqualityConstraint::shared_ptr constraint1( - new eq2D::UnaryEqualityConstraint(truth_pt1, key1)); - - // soft prior on x2 - Point2 badPt(100.0, -200.0); - simulated2D::Prior::shared_ptr factor( - new simulated2D::Prior(badPt, soft_model, key2)); - - // odometry constraint - eq2D::OdoEqualityConstraint::shared_ptr constraint2( - new eq2D::OdoEqualityConstraint( - truth_pt1.between(truth_pt2), key1, key2)); - - shared_graph graph(new Graph()); - graph->push_back(constraint1); - graph->push_back(constraint2); - graph->push_back(factor); - - shared_values initValues(new simulated2D::Values()); - initValues->insert(key1, Point2()); - initValues->insert(key2, badPt); - - Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initValues); - simulated2D::Values expected; - expected.insert(key1, truth_pt1); - expected.insert(key2, truth_pt2); - CHECK(assert_equal(expected, *actual, tol)); -} - -/* ********************************************************************* */ -TEST (testNonlinearEqualityConstraint, two_pose ) { - /* - * Determining a ground truth linear system - * with two poses seeing one landmark, with each pose - * constrained to a particular value - */ - - shared_graph graph(new Graph()); - - simulated2D::PoseKey x1(1), x2(2); - simulated2D::PointKey l1(1), l2(2); - Point2 pt_x1(1.0, 1.0), - pt_x2(5.0, 6.0); - graph->add(eq2D::UnaryEqualityConstraint(pt_x1, x1)); - graph->add(eq2D::UnaryEqualityConstraint(pt_x2, x2)); - - Point2 z1(0.0, 5.0); - SharedNoiseModel sigma(noiseModel::Isotropic::Sigma(2, 0.1)); - graph->add(simulated2D::Measurement(z1, sigma, x1,l1)); - - Point2 z2(-4.0, 0.0); - graph->add(simulated2D::Measurement(z2, sigma, x2,l2)); - - graph->add(eq2D::PointEqualityConstraint(l1, l2)); - - shared_values initialEstimate(new simulated2D::Values()); - initialEstimate->insert(x1, pt_x1); - initialEstimate->insert(x2, Point2()); - initialEstimate->insert(l1, Point2(1.0, 6.0)); // ground truth - initialEstimate->insert(l2, Point2(-4.0, 0.0)); // starting with a separate reference frame - - Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initialEstimate); - - simulated2D::Values expected; - expected.insert(x1, pt_x1); - expected.insert(l1, Point2(1.0, 6.0)); - expected.insert(l2, Point2(1.0, 6.0)); - expected.insert(x2, Point2(5.0, 6.0)); - CHECK(assert_equal(expected, *actual, 1e-5)); -} - -/* ********************************************************************* */ -TEST (testNonlinearEqualityConstraint, map_warp ) { - // get a graph - shared_graph graph(new Graph()); - - // keys - simulated2D::PoseKey x1(1), x2(2); - simulated2D::PointKey l1(1), l2(2); - - // constant constraint on x1 - Point2 pose1(1.0, 1.0); - graph->add(eq2D::UnaryEqualityConstraint(pose1, x1)); - - SharedDiagonal sigma = noiseModel::Isotropic::Sigma(1,0.1); - - // measurement from x1 to l1 - Point2 z1(0.0, 5.0); - graph->add(simulated2D::Measurement(z1, sigma, x1, l1)); - - // measurement from x2 to l2 - Point2 z2(-4.0, 0.0); - graph->add(simulated2D::Measurement(z2, sigma, x2, l2)); - - // equality constraint between l1 and l2 - graph->add(eq2D::PointEqualityConstraint(l1, l2)); - - // create an initial estimate - shared_values initialEstimate(new simulated2D::Values()); - initialEstimate->insert(x1, Point2( 1.0, 1.0)); - initialEstimate->insert(l1, Point2( 1.0, 6.0)); - initialEstimate->insert(l2, Point2(-4.0, 0.0)); // starting with a separate reference frame - initialEstimate->insert(x2, Point2( 0.0, 0.0)); // other pose starts at origin - - // optimize - Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initialEstimate); - - simulated2D::Values expected; - expected.insert(x1, Point2(1.0, 1.0)); - expected.insert(l1, Point2(1.0, 6.0)); - expected.insert(l2, Point2(1.0, 6.0)); - expected.insert(x2, Point2(5.0, 6.0)); - CHECK(assert_equal(expected, *actual, tol)); -} - -// make a realistic calibration matrix -double fov = 60; // degrees -size_t w=640,h=480; -Cal3_S2 K(fov,w,h); -boost::shared_ptr shK(new Cal3_S2(K)); - -// typedefs for visual SLAM example -typedef visualSLAM::Values VValues; -typedef boost::shared_ptr shared_vconfig; -typedef visualSLAM::Graph VGraph; -typedef NonlinearOptimizer VOptimizer; - -// factors for visual slam -typedef NonlinearEquality2 Point3Equality; - -/* ********************************************************************* */ -TEST (testNonlinearEqualityConstraint, stereo_constrained ) { - - // create initial estimates - Rot3 faceDownY(Matrix_(3,3, - 1.0, 0.0, 0.0, - 0.0, 0.0, 1.0, - 0.0, -1.0, 0.0)); - Pose3 pose1(faceDownY, Point3()); // origin, left camera - SimpleCamera camera1(K, pose1); - Pose3 pose2(faceDownY, Point3(2.0, 0.0, 0.0)); // 2 units to the left - SimpleCamera camera2(K, pose2); - Point3 landmark(1.0, 5.0, 0.0); //centered between the cameras, 5 units away - - // keys - visualSLAM::PoseKey x1(1), x2(2); - visualSLAM::PointKey l1(1), l2(2); - - // create graph - VGraph::shared_graph graph(new VGraph()); - - // create equality constraints for poses - graph->addPoseConstraint(1, camera1.pose()); - graph->addPoseConstraint(2, camera2.pose()); - - // create factors - SharedDiagonal vmodel = noiseModel::Unit::Create(3); - graph->addMeasurement(camera1.project(landmark), vmodel, 1, 1, shK); - graph->addMeasurement(camera2.project(landmark), vmodel, 2, 2, shK); - - // add equality constraint - graph->add(Point3Equality(l1, l2)); - - // create initial data - Point3 landmark1(0.5, 5.0, 0.0); - Point3 landmark2(1.5, 5.0, 0.0); - - shared_vconfig initValues(new VValues()); - initValues->insert(x1, pose1); - initValues->insert(x2, pose2); - initValues->insert(l1, landmark1); - initValues->insert(l2, landmark2); - - // optimize - VOptimizer::shared_values actual = VOptimizer::optimizeLM(graph, initValues); - - // create config - VValues truthValues; - truthValues.insert(x1, camera1.pose()); - truthValues.insert(x2, camera2.pose()); - truthValues.insert(l1, landmark); - truthValues.insert(l2, landmark); - - // check if correct - CHECK(assert_equal(truthValues, *actual, 1e-5)); -} - - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ - -