From 219dfd262daadc92b53f71874536a2c1f5fac7a9 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Sat, 6 Feb 2010 05:14:52 +0000 Subject: [PATCH] SQP now works with single configs using the TupleConfigs, without needing a separate optimizer. --- cpp/LieConfig.h | 2 +- cpp/NonlinearConstraint-inl.h | 45 +- cpp/testNonlinearConstraint.cpp | 17 +- cpp/testSQP.cpp | 1625 +++++++++++++++---------------- 4 files changed, 816 insertions(+), 873 deletions(-) diff --git a/cpp/LieConfig.h b/cpp/LieConfig.h index 6900201c8..4372d8a10 100644 --- a/cpp/LieConfig.h +++ b/cpp/LieConfig.h @@ -49,7 +49,7 @@ namespace gtsam { virtual ~LieConfig() {} /** print */ - void print(const std::string &s) const; + void print(const std::string &s="") const; /** Test whether configs are identical in keys and values */ bool equals(const LieConfig& expected, double tol=1e-9) const; diff --git a/cpp/NonlinearConstraint-inl.h b/cpp/NonlinearConstraint-inl.h index a6d64afd4..9e36cc46b 100644 --- a/cpp/NonlinearConstraint-inl.h +++ b/cpp/NonlinearConstraint-inl.h @@ -197,6 +197,8 @@ bool NonlinearConstraint2::equals(const Factor GaussianFactor::shared_ptr NonlinearConstraint2::linearize(const Config& config) const { + const size_t p = this->p_; + // extract lagrange multiplier Vector lambda = config[this->lagrange_key_]; @@ -207,17 +209,40 @@ NonlinearConstraint2::linearize(const Config& config Matrix grad1 = G1_(config); Matrix grad2 = G2_(config); - // construct probabilistic factor - Matrix A1 = vector_scale(lambda, grad1); - Matrix A2 = vector_scale(lambda, grad2); - SharedDiagonal probModel = sharedSigma(this->p_,1.0); - GaussianFactor::shared_ptr factor(new GaussianFactor(key1_, A1, key2_, A2, - this->lagrange_key_, eye(this->p_), zero(this->p_), probModel)); + // create matrices + Matrix Ax1 = zeros(grad1.size1()*2, grad1.size2()), + Ax2 = zeros(grad2.size1()*2, grad2.size2()), + AL = eye(p*2, p); - // construct the constraint - SharedDiagonal constraintModel = noiseModel::Constrained::All(this->p_); - GaussianFactor::shared_ptr constraint(new GaussianFactor(key1_, grad1, - key2_, grad2, -1.0 * g, constraintModel)); + // insert matrix components + insertSub(Ax1, vector_scale(lambda, grad1), 0, 0); + insertSub(Ax1, grad1, grad1.size1(), 0); + + insertSub(Ax2, vector_scale(lambda, grad2), 0, 0); + insertSub(Ax2, grad2, grad2.size1(), 0); + + Vector rhs = zero(p*2); + subInsert(rhs, -1*g, p); + + // construct a mixed constraint model + Vector sigmas = zero(p*2); + subInsert(sigmas, ones(p), 0); + SharedDiagonal mixedConstraint = noiseModel::Constrained::MixedSigmas(sigmas); + + GaussianFactor::shared_ptr factor(new + GaussianFactor(key1_, Ax1, key2_, Ax2, this->lagrange_key_, AL, rhs, mixedConstraint)); + +// // construct probabilistic factor +// Matrix A1 = vector_scale(lambda, grad1); +// Matrix A2 = vector_scale(lambda, grad2); +// SharedDiagonal probModel = sharedSigma(this->p_,1.0); +// GaussianFactor::shared_ptr factor(new GaussianFactor(key1_, A1, key2_, A2, +// this->lagrange_key_, eye(this->p_), zero(this->p_), probModel)); +// +// // construct the constraint +// SharedDiagonal constraintModel = noiseModel::Constrained::All(this->p_); +// GaussianFactor::shared_ptr constraint(new GaussianFactor(key1_, grad1, +// key2_, grad2, -1.0 * g, constraintModel)); return factor; } diff --git a/cpp/testNonlinearConstraint.cpp b/cpp/testNonlinearConstraint.cpp index 33f6afeaf..9dc1dae4b 100644 --- a/cpp/testNonlinearConstraint.cpp +++ b/cpp/testNonlinearConstraint.cpp @@ -186,24 +186,15 @@ TEST( NonlinearConstraint2, binary_scalar_linearize ) { // linearize the system GaussianFactor::shared_ptr actualFactor = c1.linearize(realconfig); - // verify + // verify - probabilistic component goes on top Matrix Ax0 = Matrix_(2,1, 6.0, 2.0), Ax1 = Matrix_(2,1,-3.0,-1.0), AL = Matrix_(2,1, 1.0, 0.0); - Vector rhs = Vector_(2, 0, 6.0), + Vector rhs = Vector_(2, 0.0, 6.0), sigmas = Vector_(2, 1.0, 0.0); SharedDiagonal expModel = noiseModel::Constrained::MixedSigmas(sigmas); - -// SharedDiagonal probModel = sharedSigma(p,1.0); -// GaussianFactor expectedFactor(x0, Matrix_(1,1, 6.0), -// x1, Matrix_(1,1, -3.0), -// L1, eye(1), zero(1), probModel); -// SharedDiagonal constraintModel = noiseModel::Constrained::All(p); -// GaussianFactor expectedConstraint(x0, Matrix_(1,1, 2.0), -// x1, Matrix_(1,1, -1.0), -// Vector_(1, 6.0), constraintModel); -// CHECK(assert_equal(*actualFactor, expectedFactor)); -// CHECK(assert_equal(*actualConstraint, expectedConstraint)); + GaussianFactor expFactor(x0,Ax0, x1, Ax1,L1, AL, rhs, expModel); + CHECK(assert_equal(expFactor, *actualFactor)); } /* ************************************************************************* */ diff --git a/cpp/testSQP.cpp b/cpp/testSQP.cpp index 1759b895a..3e1f5e546 100644 --- a/cpp/testSQP.cpp +++ b/cpp/testSQP.cpp @@ -12,14 +12,12 @@ #include #include -// TODO: DANGEROUS, create shared pointers -#define GTSAM_DANGEROUS_GAUSSIAN 2 #define GTSAM_MAGIC_KEY +#include #include #include #include -#include #include #include #include @@ -28,7 +26,7 @@ #include #include #include -#include +#include using namespace std; using namespace gtsam; @@ -43,851 +41,780 @@ SharedDiagonal constraintModel1 = noiseModel::Constrained::All(1); // trick from some reading group #define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL) -///* ********************************************************************* -// * This example uses a nonlinear objective function and -// * nonlinear equality constraint. The formulation is actually -// * the Cholesky form that creates the full Hessian explicitly, -// * which should really be avoided with our QR-based machinery. -// * -// * Note: the update equation used here has a fixed step size -// * and gain that is rather arbitrarily chosen, and as such, -// * will take a silly number of iterations. -// */ -//TEST (SQP, problem1_cholesky ) { -// bool verbose = false; -// // use a nonlinear function of f(x) = x^2+y^2 -// // nonlinear equality constraint: g(x) = x^2-5-y=0 -// // Lagrangian: f(x) + \lambda*g(x) -// -// // state structure: [x y \lambda] -// VectorConfig init, state; -// init.insert("x", Vector_(1, 1.0)); -// init.insert("y", Vector_(1, 1.0)); -// init.insert("L", Vector_(1, 1.0)); -// state = init; -// -// if (verbose) init.print("Initial State"); -// -// // loop until convergence -// int maxIt = 10; -// for (int i = 0; i ||Ax-b||^2 -// * where: -// * h(x) simply returns the inputs -// * z zeros(2) -// * A identity -// * b linearization point -// */ -// Matrix A = eye(2); -// Vector b = Vector_(2, x, y); -// GaussianFactor::shared_ptr f1( -// new GaussianFactor("x", sub(A, 0,2, 0,1), // A(:,1) -// "y", sub(A, 0,2, 1,2), // A(:,2) -// b, // rhs of f(x) -// probModel2)); // arbitrary sigma -// -// /** create the constraint-linear factor -// * Provides a mechanism to use variable gain to force the constraint -// * \lambda*gradG*dx + d\lambda = zero -// * formulated in matrix form as: -// * [\lambda*gradG eye(1)] [dx; d\lambda] = zero -// */ -// Matrix gradG = Matrix_(1, 2,2*x, -1.0); -// GaussianFactor::shared_ptr f2( -// new GaussianFactor("x", lambda*sub(gradG, 0,1, 0,1), // scaled gradG(:,1) -// "y", lambda*sub(gradG, 0,1, 1,2), // scaled gradG(:,2) -// "L", eye(1), // dlambda term -// Vector_(1, 0.0), // rhs is zero -// probModel1)); // arbitrary sigma -// -// // create the actual constraint -// // [gradG] [x; y] - g = 0 -// Vector g = Vector_(1,x*x-y-5); -// GaussianFactor::shared_ptr c1( -// new GaussianFactor("x", sub(gradG, 0,1, 0,1), // slice first part of gradG -// "y", sub(gradG, 0,1, 1,2), // slice second part of gradG -// g, // value of constraint function -// constraintModel1)); // force to constraint -// -// // construct graph -// GaussianFactorGraph fg; -// fg.push_back(f1); -// fg.push_back(f2); -// fg.push_back(c1); -// if (verbose) fg.print("Graph"); -// -// // solve -// Ordering ord; -// ord += "x", "y", "L"; -// VectorConfig delta = fg.optimize(ord); -// if (verbose) delta.print("Delta"); -// -// // update initial estimate -// VectorConfig newState = expmap(state, delta.scale(-1.0)); -// -// // set the state to the updated state -// state = newState; -// -// if (verbose) state.print("Updated State"); -// } -// -// // verify that it converges to the nearest optimal point -// VectorConfig expected; -// expected.insert("x", Vector_(1, 2.12)); -// expected.insert("y", Vector_(1, -0.5)); -// CHECK(assert_equal(state["x"], expected["x"], 1e-2)); -// CHECK(assert_equal(state["y"], expected["y"], 1e-2)); -//} -// -///* ********************************************************************* */ -// -//typedef simulated2D::Config Config2D; -//typedef NonlinearFactorGraph NLGraph; -//typedef NonlinearEquality NLE; -//typedef boost::shared_ptr shared; -//typedef NonlinearOptimizer Optimizer; -// -//typedef TypedSymbol LamKey; -// -///* -// * Determining a ground truth linear system -// * with two poses seeing one landmark, with each pose -// * constrained to a particular value -// */ -//TEST (SQP, two_pose_truth ) { -// bool verbose = false; -// -// // create a graph -// shared_ptr graph(new NLGraph); -// -// // 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::Measurement(z1, sigma, x1,l1)); -// graph->push_back(f1); -// -// // measurement from x2 to l1 -// Point2 z2(-4.0, 0.0); -// shared f2(new simulated2D::Measurement(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 -// shared_ptr ordering(new Ordering()); -// *ordering += "x1", "x2", "l1"; -// Optimizer::shared_solver solver(new Optimizer::solver(ordering)); -// Optimizer optimizer(graph, initialEstimate, solver); -// -// // display solution -// double relativeThreshold = 1e-5; -// double absoluteThreshold = 1e-5; -// Optimizer act_opt = optimizer.gaussNewton(relativeThreshold, absoluteThreshold); -// boost::shared_ptr actual = act_opt.config(); -// if (verbose) actual->print("Configuration after optimization"); -// -// // 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 sqp_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 sqp_test1 -// -////namespace sqp_test2 { -//// -//// // Unary Constraint on x1 -//// /** g(x) = x -[1;1] = 0 */ -//// Vector g(const Config2D& config, const list& keys) { -//// return config[keys.front()] - Vector_(2, 1.0, 1.0); -//// } -//// -//// /** jacobian at x1 */ -//// Matrix G(const Config2D& config, const list& keys) { -//// return eye(2); -//// } -//// -////} // \namespace sqp_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 (SQP, two_pose ) { -// bool verbose = false; -// -// // create the graph -// shared_ptr graph(new NLGraph); -// -// // 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::Measurement(z1, sigma, x1,l1)); -// graph->push_back(f1); -// -// // measurement from x2 to l2 -// Point2 z2(-4.0, 0.0); -// shared f2(new simulated2D::Measurement(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(sqp_test1::g, _1, keys2), -// l1, boost::bind(sqp_test1::G1, _1, keys2), -// l2, boost::bind(sqp_test1::G2, _1, keys2), -// 2, "L1")); -// graph->push_back(c2); -// -// // 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 an initial estimate for the lagrange multiplier -// shared_ptr initLagrange(new VectorConfig); -// initLagrange->insert(LamKey(1), Vector_(2, 1.0, 1.0)); // connect the landmarks -// -// // create state config variables and initialize them -// Config2D state(*initialEstimate); -// VectorConfig state_lambda(*initLagrange); -// -// // optimization loop -// int maxIt = 1; -// for (int i = 0; i >::const_iterator const_iterator; -// // iterate over all factors -// for (const_iterator factor = graph->begin(); factor < graph->end(); factor++) { -// const shared_ptr constraint = boost::shared_dynamic_cast(*factor); -// if (constraint == NULL) { -// // if a regular factor, linearize using the default linearization -// GaussianFactor::shared_ptr f = (*factor)->linearize(state); -// fg.push_back(f); -// } else { -// // if a constraint, linearize using the constraint method (2 configs) -// GaussianFactor::shared_ptr f, c; -// boost::tie(f,c) = constraint->linearize(state, state_lambda); -// fg.push_back(f); -// fg.push_back(c); -// } -// } -// -// if (verbose) fg.print("Linearized graph"); -// -// // create an ordering -// Ordering ordering; -// ordering += "x1", "x2", "l1", "l2", "L1"; -// -// // optimize linear graph to get full delta config -// VectorConfig delta = fg.optimize(ordering); -// if (verbose) delta.print("Delta Config"); -// -// // update both state variables -// state = expmap(state, delta); -// if (verbose) state.print("newState"); -// state_lambda = expmap(state_lambda, delta); -// if (verbose) state_lambda.print("newStateLam"); -// } -// -// // 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 visualSLAM::Config VConfig; -//typedef visualSLAM::Graph VGraph; -//typedef boost::shared_ptr shared_vf; -//typedef NonlinearOptimizer VOptimizer; -//typedef SQPOptimizer VSOptimizer; -//typedef NonlinearConstraint2< -// VConfig, visualSLAM::PointKey, Pose3, visualSLAM::PointKey, Pose3> VNLC2; -// -// -///** -// * Ground truth for a visual SLAM example with stereo vision -// */ -//TEST (SQP, 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 Config); -// truthConfig->insert(1, camera1.pose()); -// truthConfig->insert(2, camera2.pose()); -// truthConfig->insert(1, landmark); -// -// // create graph -// shared_ptr graph(new VGraph()); -// -// // create equality constraints for poses -// graph->push_back(shared_ptr(new PoseConstraint(1, camera1.pose()))); -// graph->push_back(shared_ptr(new PoseConstraint(2, camera2.pose()))); -// -// // create VSLAM factors -// Point2 z1 = camera1.project(landmark); -// if (verbose) z1.print("z1"); -// shared_vf vf1(new ProjectionFactor(z1, 1.0, 1, 1, shK)); -// graph->push_back(vf1); -// Point2 z2 = camera2.project(landmark); -// if (verbose) z2.print("z2"); -// shared_vf vf2(new ProjectionFactor(z2, 1.0, 2, 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 (SQP, 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 Config); -// truthConfig->insert(1, camera1.pose()); -// truthConfig->insert(2, camera2.pose()); -// truthConfig->insert(1, landmark); -// -// // create config -// boost::shared_ptr noisyConfig(new Config); -// noisyConfig->insert(1, camera1.pose()); -// noisyConfig->insert(2, camera2.pose()); -// noisyConfig->insert(1, landmarkNoisy); -// -// // create graph -// shared_ptr graph(new Graph()); -// -// // create equality constraints for poses -// graph->push_back(shared_ptr(new PoseConstraint(1, camera1.pose()))); -// graph->push_back(shared_ptr(new PoseConstraint(2, camera2.pose()))); -// -// // create VSLAM factors -// Point2 z1 = camera1.project(landmark); -// if (verbose) z1.print("z1"); -// shared_vf vf1(new ProjectionFactor(z1, 1.0, 1, 1, shK)); -// graph->push_back(vf1); -// Point2 z2 = camera2.project(landmark); -// if (verbose) z2.print("z2"); -// shared_vf vf2(new ProjectionFactor(z2, 1.0, 2, 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-9); -// -// // check if correct -// if (verbose) { -// optimizer.config()->print("After iteration"); -// cout << "Final error: " << optimizer.error() << endl; -// } -// CHECK(assert_equal(*truthConfig,*(optimizer.config()))); -//} -// -///* ********************************************************************* */ -//namespace sqp_stereo { -// -// // binary constraint between landmarks -// /** g(x) = x-y = 0 */ -// Vector g(const Config& config, const list& keys) { -// return config[PointKey(keys.front().index())].vector() -// - config[PointKey(keys.back().index())].vector(); -// } -// -// /** jacobian at l1 */ -// Matrix G1(const Config& config, const list& keys) { -// return eye(3); -// } -// -// /** jacobian at l2 */ -// Matrix G2(const Config& config, const list& keys) { -// return -1.0 * eye(3); -// } -// -//} // \namespace sqp_stereo -// -///* ********************************************************************* */ -//VGraph 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 -// VGraph graph; -// -// // create equality constraints for poses -// graph.push_back(shared_ptr(new PoseConstraint(1, camera1.pose()))); -// graph.push_back(shared_ptr(new PoseConstraint(2, camera2.pose()))); -// -// // create factors -// Point2 z1 = camera1.project(landmark1); -// shared_vf vf1(new ProjectionFactor(z1, 1.0, 1, 1, shK)); -// graph.push_back(vf1); -// Point2 z2 = camera2.project(landmark2); -// shared_vf vf2(new ProjectionFactor(z2, 1.0, 2, 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 -// list keys; keys += "l1", "l2"; -// visualSLAM::PointKey l1(1), l2(2); -// shared_ptr c2( -// new VNLC2(boost::bind(sqp_stereo::g, _1, keys), -// l1, boost::bind(sqp_stereo::G1, _1, keys), -// l2, boost::bind(sqp_stereo::G2, _1, keys), -// 3, "L12")); -// 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 Config); -// truthConfig->insert(1, camera1.pose()); -// truthConfig->insert(2, camera2.pose()); -// truthConfig->insert(1, landmark1); -// truthConfig->insert(2, landmark2); // create two landmarks in same place -// -// return truthConfig; -//} -// -///* ********************************************************************* -// * SQP version of the above stereo example, -// * with the initial case as the ground truth -// */ -//TEST (SQP, stereo_sqp ) { -// bool verbose = false; -// -// // get a graph -// VGraph graph = stereoExampleGraph(); -// if (verbose) graph.print("Graph after construction"); -// -// // get the truth config -// boost::shared_ptr truthConfig = stereoExampleTruthConfig(); -// -// // create ordering -// Ordering ord; -// ord += "x1", "x2", "l1", "l2"; -// -// // create optimizer -// VSOptimizer optimizer(graph, ord, truthConfig); -// -// // optimize -// VSOptimizer 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 (SQP, stereo_sqp_noisy ) { -// bool verbose = false; -// -// // get a graph -// Graph 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 Config); -// initConfig->insert(1, pose1); -// initConfig->insert(2, pose2); -// initConfig->insert(1, landmark1); -// initConfig->insert(2, landmark2); // create two landmarks in same place -// -// // create ordering -// Ordering ord; -// ord += "x1", "x2", "l1", "l2"; -// -// // create optimizer -// VSOptimizer optimizer(graph, ord, initConfig); -// -// // optimize -// double start_error = optimizer.error(); -// int maxIt = 2; -// for (int i=0; iprint(); -// if (verbose) optimizer.config()->print(); -// if (verbose) -// optimizer = optimizer.iterate(VSOptimizer::FULL); -// else -// optimizer = optimizer.iterate(VSOptimizer::SILENT); -// } -// -// if (verbose) cout << "Initial Error: " << start_error << "\n" -// << "Final Error: " << optimizer.error() << endl; -// -// // get the truth config -// boost::shared_ptr truthConfig = stereoExampleTruthConfig(); -// -// if (verbose) { -// initConfig->print("Initial Config"); -// truthConfig->print("Truth Config"); -// optimizer.config()->print("After optimization"); -// } -// -// // check if correct -// CHECK(assert_equal(*truthConfig,*(optimizer.config()))); -//} -// -///* ********************************************************************* -// * SQP version of the above stereo example, -// * with noise in the initial estimate and manually specified -// * lagrange multipliers -// */ -//TEST (SQP, stereo_sqp_noisy_manualLagrange ) { -// bool verbose = false; -// -// // get a graph -// Graph 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 Config); -// initConfig->insert(1, pose1); -// initConfig->insert(2, pose2); -// initConfig->insert(1, landmark1); -// initConfig->insert(2, landmark2); // create two landmarks in same place -// -// // create ordering with lagrange multiplier included -// Ordering ord; -// ord += "x1", "x2", "l1", "l2", "L12"; -// -// // create lagrange multipliers -// VSOptimizer::shared_vconfig initLagrangeConfig(new VectorConfig); -// initLagrangeConfig->insert("L12", Vector_(3, 0.0, 0.0, 0.0)); -// -// // create optimizer -// VSOptimizer optimizer(graph, ord, initConfig, initLagrangeConfig); -// -// // optimize -// double start_error = optimizer.error(); -// int maxIt = 5; -// for (int i=0; iprint("Config Before Iteration"); -// optimizer.configLagrange()->print("Lagrange Before Iteration"); -// optimizer = optimizer.iterate(VSOptimizer::FULL); -// } -// else -// optimizer = optimizer.iterate(VSOptimizer::SILENT); -// } -// -// if (verbose) cout << "Initial Error: " << start_error << "\n" -// << "Final Error: " << optimizer.error() << endl; -// -// // get the truth config -// boost::shared_ptr truthConfig = stereoExampleTruthConfig(); -// -// if (verbose) { -// initConfig->print("Initial Config"); -// truthConfig->print("Truth Config"); -// optimizer.config()->print("After optimization"); -// } -// -// // check if correct -// CHECK(assert_equal(*truthConfig,*(optimizer.config()))); -//} +/* ********************************************************************* + * This example uses a nonlinear objective function and + * nonlinear equality constraint. The formulation is actually + * the Cholesky form that creates the full Hessian explicitly, + * which should really be avoided with our QR-based machinery. + * + * Note: the update equation used here has a fixed step size + * and gain that is rather arbitrarily chosen, and as such, + * will take a silly number of iterations. + */ +TEST (SQP, problem1_cholesky ) { + bool verbose = false; + // use a nonlinear function of f(x) = x^2+y^2 + // nonlinear equality constraint: g(x) = x^2-5-y=0 + // Lagrangian: f(x) + \lambda*g(x) + + // Symbols + Symbol x1("x1"), y1("y1"), L1("L1"); + + // state structure: [x y \lambda] + VectorConfig init, state; + init.insert(x1, Vector_(1, 1.0)); + init.insert(y1, Vector_(1, 1.0)); + init.insert(L1, Vector_(1, 1.0)); + state = init; + + if (verbose) init.print("Initial State"); + + // loop until convergence + int maxIt = 10; + for (int i = 0; i ||Ax-b||^2 + * where: + * h(x) simply returns the inputs + * z zeros(2) + * A identity + * b linearization point + */ + Matrix A = eye(2); + Vector b = Vector_(2, x, y); + GaussianFactor::shared_ptr f1( + new GaussianFactor(x1, sub(A, 0,2, 0,1), // A(:,1) + y1, sub(A, 0,2, 1,2), // A(:,2) + b, // rhs of f(x) + probModel2)); // arbitrary sigma + + /** create the constraint-linear factor + * Provides a mechanism to use variable gain to force the constraint + * \lambda*gradG*dx + d\lambda = zero + * formulated in matrix form as: + * [\lambda*gradG eye(1)] [dx; d\lambda] = zero + */ + Matrix gradG = Matrix_(1, 2,2*x, -1.0); + GaussianFactor::shared_ptr f2( + new GaussianFactor(x1, lambda*sub(gradG, 0,1, 0,1), // scaled gradG(:,1) + y1, lambda*sub(gradG, 0,1, 1,2), // scaled gradG(:,2) + L1, eye(1), // dlambda term + Vector_(1, 0.0), // rhs is zero + probModel1)); // arbitrary sigma + + // create the actual constraint + // [gradG] [x; y] - g = 0 + Vector g = Vector_(1,x*x-y-5); + GaussianFactor::shared_ptr c1( + new GaussianFactor(x1, sub(gradG, 0,1, 0,1), // slice first part of gradG + y1, sub(gradG, 0,1, 1,2), // slice second part of gradG + g, // value of constraint function + constraintModel1)); // force to constraint + + // construct graph + GaussianFactorGraph fg; + fg.push_back(f1); + fg.push_back(f2); + fg.push_back(c1); + if (verbose) fg.print("Graph"); + + // solve + Ordering ord; + ord += x1, y1, L1; + VectorConfig delta = fg.optimize(ord); + if (verbose) delta.print("Delta"); + + // update initial estimate + VectorConfig newState = expmap(state, delta.scale(-1.0)); + + // set the state to the updated state + state = newState; + + if (verbose) state.print("Updated State"); + } + + // verify that it converges to the nearest optimal point + VectorConfig expected; + expected.insert(x1, Vector_(1, 2.12)); + expected.insert(y1, Vector_(1, -0.5)); + CHECK(assert_equal(state[x1], expected[x1], 1e-2)); + CHECK(assert_equal(state[y1], expected[y1], 1e-2)); +} + +/* ********************************************************************* */ + +// Basic configs +typedef LieConfig LagrangeConfig; + +// full components +typedef TupleConfig3, + LieConfig, + LieConfig > Config2D; +//typedef TupleConfig > Config2D; +typedef NonlinearFactorGraph Graph2D; +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 (SQP, two_pose_truth ) { + bool verbose = false; + + // 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 + shared_ptr ordering(new Ordering()); + *ordering += "x1", "x2", "l1"; + Optimizer::shared_solver solver(new Optimizer::solver(ordering)); + Optimizer optimizer(graph, initialEstimate, solver); + + // display solution + double relativeThreshold = 1e-5; + double absoluteThreshold = 1e-5; + Optimizer act_opt = optimizer.gaussNewton(relativeThreshold, absoluteThreshold); + boost::shared_ptr actual = act_opt.config(); + if (verbose) actual->print("Configuration after optimization"); + + // 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 sqp_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 sqp_test1 + +namespace sqp_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 sqp_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 (SQP, 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 + LagrangeKey L1(1); + list keys2; keys2 += l1, l2; + boost::shared_ptr c2(new NLC2( + boost::bind(sqp_test1::g, _1, keys2), + l1, boost::bind(sqp_test1::G1, _1, keys2), + l2, boost::bind(sqp_test1::G2, _1, keys2), + 2, L1)); + 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 + initialEstimate->insert(L1, Vector_(2, 1.0, 1.0)); // connect the landmarks + + // create state config variables and initialize them + Config2D state(*initialEstimate); + + // linearize the graph + GaussianFactorGraph fg = graph->linearize(state); + + if (verbose) fg.print("Linearized graph"); + + // create an ordering + Ordering ordering; + ordering += "x1", "x2", "l1", "l2", "L1"; + + // 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)); + expected.insert(L1, Vector_(2, 6.0, 7.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 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 (SQP, 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 (SQP, 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-9); + + // check if correct + if (verbose) { + optimizer.config()->print("After iteration"); + cout << "Final error: " << optimizer.error() << endl; + } + CHECK(assert_equal(*truthConfig,*(optimizer.config()))); +} + +/* ********************************************************************* */ +namespace sqp_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 sqp_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; + LagrangeKey L12(12); + shared_ptr c2( + new VNLC2(boost::bind(sqp_stereo::g, _1, keys), + l1, boost::bind(sqp_stereo::G1, _1, keys), + l2, boost::bind(sqp_stereo::G2, _1, keys), + 3, L12)); + 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 (SQP, stereo_sqp ) { + 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(); + truthConfig->insert(LagrangeKey(12), Vector_(3, 1.0, 1.0, 1.0)); + + // create ordering + shared_ptr ord(new Ordering()); + *ord += "x1", "x2", "l1", "l2", "L12"; + 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 (SQP, stereo_sqp_noisy ) { + bool verbose = false; + + // 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 + initConfig->insert(LagrangeKey(12), Vector_(3, 1.0, 1.0, 1.0)); + + // create ordering + shared_ptr ord(new Ordering()); + *ord += "x1", "x2", "l1", "l2", "L12"; + 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(); + truthConfig->insert(LagrangeKey(12), Vector_(3, 0.0, 1.0, 1.0)); + + // check if correct + CHECK(assert_equal(*truthConfig,*actual, 1e-5)); +} /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); }