FixedLagSmootherExample.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 
25 // This example demonstrates the use of the Fixed-Lag Smoothers in GTSAM unstable
28 
29 // In GTSAM, measurement functions are represented as 'factors'. Several common factors
30 // have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
31 // Here we will use Between factors for the relative motion described by odometry measurements.
32 // Also, we will initialize the robot at the origin using a Prior factor.
34 
35 // When the factors are created, we will add them to a Factor Graph. As the factors we are using
36 // are nonlinear factors, we will need a Nonlinear Factor Graph.
38 
39 // The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the
40 // nonlinear functions around an initial linearization point, then solve the linear system
41 // to update the linearization point. This happens repeatedly until the solver converges
42 // to a consistent set of variable values. This requires us to specify an initial guess
43 // for each variable, held in a Values container.
44 #include <gtsam/nonlinear/Values.h>
45 
46 // We will use simple integer Keys to uniquely identify each robot pose.
47 #include <gtsam/inference/Key.h>
48 
49 // We will use Pose2 variables (x, y, theta) to represent the robot positions
50 #include <gtsam/geometry/Pose2.h>
51 
52 #include <iomanip>
53 
54 using namespace std;
55 using namespace gtsam;
56 
57 
58 int main(int argc, char** argv) {
59 
60  // Define the smoother lag (in seconds)
61  double lag = 2.0;
62 
63  // Create a fixed lag smoother
64  // The Batch version uses Levenberg-Marquardt to perform the nonlinear optimization
65  BatchFixedLagSmoother smootherBatch(lag);
66  // The Incremental version uses iSAM2 to perform the nonlinear optimization
68  parameters.relinearizeThreshold = 0.0; // Set the relin threshold to zero such that the batch estimate is recovered
69  parameters.relinearizeSkip = 1; // Relinearize every time
70  IncrementalFixedLagSmoother smootherISAM2(lag, parameters);
71 
72  // Create containers to store the factors and linearization points that
73  // will be sent to the smoothers
74  NonlinearFactorGraph newFactors;
75  Values newValues;
77 
78  // Create a prior on the first pose, placing it at the origin
79  Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
80  noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
81  Key priorKey = 0;
82  newFactors.addPrior(priorKey, priorMean, priorNoise);
83  newValues.insert(priorKey, priorMean); // Initialize the first pose at the mean of the prior
84  newTimestamps[priorKey] = 0.0; // Set the timestamp associated with this key to 0.0 seconds;
85 
86  // Now, loop through several time steps, creating factors from different "sensors"
87  // and adding them to the fixed-lag smoothers
88  double deltaT = 0.25;
89  for(double time = deltaT; time <= 3.0; time += deltaT) {
90 
91  // Define the keys related to this timestamp
92  Key previousKey(1000 * (time-deltaT));
93  Key currentKey(1000 * (time));
94 
95  // Assign the current key to the current timestamp
96  newTimestamps[currentKey] = time;
97 
98  // Add a guess for this pose to the new values
99  // Since the robot moves forward at 2 m/s, then the position is simply: time[s]*2.0[m/s]
100  // {This is not a particularly good way to guess, but this is just an example}
101  Pose2 currentPose(time * 2.0, 0.0, 0.0);
102  newValues.insert(currentKey, currentPose);
103 
104  // Add odometry factors from two different sources with different error stats
105  Pose2 odometryMeasurement1 = Pose2(0.61, -0.08, 0.02);
106  noiseModel::Diagonal::shared_ptr odometryNoise1 = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.05));
107  newFactors.push_back(BetweenFactor<Pose2>(previousKey, currentKey, odometryMeasurement1, odometryNoise1));
108 
109  Pose2 odometryMeasurement2 = Pose2(0.47, 0.03, 0.01);
110  noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas(Vector3(0.05, 0.05, 0.05));
111  newFactors.push_back(BetweenFactor<Pose2>(previousKey, currentKey, odometryMeasurement2, odometryNoise2));
112 
113  // Update the smoothers with the new factors. In this example, batch smoother needs one iteration
114  // to accurately converge. The ISAM smoother doesn't, but we only start getting estiates when
115  // both are ready for simplicity.
116  if (time >= 0.50) {
117  smootherBatch.update(newFactors, newValues, newTimestamps);
118  smootherISAM2.update(newFactors, newValues, newTimestamps);
119  for(size_t i = 1; i < 2; ++i) { // Optionally perform multiple iSAM2 iterations
120  smootherISAM2.update();
121  }
122 
123  // Print the optimized current pose
124  cout << setprecision(5) << "Timestamp = " << time << endl;
125  smootherBatch.calculateEstimate<Pose2>(currentKey).print("Batch Estimate:");
126  smootherISAM2.calculateEstimate<Pose2>(currentKey).print("iSAM2 Estimate:");
127  cout << endl;
128 
129  // Clear contains for the next iteration
130  newTimestamps.clear();
131  newValues.clear();
132  newFactors.resize(0);
133  }
134  }
135 
136  // And to demonstrate the fixed-lag aspect, print the keys contained in each smoother after 3.0 seconds
137  cout << "After 3.0 seconds, " << endl;
138  cout << " Batch Smoother Keys: " << endl;
139  for(const FixedLagSmoother::KeyTimestampMap::value_type& key_timestamp: smootherBatch.timestamps()) {
140  cout << setprecision(5) << " Key: " << key_timestamp.first << " Time: " << key_timestamp.second << endl;
141  }
142  cout << " iSAM2 Smoother Keys: " << endl;
143  for(const FixedLagSmoother::KeyTimestampMap::value_type& key_timestamp: smootherISAM2.timestamps()) {
144  cout << setprecision(5) << " Key: " << key_timestamp.first << " Time: " << key_timestamp.second << endl;
145  }
146 
147  // Here is an example of how to get the full Jacobian of the problem.
148  // First, get the linearization point.
149  Values result = smootherISAM2.calculateEstimate();
150 
151  // Get the factor graph
152  auto &factorGraph = smootherISAM2.getFactors();
153 
154  // Linearize to a Gaussian factor graph
155  std::shared_ptr<GaussianFactorGraph> linearGraph = factorGraph.linearize(result);
156 
157  // Converts the linear graph into a Jacobian factor and extracts the Jacobian matrix
158  Matrix jacobian = linearGraph->jacobian().first;
159  cout << " Jacobian: " << jacobian << endl;
160 
161  return 0;
162 }
void clear()
Definition: Values.h:298
Result update(const NonlinearFactorGraph &newFactors=NonlinearFactorGraph(), const Values &newTheta=Values(), const KeyTimestampMap &timestamps=KeyTimestampMap(), const FactorIndices &factorsToRemove=FactorIndices()) override
std::map< Key, double > KeyTimestampMap
Typedef for a Key-Timestamp map/database.
Eigen::Vector3d Vector3
Definition: Vector.h:43
A non-templated config holding any types of Manifold-group elements.
Factor Graph consisting of non-linear factors.
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:190
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
Definition: BFloat16.h:88
EIGEN_STRONG_INLINE Packet4f print(const Packet4f &a)
std::shared_ptr< GaussianFactorGraph > linearize(const Values &linearizationPoint) const
Linearize a nonlinear factor graph.
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
Result update(const NonlinearFactorGraph &newFactors=NonlinearFactorGraph(), const Values &newTheta=Values(), const KeyTimestampMap &timestamps=KeyTimestampMap(), const FactorIndices &factorsToRemove=FactorIndices()) override
int main(int argc, char **argv)
Values result
const NonlinearFactorGraph & getFactors() const
An iSAM2-based fixed-lag smoother.
static const double deltaT
Pose2 priorMean(0.0, 0.0, 0.0)
#define time
static ConjugateGradientParameters parameters
RelinearizationThreshold relinearizeThreshold
Definition: ISAM2Params.h:169
traits
Definition: chartTesting.h:28
auto priorNoise
const KeyTimestampMap & timestamps() const
virtual void resize(size_t size)
Definition: FactorGraph.h:389
void insert(Key j, const Value &val)
Definition: Values.cpp:155
2D Pose
std::shared_ptr< Diagonal > shared_ptr
Definition: NoiseModel.h:307
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:13