FixedLagSmoother.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 
22 namespace gtsam {
23 
24 /* ************************************************************************* */
26  std::cout << "Nr iterations: " << iterations << '\n'
27  << "Nr intermediateSteps: " << intermediateSteps << '\n'
28  << "Nr nonlinear variables: " << nonlinearVariables << '\n'
29  << "Nr linear variables: " << linearVariables << '\n'
30  << "error: " << error << std::endl;
31 }
32 
33 /* ************************************************************************* */
34 void FixedLagSmoother::print(const std::string& s, const KeyFormatter& keyFormatter) const {
35  std::cout << s;
36  std::cout << " smoother lag: " << smootherLag_ << std::endl;
37 }
38 
39 /* ************************************************************************* */
40 bool FixedLagSmoother::equals(const FixedLagSmoother& rhs, double tol) const {
41  return std::abs(smootherLag_ - rhs.smootherLag_) < tol
42  && std::equal(timestampKeyMap_.begin(), timestampKeyMap_.end(), rhs.timestampKeyMap_.begin());
43 }
44 
45 /* ************************************************************************* */
47  // Loop through each key and add/update it in the map
48  for(const auto& key_timestamp: timestamps) {
49  // Check to see if this key already exists in the database
50  KeyTimestampMap::iterator keyIter = keyTimestampMap_.find(key_timestamp.first);
51 
52  // If the key already exists
53  if(keyIter != keyTimestampMap_.end()) {
54  // Find the entry in the Timestamp-Key database
55  std::pair<TimestampKeyMap::iterator,TimestampKeyMap::iterator> range = timestampKeyMap_.equal_range(keyIter->second);
56  TimestampKeyMap::iterator timeIter = range.first;
57  while(timeIter->second != key_timestamp.first) {
58  ++timeIter;
59  }
60  // remove the entry in the Timestamp-Key database
61  timestampKeyMap_.erase(timeIter);
62  // insert an entry at the new time
63  timestampKeyMap_.insert(TimestampKeyMap::value_type(key_timestamp.second, key_timestamp.first));
64  // update the Key-Timestamp database
65  keyIter->second = key_timestamp.second;
66  } else {
67  // Add the Key-Timestamp database
68  keyTimestampMap_.insert(key_timestamp);
69  // Add the key to the Timestamp-Key database
70  timestampKeyMap_.insert(TimestampKeyMap::value_type(key_timestamp.second, key_timestamp.first));
71  }
72  }
73 }
74 
75 /* ************************************************************************* */
77  for(Key key: keys) {
78  // Erase the key from the Timestamp->Key map
79  double timestamp = keyTimestampMap_.at(key);
80 
81  TimestampKeyMap::iterator iter = timestampKeyMap_.lower_bound(timestamp);
82  while(iter != timestampKeyMap_.end() && iter->first == timestamp) {
83  if(iter->second == key) {
84  timestampKeyMap_.erase(iter++);
85  } else {
86  ++iter;
87  }
88  }
89  // Erase the key from the Key->Timestamp map
90  keyTimestampMap_.erase(key);
91  }
92 }
93 
94 /* ************************************************************************* */
96  if(timestampKeyMap_.size() > 0) {
97  return timestampKeyMap_.rbegin()->first;
98  } else {
100  }
101 }
102 
103 /* ************************************************************************* */
105  KeyVector keys;
106  TimestampKeyMap::const_iterator end = timestampKeyMap_.lower_bound(timestamp);
107  for(TimestampKeyMap::const_iterator iter = timestampKeyMap_.begin(); iter != end; ++iter) {
108  keys.push_back(iter->second);
109  }
110  return keys;
111 }
112 
113 /* ************************************************************************* */
115  KeyVector keys;
116  TimestampKeyMap::const_iterator begin = timestampKeyMap_.upper_bound(timestamp);
117  for(TimestampKeyMap::const_iterator iter = begin; iter != timestampKeyMap_.end(); ++iter) {
118  keys.push_back(iter->second);
119  }
120  return keys;
121 }
122 
123 /* ************************************************************************* */
124 }
const gtsam::Symbol key('X', 0)
void updateKeyTimestampMap(const KeyTimestampMap &newTimestamps)
#define max(a, b)
Definition: datatypes.h:20
size_t iterations
The number of optimizer iterations performed.
std::map< Key, double > KeyTimestampMap
Typedef for a Key-Timestamp map/database.
size_t linearVariables
The number of variables that must keep a constant linearization point.
double error
The final factor graph error.
double getCurrentTimestamp() const
iterator iter(handle obj)
Definition: pytypes.h:2273
size_t intermediateSteps
The number of intermediate steps performed within the optimization. For L-M, this is the number of la...
void eraseKeyTimestampMap(const KeyVector &keys)
virtual void print(const std::string &s="FixedLagSmoother:\, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
RealScalar s
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
size_t nonlinearVariables
The number of variables that can be relinearized.
KeyVector findKeysAfter(double timestamp) const
traits
Definition: chartTesting.h:28
const KeyTimestampMap & timestamps() const
Double_ range(const Point2_ &p, const Point2_ &q)
static EIGEN_DEPRECATED const end_t end
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
bool equal(const T &obj1, const T &obj2, double tol)
Definition: Testable.h:85
#define abs(x)
Definition: datatypes.h:17
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:13