diff --git a/nonlinear/NonlinearConstraint.h b/nonlinear/NonlinearConstraint.h index d1260e2b0..99b8f2ae7 100644 --- a/nonlinear/NonlinearConstraint.h +++ b/nonlinear/NonlinearConstraint.h @@ -223,4 +223,31 @@ public: boost::optional H2 = boost::none) const = 0; }; +/** + * Simple binary equality constraint - this constraint forces two factors to + * be the same. This constraint requires the underlying type to a Lie type + */ +template +class NonlinearEquality2 : public NonlinearConstraint2 { +protected: + typedef NonlinearConstraint2 Base; + +public: + + typedef boost::shared_ptr > shared_ptr; + + NonlinearEquality2(const Key& key1, const Key& key2, double mu = 1000.0) + : Base(key1, key2, X::dim(), mu) {} + + /** 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 logmap(x1, x2); + } +}; + } diff --git a/slam/simulated2DConstraints.h b/slam/simulated2DConstraints.h index 77fddc713..7c589dd7e 100644 --- a/slam/simulated2DConstraints.h +++ b/slam/simulated2DConstraints.h @@ -18,7 +18,7 @@ namespace gtsam { namespace simulated2D { /** - * Unary factor encoding a hard equality on a Point + * Unary constraint encoding a hard equality on a Point */ template struct GenericUnaryEqualityConstraint: public NonlinearConstraint1 { @@ -39,7 +39,7 @@ namespace gtsam { }; /** - * Binary factor simulating "odometry" between two Vectors + * Binary constraint simulating "odometry" between two Poses */ template struct GenericOdoHardEqualityConstraint: public NonlinearConstraint2 { @@ -63,5 +63,9 @@ namespace gtsam { typedef GenericUnaryEqualityConstraint UnaryEqualityConstraint; typedef GenericOdoHardEqualityConstraint OdoEqualityConstraint; + /** Equality between variables */ + typedef NonlinearEquality2 PoseEqualityConstraint; + typedef NonlinearEquality2 PointEqualityConstraint; + } // namespace simulated2D } // namespace gtsam diff --git a/slam/smallExample.h b/slam/smallExample.h index 2e7225178..310ed30c3 100644 --- a/slam/smallExample.h +++ b/slam/smallExample.h @@ -82,7 +82,7 @@ namespace gtsam { GaussianFactorGraph createSmoother(int T); /* ******************************************************* */ - // Constrained Examples + // Linear Constrained Examples /* ******************************************************* */ /** @@ -106,37 +106,6 @@ namespace gtsam { GaussianFactorGraph createMultiConstraintGraph(); VectorConfig createMultiConstraintConfig(); - /** - * These are the old examples from the EqualityFactor/DeltaFunction - * They should be updated for use at some point, but are disabled for now - */ - /** - * Create configuration for constrained example - * This is the ground truth version - */ - //VectorConfig createConstrainedConfig(); - - /** - * Create a noisy configuration for linearization - */ - //VectorConfig createConstrainedLinConfig(); - - /** - * Create the correct delta configuration - */ - //VectorConfig createConstrainedCorrectDelta(); - - /** - * Create small example constrained factor graph - */ - //GaussianFactorGraph createConstrainedGaussianFactorGraph(); - - /** - * Create small example constrained nonlinear factor graph - */ - // ConstrainedNonlinearFactorGraph,VectorConfig> - // createConstrainedNonlinearFactorGraph(); - /* ******************************************************* */ // Planar graph with easy subtree for SubgraphPreconditioner /* ******************************************************* */ diff --git a/slam/visualSLAM.h b/slam/visualSLAM.h index 4a3172dd3..a9da996f0 100644 --- a/slam/visualSLAM.h +++ b/slam/visualSLAM.h @@ -27,6 +27,7 @@ namespace gtsam { namespace visualSLAM { typedef LieConfig PoseConfig; typedef LieConfig PointConfig; typedef TupleConfig2 Config; + typedef boost::shared_ptr shared_config; typedef NonlinearEquality PoseConstraint; typedef NonlinearEquality PointConstraint; @@ -50,7 +51,7 @@ namespace gtsam { namespace visualSLAM { typedef NonlinearFactor2 Base; // shorthand for a smart pointer to a factor - typedef boost::shared_ptr shared_ptr; + typedef boost::shared_ptr > shared_ptr; /** * Default constructor @@ -125,6 +126,8 @@ namespace gtsam { namespace visualSLAM { public: + typedef boost::shared_ptr shared_graph; + /** default constructor is empty graph */ Graph() { } diff --git a/tests/testNonlinearConstraint.cpp b/tests/testNonlinearConstraint.cpp index 232b2f3a4..9b9a06ae5 100644 --- a/tests/testNonlinearConstraint.cpp +++ b/tests/testNonlinearConstraint.cpp @@ -13,27 +13,26 @@ #include -//#define GTSAM_MAGIC_KEY -// -//#include -//#include -//#include -//#include -//#include -//#include -//#include -// -//// templated implementations -//#include -//#include -//#include -//#include -// -//using namespace std; -//using namespace gtsam; -//using namespace boost; -//using namespace boost::assign; -// +#define GTSAM_MAGIC_KEY + +#include +#include +#include +#include +#include +#include +#include + +// templated implementations +#include +#include +#include + +using namespace std; +using namespace gtsam; +using namespace boost; +using namespace boost::assign; + //// Models to use //SharedDiagonal probModel1 = sharedSigma(1,1.0); //SharedDiagonal probModel2 = sharedSigma(2,1.0); @@ -49,665 +48,7 @@ //typedef NonlinearEquality NLE; //typedef boost::shared_ptr > shared; //typedef NonlinearOptimizer Optimizer; -// -///* -// * Determining a ground truth linear system -// * with two poses seeing one landmark, with each pose -// * constrained to a particular value -// */ -//TEST (NonlinearConstraint, two_pose_truth ) { -// // create a graph -// shared_ptr graph(new Graph2D); -// -// // add the constraints on the ends -// // position (1, 1) constraint for x1 -// // position (5, 6) constraint for x2 -// simulated2D::PoseKey x1(1), x2(2); -// simulated2D::PointKey l1(1); -// Point2 pt_x1(1.0, 1.0), -// pt_x2(5.0, 6.0); -// shared_ptr ef1(new NLE(x1, pt_x1)), -// ef2(new NLE(x2, pt_x2)); -// graph->push_back(ef1); -// graph->push_back(ef2); -// -// // measurement from x1 to l1 -// Point2 z1(0.0, 5.0); -// SharedGaussian sigma(noiseModel::Isotropic::Sigma(2, 0.1)); -// shared f1(new simulated2D::GenericMeasurement(z1, sigma, x1,l1)); -// graph->push_back(f1); -// -// // measurement from x2 to l1 -// Point2 z2(-4.0, 0.0); -// shared f2(new simulated2D::GenericMeasurement(z2, sigma, x2,l1)); -// graph->push_back(f2); -// -// // create an initial estimate -// Point2 pt_l1( -// 1.0, 6.0 // ground truth -// //1.2, 5.6 // small error -// ); -// shared_ptr initialEstimate(new Config2D); -// initialEstimate->insert(l1, pt_l1); -// initialEstimate->insert(x1, pt_x1); -// initialEstimate->insert(x2, pt_x2); -// -// // optimize the graph -// boost::shared_ptr actual = Optimizer::optimizeGN(graph, initialEstimate); -// -// // verify -// Config2D expected; -// expected.insert(x1, pt_x1); -// expected.insert(x2, pt_x2); -// expected.insert(l1, Point2(1.0, 6.0)); -// CHECK(assert_equal(expected, *actual, 1e-5)); -//} -// -///* ********************************************************************* */ -//namespace constrained_test1 { -// -// // binary constraint between landmarks -// /** g(x) = x-y = 0 */ -// Vector g(const Config2D& config, const list& keys) { -// Point2 pt1, pt2; -// pt1 = config[simulated2D::PointKey(keys.front().index())]; -// pt2 = config[simulated2D::PointKey(keys.back().index())]; -// return Vector_(2, pt1.x() - pt2.x(), pt1.y() - pt2.y()); -// } -// -// /** jacobian at l1 */ -// Matrix G1(const Config2D& config, const list& keys) { -// return eye(2); -// } -// -// /** jacobian at l2 */ -// Matrix G2(const Config2D& config, const list& keys) { -// return -1 * eye(2); -// } -// -//} // \namespace constrained_test1 -// -//namespace constrained_test2 { -// -// // Unary Constraint on x1 -// /** g(x) = x -[1;1] = 0 */ -// Vector g(const Config2D& config, const list& keys) { -// Point2 x = config[keys.front()]; -// return Vector_(2, x.x() - 1.0, x.y() - 1.0); -// } -// -// /** jacobian at x1 */ -// Matrix G(const Config2D& config, const list& keys) { -// return eye(2); -// } -// -//} // \namespace constrained_test2 -// -// -//typedef NonlinearConstraint2< -// Config2D, simulated2D::PointKey, Point2, simulated2D::PointKey, Point2> NLC2; -// -///* ********************************************************************* -// * Version that actually uses nonlinear equality constraints -// * to to perform optimization. Same as above, but no -// * equality constraint on x2, and two landmarks that -// * should be the same. Note that this is a linear system, -// * so it will converge in one step. -// */ -//TEST (NonlinearConstraint, two_pose ) { -// bool verbose = false; -// -// // create the graph -// shared_ptr graph(new Graph2D); -// -// // add the constraints on the ends -// // position (1, 1) constraint for x1 -// // position (5, 6) constraint for x2 -// 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); -// shared_ptr ef1(new NLE(x1, pt_x1)); -// graph->push_back(ef1); -// -// // measurement from x1 to l1 -// Point2 z1(0.0, 5.0); -// SharedGaussian sigma(noiseModel::Isotropic::Sigma(2, 0.1)); -// shared f1(new simulated2D::GenericMeasurement(z1, sigma, x1,l1)); -// graph->push_back(f1); -// -// // measurement from x2 to l2 -// Point2 z2(-4.0, 0.0); -// shared f2(new simulated2D::GenericMeasurement(z2, sigma, x2,l2)); -// graph->push_back(f2); -// -// // equality constraint between l1 and l2 -// list keys2; keys2 += l1, l2; -// boost::shared_ptr c2(new NLC2( -// boost::bind(constrained_test1::g, _1, keys2), -// l1, boost::bind(constrained_test1::G1, _1, keys2), -// l2, boost::bind(constrained_test1::G2, _1, keys2), -// 2)); -// graph->push_back(c2); -// -// if (verbose) graph->print("Initial nonlinear graph with constraints"); -// -// // create an initial estimate -// shared_ptr initialEstimate(new Config2D); -// 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 -// -// // create state config variables and initialize them -// Config2D state(*initialEstimate); -// -// // linearize the graph -// boost::shared_ptr fg = graph->linearize(state); -// -// if (verbose) fg->print("Linearized graph"); -// -// // create an ordering -// Ordering ordering; -// ordering += "x1", "x2", "l1", "l2"; -// -// // optimize linear graph to get full delta config -// GaussianBayesNet cbn = fg->eliminate(ordering); -// if (verbose) cbn.print("ChordalBayesNet"); -// -// VectorConfig delta = optimize(cbn); //fg.optimize(ordering); -// if (verbose) delta.print("Delta Config"); -// -// // update both state variables -// state = expmap(state, delta); -// if (verbose) state.print("newState"); -// -// // verify -// Config2D 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, state, 1e-5)); -//} -// -///* ********************************************************************* */ -//// VSLAM Examples -///* ********************************************************************* */ -//// 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)); -// -//using namespace gtsam::visualSLAM; -//using namespace boost; -// -//// typedefs for visual SLAM example -//typedef TypedSymbol Pose3Key; -//typedef TypedSymbol Point3Key; -////typedef TupleConfig3, -//// LieConfig, -//// LieConfig > VConfig; -//typedef visualSLAM::Config VConfig; -//typedef NonlinearFactorGraph VGraph; -//typedef boost::shared_ptr > shared_vf; -//typedef NonlinearOptimizer VOptimizer; -//typedef NonlinearConstraint2< -// VConfig, visualSLAM::PointKey, Pose3, visualSLAM::PointKey, Pose3> VNLC2; -//typedef NonlinearEquality Pose3Constraint; -// -///** -// * Ground truth for a visual SLAM example with stereo vision -// */ -//TEST (NonlinearConstraint, stereo_truth ) { -// bool verbose = false; -// -// // 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 -// Point3 landmarkNoisy(1.0, 6.0, 0.0); -// -// // create truth config -// boost::shared_ptr truthConfig(new VConfig); -// truthConfig->insert(Pose3Key(1), camera1.pose()); -// truthConfig->insert(Pose3Key(2), camera2.pose()); -// truthConfig->insert(Point3Key(1), landmark); -// -// // create graph -// shared_ptr graph(new VGraph()); -// -// // create equality constraints for poses -// graph->push_back(shared_ptr(new Pose3Constraint(Pose3Key(1), camera1.pose()))); -// graph->push_back(shared_ptr(new Pose3Constraint(Pose3Key(2), camera2.pose()))); -// -// // create VSLAM factors -// Point2 z1 = camera1.project(landmark); -// if (verbose) z1.print("z1"); -// SharedDiagonal vmodel = noiseModel::Unit::Create(3); -// //ProjectionFactor test_vf(z1, vmodel, Pose3Key(1), Point3Key(1), shK); -// shared_vf vf1(new GenericProjectionFactor( -// z1, vmodel, Pose3Key(1), Point3Key(1), shK)); -// graph->push_back(vf1); -// Point2 z2 = camera2.project(landmark); -// if (verbose) z2.print("z2"); -// shared_vf vf2(new GenericProjectionFactor( -// z2, vmodel, Pose3Key(2), Point3Key(1), shK)); -// graph->push_back(vf2); -// -// if (verbose) graph->print("Graph after construction"); -// -// // create ordering -// shared_ptr ord(new Ordering()); -// *ord += "x1", "x2", "l1"; -// -// // create optimizer -// VOptimizer::shared_solver solver(new VOptimizer::solver(ord)); -// VOptimizer optimizer(graph, truthConfig, solver); -// -// // optimize -// VOptimizer afterOneIteration = optimizer.iterate(); -// -// // verify -// DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); -// -// // check if correct -// if (verbose) afterOneIteration.config()->print("After iteration"); -// CHECK(assert_equal(*truthConfig,*(afterOneIteration.config()))); -//} -// -// -///* ********************************************************************* -// * Ground truth for a visual SLAM example with stereo vision -// * with some noise injected into the initial config -// */ -//TEST (NonlinearConstraint, stereo_truth_noisy ) { -// bool verbose = false; -// -// // setting to determine how far away the noisy landmark is, -// // given that the ground truth is 5m in front of the cameras -// double noisyDist = 7.6; -// -// // 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 -// Point3 landmarkNoisy(1.0, noisyDist, 0.0); // initial point is too far out -// -// // create truth config -// boost::shared_ptr truthConfig(new VConfig); -// truthConfig->insert(Pose3Key(1), camera1.pose()); -// truthConfig->insert(Pose3Key(2), camera2.pose()); -// truthConfig->insert(Point3Key(1), landmark); -// -// // create config -// boost::shared_ptr noisyConfig(new VConfig); -// noisyConfig->insert(Pose3Key(1), camera1.pose()); -// noisyConfig->insert(Pose3Key(2), camera2.pose()); -// noisyConfig->insert(Point3Key(1), landmarkNoisy); -// -// // create graph -// shared_ptr graph(new VGraph()); -// -// // create equality constraints for poses -// graph->push_back(shared_ptr(new Pose3Constraint(Pose3Key(1), camera1.pose()))); -// graph->push_back(shared_ptr(new Pose3Constraint(Pose3Key(2), camera2.pose()))); -// -// // create VSLAM factors -// Point2 z1 = camera1.project(landmark); -// if (verbose) z1.print("z1"); -// SharedDiagonal vmodel = noiseModel::Unit::Create(3); -// shared_vf vf1(new GenericProjectionFactor( -// z1, vmodel, Pose3Key(1), Point3Key(1), shK)); -// graph->push_back(vf1); -// Point2 z2 = camera2.project(landmark); -// if (verbose) z2.print("z2"); -// shared_vf vf2(new GenericProjectionFactor( -// z2, vmodel, Pose3Key(2), Point3Key(1), shK)); -// graph->push_back(vf2); -// -// if (verbose) { -// graph->print("Graph after construction"); -// noisyConfig->print("Initial config"); -// } -// -// // create ordering -// shared_ptr ord(new Ordering()); -// *ord += "x1", "x2", "l1"; -// -// // create optimizer -// VOptimizer::shared_solver solver(new VOptimizer::solver(ord)); -// VOptimizer optimizer0(graph, noisyConfig, solver); -// -// if (verbose) -// cout << "Initial Error: " << optimizer0.error() << endl; -// -// // use Levenberg-Marquardt optimization -// double relThresh = 1e-5, absThresh = 1e-5; -// VOptimizer optimizer(optimizer0.levenbergMarquardt(relThresh, absThresh, VOptimizer::SILENT)); -// -// // verify -// DOUBLES_EQUAL(0.0, optimizer.error(), 1e-5); -// -// // check if correct -// if (verbose) { -// optimizer.config()->print("After iteration"); -// cout << "Final error: " << optimizer.error() << endl; -// } -// CHECK(assert_equal(*truthConfig,*(optimizer.config()), 1e-5)); -//} -// -///* ********************************************************************* */ -//namespace constrained_stereo { -// -// // binary constraint between landmarks -// /** g(x) = x-y = 0 */ -// Vector g(const VConfig& config, const list& keys) { -// return config[keys.front()].vector() -// - config[keys.back()].vector(); -// } -// -// /** jacobian at l1 */ -// Matrix G1(const VConfig& config, const list& keys) { -// return eye(3); -// } -// -// /** jacobian at l2 */ -// Matrix G2(const VConfig& config, const list& keys) { -// return -1.0 * eye(3); -// } -// -//} // \namespace constrained_stereo -// -///* ********************************************************************* */ -//boost::shared_ptr stereoExampleGraph() { -// // 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 landmark1(1.0, 5.0, 0.0); //centered between the cameras, 5 units away -// Point3 landmark2(1.0, 5.0, 0.0); -// -// // create graph -// shared_ptr graph(new VGraph); -// -// // create equality constraints for poses -// graph->push_back(shared_ptr(new Pose3Constraint(Pose3Key(1), camera1.pose()))); -// graph->push_back(shared_ptr(new Pose3Constraint(Pose3Key(2), camera2.pose()))); -// -// // create factors -// Point2 z1 = camera1.project(landmark1); -// SharedDiagonal vmodel = noiseModel::Unit::Create(3); -// shared_vf vf1(new GenericProjectionFactor( -// z1, vmodel, Pose3Key(1), Point3Key(1), shK)); -// graph->push_back(vf1); -// Point2 z2 = camera2.project(landmark2); -// shared_vf vf2(new GenericProjectionFactor( -// z2, vmodel, Pose3Key(2), Point3Key(2), shK)); -// graph->push_back(vf2); -// -// // create the binary equality constraint between the landmarks -// // NOTE: this is really just a linear constraint that is exactly the same -// // as the previous examples -// visualSLAM::PointKey l1(1), l2(2); -// list keys; keys += l1, l2; -// shared_ptr c2( -// new VNLC2(boost::bind(constrained_stereo::g, _1, keys), -// l1, boost::bind(constrained_stereo::G1, _1, keys), -// l2, boost::bind(constrained_stereo::G2, _1, keys), -// 3)); -// graph->push_back(c2); -// -// return graph; -//} -// -///* ********************************************************************* */ -//boost::shared_ptr stereoExampleTruthConfig() { -// // 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 landmark1(1.0, 5.0, 0.0); //centered between the cameras, 5 units away -// Point3 landmark2(1.0, 5.0, 0.0); -// -// // create config -// boost::shared_ptr truthConfig(new VConfig); -// truthConfig->insert(Pose3Key(1), camera1.pose()); -// truthConfig->insert(Pose3Key(2), camera2.pose()); -// truthConfig->insert(Point3Key(1), landmark1); -// truthConfig->insert(Point3Key(2), landmark2); // create two landmarks in same place -// //truthConfig->insert(LagrangeKey(12), Vector_(3, 1.0, 1.0, 1.0)); -// -// return truthConfig; -//} -// -///* ********************************************************************* -// * SQP version of the above stereo example, -// * with the initial case as the ground truth -// */ -//TEST (NonlinearConstraint, stereo_constrained ) { -// bool verbose = false; -// -// // get a graph -// boost::shared_ptr graph = stereoExampleGraph(); -// if (verbose) graph->print("Graph after construction"); -// -// // get the truth config -// boost::shared_ptr truthConfig = stereoExampleTruthConfig(); -// -// // create ordering -// shared_ptr ord(new Ordering()); -// *ord += "x1", "x2", "l1", "l2"; -// VOptimizer::shared_solver solver(new VOptimizer::solver(ord)); -// -// // create optimizer -// VOptimizer optimizer(graph, truthConfig, solver); -// -// // optimize -// VOptimizer afterOneIteration = optimizer.iterate(); -// -// // check if correct -// CHECK(assert_equal(*truthConfig,*(afterOneIteration.config()))); -//} -// -///* ********************************************************************* -// * SQP version of the above stereo example, -// * with noise in the initial estimate -// */ -//TEST (NonlinearConstraint, stereo_constrained_noisy ) { -// -// // get a graph -// boost::shared_ptr graph = stereoExampleGraph(); -// -// // create initial data -// 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 -// Pose3 pose2(faceDownY, Point3(2.0, 0.0, 0.0)); // 2 units to the left -// Point3 landmark1(0.5, 5.0, 0.0); //centered between the cameras, 5 units away -// Point3 landmark2(1.5, 5.0, 0.0); -// -// // noisy config -// boost::shared_ptr initConfig(new VConfig); -// initConfig->insert(Pose3Key(1), pose1); -// initConfig->insert(Pose3Key(2), pose2); -// initConfig->insert(Point3Key(1), landmark1); -// initConfig->insert(Point3Key(2), landmark2); // create two landmarks in same place -// -// // create ordering -// shared_ptr ord(new Ordering()); -// *ord += "x1", "x2", "l1", "l2"; -// VOptimizer::shared_solver solver(new VOptimizer::solver(ord)); -// -// // create optimizer -// VOptimizer optimizer(graph, initConfig, solver); -// -// // optimize -// VOptimizer *pointer = new VOptimizer(optimizer); -// for (int i=0;i<1;i++) { -// VOptimizer* newOptimizer = new VOptimizer(pointer->iterateLM()); -// delete pointer; -// pointer = newOptimizer; -// } -// VOptimizer::shared_config actual = pointer->config(); -// delete(pointer); -// -// // get the truth config -// boost::shared_ptr truthConfig = stereoExampleTruthConfig(); -// -// // check if correct -// CHECK(assert_equal(*truthConfig,*actual, 1e-5)); -//} -// -//static SharedGaussian sigma(noiseModel::Isotropic::Sigma(1,0.1)); -// -//namespace map_warp_example { -//typedef NonlinearConstraint1< -// Config2D, simulated2D::PoseKey, Point2> NLC1; -//} // \namespace map_warp_example -// -///* ********************************************************************* */ -//// Example that moves two separate maps into the same frame of reference -//// Note that this is a linear example, so it should converge in one step -///* ********************************************************************* */ -// -//namespace constrained_LinearMapWarp2 { -//// binary constraint between landmarks -///** g(x) = x-y = 0 */ -//Vector g_func(const Config2D& config, const simulated2D::PointKey& key1, const simulated2D::PointKey& key2) { -// Point2 p = config[key1]-config[key2]; -// return Vector_(2, p.x(), p.y()); -//} -// -///** jacobian at l1 */ -//Matrix jac_g1(const Config2D& config) { -// return eye(2); -//} -// -///** jacobian at l2 */ -//Matrix jac_g2(const Config2D& config) { -// return -1*eye(2); -//} -//} // \namespace constrained_LinearMapWarp2 -// -//namespace constrained_LinearMapWarp1 { -//// Unary Constraint on x1 -///** g(x) = x -[1;1] = 0 */ -//Vector g_func(const Config2D& config, const simulated2D::PoseKey& key) { -// Point2 p = config[key]-Point2(1.0, 1.0); -// return Vector_(2, p.x(), p.y()); -//} -// -///** jacobian at x1 */ -//Matrix jac_g(const Config2D& config) { -// return eye(2); -//} -//} // \namespace constrained_LinearMapWarp12 -// -////typedef NonlinearOptimizer Optimizer; -// -///** -// * Creates the graph with each robot seeing the landmark, and it is -// * known that it is the same landmark -// */ -//boost::shared_ptr linearMapWarpGraph() { -// using namespace map_warp_example; -// // keys -// simulated2D::PoseKey x1(1), x2(2); -// simulated2D::PointKey l1(1), l2(2); -// -// // constant constraint on x1 -// shared_ptr c1(new NLC1(boost::bind(constrained_LinearMapWarp1::g_func, _1, x1), -// x1, boost::bind(constrained_LinearMapWarp1::jac_g, _1), -// 2)); -// -// // measurement from x1 to l1 -// Point2 z1(0.0, 5.0); -// shared f1(new simulated2D::GenericMeasurement(z1, sigma, x1,l1)); -// -// // measurement from x2 to l2 -// Point2 z2(-4.0, 0.0); -// shared f2(new simulated2D::GenericMeasurement(z2, sigma, x2,l2)); -// -// // equality constraint between l1 and l2 -// shared_ptr c2 (new NLC2( -// boost::bind(constrained_LinearMapWarp2::g_func, _1, l1, l2), -// l1, boost::bind(constrained_LinearMapWarp2::jac_g1, _1), -// l2, boost::bind(constrained_LinearMapWarp2::jac_g2, _1), -// 2)); -// -// // construct the graph -// boost::shared_ptr graph(new Graph2D()); -// graph->push_back(c1); -// graph->push_back(c2); -// graph->push_back(f1); -// graph->push_back(f2); -// -// return graph; -//} -// -///* ********************************************************************* */ -//TEST ( SQPOptimizer, map_warp_initLam ) { -// // get a graph -// boost::shared_ptr graph = linearMapWarpGraph(); -// -// // keys -// simulated2D::PoseKey x1(1), x2(2); -// simulated2D::PointKey l1(1), l2(2); -// -// // create an initial estimate -// shared_ptr initialEstimate(new Config2D); -// 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 -// -// // create an ordering -// shared_ptr ordering(new Ordering()); -// *ordering += "x1", "x2", "l1", "l2"; -// -// // create an optimizer -// Optimizer::shared_solver solver(new Optimizer::solver(ordering)); -// Optimizer optimizer(graph, initialEstimate, solver); -// -// // perform an iteration of optimization -// Optimizer oneIteration = optimizer.iterate(Optimizer::SILENT); -// -// // get the config back out and verify -// Config2D actual = *(oneIteration.config()); -// Config2D 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)); -//} -// + ///* ********************************************************************* */ //// This is an obstacle avoidance demo, where there is a trajectory of //// three points, where there is a circular obstacle in the middle. There diff --git a/tests/testNonlinearEqualityConstraint.cpp b/tests/testNonlinearEqualityConstraint.cpp index 8937315d7..9348b2631 100644 --- a/tests/testNonlinearEqualityConstraint.cpp +++ b/tests/testNonlinearEqualityConstraint.cpp @@ -6,13 +6,14 @@ #include #include +#include #include #include using namespace std; using namespace gtsam; -static const double tol = 1e-9; +static const double tol = 1e-5; SharedDiagonal hard_model = noiseModel::Constrained::All(2); SharedDiagonal soft_model = noiseModel::Isotropic::Sigma(2, 1.0); @@ -181,9 +182,167 @@ TEST( testNonlinearEqualityConstraint, odo_simple_optimize ) { 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(simulated2D::UnaryEqualityConstraint(pt_x1, x1)); + graph->add(simulated2D::UnaryEqualityConstraint(pt_x2, x2)); + + Point2 z1(0.0, 5.0); + SharedGaussian 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(simulated2D::PointEqualityConstraint(l1, l2)); + + shared_config initialEstimate(new simulated2D::Config()); + 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_config actual = Optimizer::optimizeLM(graph, initialEstimate); + + simulated2D::Config 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(simulated2D::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(simulated2D::PointEqualityConstraint(l1, l2)); + + // create an initial estimate + shared_config initialEstimate(new simulated2D::Config()); + 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_config actual = Optimizer::optimizeLM(graph, initialEstimate); + + simulated2D::Config 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::Config VConfig; +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 initConfig(new VConfig()); + initConfig->insert(x1, pose1); + initConfig->insert(x2, pose2); + initConfig->insert(l1, landmark1); + initConfig->insert(l2, landmark2); + + // optimize + VOptimizer::shared_config actual = VOptimizer::optimizeLM(graph, initConfig); + + // create config + VConfig truthConfig; + truthConfig.insert(x1, camera1.pose()); + truthConfig.insert(x2, camera2.pose()); + truthConfig.insert(l1, landmark); + truthConfig.insert(l2, landmark); + + // check if correct + CHECK(assert_equal(truthConfig, *actual, 1e-5)); +} + + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */