testIncrementalFixedLagSmoother.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 
22 #include <gtsam/geometry/Point2.h>
24 #include <gtsam/nonlinear/Values.h>
27 #include <gtsam/inference/Key.h>
29 #include <gtsam/inference/Symbol.h>
30 #include <gtsam/base/debug.h>
31 
33 
34 using namespace std;
35 using namespace gtsam;
36 
37 Key MakeKey(size_t index) { return Symbol('x', index); }
38 
39 /* ************************************************************************* */
40 bool check_smoother(const NonlinearFactorGraph& fullgraph, const Values& fullinit, const IncrementalFixedLagSmoother& smoother, const Key& key) {
41 
42  GaussianFactorGraph linearized = *fullgraph.linearize(fullinit);
43  VectorValues delta = linearized.optimize();
44  Values fullfinal = fullinit.retract(delta);
45 
46  Point2 expected = fullfinal.at<Point2>(key);
47  Point2 actual = smoother.calculateEstimate<Point2>(key);
48 
49  return assert_equal(expected, actual);
50 }
51 
52 /* ************************************************************************* */
54 {
55  // Test the IncrementalFixedLagSmoother in a pure linear environment. Thus, full optimization and
56  // the IncrementalFixedLagSmoother should be identical (even with the linearized approximations at
57  // the end of the smoothing lag)
58 
59  SETDEBUG("IncrementalFixedLagSmoother update", true);
60 
61  // Set up parameters
62  SharedDiagonal odometerNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1));
63  SharedDiagonal loopNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1));
64 
65  // Create a Fixed-Lag Smoother
66  typedef IncrementalFixedLagSmoother::KeyTimestampMap Timestamps;
68 
69  // Create containers to keep the full graph
70  Values fullinit;
71  NonlinearFactorGraph fullgraph;
72 
73  // i keeps track of the time step
74  size_t i = 0;
75 
76  // Add a prior at time 0 and update the HMF
77  {
78  Key key0 = MakeKey(0);
79 
80  NonlinearFactorGraph newFactors;
81  Values newValues;
82  Timestamps newTimestamps;
83 
84  newFactors.addPrior(key0, Point2(0.0, 0.0), odometerNoise);
85  newValues.insert(key0, Point2(0.01, 0.01));
86  newTimestamps[key0] = 0.0;
87 
88  fullgraph.push_back(newFactors);
89  fullinit.insert(newValues);
90 
91  // Update the smoother
92  smoother.update(newFactors, newValues, newTimestamps);
93 
94  // Check
95  CHECK(check_smoother(fullgraph, fullinit, smoother, key0));
96 
97  ++i;
98  }
99 
100  // Add odometry from time 0 to time 5
101  while(i <= 5) {
102  Key key1 = MakeKey(i-1);
103  Key key2 = MakeKey(i);
104 
105  NonlinearFactorGraph newFactors;
106  Values newValues;
107  Timestamps newTimestamps;
108 
109  newFactors.push_back(BetweenFactor<Point2>(key1, key2, Point2(1.0, 0.0), odometerNoise));
110  newValues.insert(key2, Point2(double(i)+0.1, -0.1));
111  newTimestamps[key2] = double(i);
112 
113  fullgraph.push_back(newFactors);
114  fullinit.insert(newValues);
115 
116  // Update the smoother
117  smoother.update(newFactors, newValues, newTimestamps);
118 
119  // Check
120  CHECK(check_smoother(fullgraph, fullinit, smoother, key2));
121 
122  ++i;
123  }
124 
125  // Add odometry from time 5 to 6 to the HMF and a loop closure at time 5 to the TSM
126  {
127  // Add the odometry factor to the HMF
128  Key key1 = MakeKey(i-1);
129  Key key2 = MakeKey(i);
130 
131  NonlinearFactorGraph newFactors;
132  Values newValues;
133  Timestamps newTimestamps;
134 
135  newFactors.push_back(BetweenFactor<Point2>(key1, key2, Point2(1.0, 0.0), odometerNoise));
136  newFactors.push_back(BetweenFactor<Point2>(MakeKey(2), MakeKey(5), Point2(3.5, 0.0), loopNoise));
137  newValues.insert(key2, Point2(double(i)+0.1, -0.1));
138  newTimestamps[key2] = double(i);
139 
140  fullgraph.push_back(newFactors);
141  fullinit.insert(newValues);
142 
143  // Update the smoother
144  smoother.update(newFactors, newValues, newTimestamps);
145 
146  // Check
147  CHECK(check_smoother(fullgraph, fullinit, smoother, key2));
148 
149  ++i;
150  }
151 
152  // Add odometry from time 6 to time 15
153  while(i <= 15) {
154  Key key1 = MakeKey(i-1);
155  Key key2 = MakeKey(i);
156 
157  NonlinearFactorGraph newFactors;
158  Values newValues;
159  Timestamps newTimestamps;
160 
161  newFactors.push_back(BetweenFactor<Point2>(key1, key2, Point2(1.0, 0.0), odometerNoise));
162  newValues.insert(key2, Point2(double(i)+0.1, -0.1));
163  newTimestamps[key2] = double(i);
164 
165  fullgraph.push_back(newFactors);
166  fullinit.insert(newValues);
167 
168  // Update the smoother
169  smoother.update(newFactors, newValues, newTimestamps);
170 
171  // Check
172  CHECK(check_smoother(fullgraph, fullinit, smoother, key2));
173 
174  ++i;
175  }
176 
177  // add/remove an extra factor
178  {
179  Key key1 = MakeKey(i-1);
180  Key key2 = MakeKey(i);
181 
182  NonlinearFactorGraph newFactors;
183  Values newValues;
184  Timestamps newTimestamps;
185 
186  // add 2 odometry factors
187  newFactors.push_back(BetweenFactor<Point2>(key1, key2, Point2(1.0, 0.0), odometerNoise));
188  newFactors.push_back(BetweenFactor<Point2>(key1, key2, Point2(1.0, 0.0), odometerNoise));
189  newValues.insert(key2, Point2(double(i)+0.1, -0.1));
190  newTimestamps[key2] = double(i);
191 
192  fullgraph.push_back(newFactors);
193  fullinit.insert(newValues);
194 
195  // Update the smoother
196  smoother.update(newFactors, newValues, newTimestamps);
197 
198  // Check
199  CHECK(check_smoother(fullgraph, fullinit, smoother, key2));
200 
201  // now remove one of the two and try again
202  // empty values and new factors for fake update in which we only remove factors
203  NonlinearFactorGraph emptyNewFactors;
204  Values emptyNewValues;
205  Timestamps emptyNewTimestamps;
206 
207  size_t factorIndex = 25; // any index that does not break connectivity of the graph
208  FactorIndices factorToRemove;
209  factorToRemove.push_back(factorIndex);
210 
211  const NonlinearFactorGraph smootherFactorsBeforeRemove = smoother.getFactors();
212 
213  // remove factor
214  smoother.update(emptyNewFactors, emptyNewValues, emptyNewTimestamps,factorToRemove);
215 
216  // Note: the following test (checking that the number of factor is reduced by 1)
217  // fails since we are not reusing slots, hence also when removing a factor we do not change
218  // the size of the factor graph
219  // size_t nrFactorsAfterRemoval = smoother.getFactors().size();
220  // DOUBLES_EQUAL(nrFactorsBeforeRemoval-1, nrFactorsAfterRemoval, 1e-5);
221 
222  // check that the factors in the smoother are right
223  NonlinearFactorGraph actual = smoother.getFactors();
224  for(size_t i=0; i< smootherFactorsBeforeRemove.size(); i++){
225  // check that the factors that were not removed are there
226  if(smootherFactorsBeforeRemove[i] && i != factorIndex){
227  EXPECT(smootherFactorsBeforeRemove[i]->equals(*actual[i]));
228  }
229  else{ // while the factors that were not there or were removed are no longer there
230  EXPECT(!actual[i]);
231  }
232  }
233  }
234 }
235 
236 /* ************************************************************************* */
237 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
238 /* ************************************************************************* */
key1
const Symbol key1('v', 1)
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::ISAM2Params
Definition: ISAM2Params.h:136
check_smoother
bool check_smoother(const NonlinearFactorGraph &fullgraph, const Values &fullinit, const IncrementalFixedLagSmoother &smoother, const Key &key)
Definition: testIncrementalFixedLagSmoother.cpp:40
main
int main()
Definition: testIncrementalFixedLagSmoother.cpp:237
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
SETDEBUG
#define SETDEBUG(S, V)
Definition: debug.h:61
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
gtsam::Vector2
Eigen::Vector2d Vector2
Definition: Vector.h:42
TestHarness.h
gtsam::NonlinearFactorGraph::linearize
std::shared_ptr< GaussianFactorGraph > linearize(const Values &linearizationPoint) const
Linearize a nonlinear factor graph.
Definition: NonlinearFactorGraph.cpp:239
Ordering.h
Variable ordering for the elimination algorithm.
GaussianBayesNet.h
Chordal Bayes Net, the result of eliminating a factor graph.
Point2.h
2D Point
Key.h
gtsam::GaussianFactorGraph
Definition: GaussianFactorGraph.h:73
gtsam::Values::at
const ValueType at(Key j) const
Definition: Values-inl.h:261
gtsam::Values::retract
Values retract(const VectorValues &delta) const
Definition: Values.cpp:98
gtsam::IncrementalFixedLagSmoother::update
Result update(const NonlinearFactorGraph &newFactors=NonlinearFactorGraph(), const Values &newTheta=Values(), const KeyTimestampMap &timestamps=KeyTimestampMap(), const FactorIndices &factorsToRemove=FactorIndices()) override
Definition: IncrementalFixedLagSmoother.cpp:66
gtsam::IncrementalFixedLagSmoother
Definition: IncrementalFixedLagSmoother.h:34
gtsam.examples.PlanarManipulatorExample.delta
def delta(g0, g1)
Definition: PlanarManipulatorExample.py:45
key2
const Symbol key2('v', 2)
gtsam::VectorValues
Definition: VectorValues.h:74
BetweenFactor.h
MakeKey
Key MakeKey(size_t index)
Definition: testIncrementalFixedLagSmoother.cpp:37
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
gtsam::SharedDiagonal
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:764
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
gtsam::NonlinearFactorGraph::addPrior
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
Definition: NonlinearFactorGraph.h:199
Symbol.h
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
gtsam::equals
Definition: Testable.h:112
TestResult
Definition: TestResult.h:26
key
const gtsam::Symbol key('X', 0)
IncrementalFixedLagSmoother.h
An iSAM2-based fixed-lag smoother.
gtsam::FactorGraph::size
size_t size() const
Definition: FactorGraph.h:297
gtsam::IncrementalFixedLagSmoother::getFactors
const NonlinearFactorGraph & getFactors() const
Definition: IncrementalFixedLagSmoother.h:95
gtsam
traits
Definition: SFMdata.h:40
TEST
TEST(IncrementalFixedLagSmoother, Example)
Definition: testIncrementalFixedLagSmoother.cpp:53
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
gtsam::FactorGraph::push_back
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:147
gtsam::Values
Definition: Values.h:65
CHECK
#define CHECK(condition)
Definition: Test.h:108
std
Definition: BFloat16.h:88
gtsam::Values::insert
void insert(Key j, const Value &val)
Definition: Values.cpp:155
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
gtsam::IncrementalFixedLagSmoother::calculateEstimate
Values calculateEstimate() const override
Definition: IncrementalFixedLagSmoother.h:74
gtsam::GaussianFactorGraph::optimize
VectorValues optimize(const Eliminate &function=EliminationTraitsType::DefaultEliminate) const
Definition: GaussianFactorGraph.cpp:309
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
Values.h
A non-templated config holding any types of Manifold-group elements.
debug.h
Global debugging flags.
gtsam::Symbol
Definition: inference/Symbol.h:37
gtsam::FactorIndices
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition: Factor.h:37
gtsam::BetweenFactor
Definition: BetweenFactor.h:40


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:40:16