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 FisheyeExample.cpp
14 * @brief A visualSLAM example for the structure-from-motion problem on a
15 * simulated dataset. This version uses a fisheye camera model and a GaussNewton
16 * solver to solve the graph in one batch
17 * @author ghaggin
18 * @Date Apr 9,2020
19 */
20
21/**
22 * A structure-from-motion example with landmarks
23 * - The landmarks form a 10 meter cube
24 * - The robot rotates around the landmarks, always facing towards the cube
25 */
26
27// For loading the data
28#include "SFMdata.h"
29
30// Camera observations of landmarks will be stored as Point2 (x, y).
31#include <gtsam/geometry/Point2.h>
32
33// Each variable in the system (poses and landmarks) must be identified with a
34// unique key. We can either use simple integer keys (1, 2, 3, ...) or symbols
35// (X1, X2, L1). Here we will use Symbols
36#include <gtsam/inference/Symbol.h>
37
38// Use GaussNewtonOptimizer to solve graph
39#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
40#include <gtsam/nonlinear/NonlinearFactorGraph.h>
41#include <gtsam/nonlinear/Values.h>
42
43// In GTSAM, measurement functions are represented as 'factors'. Several common
44// factors have been provided with the library for solving robotics/SLAM/Bundle
45// Adjustment problems. Here we will use Projection factors to model the
46// camera's landmark observations. Also, we will initialize the robot at some
47// location using a Prior factor.
48#include <gtsam/geometry/Cal3Fisheye.h>
49#include <gtsam/slam/PriorFactor.h>
50#include <gtsam/slam/ProjectionFactor.h>
51
52#include <fstream>
53#include <vector>
54
55using namespace std;
56using namespace gtsam;
57
58using symbol_shorthand::L; // for landmarks
59using symbol_shorthand::X; // for poses
60
61/* ************************************************************************* */
62int main(int argc, char *argv[]) {
63 // Define the camera calibration parameters
64 auto K = std::make_shared<Cal3Fisheye>(
65 args: 278.66, args: 278.48, args: 0.0, args: 319.75, args: 241.96, args: -0.013721808247486035,
66 args: 0.020727425669427896, args: -0.012786476702685545, args: 0.0025242267320687625);
67
68 // Define the camera observation noise model, 1 pixel stddev
69 auto measurementNoise = noiseModel::Isotropic::Sigma(dim: 2, sigma: 1.0);
70
71 // Create the set of ground-truth landmarks
72 const vector<Point3> points = createPoints();
73
74 // Create the set of ground-truth poses
75 const vector<Pose3> poses = createPoses();
76
77 // Create a Factor Graph and Values to hold the new data
78 NonlinearFactorGraph graph;
79 Values initialEstimate;
80
81 // Add a prior on pose x0, 0.1 rad on roll,pitch,yaw, and 30cm std on x,y,z
82 auto posePrior = noiseModel::Diagonal::Sigmas(
83 sigmas: (Vector(6) << Vector3::Constant(value: 0.1), Vector3::Constant(value: 0.3)).finished());
84 graph.emplace_shared<PriorFactor<Pose3>>(args: X(j: 0), args: poses[0], args&: posePrior);
85
86 // Add a prior on landmark l0
87 auto pointPrior = noiseModel::Isotropic::Sigma(dim: 3, sigma: 0.1);
88 graph.emplace_shared<PriorFactor<Point3>>(args: L(j: 0), args: points[0], args&: pointPrior);
89
90 // Add initial guesses to all observed landmarks
91 // Intentionally initialize the variables off from the ground truth
92 static const Point3 kDeltaPoint(-0.25, 0.20, 0.15);
93 for (size_t j = 0; j < points.size(); ++j)
94 initialEstimate.insert<Point3>(j: L(j), val: points[j] + kDeltaPoint);
95
96 // Loop over the poses, adding the observations to the graph
97 for (size_t i = 0; i < poses.size(); ++i) {
98 // Add factors for each landmark observation
99 for (size_t j = 0; j < points.size(); ++j) {
100 PinholeCamera<Cal3Fisheye> camera(poses[i], *K);
101 Point2 measurement = camera.project(pw: points[j]);
102 graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3Fisheye>>(
103 args&: measurement, args&: measurementNoise, args: X(j: i), args: L(j), args&: K);
104 }
105
106 // Add an initial guess for the current pose
107 // Intentionally initialize the variables off from the ground truth
108 static const Pose3 kDeltaPose(Rot3::Rodrigues(wx: -0.1, wy: 0.2, wz: 0.25),
109 Point3(0.05, -0.10, 0.20));
110 initialEstimate.insert(j: X(j: i), val: poses[i] * kDeltaPose);
111 }
112
113 GaussNewtonParams params;
114 params.setVerbosity("TERMINATION");
115 params.maxIterations = 10000;
116
117 std::cout << "Optimizing the factor graph" << std::endl;
118 GaussNewtonOptimizer optimizer(graph, initialEstimate, params);
119 Values result = optimizer.optimize();
120 std::cout << "Optimization complete" << std::endl;
121
122 std::cout << "initial error=" << graph.error(values: initialEstimate) << std::endl;
123 std::cout << "final error=" << graph.error(values: result) << std::endl;
124
125 graph.saveGraph(filename: "examples/vio_batch.dot", values: result);
126
127 return 0;
128}
129/* ************************************************************************* */
130