37 using namespace gtsam;
38 using namespace noiseModel;
45 int main(
int argc,
char* argv[]) {
48 if (argc > 1) filename = string(argv[1]);
51 SfmData mydata = SfmData::FromBalFile(filename);
64 noiseModel::Isotropic::Sigma(6, 0.1));
69 noiseModel::Isotropic::Sigma(3, 0.1));
72 auto noise = noiseModel::Isotropic::Sigma(2, 1.0);
84 Point2_ predict_ = project2<SfmCamera>(camera_, point_);
105 }
catch (exception&
e) {
108 cout <<
"final error: " << graph.
error(result) << endl;
Pose3_ getPose(const Expression< PinholeCamera< CALIBRATION > > &cam)
virtual const Values & optimize()
SfmData stores a bunch of SfmTracks.
NonlinearFactorGraph graph
static const SmartProjectionParams params
std::vector< SfmTrack > tracks
Sparse set of points.
Factor graph that supports adding ExpressionFactors directly.
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)
void addExpressionFactor(const Expression< T > &h, const T &z, const SharedNoiseModel &R)
size_t numberTracks() const
The number of reconstructed 3D points.
static const CalibratedCamera camera(kDefaultPose)
utility functions for loading datasets