102 lines
3.4 KiB
C++
102 lines
3.4 KiB
C++
/* ----------------------------------------------------------------------------
|
|
|
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
|
* Atlanta, Georgia 30332-0415
|
|
* All Rights Reserved
|
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
|
|
|
* See LICENSE for the license information
|
|
|
|
* -------------------------------------------------------------------------- */
|
|
|
|
/**
|
|
* @file SFMExample.cpp
|
|
* @brief Solve a structure-from-motion problem from a "Bundle Adjustment in the Large" file
|
|
* @author Frank Dellaert
|
|
*/
|
|
|
|
// For an explanation of headers, see SFMExample.cpp
|
|
#include <gtsam/inference/Symbol.h>
|
|
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
|
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
|
#include <gtsam/slam/PriorFactor.h>
|
|
#include <gtsam/slam/GeneralSFMFactor.h>
|
|
#include <gtsam/slam/dataset.h> // for loading BAL datasets !
|
|
#include <vector>
|
|
|
|
using namespace std;
|
|
using namespace gtsam;
|
|
|
|
// We will be using a projection factor that ties a SFM_Camera to a 3D point.
|
|
// An SFM_Camera is defined in datase.h as a camera with unknown Cal3Bundler calibration
|
|
// and has a total of 9 free parameters
|
|
typedef GeneralSFMFactor<SfM_Camera,Point3> MyFactor;
|
|
|
|
/* ************************************************************************* */
|
|
int main (int argc, char* argv[]) {
|
|
|
|
// default file
|
|
string filename = findExampleDataFile("dubrovnik-3-7-pre");
|
|
|
|
// If an argument is given, try loading a file
|
|
if (argc>1) filename = string(argv[1]);
|
|
|
|
///< The structure where we will save the SfM data
|
|
SfM_data mydata;
|
|
assert(readBAL(filename, mydata));
|
|
|
|
// Create a factor graph
|
|
NonlinearFactorGraph graph;
|
|
|
|
// Define the camera observation noise model
|
|
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& measurement, track.measurements) {
|
|
size_t i; Point2 uv;
|
|
boost::tie(i, uv) = measurement;
|
|
graph.push_back(MyFactor(uv, noise, Symbol('x', i), Symbol('p', j)));
|
|
}
|
|
j += 1;
|
|
}
|
|
|
|
// Add a prior on pose x1. This indirectly specifies where the origin is.
|
|
graph.push_back(
|
|
PriorFactor<SfM_Camera>(Symbol('x', 0), mydata.cameras[0],
|
|
noiseModel::Isotropic::Sigma(9, 0.1)));
|
|
|
|
// Add a prior on the position of the first landmark to fix the scale
|
|
graph.push_back(
|
|
PriorFactor<Point3>(Symbol('p', 0), mydata.tracks[0].p,
|
|
noiseModel::Isotropic::Sigma(3, 0.1)));
|
|
|
|
// Create the data structure to hold the initial estimate to the solution
|
|
// Intentionally initialize the variables off from the ground truth
|
|
Values initial;
|
|
size_t i = 0;
|
|
BOOST_FOREACH(const SfM_Camera& camera, mydata.cameras)
|
|
initial.insert(Symbol('x', i++), camera);
|
|
j = 0;
|
|
BOOST_FOREACH(const SfM_Track& track, mydata.tracks)
|
|
initial.insert(Symbol('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;
|
|
}
|
|
/* ************************************************************************* */
|
|
|