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
12/**
13 * @file StereoVOExample.cpp
14 * @brief A stereo visual odometry example
15 * @date May 25, 2014
16 * @author Stephen Camp
17 */
18
19/**
20 * A 3D stereo visual odometry example
21 * - robot starts at origin
22 * -moves forward 1 meter
23 * -takes stereo readings on three landmarks
24 */
25
26#include <gtsam/geometry/Pose3.h>
27#include <gtsam/geometry/Cal3_S2Stereo.h>
28#include <gtsam/nonlinear/Values.h>
29#include <gtsam/nonlinear/NonlinearEquality.h>
30#include <gtsam/nonlinear/NonlinearFactorGraph.h>
31#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
32#include <gtsam/slam/StereoFactor.h>
33
34using namespace std;
35using namespace gtsam;
36
37int main(int argc, char** argv) {
38 // create graph object, add first pose at origin with key '1'
39 NonlinearFactorGraph graph;
40 Pose3 first_pose;
41 graph.emplace_shared<NonlinearEquality<Pose3> >(args: 1, args: Pose3());
42
43 // create factor noise model with 3 sigmas of value 1
44 const auto model = noiseModel::Isotropic::Sigma(dim: 3, sigma: 1);
45 // create stereo camera calibration object with .2m between cameras
46 const Cal3_S2Stereo::shared_ptr K(
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
50 graph.emplace_shared<GenericStereoFactor<Pose3,Point3> >(args: StereoPoint2(520, 480, 440), args: model, args: 1, args: 3, args: K);
51 graph.emplace_shared<GenericStereoFactor<Pose3,Point3> >(args: StereoPoint2(120, 80, 440), args: model, args: 1, args: 4, args: K);
52 graph.emplace_shared<GenericStereoFactor<Pose3,Point3> >(args: StereoPoint2(320, 280, 140), args: model, args: 1, args: 5, args: K);
53
54 //create and add stereo factors between second pose and the three landmarks
55 graph.emplace_shared<GenericStereoFactor<Pose3,Point3> >(args: StereoPoint2(570, 520, 490), args: model, args: 2, args: 3, args: K);
56 graph.emplace_shared<GenericStereoFactor<Pose3,Point3> >(args: StereoPoint2(70, 20, 490), args: model, args: 2, args: 4, args: K);
57 graph.emplace_shared<GenericStereoFactor<Pose3,Point3> >(args: StereoPoint2(320, 270, 115), args: model, args: 2, args: 5, args: K);
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(j: 1, val: first_pose);
65 initial_estimate.insert(j: 2, val: Pose3(Rot3(), Point3(0.1, -0.1, 1.1)));
66 initial_estimate.insert(j: 3, val: Point3(1, 1, 5));
67 initial_estimate.insert(j: 4, val: Point3(-1, 1, 5));
68 initial_estimate.insert(j: 5, val: 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(str: "Final result:\n");
75
76 return 0;
77}
78