IncrementalFixedLagSmoother.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 
23 #include <gtsam/base/debug.h>
24 
25 namespace gtsam {
26 
27 /* ************************************************************************* */
29  const ISAM2Clique::shared_ptr& clique, std::set<Key>& additionalKeys) {
30 
31  // Check if the separator keys of the current clique contain the specified key
32  if (std::find(clique->conditional()->beginParents(),
33  clique->conditional()->endParents(), key)
34  != clique->conditional()->endParents()) {
35 
36  // Mark the frontal keys of the current clique
37  for(Key i: clique->conditional()->frontals()) {
38  additionalKeys.insert(i);
39  }
40 
41  // Recursively mark all of the children
42  for(const ISAM2Clique::shared_ptr& child: clique->children) {
43  recursiveMarkAffectedKeys(key, child, additionalKeys);
44  }
45  }
46  // If the key was not found in the separator/parents, then none of its children can have it either
47 }
48 
49 /* ************************************************************************* */
50 void IncrementalFixedLagSmoother::print(const std::string& s,
51  const KeyFormatter& keyFormatter) const {
52  FixedLagSmoother::print(s, keyFormatter);
53  // TODO: What else to print?
54 }
55 
56 /* ************************************************************************* */
58  double tol) const {
60  dynamic_cast<const IncrementalFixedLagSmoother*>(&rhs);
61  return e != nullptr && FixedLagSmoother::equals(*e, tol)
62  && isam_.equals(e->isam_, tol);
63 }
64 
65 /* ************************************************************************* */
67  const NonlinearFactorGraph& newFactors, const Values& newTheta,
68  const KeyTimestampMap& timestamps, const FactorIndices& factorsToRemove) {
69 
70  const bool debug = ISDEBUG("IncrementalFixedLagSmoother update");
71 
72  if (debug) {
73  std::cout << "IncrementalFixedLagSmoother::update() Start" << std::endl;
74  PrintSymbolicTree(isam_, "Bayes Tree Before Update:");
75  std::cout << "END" << std::endl;
76  }
77 
78  FastVector<size_t> removedFactors;
79  std::optional<FastMap<Key, int> > constrainedKeys = {};
80 
81  // Update the Timestamps associated with the factor keys
82  updateKeyTimestampMap(timestamps);
83 
84  // Get current timestamp
85  double current_timestamp = getCurrentTimestamp();
86 
87  if (debug)
88  std::cout << "Current Timestamp: " << current_timestamp << std::endl;
89 
90  // Find the set of variables to be marginalized out
91  KeyVector marginalizableKeys = findKeysBefore(
92  current_timestamp - smootherLag_);
93 
94  if (debug) {
95  std::cout << "Marginalizable Keys: ";
96  for(Key key: marginalizableKeys) {
97  std::cout << DefaultKeyFormatter(key) << " ";
98  }
99  std::cout << std::endl;
100  }
101 
102  // Force iSAM2 to put the marginalizable variables at the beginning
103  createOrderingConstraints(marginalizableKeys, constrainedKeys);
104 
105  if (debug) {
106  std::cout << "Constrained Keys: ";
107  if (constrainedKeys) {
108  for (FastMap<Key, int>::const_iterator iter = constrainedKeys->begin();
109  iter != constrainedKeys->end(); ++iter) {
110  std::cout << DefaultKeyFormatter(iter->first) << "(" << iter->second
111  << ") ";
112  }
113  }
114  std::cout << std::endl;
115  }
116 
117  // Mark additional keys between the marginalized keys and the leaves
118  std::set<Key> additionalKeys;
119  for(Key key: marginalizableKeys) {
121  for(const ISAM2Clique::shared_ptr& child: clique->children) {
122  recursiveMarkAffectedKeys(key, child, additionalKeys);
123  }
124  }
125  KeyList additionalMarkedKeys(additionalKeys.begin(), additionalKeys.end());
126 
127  // Update iSAM2
128  isamResult_ = isam_.update(newFactors, newTheta,
129  factorsToRemove, constrainedKeys, {}, additionalMarkedKeys);
130 
131  if (debug) {
133  "Bayes Tree After Update, Before Marginalization:");
134  std::cout << "END" << std::endl;
135  }
136 
137  // Marginalize out any needed variables
138  if (marginalizableKeys.size() > 0) {
139  FastList<Key> leafKeys(marginalizableKeys.begin(),
140  marginalizableKeys.end());
141  isam_.marginalizeLeaves(leafKeys);
142  }
143 
144  // Remove marginalized keys from the KeyTimestampMap
145  eraseKeyTimestampMap(marginalizableKeys);
146 
147  if (debug) {
148  PrintSymbolicTree(isam_, "Final Bayes Tree:");
149  std::cout << "END" << std::endl;
150  }
151 
152  // TODO: Fill in result structure
153  Result result;
154  result.iterations = 1;
155  result.linearVariables = 0;
156  result.nonlinearVariables = 0;
157  result.error = 0;
158 
159  if (debug)
160  std::cout << "IncrementalFixedLagSmoother::update() Finish" << std::endl;
161 
162  return result;
163 }
164 
165 /* ************************************************************************* */
167  TimestampKeyMap::iterator end = timestampKeyMap_.lower_bound(timestamp);
168  TimestampKeyMap::iterator iter = timestampKeyMap_.begin();
169  while (iter != end) {
170  keyTimestampMap_.erase(iter->second);
171  timestampKeyMap_.erase(iter++);
172  }
173 }
174 
175 /* ************************************************************************* */
177  const KeyVector& marginalizableKeys,
178  std::optional<FastMap<Key, int> >& constrainedKeys) const {
179  if (marginalizableKeys.size() > 0) {
180  constrainedKeys = FastMap<Key, int>();
181  // Generate ordering constraints so that the marginalizable variables will be eliminated first
182  // Set all variables to Group1
183  for(const TimestampKeyMap::value_type& timestamp_key: timestampKeyMap_) {
184  constrainedKeys->operator[](timestamp_key.second) = 1;
185  }
186  // Set marginalizable variables to Group0
187  for(Key key: marginalizableKeys) {
188  constrainedKeys->operator[](key) = 0;
189  }
190  }
191 }
192 
193 /* ************************************************************************* */
195  const std::string& label) {
196  std::cout << label;
197  for(Key key: keys) {
198  std::cout << " " << DefaultKeyFormatter(key);
199  }
200  std::cout << std::endl;
201 }
202 
203 /* ************************************************************************* */
205  const GaussianFactor::shared_ptr& factor) {
206  std::cout << "f(";
207  for(Key key: factor->keys()) {
208  std::cout << " " << DefaultKeyFormatter(key);
209  }
210  std::cout << " )" << std::endl;
211 }
212 
213 /* ************************************************************************* */
215  const GaussianFactorGraph& graph, const std::string& label) {
216  std::cout << label << std::endl;
217  for(const GaussianFactor::shared_ptr& factor: graph) {
218  PrintSymbolicFactor(factor);
219  }
220 }
221 
222 /* ************************************************************************* */
224  const std::string& label) {
225  std::cout << label << std::endl;
226  if (!isam.roots().empty()) {
227  for(const ISAM2::sharedClique& root: isam.roots()) {
229  }
230  } else
231  std::cout << "{Empty Tree}" << std::endl;
232 }
233 
234 /* ************************************************************************* */
236  const ISAM2Clique::shared_ptr& clique, const std::string indent) {
237 
238  // Print the current clique
239  std::cout << indent << "P( ";
240  for(Key key: clique->conditional()->frontals()) {
241  std::cout << DefaultKeyFormatter(key) << " ";
242  }
243  if (clique->conditional()->nrParents() > 0)
244  std::cout << "| ";
245  for(Key key: clique->conditional()->parents()) {
246  std::cout << DefaultKeyFormatter(key) << " ";
247  }
248  std::cout << ")" << std::endl;
249 
250  // Recursively print all of the children
251  for(const ISAM2Clique::shared_ptr& child: clique->children) {
252  PrintSymbolicTreeHelper(child, indent + " ");
253  }
254 }
255 
256 /* ************************************************************************* */
257 }
const gtsam::Symbol key('X', 0)
void updateKeyTimestampMap(const KeyTimestampMap &newTimestamps)
static void PrintSymbolicFactor(const GaussianFactor::shared_ptr &factor)
size_t iterations
The number of optimizer iterations performed.
void print(const std::string &s="IncrementalFixedLagSmoother:\, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
std::map< Key, double > KeyTimestampMap
Typedef for a Key-Timestamp map/database.
bool equals(const FixedLagSmoother &rhs, double tol=1e-9) const override
size_t linearVariables
The number of variables that must keep a constant linearization point.
Global debugging flags.
double error
The final factor graph error.
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
Definition: FastVector.h:34
double getCurrentTimestamp() const
iterator iter(handle obj)
Definition: pytypes.h:2273
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
virtual bool equals(const ISAM2 &other, double tol=1e-9) const
Definition: ISAM2.cpp:57
NonlinearFactorGraph graph
static const KeyFormatter DefaultKeyFormatter
Definition: Key.h:43
static constexpr bool debug
void createOrderingConstraints(const KeyVector &marginalizableKeys, std::optional< FastMap< Key, int > > &constrainedKeys) const
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition: Factor.h:36
void eraseKeyTimestampMap(const KeyVector &keys)
std::shared_ptr< This > shared_ptr
Definition: ISAM2Clique.h:41
virtual void print(const std::string &s="FixedLagSmoother:\, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Result update(const NonlinearFactorGraph &newFactors=NonlinearFactorGraph(), const Values &newTheta=Values(), const KeyTimestampMap &timestamps=KeyTimestampMap(), const FactorIndices &factorsToRemove=FactorIndices()) override
#define ISDEBUG(S)
Definition: debug.h:60
Values result
An iSAM2-based fixed-lag smoother.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
RealScalar s
NonlinearISAM isam(relinearizeInterval)
Base::sharedClique sharedClique
Shared pointer to a clique.
Definition: ISAM2.h:103
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
void marginalizeLeaves(const FastList< Key > &leafKeys, FactorIndices *marginalFactorsIndices=nullptr, FactorIndices *deletedFactorsIndices=nullptr)
Definition: ISAM2.cpp:483
std::shared_ptr< This > shared_ptr
shared_ptr to this class
size_t nonlinearVariables
The number of variables that can be relinearized.
void recursiveMarkAffectedKeys(const Key &key, const ISAM2Clique::shared_ptr &clique, std::set< Key > &additionalKeys)
traits
Definition: chartTesting.h:28
const KeyTimestampMap & timestamps() const
static void PrintSymbolicTreeHelper(const gtsam::ISAM2Clique::shared_ptr &clique, const std::string indent="")
static void PrintSymbolicTree(const gtsam::ISAM2 &isam, const std::string &label="Bayes Tree:")
const Roots & roots() const
Definition: BayesTree.h:152
static void PrintKeySet(const std::set< Key > &keys, const std::string &label="Keys:")
static EIGEN_DEPRECATED const end_t end
static void PrintSymbolicGraph(const GaussianFactorGraph &graph, const std::string &label="Factor Graph:")
const G double tol
Definition: Group.h:86
const KeyVector keys
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
virtual bool equals(const FixedLagSmoother &rhs, double tol=1e-9) const
KeyVector findKeysBefore(double timestamp) const


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