29 using namespace gtsam;
39 int main (
int argc,
char* argv[]) {
43 if (argc>1) filename = string(argv[1]);
46 SfmData mydata = SfmData::FromBalFile(filename);
54 noiseModel::Isotropic::Sigma(2, 1.0);
67 graph.
addPrior(
C(0), mydata.
cameras[0], noiseModel::Isotropic::Sigma(9, 0.1));
68 graph.
addPrior(
P(0), mydata.
tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1));
83 }
catch (exception&
e) {
86 cout <<
"final error: " << graph.
error(result) << 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.
NonlinearFactorGraph graph
GeneralSFMFactor< SfmCamera, Point3 > MyFactor
static const SmartProjectionParams params
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
int main(int argc, char *argv[])
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.
static const CalibratedCamera camera(kDefaultPose)
a general SFM factor with an unknown calibration
utility functions for loading datasets