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


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:37:52