ConcurrentIncrementalSmoother.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/timing.h>
22 #include <gtsam/base/debug.h>
23 
24 namespace gtsam {
25 
26 /* ************************************************************************* */
27 void ConcurrentIncrementalSmoother::print(const std::string& s, const KeyFormatter& keyFormatter) const {
28  std::cout << s;
29  isam2_.print("");
30 }
31 
32 /* ************************************************************************* */
34  const ConcurrentIncrementalSmoother* smoother = dynamic_cast<const ConcurrentIncrementalSmoother*>(&rhs);
35  return smoother
36  && isam2_.equals(smoother->isam2_)
41  && (filterSummarizationSlots_.size() == smoother->filterSummarizationSlots_.size())
44 }
45 
46 /* ************************************************************************* */
48  const std::optional<FactorIndices>& removeFactorIndices) {
49 
50  gttic(update);
51 
52  // Create the return result meta-data
53  Result result;
54 
55  FastVector<size_t> removedFactors;
56 
57  if(removeFactorIndices){
58  // Be very careful to this line
59  std::cout << "ConcurrentIncrementalSmoother::update removeFactorIndices - not implemented yet" << std::endl;
60  filterSummarizationSlots_.insert(filterSummarizationSlots_.end(), removeFactorIndices->begin(), removeFactorIndices->end());
61  // before it was:
62  // removedFactors.insert(removedFactors.end(), removeFactorIndices->begin(), removeFactorIndices->end());
63  }
64 
65  // Constrain the separator keys to remain in the root
66  // Also, mark the separator keys as fixed linearization points
67  FastMap<Key,int> constrainedKeys;
68  FastList<Key> noRelinKeys;
69  for (const auto key : separatorValues_.keys()) {
70  constrainedKeys[key] = 1;
71  noRelinKeys.push_back(key);
72  }
73 
74  // Use iSAM2 to perform an update
75  ISAM2Result isam2Result;
78  // Augment any new factors/values with the cached data from the last synchronization
79  NonlinearFactorGraph graph(newFactors);
82  Values values(newTheta);
83  // Unfortunately, we must be careful here, as some of the smoother values
84  // and/or separator values may have been added previously
85  for(const auto key: smootherValues_.keys()) {
87  values.insert(key, smootherValues_.at(key));
88  }
89  }
90  for(const auto key: separatorValues_.keys()) {
92  values.insert(key, separatorValues_.at(key));
93  }
94  }
95 
96  // Update the system using iSAM2
97  isam2Result = isam2_.update(graph, values, filterSummarizationSlots_, constrainedKeys, noRelinKeys);
98 
99  // Clear out the cache and update the filter summarization slots
104  isam2Result.newFactorsIndices.end()-filterSummarizationFactors_.size(), isam2Result.newFactorsIndices.end());
107  } else {
108  // Update the system using iSAM2
109  isam2Result = isam2_.update(newFactors, newTheta, FactorIndices(), constrainedKeys, noRelinKeys);
110  }
111  }
112 
113  // Extract the ConcurrentIncrementalSmoother::Result information
114  result.iterations = 1;
118 
119  // Calculate the marginal on the separator from the smoother factors
120  if(separatorValues_.size() > 0) {
121  gttic(presync);
123  gttoc(presync);
124  }
125 
126  gttoc(update);
127 
128  return result;
129 }
130 
131 /* ************************************************************************* */
133 
134  gttic(presync);
135 
136  gttoc(presync);
137 }
138 
139 /* ************************************************************************* */
141 
142  gttic(get_summarized_factors);
143 
144  // Copy the previous calculated smoother summarization factors into the output
145  summarizedFactors.push_back(smootherSummarization_);
146 
147  // Copy the separator values into the output
148  separatorValues.insert(separatorValues_);
149 
150  gttoc(get_summarized_factors);
151 }
152 
153 /* ************************************************************************* */
154 void ConcurrentIncrementalSmoother::synchronize(const NonlinearFactorGraph& smootherFactors, const Values& smootherValues,
155  const NonlinearFactorGraph& summarizedFactors, const Values& separatorValues) {
156 
158 
159  // Store the new smoother factors and values for addition during the next update call
160  smootherFactors_ = smootherFactors;
161  smootherValues_ = smootherValues;
162 
163  // Store the new filter summarization and separator, to be replaced during the next update call
164  filterSummarizationFactors_ = summarizedFactors;
165  separatorValues_ = separatorValues;
166 
167  // Flag the next smoother update to include the synchronization data
169 
171 }
172 
173 /* ************************************************************************* */
175 
176  gttic(postsync);
177 
178  gttoc(postsync);
179 }
180 
181 /* ************************************************************************* */
183 
184  // The smoother summarization factors are the resulting marginal factors on the separator
185  // variables that result from marginalizing out all of the other variables
186  // These marginal factors will be cached for later transmission to the filter using
187  // linear container factors
188 
189  // Find all cliques that contain any separator variables
190  std::set<ISAM2Clique::shared_ptr> separatorCliques;
191  for(Key key: separatorValues_.keys()) {
193  separatorCliques.insert( clique );
194  }
195 
196  // Create the set of clique keys LC:
197  KeyVector cliqueKeys;
198  for(const ISAM2Clique::shared_ptr& clique: separatorCliques) {
199  for(Key key: clique->conditional()->frontals()) {
200  cliqueKeys.push_back(key);
201  }
202  }
203  std::sort(cliqueKeys.begin(), cliqueKeys.end());
204 
205  // Gather all factors that involve only clique keys
206  std::set<size_t> cliqueFactorSlots;
207  for(Key key: cliqueKeys) {
208  for(size_t slot: isam2_.getVariableIndex()[key]) {
209  const NonlinearFactor::shared_ptr& factor = isam2_.getFactorsUnsafe().at(slot);
210  if(factor) {
211  std::set<Key> factorKeys(factor->begin(), factor->end());
212  if(std::includes(cliqueKeys.begin(), cliqueKeys.end(), factorKeys.begin(), factorKeys.end())) {
213  cliqueFactorSlots.insert(slot);
214  }
215  }
216  }
217  }
218 
219  // Remove any factor included in the filter summarization
220  for(size_t slot: filterSummarizationSlots_) {
221  cliqueFactorSlots.erase(slot);
222  }
223 
224  // Create a factor graph from the identified factors
226  for(size_t slot: cliqueFactorSlots) {
227  graph.push_back(isam2_.getFactorsUnsafe().at(slot));
228  }
229 
230  // Find the set of children of the separator cliques
231  std::set<ISAM2Clique::shared_ptr> childCliques;
232  // Add all of the children
233  for(const ISAM2Clique::shared_ptr& clique: separatorCliques) {
234  childCliques.insert(clique->children.begin(), clique->children.end());
235  }
236  // Remove any separator cliques that were added because they were children of other separator cliques
237  for(const ISAM2Clique::shared_ptr& clique: separatorCliques) {
238  childCliques.erase(clique);
239  }
240 
241  // Augment the factor graph with cached factors from the children
242  for(const ISAM2Clique::shared_ptr& clique: childCliques) {
244  graph.push_back( factor );
245  }
246 
247  // Get the set of separator keys
248  const KeySet separatorKeys = separatorValues_.keySet();
249 
250  // Calculate the marginal factors on the separator
253 }
254 
255 /* ************************************************************************* */
256 
257 }
const gtsam::Symbol key('X', 0)
size_t linearVariables
The number of variables that must keep a constant linearization point.
Values smootherValues_
New variables to be added to the smoother during the next update.
void clear()
Definition: Values.h:298
size_t iterations
The number of optimizer iterations performed.
Wrap Jacobian and Hessian linear factors to allow simple injection into a nonlinear graph...
Global debugging flags.
const ValueType at(Key j) const
Definition: Values-inl.h:204
FactorIndices filterSummarizationSlots_
The slots in factor graph that correspond to the current filter summarization factors.
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:190
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
Definition: FastVector.h:34
size_t nonlinearVariables
The number of variables that can be relinearized.
leaf::MyValues values
KeyVector keys() const
Definition: Values.cpp:218
NonlinearFactorGraph smootherSummarization_
A temporary holding place for calculated smoother summarization.
void print(const std::string &s="Concurrent Incremental Smoother:\, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
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
An iSAM2-based Smoother that implements the Concurrent Filtering and Smoothing interface.
virtual bool equals(const ISAM2 &other, double tol=1e-9) const
Definition: ISAM2.cpp:57
NonlinearFactorGraph graph
NonlinearFactorGraph smootherFactors_
New factors to be added to the smoother during the next update.
void getSummarizedFactors(NonlinearFactorGraph &summarizedFactors, Values &separatorValues) override
const Values & getLinearizationPoint() const
Access the current linearization point.
Definition: ISAM2.h:217
FactorIndices newFactorsIndices
Definition: ISAM2Result.h:95
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition: Factor.h:36
size_t size() const
Definition: FactorGraph.h:334
std::shared_ptr< This > shared_ptr
Definition: ISAM2Clique.h:41
#define gttic(label)
Definition: timing.h:295
const NonlinearFactorGraph & getFactorsUnsafe() const
Definition: ISAM2.h:269
bool equals(const ConcurrentSmoother &rhs, double tol=1e-9) const override
void synchronize(const NonlinearFactorGraph &smootherFactors, const Values &smootherValues, const NonlinearFactorGraph &summarizedFactors, const Values &separatorValues) override
Values result
GaussianFactorGraph::Eliminate getEliminationFunction() const
Definition: ISAM2Params.h:323
std::shared_ptr< This > shared_ptr
NonlinearFactorGraph calculateMarginalFactors(const NonlinearFactorGraph &graph, const Values &theta, const KeySet &remainingKeys, const GaussianFactorGraph::Eliminate &eliminateFunction)
Result update(const NonlinearFactorGraph &newFactors=NonlinearFactorGraph(), const Values &newTheta=Values(), const std::optional< FactorIndices > &removeFactorIndices={})
RealScalar s
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
size_t size() const
Definition: Values.h:178
bool equals(const Values &other, double tol=1e-9) const
Definition: Values.cpp:77
bool equals(const NonlinearFactorGraph &other, double tol=1e-9) const
double error(const Values &values) const
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
traits
Definition: chartTesting.h:28
NonlinearFactorGraph filterSummarizationFactors_
New filter summarization factors to replace the existing filter summarization during the next update...
Values separatorValues_
The linearization points of the separator variables. These should not be changed during optimization...
virtual void resize(size_t size)
Definition: FactorGraph.h:389
#define gttoc(label)
Definition: timing.h:296
const sharedFactor at(size_t i) const
Definition: FactorGraph.h:343
std::shared_ptr< This > shared_ptr
const VariableIndex & getVariableIndex() const
Definition: ISAM2.h:274
void insert(Key j, const Value &val)
Definition: Values.cpp:155
const G double tol
Definition: Group.h:86
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
bool exists(Key j) const
Definition: Values.cpp:93
bool equal(const T &obj1, const T &obj2, double tol)
Definition: Testable.h:85
bool synchronizationUpdatesAvailable_
Flag indicating the currently stored synchronization updates have not been applied yet...
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
KeySet keySet() const
Definition: Values.cpp:227
Values calculateEstimate() const
Definition: ISAM2.cpp:766
const ISAM2Params & params() const
Definition: ISAM2.h:279
Timing utilities.


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