32 using namespace gtsam;
42 int main(
int argc,
char* argv[]) {
45 if (argc > 1) filename = string(argv[1]);
48 SfmData mydata = SfmData::FromBalFile(filename);
55 auto noise = noiseModel::Isotropic::Sigma(2, 1.0);
62 uv, noise,
C(
i),
P(j));
69 graph.
addPrior(
C(0), mydata.
cameras[0], noiseModel::Isotropic::Sigma(9, 0.1));
71 noiseModel::Isotropic::Sigma(3, 0.1));
87 params_using_METIS.
ordering = Ordering::Create(Ordering::METIS, graph);
92 params_using_COLAMD.
ordering = Ordering::Create(Ordering::COLAMD, graph);
94 }
catch (exception&
e) {
100 cout <<
"COLAMD and METIS produce the same ordering. " 101 <<
"Problem here!!!" << endl;
106 Values result_METIS, result_COLAMD;
108 gttic_(OPTIMIZE_WITH_METIS);
111 gttoc_(OPTIMIZE_WITH_METIS);
113 gttic_(OPTIMIZE_WITH_COLAMD);
115 result_COLAMD = lm_COLAMD.
optimize();
116 gttoc_(OPTIMIZE_WITH_COLAMD);
117 }
catch (exception& e) {
123 cout <<
"COLAMD final error: " << graph.
error(result_COLAMD) << endl;
124 cout <<
"METIS final error: " << graph.
error(result_METIS) << endl;
126 cout << endl << endl;
128 cout <<
"Time comparison by solving " << filename <<
" results:" << endl;
131 <<
" cameras" << endl;
virtual const Values & optimize()
SfmData stores a bunch of SfmTracks.
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Factor Graph consisting of non-linear factors.
int main(int argc, char *argv[])
NonlinearFactorGraph graph
std::vector< SfmTrack > tracks
Sparse set of points.
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
void setVerbosity(const std::string &src)
Data structure for dealing with Structure from Motion data.
std::vector< SfmMeasurement > measurements
The 2D image projections (id,(u,v))
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
size_t numberCameras() const
The number of cameras.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
double error(const Values &values) const
Matrix< Scalar, Dynamic, Dynamic > C
GTSAM_EXPORT std::string findExampleDataFile(const std::string &name)
std::vector< SfmCamera > cameras
Set of cameras.
Point3 p
3D position of the point
void insert(Key j, const Value &val)
size_t numberTracks() const
The number of reconstructed 3D points.
std::optional< Ordering > ordering
The optional variable elimination ordering, or empty to use COLAMD (default: empty) ...
static const CalibratedCamera camera(kDefaultPose)
a general SFM factor with an unknown calibration
utility functions for loading datasets
GeneralSFMFactor< SfmCamera, Point3 > MyFactor