Go to the documentation of this file.
49 const bool debug =
ISDEBUG(
"IncrementalFixedLagSmoother update");
52 std::cout <<
"IncrementalFixedLagSmoother::update() Start" << std::endl;
54 std::cout <<
"END" << std::endl;
58 std::optional<FastMap<Key, int> > constrainedKeys = {};
67 std::cout <<
"Current Timestamp: " << current_timestamp << std::endl;
74 std::cout <<
"Marginalizable Keys: ";
75 for(
Key key: marginalizableKeys) {
78 std::cout << std::endl;
85 std::cout <<
"Constrained Keys: ";
86 if (constrainedKeys) {
88 iter != constrainedKeys->end(); ++
iter) {
93 std::cout << std::endl;
96 std::unordered_set<Key> additionalKeys =
98 isam_, marginalizableKeys);
99 KeyList additionalMarkedKeys(additionalKeys.begin(), additionalKeys.end());
103 factorsToRemove, constrainedKeys, {}, additionalMarkedKeys);
107 "Bayes Tree After Update, Before Marginalization:");
108 std::cout <<
"END" << std::endl;
112 if (marginalizableKeys.size() > 0) {
114 marginalizableKeys.end());
123 std::cout <<
"END" << std::endl;
129 result.linearVariables = 0;
130 result.nonlinearVariables = 0;
134 std::cout <<
"IncrementalFixedLagSmoother::update() Finish" << std::endl;
153 if (marginalizableKeys.size() > 0) {
158 constrainedKeys->operator[](timestamp_key.second) = 1;
161 for(
Key key: marginalizableKeys) {
162 constrainedKeys->operator[](
key) = 0;
169 const std::string& label) {
174 std::cout << std::endl;
184 std::cout <<
" )" << std::endl;
190 std::cout << label << std::endl;
198 const std::string& label) {
199 std::cout << label << std::endl;
200 if (!
isam.roots().empty()) {
205 std::cout <<
"{Empty Tree}" << std::endl;
213 std::cout << indent <<
"P( ";
214 for(
Key key: clique->conditional()->frontals()) {
217 if (clique->conditional()->nrParents() > 0)
219 for(
Key key: clique->conditional()->parents()) {
222 std::cout <<
")" << std::endl;
void marginalizeLeaves(const FastList< Key > &leafKeys, FactorIndices *marginalFactorsIndices=nullptr, FactorIndices *deletedFactorsIndices=nullptr)
Helper functions for marginalizing variables from a Bayes Tree.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
static void PrintKeySet(const std::set< Key > &keys, const std::string &label="Keys:")
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
bool equals(const FixedLagSmoother &rhs, double tol=1e-9) const override
const KeyTimestampMap & timestamps() const
void eraseKeyTimestampMap(const KeyVector &keys)
static void PrintSymbolicTreeHelper(const gtsam::ISAM2Clique::shared_ptr &clique, const std::string indent="")
static void PrintSymbolicFactor(const GaussianFactor::shared_ptr &factor)
Base::sharedClique sharedClique
Shared pointer to a clique.
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
static std::unordered_set< Key > gatherAdditionalKeysToReEliminate(const BayesTree &bayesTree, const KeyVector &marginalizableKeys)
virtual bool equals(const ISAM2 &other, double tol=1e-9) const
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
DecisionTreeFactor factor(D &C &B &A, "0.0 0.0 0.0 0.60658897 0.61241912 0.61241969 0.61247685 0.61247742 0.0 " "0.0 0.0 0.99995287 1.0 1.0 1.0 1.0")
TimestampKeyMap timestampKeyMap_
Result update(const NonlinearFactorGraph &newFactors=NonlinearFactorGraph(), const Values &newTheta=Values(), const KeyTimestampMap ×tamps=KeyTimestampMap(), const FactorIndices &factorsToRemove=FactorIndices()) override
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
void print(const std::string &s="IncrementalFixedLagSmoother:\n", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
NonlinearISAM isam(relinearizeInterval)
static constexpr bool debug
std::shared_ptr< This > shared_ptr
shared_ptr to this class
double getCurrentTimestamp() const
const gtsam::Symbol key('X', 0)
An iSAM2-based fixed-lag smoother.
std::shared_ptr< This > shared_ptr
virtual bool equals(const FixedLagSmoother &rhs, double tol=1e-9) const
static void PrintSymbolicTree(const gtsam::ISAM2 &isam, const std::string &label="Bayes Tree:")
std::map< Key, double > KeyTimestampMap
Typedef for a Key-Timestamp map/database.
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)
iterator iter(handle obj)
void eraseKeysBefore(double timestamp)
KeyTimestampMap keyTimestampMap_
static const EIGEN_DEPRECATED end_t end
NonlinearFactorGraph graph
void createOrderingConstraints(const KeyVector &marginalizableKeys, std::optional< FastMap< Key, int > > &constrainedKeys) const
KeyVector findKeysBefore(double timestamp) const
virtual void print(const std::string &s="FixedLagSmoother:\n", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
std::uint64_t Key
Integer nonlinear key type.
void updateKeyTimestampMap(const KeyTimestampMap &newTimestamps)
FastVector< FactorIndex > FactorIndices
Define collection types:
static void PrintSymbolicGraph(const GaussianFactorGraph &graph, const std::string &label="Factor Graph:")
gtsam
Author(s):
autogenerated on Sun Dec 22 2024 04:11:41