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 }
s
RealScalar s
Definition: level1_cplx_impl.h:126
keys
const KeyVector keys
Definition: testRegularImplicitSchurFactor.cpp:40
gtsam::FixedLagSmoother::timestamps
const KeyTimestampMap & timestamps() const
Definition: nonlinear/FixedLagSmoother.h:90
gtsam::FixedLagSmoother::eraseKeyTimestampMap
void eraseKeyTimestampMap(const KeyVector &keys)
Definition: FixedLagSmoother.cpp:76
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:92
gtsam::FixedLagSmoother::Result::linearVariables
size_t linearVariables
The number of variables that must keep a constant linearization point.
Definition: nonlinear/FixedLagSmoother.h:52
gtsam::range
Double_ range(const Point2_ &p, const Point2_ &q)
Definition: slam/expressions.h:30
gtsam::FixedLagSmoother::timestampKeyMap_
TimestampKeyMap timestampKeyMap_
Definition: nonlinear/FixedLagSmoother.h:113
gtsam::FixedLagSmoother
Definition: nonlinear/FixedLagSmoother.h:33
gtsam::FixedLagSmoother::findKeysAfter
KeyVector findKeysAfter(double timestamp) const
Definition: FixedLagSmoother.cpp:114
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::FixedLagSmoother::getCurrentTimestamp
double getCurrentTimestamp() const
Definition: FixedLagSmoother.cpp:95
gtsam::FixedLagSmoother::Result::iterations
size_t iterations
The number of optimizer iterations performed.
Definition: nonlinear/FixedLagSmoother.h:49
gtsam::FixedLagSmoother::Result::intermediateSteps
size_t intermediateSteps
The number of intermediate steps performed within the optimization. For L-M, this is the number of la...
Definition: nonlinear/FixedLagSmoother.h:50
gtsam::FixedLagSmoother::Result::error
double error
The final factor graph error.
Definition: nonlinear/FixedLagSmoother.h:53
key
const gtsam::Symbol key('X', 0)
FixedLagSmoother.h
Base class for a fixed-lag smoother. This mimics the basic interface to iSAM2.
gtsam::FixedLagSmoother::equals
virtual bool equals(const FixedLagSmoother &rhs, double tol=1e-9) const
Definition: FixedLagSmoother.cpp:40
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::FixedLagSmoother::Result::print
void print() const
Definition: FixedLagSmoother.cpp:25
iter
iterator iter(handle obj)
Definition: pytypes.h:2475
gtsam::FixedLagSmoother::Result::nonlinearVariables
size_t nonlinearVariables
The number of variables that can be relinearized.
Definition: nonlinear/FixedLagSmoother.h:51
gtsam::tol
const G double tol
Definition: Group.h:79
abs
#define abs(x)
Definition: datatypes.h:17
gtsam::FixedLagSmoother::keyTimestampMap_
KeyTimestampMap keyTimestampMap_
Definition: nonlinear/FixedLagSmoother.h:114
Eigen::placeholders::end
static const EIGEN_DEPRECATED end_t end
Definition: IndexedViewHelper.h:181
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
max
#define max(a, b)
Definition: datatypes.h:20
gtsam::equal
bool equal(const T &obj1, const T &obj2, double tol)
Definition: Testable.h:85
gtsam::FixedLagSmoother::updateKeyTimestampMap
void updateKeyTimestampMap(const KeyTimestampMap &newTimestamps)
Definition: FixedLagSmoother.cpp:46


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