examples/VisualISAMExample.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 // For loading the data
27 #include "SFMdata.h"
28 
29 // Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y).
30 #include <gtsam/geometry/Point2.h>
31 
32 // Each variable in the system (poses and landmarks) must be identified with a unique key.
33 // We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1).
34 // Here we will use Symbols
35 #include <gtsam/inference/Symbol.h>
36 
37 // In GTSAM, measurement functions are represented as 'factors'. Several common factors
38 // have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
39 // Here we will use Projection factors to model the camera's landmark observations.
40 // Also, we will initialize the robot at some location using a Prior factor.
42 
43 // We want to use iSAM to solve the structure-from-motion problem incrementally, so
44 // include iSAM here
46 
47 // iSAM requires as input a set set of new factors to be added stored in a factor graph,
48 // and initial guesses for any new variables used in the added factors
50 #include <gtsam/nonlinear/Values.h>
51 
52 #include <vector>
53 
54 using namespace std;
55 using namespace gtsam;
56 
57 /* ************************************************************************* */
58 int main(int argc, char* argv[]) {
59  // Define the camera calibration parameters
60  Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
61 
62  // Define the camera observation noise model
63  auto noise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
64 
65  // Create the set of ground-truth landmarks
66  vector<Point3> points = createPoints();
67 
68  // Create the set of ground-truth poses
69  vector<Pose3> poses = createPoses();
70 
71  // Create a NonlinearISAM object which will relinearize and reorder the variables
72  // every "relinearizeInterval" updates
73  int relinearizeInterval = 3;
74  NonlinearISAM isam(relinearizeInterval);
75 
76  // Create a Factor Graph and Values to hold the new data
78  Values initialEstimate;
79 
80  // Loop over the different poses, adding the observations to iSAM incrementally
81  for (size_t i = 0; i < poses.size(); ++i) {
82  // Add factors for each landmark observation
83  for (size_t j = 0; j < points.size(); ++j) {
84  // Create ground truth measurement
85  PinholeCamera<Cal3_S2> camera(poses[i], *K);
86  Point2 measurement = camera.project(points[j]);
87  // Add measurement
89  Symbol('x', i), Symbol('l', j), K);
90  }
91 
92  // Intentionally initialize the variables off from the ground truth
93  Pose3 noise(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
94  Pose3 initial_xi = poses[i].compose(noise);
95 
96  // Add an initial guess for the current pose
97  initialEstimate.insert(Symbol('x', i), initial_xi);
98 
99  // If this is the first iteration, add a prior on the first pose to set the coordinate frame
100  // and a prior on the first landmark to set the scale
101  // Also, as iSAM solves incrementally, we must wait until each is observed at least twice before
102  // adding it to iSAM.
103  if (i == 0) {
104  // Add a prior on pose x0, with 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
105  auto poseNoise = noiseModel::Diagonal::Sigmas(
106  (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished());
107  graph.addPrior(Symbol('x', 0), poses[0], poseNoise);
108 
109  // Add a prior on landmark l0
110  auto pointNoise =
111  noiseModel::Isotropic::Sigma(3, 0.1);
112  graph.addPrior(Symbol('l', 0), points[0], pointNoise);
113 
114  // Add initial guesses to all observed landmarks
115  Point3 noise(-0.25, 0.20, 0.15);
116  for (size_t j = 0; j < points.size(); ++j) {
117  // Intentionally initialize the variables off from the ground truth
118  Point3 initial_lj = points[j] + noise;
119  initialEstimate.insert(Symbol('l', j), initial_lj);
120  }
121 
122  } else {
123  // Update iSAM with the new factors
124  isam.update(graph, initialEstimate);
125  Values currentEstimate = isam.estimate();
126  cout << "****************************************************" << endl;
127  cout << "Frame " << i << ": " << endl;
128  currentEstimate.print("Current estimate: ");
129 
130  // Clear the factor graph and values for the next iteration
131  graph.resize(0);
132  initialEstimate.clear();
133  }
134  }
135 
136  return 0;
137 }
138 /* ************************************************************************* */
int relinearizeInterval
void clear()
Definition: Values.h:311
A non-templated config holding any types of Manifold-group elements.
Factor Graph consisting of non-linear factors.
void insert(Key j, const Value &val)
Definition: Values.cpp:140
Vector2 Point2
Definition: Point2.h:27
static Cal3_S2 K(500, 500, 0.1, 640/2, 480/2)
double measurement(10.0)
Definition: Half.h:150
void update(const NonlinearFactorGraph &newFactors, const Values &initialValues)
Values estimate() const
NonlinearFactorGraph graph
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
int main(int argc, char *argv[])
std::vector< gtsam::Point3 > createPoints()
Definition: SFMdata.h:37
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&...args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:172
Class compose(const Class &g) const
Definition: Lie.h:47
NonlinearISAM isam(relinearizeInterval)
Basic bearing factor from 2D measurement.
traits
Definition: chartTesting.h:28
void print(const std::string &str="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Definition: Values.cpp:77
std::vector< gtsam::Pose3 > createPoses(const gtsam::Pose3 &init=gtsam::Pose3(gtsam::Rot3::Ypr(M_PI/2, 0,-M_PI/2), gtsam::Point3(30, 0, 0)), const gtsam::Pose3 &delta=gtsam::Pose3(gtsam::Rot3::Ypr(0,-M_PI/4, 0), gtsam::Point3(sin(M_PI/4)*30, 0, 30 *(1-sin(M_PI/4)))), int steps=8)
Definition: SFMdata.h:54
Simple example for the structure-from-motion problems.
Point2 project(const Point3 &pw, OptionalJacobian< 2, 6 > Dpose=boost::none, OptionalJacobian< 2, 3 > Dpoint=boost::none, OptionalJacobian< 2, DimK > Dcal=boost::none) const
project a 3D point from world coordinates into the image
Definition: PinholePose.h:118
void resize(size_t size)
Definition: FactorGraph.h:358
Vector3 Point3
Definition: Point3.h:35
static const CalibratedCamera camera(kDefaultPose)
2D Point
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
boost::shared_ptr< Cal3_S2 > shared_ptr
Definition: Cal3_S2.h:39
std::ptrdiff_t j


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:51:25