ConcurrentIncrementalFilter.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 
20 #include <gtsam/base/timing.h>
21 #include <gtsam/base/debug.h>
22 
23 namespace gtsam {
24 
25 /* ************************************************************************* */
26 void ConcurrentIncrementalFilter::print(const std::string& s, const KeyFormatter& keyFormatter) const {
27  std::cout << s;
28  isam2_.print("");
29 }
30 
31 /* ************************************************************************* */
33  const ConcurrentIncrementalFilter* filter = dynamic_cast<const ConcurrentIncrementalFilter*>(&rhs);
34  return filter
35  && isam2_.equals(filter->isam2_)
42 }
43 
44 /* ************************************************************************* */
46  const std::optional<FastList<Key> >& keysToMove, const std::optional< FactorIndices >& removeFactorIndices) {
47 
48  gttic(update);
49 
50 // const bool debug = ISDEBUG("ConcurrentIncrementalFilter update");
51  const bool debug = false;
52 
53  if(debug) std::cout << "ConcurrentIncrementalFilter::update Begin" << std::endl;
54 
55  // Create the return result meta-data
56  Result result;
57 
58  // We do not need to remove any factors at this time
59  FactorIndices removedFactors;
60 
61  if(removeFactorIndices){
62  removedFactors.insert(removedFactors.end(), removeFactorIndices->begin(), removeFactorIndices->end());
63  }
64 
65  // Generate ordering constraints that force the 'keys to move' to the end
66  std::optional<FastMap<Key,int> > orderingConstraints = {};
67  if(keysToMove && keysToMove->size() > 0) {
68  orderingConstraints = FastMap<Key,int>();
69  int group = 1;
70  // Set all existing variables to Group1
71  if(isam2_.getLinearizationPoint().size() > 0) {
72  for(const auto key: isam2_.getLinearizationPoint().keys()) {
73  orderingConstraints->operator[](key) = group;
74  }
75  ++group;
76  }
77  // Assign new variables to the root
78  for(const auto key: newTheta.keys()) {
79  orderingConstraints->operator[](key) = group;
80  }
81  // Set marginalizable variables to Group0
82  for(Key key: *keysToMove){
83  orderingConstraints->operator[](key) = 0;
84  }
85  }
86 
87  // Create the set of linear keys that iSAM2 should hold constant
88  // iSAM2 takes care of this for us; no need to specify additional noRelin keys
89  std::optional<FastList<Key> > noRelinKeys = {};
90 
91  // Mark additional keys between the 'keys to move' and the leaves
92  std::optional<FastList<Key> > additionalKeys = {};
93  if(keysToMove && keysToMove->size() > 0) {
94  std::set<Key> markedKeys;
95  for(Key key: *keysToMove) {
98  GaussianConditional::const_iterator key_iter = clique->conditional()->begin();
99  while(*key_iter != key) {
100  markedKeys.insert(*key_iter);
101  ++key_iter;
102  }
103  for(const ISAM2Clique::shared_ptr& child: clique->children) {
104  RecursiveMarkAffectedKeys(key, child, markedKeys);
105  }
106  }
107  }
108  additionalKeys = FastList<Key>(markedKeys.begin(), markedKeys.end());
109  }
110 
111  // Update the system using iSAM2
112  gttic(isam2);
113  ISAM2Result isam2Result = isam2_.update(newFactors, newTheta, removedFactors, orderingConstraints, noRelinKeys, additionalKeys);
114  gttoc(isam2);
115 
116  if(keysToMove && keysToMove->size() > 0) {
117 
118  gttic(cache_smoother_factors);
119  // Find the set of factors that will be removed
121  // Cache these factors for later transmission to the smoother
122  NonlinearFactorGraph removedFactors;
123  for(size_t slot: removedFactorSlots) {
124  const NonlinearFactor::shared_ptr& factor = isam2_.getFactorsUnsafe().at(slot);
125  if(factor) {
126  smootherFactors_.push_back(factor);
127  removedFactors.push_back(factor);
128  }
129  }
130  // Cache removed values for later transmission to the smoother
131  for(Key key: *keysToMove) {
133  }
134  gttoc(cache_smoother_factors);
135 
136  gttic(marginalize);
137  FactorIndices marginalFactorsIndices;
138  FactorIndices deletedFactorsIndices;
139  isam2_.marginalizeLeaves(*keysToMove, marginalFactorsIndices, deletedFactorsIndices);
140  currentSmootherSummarizationSlots_.insert(currentSmootherSummarizationSlots_.end(), marginalFactorsIndices.begin(), marginalFactorsIndices.end());
141  for(size_t index: deletedFactorsIndices) {
143  }
144  gttoc(marginalize);
145 
146  // Calculate a new shortcut
147  updateShortcut(removedFactors);
148  }
149 
150  // Extract the ConcurrentIncrementalFilter::Result information
151  result.iterations = 1;
152  result.linearVariables = isam2_.getFixedVariables().size();
153  result.nonlinearVariables = isam2_.getLinearizationPoint().size() - result.linearVariables;
154  result.newFactorsIndices = isam2Result.newFactorsIndices;
155  result.variablesReeliminated = isam2Result.variablesReeliminated;
156  result.variablesRelinearized = isam2Result.variablesRelinearized;
157 // result.error = isam2_.getFactorsUnsafe().error(isam2_.calculateEstimate());
158 
159  if(debug) std::cout << "ConcurrentIncrementalFilter::update End" << std::endl;
160 
161  gttoc(update);
162 
163  return result;
164 }
165 
166 /* ************************************************************************* */
168 
169  gttic(presync);
170 
171  gttoc(presync);
172 }
173 
174 /* ************************************************************************* */
175 void ConcurrentIncrementalFilter::synchronize(const NonlinearFactorGraph& smootherSummarization, const Values& smootherSummarizationValues) {
176 
178 // const bool debug = ISDEBUG("ConcurrentIncrementalFilter synchronize");
179  const bool debug = false;
180 
181  if(debug) std::cout << "ConcurrentIncrementalFilter::synchronize Begin" << std::endl;
182 
183  // Update the smoother summarization on the old separator
184  previousSmootherSummarization_ = smootherSummarization;
185 
186  // Find the set of new separator keys
187  const KeySet& newSeparatorKeys = isam2_.getFixedVariables();
188 
189  // Use the shortcut to calculate an updated marginal on the current separator
190  // Combine just the shortcut and the previousSmootherSummarization
193  graph.push_back(smootherShortcut_);
194  Values values;
195  values.insert(smootherSummarizationValues);
196  for(Key key: newSeparatorKeys) {
197  if(!values.exists(key)) {
199  }
200  }
201 
202  // Force iSAM2 not to relinearize anything during this iteration
203  FastList<Key> noRelinKeys;
204  for(const auto key: isam2_.getLinearizationPoint().keys()) {
205  noRelinKeys.push_back(key);
206  }
207 
208  // Calculate the summarized factor on just the new separator keys
209  NonlinearFactorGraph currentSmootherSummarization = internal::calculateMarginalFactors(graph, values, newSeparatorKeys,
211 
212  // Remove the old factors on the separator and insert the new ones
214  ISAM2Result result = isam2_.update(currentSmootherSummarization, Values(), removeFactors, {}, noRelinKeys, {}, false);
215  currentSmootherSummarizationSlots_ = result.newFactorsIndices;
216 
217  // Set the previous smoother summarization to the current smoother summarization and clear the smoother shortcut
218  previousSmootherSummarization_ = currentSmootherSummarization;
220 
221  if(debug) std::cout << "ConcurrentIncrementalFilter::synchronize End" << std::endl;
222 
224 }
225 
226 /* ************************************************************************* */
227 void ConcurrentIncrementalFilter::getSummarizedFactors(NonlinearFactorGraph& filterSummarization, Values& filterSummarizationValues) {
228 
229  gttic(get_summarized_factors);
230 
231  // Calculate the current filter summarization
232  filterSummarization = calculateFilterSummarization();
233 
234  // Copy the current separator values into the output
235  for(Key key: isam2_.getFixedVariables()) {
236  filterSummarizationValues.insert(key, isam2_.getLinearizationPoint().at(key));
237  }
238 
239  gttoc(get_summarized_factors);
240 }
241 
242 /* ************************************************************************* */
244 
245  gttic(get_smoother_factors);
246 
247  // Copy the previous calculated smoother summarization factors into the output
248  smootherFactors.push_back(smootherFactors_);
249  smootherValues.insert(smootherValues_);
250 
251  gttoc(get_smoother_factors);
252 }
253 
254 /* ************************************************************************* */
256 
257  gttic(postsync);
258 
259  // Clear out the factors and values that were just sent to the smoother
262 
263  gttoc(postsync);
264 }
265 
266 
267 /* ************************************************************************* */
268 void ConcurrentIncrementalFilter::RecursiveMarkAffectedKeys(const Key& key, const ISAM2Clique::shared_ptr& clique, std::set<Key>& additionalKeys) {
269 
270  // Check if the separator keys of the current clique contain the specified key
271  if(std::find(clique->conditional()->beginParents(), clique->conditional()->endParents(), key) != clique->conditional()->endParents()) {
272 
273  // Mark the frontal keys of the current clique
274  for(Key idx: clique->conditional()->frontals()) {
275  additionalKeys.insert(idx);
276  }
277 
278  // Recursively mark all of the children
279  for(const ISAM2Clique::shared_ptr& child: clique->children) {
280  RecursiveMarkAffectedKeys(key, child, additionalKeys);
281  }
282  }
283  // If the key was not found in the separator/parents, then none of its children can have it either
284 
285 }
286 
287 /* ************************************************************************* */
289 
290  // Identify any new factors to be sent to the smoother (i.e. any factor involving keysToMove)
291  FactorIndices removedFactorSlots;
292  const VariableIndex& variableIndex = isam2.getVariableIndex();
293  for(Key key: keys) {
294  const auto& slots = variableIndex[key];
295  removedFactorSlots.insert(removedFactorSlots.end(), slots.begin(), slots.end());
296  }
297 
298  // Sort and remove duplicates
299  std::sort(removedFactorSlots.begin(), removedFactorSlots.end());
300  removedFactorSlots.erase(std::unique(removedFactorSlots.begin(), removedFactorSlots.end()), removedFactorSlots.end());
301 
302  // Remove any linear/marginal factor that made it into the set
303  for(size_t index: factorsToIgnore) {
304  removedFactorSlots.erase(std::remove(removedFactorSlots.begin(), removedFactorSlots.end(), index), removedFactorSlots.end());
305  }
306 
307  return removedFactorSlots;
308 }
309 
311 // TODO: Make this a static function
313 
314  // Calculate the set of shortcut keys: NewSeparatorKeys + OldSeparatorKeys
315  KeySet shortcutKeys;
316  for(size_t slot: currentSmootherSummarizationSlots_) {
317  const NonlinearFactor::shared_ptr& factor = isam2_.getFactorsUnsafe().at(slot);
318  if(factor) {
319  shortcutKeys.insert(factor->begin(), factor->end());
320  }
321  }
323  shortcutKeys.insert(key);
324  }
325 
326  // Combine the previous shortcut factor with all of the new factors being sent to the smoother
328  graph.push_back(removedFactors);
329  graph.push_back(smootherShortcut_);
330  Values values;
331  values.insert(smootherValues_);
333  // Calculate the summarized factor on the shortcut keys
336 }
337 
338 /* ************************************************************************* */
339 // TODO: Make this a static function
341 
342  // The filter summarization factors are the resulting marginal factors on the separator
343  // variables that result from marginalizing out all of the other variables
344 
345  // Find the set of current separator keys
346  const KeySet& separatorKeys = isam2_.getFixedVariables();
347 
348  // Find all cliques that contain any separator variables
349  std::set<ISAM2Clique::shared_ptr> separatorCliques;
350  for(Key key: separatorKeys) {
352  separatorCliques.insert( clique );
353  }
354 
355  // Create the set of clique keys LC:
356  KeyVector cliqueKeys;
357  for(const ISAM2Clique::shared_ptr& clique: separatorCliques) {
358  for(Key key: clique->conditional()->frontals()) {
359  cliqueKeys.push_back(key);
360  }
361  }
362  std::sort(cliqueKeys.begin(), cliqueKeys.end());
363 
364  // Gather all factors that involve only clique keys
365  std::set<size_t> cliqueFactorSlots;
366  for(Key key: cliqueKeys) {
367  for(size_t slot: isam2_.getVariableIndex()[key]) {
368  const NonlinearFactor::shared_ptr& factor = isam2_.getFactorsUnsafe().at(slot);
369  if(factor) {
370  std::set<Key> factorKeys(factor->begin(), factor->end());
371  if(std::includes(cliqueKeys.begin(), cliqueKeys.end(), factorKeys.begin(), factorKeys.end())) {
372  cliqueFactorSlots.insert(slot);
373  }
374  }
375  }
376  }
377 
378  // Remove any factor included in the current smoother summarization
379  for(size_t slot: currentSmootherSummarizationSlots_) {
380  cliqueFactorSlots.erase(slot);
381  }
382 
383  // Create a factor graph from the identified factors
385  for(size_t slot: cliqueFactorSlots) {
386  graph.push_back(isam2_.getFactorsUnsafe().at(slot));
387  }
388 
389  // Find the set of children of the separator cliques
390  std::set<ISAM2Clique::shared_ptr> childCliques;
391  // Add all of the children
392  for(const ISAM2Clique::shared_ptr& clique: separatorCliques) {
393  childCliques.insert(clique->children.begin(), clique->children.end());
394  }
395  // Remove any separator cliques that were added because they were children of other separator cliques
396  for(const ISAM2Clique::shared_ptr& clique: separatorCliques) {
397  childCliques.erase(clique);
398  }
399 
400  // Augment the factor graph with cached factors from the children
401  for(const ISAM2Clique::shared_ptr& clique: childCliques) {
403  graph.push_back( factor );
404  }
405 
406  // Calculate the marginal factors on the separator
409 
410  return filterSummarization;
411 }
412 
413 /* ************************************************************************* */
414 }
gttoc
#define gttoc(label)
Definition: timing.h:296
timing.h
Timing utilities.
gtsam::NonlinearFactor::shared_ptr
std::shared_ptr< This > shared_ptr
Definition: NonlinearFactor.h:78
gtsam::ISAM2::getVariableIndex
const VariableIndex & getVariableIndex() const
Definition: ISAM2.h:274
gtsam::ConcurrentIncrementalFilter::Result
Definition: ConcurrentIncrementalFilter.h:38
gtsam::Values::exists
bool exists(Key j) const
Definition: Values.cpp:93
gtsam::NonlinearFactorGraph::equals
bool equals(const NonlinearFactorGraph &other, double tol=1e-9) const
Definition: NonlinearFactorGraph.cpp:97
gtsam::ISAM2
Definition: ISAM2.h:45
gtsam::Values::keys
KeyVector keys() const
Definition: Values.cpp:218
gtsam::ISAM2::marginalizeLeaves
void marginalizeLeaves(const FastList< Key > &leafKeys, FactorIndices *marginalFactorsIndices=nullptr, FactorIndices *deletedFactorsIndices=nullptr)
Definition: ISAM2.cpp:486
gtsam::Values::size
size_t size() const
Definition: Values.h:178
s
RealScalar s
Definition: level1_cplx_impl.h:126
LinearContainerFactor.h
Wrap Jacobian and Hessian linear factors to allow simple injection into a nonlinear graph.
keys
const KeyVector keys
Definition: testRegularImplicitSchurFactor.cpp:40
gtsam::ISAM2::getFactorsUnsafe
const NonlinearFactorGraph & getFactorsUnsafe() const
Definition: ISAM2.h:269
gtsam::ConcurrentIncrementalFilter::smootherValues_
Values smootherValues_
A temporary holding place for the linearization points of all keys being sent to the smoother.
Definition: ConcurrentIncrementalFilter.h:178
gtsam::ISAM2Result::newFactorsIndices
FactorIndices newFactorsIndices
Definition: ISAM2Result.h:95
gtsam::FastMap
Definition: FastMap.h:39
different_sigmas::values
HybridValues values
Definition: testHybridBayesNet.cpp:245
gtsam::FastSet< Key >
gtsam::BayesTree::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Definition: BayesTree-inst.h:251
gtsam::ConcurrentIncrementalFilter::FindAdjacentFactors
static FactorIndices FindAdjacentFactors(const ISAM2 &isam2, const FastList< Key > &keys, const FactorIndices &factorsToIgnore)
Definition: ConcurrentIncrementalFilter.cpp:288
gtsam::ConcurrentIncrementalFilter::smootherShortcut_
NonlinearFactorGraph smootherShortcut_
A set of conditional factors from the old separator to the current separator (recursively calculated ...
Definition: ConcurrentIncrementalFilter.h:174
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:92
result
Values result
Definition: OdometryOptimize.cpp:8
gtsam::ISAM2Params::getEliminationFunction
GaussianFactorGraph::Eliminate getEliminationFunction() const
Definition: ISAM2Params.h:323
gtsam::ConcurrentIncrementalFilter::getSummarizedFactors
void getSummarizedFactors(NonlinearFactorGraph &filterSummarization, Values &filterSummarizationValues) override
Definition: ConcurrentIncrementalFilter.cpp:227
gtsam::ConcurrentIncrementalFilter::update
Result update(const NonlinearFactorGraph &newFactors=NonlinearFactorGraph(), const Values &newTheta=Values(), const std::optional< FastList< Key > > &keysToMove={}, const std::optional< FactorIndices > &removeFactorIndices={})
Definition: ConcurrentIncrementalFilter.cpp:45
gtsam::ISAM2::equals
virtual bool equals(const ISAM2 &other, double tol=1e-9) const
Definition: ISAM2.cpp:57
gtsam::ConcurrentIncrementalFilter::synchronize
void synchronize(const NonlinearFactorGraph &smootherSummarization, const Values &smootherSummarizationValues) override
Definition: ConcurrentIncrementalFilter.cpp:175
gtsam::ConcurrentIncrementalFilter::isam2_
ISAM2 isam2_
The iSAM2 inference engine.
Definition: ConcurrentIncrementalFilter.h:169
gtsam::FactorGraph::at
const sharedFactor at(size_t i) const
Definition: FactorGraph.h:306
gtsam::ConcurrentFilter
Definition: ConcurrentFilteringAndSmoothing.h:39
gtsam::ISAM2::getLinearizationPoint
const Values & getLinearizationPoint() const
Access the current linearization point.
Definition: ISAM2.h:217
gtsam::Values::at
const ValueType at(Key j) const
Definition: Values-inl.h:261
gtsam::FactorGraph::resize
virtual void resize(size_t size)
Definition: FactorGraph.h:367
gtsam::ConcurrentIncrementalFilter::smootherFactors_
NonlinearFactorGraph smootherFactors_
A temporary holding place for the set of full nonlinear factors being sent to the smoother.
Definition: ConcurrentIncrementalFilter.h:177
gtsam::internal::calculateMarginalFactors
NonlinearFactorGraph calculateMarginalFactors(const NonlinearFactorGraph &graph, const Values &theta, const KeySet &remainingKeys, const GaussianFactorGraph::Eliminate &eliminateFunction)
Definition: ConcurrentFilteringAndSmoothing.cpp:54
gtsam::ConcurrentIncrementalFilter::postsync
void postsync() override
Definition: ConcurrentIncrementalFilter.cpp:255
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition: Key.h:35
ConcurrentIncrementalFilter.h
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
debug
static constexpr bool debug
Definition: testDiscreteBayesTree.cpp:31
gtsam::ConcurrentIncrementalFilter::currentSmootherSummarizationSlots_
FactorIndices currentSmootherSummarizationSlots_
The slots in factor graph that correspond to the current smoother summarization on the current separa...
Definition: ConcurrentIncrementalFilter.h:173
gtsam::ConcurrentIncrementalFilter::previousSmootherSummarization_
NonlinearFactorGraph previousSmootherSummarization_
The smoother summarization on the old separator sent by the smoother during the last synchronization.
Definition: ConcurrentIncrementalFilter.h:172
gtsam::FactorGraph::keys
KeySet keys() const
Definition: FactorGraph-inst.h:85
gtsam::Values::equals
bool equals(const Values &other, double tol=1e-9) const
Definition: Values.cpp:77
gtsam::ISAM2Result::variablesRelinearized
size_t variablesRelinearized
Definition: ISAM2Result.h:74
gtsam::FastList
Definition: FastList.h:43
gtsam::ConcurrentIncrementalFilter::calculateFilterSummarization
NonlinearFactorGraph calculateFilterSummarization() const
Definition: ConcurrentIncrementalFilter.cpp:340
gtsam::VariableIndex
Definition: VariableIndex.h:41
gtsam::ConcurrentIncrementalFilter::RecursiveMarkAffectedKeys
static void RecursiveMarkAffectedKeys(const Key &key, const ISAM2Clique::shared_ptr &clique, std::set< Key > &additionalKeys)
Definition: ConcurrentIncrementalFilter.cpp:268
key
const gtsam::Symbol key('X', 0)
gtsam::LinearContainerFactor::shared_ptr
std::shared_ptr< This > shared_ptr
Definition: LinearContainerFactor.h:44
gtsam::ISAM2Clique::shared_ptr
std::shared_ptr< This > shared_ptr
Definition: ISAM2Clique.h:41
gtsam::ConcurrentIncrementalFilter::updateShortcut
void updateShortcut(const NonlinearFactorGraph &removedFactors)
Definition: ConcurrentIncrementalFilter.cpp:312
gtsam::Factor::const_iterator
KeyVector::const_iterator const_iterator
Const iterator over keys.
Definition: Factor.h:83
gtsam::ConcurrentIncrementalFilter::presync
void presync() override
Definition: ConcurrentIncrementalFilter.cpp:167
Values
std::vector< float > Values
Definition: sparse_setter.cpp:45
gtsam
traits
Definition: SFMdata.h:40
gtsam::ISAM2::update
virtual ISAM2Result update(const NonlinearFactorGraph &newFactors=NonlinearFactorGraph(), const Values &newTheta=Values(), const FactorIndices &removeFactorIndices=FactorIndices(), const std::optional< FastMap< Key, int > > &constrainedKeys={}, const std::optional< FastList< Key > > &noRelinKeys={}, const std::optional< FastList< Key > > &extraReelimKeys={}, bool force_relinearize=false)
Definition: ISAM2.cpp:400
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
gtsam::ConcurrentIncrementalFilter::print
void print(const std::string &s="Concurrent Incremental Filter:\n", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Definition: ConcurrentIncrementalFilter.cpp:26
gtsam::Values::clear
void clear()
Definition: Values.h:347
gtsam::Values::insert
void insert(Key j, const Value &val)
Definition: Values.cpp:155
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::ConcurrentIncrementalFilter
Definition: ConcurrentIncrementalFilter.h:30
gtsam::ISAM2Result::variablesReeliminated
size_t variablesReeliminated
Definition: ISAM2Result.h:82
gtsam::ISAM2Result
Definition: ISAM2Result.h:39
gtsam::ISAM2::getFixedVariables
const KeySet & getFixedVariables() const
Definition: ISAM2.h:277
gtsam::LinearContainerFactor
Definition: LinearContainerFactor.h:29
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
gtsam::ConcurrentIncrementalFilter::equals
bool equals(const ConcurrentFilter &rhs, double tol=1e-9) const override
Definition: ConcurrentIncrementalFilter.cpp:32
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::ConcurrentIncrementalFilter::getSmootherFactors
void getSmootherFactors(NonlinearFactorGraph &smootherFactors, Values &smootherValues) override
Definition: ConcurrentIncrementalFilter.cpp:243
gtsam::equal
bool equal(const T &obj1, const T &obj2, double tol)
Definition: Testable.h:85
gttic
#define gttic(label)
Definition: timing.h:295
debug.h
Global debugging flags.
gtsam::FactorIndices
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition: Factor.h:37
gtsam::ISAM2::params
const ISAM2Params & params() const
Definition: ISAM2.h:279


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:09