timeSFMBALsmart.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
19 #include "timeSFMBAL.h"
20 
24 #include <gtsam/geometry/Point3.h>
25 
26 using namespace std;
27 using namespace gtsam;
28 
31 
32 int main(int argc, char* argv[]) {
33  // parse options and read BAL file
34  SfmData db = preamble(argc, argv);
35 
36  // Add smart factors to graph
38  for (size_t j = 0; j < db.numberTracks(); j++) {
39  auto smartFactor = std::make_shared<SfmFactor>(gNoiseModel);
40  for (const SfmMeasurement& m : db.tracks[j].measurements) {
41  size_t i = m.first;
42  Point2 z = m.second;
43  smartFactor->add(z, C(i));
44  }
45  graph.push_back(smartFactor);
46  }
47 
49  size_t i = 0;
50  gUseSchur = false;
51  for (const SfmCamera& camera : db.cameras)
52  initial.insert(C(i++), camera);
53 
54  return optimize(db, graph, initial);
55 }
Matrix3f m
static SharedNoiseModel gNoiseModel
Definition: timeSFMBAL.h:41
SfmData stores a bunch of SfmTracks.
Definition: SfmData.h:39
Vector2 Point2
Definition: Point2.h:32
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:190
Values initial
Definition: BFloat16.h:88
NonlinearFactorGraph graph
std::vector< SfmTrack > tracks
Sparse set of points.
Definition: SfmData.h:42
Common code for timeSFMBAL scripts.
Base class for all pinhole cameras.
std::pair< size_t, Point2 > SfmMeasurement
A measurement with its camera index.
Definition: SfmTrack.h:32
Smart factor on cameras (pose + calibration)
SmartProjectionFactor< Camera > SfmFactor
static bool gUseSchur
Definition: timeSFMBAL.h:40
Matrix< Scalar, Dynamic, Dynamic > C
Definition: bench_gemm.cpp:50
traits
Definition: chartTesting.h:28
std::vector< SfmCamera > cameras
Set of cameras.
Definition: SfmData.h:40
SfmData preamble(int argc, char *argv[])
Definition: timeSFMBAL.h:44
3D Point
int optimize(const SfmData &db, const NonlinearFactorGraph &graph, const Values &initial, bool separateCalibration=false)
Definition: timeSFMBAL.h:64
int main(int argc, char *argv[])
void insert(Key j, const Value &val)
Definition: Values.cpp:155
size_t numberTracks() const
The number of reconstructed 3D points.
Definition: SfmData.h:74
static const CalibratedCamera camera(kDefaultPose)
Calibration used by Bundler.
std::ptrdiff_t j
PinholeCamera< Cal3Bundler > Camera


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:40:16