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);
80  graph.push_back(smootherFactors_);
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;
115  result.linearVariables = separatorValues_.size();
116  result.nonlinearVariables = isam2_.getLinearizationPoint().size() - separatorValues_.size();
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 }
gttoc
#define gttoc(label)
Definition: timing.h:296
timing.h
Timing utilities.
gtsam::ConcurrentIncrementalSmoother::smootherValues_
Values smootherValues_
New variables to be added to the smoother during the next update.
Definition: ConcurrentIncrementalSmoother.h:152
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::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::ConcurrentIncrementalSmoother::filterSummarizationFactors_
NonlinearFactorGraph filterSummarizationFactors_
New filter summarization factors to replace the existing filter summarization during the next update.
Definition: ConcurrentIncrementalSmoother.h:153
gtsam::Values::keys
KeyVector keys() const
Definition: Values.cpp:218
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.
gtsam::ConcurrentIncrementalSmoother::update
Result update(const NonlinearFactorGraph &newFactors=NonlinearFactorGraph(), const Values &newTheta=Values(), const std::optional< FactorIndices > &removeFactorIndices={})
Definition: ConcurrentIncrementalSmoother.cpp:47
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
Definition: FastVector.h:34
gtsam::ISAM2::getFactorsUnsafe
const NonlinearFactorGraph & getFactorsUnsafe() const
Definition: ISAM2.h:269
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::ConcurrentIncrementalSmoother
Definition: ConcurrentIncrementalSmoother.h:30
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::ConcurrentIncrementalSmoother::postsync
void postsync() override
Definition: ConcurrentIncrementalSmoother.cpp:174
gtsam::ISAM2::equals
virtual bool equals(const ISAM2 &other, double tol=1e-9) const
Definition: ISAM2.cpp:57
gtsam::ConcurrentIncrementalSmoother::updateSmootherSummarization
void updateSmootherSummarization()
Definition: ConcurrentIncrementalSmoother.cpp:182
gtsam::FactorGraph::at
const sharedFactor at(size_t i) const
Definition: FactorGraph.h:306
gtsam::ConcurrentIncrementalSmoother::synchronizationUpdatesAvailable_
bool synchronizationUpdatesAvailable_
Flag indicating the currently stored synchronization updates have not been applied yet.
Definition: ConcurrentIncrementalSmoother.h:156
gtsam::ISAM2::getLinearizationPoint
const Values & getLinearizationPoint() const
Access the current linearization point.
Definition: ISAM2.h:217
gtsam::ConcurrentIncrementalSmoother::Result
Definition: ConcurrentIncrementalSmoother.h:37
gtsam::NonlinearFactorGraph::error
double error(const Values &values) const
Definition: NonlinearFactorGraph.cpp:170
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::ConcurrentIncrementalSmoother::separatorValues_
Values separatorValues_
The linearization points of the separator variables. These should not be changed during optimization.
Definition: ConcurrentIncrementalSmoother.h:154
gtsam::internal::calculateMarginalFactors
NonlinearFactorGraph calculateMarginalFactors(const NonlinearFactorGraph &graph, const Values &theta, const KeySet &remainingKeys, const GaussianFactorGraph::Eliminate &eliminateFunction)
Definition: ConcurrentFilteringAndSmoothing.cpp:54
gtsam::ConcurrentIncrementalSmoother::smootherSummarization_
NonlinearFactorGraph smootherSummarization_
A temporary holding place for calculated smoother summarization.
Definition: ConcurrentIncrementalSmoother.h:159
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
gtsam::ConcurrentSmoother
Definition: ConcurrentFilteringAndSmoothing.h:101
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
gtsam::ConcurrentIncrementalSmoother::isam2_
ISAM2 isam2_
iSAM2 inference engine
Definition: ConcurrentIncrementalSmoother.h:148
gtsam::Values::equals
bool equals(const Values &other, double tol=1e-9) const
Definition: Values.cpp:77
gtsam::ConcurrentIncrementalSmoother::equals
bool equals(const ConcurrentSmoother &rhs, double tol=1e-9) const override
Definition: ConcurrentIncrementalSmoother.cpp:33
gtsam::FastList
Definition: FastList.h:43
gtsam::ConcurrentIncrementalSmoother::getSummarizedFactors
void getSummarizedFactors(NonlinearFactorGraph &summarizedFactors, Values &separatorValues) override
Definition: ConcurrentIncrementalSmoother.cpp:140
key
const gtsam::Symbol key('X', 0)
gtsam::ISAM2::calculateEstimate
Values calculateEstimate() const
Definition: ISAM2.cpp:785
gtsam::FactorGraph::size
size_t size() const
Definition: FactorGraph.h:297
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::ConcurrentIncrementalSmoother::filterSummarizationSlots_
FactorIndices filterSummarizationSlots_
The slots in factor graph that correspond to the current filter summarization factors.
Definition: ConcurrentIncrementalSmoother.h:155
gtsam::Values::keySet
KeySet keySet() const
Definition: Values.cpp:227
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::Values::clear
void clear()
Definition: Values.h:347
gtsam::Values::insert
void insert(Key j, const Value &val)
Definition: Values.cpp:155
gtsam::ConcurrentIncrementalSmoother::smootherFactors_
NonlinearFactorGraph smootherFactors_
New factors to be added to the smoother during the next update.
Definition: ConcurrentIncrementalSmoother.h:151
gtsam::ConcurrentIncrementalSmoother::synchronize
void synchronize(const NonlinearFactorGraph &smootherFactors, const Values &smootherValues, const NonlinearFactorGraph &summarizedFactors, const Values &separatorValues) override
Definition: ConcurrentIncrementalSmoother.cpp:154
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::ISAM2Result
Definition: ISAM2Result.h:39
gtsam::LinearContainerFactor
Definition: LinearContainerFactor.h:29
ConcurrentIncrementalSmoother.h
An iSAM2-based Smoother that implements the Concurrent Filtering and Smoothing interface.
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
gtsam::ConcurrentIncrementalSmoother::presync
void presync() override
Definition: ConcurrentIncrementalSmoother.cpp:132
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::equal
bool equal(const T &obj1, const T &obj2, double tol)
Definition: Testable.h:85
gtsam::ConcurrentIncrementalSmoother::print
void print(const std::string &s="Concurrent Incremental Smoother:\n", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Definition: ConcurrentIncrementalSmoother.cpp:27
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