diff --git a/cpp/testSQP.cpp b/cpp/testSQP.cpp index 713b74eb9..1759b895a 100644 --- a/cpp/testSQP.cpp +++ b/cpp/testSQP.cpp @@ -43,851 +43,851 @@ 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 { +///* ********************************************************************* +// * 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) // -// // 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); +// // 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& keys) { +// // verify that it converges to the nearest optimal point +// VectorConfig expected; +// expected.insert("L", Vector_(1, -1.0)); +// expected.insert("x", Vector_(1, 2.12)); +// expected.insert("y", Vector_(1, -0.5)); +// CHECK(assert_equal(expected,state, 1e-2)); +//} +// +///* ********************************************************************* +// * This example uses a nonlinear objective function and +// * nonlinear equality constraint. This formulation splits +// * the constraint into a factor and a linear constraint. +// * +// * This example uses the same silly number of iterations as the +// * previous example. +// */ +//TEST (SQP, problem1_sqp ) { +// 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 = 5; +// 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); // } // -//} // \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()))); -} +// /** 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()))); +//} /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } diff --git a/cpp/testSQPOptimizer.cpp b/cpp/testSQPOptimizer.cpp index 8a66856a6..9e6edc17c 100644 --- a/cpp/testSQPOptimizer.cpp +++ b/cpp/testSQPOptimizer.cpp @@ -17,11 +17,11 @@ #include "NonlinearEquality.h" #include "VectorConfig.h" #include "Ordering.h" -#include "SQPOptimizer.h" +//#include "SQPOptimizer.h" // implementations #include "NonlinearConstraint-inl.h" -#include "SQPOptimizer-inl.h" +//#include "SQPOptimizer-inl.h" using namespace std; using namespace gtsam; @@ -31,349 +31,335 @@ using namespace simulated2D; static SharedGaussian sigma(noiseModel::Isotropic::Sigma(1,0.1)); -// typedefs -typedef simulated2D::Config Config2D; -typedef boost::shared_ptr shared_config; -typedef NonlinearFactorGraph NLGraph; -typedef boost::shared_ptr > shared; +//// typedefs +//typedef simulated2D::Config Config2D; +//typedef boost::shared_ptr shared_config; +//typedef NonlinearFactorGraph NLGraph; +//typedef boost::shared_ptr > shared; +// +//namespace map_warp_example { +//typedef NonlinearConstraint1< +// Config2D, simulated2D::PoseKey, Point2> NLC1; +//typedef NonlinearConstraint2< +// Config2D, simulated2D::PointKey, Point2, simulated2D::PointKey, Point2> NLC2; +//} // \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 sqp_LinearMapWarp2 { +//// binary constraint between landmarks +///** g(x) = x-y = 0 */ +//Vector g_func(const Config2D& config, const PointKey& key1, const 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 sqp_LinearMapWarp2 +// +//namespace sqp_LinearMapWarp1 { +//// Unary Constraint on x1 +///** g(x) = x -[1;1] = 0 */ +//Vector g_func(const Config2D& config, const 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 sqp_LinearMapWarp12 +// +////typedef NonlinearOptimizer Optimizer; +// +///** +// * Creates the graph with each robot seeing the landmark, and it is +// * known that it is the same landmark +// */ +//NLGraph linearMapWarpGraph() { +// using namespace map_warp_example; +// // keys +// PoseKey x1(1), x2(2); +// PointKey l1(1), l2(2); +// +// // constant constraint on x1 +// list keyx; keyx += "x1"; +// LagrangeKey L1(1); +// shared_ptr c1(new NLC1(boost::bind(sqp_LinearMapWarp1::g_func, _1, x1), +// x1, boost::bind(sqp_LinearMapWarp1::jac_g, _1), +// 2, L1)); +// +// // measurement from x1 to l1 +// Point2 z1(0.0, 5.0); +// shared f1(new simulated2D::Measurement(z1, sigma, 1,1)); +// +// // measurement from x2 to l2 +// Point2 z2(-4.0, 0.0); +// shared f2(new simulated2D::Measurement(z2, sigma, 2,2)); +// +// // equality constraint between l1 and l2 +// LagrangeKey L12(12); +// list keys; keys += "l1", "l2"; +// shared_ptr c2 (new NLC2( +// boost::bind(sqp_LinearMapWarp2::g_func, _1, l1, l2), +// l1, boost::bind(sqp_LinearMapWarp2::jac_g1, _1), +// l2, boost::bind(sqp_LinearMapWarp2::jac_g2, _1), +// 2, L12)); +// +// // construct the graph +// NLGraph graph; +// graph.push_back(c1); +// graph.push_back(c2); +// graph.push_back(f1); +// graph.push_back(f2); +// +// return graph; +//} -namespace map_warp_example { -typedef NonlinearConstraint1< - Config2D, simulated2D::PoseKey, Point2> NLC1; -typedef NonlinearConstraint2< - Config2D, simulated2D::PointKey, Point2, simulated2D::PointKey, Point2> NLC2; -} // \namespace map_warp_example +///* ********************************************************************* */ +//TEST ( SQPOptimizer, map_warp_initLam ) { +// bool verbose = false; +// // get a graph +// NLGraph graph = linearMapWarpGraph(); +// +// // keys +// PoseKey x1(1), x2(2); +// PointKey l1(1), l2(2); +// LagrangeKey L1(1), L12(12); +// +// // create an initial estimate +// shared_config 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 +// initialEstimate->insert(L12, Vector_(2, 1.0, 1.0)); +// initialEstimate->insert(L1, Vector_(2, 1.0, 1.0)); +// +// // create an ordering +// Ordering ordering; +// ordering += "x1", "x2", "l1", "l2", "L12", "L1"; +// +// // create an optimizer +// Optimizer optimizer(graph, ordering, initialEstimate); +// if (verbose) optimizer.print("Initialized Optimizer"); +// +// // 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)); +//} -/* ********************************************************************* */ -TEST ( SQPOptimizer, basic ) { - // create a basic optimizer - NLGraph graph; - Ordering ordering; - shared_config config(new Config2D); - - SQPOptimizer optimizer(graph, ordering, config); - - // verify components - CHECK(assert_equal(graph, *(optimizer.graph()))); - CHECK(assert_equal(ordering, *(optimizer.ordering()))); - CHECK(assert_equal(*config, *(optimizer.config()))); -} - -/* ********************************************************************* */ -// 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 sqp_LinearMapWarp2 { -// binary constraint between landmarks -/** g(x) = x-y = 0 */ -Vector g_func(const Config2D& config, const PointKey& key1, const 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 sqp_LinearMapWarp2 - -namespace sqp_LinearMapWarp1 { -// Unary Constraint on x1 -/** g(x) = x -[1;1] = 0 */ -Vector g_func(const Config2D& config, const 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 sqp_LinearMapWarp12 - -typedef SQPOptimizer Optimizer; - -/** - * Creates the graph with each robot seeing the landmark, and it is - * known that it is the same landmark - */ -NLGraph linearMapWarpGraph() { - using namespace map_warp_example; - // keys - PoseKey x1(1), x2(2); - PointKey l1(1), l2(2); - - // constant constraint on x1 - list keyx; keyx += "x1"; - shared_ptr c1(new NLC1(boost::bind(sqp_LinearMapWarp1::g_func, _1, x1), - x1, boost::bind(sqp_LinearMapWarp1::jac_g, _1), - 2, "L1")); - - // measurement from x1 to l1 - Point2 z1(0.0, 5.0); - shared f1(new simulated2D::Measurement(z1, sigma, 1,1)); - - // measurement from x2 to l2 - Point2 z2(-4.0, 0.0); - shared f2(new simulated2D::Measurement(z2, sigma, 2,2)); - - // equality constraint between l1 and l2 - list keys; keys += "l1", "l2"; - shared_ptr c2 (new NLC2( - boost::bind(sqp_LinearMapWarp2::g_func, _1, l1, l2), - l1, boost::bind(sqp_LinearMapWarp2::jac_g1, _1), - l2, boost::bind(sqp_LinearMapWarp2::jac_g2, _1), - 2, "L12")); - - // construct the graph - NLGraph graph; - graph.push_back(c1); - graph.push_back(c2); - graph.push_back(f1); - graph.push_back(f2); - - return graph; -} - -/* ********************************************************************* */ -TEST ( SQPOptimizer, map_warp_initLam ) { - bool verbose = false; - // get a graph - NLGraph graph = linearMapWarpGraph(); - - // keys - PoseKey x1(1), x2(2); - PointKey l1(1), l2(2); - - // create an initial estimate - shared_config 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 initial estimate for the lagrange multiplier - shared_ptr initLagrange(new VectorConfig); - initLagrange->insert("L12", Vector_(2, 1.0, 1.0)); - initLagrange->insert("L1", Vector_(2, 1.0, 1.0)); - - // create an ordering - Ordering ordering; - ordering += "x1", "x2", "l1", "l2", "L12", "L1"; - - // create an optimizer - Optimizer optimizer(graph, ordering, initialEstimate, initLagrange); - if (verbose) optimizer.print("Initialized Optimizer"); - - // 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)); -} - -/* ********************************************************************* */ -TEST ( SQPOptimizer, map_warp ) { - bool verbose = false; - // get a graph - NLGraph graph = linearMapWarpGraph(); - if (verbose) graph.print("Initial map warp graph"); - - // keys - PoseKey x1(1), x2(2); - PointKey l1(1), l2(2); - - // create an initial estimate - shared_config initialEstimate(new Config2D); - initialEstimate->insert(x1, Point2(1.0, 1.0)); - initialEstimate->insert(l1, Point2(.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 - Ordering ordering; - ordering += "x1", "x2", "l1", "l2"; - - // create an optimizer - Optimizer optimizer(graph, ordering, initialEstimate); - - // 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 -// is a binary inequality constraint connecting the obstacle to the -// states, which enforces a minimum distance. -/* ********************************************************************* */ - -typedef NonlinearConstraint2 AvoidConstraint; -typedef shared_ptr shared_a; -typedef NonlinearEquality PoseConstraint; -typedef shared_ptr shared_pc; -typedef NonlinearEquality ObstacleConstraint; -typedef shared_ptr shared_oc; - - -namespace sqp_avoid1 { -// avoidance radius -double radius = 1.0; - -// binary avoidance constraint -/** g(x) = ||x2-obs||^2 - radius^2 > 0 */ -Vector g_func(const Config2D& config, const PoseKey& x, const PointKey& obs) { - double dist2 = config[x].dist(config[obs]); - double thresh = radius*radius; - return Vector_(1, dist2-thresh); -} - -/** jacobian at pose */ -Matrix jac_g1(const Config2D& config, const PoseKey& x, const PointKey& obs) { - Point2 p = config[x]-config[obs]; - return Matrix_(1,2, 2.0*p.x(), 2.0*p.y()); -} - -/** jacobian at obstacle */ -Matrix jac_g2(const Config2D& config, const PoseKey& x, const PointKey& obs) { - Point2 p = config[x]-config[obs]; - return Matrix_(1,2, -2.0*p.x(), -2.0*p.y()); -} -} - -pair obstacleAvoidGraph() { - // Keys - PoseKey x1(1), x2(2), x3(3); - PointKey l1(1); - - // Constrained Points - Point2 pt_x1, - pt_x3(10.0, 0.0), - pt_l1(5.0, -0.5); - - shared_pc e1(new PoseConstraint(x1, pt_x1)); - shared_pc e2(new PoseConstraint(x3, pt_x3)); - shared_oc e3(new ObstacleConstraint(l1, pt_l1)); - - // measurement from x1 to x2 - Point2 x1x2(5.0, 0.0); - shared f1(new simulated2D::Odometry(x1x2, sigma, 1,2)); - - // measurement from x2 to x3 - Point2 x2x3(5.0, 0.0); - shared f2(new simulated2D::Odometry(x2x3, sigma, 2,3)); - - // create a binary inequality constraint that forces the middle point away from - // the obstacle - shared_a c1(new AvoidConstraint(boost::bind(sqp_avoid1::g_func, _1, x2, l1), - x2, boost::bind(sqp_avoid1::jac_g1, _1, x2, l1), - l1,boost::bind(sqp_avoid1::jac_g2, _1, x2, l1), - 1, "L20", false)); - - // construct the graph - NLGraph graph; - graph.push_back(e1); - graph.push_back(e2); - graph.push_back(e3); - graph.push_back(c1); - graph.push_back(f1); - graph.push_back(f2); - - // make a config of the fixed values, for convenience - Config2D config; - config.insert(x1, pt_x1); - config.insert(x3, pt_x3); - config.insert(l1, pt_l1); - - return make_pair(graph, config); -} - -/* ********************************************************************* */ -TEST ( SQPOptimizer, inequality_avoid ) { - // create the graph - NLGraph graph; Config2D feasible; - boost::tie(graph, feasible) = obstacleAvoidGraph(); - - // create the rest of the config - shared_ptr init(new Config2D(feasible)); - PoseKey x2(2); - init->insert(x2, Point2(5.0, 100.0)); - - // create an ordering - Ordering ord; - ord += "x1", "x2", "x3", "l1"; - - // create an optimizer - Optimizer optimizer(graph, ord, init); - - // perform an iteration of optimization - // NOTE: the constraint will be inactive in the first iteration, - // so it will violate the constraint after one iteration - Optimizer afterOneIteration = optimizer.iterate(Optimizer::SILENT); - - Config2D exp1(feasible); - exp1.insert(x2, Point2(5.0, 0.0)); - CHECK(assert_equal(exp1, *(afterOneIteration.config()))); - - // the second iteration will activate the constraint and force the - // config to a viable configuration. - Optimizer after2ndIteration = afterOneIteration.iterate(Optimizer::SILENT); - - Config2D exp2(feasible); - exp2.insert(x2, Point2(5.0, 0.5)); - CHECK(assert_equal(exp2, *(after2ndIteration.config()))); -} - -/* ********************************************************************* */ -TEST ( SQPOptimizer, inequality_avoid_iterative ) { - // create the graph - NLGraph graph; Config2D feasible; - boost::tie(graph, feasible) = obstacleAvoidGraph(); - - // create the rest of the config - shared_ptr init(new Config2D(feasible)); - PoseKey x2(2); - init->insert(x2, Point2(5.0, 100.0)); - - // create an ordering - Ordering ord; - ord += "x1", "x2", "x3", "l1"; - - // create an optimizer - Optimizer optimizer(graph, ord, init); - - double relThresh = 1e-5; // minimum change in error between iterations - double absThresh = 1e-5; // minimum error necessary to converge - double constraintThresh = 1e-9; // minimum constraint error to be feasible - Optimizer final = optimizer.iterateSolve(relThresh, absThresh, constraintThresh); - - // verify - Config2D exp2(feasible); - exp2.insert(x2, Point2(5.0, 0.5)); - CHECK(assert_equal(exp2, *(final.config()))); -} +///* ********************************************************************* */ +//TEST ( SQPOptimizer, map_warp ) { +// bool verbose = false; +// // get a graph +// NLGraph graph = linearMapWarpGraph(); +// if (verbose) graph.print("Initial map warp graph"); +// +// // keys +// PoseKey x1(1), x2(2); +// PointKey l1(1), l2(2); +// +// // create an initial estimate +// shared_config initialEstimate(new Config2D); +// initialEstimate->insert(x1, Point2(1.0, 1.0)); +// initialEstimate->insert(l1, Point2(.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 +// Ordering ordering; +// ordering += "x1", "x2", "l1", "l2"; +// +// // create an optimizer +// Optimizer optimizer(graph, ordering, initialEstimate); +// +// // 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 +//// is a binary inequality constraint connecting the obstacle to the +//// states, which enforces a minimum distance. +///* ********************************************************************* */ +// +//typedef NonlinearConstraint2 AvoidConstraint; +//typedef shared_ptr shared_a; +//typedef NonlinearEquality PoseConstraint; +//typedef shared_ptr shared_pc; +//typedef NonlinearEquality ObstacleConstraint; +//typedef shared_ptr shared_oc; +// +// +//namespace sqp_avoid1 { +//// avoidance radius +//double radius = 1.0; +// +//// binary avoidance constraint +///** g(x) = ||x2-obs||^2 - radius^2 > 0 */ +//Vector g_func(const Config2D& config, const PoseKey& x, const PointKey& obs) { +// double dist2 = config[x].dist(config[obs]); +// double thresh = radius*radius; +// return Vector_(1, dist2-thresh); +//} +// +///** jacobian at pose */ +//Matrix jac_g1(const Config2D& config, const PoseKey& x, const PointKey& obs) { +// Point2 p = config[x]-config[obs]; +// return Matrix_(1,2, 2.0*p.x(), 2.0*p.y()); +//} +// +///** jacobian at obstacle */ +//Matrix jac_g2(const Config2D& config, const PoseKey& x, const PointKey& obs) { +// Point2 p = config[x]-config[obs]; +// return Matrix_(1,2, -2.0*p.x(), -2.0*p.y()); +//} +//} +// +//pair obstacleAvoidGraph() { +// // Keys +// PoseKey x1(1), x2(2), x3(3); +// PointKey l1(1); +// LagrangeKey L20(20); +// +// // Constrained Points +// Point2 pt_x1, +// pt_x3(10.0, 0.0), +// pt_l1(5.0, -0.5); +// +// shared_pc e1(new PoseConstraint(x1, pt_x1)); +// shared_pc e2(new PoseConstraint(x3, pt_x3)); +// shared_oc e3(new ObstacleConstraint(l1, pt_l1)); +// +// // measurement from x1 to x2 +// Point2 x1x2(5.0, 0.0); +// shared f1(new simulated2D::Odometry(x1x2, sigma, 1,2)); +// +// // measurement from x2 to x3 +// Point2 x2x3(5.0, 0.0); +// shared f2(new simulated2D::Odometry(x2x3, sigma, 2,3)); +// +// // create a binary inequality constraint that forces the middle point away from +// // the obstacle +// shared_a c1(new AvoidConstraint(boost::bind(sqp_avoid1::g_func, _1, x2, l1), +// x2, boost::bind(sqp_avoid1::jac_g1, _1, x2, l1), +// l1,boost::bind(sqp_avoid1::jac_g2, _1, x2, l1), +// 1, L20, false)); +// +// // construct the graph +// NLGraph graph; +// graph.push_back(e1); +// graph.push_back(e2); +// graph.push_back(e3); +// graph.push_back(c1); +// graph.push_back(f1); +// graph.push_back(f2); +// +// // make a config of the fixed values, for convenience +// Config2D config; +// config.insert(x1, pt_x1); +// config.insert(x3, pt_x3); +// config.insert(l1, pt_l1); +// +// return make_pair(graph, config); +//} +// +///* ********************************************************************* */ +//TEST ( SQPOptimizer, inequality_avoid ) { +// // create the graph +// NLGraph graph; Config2D feasible; +// boost::tie(graph, feasible) = obstacleAvoidGraph(); +// +// // create the rest of the config +// shared_ptr init(new Config2D(feasible)); +// PoseKey x2(2); +// init->insert(x2, Point2(5.0, 100.0)); +// +// // create an ordering +// Ordering ord; +// ord += "x1", "x2", "x3", "l1"; +// +// // create an optimizer +// Optimizer optimizer(graph, ord, init); +// +// // perform an iteration of optimization +// // NOTE: the constraint will be inactive in the first iteration, +// // so it will violate the constraint after one iteration +// Optimizer afterOneIteration = optimizer.iterate(Optimizer::SILENT); +// +// Config2D exp1(feasible); +// exp1.insert(x2, Point2(5.0, 0.0)); +// CHECK(assert_equal(exp1, *(afterOneIteration.config()))); +// +// // the second iteration will activate the constraint and force the +// // config to a viable configuration. +// Optimizer after2ndIteration = afterOneIteration.iterate(Optimizer::SILENT); +// +// Config2D exp2(feasible); +// exp2.insert(x2, Point2(5.0, 0.5)); +// CHECK(assert_equal(exp2, *(after2ndIteration.config()))); +//} +// +///* ********************************************************************* */ +//TEST ( SQPOptimizer, inequality_avoid_iterative ) { +// // create the graph +// NLGraph graph; Config2D feasible; +// boost::tie(graph, feasible) = obstacleAvoidGraph(); +// +// // create the rest of the config +// shared_ptr init(new Config2D(feasible)); +// PoseKey x2(2); +// init->insert(x2, Point2(5.0, 100.0)); +// +// // create an ordering +// Ordering ord; +// ord += "x1", "x2", "x3", "l1"; +// +// // create an optimizer +// Optimizer optimizer(graph, ord, init); +// +// double relThresh = 1e-5; // minimum change in error between iterations +// double absThresh = 1e-5; // minimum error necessary to converge +// double constraintThresh = 1e-9; // minimum constraint error to be feasible +// Optimizer final = optimizer.iterateSolve(relThresh, absThresh, constraintThresh); +// +// // verify +// Config2D exp2(feasible); +// exp2.insert(x2, Point2(5.0, 0.5)); +// CHECK(assert_equal(exp2, *(final.config()))); +//} /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); }