StereoVOExample.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 
26 #include <gtsam/geometry/Pose3.h>
28 #include <gtsam/nonlinear/Values.h>
33 
34 using namespace std;
35 using namespace gtsam;
36 
37 int main(int argc, char** argv) {
38  // create graph object, add first pose at origin with key '1'
40  Pose3 first_pose;
42 
43  // create factor noise model with 3 sigmas of value 1
44  const auto model = noiseModel::Isotropic::Sigma(3, 1);
45  // create stereo camera calibration object with .2m between cameras
47  new Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2));
48 
49  //create and add stereo factors between first pose (key value 1) and the three landmarks
53 
54  //create and add stereo factors between second pose and the three landmarks
58 
59  // create Values object to contain initial estimates of camera poses and
60  // landmark locations
61  Values initial_estimate;
62 
63  // create and add iniital estimates
64  initial_estimate.insert(1, first_pose);
65  initial_estimate.insert(2, Pose3(Rot3(), Point3(0.1, -0.1, 1.1)));
66  initial_estimate.insert(3, Point3(1, 1, 5));
67  initial_estimate.insert(4, Point3(-1, 1, 5));
68  initial_estimate.insert(5, Point3(0, -0.5, 5));
69 
70  // create Levenberg-Marquardt optimizer for resulting factor graph, optimize
71  LevenbergMarquardtOptimizer optimizer(graph, initial_estimate);
72  Values result = optimizer.optimize();
73 
74  result.print("Final result:\n");
75 
76  return 0;
77 }
virtual const Values & optimize()
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:196
A non-templated config holding any types of Manifold-group elements.
Factor Graph consisting of non-linear factors.
noiseModel::Diagonal::shared_ptr model
static Cal3_S2 K(500, 500, 0.1, 640/2, 480/2)
Definition: BFloat16.h:88
NonlinearFactorGraph graph
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
void print(const std::string &str="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Definition: Values.cpp:66
Values result
The most common 5DOF 3D->2D calibration, stereo version.
Definition: Cal3_S2Stereo.h:30
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
std::shared_ptr< Cal3_S2Stereo > shared_ptr
Definition: Cal3_S2Stereo.h:38
A non-linear factor for stereo measurements.
traits
Definition: chartTesting.h:28
The most common 5DOF 3D->2D calibration + Stereo baseline.
Vector3 Point3
Definition: Point3.h:38
int main(int argc, char **argv)
3D Pose


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:36:20