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
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 }
gtsam::recursiveMarkAffectedKeys
void recursiveMarkAffectedKeys(const Key &key, const ISAM2Clique::shared_ptr &clique, std::set< Key > &additionalKeys)
Definition: IncrementalFixedLagSmoother.cpp:28
gtsam::ISAM2
Definition: ISAM2.h:45
gtsam::ISAM2::marginalizeLeaves
void marginalizeLeaves(const FastList< Key > &leafKeys, FactorIndices *marginalFactorsIndices=nullptr, FactorIndices *deletedFactorsIndices=nullptr)
Definition: ISAM2.cpp:486
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
keys
const KeyVector keys
Definition: testRegularImplicitSchurFactor.cpp:40
gtsam::IncrementalFixedLagSmoother::PrintKeySet
static void PrintKeySet(const std::set< Key > &keys, const std::string &label="Keys:")
Definition: IncrementalFixedLagSmoother.cpp:194
gtsam::IncrementalFixedLagSmoother::isam_
ISAM2 isam_
Definition: IncrementalFixedLagSmoother.h:131
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
Definition: FastVector.h:34
gtsam::IncrementalFixedLagSmoother::equals
bool equals(const FixedLagSmoother &rhs, double tol=1e-9) const override
Definition: IncrementalFixedLagSmoother.cpp:57
gtsam::FixedLagSmoother::timestamps
const KeyTimestampMap & timestamps() const
Definition: nonlinear/FixedLagSmoother.h:90
gtsam::FastMap
Definition: FastMap.h:39
gtsam::FixedLagSmoother::eraseKeyTimestampMap
void eraseKeyTimestampMap(const KeyVector &keys)
Definition: FixedLagSmoother.cpp:76
gtsam::IncrementalFixedLagSmoother::PrintSymbolicTreeHelper
static void PrintSymbolicTreeHelper(const gtsam::ISAM2Clique::shared_ptr &clique, const std::string indent="")
Definition: IncrementalFixedLagSmoother.cpp:235
gtsam::IncrementalFixedLagSmoother::PrintSymbolicFactor
static void PrintSymbolicFactor(const GaussianFactor::shared_ptr &factor)
Definition: IncrementalFixedLagSmoother.cpp:204
gtsam::ISAM2::sharedClique
Base::sharedClique sharedClique
Shared pointer to a clique.
Definition: ISAM2.h:103
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::ISAM2::equals
virtual bool equals(const ISAM2 &other, double tol=1e-9) const
Definition: ISAM2.cpp:57
gtsam::DefaultKeyFormatter
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
Definition: Key.cpp:30
gtsam::FixedLagSmoother::timestampKeyMap_
TimestampKeyMap timestampKeyMap_
Definition: nonlinear/FixedLagSmoother.h:113
gtsam::GaussianFactorGraph
Definition: GaussianFactorGraph.h:73
ISDEBUG
#define ISDEBUG(S)
Definition: debug.h:60
gtsam::IncrementalFixedLagSmoother::isamResult_
ISAM2Result isamResult_
Definition: IncrementalFixedLagSmoother.h:134
gtsam::FixedLagSmoother
Definition: nonlinear/FixedLagSmoother.h:33
gtsam::IncrementalFixedLagSmoother::update
Result update(const NonlinearFactorGraph &newFactors=NonlinearFactorGraph(), const Values &newTheta=Values(), const KeyTimestampMap &timestamps=KeyTimestampMap(), const FactorIndices &factorsToRemove=FactorIndices()) override
Definition: IncrementalFixedLagSmoother.cpp:66
gtsam::IncrementalFixedLagSmoother
Definition: IncrementalFixedLagSmoother.h:34
gtsam::FixedLagSmoother::smootherLag_
double smootherLag_
Definition: nonlinear/FixedLagSmoother.h:110
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::IncrementalFixedLagSmoother::print
void print(const std::string &s="IncrementalFixedLagSmoother:\n", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Definition: IncrementalFixedLagSmoother.cpp:50
gtsam::FixedLagSmoother::Result
Definition: nonlinear/FixedLagSmoother.h:48
isam
NonlinearISAM isam(relinearizeInterval)
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
debug
static constexpr bool debug
Definition: testDiscreteBayesTree.cpp:31
gtsam::GaussianFactor::shared_ptr
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactor.h:42
gtsam::FixedLagSmoother::getCurrentTimestamp
double getCurrentTimestamp() const
Definition: FixedLagSmoother.cpp:95
gtsam::FastList
Definition: FastList.h:43
key
const gtsam::Symbol key('X', 0)
IncrementalFixedLagSmoother.h
An iSAM2-based fixed-lag smoother.
gtsam::ISAM2Clique::shared_ptr
std::shared_ptr< This > shared_ptr
Definition: ISAM2Clique.h:41
gtsam::FixedLagSmoother::equals
virtual bool equals(const FixedLagSmoother &rhs, double tol=1e-9) const
Definition: FixedLagSmoother.cpp:40
gtsam::IncrementalFixedLagSmoother::PrintSymbolicTree
static void PrintSymbolicTree(const gtsam::ISAM2 &isam, const std::string &label="Bayes Tree:")
Definition: IncrementalFixedLagSmoother.cpp:223
gtsam::FixedLagSmoother::KeyTimestampMap
std::map< Key, double > KeyTimestampMap
Typedef for a Key-Timestamp map/database.
Definition: nonlinear/FixedLagSmoother.h:41
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::Values
Definition: Values.h:65
iter
iterator iter(handle obj)
Definition: pytypes.h:2475
gtsam::IncrementalFixedLagSmoother::eraseKeysBefore
void eraseKeysBefore(double timestamp)
Definition: IncrementalFixedLagSmoother.cpp:166
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::FixedLagSmoother::keyTimestampMap_
KeyTimestampMap keyTimestampMap_
Definition: nonlinear/FixedLagSmoother.h:114
Eigen::placeholders::end
static const EIGEN_DEPRECATED end_t end
Definition: IndexedViewHelper.h:181
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
gtsam::IncrementalFixedLagSmoother::createOrderingConstraints
void createOrderingConstraints(const KeyVector &marginalizableKeys, std::optional< FastMap< Key, int > > &constrainedKeys) const
Definition: IncrementalFixedLagSmoother.cpp:176
gtsam::FixedLagSmoother::findKeysBefore
KeyVector findKeysBefore(double timestamp) const
Definition: FixedLagSmoother.cpp:104
gtsam::FixedLagSmoother::print
virtual void print(const std::string &s="FixedLagSmoother:\n", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Definition: FixedLagSmoother.cpp:34
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
gtsam::FixedLagSmoother::updateKeyTimestampMap
void updateKeyTimestampMap(const KeyTimestampMap &newTimestamps)
Definition: FixedLagSmoother.cpp:46
debug.h
Global debugging flags.
gtsam::FactorIndices
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition: Factor.h:37
gtsam::IncrementalFixedLagSmoother::PrintSymbolicGraph
static void PrintSymbolicGraph(const GaussianFactorGraph &graph, const std::string &label="Factor Graph:")
Definition: IncrementalFixedLagSmoother.cpp:214


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:02:27