timeSFMBALcamTnav.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 
21 #include <gtsam/slam/expressions.h>
24 #include <gtsam/geometry/Point3.h>
25 
26 
27 using namespace std;
28 using namespace gtsam;
29 
30 int main(int argc, char* argv[]) {
31  // parse options and read BAL file
32  SfmData db = preamble(argc, argv);
33 
34  // Build graph using conventional GeneralSFMFactor
36  for (size_t j = 0; j < db.numberTracks(); j++) {
37  for (const SfmMeasurement& m: db.tracks[j].measurements) {
38  size_t i = m.first;
39  Point2 z = m.second;
40  Pose3_ camTnav_(C(i));
41  Cal3Bundler_ calibration_(K(i));
42  Point3_ nav_point_(P(j));
44  gNoiseModel, z,
45  uncalibrate(calibration_, // now using transformFrom !!!:
46  project(transformFrom(camTnav_, nav_point_))));
47  }
48  }
49 
51  size_t i = 0, j = 0;
52  for (const SfmCamera& camera: db.cameras) {
53  initial.insert(C(i), camera.pose().inverse()); // inverse !!!
54  initial.insert(K(i), camera.calibration());
55  i += 1;
56  }
57  for (const SfmTrack& track: db.tracks)
58  initial.insert(P(j++), track.p);
59 
60  bool separateCalibration = true;
61  return optimize(db, graph, initial, separateCalibration);
62 }
optimize
int optimize(const SfmData &db, const NonlinearFactorGraph &graph, const Values &initial, bool separateCalibration=false)
Definition: timeSFMBAL.h:64
initial
Values initial
Definition: OdometryOptimize.cpp:2
Point3.h
3D Point
gtsam::SfmData
SfmData stores a bunch of SfmTracks.
Definition: SfmData.h:39
uncalibrate
Point2 uncalibrate(const CAL &K, const Point2 &p, OptionalJacobian< 2, 5 > Dcal, OptionalJacobian< 2, 2 > Dp)
Definition: testExpression.cpp:37
gtsam::SfmTrack
Definition: SfmTrack.h:125
gNoiseModel
static SharedNoiseModel gNoiseModel
Definition: timeSFMBAL.h:41
gtsam::Expression
Definition: Expression.h:47
gtsam::SfmData::tracks
std::vector< SfmTrack > tracks
Sparse set of points.
Definition: SfmData.h:42
gtsam::transformFrom
Point3_ transformFrom(const Pose3_ &x, const Point3_ &p)
Definition: slam/expressions.h:47
j
std::ptrdiff_t j
Definition: tut_arithmetic_redux_minmax.cpp:2
gtsam::Pose3::inverse
Pose3 inverse() const
inverse transformation with derivatives
Definition: Pose3.cpp:49
gtsam::PinholeCamera< Cal3Bundler >
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
timeSFMBAL.h
Common code for timeSFMBAL scripts.
pybind_wrapper_test_script.z
z
Definition: pybind_wrapper_test_script.py:61
m
Matrix3f m
Definition: AngleAxis_mimic_euler.cpp:1
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
preamble
SfmData preamble(int argc, char *argv[])
Definition: timeSFMBAL.h:44
C
Matrix< Scalar, Dynamic, Dynamic > C
Definition: bench_gemm.cpp:50
gtsam
traits
Definition: SFMdata.h:40
K
#define K
Definition: igam.h:8
project
static Point2 project(const Pose3 &pose, const Unit3 &pointAtInfinity, const Cal3_S2::shared_ptr &cal)
Definition: testPinholePose.cpp:188
gtsam::SfmData::cameras
std::vector< SfmCamera > cameras
Set of cameras.
Definition: SfmData.h:40
gtsam::Values
Definition: Values.h:65
expressions.h
Common expressions for solving geometry/slam/sfm problems.
std
Definition: BFloat16.h:88
gtsam::PinholeBase::pose
const Pose3 & pose() const
return pose, constant version
Definition: CalibratedCamera.h:154
P
static double P[]
Definition: ellpe.c:68
ExpressionFactor.h
initial
Definition: testScenarioRunner.cpp:148
camera
static const CalibratedCamera camera(kDefaultPose)
main
int main(int argc, char *argv[])
Definition: timeSFMBALcamTnav.cpp:30
gtsam::SfmTrack::p
Point3 p
3D position of the point
Definition: SfmTrack.h:126
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
gtsam::SfmData::numberTracks
size_t numberTracks() const
The number of reconstructed 3D points.
Definition: SfmData.h:74
Cal3Bundler.h
Calibration used by Bundler.
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
gtsam::NonlinearFactorGraph::addExpressionFactor
void addExpressionFactor(const SharedNoiseModel &R, const T &z, const Expression< T > &h)
Definition: NonlinearFactorGraph.h:187
gtsam::SfmMeasurement
std::pair< size_t, Point2 > SfmMeasurement
A measurement with its camera index.
Definition: SfmTrack.h:32


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:42:20