/* ************************************************************************** */
TEST(JointLimitFactorPose2Vector, optimization_2) {
// over down limit
// settings
noiseModel::Gaussian::shared_ptr cost_model = noiseModel::Isotropic::Sigma(5, 0.001);
noiseModel::Gaussian::shared_ptr prior_model = noiseModel::Isotropic::Sigma(5, 1000);
Key qkey = Symbol('x', 0);
Vector5 dlimit = (Vector5() << 0, 0, 0, -5.0, -10.0).finished();
Vector5 ulimit = (Vector5() << 0, 0, 0, 5, 10.0).finished();
Vector5 thresh = (Vector5() << 0, 0, 0, 2.0, 2.0).finished();
Pose2Vector conf(Pose2(1, -2, 3), Vector2(-10.0, -10.0));
NonlinearFactorGraph graph;
graph.add(JointLimitFactorPose2Vector(qkey, cost_model, dlimit, ulimit, thresh));
graph.add(PriorFactor<Pose2Vector>(qkey, conf, prior_model));
Values init_values;
init_values.insert(qkey, conf);
GaussNewtonParams parameters;
parameters.setVerbosity("ERROR");
parameters.setAbsoluteErrorTol(1e-12);
GaussNewtonOptimizer optimizer(graph, init_values, parameters);
optimizer.optimize();
Values results = optimizer.values();
Vector conf_limit = (Vector(2) << -3.0, -8.0).finished();
EXPECT(assert_equal(conf_limit, results.at<Pose2Vector>(qkey).configuration(), 1e-6));
}
/* ************************************************************************* */
NonlinearFactorGraph planarSLAMGraph() {
NonlinearFactorGraph graph;
// Prior on pose x1 at the origin.
Pose2 prior(0.0, 0.0, 0.0);
auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.add(PriorFactor<Pose2>(x1, prior, priorNoise));
// Two odometry factors
Pose2 odometry(2.0, 0.0, 0.0);
auto odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
graph.add(BetweenFactor<Pose2>(x1, x2, odometry, odometryNoise));
graph.add(BetweenFactor<Pose2>(x2, x3, odometry, odometryNoise));
// Add Range-Bearing measurements to two different landmarks
auto measurementNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.2));
Rot2 bearing11 = Rot2::fromDegrees(45), bearing21 = Rot2::fromDegrees(90),
bearing32 = Rot2::fromDegrees(90);
double range11 = std::sqrt(4.0 + 4.0), range21 = 2.0, range32 = 2.0;
graph.add(BearingRangeFactor<Pose2, Point2>(x1, l1, bearing11, range11, measurementNoise));
graph.add(BearingRangeFactor<Pose2, Point2>(x2, l1, bearing21, range21, measurementNoise));
graph.add(BearingRangeFactor<Pose2, Point2>(x3, l2, bearing32, range32, measurementNoise));
return graph;
}
/* ************************************************************************* */
TEST( StereoFactor, singlePoint)
{
NonlinearFactorGraph graph;
graph.add(NonlinearEquality<Pose3>(X(1), camera1));
StereoPoint2 measurement(320, 320.0-50, 240);
// arguments: measurement, sigma, cam#, measurement #, K, baseline (m)
graph.add(GenericStereoFactor<Pose3, Point3>(measurement, model, X(1), L(1), K));
// Create a configuration corresponding to the ground truth
Values values;
values.insert(X(1), camera1); // add camera at z=6.25m looking towards origin
Point3 l1(0, 0, 0);
values.insert(L(1), l1); // add point at origin;
GaussNewtonOptimizer optimizer(graph, values);
// We expect the initial to be zero because config is the ground truth
DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
// Iterate once, and the config should not have changed
optimizer.iterate();
DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
// Complete solution
optimizer.optimize();
DOUBLES_EQUAL(0.0, optimizer.error(), 1e-6);
}
/* ************************************************************************* */
TEST ( NonlinearEquality, allow_error_optimize_with_factors ) {
// create a hard constraint
Symbol key1('x',1);
Pose2 feasible1(1.0, 2.0, 3.0);
// initialize away from the ideal
Values init;
Pose2 initPose(0.0, 2.0, 3.0);
init.insert(key1, initPose);
double error_gain = 500.0;
PoseNLE nle(key1, feasible1, error_gain);
// create a soft prior that conflicts
PosePrior prior(key1, initPose, noiseModel::Isotropic::Sigma(3, 0.1));
// add to a graph
NonlinearFactorGraph graph;
graph.add(nle);
graph.add(prior);
// optimize
Ordering ordering;
ordering.push_back(key1);
Values actual = LevenbergMarquardtOptimizer(graph, init, ordering).optimize();
// verify
Values expected;
expected.insert(key1, feasible1);
EXPECT(assert_equal(expected, actual));
}
int main(int argc, char** argv) {
// 1. Create a factor graph container and add factors to it
NonlinearFactorGraph graph;
// 2a. Add a prior on the first pose, setting it to the origin
// A prior factor consists of a mean and a noise model (covariance matrix)
Pose2 prior(0.0, 0.0, 0.0); // prior at origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1));
graph.add(PriorFactor<Pose2>(1, prior, priorNoise));
// 2b. Add odometry factors
// For simplicity, we will use the same noise model for each odometry factor
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1));
// Create odometry (Between) factors between consecutive poses
graph.add(BetweenFactor<Pose2>(1, 2, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
graph.add(BetweenFactor<Pose2>(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
graph.add(BetweenFactor<Pose2>(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
graph.add(BetweenFactor<Pose2>(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
// 2c. Add the loop closure constraint
// This factor encodes the fact that we have returned to the same pose. In real systems,
// these constraints may be identified in many ways, such as appearance-based techniques
// with camera images.
// We will use another Between Factor to enforce this constraint, with the distance set to zero,
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1));
graph.add(BetweenFactor<Pose2>(5, 1, Pose2(0.0, 0.0, 0.0), model));
graph.print("\nFactor Graph:\n"); // print
// 3. Create the data structure to hold the initialEstimate estimate to the solution
// For illustrative purposes, these have been deliberately set to incorrect values
Values initialEstimate;
initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2));
initialEstimate.insert(2, Pose2(2.3, 0.1, 1.1));
initialEstimate.insert(3, Pose2(2.1, 1.9, 2.8));
initialEstimate.insert(4, Pose2(-.3, 2.5, 4.2));
initialEstimate.insert(5, Pose2(0.1,-0.7, 5.8));
initialEstimate.print("\nInitial Estimate:\n"); // print
// 4. Single Step Optimization using Levenberg-Marquardt
LevenbergMarquardtParams parameters;
parameters.verbosity = NonlinearOptimizerParams::ERROR;
parameters.verbosityLM = LevenbergMarquardtParams::LAMBDA;
parameters.linearSolverType = SuccessiveLinearizationParams::CONJUGATE_GRADIENT;
{
parameters.iterativeParams = boost::make_shared<SubgraphSolverParameters>();
LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters);
Values result = optimizer.optimize();
result.print("Final Result:\n");
cout << "subgraph solver final error = " << graph.error(result) << endl;
}
return 0;
}
/* ************************************************************************* */
TEST( dataSet, readG2oTukey)
{
const string g2oFile = findExampleDataFile("pose2example");
NonlinearFactorGraph::shared_ptr actualGraph;
Values::shared_ptr actualValues;
bool is3D = false;
boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D, KernelFunctionTypeTUKEY);
noiseModel::Diagonal::shared_ptr baseModel = noiseModel::Diagonal::Precisions((Vector(3) << 44.721360, 44.721360, 30.901699));
SharedNoiseModel model = noiseModel::Robust::Create(noiseModel::mEstimator::Tukey::Create(4.6851), baseModel);
NonlinearFactorGraph expectedGraph;
expectedGraph.add(BetweenFactor<Pose2>(0, 1, Pose2(1.030390, 0.011350, -0.081596), model));
expectedGraph.add(BetweenFactor<Pose2>(1, 2, Pose2(1.013900, -0.058639, -0.220291), model));
expectedGraph.add(BetweenFactor<Pose2>(2, 3, Pose2(1.027650, -0.007456, -0.043627), model));
expectedGraph.add(BetweenFactor<Pose2>(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model));
expectedGraph.add(BetweenFactor<Pose2>(4, 5, Pose2(1.016030, 0.014565, -0.030930), model));
expectedGraph.add(BetweenFactor<Pose2>(5, 6, Pose2(1.023890, 0.006808, -0.007452), model));
expectedGraph.add(BetweenFactor<Pose2>(6, 7, Pose2(0.957734, 0.003159, 0.082836), model));
expectedGraph.add(BetweenFactor<Pose2>(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model));
expectedGraph.add(BetweenFactor<Pose2>(8, 9, Pose2(1.023440, 0.013984, -0.127624), model));
expectedGraph.add(BetweenFactor<Pose2>(9,10, Pose2(1.003350, 0.022250, -0.195918), model));
expectedGraph.add(BetweenFactor<Pose2>(5, 9, Pose2(0.033943, 0.032439, 3.073637), model));
expectedGraph.add(BetweenFactor<Pose2>(3,10, Pose2(0.044020, 0.988477, -1.553511), model));
EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5));
}
//*************************************************************************
TEST (EssentialMatrixFactor2, minimization) {
// Here we want to optimize for E and inverse depths at the same time
// We start with a factor graph and add constraints to it
// Noise sigma is 1cm, assuming metric measurements
NonlinearFactorGraph graph;
for (size_t i = 0; i < 5; i++)
graph.add(EssentialMatrixFactor2(100, i, pA(i), pB(i), model2));
// Check error at ground truth
Values truth;
truth.insert(100, trueE);
for (size_t i = 0; i < 5; i++) {
Point3 P1 = data.tracks[i].p;
truth.insert(i, double(baseline / P1.z()));
}
EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);
// Optimize
LevenbergMarquardtParams parameters;
// parameters.setVerbosity("ERROR");
LevenbergMarquardtOptimizer optimizer(graph, truth, parameters);
Values result = optimizer.optimize();
// Check result
EssentialMatrix actual = result.at<EssentialMatrix>(100);
EXPECT(assert_equal(trueE, actual, 1e-1));
for (size_t i = 0; i < 5; i++)
EXPECT_DOUBLES_EQUAL(truth.at<double>(i), result.at<double>(i), 1e-1);
// Check error at result
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
}
/* ************************************************************************** */
TEST(GaussianPriorWorkspacePoseArm, optimization) {
noiseModel::Gaussian::shared_ptr cost_model = noiseModel::Isotropic::Sigma(6, 0.1);
Vector a = (Vector(2) << 1, 1).finished();
Vector alpha = (Vector(2) << 0, 0).finished();
Vector d = (Vector(2) << 0, 0).finished();
ArmModel arm = ArmModel(Arm(2, a, alpha, d), BodySphereVector());
Pose3 des = Pose3(Rot3(), Point3(2, 0, 0));
Key qkey = Symbol('x', 0);
Vector q = (Vector(2) << 0, 0).finished();
Vector qinit = (Vector(2) << M_PI/2, M_PI/2).finished();
NonlinearFactorGraph graph;
graph.add(GaussianPriorWorkspacePoseArm(qkey, arm, 1, des, cost_model));
Values init_values;
init_values.insert(qkey, qinit);
LevenbergMarquardtParams parameters;
parameters.setVerbosity("ERROR");
parameters.setAbsoluteErrorTol(1e-12);
LevenbergMarquardtOptimizer optimizer(graph, init_values, parameters);
optimizer.optimize();
Values results = optimizer.values();
EXPECT_DOUBLES_EQUAL(0, graph.error(results), 1e-3);
EXPECT(assert_equal(q, results.at<Vector>(qkey), 1e-3));
}
/* ************************************************************************** */
TEST(GoalFactorArm, optimization_1) {
// use optimization to solve inverse kinematics
noiseModel::Gaussian::shared_ptr cost_model = noiseModel::Isotropic::Sigma(3, 0.1);
// 2 link simple example
Vector a = (Vector(2) << 1, 1).finished();
Vector alpha = (Vector(2) << 0, 0).finished();
Vector d = (Vector(2) << 0, 0).finished();
Arm arm(2, a, alpha, d);
Point3 goal(1.414213562373095, 1.414213562373095, 0);
Key qkey = Symbol('x', 0);
Vector q = (Vector(2) << M_PI/4.0, 0).finished();
Vector qinit = (Vector(2) << 0, 0).finished();
NonlinearFactorGraph graph;
graph.add(GoalFactorArm(qkey, cost_model, arm, goal));
Values init_values;
init_values.insert(qkey, qinit);
LevenbergMarquardtParams parameters;
parameters.setVerbosity("ERROR");
parameters.setAbsoluteErrorTol(1e-12);
LevenbergMarquardtOptimizer optimizer(graph, init_values, parameters);
optimizer.optimize();
Values results = optimizer.values();
EXPECT_DOUBLES_EQUAL(0, graph.error(results), 1e-3);
EXPECT(assert_equal(q, results.at<Vector>(qkey), 1e-3));
}
int main(const int argc, const char *argv[]) {
// Read graph from file
string g2oFile;
if (argc < 2)
g2oFile = findExampleDataFile("noisyToyGraph.txt");
else
g2oFile = argv[1];
NonlinearFactorGraph::shared_ptr graph;
Values::shared_ptr initial;
boost::tie(graph, initial) = readG2o(g2oFile);
// Add prior on the pose having index (key) = 0
NonlinearFactorGraph graphWithPrior = *graph;
noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8));
graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
graphWithPrior.print();
std::cout << "Computing LAGO estimate" << std::endl;
Values estimateLago = lago::initialize(graphWithPrior);
std::cout << "done!" << std::endl;
if (argc < 3) {
estimateLago.print("estimateLago");
} else {
const string outputFile = argv[2];
std::cout << "Writing results to file: " << outputFile << std::endl;
writeG2o(*graph, estimateLago, outputFile);
std::cout << "done! " << std::endl;
}
return 0;
}
//*************************************************************************
TEST (EssentialMatrixFactor2, extraMinimization) {
// Additional test with camera moving in positive X direction
// We start with a factor graph and add constraints to it
// Noise sigma is 1, assuming pixel measurements
NonlinearFactorGraph graph;
for (size_t i = 0; i < data.number_tracks(); i++)
graph.add(EssentialMatrixFactor2(100, i, pA(i), pB(i), model2, K));
// Check error at ground truth
Values truth;
truth.insert(100, trueE);
for (size_t i = 0; i < data.number_tracks(); i++) {
Point3 P1 = data.tracks[i].p;
truth.insert(i, double(baseline / P1.z()));
}
EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);
// Optimize
LevenbergMarquardtParams parameters;
// parameters.setVerbosity("ERROR");
LevenbergMarquardtOptimizer optimizer(graph, truth, parameters);
Values result = optimizer.optimize();
// Check result
EssentialMatrix actual = result.at<EssentialMatrix>(100);
EXPECT(assert_equal(trueE, actual, 1e-1));
for (size_t i = 0; i < data.number_tracks(); i++)
EXPECT_DOUBLES_EQUAL(truth.at<double>(i), result.at<double>(i), 1e-1);
// Check error at result
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
}
/* ************************************************************************* */
TEST ( NonlinearEquality, allow_error_optimize ) {
Symbol key1('x',1);
Pose2 feasible1(1.0, 2.0, 3.0);
double error_gain = 500.0;
PoseNLE nle(key1, feasible1, error_gain);
// add to a graph
NonlinearFactorGraph graph;
graph.add(nle);
// initialize away from the ideal
Pose2 initPose(0.0, 2.0, 3.0);
Values init;
init.insert(key1, initPose);
// optimize
Ordering ordering;
ordering.push_back(key1);
Values result = LevenbergMarquardtOptimizer(graph, init, ordering).optimize();
// verify
Values expected;
expected.insert(key1, feasible1);
EXPECT(assert_equal(expected, result));
}
//*************************************************************************
TEST (EssentialMatrixFactor3, minimization) {
// As before, we start with a factor graph and add constraints to it
NonlinearFactorGraph graph;
for (size_t i = 0; i < 5; i++)
// but now we specify the rotation bRc
graph.add(EssentialMatrixFactor3(100, i, pA(i), pB(i), cRb, model2));
// Check error at ground truth
Values truth;
truth.insert(100, bodyE);
for (size_t i = 0; i < 5; i++) {
Point3 P1 = data.tracks[i].p;
truth.insert(i, double(baseline / P1.z()));
}
EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);
// Optimize
LevenbergMarquardtParams parameters;
// parameters.setVerbosity("ERROR");
LevenbergMarquardtOptimizer optimizer(graph, truth, parameters);
Values result = optimizer.optimize();
// Check result
EssentialMatrix actual = result.at<EssentialMatrix>(100);
EXPECT(assert_equal(bodyE, actual, 1e-1));
for (size_t i = 0; i < 5; i++)
EXPECT_DOUBLES_EQUAL(truth.at<double>(i), result.at<double>(i), 1e-1);
// Check error at result
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
}
int main(int argc, char** argv) {
// 1. Create a factor graph container and add factors to it
NonlinearFactorGraph graph;
// 2a. Add odometry factors
// For simplicity, we will use the same noise model for each odometry factor
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
// Create odometry (Between) factors between consecutive poses
graph.add(BetweenFactor<Pose2>(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise));
graph.add(BetweenFactor<Pose2>(2, 3, Pose2(2.0, 0.0, 0.0), odometryNoise));
// 2b. Add "GPS-like" measurements
// We will use our custom UnaryFactor for this.
noiseModel::Diagonal::shared_ptr unaryNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1)); // 10cm std on x,y
graph.add(boost::make_shared<UnaryFactor>(1, 0.0, 0.0, unaryNoise));
graph.add(boost::make_shared<UnaryFactor>(2, 2.0, 0.0, unaryNoise));
graph.add(boost::make_shared<UnaryFactor>(3, 4.0, 0.0, unaryNoise));
graph.print("\nFactor Graph:\n"); // print
// 3. Create the data structure to hold the initialEstimate estimate to the solution
// For illustrative purposes, these have been deliberately set to incorrect values
Values initialEstimate;
initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2));
initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2));
initialEstimate.insert(3, Pose2(4.1, 0.1, 0.1));
initialEstimate.print("\nInitial Estimate:\n"); // print
// 4. Optimize using Levenberg-Marquardt optimization. The optimizer
// accepts an optional set of configuration parameters, controlling
// things like convergence criteria, the type of linear system solver
// to use, and the amount of information displayed during optimization.
// Here we will use the default set of parameters. See the
// documentation for the full set of parameters.
LevenbergMarquardtOptimizer optimizer(graph, initialEstimate);
Values result = optimizer.optimize();
result.print("Final Result:\n");
// 5. Calculate and print marginal covariances for all variables
Marginals marginals(graph, result);
cout << "x1 covariance:\n" << marginals.marginalCovariance(1) << endl;
cout << "x2 covariance:\n" << marginals.marginalCovariance(2) << endl;
cout << "x3 covariance:\n" << marginals.marginalCovariance(3) << endl;
return 0;
}
/* ********************************************************************* */
TEST (testNonlinearEqualityConstraint, map_warp ) {
// get a graph
NonlinearFactorGraph graph;
// keys
Symbol x1('x',1), x2('x',2);
Symbol l1('l',1), l2('l',2);
// constant constraint on x1
Point2 pose1(1.0, 1.0);
graph.add(eq2D::UnaryEqualityConstraint(pose1, x1));
SharedDiagonal sigma = noiseModel::Isotropic::Sigma(1,0.1);
// measurement from x1 to l1
Point2 z1(0.0, 5.0);
graph.add(simulated2D::Measurement(z1, sigma, x1, l1));
// measurement from x2 to l2
Point2 z2(-4.0, 0.0);
graph.add(simulated2D::Measurement(z2, sigma, x2, l2));
// equality constraint between l1 and l2
graph.add(eq2D::PointEqualityConstraint(l1, l2));
// create an initial estimate
Values initialEstimate;
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
Values actual = LevenbergMarquardtOptimizer(graph, initialEstimate).optimize();
Values expected;
expected.insert(x1, Point2(1.0, 1.0));
expected.insert(l1, Point2(1.0, 6.0));
expected.insert(l2, Point2(1.0, 6.0));
expected.insert(x2, Point2(5.0, 6.0));
CHECK(assert_equal(expected, actual, tol));
}
int main(int argc, char** argv) {
// Create an empty nonlinear factor graph
NonlinearFactorGraph graph;
// Add a prior on the first pose, setting it to the origin
// A prior factor consists of a mean and a noise model (covariance matrix)
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.add(PriorFactor<Pose2>(1, priorMean, priorNoise));
// Add odometry factors
Pose2 odometry(2.0, 0.0, 0.0);
// For simplicity, we will use the same noise model for each odometry factor
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
// Create odometry (Between) factors between consecutive poses
graph.add(BetweenFactor<Pose2>(1, 2, odometry, odometryNoise));
graph.add(BetweenFactor<Pose2>(2, 3, odometry, odometryNoise));
graph.print("\nFactor Graph:\n"); // print
// Create the data structure to hold the initialEstimate estimate to the solution
// For illustrative purposes, these have been deliberately set to incorrect values
Values initial;
initial.insert(1, Pose2(0.5, 0.0, 0.2));
initial.insert(2, Pose2(2.3, 0.1, -0.2));
initial.insert(3, Pose2(4.1, 0.1, 0.1));
initial.print("\nInitial Estimate:\n"); // print
// optimize using Levenberg-Marquardt optimization
Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();
result.print("Final Result:\n");
// Calculate and print marginal covariances for all variables
cout.precision(2);
Marginals marginals(graph, result);
cout << "x1 covariance:\n" << marginals.marginalCovariance(1) << endl;
cout << "x2 covariance:\n" << marginals.marginalCovariance(2) << endl;
cout << "x3 covariance:\n" << marginals.marginalCovariance(3) << endl;
return 0;
}
/* ************************************************************************* */
TEST(NonlinearOptimizer, MoreOptimization) {
NonlinearFactorGraph fg;
fg.add(PriorFactor<Pose2>(0, Pose2(0,0,0), noiseModel::Isotropic::Sigma(3,1)));
fg.add(BetweenFactor<Pose2>(0, 1, Pose2(1,0,M_PI/2), noiseModel::Isotropic::Sigma(3,1)));
fg.add(BetweenFactor<Pose2>(1, 2, Pose2(1,0,M_PI/2), noiseModel::Isotropic::Sigma(3,1)));
Values init;
init.insert(0, Pose2(3,4,-M_PI));
init.insert(1, Pose2(10,2,-M_PI));
init.insert(2, Pose2(11,7,-M_PI));
Values expected;
expected.insert(0, Pose2(0,0,0));
expected.insert(1, Pose2(1,0,M_PI/2));
expected.insert(2, Pose2(1,1,M_PI));
// Try LM and Dogleg
EXPECT(assert_equal(expected, LevenbergMarquardtOptimizer(fg, init).optimize()));
EXPECT(assert_equal(expected, DoglegOptimizer(fg, init).optimize()));
}
/* ************************************************************************* */
TEST( dataSet, readG2o3D)
{
const string g2oFile = findExampleDataFile("pose3example");
NonlinearFactorGraph::shared_ptr actualGraph;
Values::shared_ptr actualValues;
bool is3D = true;
boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D);
Values expectedValues;
Rot3 R0 = Rot3::quaternion(1.000000, 0.000000, 0.000000, 0.000000 );
Point3 p0 = Point3(0.000000, 0.000000, 0.000000);
expectedValues.insert(0, Pose3(R0, p0));
Rot3 R1 = Rot3::quaternion(0.854230, 0.190253, 0.283162, -0.392318 );
Point3 p1 = Point3(1.001367, 0.015390, 0.004948);
expectedValues.insert(1, Pose3(R1, p1));
Rot3 R2 = Rot3::quaternion(0.421446, -0.351729, -0.597838, 0.584174 );
Point3 p2 = Point3(1.993500, 0.023275, 0.003793);
expectedValues.insert(2, Pose3(R2, p2));
Rot3 R3 = Rot3::quaternion(0.067024, 0.331798, -0.200659, 0.919323);
Point3 p3 = Point3(2.004291, 1.024305, 0.018047);
expectedValues.insert(3, Pose3(R3, p3));
Rot3 R4 = Rot3::quaternion(0.765488, -0.035697, -0.462490, 0.445933);
Point3 p4 = Point3(0.999908, 1.055073, 0.020212);
expectedValues.insert(4, Pose3(R4, p4));
EXPECT(assert_equal(expectedValues,*actualValues,1e-5));
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions((Vector(6) << 10000.0,10000.0,10000.0,10000.0,10000.0,10000.0));
NonlinearFactorGraph expectedGraph;
Point3 p01 = Point3(1.001367, 0.015390, 0.004948);
Rot3 R01 = Rot3::quaternion(0.854230, 0.190253, 0.283162, -0.392318 );
expectedGraph.add(BetweenFactor<Pose3>(0, 1, Pose3(R01,p01), model));
Point3 p12 = Point3(0.523923, 0.776654, 0.326659);
Rot3 R12 = Rot3::quaternion(0.105373 , 0.311512, 0.656877, -0.678505 );
expectedGraph.add(BetweenFactor<Pose3>(1, 2, Pose3(R12,p12), model));
Point3 p23 = Point3(0.910927, 0.055169, -0.411761);
Rot3 R23 = Rot3::quaternion(0.568551 , 0.595795, -0.561677, 0.079353 );
expectedGraph.add(BetweenFactor<Pose3>(2, 3, Pose3(R23,p23), model));
Point3 p34 = Point3(0.775288, 0.228798, -0.596923);
Rot3 R34 = Rot3::quaternion(0.542221 , -0.592077, 0.303380, -0.513226 );
expectedGraph.add(BetweenFactor<Pose3>(3, 4, Pose3(R34,p34), model));
Point3 p14 = Point3(-0.577841, 0.628016, -0.543592);
Rot3 R14 = Rot3::quaternion(0.327419 , -0.125250, -0.534379, 0.769122 );
expectedGraph.add(BetweenFactor<Pose3>(1, 4, Pose3(R14,p14), model));
Point3 p30 = Point3(-0.623267, 0.086928, 0.773222);
Rot3 R30 = Rot3::quaternion(0.083672 , 0.104639, 0.627755, 0.766795 );
expectedGraph.add(BetweenFactor<Pose3>(3, 0, Pose3(R30,p30), model));
EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5));
}
/* ********************************************************************* */
TEST (testNonlinearEqualityConstraint, two_pose ) {
/*
* Determining a ground truth linear system
* with two poses seeing one landmark, with each pose
* constrained to a particular value
*/
NonlinearFactorGraph graph;
Symbol x1('x',1), x2('x',2);
Symbol l1('l',1), l2('l',2);
Point2 pt_x1(1.0, 1.0),
pt_x2(5.0, 6.0);
graph.add(eq2D::UnaryEqualityConstraint(pt_x1, x1));
graph.add(eq2D::UnaryEqualityConstraint(pt_x2, x2));
Point2 z1(0.0, 5.0);
SharedNoiseModel sigma(noiseModel::Isotropic::Sigma(2, 0.1));
graph.add(simulated2D::Measurement(z1, sigma, x1,l1));
Point2 z2(-4.0, 0.0);
graph.add(simulated2D::Measurement(z2, sigma, x2,l2));
graph.add(eq2D::PointEqualityConstraint(l1, l2));
Values initialEstimate;
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
Values actual = LevenbergMarquardtOptimizer(graph, initialEstimate).optimize();
Values expected;
expected.insert(x1, pt_x1);
expected.insert(l1, Point2(1.0, 6.0));
expected.insert(l2, Point2(1.0, 6.0));
expected.insert(x2, Point2(5.0, 6.0));
CHECK(assert_equal(expected, actual, 1e-5));
}
/* ************************************************************************* */
TEST( NonlinearOptimizer, Factorization )
{
Values config;
config.insert(X(1), Pose2(0.,0.,0.));
config.insert(X(2), Pose2(1.5,0.,0.));
NonlinearFactorGraph graph;
graph.add(PriorFactor<Pose2>(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10)));
graph.add(BetweenFactor<Pose2>(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1)));
Ordering ordering;
ordering.push_back(X(1));
ordering.push_back(X(2));
LevenbergMarquardtOptimizer optimizer(graph, config, ordering);
optimizer.iterate();
Values expected;
expected.insert(X(1), Pose2(0.,0.,0.));
expected.insert(X(2), Pose2(1.,0.,0.));
CHECK(assert_equal(expected, optimizer.values(), 1e-5));
}
//*************************************************************************
TEST (RotateFactor, minimization) {
// Let's try to recover the correct iRc by minimizing
NonlinearFactorGraph graph;
Model model = noiseModel::Isotropic::Sigma(3, 0.01);
graph.add(RotateFactor(1, i1Ri2, c1Zc2, model));
graph.add(RotateFactor(1, i2Ri3, c2Zc3, model));
graph.add(RotateFactor(1, i3Ri4, c3Zc4, model));
// Check error at ground truth
Values truth;
truth.insert(1, iRc);
EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);
// Check error at initial estimate
Values initial;
double degree = M_PI / 180;
Rot3 initialE = iRc.retract(degree * (Vector(3) << 20, -20, 20));
initial.insert(1, initialE);
#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
EXPECT_DOUBLES_EQUAL(3545.40, graph.error(initial), 1);
#else
EXPECT_DOUBLES_EQUAL(3349, graph.error(initial), 1);
#endif
// Optimize
LevenbergMarquardtParams parameters;
//parameters.setVerbosity("ERROR");
LevenbergMarquardtOptimizer optimizer(graph, initial, parameters);
Values result = optimizer.optimize();
// Check result
Rot3 actual = result.at<Rot3>(1);
EXPECT(assert_equal(iRc, actual,1e-1));
// Check error at result
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
}
/* ************************************************************************* */
TEST( inference, marginals2)
{
NonlinearFactorGraph fg;
SharedDiagonal poseModel(noiseModel::Isotropic::Sigma(3, 0.1));
SharedDiagonal pointModel(noiseModel::Isotropic::Sigma(2, 0.1));
fg.add(PriorFactor<Pose2>(X(0), Pose2(), poseModel));
fg.add(BetweenFactor<Pose2>(X(0), X(1), Pose2(1.0,0.0,0.0), poseModel));
fg.add(BetweenFactor<Pose2>(X(1), X(2), Pose2(1.0,0.0,0.0), poseModel));
fg.add(BearingRangeFactor<Pose2, Point2>(X(0), L(0), Rot2(), 1.0, pointModel));
fg.add(BearingRangeFactor<Pose2, Point2>(X(1), L(0), Rot2(), 1.0, pointModel));
fg.add(BearingRangeFactor<Pose2, Point2>(X(2), L(0), Rot2(), 1.0, pointModel));
Values init;
init.insert(X(0), Pose2(0.0,0.0,0.0));
init.insert(X(1), Pose2(1.0,0.0,0.0));
init.insert(X(2), Pose2(2.0,0.0,0.0));
init.insert(L(0), Point2(1.0,1.0));
Ordering ordering(*fg.orderingCOLAMD(init));
FactorGraph<GaussianFactor>::shared_ptr gfg(fg.linearize(init, ordering));
GaussianMultifrontalSolver solver(*gfg);
solver.marginalFactor(ordering[L(0)]);
}
//*************************************************************************
TEST (EssentialMatrixFactor, minimization) {
// Here we want to optimize directly on essential matrix constraints
// Yi Ma's algorithm (Ma01ijcv) is a bit cumbersome to implement,
// but GTSAM does the equivalent anyway, provided we give the right
// factors. In this case, the factors are the constraints.
// We start with a factor graph and add constraints to it
// Noise sigma is 1cm, assuming metric measurements
NonlinearFactorGraph graph;
for (size_t i = 0; i < 5; i++)
graph.add(EssentialMatrixFactor(1, pA(i), pB(i), model1));
// Check error at ground truth
Values truth;
truth.insert(1, trueE);
EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);
// Check error at initial estimate
Values initial;
EssentialMatrix initialE = trueE.retract(
(Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished());
initial.insert(1, initialE);
#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
EXPECT_DOUBLES_EQUAL(643.26, graph.error(initial), 1e-2);
#else
EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2);
#endif
// Optimize
LevenbergMarquardtParams parameters;
LevenbergMarquardtOptimizer optimizer(graph, initial, parameters);
Values result = optimizer.optimize();
// Check result
EssentialMatrix actual = result.at<EssentialMatrix>(1);
EXPECT(assert_equal(trueE, actual, 1e-1));
// Check error at result
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
// Check errors individually
for (size_t i = 0; i < 5; i++)
EXPECT_DOUBLES_EQUAL(0, actual.error(vA(i), vB(i)), 1e-6);
}
/* ************************************************************************* */
TEST( dataSet, readG2o)
{
const string g2oFile = findExampleDataFile("pose2example");
NonlinearFactorGraph::shared_ptr actualGraph;
Values::shared_ptr actualValues;
boost::tie(actualGraph, actualValues) = readG2o(g2oFile);
Values expectedValues;
expectedValues.insert(0, Pose2(0.000000, 0.000000, 0.000000));
expectedValues.insert(1, Pose2(1.030390, 0.011350, -0.081596));
expectedValues.insert(2, Pose2(2.036137, -0.129733, -0.301887));
expectedValues.insert(3, Pose2(3.015097, -0.442395, -0.345514));
expectedValues.insert(4, Pose2(3.343949, 0.506678, 1.214715));
expectedValues.insert(5, Pose2(3.684491, 1.464049, 1.183785));
expectedValues.insert(6, Pose2(4.064626, 2.414783, 1.176333));
expectedValues.insert(7, Pose2(4.429778, 3.300180, 1.259169));
expectedValues.insert(8, Pose2(4.128877, 2.321481, -1.825391));
expectedValues.insert(9, Pose2(3.884653, 1.327509, -1.953016));
expectedValues.insert(10, Pose2(3.531067, 0.388263, -2.148934));
EXPECT(assert_equal(expectedValues,*actualValues,1e-5));
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions((Vector(3) << 44.721360, 44.721360, 30.901699));
NonlinearFactorGraph expectedGraph;
expectedGraph.add(BetweenFactor<Pose2>(0, 1, Pose2(1.030390, 0.011350, -0.081596), model));
expectedGraph.add(BetweenFactor<Pose2>(1, 2, Pose2(1.013900, -0.058639, -0.220291), model));
expectedGraph.add(BetweenFactor<Pose2>(2, 3, Pose2(1.027650, -0.007456, -0.043627), model));
expectedGraph.add(BetweenFactor<Pose2>(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model));
expectedGraph.add(BetweenFactor<Pose2>(4, 5, Pose2(1.016030, 0.014565, -0.030930), model));
expectedGraph.add(BetweenFactor<Pose2>(5, 6, Pose2(1.023890, 0.006808, -0.007452), model));
expectedGraph.add(BetweenFactor<Pose2>(6, 7, Pose2(0.957734, 0.003159, 0.082836), model));
expectedGraph.add(BetweenFactor<Pose2>(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model));
expectedGraph.add(BetweenFactor<Pose2>(8, 9, Pose2(1.023440, 0.013984, -0.127624), model));
expectedGraph.add(BetweenFactor<Pose2>(9,10, Pose2(1.003350, 0.022250, -0.195918), model));
expectedGraph.add(BetweenFactor<Pose2>(5, 9, Pose2(0.033943, 0.032439, 3.073637), model));
expectedGraph.add(BetweenFactor<Pose2>(3,10, Pose2(0.044020, 0.988477, -1.553511), model));
EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5));
}
int main(const int argc, const char *argv[]) {
// Read graph from file
string g2oFile;
if (argc < 2)
g2oFile = findExampleDataFile("pose3example.txt");
else
g2oFile = argv[1];
NonlinearFactorGraph::shared_ptr graph;
Values::shared_ptr initial;
bool is3D = true;
boost::tie(graph, initial) = readG2o(g2oFile, is3D);
// Add prior on the first key
NonlinearFactorGraph graphWithPrior = *graph;
noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4));
Key firstKey = 0;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) {
std::cout << "Adding prior to g2o file " << std::endl;
firstKey = key_value.key;
graphWithPrior.add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));
break;
}
std::cout << "Initializing Pose3 - Riemannian gradient" << std::endl;
bool useGradient = true;
Values initialization = InitializePose3::initialize(graphWithPrior, *initial, useGradient);
std::cout << "done!" << std::endl;
std::cout << "initial error=" <<graph->error(*initial)<< std::endl;
std::cout << "initialization error=" <<graph->error(initialization)<< std::endl;
if (argc < 3) {
initialization.print("initialization");
} else {
const string outputFile = argv[2];
std::cout << "Writing results to file: " << outputFile << std::endl;
writeG2o(*graph, initialization, outputFile);
std::cout << "done! " << std::endl;
}
return 0;
}
//*************************************************************************
TEST (EssentialMatrixFactor, extraMinimization) {
// Additional test with camera moving in positive X direction
NonlinearFactorGraph graph;
for (size_t i = 0; i < 5; i++)
graph.add(EssentialMatrixFactor(1, pA(i), pB(i), model1, K));
// Check error at ground truth
Values truth;
truth.insert(1, trueE);
EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);
// Check error at initial estimate
Values initial;
EssentialMatrix initialE = trueE.retract(
(Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished());
initial.insert(1, initialE);
#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
EXPECT_DOUBLES_EQUAL(643.26, graph.error(initial), 1e-2);
#else
EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2);
#endif
// Optimize
LevenbergMarquardtParams parameters;
LevenbergMarquardtOptimizer optimizer(graph, initial, parameters);
Values result = optimizer.optimize();
// Check result
EssentialMatrix actual = result.at<EssentialMatrix>(1);
EXPECT(assert_equal(trueE, actual, 1e-1));
// Check error at result
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
// Check errors individually
for (size_t i = 0; i < 5; i++)
EXPECT_DOUBLES_EQUAL(0, actual.error(vA(i), vB(i)), 1e-6);
}
/* ************************************************************************* */
TEST( dataSet, readG2o3DNonDiagonalNoise)
{
const string g2oFile = findExampleDataFile("pose3example-offdiagonal.txt");
NonlinearFactorGraph::shared_ptr actualGraph;
Values::shared_ptr actualValues;
bool is3D = true;
boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D);
Values expectedValues;
Rot3 R0 = Rot3::quaternion(1.000000, 0.000000, 0.000000, 0.000000 );
Point3 p0 = Point3(0.000000, 0.000000, 0.000000);
expectedValues.insert(0, Pose3(R0, p0));
Rot3 R1 = Rot3::quaternion(0.854230, 0.190253, 0.283162, -0.392318 );
Point3 p1 = Point3(1.001367, 0.015390, 0.004948);
expectedValues.insert(1, Pose3(R1, p1));
EXPECT(assert_equal(expectedValues,*actualValues,1e-5));
Matrix Info = Matrix(6,6);
for (int i = 0; i < 6; i++){
for (int j = i; j < 6; j++){
if(i==j)
Info(i, j) = 10000;
else{
Info(i, j) = i+1; // arbitrary nonzero number
Info(j, i) = i+1;
}
}
}
noiseModel::Gaussian::shared_ptr model = noiseModel::Gaussian::Covariance(Info.inverse());
NonlinearFactorGraph expectedGraph;
Point3 p01 = Point3(1.001367, 0.015390, 0.004948);
Rot3 R01 = Rot3::quaternion(0.854230, 0.190253, 0.283162, -0.392318 );
expectedGraph.add(BetweenFactor<Pose3>(0, 1, Pose3(R01,p01), model));
EXPECT(assert_equal(expectedGraph,*actualGraph,1e-2));
}
/* ************************************************************************* */
int main(int argc, char* argv[]) {
// Define the camera calibration parameters
Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
// Define the camera observation noise model
noiseModel::Isotropic::shared_ptr noise = //
noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
// Create the set of ground-truth landmarks
vector<Point3> points = createPoints();
// Create the set of ground-truth poses
vector<Pose3> poses = createPoses();
// Create a NonlinearISAM object which will relinearize and reorder the variables
// every "relinearizeInterval" updates
int relinearizeInterval = 3;
NonlinearISAM isam(relinearizeInterval);
// Create a Factor Graph and Values to hold the new data
NonlinearFactorGraph graph;
Values initialEstimate;
// Loop over the different poses, adding the observations to iSAM incrementally
for (size_t i = 0; i < poses.size(); ++i) {
// Add factors for each landmark observation
for (size_t j = 0; j < points.size(); ++j) {
// Create ground truth measurement
SimpleCamera camera(poses[i], *K);
Point2 measurement = camera.project(points[j]);
// Add measurement
graph.add(
GenericProjectionFactor<Pose3, Point3, Cal3_S2>(measurement, noise,
Symbol('x', i), Symbol('l', j), K));
}
// Intentionally initialize the variables off from the ground truth
Pose3 noise(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
Pose3 initial_xi = poses[i].compose(noise);
// Add an initial guess for the current pose
initialEstimate.insert(Symbol('x', i), initial_xi);
// If this is the first iteration, add a prior on the first pose to set the coordinate frame
// and a prior on the first landmark to set the scale
// Also, as iSAM solves incrementally, we must wait until each is observed at least twice before
// adding it to iSAM.
if (i == 0) {
// Add a prior on pose x0, with 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas(
(Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished());
graph.add(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise));
// Add a prior on landmark l0
noiseModel::Isotropic::shared_ptr pointNoise =
noiseModel::Isotropic::Sigma(3, 0.1);
graph.add(PriorFactor<Point3>(Symbol('l', 0), points[0], pointNoise));
// Add initial guesses to all observed landmarks
Point3 noise(-0.25, 0.20, 0.15);
for (size_t j = 0; j < points.size(); ++j) {
// Intentionally initialize the variables off from the ground truth
Point3 initial_lj = points[j].compose(noise);
initialEstimate.insert(Symbol('l', j), initial_lj);
}
} else {
// Update iSAM with the new factors
isam.update(graph, initialEstimate);
Values currentEstimate = isam.estimate();
cout << "****************************************************" << endl;
cout << "Frame " << i << ": " << endl;
currentEstimate.print("Current estimate: ");
// Clear the factor graph and values for the next iteration
graph.resize(0);
initialEstimate.clear();
}
}
return 0;
}
int main() {
/**
* Step 1: Create a factor to express a unary constraint
* The "prior" in this case is the measurement from a sensor,
* with a model of the noise on the measurement.
*
* The "Key" created here is a label used to associate parts of the
* state (stored in "RotValues") with particular factors. They require
* an index to allow for lookup, and should be unique.
*
* In general, creating a factor requires:
* - A key or set of keys labeling the variables that are acted upon
* - A measurement value
* - A measurement model with the correct dimensionality for the factor
*/
Rot2 prior = Rot2::fromAngle(30 * degree);
prior.print("goal angle");
noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(1, 1 * degree);
Symbol key('x',1);
PriorFactor<Rot2> factor(key, prior, model);
/**
* Step 2: Create a graph container and add the factor to it
* Before optimizing, all factors need to be added to a Graph container,
* which provides the necessary top-level functionality for defining a
* system of constraints.
*
* In this case, there is only one factor, but in a practical scenario,
* many more factors would be added.
*/
NonlinearFactorGraph graph;
graph.add(factor);
graph.print("full graph");
/**
* Step 3: Create an initial estimate
* An initial estimate of the solution for the system is necessary to
* start optimization. This system state is the "RotValues" structure,
* which is similar in structure to a STL map, in that it maps
* keys (the label created in step 1) to specific values.
*
* The initial estimate provided to optimization will be used as
* a linearization point for optimization, so it is important that
* all of the variables in the graph have a corresponding value in
* this structure.
*
* The interface to all RotValues types is the same, it only depends
* on the type of key used to find the appropriate value map if there
* are multiple types of variables.
*/
Values initial;
initial.insert(key, Rot2::fromAngle(20 * degree));
initial.print("initial estimate");
/**
* Step 4: Optimize
* After formulating the problem with a graph of constraints
* and an initial estimate, executing optimization is as simple
* as calling a general optimization function with the graph and
* initial estimate. This will yield a new RotValues structure
* with the final state of the optimization.
*/
Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();
result.print("final result");
return 0;
}
int main(const int argc, const char *argv[]) {
// Read graph from file
string g2oFile;
if (argc < 2)
g2oFile = findExampleDataFile("pose3example.txt");
else
g2oFile = argv[1];
NonlinearFactorGraph::shared_ptr graph;
Values::shared_ptr initial;
bool is3D = true;
boost::tie(graph, initial) = readG2o(g2oFile, is3D);
bool add = false;
Key firstKey = 8646911284551352320;
std::cout << "Using reference key: " << firstKey << std::endl;
if(add)
std::cout << "adding key " << std::endl;
else
std::cout << "subtracting key " << std::endl;
if (argc < 3) {
std::cout << "Please provide output file to write " << std::endl;
} else {
const string inputFileRewritten = argv[2];
std::cout << "Rewriting input to file: " << inputFileRewritten << std::endl;
// Additional: rewrite input with simplified keys 0,1,...
Values simpleInitial;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) {
Key key;
if(add)
key = key_value.key + firstKey;
else
key = key_value.key - firstKey;
simpleInitial.insert(key, initial->at(key_value.key));
}
NonlinearFactorGraph simpleGraph;
BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, *graph) {
boost::shared_ptr<BetweenFactor<Pose3> > pose3Between =
boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
if (pose3Between){
Key key1, key2;
if(add){
key1 = pose3Between->key1() + firstKey;
key2 = pose3Between->key2() + firstKey;
}else{
key1 = pose3Between->key1() - firstKey;
key2 = pose3Between->key2() - firstKey;
}
NonlinearFactor::shared_ptr simpleFactor(
new BetweenFactor<Pose3>(key1, key2, pose3Between->measured(), pose3Between->get_noiseModel()));
simpleGraph.add(simpleFactor);
}
}
writeG2o(simpleGraph, simpleInitial, inputFileRewritten);
}
return 0;
}