12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Moved remaining equality constraint tests to testNonlinearEqualityConstraint. Added NonlinearEquality2, a binary constraint expressing exact equality between variables.

release/4.3a0
Alex Cunningham 2010-08-06 20:07:55 +00:00
parent a55860eb1d
commit 5ada49ea3a
6 changed files with 220 additions and 717 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
nonlinear/NonlinearConstraint.h
View File

@ -223,4 +223,31 @@ public:
boost::optional<Matrix&> H2 = boost::none) const = 0;
};
/**
* Simple binary equality constraint - this constraint forces two factors to
* be the same. This constraint requires the underlying type to a Lie type
*/
template<class Config, class Key, class X>
class NonlinearEquality2 : public NonlinearConstraint2<Config, Key, X, Key, X> {
protected:
typedef NonlinearConstraint2<Config, Key, X, Key, X> Base;
public:
typedef boost::shared_ptr<NonlinearEquality2<Config, Key, X> > shared_ptr;
NonlinearEquality2(const Key& key1, const Key& key2, double mu = 1000.0)
: Base(key1, key2, X::dim(), mu) {}
/** g(x) with optional derivative2 */
Vector evaluateError(const X& x1, const X& x2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
const size_t p = X::dim();
if (H1) *H1 = -eye(p);
if (H2) *H2 = eye(p);
return logmap(x1, x2);
}
};
}

8
slam/simulated2DConstraints.h
View File

@ -18,7 +18,7 @@ namespace gtsam {
namespace simulated2D {
/**
* Unary factor encoding a hard equality on a Point
* Unary constraint encoding a hard equality on a Point
*/
template<class Cfg = Config, class Key = PoseKey>
struct GenericUnaryEqualityConstraint: public NonlinearConstraint1<Cfg, Key, Point2> {
@ -39,7 +39,7 @@ namespace gtsam {
};
/**
* Binary factor simulating "odometry" between two Vectors
* Binary constraint simulating "odometry" between two Poses
*/
template<class Cfg = Config, class Key = PoseKey>
struct GenericOdoHardEqualityConstraint: public NonlinearConstraint2<Cfg, Key, Point2, Key, Point2> {
@ -63,5 +63,9 @@ namespace gtsam {
typedef GenericUnaryEqualityConstraint<Config, PoseKey> UnaryEqualityConstraint;
typedef GenericOdoHardEqualityConstraint<Config, PoseKey> OdoEqualityConstraint;
/** Equality between variables */
typedef NonlinearEquality2<Config, PoseKey, Point2> PoseEqualityConstraint;
typedef NonlinearEquality2<Config, PointKey, Point2> PointEqualityConstraint;
} // namespace simulated2D
} // namespace gtsam

33
slam/smallExample.h
View File

@ -82,7 +82,7 @@ namespace gtsam {
GaussianFactorGraph createSmoother(int T);
/* ******************************************************* */
// Constrained Examples
// Linear Constrained Examples
/* ******************************************************* */
/**
@ -106,37 +106,6 @@ namespace gtsam {
GaussianFactorGraph createMultiConstraintGraph();
VectorConfig createMultiConstraintConfig();
/**
* These are the old examples from the EqualityFactor/DeltaFunction
* They should be updated for use at some point, but are disabled for now
*/
/**
* Create configuration for constrained example
* This is the ground truth version
*/
//VectorConfig createConstrainedConfig();
/**
* Create a noisy configuration for linearization
*/
//VectorConfig createConstrainedLinConfig();
/**
* Create the correct delta configuration
*/
//VectorConfig createConstrainedCorrectDelta();
/**
* Create small example constrained factor graph
*/
//GaussianFactorGraph createConstrainedGaussianFactorGraph();
/**
* Create small example constrained nonlinear factor graph
*/
// ConstrainedNonlinearFactorGraph<NonlinearFactor<VectorConfig>,VectorConfig>
// createConstrainedNonlinearFactorGraph();
/* ******************************************************* */
// Planar graph with easy subtree for SubgraphPreconditioner
/* ******************************************************* */

5
slam/visualSLAM.h
View File

@ -27,6 +27,7 @@ namespace gtsam { namespace visualSLAM {
typedef LieConfig<PoseKey, Pose3> PoseConfig;
typedef LieConfig<PointKey, Point3> PointConfig;
typedef TupleConfig2<PoseConfig, PointConfig> Config;
typedef boost::shared_ptr<Config> shared_config;
typedef NonlinearEquality<Config, PoseKey, Pose3> PoseConstraint;
typedef NonlinearEquality<Config, PointKey, Point3> PointConstraint;
@ -50,7 +51,7 @@ namespace gtsam { namespace visualSLAM {
typedef NonlinearFactor2<Cfg, PosK, Pose3, LmK, Point3> Base;
// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<GenericProjectionFactor> shared_ptr;
typedef boost::shared_ptr<GenericProjectionFactor<Cfg, LmK, PosK> > shared_ptr;
/**
* Default constructor
@ -125,6 +126,8 @@ namespace gtsam { namespace visualSLAM {
public:
typedef boost::shared_ptr<Graph> shared_graph;
/** default constructor is empty graph */
Graph() {
}

701
tests/testNonlinearConstraint.cpp
View File

@ -13,27 +13,26 @@
#include <CppUnitLite/TestHarness.h>
//#define GTSAM_MAGIC_KEY
//
//#include <Point2.h>
//#include <Pose3.h>
//#include <GaussianFactorGraph.h>
//#include <NonlinearOptimizer.h>
//#include <simulated2D.h>
//#include <Ordering.h>
//#include <visualSLAM.h>
//
//// templated implementations
//#include <NonlinearFactorGraph-inl.h>
//#include <NonlinearConstraint-inl.h>
//#include <NonlinearOptimizer-inl.h>
//#include <TupleConfig-inl.h>
//
//using namespace std;
//using namespace gtsam;
//using namespace boost;
//using namespace boost::assign;
//
#define GTSAM_MAGIC_KEY
#include <Point2.h>
#include <Pose3.h>
#include <GaussianFactorGraph.h>
#include <NonlinearOptimizer.h>
#include <simulated2D.h>
#include <Ordering.h>
#include <visualSLAM.h>
// templated implementations
#include <NonlinearFactorGraph-inl.h>
#include <NonlinearOptimizer-inl.h>
#include <TupleConfig-inl.h>
using namespace std;
using namespace gtsam;
using namespace boost;
using namespace boost::assign;
//// Models to use
//SharedDiagonal probModel1 = sharedSigma(1,1.0);
//SharedDiagonal probModel2 = sharedSigma(2,1.0);
@ -49,665 +48,7 @@
//typedef NonlinearEquality<Config2D, simulated2D::PoseKey, Point2> NLE;
//typedef boost::shared_ptr<simulated2D::GenericMeasurement<Config2D> > shared;
//typedef NonlinearOptimizer<Graph2D, Config2D> Optimizer;
//
///*
// * Determining a ground truth linear system
// * with two poses seeing one landmark, with each pose
// * constrained to a particular value
// */
//TEST (NonlinearConstraint, two_pose_truth ) {
// // create a graph
// shared_ptr<Graph2D> 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<NLE> 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<Config2D>(z1, sigma, x1,l1));
// graph->push_back(f1);
//
// // measurement from x2 to l1
// Point2 z2(-4.0, 0.0);
// shared f2(new simulated2D::GenericMeasurement<Config2D>(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<Config2D> initialEstimate(new Config2D);
// initialEstimate->insert(l1, pt_l1);
// initialEstimate->insert(x1, pt_x1);
// initialEstimate->insert(x2, pt_x2);
//
// // optimize the graph
// boost::shared_ptr<const Config2D> actual = Optimizer::optimizeGN(graph, initialEstimate);
//
// // verify
// Config2D expected;
// expected.insert(x1, pt_x1);
// expected.insert(x2, pt_x2);
// expected.insert(l1, Point2(1.0, 6.0));
// CHECK(assert_equal(expected, *actual, 1e-5));
//}
//
///* ********************************************************************* */
//namespace constrained_test1 {
//
// // binary constraint between landmarks
// /** g(x) = x-y = 0 */
// Vector g(const Config2D& config, const list<simulated2D::PointKey>& 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<simulated2D::PointKey>& keys) {
// return eye(2);
// }
//
// /** jacobian at l2 */
// Matrix G2(const Config2D& config, const list<simulated2D::PointKey>& keys) {
// return -1 * eye(2);
// }
//
//} // \namespace constrained_test1
//
//namespace constrained_test2 {
//
// // Unary Constraint on x1
// /** g(x) = x -[1;1] = 0 */
// Vector g(const Config2D& config, const list<simulated2D::PoseKey>& 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<simulated2D::PoseKey>& keys) {
// return eye(2);
// }
//
//} // \namespace constrained_test2
//
//
//typedef NonlinearConstraint2<
// Config2D, simulated2D::PointKey, Point2, simulated2D::PointKey, Point2> NLC2;
//
///* *********************************************************************
// * Version that actually uses nonlinear equality constraints
// * to to perform optimization. Same as above, but no
// * equality constraint on x2, and two landmarks that
// * should be the same. Note that this is a linear system,
// * so it will converge in one step.
// */
//TEST (NonlinearConstraint, two_pose ) {
// bool verbose = false;
//
// // create the graph
// shared_ptr<Graph2D> 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<NLE> 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<Config2D>(z1, sigma, x1,l1));
// graph->push_back(f1);
//
// // measurement from x2 to l2
// Point2 z2(-4.0, 0.0);
// shared f2(new simulated2D::GenericMeasurement<Config2D>(z2, sigma, x2,l2));
// graph->push_back(f2);
//
// // equality constraint between l1 and l2
// list<simulated2D::PointKey> keys2; keys2 += l1, l2;
// boost::shared_ptr<NLC2 > c2(new NLC2(
// boost::bind(constrained_test1::g, _1, keys2),
// l1, boost::bind(constrained_test1::G1, _1, keys2),
// l2, boost::bind(constrained_test1::G2, _1, keys2),
// 2));
// graph->push_back(c2);
//
// if (verbose) graph->print("Initial nonlinear graph with constraints");
//
// // create an initial estimate
// shared_ptr<Config2D> initialEstimate(new Config2D);
// initialEstimate->insert(x1, pt_x1);
// initialEstimate->insert(x2, Point2());
// initialEstimate->insert(l1, Point2(1.0, 6.0)); // ground truth
// initialEstimate->insert(l2, Point2(-4.0, 0.0)); // starting with a separate reference frame
//
// // create state config variables and initialize them
// Config2D state(*initialEstimate);
//
// // linearize the graph
// boost::shared_ptr<GaussianFactorGraph> fg = graph->linearize(state);
//
// if (verbose) fg->print("Linearized graph");
//
// // create an ordering
// Ordering ordering;
// ordering += "x1", "x2", "l1", "l2";
//
// // optimize linear graph to get full delta config
// GaussianBayesNet cbn = fg->eliminate(ordering);
// if (verbose) cbn.print("ChordalBayesNet");
//
// VectorConfig delta = optimize(cbn); //fg.optimize(ordering);
// if (verbose) delta.print("Delta Config");
//
// // update both state variables
// state = expmap(state, delta);
// if (verbose) state.print("newState");
//
// // verify
// Config2D expected;
// expected.insert(x1, pt_x1);
// expected.insert(l1, Point2(1.0, 6.0));
// expected.insert(l2, Point2(1.0, 6.0));
// expected.insert(x2, Point2(5.0, 6.0));
// CHECK(assert_equal(expected, state, 1e-5));
//}
//
///* ********************************************************************* */
//// VSLAM Examples
///* ********************************************************************* */
//// make a realistic calibration matrix
//double fov = 60; // degrees
//size_t w=640,h=480;
//Cal3_S2 K(fov,w,h);
//boost::shared_ptr<Cal3_S2> shK(new Cal3_S2(K));
//
//using namespace gtsam::visualSLAM;
//using namespace boost;
//
//// typedefs for visual SLAM example
//typedef TypedSymbol<Pose3, 'x'> Pose3Key;
//typedef TypedSymbol<Point3, 'l'> Point3Key;
////typedef TupleConfig3<LieConfig<LagrangeKey, Vector>,
//// LieConfig<Pose3Key, Pose3>,
//// LieConfig<Point3Key, Point3> > VConfig;
//typedef visualSLAM::Config VConfig;
//typedef NonlinearFactorGraph<VConfig> VGraph;
//typedef boost::shared_ptr<GenericProjectionFactor<VConfig> > shared_vf;
//typedef NonlinearOptimizer<VGraph,VConfig> VOptimizer;
//typedef NonlinearConstraint2<
// VConfig, visualSLAM::PointKey, Pose3, visualSLAM::PointKey, Pose3> VNLC2;
//typedef NonlinearEquality<VConfig, Pose3Key, Pose3> Pose3Constraint;
//
///**
// * Ground truth for a visual SLAM example with stereo vision
// */
//TEST (NonlinearConstraint, stereo_truth ) {
// bool verbose = false;
//
// // create initial estimates
// Rot3 faceDownY(Matrix_(3,3,
// 1.0, 0.0, 0.0,
// 0.0, 0.0, 1.0,
// 0.0, 1.0, 0.0));
// Pose3 pose1(faceDownY, Point3()); // origin, left camera
// SimpleCamera camera1(K, pose1);
// Pose3 pose2(faceDownY, Point3(2.0, 0.0, 0.0)); // 2 units to the left
// SimpleCamera camera2(K, pose2);
// Point3 landmark(1.0, 5.0, 0.0); //centered between the cameras, 5 units away
// Point3 landmarkNoisy(1.0, 6.0, 0.0);
//
// // create truth config
// boost::shared_ptr<VConfig> truthConfig(new VConfig);
// truthConfig->insert(Pose3Key(1), camera1.pose());
// truthConfig->insert(Pose3Key(2), camera2.pose());
// truthConfig->insert(Point3Key(1), landmark);
//
// // create graph
// shared_ptr<VGraph> graph(new VGraph());
//
// // create equality constraints for poses
// graph->push_back(shared_ptr<Pose3Constraint>(new Pose3Constraint(Pose3Key(1), camera1.pose())));
// graph->push_back(shared_ptr<Pose3Constraint>(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<VConfig>(
// 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<VConfig>(
// z2, vmodel, Pose3Key(2), Point3Key(1), shK));
// graph->push_back(vf2);
//
// if (verbose) graph->print("Graph after construction");
//
// // create ordering
// shared_ptr<Ordering> ord(new Ordering());
// *ord += "x1", "x2", "l1";
//
// // create optimizer
// VOptimizer::shared_solver solver(new VOptimizer::solver(ord));
// VOptimizer optimizer(graph, truthConfig, solver);
//
// // optimize
// VOptimizer afterOneIteration = optimizer.iterate();
//
// // verify
// DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
//
// // check if correct
// if (verbose) afterOneIteration.config()->print("After iteration");
// CHECK(assert_equal(*truthConfig,*(afterOneIteration.config())));
//}
//
//
///* *********************************************************************
// * Ground truth for a visual SLAM example with stereo vision
// * with some noise injected into the initial config
// */
//TEST (NonlinearConstraint, stereo_truth_noisy ) {
// bool verbose = false;
//
// // setting to determine how far away the noisy landmark is,
// // given that the ground truth is 5m in front of the cameras
// double noisyDist = 7.6;
//
// // create initial estimates
// Rot3 faceDownY(Matrix_(3,3,
// 1.0, 0.0, 0.0,
// 0.0, 0.0, 1.0,
// 0.0, 1.0, 0.0));
// Pose3 pose1(faceDownY, Point3()); // origin, left camera
// SimpleCamera camera1(K, pose1);
// Pose3 pose2(faceDownY, Point3(2.0, 0.0, 0.0)); // 2 units to the left
// SimpleCamera camera2(K, pose2);
// Point3 landmark(1.0, 5.0, 0.0); //centered between the cameras, 5 units away
// Point3 landmarkNoisy(1.0, noisyDist, 0.0); // initial point is too far out
//
// // create truth config
// boost::shared_ptr<VConfig> 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<VConfig> noisyConfig(new VConfig);
// noisyConfig->insert(Pose3Key(1), camera1.pose());
// noisyConfig->insert(Pose3Key(2), camera2.pose());
// noisyConfig->insert(Point3Key(1), landmarkNoisy);
//
// // create graph
// shared_ptr<VGraph> graph(new VGraph());
//
// // create equality constraints for poses
// graph->push_back(shared_ptr<Pose3Constraint>(new Pose3Constraint(Pose3Key(1), camera1.pose())));
// graph->push_back(shared_ptr<Pose3Constraint>(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<VConfig>(
// 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<VConfig>(
// 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<Ordering> ord(new Ordering());
// *ord += "x1", "x2", "l1";
//
// // create optimizer
// VOptimizer::shared_solver solver(new VOptimizer::solver(ord));
// VOptimizer optimizer0(graph, noisyConfig, solver);
//
// if (verbose)
// cout << "Initial Error: " << optimizer0.error() << endl;
//
// // use Levenberg-Marquardt optimization
// double relThresh = 1e-5, absThresh = 1e-5;
// VOptimizer optimizer(optimizer0.levenbergMarquardt(relThresh, absThresh, VOptimizer::SILENT));
//
// // verify
// DOUBLES_EQUAL(0.0, optimizer.error(), 1e-5);
//
// // check if correct
// if (verbose) {
// optimizer.config()->print("After iteration");
// cout << "Final error: " << optimizer.error() << endl;
// }
// CHECK(assert_equal(*truthConfig,*(optimizer.config()), 1e-5));
//}
//
///* ********************************************************************* */
//namespace constrained_stereo {
//
// // binary constraint between landmarks
// /** g(x) = x-y = 0 */
// Vector g(const VConfig& config, const list<Point3Key>& keys) {
// return config[keys.front()].vector()
// - config[keys.back()].vector();
// }
//
// /** jacobian at l1 */
// Matrix G1(const VConfig& config, const list<Point3Key>& keys) {
// return eye(3);
// }
//
// /** jacobian at l2 */
// Matrix G2(const VConfig& config, const list<Point3Key>& keys) {
// return -1.0 * eye(3);
// }
//
//} // \namespace constrained_stereo
//
///* ********************************************************************* */
//boost::shared_ptr<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
// shared_ptr<VGraph> graph(new VGraph);
//
// // create equality constraints for poses
// graph->push_back(shared_ptr<Pose3Constraint>(new Pose3Constraint(Pose3Key(1), camera1.pose())));
// graph->push_back(shared_ptr<Pose3Constraint>(new Pose3Constraint(Pose3Key(2), camera2.pose())));
//
// // create factors
// Point2 z1 = camera1.project(landmark1);
// SharedDiagonal vmodel = noiseModel::Unit::Create(3);
// shared_vf vf1(new GenericProjectionFactor<VConfig>(
// z1, vmodel, Pose3Key(1), Point3Key(1), shK));
// graph->push_back(vf1);
// Point2 z2 = camera2.project(landmark2);
// shared_vf vf2(new GenericProjectionFactor<VConfig>(
// 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<Point3Key> keys; keys += l1, l2;
// shared_ptr<VNLC2> c2(
// new VNLC2(boost::bind(constrained_stereo::g, _1, keys),
// l1, boost::bind(constrained_stereo::G1, _1, keys),
// l2, boost::bind(constrained_stereo::G2, _1, keys),
// 3));
// graph->push_back(c2);
//
// return graph;
//}
//
///* ********************************************************************* */
//boost::shared_ptr<VConfig> 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<VConfig> truthConfig(new VConfig);
// truthConfig->insert(Pose3Key(1), camera1.pose());
// truthConfig->insert(Pose3Key(2), camera2.pose());
// truthConfig->insert(Point3Key(1), landmark1);
// truthConfig->insert(Point3Key(2), landmark2); // create two landmarks in same place
// //truthConfig->insert(LagrangeKey(12), Vector_(3, 1.0, 1.0, 1.0));
//
// return truthConfig;
//}
//
///* *********************************************************************
// * SQP version of the above stereo example,
// * with the initial case as the ground truth
// */
//TEST (NonlinearConstraint, stereo_constrained ) {
// bool verbose = false;
//
// // get a graph
// boost::shared_ptr<VGraph> graph = stereoExampleGraph();
// if (verbose) graph->print("Graph after construction");
//
// // get the truth config
// boost::shared_ptr<VConfig> truthConfig = stereoExampleTruthConfig();
//
// // create ordering
// shared_ptr<Ordering> ord(new Ordering());
// *ord += "x1", "x2", "l1", "l2";
// VOptimizer::shared_solver solver(new VOptimizer::solver(ord));
//
// // create optimizer
// VOptimizer optimizer(graph, truthConfig, solver);
//
// // optimize
// VOptimizer afterOneIteration = optimizer.iterate();
//
// // check if correct
// CHECK(assert_equal(*truthConfig,*(afterOneIteration.config())));
//}
//
///* *********************************************************************
// * SQP version of the above stereo example,
// * with noise in the initial estimate
// */
//TEST (NonlinearConstraint, stereo_constrained_noisy ) {
//
// // get a graph
// boost::shared_ptr<VGraph> 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<VConfig> initConfig(new VConfig);
// initConfig->insert(Pose3Key(1), pose1);
// initConfig->insert(Pose3Key(2), pose2);
// initConfig->insert(Point3Key(1), landmark1);
// initConfig->insert(Point3Key(2), landmark2); // create two landmarks in same place
//
// // create ordering
// shared_ptr<Ordering> ord(new Ordering());
// *ord += "x1", "x2", "l1", "l2";
// VOptimizer::shared_solver solver(new VOptimizer::solver(ord));
//
// // create optimizer
// VOptimizer optimizer(graph, initConfig, solver);
//
// // optimize
// VOptimizer *pointer = new VOptimizer(optimizer);
// for (int i=0;i<1;i++) {
// VOptimizer* newOptimizer = new VOptimizer(pointer->iterateLM());
// delete pointer;
// pointer = newOptimizer;
// }
// VOptimizer::shared_config actual = pointer->config();
// delete(pointer);
//
// // get the truth config
// boost::shared_ptr<VConfig> truthConfig = stereoExampleTruthConfig();
//
// // check if correct
// CHECK(assert_equal(*truthConfig,*actual, 1e-5));
//}
//
//static SharedGaussian sigma(noiseModel::Isotropic::Sigma(1,0.1));
//
//namespace map_warp_example {
//typedef NonlinearConstraint1<
// Config2D, simulated2D::PoseKey, Point2> NLC1;
//} // \namespace map_warp_example
//
///* ********************************************************************* */
//// Example that moves two separate maps into the same frame of reference
//// Note that this is a linear example, so it should converge in one step
///* ********************************************************************* */
//
//namespace constrained_LinearMapWarp2 {
//// binary constraint between landmarks
///** g(x) = x-y = 0 */
//Vector g_func(const Config2D& config, const simulated2D::PointKey& key1, const simulated2D::PointKey& key2) {
// Point2 p = config[key1]-config[key2];
// return Vector_(2, p.x(), p.y());
//}
//
///** jacobian at l1 */
//Matrix jac_g1(const Config2D& config) {
// return eye(2);
//}
//
///** jacobian at l2 */
//Matrix jac_g2(const Config2D& config) {
// return -1*eye(2);
//}
//} // \namespace constrained_LinearMapWarp2
//
//namespace constrained_LinearMapWarp1 {
//// Unary Constraint on x1
///** g(x) = x -[1;1] = 0 */
//Vector g_func(const Config2D& config, const simulated2D::PoseKey& key) {
// Point2 p = config[key]-Point2(1.0, 1.0);
// return Vector_(2, p.x(), p.y());
//}
//
///** jacobian at x1 */
//Matrix jac_g(const Config2D& config) {
// return eye(2);
//}
//} // \namespace constrained_LinearMapWarp12
//
////typedef NonlinearOptimizer<NLGraph, Config2D> Optimizer;
//
///**
// * Creates the graph with each robot seeing the landmark, and it is
// * known that it is the same landmark
// */
//boost::shared_ptr<Graph2D> linearMapWarpGraph() {
// using namespace map_warp_example;
// // keys
// simulated2D::PoseKey x1(1), x2(2);
// simulated2D::PointKey l1(1), l2(2);
//
// // constant constraint on x1
// shared_ptr<NLC1> c1(new NLC1(boost::bind(constrained_LinearMapWarp1::g_func, _1, x1),
// x1, boost::bind(constrained_LinearMapWarp1::jac_g, _1),
// 2));
//
// // measurement from x1 to l1
// Point2 z1(0.0, 5.0);
// shared f1(new simulated2D::GenericMeasurement<Config2D>(z1, sigma, x1,l1));
//
// // measurement from x2 to l2
// Point2 z2(-4.0, 0.0);
// shared f2(new simulated2D::GenericMeasurement<Config2D>(z2, sigma, x2,l2));
//
// // equality constraint between l1 and l2
// shared_ptr<NLC2> c2 (new NLC2(
// boost::bind(constrained_LinearMapWarp2::g_func, _1, l1, l2),
// l1, boost::bind(constrained_LinearMapWarp2::jac_g1, _1),
// l2, boost::bind(constrained_LinearMapWarp2::jac_g2, _1),
// 2));
//
// // construct the graph
// boost::shared_ptr<Graph2D> graph(new Graph2D());
// graph->push_back(c1);
// graph->push_back(c2);
// graph->push_back(f1);
// graph->push_back(f2);
//
// return graph;
//}
//
///* ********************************************************************* */
//TEST ( SQPOptimizer, map_warp_initLam ) {
// // get a graph
// boost::shared_ptr<Graph2D> graph = linearMapWarpGraph();
//
// // keys
// simulated2D::PoseKey x1(1), x2(2);
// simulated2D::PointKey l1(1), l2(2);
//
// // create an initial estimate
// shared_ptr<Config2D> initialEstimate(new Config2D);
// initialEstimate->insert(x1, Point2(1.0, 1.0));
// initialEstimate->insert(l1, Point2(1.0, 6.0));
// initialEstimate->insert(l2, Point2(-4.0, 0.0)); // starting with a separate reference frame
// initialEstimate->insert(x2, Point2(0.0, 0.0)); // other pose starts at origin
//
// // create an ordering
// shared_ptr<Ordering> ordering(new Ordering());
// *ordering += "x1", "x2", "l1", "l2";
//
// // create an optimizer
// Optimizer::shared_solver solver(new Optimizer::solver(ordering));
// Optimizer optimizer(graph, initialEstimate, solver);
//
// // perform an iteration of optimization
// Optimizer oneIteration = optimizer.iterate(Optimizer::SILENT);
//
// // get the config back out and verify
// Config2D actual = *(oneIteration.config());
// Config2D expected;
// expected.insert(x1, Point2(1.0, 1.0));
// expected.insert(l1, Point2(1.0, 6.0));
// expected.insert(l2, Point2(1.0, 6.0));
// expected.insert(x2, Point2(5.0, 6.0));
// CHECK(assert_equal(expected, actual));
//}
//
///* ********************************************************************* */
//// This is an obstacle avoidance demo, where there is a trajectory of
//// three points, where there is a circular obstacle in the middle. There

163
tests/testNonlinearEqualityConstraint.cpp
View File

@ -6,13 +6,14 @@
#include <CppUnitLite/TestHarness.h>
#include <simulated2DConstraints.h>
#include <visualSLAM.h>
#include <NonlinearFactorGraph-inl.h>
#include <NonlinearOptimizer-inl.h>
using namespace std;
using namespace gtsam;
static const double tol = 1e-9;
static const double tol = 1e-5;
SharedDiagonal hard_model = noiseModel::Constrained::All(2);
SharedDiagonal soft_model = noiseModel::Isotropic::Sigma(2, 1.0);
@ -181,9 +182,167 @@ TEST( testNonlinearEqualityConstraint, odo_simple_optimize ) {
expected.insert(key1, truth_pt1);
expected.insert(key2, truth_pt2);
CHECK(assert_equal(expected, *actual, tol));
}
/* ********************************************************************* */
TEST (testNonlinearEqualityConstraint, two_pose ) {
/*
* Determining a ground truth linear system
* with two poses seeing one landmark, with each pose
* constrained to a particular value
*/
shared_graph graph(new Graph());
simulated2D::PoseKey x1(1), x2(2);
simulated2D::PointKey l1(1), l2(2);
Point2 pt_x1(1.0, 1.0),
pt_x2(5.0, 6.0);
graph->add(simulated2D::UnaryEqualityConstraint(pt_x1, x1));
graph->add(simulated2D::UnaryEqualityConstraint(pt_x2, x2));
Point2 z1(0.0, 5.0);
SharedGaussian sigma(noiseModel::Isotropic::Sigma(2, 0.1));
graph->add(simulated2D::Measurement(z1, sigma, x1,l1));
Point2 z2(-4.0, 0.0);
graph->add(simulated2D::Measurement(z2, sigma, x2,l2));
graph->add(simulated2D::PointEqualityConstraint(l1, l2));
shared_config initialEstimate(new simulated2D::Config());
initialEstimate->insert(x1, pt_x1);
initialEstimate->insert(x2, Point2());
initialEstimate->insert(l1, Point2(1.0, 6.0)); // ground truth
initialEstimate->insert(l2, Point2(-4.0, 0.0)); // starting with a separate reference frame
Optimizer::shared_config actual = Optimizer::optimizeLM(graph, initialEstimate);
simulated2D::Config expected;
expected.insert(x1, pt_x1);
expected.insert(l1, Point2(1.0, 6.0));
expected.insert(l2, Point2(1.0, 6.0));
expected.insert(x2, Point2(5.0, 6.0));
CHECK(assert_equal(expected, *actual, 1e-5));
}
/* ********************************************************************* */
TEST (testNonlinearEqualityConstraint, map_warp ) {
// get a graph
shared_graph graph(new Graph());
// keys
simulated2D::PoseKey x1(1), x2(2);
simulated2D::PointKey l1(1), l2(2);
// constant constraint on x1
Point2 pose1(1.0, 1.0);
graph->add(simulated2D::UnaryEqualityConstraint(pose1, x1));
SharedDiagonal sigma = noiseModel::Isotropic::Sigma(1,0.1);
// measurement from x1 to l1
Point2 z1(0.0, 5.0);
graph->add(simulated2D::Measurement(z1, sigma, x1, l1));
// measurement from x2 to l2
Point2 z2(-4.0, 0.0);
graph->add(simulated2D::Measurement(z2, sigma, x2, l2));
// equality constraint between l1 and l2
graph->add(simulated2D::PointEqualityConstraint(l1, l2));
// create an initial estimate
shared_config initialEstimate(new simulated2D::Config());
initialEstimate->insert(x1, Point2( 1.0, 1.0));
initialEstimate->insert(l1, Point2( 1.0, 6.0));
initialEstimate->insert(l2, Point2(-4.0, 0.0)); // starting with a separate reference frame
initialEstimate->insert(x2, Point2( 0.0, 0.0)); // other pose starts at origin
// optimize
Optimizer::shared_config actual = Optimizer::optimizeLM(graph, initialEstimate);
simulated2D::Config expected;
expected.insert(x1, Point2(1.0, 1.0));
expected.insert(l1, Point2(1.0, 6.0));
expected.insert(l2, Point2(1.0, 6.0));
expected.insert(x2, Point2(5.0, 6.0));
CHECK(assert_equal(expected, *actual, tol));
}
// make a realistic calibration matrix
double fov = 60; // degrees
size_t w=640,h=480;
Cal3_S2 K(fov,w,h);
boost::shared_ptr<Cal3_S2> shK(new Cal3_S2(K));
// typedefs for visual SLAM example
typedef visualSLAM::Config VConfig;
typedef boost::shared_ptr<VConfig> shared_vconfig;
typedef visualSLAM::Graph VGraph;
typedef NonlinearOptimizer<VGraph,VConfig> VOptimizer;
// factors for visual slam
typedef NonlinearEquality2<VConfig, visualSLAM::PointKey, Point3> Point3Equality;
/* ********************************************************************* */
TEST (testNonlinearEqualityConstraint, stereo_constrained ) {
// create initial estimates
Rot3 faceDownY(Matrix_(3,3,
1.0, 0.0, 0.0,
0.0, 0.0, 1.0,
0.0, 1.0, 0.0));
Pose3 pose1(faceDownY, Point3()); // origin, left camera
SimpleCamera camera1(K, pose1);
Pose3 pose2(faceDownY, Point3(2.0, 0.0, 0.0)); // 2 units to the left
SimpleCamera camera2(K, pose2);
Point3 landmark(1.0, 5.0, 0.0); //centered between the cameras, 5 units away
// keys
visualSLAM::PoseKey x1(1), x2(2);
visualSLAM::PointKey l1(1), l2(2);
// create graph
VGraph::shared_graph graph(new VGraph());
// create equality constraints for poses
graph->addPoseConstraint(1, camera1.pose());
graph->addPoseConstraint(2, camera2.pose());
// create factors
SharedDiagonal vmodel = noiseModel::Unit::Create(3);
graph->addMeasurement(camera1.project(landmark), vmodel, 1, 1, shK);
graph->addMeasurement(camera2.project(landmark), vmodel, 2, 2, shK);
// add equality constraint
graph->add(Point3Equality(l1, l2));
// create initial data
Point3 landmark1(0.5, 5.0, 0.0);
Point3 landmark2(1.5, 5.0, 0.0);
shared_vconfig initConfig(new VConfig());
initConfig->insert(x1, pose1);
initConfig->insert(x2, pose2);
initConfig->insert(l1, landmark1);
initConfig->insert(l2, landmark2);
// optimize
VOptimizer::shared_config actual = VOptimizer::optimizeLM(graph, initConfig);
// create config
VConfig truthConfig;
truthConfig.insert(x1, camera1.pose());
truthConfig.insert(x2, camera2.pose());
truthConfig.insert(l1, landmark);
truthConfig.insert(l2, landmark);
// check if correct
CHECK(assert_equal(truthConfig, *actual, 1e-5));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */