//*************************************************************************
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 (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(DoglegOptimizer, Iterate) {
// really non-linear factor graph
NonlinearFactorGraph fg = example::createReallyNonlinearFactorGraph();
// config far from minimum
Point2 x0(3,0);
Values config;
config.insert(X(1), x0);
double Delta = 1.0;
for(size_t it=0; it<10; ++it) {
GaussianBayesNet gbn = *fg.linearize(config)->eliminateSequential();
// Iterate assumes that linear error = nonlinear error at the linearization point, and this should be true
double nonlinearError = fg.error(config);
double linearError = GaussianFactorGraph(gbn).error(config.zeroVectors());
DOUBLES_EQUAL(nonlinearError, linearError, 1e-5);
// cout << "it " << it << ", Delta = " << Delta << ", error = " << fg->error(*config) << endl;
VectorValues dx_u = gbn.optimizeGradientSearch();
VectorValues dx_n = gbn.optimize();
DoglegOptimizerImpl::IterationResult result = DoglegOptimizerImpl::Iterate(Delta, DoglegOptimizerImpl::SEARCH_EACH_ITERATION, dx_u, dx_n, gbn, fg, config, fg.error(config));
Delta = result.Delta;
EXPECT(result.f_error < fg.error(config)); // Check that error decreases
Values newConfig(config.retract(result.dx_d));
config = newConfig;
DOUBLES_EQUAL(fg.error(config), result.f_error, 1e-5); // Check that error is correctly filled in
}
}
//*************************************************************************
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);
}
/* ************************************************************************** */
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));
}
/* ************************************************************************** */
TEST(JointLimitFactorPose2Vector, optimization_1) {
// zero point
// 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;
conf = Pose2Vector(Pose2(), Vector2(0, 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();
EXPECT_DOUBLES_EQUAL(0, graph.error(results), 1e-6);
EXPECT(assert_equal(conf, results.at<Pose2Vector>(qkey), 1e-6));
}
//*************************************************************************
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);
}
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;
}
/* ************************************************************************* */
int main (int argc, char* argv[]) {
// Find default file, but if an argument is given, try loading a file
string filename = findExampleDataFile("dubrovnik-3-7-pre");
if (argc>1) filename = string(argv[1]);
// Load the SfM data from file
SfM_data mydata;
assert(readBAL(filename, mydata));
cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.number_tracks() % mydata.number_cameras();
// Create a factor graph
NonlinearFactorGraph graph;
// We share *one* noiseModel between all projection factors
noiseModel::Isotropic::shared_ptr noise =
noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
// Add measurements to the factor graph
size_t j = 0;
BOOST_FOREACH(const SfM_Track& track, mydata.tracks) {
BOOST_FOREACH(const SfM_Measurement& m, track.measurements) {
size_t i = m.first;
Point2 uv = m.second;
graph.push_back(MyFactor(uv, noise, C(i), P(j))); // note use of shorthand symbols C and P
}
j += 1;
}
// Add a prior on pose x1. This indirectly specifies where the origin is.
// and a prior on the position of the first landmark to fix the scale
graph.push_back(PriorFactor<SfM_Camera>(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1)));
graph.push_back(PriorFactor<Point3> (P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1)));
// Create initial estimate
Values initial;
size_t i = 0; j = 0;
BOOST_FOREACH(const SfM_Camera& camera, mydata.cameras) initial.insert(C(i++), camera);
BOOST_FOREACH(const SfM_Track& track, mydata.tracks) initial.insert(P(j++), track.p);
/* Optimize the graph and print results */
Values result;
try {
LevenbergMarquardtParams params;
params.setVerbosity("ERROR");
LevenbergMarquardtOptimizer lm(graph, initial, params);
result = lm.optimize();
} catch (exception& e) {
cout << e.what();
}
cout << "final error: " << graph.error(result) << 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( SmartProjectionCameraFactor, comparisonGeneralSfMFactor ) {
using namespace bundler;
Values values;
values.insert(c1, level_camera);
values.insert(c2, level_camera_right);
NonlinearFactorGraph smartGraph;
SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
factor1->add(level_uv, c1);
factor1->add(level_uv_right, c2);
smartGraph.push_back(factor1);
double expectedError = factor1->error(values);
double expectedErrorGraph = smartGraph.error(values);
Point3 expectedPoint;
if (factor1->point())
expectedPoint = *(factor1->point());
// cout << "expectedPoint " << expectedPoint.vector() << endl;
// COMMENTS:
// 1) triangulation introduces small errors, then for a fair comparison we use expectedPoint as
// value in the generalGrap
NonlinearFactorGraph generalGraph;
SFMFactor sfm1(level_uv, unit2, c1, l1);
SFMFactor sfm2(level_uv_right, unit2, c2, l1);
generalGraph.push_back(sfm1);
generalGraph.push_back(sfm2);
values.insert(l1, expectedPoint); // note: we get rid of possible errors in the triangulation
Vector e1 = sfm1.evaluateError(values.at<Camera>(c1), values.at<Point3>(l1));
Vector e2 = sfm2.evaluateError(values.at<Camera>(c2), values.at<Point3>(l1));
double actualError = 0.5
* (norm_2(e1) * norm_2(e1) + norm_2(e2) * norm_2(e2));
double actualErrorGraph = generalGraph.error(values);
DOUBLES_EQUAL(expectedErrorGraph, actualErrorGraph, 1e-7);
DOUBLES_EQUAL(expectedErrorGraph, expectedError, 1e-7);
DOUBLES_EQUAL(actualErrorGraph, actualError, 1e-7);
DOUBLES_EQUAL(expectedError, actualError, 1e-7);
}
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
Pose2 prior(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.push_back(PriorFactor<Pose2>(1, prior, priorNoise));
// 2b. Add odometry factors
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
graph.push_back(BetweenFactor<Pose2>(1, 2, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
graph.push_back(BetweenFactor<Pose2>(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
graph.push_back(BetweenFactor<Pose2>(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
graph.push_back(BetweenFactor<Pose2>(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
// 2c. Add the loop closure constraint
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
graph.push_back(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
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;
// LM is still the outer optimization loop, but by specifying "Iterative" below
// We indicate that an iterative linear solver should be used.
// In addition, the *type* of the iterativeParams decides on the type of
// iterative solver, in this case the SPCG (subgraph PCG)
parameters.linearSolverType = NonlinearOptimizerParams::Iterative;
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 (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(PinholeCamera, BAL) {
string filename = findExampleDataFile("dubrovnik-3-7-pre");
SfM_data db;
bool success = readBAL(filename, db);
if (!success) throw runtime_error("Could not access file!");
SharedNoiseModel unit2 = noiseModel::Unit::Create(2);
NonlinearFactorGraph graph;
for (size_t j = 0; j < db.number_tracks(); j++) {
for (const SfM_Measurement& m: db.tracks[j].measurements)
graph.push_back(sfmFactor(m.second, unit2, m.first, P(j)));
}
Values initial = initialCamerasAndPointsEstimate(db);
LevenbergMarquardtOptimizer lm(graph, initial);
Values actual = lm.optimize();
double actualError = graph.error(actual);
EXPECT_DOUBLES_EQUAL(0.0199833, actualError, 1e-5);
}
/* *************************************************************************/
TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){
// make a realistic calibration matrix
double fov = 60; // degrees
size_t w=640,h=480;
Cal3_S2::shared_ptr K(new Cal3_S2(fov,w,h));
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
Pose3 cameraPose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); // body poses
Pose3 cameraPose2 = cameraPose1 * Pose3(Rot3(), Point3(1,0,0));
Pose3 cameraPose3 = cameraPose1 * Pose3(Rot3(), Point3(0,-1,0));
SimpleCamera cam1(cameraPose1, *K); // with camera poses
SimpleCamera cam2(cameraPose2, *K);
SimpleCamera cam3(cameraPose3, *K);
// create arbitrary body_Pose_sensor (transforms from sensor to body)
Pose3 sensor_to_body = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(1, 1, 1)); // Pose3(); //
// These are the poses we want to estimate, from camera measurements
Pose3 bodyPose1 = cameraPose1.compose(sensor_to_body.inverse());
Pose3 bodyPose2 = cameraPose2.compose(sensor_to_body.inverse());
Pose3 bodyPose3 = cameraPose3.compose(sensor_to_body.inverse());
// three landmarks ~5 meters infront of camera
Point3 landmark1(5, 0.5, 1.2);
Point3 landmark2(5, -0.5, 1.2);
Point3 landmark3(5, 0, 3.0);
vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;
// Project three landmarks into three cameras
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
// Create smart factors
std::vector<Key> views;
views.push_back(x1);
views.push_back(x2);
views.push_back(x3);
SmartProjectionParams params;
params.setRankTolerance(1.0);
params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY);
params.setEnableEPI(false);
SmartProjectionPoseFactor<Cal3_S2> smartFactor1(model, K, sensor_to_body, params);
smartFactor1.add(measurements_cam1, views);
SmartProjectionPoseFactor<Cal3_S2> smartFactor2(model, K, sensor_to_body, params);
smartFactor2.add(measurements_cam2, views);
SmartProjectionPoseFactor<Cal3_S2> smartFactor3(model, K, sensor_to_body, params);
smartFactor3.add(measurements_cam3, views);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
// Put all factors in factor graph, adding priors
NonlinearFactorGraph graph;
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Pose3>(x1, bodyPose1, noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, bodyPose2, noisePrior));
// Check errors at ground truth poses
Values gtValues;
gtValues.insert(x1, bodyPose1);
gtValues.insert(x2, bodyPose2);
gtValues.insert(x3, bodyPose3);
double actualError = graph.error(gtValues);
double expectedError = 0.0;
DOUBLES_EQUAL(expectedError, actualError, 1e-7)
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1));
Values values;
values.insert(x1, bodyPose1);
values.insert(x2, bodyPose2);
// initialize third pose with some noise, we expect it to move back to original pose3
values.insert(x3, bodyPose3*noise_pose);
LevenbergMarquardtParams lmParams;
Values result;
LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
result = optimizer.optimize();
EXPECT(assert_equal(bodyPose3,result.at<Pose3>(x3)));
}
/* ************************************************************************* */
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 measurementNoise =
noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
// Create the set of ground-truth landmarks and poses
vector<Point3> points = createPoints();
vector<Pose3> poses = createPoses();
// Create a factor graph
NonlinearFactorGraph graph;
// Simulated measurements from each camera pose, adding them to the factor graph
for (size_t j = 0; j < points.size(); ++j) {
// every landmark represent a single landmark, we use shared pointer to init the factor, and then insert measurements.
SmartFactor::shared_ptr smartfactor(new SmartFactor(measurementNoise, K));
for (size_t i = 0; i < poses.size(); ++i) {
// generate the 2D measurement
Camera camera(poses[i], K);
Point2 measurement = camera.project(points[j]);
// call add() function to add measurement into a single factor, here we need to add:
// 1. the 2D measurement
// 2. the corresponding camera's key
// 3. camera noise model
// 4. camera calibration
smartfactor->add(measurement, i);
}
// insert the smart factor in the graph
graph.push_back(smartfactor);
}
// Add a prior on pose x0. This indirectly specifies where the origin is.
// 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas(
(Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished());
graph.push_back(PriorFactor<Pose3>(0, poses[0], noise));
// Because the structure-from-motion problem has a scale ambiguity, the problem is
// still under-constrained. Here we add a prior on the second pose x1, so this will
// fix the scale by indicating the distance between x0 and x1.
// Because these two are fixed, the rest of the poses will be also be fixed.
graph.push_back(PriorFactor<Pose3>(1, poses[1], noise)); // add directly to graph
graph.print("Factor Graph:\n");
// Create the initial estimate to the solution
// Intentionally initialize the variables off from the ground truth
Values initialEstimate;
Pose3 delta(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
for (size_t i = 0; i < poses.size(); ++i)
initialEstimate.insert(i, poses[i].compose(delta));
initialEstimate.print("Initial Estimates:\n");
// Optimize the graph and print results
LevenbergMarquardtOptimizer optimizer(graph, initialEstimate);
Values result = optimizer.optimize();
result.print("Final results:\n");
// A smart factor represent the 3D point inside the factor, not as a variable.
// The 3D position of the landmark is not explicitly calculated by the optimizer.
// To obtain the landmark's 3D position, we use the "point" method of the smart factor.
Values landmark_result;
for (size_t j = 0; j < points.size(); ++j) {
// The graph stores Factor shared_ptrs, so we cast back to a SmartFactor first
SmartFactor::shared_ptr smart = boost::dynamic_pointer_cast<SmartFactor>(graph[j]);
if (smart) {
// The output of point() is in boost::optional<Point3>, as sometimes
// the triangulation operation inside smart factor will encounter degeneracy.
boost::optional<Point3> point = smart->point(result);
if (point) // ignore if boost::optional return NULL
landmark_result.insert(j, *point);
}
}
landmark_result.print("Landmark results:\n");
cout << "final error: " << graph.error(result) << endl;
cout << "number of iterations: " << optimizer.iterations() << endl;
return 0;
}
smartFactor3->add(measurements_cam3, views);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
NonlinearFactorGraph graph;
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Pose3>(x1, cam1.pose(), noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, cam2.pose(), noisePrior));
Values groundTruth;
groundTruth.insert(x1, cam1.pose());
groundTruth.insert(x2, cam2.pose());
groundTruth.insert(x3, cam3.pose());
DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9);
// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.1, 0.1, 0.1)); // smaller noise
Values values;
values.insert(x1, cam1.pose());
values.insert(x2, cam2.pose());
// initialize third pose with some noise, we expect it to move back to original pose_above
values.insert(x3, pose_above * noise_pose);
EXPECT(
assert_equal(
Pose3(
Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598,
-0.000986635786, 0.0314107591, -0.999013364, -0.0313952598),
Point3(0.1, -0.1, 1.9)), values.at<Pose3>(x3)));
/* ************************************************************************* */
int main (int argc, char* argv[]) {
// Find default file, but if an argument is given, try loading a file
string filename = findExampleDataFile("dubrovnik-3-7-pre");
if (argc>1) filename = string(argv[1]);
// Load the SfM data from file
SfM_data mydata;
readBAL(filename, mydata);
cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.number_tracks() % mydata.number_cameras();
// Create a factor graph
NonlinearFactorGraph graph;
// We share *one* noiseModel between all projection factors
noiseModel::Isotropic::shared_ptr noise =
noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
// Add measurements to the factor graph
size_t j = 0;
BOOST_FOREACH(const SfM_Track& track, mydata.tracks) {
BOOST_FOREACH(const SfM_Measurement& m, track.measurements) {
size_t i = m.first;
Point2 uv = m.second;
graph.push_back(MyFactor(uv, noise, C(i), P(j))); // note use of shorthand symbols C and P
}
j += 1;
}
// Add a prior on pose x1. This indirectly specifies where the origin is.
// and a prior on the position of the first landmark to fix the scale
graph.push_back(PriorFactor<SfM_Camera>(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1)));
graph.push_back(PriorFactor<Point3> (P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1)));
// Create initial estimate
Values initial;
size_t i = 0; j = 0;
BOOST_FOREACH(const SfM_Camera& camera, mydata.cameras) initial.insert(C(i++), camera);
BOOST_FOREACH(const SfM_Track& track, mydata.tracks) initial.insert(P(j++), track.p);
/** --------------- COMPARISON -----------------------**/
/** ----------------------------------------------------**/
LevenbergMarquardtParams params_using_COLAMD, params_using_METIS;
try {
params_using_METIS.setVerbosity("ERROR");
gttic_(METIS_ORDERING);
params_using_METIS.ordering = Ordering::Create(Ordering::METIS, graph);
gttoc_(METIS_ORDERING);
params_using_COLAMD.setVerbosity("ERROR");
gttic_(COLAMD_ORDERING);
params_using_COLAMD.ordering = Ordering::Create(Ordering::COLAMD, graph);
gttoc_(COLAMD_ORDERING);
} catch (exception& e) {
cout << e.what();
}
// expect they have different ordering results
if(params_using_COLAMD.ordering == params_using_METIS.ordering) {
cout << "COLAMD and METIS produce the same ordering. "
<< "Problem here!!!" << endl;
}
/* Optimize the graph with METIS and COLAMD and time the results */
Values result_METIS, result_COLAMD;
try {
gttic_(OPTIMIZE_WITH_METIS);
LevenbergMarquardtOptimizer lm_METIS(graph, initial, params_using_METIS);
result_METIS = lm_METIS.optimize();
gttoc_(OPTIMIZE_WITH_METIS);
gttic_(OPTIMIZE_WITH_COLAMD);
LevenbergMarquardtOptimizer lm_COLAMD(graph, initial, params_using_COLAMD);
result_COLAMD = lm_COLAMD.optimize();
gttoc_(OPTIMIZE_WITH_COLAMD);
} catch (exception& e) {
cout << e.what();
}
{ // printing the result
cout << "COLAMD final error: " << graph.error(result_COLAMD) << endl;
cout << "METIS final error: " << graph.error(result_METIS) << endl;
cout << endl << endl;
cout << "Time comparison by solving " << filename << " results:" << endl;
cout << boost::format("%1% point tracks and %2% cameras\n") \
% mydata.number_tracks() % mydata.number_cameras() \
<< endl;
tictoc_print_();
}
return 0;
}