/* ************************************************************************* */
TEST( testNonlinearEqualityConstraint, unary_simple_optimization ) {
// create a single-node graph with a soft and hard constraint to
// ensure that the hard constraint overrides the soft constraint
Point2 truth_pt(1.0, 2.0);
Symbol key('x',1);
double mu = 10.0;
eq2D::UnaryEqualityConstraint::shared_ptr constraint(
new eq2D::UnaryEqualityConstraint(truth_pt, key, mu));
Point2 badPt(100.0, -200.0);
simulated2D::Prior::shared_ptr factor(
new simulated2D::Prior(badPt, soft_model, key));
NonlinearFactorGraph graph;
graph.push_back(constraint);
graph.push_back(factor);
Values initValues;
initValues.insert(key, badPt);
// verify error values
EXPECT(constraint->active(initValues));
Values expected;
expected.insert(key, truth_pt);
EXPECT(constraint->active(expected));
EXPECT_DOUBLES_EQUAL(0.0, constraint->error(expected), tol);
Values actual = LevenbergMarquardtOptimizer(graph, initValues).optimize();
EXPECT(assert_equal(expected, actual, tol));
}
/*******************************************************************************
* Camera: f = 1, Image: 100x100, center: 50, 50.0
* Pose (ground truth): (Xw, -Yw, -Zw, [0,0,2.0]')
* Known landmarks:
* 3D Points: (10,10,0) (-10,10,0) (-10,-10,0) (10,-10,0)
* Perfect measurements:
* 2D Point: (55,45) (45,45) (45,55) (55,55)
*******************************************************************************/
int main(int argc, char* argv[]) {
/* read camera intrinsic parameters */
Cal3_S2::shared_ptr calib(new Cal3_S2(1, 1, 0, 50, 50));
/* 1. create graph */
NonlinearFactorGraph graph;
/* 2. add factors to the graph */
// add measurement factors
SharedDiagonal measurementNoise = Diagonal::Sigmas((Vector(2) << 0.5, 0.5));
boost::shared_ptr<ResectioningFactor> factor;
graph.push_back(
boost::make_shared<ResectioningFactor>(measurementNoise, X(1), calib,
Point2(55, 45), Point3(10, 10, 0)));
graph.push_back(
boost::make_shared<ResectioningFactor>(measurementNoise, X(1), calib,
Point2(45, 45), Point3(-10, 10, 0)));
graph.push_back(
boost::make_shared<ResectioningFactor>(measurementNoise, X(1), calib,
Point2(45, 55), Point3(-10, -10, 0)));
graph.push_back(
boost::make_shared<ResectioningFactor>(measurementNoise, X(1), calib,
Point2(55, 55), Point3(10, -10, 0)));
/* 3. Create an initial estimate for the camera pose */
Values initial;
initial.insert(X(1),
Pose3(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(0, 0, 2)));
/* 4. Optimize the graph using Levenberg-Marquardt*/
Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();
result.print("Final result:\n");
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;
}
/* ************************************************************************* */
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
vector<Point3> points = createPoints();
// Create the set of ground-truth poses
vector<Pose3> poses = createPoses();
// Create a factor graph
NonlinearFactorGraph graph;
// Add a prior on pose x1. This indirectly specifies where the origin is.
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
graph.push_back(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise)); // add directly to graph
// Simulated measurements from each camera pose, adding them to the factor graph
for (size_t i = 0; i < poses.size(); ++i) {
for (size_t j = 0; j < points.size(); ++j) {
SimpleCamera camera(poses[i], *K);
Point2 measurement = camera.project(points[j]);
graph.push_back(GenericProjectionFactor<Pose3, Point3, Cal3_S2>(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K));
}
}
// Because the structure-from-motion problem has a scale ambiguity, the problem is still under-constrained
// Here we add a prior on the position of the first landmark. This fixes the scale by indicating the distance
// between the first camera and the first landmark. All other landmark positions are interpreted using this scale.
noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1);
graph.push_back(PriorFactor<Point3>(Symbol('l', 0), points[0], pointNoise)); // add directly to graph
graph.print("Factor Graph:\n");
// Create the data structure to hold the initial estimate to the solution
// Intentionally initialize the variables off from the ground truth
Values initialEstimate;
for (size_t i = 0; i < poses.size(); ++i)
initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
for (size_t j = 0; j < points.size(); ++j)
initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15)));
initialEstimate.print("Initial Estimates:\n");
/* Optimize the graph and print results */
Values result = DoglegOptimizer(graph, initialEstimate).optimize();
result.print("Final results:\n");
return 0;
}
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;
}
/* ************************************************************************* */
int main(int argc, char* argv[]) {
// Create the set of ground-truth
vector<Point3> points = createPoints();
vector<Pose3> poses = createPoses();
// Create the factor graph
NonlinearFactorGraph graph;
// Add a prior on pose x1.
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
graph.push_back(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise));
// Simulated measurements from each camera pose, adding them to the factor graph
Cal3_S2 K(50.0, 50.0, 0.0, 50.0, 50.0);
noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0);
for (size_t i = 0; i < poses.size(); ++i) {
for (size_t j = 0; j < points.size(); ++j) {
SimpleCamera camera(poses[i], K);
Point2 measurement = camera.project(points[j]);
// The only real difference with the Visual SLAM example is that here we use a
// different factor type, that also calculates the Jacobian with respect to calibration
graph.push_back(GeneralSFMFactor2<Cal3_S2>(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), Symbol('K', 0)));
}
}
// Add a prior on the position of the first landmark.
noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1);
graph.push_back(PriorFactor<Point3>(Symbol('l', 0), points[0], pointNoise)); // add directly to graph
// Add a prior on the calibration.
noiseModel::Diagonal::shared_ptr calNoise = noiseModel::Diagonal::Sigmas((Vector(5) << 500, 500, 0.1, 100, 100).finished());
graph.push_back(PriorFactor<Cal3_S2>(Symbol('K', 0), K, calNoise));
// Create the initial estimate to the solution
// now including an estimate on the camera calibration parameters
Values initialEstimate;
initialEstimate.insert(Symbol('K', 0), Cal3_S2(60.0, 60.0, 0.0, 45.0, 45.0));
for (size_t i = 0; i < poses.size(); ++i)
initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
for (size_t j = 0; j < points.size(); ++j)
initialEstimate.insert<Point3>(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15));
/* Optimize the graph and print results */
Values result = DoglegOptimizer(graph, initialEstimate).optimize();
result.print("Final results:\n");
return 0;
}
/* ************************************************************************* */
TEST( ConcurrentIncrementalSmootherDL, synchronize_1 )
{
// Create a set of optimizer parameters
ISAM2Params parameters;
parameters.optimizationParams = ISAM2DoglegParams();
// parameters.maxIterations = 1;
// Create a Concurrent Batch Smoother
ConcurrentIncrementalSmoother smoother(parameters);
// Create a simple separator *from* the filter
NonlinearFactorGraph smootherFactors, filterSumarization;
Values smootherValues, filterSeparatorValues;
filterSeparatorValues.insert(1, Pose3().compose(poseError));
filterSeparatorValues.insert(2, filterSeparatorValues.at<Pose3>(1).compose(poseOdometry).compose(poseError));
filterSumarization.push_back(LinearContainerFactor(PriorFactor<Pose3>(1, poseInitial, noisePrior).linearize(filterSeparatorValues), filterSeparatorValues));
filterSumarization.push_back(LinearContainerFactor(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues), filterSeparatorValues));
// Create expected values: the smoother output will be empty for this case
NonlinearFactorGraph expectedSmootherSummarization;
Values expectedSmootherSeparatorValues;
NonlinearFactorGraph actualSmootherSummarization;
Values actualSmootherSeparatorValues;
smoother.presync();
smoother.getSummarizedFactors(actualSmootherSummarization, actualSmootherSeparatorValues);
smoother.synchronize(smootherFactors, smootherValues, filterSumarization, filterSeparatorValues);
smoother.postsync();
// Check
CHECK(assert_equal(expectedSmootherSummarization, actualSmootherSummarization, 1e-6));
CHECK(assert_equal(expectedSmootherSeparatorValues, actualSmootherSeparatorValues, 1e-6));
// Update the smoother
smoother.update();
// Check the factor graph. It should contain only the filter-provided factors
NonlinearFactorGraph expectedFactorGraph = filterSumarization;
NonlinearFactorGraph actualFactorGraph = smoother.getFactors();
CHECK(assert_equal(expectedFactorGraph, actualFactorGraph, 1e-6));
// Check the optimized value of smoother state
NonlinearFactorGraph allFactors;
allFactors.push_back(filterSumarization);
Values allValues;
allValues.insert(filterSeparatorValues);
Values expectedValues = BatchOptimize(allFactors, allValues,1);
Values actualValues = smoother.calculateEstimate();
CHECK(assert_equal(expectedValues, actualValues, 1e-6));
// Check the linearization point. The separator should remain identical to the filter provided values
Values expectedLinearizationPoint = filterSeparatorValues;
Values actualLinearizationPoint = smoother.getLinearizationPoint();
CHECK(assert_equal(expectedLinearizationPoint, actualLinearizationPoint, 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
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1));
graph.push_back(PriorFactor<Pose2>(1, Pose2(0, 0, 0), priorNoise));
// For simplicity, we will use the same noise model for odometry and loop closures
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1));
// 2b. Add odometry factors
graph.push_back(BetweenFactor<Pose2>(1, 2, Pose2(2, 0, 0 ), model));
graph.push_back(BetweenFactor<Pose2>(2, 3, Pose2(2, 0, M_PI_2), model));
graph.push_back(BetweenFactor<Pose2>(3, 4, Pose2(2, 0, M_PI_2), model));
graph.push_back(BetweenFactor<Pose2>(4, 5, Pose2(2, 0, M_PI_2), model));
// 2c. Add the loop closure constraint
graph.push_back(BetweenFactor<Pose2>(5, 2, Pose2(2, 0, M_PI_2), model));
// 3. Create the data structure to hold the initial 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, M_PI_2));
initial.insert(4, Pose2(4.0, 2.0, M_PI ));
initial.insert(5, Pose2(2.1, 2.1, -M_PI_2));
// Single Step Optimization using Levenberg-Marquardt
Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();
// save factor graph as graphviz dot file
// Render to PDF using "fdp Pose2SLAMExample.dot -Tpdf > graph.pdf"
ofstream os("Pose2SLAMExample.dot");
graph.saveGraph(os, result);
// Also print out to console
graph.saveGraph(cout, result);
return 0;
}
/* *************************************************************************/
TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor1 ) {
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);
Matrix expectedHessian = smartGraph.linearize(values)->hessian().first;
Vector expectedInfoVector = smartGraph.linearize(values)->hessian().second;
Point3 expectedPoint;
if (factor1->point())
expectedPoint = *(factor1->point());
// 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
Matrix actualFullHessian = generalGraph.linearize(values)->hessian().first;
Matrix actualFullInfoVector = generalGraph.linearize(values)->hessian().second;
Matrix actualHessian = actualFullHessian.block(0, 0, 18, 18)
- actualFullHessian.block(0, 18, 18, 3)
* (actualFullHessian.block(18, 18, 3, 3)).inverse()
* actualFullHessian.block(18, 0, 3, 18);
Vector actualInfoVector = actualFullInfoVector.block(0, 0, 18, 1)
- actualFullHessian.block(0, 18, 18, 3)
* (actualFullHessian.block(18, 18, 3, 3)).inverse()
* actualFullInfoVector.block(18, 0, 3, 1);
EXPECT(assert_equal(expectedHessian, actualHessian, 1e-7));
EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-7));
}
/* *************************************************************************/
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);
}
/* ************************************************************************* */
TEST( testNonlinearEqualityConstraint, odo_simple_optimize ) {
// create a two-node graph, connected by an odometry constraint, with
// a hard prior on one variable, and a conflicting soft prior
// on the other variable - the constraints should override the soft constraint
Point2 truth_pt1(1.0, 2.0), truth_pt2(3.0, 2.0);
Symbol key1('x',1), key2('x',2);
// hard prior on x1
eq2D::UnaryEqualityConstraint::shared_ptr constraint1(
new eq2D::UnaryEqualityConstraint(truth_pt1, key1));
// soft prior on x2
Point2 badPt(100.0, -200.0);
simulated2D::Prior::shared_ptr factor(
new simulated2D::Prior(badPt, soft_model, key2));
// odometry constraint
eq2D::OdoEqualityConstraint::shared_ptr constraint2(
new eq2D::OdoEqualityConstraint(
truth_pt1.between(truth_pt2), key1, key2));
NonlinearFactorGraph graph;
graph.push_back(constraint1);
graph.push_back(constraint2);
graph.push_back(factor);
Values initValues;
initValues.insert(key1, Point2());
initValues.insert(key2, badPt);
Values actual = LevenbergMarquardtOptimizer(graph, initValues).optimize();
Values expected;
expected.insert(key1, truth_pt1);
expected.insert(key2, truth_pt2);
CHECK(assert_equal(expected, actual, tol));
}
gtsam::NonlinearFactorGraph Matcher2D::findLocalLoopClosure(
const PoseD slam_pose, LaserScan2D& scan) {
NonlinearFactorGraph graph;
#if 0
// get looped index
vector<LoopResult2d> loop_result;
loop_result = this->findLoopClosure(scan);
// perform small EM only after init
if (local_smallEM_.flag_init) {
for (size_t i = 0; i < loop_result.size(); i++) {
Pose2 relpose = loop_result[i].delta_pose;
pair<size_t, size_t> relidx = make_pair(loop_result[i].loop_idx, pose_count_);
// inlier
if (pose_count_ - loop_result[i].loop_idx > setting_.local_loop_interval &&
local_smallEM_.perform(relpose, relidx, curr_values_, isam_.getFactorsUnsafe())) {
cout << "local loop detected! " << endl;
cout << "robot_" << ID_ << ": [" << loop_result[i].loop_idx << ", " << pose_count_ << "]" << endl;
cout << "Press Enter to continue ... " << endl;
cin.ignore(1);
// matched: insert between robot factor
graph.push_back(BetweenFactor<Pose2>(Symbol(ID_, loop_result[i].loop_idx), Symbol(ID_, pose_count_),
relpose, setting_.loop_default_model));
}
}
} else {
// only init small EM after certain count
if (local_measure_poses_.size() >= setting_.local_loop_count_smallEM) {
local_smallEM_.init(local_measure_poses_, local_measure_index_, Pose2());
local_measure_poses_.clear();
local_measure_index_.clear();
// insert in local cache
} else {
for (size_t i = 0; i < loop_result.size(); i++) {
local_measure_poses_.push_back(loop_result[i].delta_pose);
local_measure_index_.push_back(make_pair(loop_result[i].loop_idx, pose_count_));
}
}
}
#endif
return graph;
}
/* ************************************************************************* */
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( SmartProjectionCameraFactor, computeImplicitJacobian ) {
using namespace bundler;
Values values;
values.insert(c1, level_camera);
values.insert(c2, level_camera_right);
SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
factor1->add(level_uv, c1);
factor1->add(level_uv_right, c2);
Matrix expectedE;
Vector expectedb;
CameraSet<Camera> cameras;
cameras.push_back(level_camera);
cameras.push_back(level_camera_right);
factor1->error(values); // this is important to have a triangulation of the point
Point3 point;
if (factor1->point())
point = *(factor1->point());
vector<Matrix29> Fblocks;
factor1->computeJacobians(Fblocks, expectedE, expectedb, cameras, point);
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, point); // note: we get rid of possible errors in the triangulation
Matrix actualFullHessian = generalGraph.linearize(values)->hessian().first;
Matrix actualVinv = (actualFullHessian.block(18, 18, 3, 3)).inverse();
Matrix3 expectedVinv = factor1->PointCov(expectedE);
EXPECT(assert_equal(expectedVinv, actualVinv, 1e-7));
}
/* *************************************************************************/
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){
typedef SmartStereoProjectionPoseFactor SmartFactor;
bool output_poses = true;
string poseOutput("../../../examples/data/optimized_poses.txt");
string init_poseOutput("../../../examples/data/initial_poses.txt");
Values initial_estimate;
NonlinearFactorGraph graph;
const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,1);
ofstream pose3Out, init_pose3Out;
bool add_initial_noise = true;
string calibration_loc = findExampleDataFile("VO_calibration.txt");
string pose_loc = findExampleDataFile("VO_camera_poses_large.txt");
string factor_loc = findExampleDataFile("VO_stereo_factors_large.txt");
//read camera calibration info from file
// focal lengths fx, fy, skew s, principal point u0, v0, baseline b
cout << "Reading calibration info" << endl;
ifstream calibration_file(calibration_loc.c_str());
double fx, fy, s, u0, v0, b;
calibration_file >> fx >> fy >> s >> u0 >> v0 >> b;
const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fx, fy, s, u0, v0,b));
cout << "Reading camera poses" << endl;
ifstream pose_file(pose_loc.c_str());
int pose_id;
MatrixRowMajor m(4,4);
//read camera pose parameters and use to make initial estimates of camera poses
while (pose_file >> pose_id) {
for (int i = 0; i < 16; i++) {
pose_file >> m.data()[i];
}
if(add_initial_noise){
m(1,3) += (pose_id % 10)/10.0;
}
initial_estimate.insert(Symbol('x', pose_id), Pose3(m));
}
Values initial_pose_values = initial_estimate.filter<Pose3>();
if (output_poses) {
init_pose3Out.open(init_poseOutput.c_str(), ios::out);
for (size_t i = 1; i <= initial_pose_values.size(); i++) {
init_pose3Out
<< i << " "
<< initial_pose_values.at<Pose3>(Symbol('x', i)).matrix().format(
Eigen::IOFormat(Eigen::StreamPrecision, 0, " ", " ")) << endl;
}
}
// camera and landmark keys
size_t x, l;
// pixel coordinates uL, uR, v (same for left/right images due to rectification)
// landmark coordinates X, Y, Z in camera frame, resulting from triangulation
double uL, uR, v, X, Y, Z;
ifstream factor_file(factor_loc.c_str());
cout << "Reading stereo factors" << endl;
//read stereo measurements and construct smart factors
SmartFactor::shared_ptr factor(new SmartFactor(model));
size_t current_l = 3; // hardcoded landmark ID from first measurement
while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) {
if(current_l != l) {
graph.push_back(factor);
factor = SmartFactor::shared_ptr(new SmartFactor(model));
current_l = l;
}
factor->add(StereoPoint2(uL,uR,v), Symbol('x',x), K);
}
Pose3 first_pose = initial_estimate.at<Pose3>(Symbol('x',1));
//constrain the first pose such that it cannot change from its original value during optimization
// NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky
// QR is much slower than Cholesky, but numerically more stable
graph.push_back(NonlinearEquality<Pose3>(Symbol('x',1),first_pose));
LevenbergMarquardtParams params;
params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
params.verbosity = NonlinearOptimizerParams::ERROR;
cout << "Optimizing" << endl;
//create Levenberg-Marquardt optimizer to optimize the factor graph
LevenbergMarquardtOptimizer optimizer(graph, initial_estimate, params);
Values result = optimizer.optimize();
cout << "Final result sample:" << endl;
Values pose_values = result.filter<Pose3>();
pose_values.print("Final camera poses:\n");
if(output_poses){
pose3Out.open(poseOutput.c_str(),ios::out);
for(size_t i = 1; i<=pose_values.size(); i++){
pose3Out << i << " " << pose_values.at<Pose3>(Symbol('x',i)).matrix().format(Eigen::IOFormat(Eigen::StreamPrecision, 0,
" ", " ")) << endl;
}
cout << "Writing output" << endl;
}
return 0;
}
// main
int main (int argc, char** argv) {
// load Plaza2 data
list<TimedOdometry> odometry = readOdometry();
// size_t M = odometry.size();
vector<RangeTriple> triples = readTriples();
size_t K = triples.size();
// parameters
size_t minK = 150; // minimum number of range measurements to process initially
size_t incK = 25; // minimum number of range measurements to process after
bool groundTruth = false;
bool robust = true;
// Set Noise parameters
Vector priorSigmas = Vector3(1,1,M_PI);
Vector odoSigmas = Vector3(0.05, 0.01, 0.2);
double sigmaR = 100; // range standard deviation
const NM::Base::shared_ptr // all same type
priorNoise = NM::Diagonal::Sigmas(priorSigmas), //prior
odoNoise = NM::Diagonal::Sigmas(odoSigmas), // odometry
gaussian = NM::Isotropic::Sigma(1, sigmaR), // non-robust
tukey = NM::Robust::Create(NM::mEstimator::Tukey::Create(15), gaussian), //robust
rangeNoise = robust ? tukey : gaussian;
// Initialize iSAM
ISAM2 isam;
// Add prior on first pose
Pose2 pose0 = Pose2(-34.2086489999201, 45.3007639991120,
M_PI - 2.02108900000000);
NonlinearFactorGraph newFactors;
newFactors.push_back(PriorFactor<Pose2>(0, pose0, priorNoise));
Values initial;
initial.insert(0, pose0);
// initialize points
if (groundTruth) { // from TL file
initial.insert(symbol('L', 1), Point2(-68.9265, 18.3778));
initial.insert(symbol('L', 6), Point2(-37.5805, 69.2278));
initial.insert(symbol('L', 0), Point2(-33.6205, 26.9678));
initial.insert(symbol('L', 5), Point2(1.7095, -5.8122));
} else { // drawn from sigma=1 Gaussian in matlab version
initial.insert(symbol('L', 1), Point2(3.5784, 2.76944));
initial.insert(symbol('L', 6), Point2(-1.34989, 3.03492));
initial.insert(symbol('L', 0), Point2(0.725404, -0.0630549));
initial.insert(symbol('L', 5), Point2(0.714743, -0.204966));
}
// set some loop variables
size_t i = 1; // step counter
size_t k = 0; // range measurement counter
bool initialized = false;
Pose2 lastPose = pose0;
size_t countK = 0;
// Loop over odometry
gttic_(iSAM);
BOOST_FOREACH(const TimedOdometry& timedOdometry, odometry) {
//--------------------------------- odometry loop -----------------------------------------
double t;
Pose2 odometry;
boost::tie(t, odometry) = timedOdometry;
// add odometry factor
newFactors.push_back(BetweenFactor<Pose2>(i-1, i, odometry,NM::Diagonal::Sigmas(odoSigmas)));
// predict pose and add as initial estimate
Pose2 predictedPose = lastPose.compose(odometry);
lastPose = predictedPose;
initial.insert(i, predictedPose);
// Check if there are range factors to be added
while (k < K && t >= boost::get<0>(triples[k])) {
size_t j = boost::get<1>(triples[k]);
double range = boost::get<2>(triples[k]);
newFactors.push_back(RangeFactor<Pose2, Point2>(i, symbol('L', j), range,rangeNoise));
k = k + 1;
countK = countK + 1;
}
// Check whether to update iSAM 2
if ((k > minK) && (countK > incK)) {
if (!initialized) { // Do a full optimize for first minK ranges
gttic_(batchInitialization);
LevenbergMarquardtOptimizer batchOptimizer(newFactors, initial);
initial = batchOptimizer.optimize();
gttoc_(batchInitialization);
initialized = true;
}
gttic_(update);
isam.update(newFactors, initial);
gttoc_(update);
gttic_(calculateEstimate);
Values result = isam.calculateEstimate();
gttoc_(calculateEstimate);
lastPose = result.at<Pose2>(i);
newFactors = NonlinearFactorGraph();
initial = Values();
countK = 0;
}
i += 1;
//--------------------------------- odometry loop -----------------------------------------
} // BOOST_FOREACH
int main(int argc, char** argv) {
// Define the smoother lag (in seconds)
double lag = 2.0;
// Create a fixed lag smoother
// The Batch version uses Levenberg-Marquardt to perform the nonlinear optimization
BatchFixedLagSmoother smootherBatch(lag);
// The Incremental version uses iSAM2 to perform the nonlinear optimization
ISAM2Params parameters;
parameters.relinearizeThreshold = 0.0; // Set the relin threshold to zero such that the batch estimate is recovered
parameters.relinearizeSkip = 1; // Relinearize every time
IncrementalFixedLagSmoother smootherISAM2(lag, parameters);
// Create containers to store the factors and linearization points that
// will be sent to the smoothers
NonlinearFactorGraph newFactors;
Values newValues;
FixedLagSmoother::KeyTimestampMap newTimestamps;
// Create a prior on the first pose, placing it at the origin
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));
Key priorKey = 0;
newFactors.push_back(PriorFactor<Pose2>(priorKey, priorMean, priorNoise));
newValues.insert(priorKey, priorMean); // Initialize the first pose at the mean of the prior
newTimestamps[priorKey] = 0.0; // Set the timestamp associated with this key to 0.0 seconds;
// Now, loop through several time steps, creating factors from different "sensors"
// and adding them to the fixed-lag smoothers
double deltaT = 0.25;
for(double time = deltaT; time <= 3.0; time += deltaT) {
// Define the keys related to this timestamp
Key previousKey(1000 * (time-deltaT));
Key currentKey(1000 * (time));
// Assign the current key to the current timestamp
newTimestamps[currentKey] = time;
// Add a guess for this pose to the new values
// Since the robot moves forward at 2 m/s, then the position is simply: time[s]*2.0[m/s]
// {This is not a particularly good way to guess, but this is just an example}
Pose2 currentPose(time * 2.0, 0.0, 0.0);
newValues.insert(currentKey, currentPose);
// Add odometry factors from two different sources with different error stats
Pose2 odometryMeasurement1 = Pose2(0.61, -0.08, 0.02);
noiseModel::Diagonal::shared_ptr odometryNoise1 = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.05));
newFactors.push_back(BetweenFactor<Pose2>(previousKey, currentKey, odometryMeasurement1, odometryNoise1));
Pose2 odometryMeasurement2 = Pose2(0.47, 0.03, 0.01);
noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas(Vector3(0.05, 0.05, 0.05));
newFactors.push_back(BetweenFactor<Pose2>(previousKey, currentKey, odometryMeasurement2, odometryNoise2));
// Update the smoothers with the new factors
smootherBatch.update(newFactors, newValues, newTimestamps);
smootherISAM2.update(newFactors, newValues, newTimestamps);
for(size_t i = 1; i < 2; ++i) { // Optionally perform multiple iSAM2 iterations
smootherISAM2.update();
}
// Print the optimized current pose
cout << setprecision(5) << "Timestamp = " << time << endl;
smootherBatch.calculateEstimate<Pose2>(currentKey).print("Batch Estimate:");
smootherISAM2.calculateEstimate<Pose2>(currentKey).print("iSAM2 Estimate:");
cout << endl;
// Clear contains for the next iteration
newTimestamps.clear();
newValues.clear();
newFactors.resize(0);
}
// And to demonstrate the fixed-lag aspect, print the keys contained in each smoother after 3.0 seconds
cout << "After 3.0 seconds, " << endl;
cout << " Batch Smoother Keys: " << endl;
for(const FixedLagSmoother::KeyTimestampMap::value_type& key_timestamp: smootherBatch.timestamps()) {
cout << setprecision(5) << " Key: " << key_timestamp.first << " Time: " << key_timestamp.second << endl;
}
cout << " iSAM2 Smoother Keys: " << endl;
for(const FixedLagSmoother::KeyTimestampMap::value_type& key_timestamp: smootherISAM2.timestamps()) {
cout << setprecision(5) << " Key: " << key_timestamp.first << " Time: " << key_timestamp.second << endl;
}
return 0;
}
int main(int argc, char** argv){
Values initial_estimate;
NonlinearFactorGraph graph;
const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,1);
string calibration_loc = findExampleDataFile("VO_calibration.txt");
string pose_loc = findExampleDataFile("VO_camera_poses_large.txt");
string factor_loc = findExampleDataFile("VO_stereo_factors_large.txt");
//read camera calibration info from file
// focal lengths fx, fy, skew s, principal point u0, v0, baseline b
double fx, fy, s, u0, v0, b;
ifstream calibration_file(calibration_loc.c_str());
cout << "Reading calibration info" << endl;
calibration_file >> fx >> fy >> s >> u0 >> v0 >> b;
//create stereo camera calibration object
const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fx,fy,s,u0,v0,b));
ifstream pose_file(pose_loc.c_str());
cout << "Reading camera poses" << endl;
int pose_id;
MatrixRowMajor m(4,4);
//read camera pose parameters and use to make initial estimates of camera poses
while (pose_file >> pose_id) {
for (int i = 0; i < 16; i++) {
pose_file >> m.data()[i];
}
initial_estimate.insert(Symbol('x', pose_id), Pose3(m));
}
// camera and landmark keys
size_t x, l;
// pixel coordinates uL, uR, v (same for left/right images due to rectification)
// landmark coordinates X, Y, Z in camera frame, resulting from triangulation
double uL, uR, v, X, Y, Z;
ifstream factor_file(factor_loc.c_str());
cout << "Reading stereo factors" << endl;
//read stereo measurement details from file and use to create and add GenericStereoFactor objects to the graph representation
while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) {
graph.push_back(
GenericStereoFactor<Pose3, Point3>(StereoPoint2(uL, uR, v), model,
Symbol('x', x), Symbol('l', l), K));
//if the landmark variable included in this factor has not yet been added to the initial variable value estimate, add it
if (!initial_estimate.exists(Symbol('l', l))) {
Pose3 camPose = initial_estimate.at<Pose3>(Symbol('x', x));
//transform_from() transforms the input Point3 from the camera pose space, camPose, to the global space
Point3 worldPoint = camPose.transform_from(Point3(X, Y, Z));
initial_estimate.insert(Symbol('l', l), worldPoint);
}
}
Pose3 first_pose = initial_estimate.at<Pose3>(Symbol('x',1));
//constrain the first pose such that it cannot change from its original value during optimization
// NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky
// QR is much slower than Cholesky, but numerically more stable
graph.push_back(NonlinearEquality<Pose3>(Symbol('x',1),first_pose));
cout << "Optimizing" << endl;
//create Levenberg-Marquardt optimizer to optimize the factor graph
LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate);
Values result = optimizer.optimize();
cout << "Final result sample:" << endl;
Values pose_values = result.filter<Pose3>();
pose_values.print("Final camera poses:\n");
return 0;
}
/* ************************************************************************* */
TEST( IncrementalFixedLagSmoother, Example )
{
// Test the IncrementalFixedLagSmoother in a pure linear environment. Thus, full optimization and
// the IncrementalFixedLagSmoother should be identical (even with the linearized approximations at
// the end of the smoothing lag)
SETDEBUG("IncrementalFixedLagSmoother update", true);
// Set up parameters
SharedDiagonal odometerNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1));
SharedDiagonal loopNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1));
// Create a Fixed-Lag Smoother
typedef IncrementalFixedLagSmoother::KeyTimestampMap Timestamps;
IncrementalFixedLagSmoother smoother(7.0, ISAM2Params());
// Create containers to keep the full graph
Values fullinit;
NonlinearFactorGraph fullgraph;
// i keeps track of the time step
size_t i = 0;
// Add a prior at time 0 and update the HMF
{
Key key0 = MakeKey(0);
NonlinearFactorGraph newFactors;
Values newValues;
Timestamps newTimestamps;
newFactors.push_back(PriorFactor<Point2>(key0, Point2(0.0, 0.0), odometerNoise));
newValues.insert(key0, Point2(0.01, 0.01));
newTimestamps[key0] = 0.0;
fullgraph.push_back(newFactors);
fullinit.insert(newValues);
// Update the smoother
smoother.update(newFactors, newValues, newTimestamps);
// Check
CHECK(check_smoother(fullgraph, fullinit, smoother, key0));
++i;
}
// Add odometry from time 0 to time 5
while(i <= 5) {
Key key1 = MakeKey(i-1);
Key key2 = MakeKey(i);
NonlinearFactorGraph newFactors;
Values newValues;
Timestamps newTimestamps;
newFactors.push_back(BetweenFactor<Point2>(key1, key2, Point2(1.0, 0.0), odometerNoise));
newValues.insert(key2, Point2(double(i)+0.1, -0.1));
newTimestamps[key2] = double(i);
fullgraph.push_back(newFactors);
fullinit.insert(newValues);
// Update the smoother
smoother.update(newFactors, newValues, newTimestamps);
// Check
CHECK(check_smoother(fullgraph, fullinit, smoother, key2));
++i;
}
// Add odometry from time 5 to 6 to the HMF and a loop closure at time 5 to the TSM
{
// Add the odometry factor to the HMF
Key key1 = MakeKey(i-1);
Key key2 = MakeKey(i);
NonlinearFactorGraph newFactors;
Values newValues;
Timestamps newTimestamps;
newFactors.push_back(BetweenFactor<Point2>(key1, key2, Point2(1.0, 0.0), odometerNoise));
newFactors.push_back(BetweenFactor<Point2>(MakeKey(2), MakeKey(5), Point2(3.5, 0.0), loopNoise));
newValues.insert(key2, Point2(double(i)+0.1, -0.1));
newTimestamps[key2] = double(i);
fullgraph.push_back(newFactors);
fullinit.insert(newValues);
// Update the smoother
smoother.update(newFactors, newValues, newTimestamps);
// Check
CHECK(check_smoother(fullgraph, fullinit, smoother, key2));
++i;
}
// Add odometry from time 6 to time 15
while(i <= 15) {
Key key1 = MakeKey(i-1);
Key key2 = MakeKey(i);
NonlinearFactorGraph newFactors;
Values newValues;
Timestamps newTimestamps;
newFactors.push_back(BetweenFactor<Point2>(key1, key2, Point2(1.0, 0.0), odometerNoise));
newValues.insert(key2, Point2(double(i)+0.1, -0.1));
newTimestamps[key2] = double(i);
fullgraph.push_back(newFactors);
fullinit.insert(newValues);
// Update the smoother
smoother.update(newFactors, newValues, newTimestamps);
// Check
CHECK(check_smoother(fullgraph, fullinit, smoother, key2));
++i;
}
}
/* ************************************************************************* */
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;
}
/* ************************************************************************* */
TEST( ConcurrentIncrementalSmootherDL, synchronize_2 )
{
// Create a set of optimizer parameters
ISAM2Params parameters;
parameters.optimizationParams = ISAM2DoglegParams();
// parameters.maxIterations = 1;
// parameters.lambdaUpperBound = 1;
// parameters.lambdaInitial = 1;
// parameters.verbosity = NonlinearOptimizerParams::ERROR;
// parameters.verbosityLM = ISAM2Params::DAMPED;
// Create a Concurrent Batch Smoother
ConcurrentIncrementalSmoother smoother(parameters);
// Create a separator and cached smoother factors *from* the filter
NonlinearFactorGraph smootherFactors, filterSumarization;
Values smootherValues, filterSeparatorValues;
filterSeparatorValues.insert(1, Pose3().compose(poseError));
filterSeparatorValues.insert(2, filterSeparatorValues.at<Pose3>(1).compose(poseOdometry).compose(poseError));
filterSumarization.push_back(LinearContainerFactor(PriorFactor<Pose3>(1, poseInitial, noisePrior).linearize(filterSeparatorValues), filterSeparatorValues));
filterSumarization.push_back(LinearContainerFactor(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues), filterSeparatorValues));
smootherFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
smootherFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
smootherValues.insert(3, filterSeparatorValues.at<Pose3>(2).compose(poseOdometry).compose(poseError));
smootherValues.insert(4, smootherValues.at<Pose3>(3).compose(poseOdometry).compose(poseError));
// Create expected values: the smoother output will be empty for this case
NonlinearFactorGraph expectedSmootherSummarization;
Values expectedSmootherSeparatorValues;
NonlinearFactorGraph actualSmootherSummarization;
Values actualSmootherSeparatorValues;
smoother.presync();
smoother.getSummarizedFactors(actualSmootherSummarization, actualSmootherSeparatorValues);
smoother.synchronize(smootherFactors, smootherValues, filterSumarization, filterSeparatorValues);
smoother.postsync();
// Check
CHECK(assert_equal(expectedSmootherSummarization, actualSmootherSummarization, 1e-6));
CHECK(assert_equal(expectedSmootherSeparatorValues, actualSmootherSeparatorValues, 1e-6));
// Update the smoother
smoother.update();
// Check the factor graph. It should contain only the filter-provided factors
NonlinearFactorGraph expectedFactorGraph;
expectedFactorGraph.push_back(smootherFactors);
expectedFactorGraph.push_back(filterSumarization);
NonlinearFactorGraph actualFactorGraph = smoother.getFactors();
CHECK(assert_equal(expectedFactorGraph, actualFactorGraph, 1e-6));
// Check the optimized value of smoother state
NonlinearFactorGraph allFactors;
allFactors.push_back(filterSumarization);
allFactors.push_back(smootherFactors);
Values allValues;
allValues.insert(filterSeparatorValues);
allValues.insert(smootherValues);
// allValues.print("Batch LinPoint:\n");
Values expectedValues = BatchOptimize(allFactors, allValues, 1);
Values actualValues = smoother.calculateEstimate();
CHECK(assert_equal(expectedValues, actualValues, 1e-6));
// Check the linearization point. The separator should remain identical to the filter provided values, but the others should be the optimal values
// Isam2 is changing internally the linearization points, so the following check is done only on the separator variables
// Values expectedLinearizationPoint = BatchOptimize(allFactors, allValues, 1);
Values expectedLinearizationPoint = filterSeparatorValues;
Values actualLinearizationPoint;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, filterSeparatorValues) {
actualLinearizationPoint.insert(key_value.key, smoother.getLinearizationPoint().at(key_value.key));
}
int main(int argc, char** argv) {
// Define the smoother lag (in seconds)
double lag = 2.0;
// Create a Concurrent Filter and Smoother
ConcurrentBatchFilter concurrentFilter;
ConcurrentBatchSmoother concurrentSmoother;
// And a fixed lag smoother with a short lag
BatchFixedLagSmoother fixedlagSmoother(lag);
// And a fixed lag smoother with very long lag (i.e. a full batch smoother)
BatchFixedLagSmoother batchSmoother(1000.0);
// Create containers to store the factors and linearization points that
// will be sent to the smoothers
NonlinearFactorGraph newFactors;
Values newValues;
FixedLagSmoother::KeyTimestampMap newTimestamps;
// Create a prior on the first pose, placing it at the origin
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));
Key priorKey = 0;
newFactors.push_back(PriorFactor<Pose2>(priorKey, priorMean, priorNoise));
newValues.insert(priorKey, priorMean); // Initialize the first pose at the mean of the prior
newTimestamps[priorKey] = 0.0; // Set the timestamp associated with this key to 0.0 seconds;
// Now, loop through several time steps, creating factors from different "sensors"
// and adding them to the fixed-lag smoothers
double deltaT = 0.25;
for(double time = 0.0+deltaT; time <= 5.0; time += deltaT) {
// Define the keys related to this timestamp
Key previousKey(1000 * (time-deltaT));
Key currentKey(1000 * (time));
// Assign the current key to the current timestamp
newTimestamps[currentKey] = time;
// Add a guess for this pose to the new values
// Since the robot moves forward at 2 m/s, then the position is simply: time[s]*2.0[m/s]
// {This is not a particularly good way to guess, but this is just an example}
Pose2 currentPose(time * 2.0, 0.0, 0.0);
newValues.insert(currentKey, currentPose);
// Add odometry factors from two different sources with different error stats
Pose2 odometryMeasurement1 = Pose2(0.61, -0.08, 0.02);
noiseModel::Diagonal::shared_ptr odometryNoise1 = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.05));
newFactors.push_back(BetweenFactor<Pose2>(previousKey, currentKey, odometryMeasurement1, odometryNoise1));
Pose2 odometryMeasurement2 = Pose2(0.47, 0.03, 0.01);
noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas(Vector3(0.05, 0.05, 0.05));
newFactors.push_back(BetweenFactor<Pose2>(previousKey, currentKey, odometryMeasurement2, odometryNoise2));
// Unlike the fixed-lag versions, the concurrent filter implementation
// requires the user to supply the specify which keys to move to the smoother
FastList<Key> oldKeys;
if(time >= lag+deltaT) {
oldKeys.push_back(1000 * (time-lag-deltaT));
}
// Update the various inference engines
concurrentFilter.update(newFactors, newValues, oldKeys);
fixedlagSmoother.update(newFactors, newValues, newTimestamps);
batchSmoother.update(newFactors, newValues, newTimestamps);
// Manually synchronize the Concurrent Filter and Smoother every 1.0 s
if(fmod(time, 1.0) < 0.01) {
// Synchronize the Filter and Smoother
concurrentSmoother.update();
synchronize(concurrentFilter, concurrentSmoother);
}
// Print the optimized current pose
cout << setprecision(5) << "Timestamp = " << time << endl;
concurrentFilter.calculateEstimate<Pose2>(currentKey).print("Concurrent Estimate: ");
fixedlagSmoother.calculateEstimate<Pose2>(currentKey).print("Fixed Lag Estimate: ");
batchSmoother.calculateEstimate<Pose2>(currentKey).print("Batch Estimate: ");
cout << endl;
// Clear contains for the next iteration
newTimestamps.clear();
newValues.clear();
newFactors.resize(0);
}
cout << "******************************************************************" << endl;
cout << "All three versions should be identical." << endl;
cout << "Adding a loop closure factor to the Batch version only." << endl;
cout << "******************************************************************" << endl;
cout << endl;
// At the moment, all three versions produce the same output.
// Now lets create a "loop closure" factor between the first pose and the current pose
Key loopKey1(1000 * (0.0));
Key loopKey2(1000 * (5.0));
Pose2 loopMeasurement = Pose2(9.5, 1.00, 0.00);
noiseModel::Diagonal::shared_ptr loopNoise = noiseModel::Diagonal::Sigmas(Vector3(0.5, 0.5, 0.25));
NonlinearFactor::shared_ptr loopFactor(new BetweenFactor<Pose2>(loopKey1, loopKey2, loopMeasurement, loopNoise));
// This measurement cannot be added directly to the concurrent filter because it connects a filter state to a smoother state
// This measurement can never be added to the fixed-lag smoother, as one of the poses has been permanently marginalized out
// This measurement can be incorporated into the full batch version though
newFactors.push_back(loopFactor);
batchSmoother.update(newFactors, Values(), FixedLagSmoother::KeyTimestampMap());
newFactors.resize(0);
// Continue adding odometry factors until the loop closure may be incorporated into the concurrent smoother
for(double time = 5.0+deltaT; time <= 8.0; time += deltaT) {
// Define the keys related to this timestamp
Key previousKey(1000 * (time-deltaT));
Key currentKey(1000 * (time));
// Assign the current key to the current timestamp
newTimestamps[currentKey] = time;
// Add a guess for this pose to the new values
// Since the robot moves forward at 2 m/s, then the position is simply: time[s]*2.0[m/s]
// {This is not a particularly good way to guess, but this is just an example}
Pose2 currentPose(time * 2.0, 0.0, 0.0);
newValues.insert(currentKey, currentPose);
// Add odometry factors from two different sources with different error stats
Pose2 odometryMeasurement1 = Pose2(0.61, -0.08, 0.02);
noiseModel::Diagonal::shared_ptr odometryNoise1 = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.05));
newFactors.push_back(BetweenFactor<Pose2>(previousKey, currentKey, odometryMeasurement1, odometryNoise1));
Pose2 odometryMeasurement2 = Pose2(0.47, 0.03, 0.01);
noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas(Vector3(0.05, 0.05, 0.05));
newFactors.push_back(BetweenFactor<Pose2>(previousKey, currentKey, odometryMeasurement2, odometryNoise2));
// Unlike the fixed-lag versions, the concurrent filter implementation
// requires the user to supply the specify which keys to marginalize
FastList<Key> oldKeys;
if(time >= lag+deltaT) {
oldKeys.push_back(1000 * (time-lag-deltaT));
}
// Update the various inference engines
concurrentFilter.update(newFactors, newValues, oldKeys);
fixedlagSmoother.update(newFactors, newValues, newTimestamps);
batchSmoother.update(newFactors, newValues, newTimestamps);
// Manually synchronize the Concurrent Filter and Smoother every 1.0 s
if(fmod(time, 1.0) < 0.01) {
// Synchronize the Filter and Smoother
concurrentSmoother.update();
synchronize(concurrentFilter, concurrentSmoother);
}
// Print the optimized current pose
cout << setprecision(5) << "Timestamp = " << time << endl;
concurrentFilter.calculateEstimate<Pose2>(currentKey).print("Concurrent Estimate: ");
fixedlagSmoother.calculateEstimate<Pose2>(currentKey).print("Fixed Lag Estimate: ");
batchSmoother.calculateEstimate<Pose2>(currentKey).print("Batch Estimate: ");
cout << endl;
// Clear contains for the next iteration
newTimestamps.clear();
newValues.clear();
newFactors.resize(0);
}
cout << "******************************************************************" << endl;
cout << "The Concurrent system and the Fixed-Lag Smoother should be " << endl;
cout << "the same, but the Batch version has a loop closure." << endl;
cout << "Adding the loop closure factor to the Concurrent version." << endl;
cout << "This will not update the Concurrent Filter until the next " << endl;
cout << "synchronization, but the Concurrent solution should be identical " << endl;
cout << "to the Batch solution afterwards." << endl;
cout << "******************************************************************" << endl;
cout << endl;
// The state at 5.0s should have been transferred to the concurrent smoother at this point. Add the loop closure.
newFactors.push_back(loopFactor);
concurrentSmoother.update(newFactors, Values());
newFactors.resize(0);
// Now run for a few more seconds so the concurrent smoother and filter have to to re-sync
// Continue adding odometry factors until the loop closure may be incorporated into the concurrent smoother
for(double time = 8.0+deltaT; time <= 15.0; time += deltaT) {
// Define the keys related to this timestamp
Key previousKey(1000 * (time-deltaT));
Key currentKey(1000 * (time));
// Assign the current key to the current timestamp
newTimestamps[currentKey] = time;
// Add a guess for this pose to the new values
// Since the robot moves forward at 2 m/s, then the position is simply: time[s]*2.0[m/s]
// {This is not a particularly good way to guess, but this is just an example}
Pose2 currentPose(time * 2.0, 0.0, 0.0);
newValues.insert(currentKey, currentPose);
// Add odometry factors from two different sources with different error stats
Pose2 odometryMeasurement1 = Pose2(0.61, -0.08, 0.02);
noiseModel::Diagonal::shared_ptr odometryNoise1 = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.05));
newFactors.push_back(BetweenFactor<Pose2>(previousKey, currentKey, odometryMeasurement1, odometryNoise1));
Pose2 odometryMeasurement2 = Pose2(0.47, 0.03, 0.01);
noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas(Vector3(0.05, 0.05, 0.05));
newFactors.push_back(BetweenFactor<Pose2>(previousKey, currentKey, odometryMeasurement2, odometryNoise2));
// Unlike the fixed-lag versions, the concurrent filter implementation
// requires the user to supply the specify which keys to marginalize
FastList<Key> oldKeys;
if(time >= lag+deltaT) {
oldKeys.push_back(1000 * (time-lag-deltaT));
}
// Update the various inference engines
concurrentFilter.update(newFactors, newValues, oldKeys);
fixedlagSmoother.update(newFactors, newValues, newTimestamps);
batchSmoother.update(newFactors, newValues, newTimestamps);
// Manually synchronize the Concurrent Filter and Smoother every 1.0 s
if(fmod(time, 1.0) < 0.01) {
// Synchronize the Filter and Smoother
concurrentSmoother.update();
synchronize(concurrentFilter, concurrentSmoother);
cout << "******************************************************************" << endl;
cout << "Syncing Concurrent Filter and Smoother." << endl;
cout << "******************************************************************" << endl;
cout << endl;
}
// Print the optimized current pose
cout << setprecision(5) << "Timestamp = " << time << endl;
concurrentFilter.calculateEstimate<Pose2>(currentKey).print("Concurrent Estimate: ");
fixedlagSmoother.calculateEstimate<Pose2>(currentKey).print("Fixed Lag Estimate: ");
batchSmoother.calculateEstimate<Pose2>(currentKey).print("Batch Estimate: ");
cout << endl;
// Clear contains for the next iteration
newTimestamps.clear();
newValues.clear();
newFactors.resize(0);
}
// And to demonstrate the fixed-lag aspect, print the keys contained in each smoother after 3.0 seconds
cout << "After 15.0 seconds, each version contains to the following keys:" << endl;
cout << " Concurrent Filter Keys: " << endl;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, concurrentFilter.getLinearizationPoint()) {
cout << setprecision(5) << " Key: " << key_value.key << endl;
}
cout << " Concurrent Smoother Keys: " << endl;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, concurrentSmoother.getLinearizationPoint()) {
cout << setprecision(5) << " Key: " << key_value.key << endl;
}
cout << " Fixed-Lag Smoother Keys: " << endl;
BOOST_FOREACH(const FixedLagSmoother::KeyTimestampMap::value_type& key_timestamp, fixedlagSmoother.timestamps()) {
cout << setprecision(5) << " Key: " << key_timestamp.first << endl;
}
cout << " Batch Smoother Keys: " << endl;
BOOST_FOREACH(const FixedLagSmoother::KeyTimestampMap::value_type& key_timestamp, batchSmoother.timestamps()) {
cout << setprecision(5) << " Key: " << key_timestamp.first << endl;
}
return 0;
}
/* *************************************************************************/
TEST( SmartProjectionCameraFactor, Cal3Bundler2 ) {
using namespace bundler;
vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3,
measurements_cam4, measurements_cam5;
// 1. Project three landmarks into three cameras and triangulate
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4);
projectToMultipleCameras(cam1, cam2, cam3, landmark5, measurements_cam5);
vector<Key> views;
views.push_back(c1);
views.push_back(c2);
views.push_back(c3);
SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2));
smartFactor1->add(measurements_cam1, views);
SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2));
smartFactor2->add(measurements_cam2, views);
SmartFactor::shared_ptr smartFactor3(new SmartFactor(unit2));
smartFactor3->add(measurements_cam3, views);
SmartFactor::shared_ptr smartFactor4(new SmartFactor(unit2));
smartFactor4->add(measurements_cam4, views);
SmartFactor::shared_ptr smartFactor5(new SmartFactor(unit2));
smartFactor5->add(measurements_cam5, views);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(9, 1e-6);
NonlinearFactorGraph graph;
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Camera>(c1, cam1, noisePrior));
graph.push_back(PriorFactor<Camera>(c2, cam2, noisePrior));
Values values;
values.insert(c1, cam1);
values.insert(c2, cam2);
// initialize third pose with some noise, we expect it to move back to original pose3
values.insert(c3, perturbCameraPoseAndCalibration(cam3));
if (isDebugTest)
values.at<Camera>(c3).print("Smart: Pose3 before optimization: ");
LevenbergMarquardtParams lmParams;
lmParams.relativeErrorTol = 1e-8;
lmParams.absoluteErrorTol = 0;
lmParams.maxIterations = 20;
if (isDebugTest)
lmParams.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
if (isDebugTest)
lmParams.verbosity = NonlinearOptimizerParams::ERROR;
Values result;
LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
result = optimizer.optimize();
if (isDebugTest)
result.at<Camera>(c3).print("Smart: Pose3 after optimization: ");
EXPECT(assert_equal(cam1, result.at<Camera>(c1), 1e-4));
EXPECT(assert_equal(cam2, result.at<Camera>(c2), 1e-4));
EXPECT(assert_equal(result.at<Camera>(c3).pose(), cam3.pose(), 1e-1));
EXPECT(
assert_equal(result.at<Camera>(c3).calibration(), cam3.calibration(), 2));
if (isDebugTest)
tictoc_print_();
}
/* *************************************************************************/
TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ) {
using namespace vanilla;
// Project three landmarks into three cameras
vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
vector<Key> views;
views.push_back(c1);
views.push_back(c2);
views.push_back(c3);
SfM_Track track1;
for (size_t i = 0; i < 3; ++i) {
SfM_Measurement measures;
measures.first = i + 1; // cameras are from 1 to 3
measures.second = measurements_cam1.at(i);
track1.measurements.push_back(measures);
}
SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2));
smartFactor1->add(track1);
SfM_Track track2;
for (size_t i = 0; i < 3; ++i) {
SfM_Measurement measures;
measures.first = i + 1; // cameras are from 1 to 3
measures.second = measurements_cam2.at(i);
track2.measurements.push_back(measures);
}
SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2));
smartFactor2->add(track2);
SmartFactor::shared_ptr smartFactor3(new SmartFactor(unit2));
smartFactor3->add(measurements_cam3, views);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6 + 5, 1e-5);
NonlinearFactorGraph graph;
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Camera>(c1, cam1, noisePrior));
graph.push_back(PriorFactor<Camera>(c2, cam2, noisePrior));
Values values;
values.insert(c1, cam1);
values.insert(c2, cam2);
values.insert(c3, perturbCameraPose(cam3));
if (isDebugTest)
values.at<Camera>(c3).print("Smart: Pose3 before optimization: ");
LevenbergMarquardtParams lmParams;
if (isDebugTest)
lmParams.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
if (isDebugTest)
lmParams.verbosity = NonlinearOptimizerParams::ERROR;
Values result;
LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
result = optimizer.optimize();
// GaussianFactorGraph::shared_ptr GFG = graph.linearize(values);
// VectorValues delta = GFG->optimize();
if (isDebugTest)
result.at<Camera>(c3).print("Smart: Pose3 after optimization: ");
EXPECT(assert_equal(cam1, result.at<Camera>(c1)));
EXPECT(assert_equal(cam2, result.at<Camera>(c2)));
EXPECT(assert_equal(result.at<Camera>(c3).pose(), cam3.pose(), 1e-4));
EXPECT(
assert_equal(result.at<Camera>(c3).calibration(), cam3.calibration(), 2));
if (isDebugTest)
tictoc_print_();
}
/* ************************************************************************* */
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;
}
int main(int argc, char** argv){
typedef PinholePose<Cal3_S2> Camera;
typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor;
Values initial_estimate;
NonlinearFactorGraph graph;
const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(2,1);
string calibration_loc = findExampleDataFile("VO_calibration.txt");
string pose_loc = findExampleDataFile("VO_camera_poses_large.txt");
string factor_loc = findExampleDataFile("VO_stereo_factors_large.txt");
//read camera calibration info from file
// focal lengths fx, fy, skew s, principal point u0, v0, baseline b
cout << "Reading calibration info" << endl;
ifstream calibration_file(calibration_loc.c_str());
double fx, fy, s, u0, v0, b;
calibration_file >> fx >> fy >> s >> u0 >> v0 >> b;
const Cal3_S2::shared_ptr K(new Cal3_S2(fx, fy, s, u0, v0));
cout << "Reading camera poses" << endl;
ifstream pose_file(pose_loc.c_str());
int pose_index;
MatrixRowMajor m(4,4);
//read camera pose parameters and use to make initial estimates of camera poses
while (pose_file >> pose_index) {
for (int i = 0; i < 16; i++)
pose_file >> m.data()[i];
initial_estimate.insert(pose_index, Pose3(m));
}
// landmark keys
size_t landmark_key;
// pixel coordinates uL, uR, v (same for left/right images due to rectification)
// landmark coordinates X, Y, Z in camera frame, resulting from triangulation
double uL, uR, v, X, Y, Z;
ifstream factor_file(factor_loc.c_str());
cout << "Reading stereo factors" << endl;
//read stereo measurements and construct smart factors
SmartFactor::shared_ptr factor(new SmartFactor(model, K));
size_t current_l = 3; // hardcoded landmark ID from first measurement
while (factor_file >> pose_index >> landmark_key >> uL >> uR >> v >> X >> Y >> Z) {
if(current_l != landmark_key) {
graph.push_back(factor);
factor = SmartFactor::shared_ptr(new SmartFactor(model, K));
current_l = landmark_key;
}
factor->add(Point2(uL,v), pose_index);
}
Pose3 firstPose = initial_estimate.at<Pose3>(1);
//constrain the first pose such that it cannot change from its original value during optimization
// NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky
// QR is much slower than Cholesky, but numerically more stable
graph.push_back(NonlinearEquality<Pose3>(1,firstPose));
LevenbergMarquardtParams params;
params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
params.verbosity = NonlinearOptimizerParams::ERROR;
cout << "Optimizing" << endl;
//create Levenberg-Marquardt optimizer to optimize the factor graph
LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate, params);
Values result = optimizer.optimize();
cout << "Final result sample:" << endl;
Values pose_values = result.filter<Pose3>();
pose_values.print("Final camera poses:\n");
return 0;
}
// main
int main(int argc, char** argv) {
// load Plaza1 data
list<TimedOdometry> odometry = readOdometry();
// size_t M = odometry.size();
vector<RangeTriple> triples = readTriples();
size_t K = triples.size();
// parameters
size_t start = 220, end=3000;
size_t minK = 100; // first batch of smart factors
size_t incK = 50; // minimum number of range measurements to process after
bool robust = true;
bool smart = true;
// Set Noise parameters
Vector priorSigmas = Vector3(1, 1, M_PI);
Vector odoSigmas = Vector3(0.05, 0.01, 0.2);
double sigmaR = 100; // range standard deviation
const NM::Base::shared_ptr // all same type
priorNoise = NM::Diagonal::Sigmas(priorSigmas), //prior
odoNoise = NM::Diagonal::Sigmas(odoSigmas), // odometry
gaussian = NM::Isotropic::Sigma(1, sigmaR), // non-robust
tukey = NM::Robust::Create(NM::mEstimator::Tukey::Create(15), gaussian), //robust
rangeNoise = robust ? tukey : gaussian;
// Initialize iSAM
ISAM2 isam;
// Add prior on first pose
Pose2 pose0 = Pose2(-34.2086489999201, 45.3007639991120,
M_PI - 2.02108900000000);
NonlinearFactorGraph newFactors;
newFactors.push_back(PriorFactor<Pose2>(0, pose0, priorNoise));
ofstream os2(
"/Users/dellaert/borg/gtsam/gtsam_unstable/examples/rangeResultLM.txt");
ofstream os3(
"/Users/dellaert/borg/gtsam/gtsam_unstable/examples/rangeResultSR.txt");
// initialize points (Gaussian)
Values initial;
if (!smart) {
initial.insert(symbol('L', 1), Point2(-68.9265, 18.3778));
initial.insert(symbol('L', 6), Point2(-37.5805, 69.2278));
initial.insert(symbol('L', 0), Point2(-33.6205, 26.9678));
initial.insert(symbol('L', 5), Point2(1.7095, -5.8122));
}
Values landmarkEstimates = initial; // copy landmarks
initial.insert(0, pose0);
// initialize smart range factors
size_t ids[] = { 1, 6, 0, 5 };
typedef boost::shared_ptr<SmartRangeFactor> SmartPtr;
map<size_t, SmartPtr> smartFactors;
if (smart) {
for(size_t jj: ids) {
smartFactors[jj] = SmartPtr(new SmartRangeFactor(sigmaR));
newFactors.push_back(smartFactors[jj]);
}
}
// set some loop variables
size_t i = 1; // step counter
size_t k = 0; // range measurement counter
Pose2 lastPose = pose0;
size_t countK = 0, totalCount=0;
// Loop over odometry
gttic_(iSAM);
for(const TimedOdometry& timedOdometry: odometry) {
//--------------------------------- odometry loop -----------------------------------------
double t;
Pose2 odometry;
boost::tie(t, odometry) = timedOdometry;
// add odometry factor
newFactors.push_back(
BetweenFactor<Pose2>(i - 1, i, odometry,
NM::Diagonal::Sigmas(odoSigmas)));
// predict pose and add as initial estimate
Pose2 predictedPose = lastPose.compose(odometry);
lastPose = predictedPose;
initial.insert(i, predictedPose);
landmarkEstimates.insert(i, predictedPose);
// Check if there are range factors to be added
while (k < K && t >= boost::get<0>(triples[k])) {
size_t j = boost::get<1>(triples[k]);
double range = boost::get<2>(triples[k]);
if (i > start) {
if (smart && totalCount < minK) {
smartFactors[j]->addRange(i, range);
printf("adding range %g for %d on %d",range,(int)j,(int)i);cout << endl;
}
else {
RangeFactor<Pose2, Point2> factor(i, symbol('L', j), range,
rangeNoise);
// Throw out obvious outliers based on current landmark estimates
Vector error = factor.unwhitenedError(landmarkEstimates);
if (k <= 200 || fabs(error[0]) < 5)
newFactors.push_back(factor);
}
totalCount += 1;
}
k = k + 1;
countK = countK + 1;
}
// Check whether to update iSAM 2
if (k >= minK && countK >= incK) {
gttic_(update);
isam.update(newFactors, initial);
gttoc_(update);
gttic_(calculateEstimate);
Values result = isam.calculateEstimate();
gttoc_(calculateEstimate);
lastPose = result.at<Pose2>(i);
bool hasLandmarks = result.exists(symbol('L', ids[0]));
if (hasLandmarks) {
// update landmark estimates
landmarkEstimates = Values();
for(size_t jj: ids)
landmarkEstimates.insert(symbol('L', jj), result.at(symbol('L', jj)));
}
newFactors = NonlinearFactorGraph();
initial = Values();
if (smart && !hasLandmarks) {
cout << "initialize from smart landmarks" << endl;
for(size_t jj: ids) {
Point2 landmark = smartFactors[jj]->triangulate(result);
initial.insert(symbol('L', jj), landmark);
landmarkEstimates.insert(symbol('L', jj), landmark);
}
}
countK = 0;
for(const Values::ConstFiltered<Point2>::KeyValuePair& it: result.filter<Point2>())
os2 << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t1"
<< endl;
if (smart) {
for(size_t jj: ids) {
Point2 landmark = smartFactors[jj]->triangulate(result);
os3 << jj << "\t" << landmark.x() << "\t" << landmark.y() << "\t1"
<< endl;
}
}
}
i += 1;
if (i>end) break;
//--------------------------------- odometry loop -----------------------------------------
} // end for
gttoc_(iSAM);
// Print timings
tictoc_print_();
// Write result to file
Values result = isam.calculateEstimate();
ofstream os(
"/Users/dellaert/borg/gtsam/gtsam_unstable/examples/rangeResult.txt");
for(const Values::ConstFiltered<Pose2>::KeyValuePair& it: result.filter<Pose2>())
os << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t"
<< it.value.theta() << endl;
exit(0);
}
/* *************************************************************************/
TEST( SmartProjectionCameraFactor, perturbPoseAndOptimize ) {
using namespace vanilla;
// Project three landmarks into three cameras
vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
// Create and fill smartfactors
SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2));
SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2));
SmartFactor::shared_ptr smartFactor3(new SmartFactor(unit2));
vector<Key> views;
views.push_back(c1);
views.push_back(c2);
views.push_back(c3);
smartFactor1->add(measurements_cam1, views);
smartFactor2->add(measurements_cam2, views);
smartFactor3->add(measurements_cam3, views);
// Create factor graph and add priors on two cameras
NonlinearFactorGraph graph;
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6 + 5, 1e-5);
graph.push_back(PriorFactor<Camera>(c1, cam1, noisePrior));
graph.push_back(PriorFactor<Camera>(c2, cam2, noisePrior));
// Create initial estimate
Values initial;
initial.insert(c1, cam1);
initial.insert(c2, cam2);
initial.insert(c3, perturbCameraPose(cam3));
if (isDebugTest)
initial.at<Camera>(c3).print("Smart: Pose3 before optimization: ");
// Points are now uninitialized before a triangulation event
EXPECT(!smartFactor1->point());
EXPECT(!smartFactor2->point());
EXPECT(!smartFactor3->point());
EXPECT_DOUBLES_EQUAL(75711, smartFactor1->error(initial), 1);
EXPECT_DOUBLES_EQUAL(58524, smartFactor2->error(initial), 1);
EXPECT_DOUBLES_EQUAL(77564, smartFactor3->error(initial), 1);
// Error should trigger this and initialize the points, abort if not so
CHECK(smartFactor1->point());
CHECK(smartFactor2->point());
CHECK(smartFactor3->point());
EXPECT(
assert_equal(Point3(2.57696, -0.182566, 1.04085), *smartFactor1->point(),
1e-4));
EXPECT(
assert_equal(Point3(2.80114, -0.702153, 1.06594), *smartFactor2->point(),
1e-4));
EXPECT(
assert_equal(Point3(1.82593, -0.289569, 2.13438), *smartFactor3->point(),
1e-4));
// Check whitened errors
Vector expected(6);
expected << 256, 29, -26, 29, -206, -202;
Point3 point1 = *smartFactor1->point();
SmartFactor::Cameras cameras1 = smartFactor1->cameras(initial);
Vector reprojectionError = cameras1.reprojectionError(point1,
measurements_cam1);
EXPECT(assert_equal(expected, reprojectionError, 1));
Vector actual = smartFactor1->whitenedError(cameras1, point1);
EXPECT(assert_equal(expected, actual, 1));
// Optimize
LevenbergMarquardtParams lmParams;
if (isDebugTest) {
lmParams.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
lmParams.verbosity = NonlinearOptimizerParams::ERROR;
}
LevenbergMarquardtOptimizer optimizer(graph, initial, lmParams);
Values result = optimizer.optimize();
EXPECT(assert_equal(landmark1, *smartFactor1->point(), 1e-5));
EXPECT(assert_equal(landmark2, *smartFactor2->point(), 1e-5));
EXPECT(assert_equal(landmark3, *smartFactor3->point(), 1e-5));
// GaussianFactorGraph::shared_ptr GFG = graph.linearize(initial);
// VectorValues delta = GFG->optimize();
if (isDebugTest)
result.at<Camera>(c3).print("Smart: Pose3 after optimization: ");
EXPECT(assert_equal(cam1, result.at<Camera>(c1)));
EXPECT(assert_equal(cam2, result.at<Camera>(c2)));
EXPECT(assert_equal(result.at<Camera>(c3).pose(), cam3.pose(), 1e-4));
EXPECT(
assert_equal(result.at<Camera>(c3).calibration(), cam3.calibration(), 2));
if (isDebugTest)
tictoc_print_();
}