Go to the documentation of this file.
32 if (std::find(clique->conditional()->beginParents(),
33 clique->conditional()->endParents(),
key)
34 != clique->conditional()->endParents()) {
37 for(
Key i: clique->conditional()->frontals()) {
38 additionalKeys.insert(
i);
70 const bool debug =
ISDEBUG(
"IncrementalFixedLagSmoother update");
73 std::cout <<
"IncrementalFixedLagSmoother::update() Start" << std::endl;
75 std::cout <<
"END" << std::endl;
79 std::optional<FastMap<Key, int> > constrainedKeys = {};
88 std::cout <<
"Current Timestamp: " << current_timestamp << std::endl;
95 std::cout <<
"Marginalizable Keys: ";
96 for(
Key key: marginalizableKeys) {
99 std::cout << std::endl;
106 std::cout <<
"Constrained Keys: ";
107 if (constrainedKeys) {
109 iter != constrainedKeys->end(); ++
iter) {
114 std::cout << std::endl;
118 std::set<Key> additionalKeys;
119 for(
Key key: marginalizableKeys) {
125 KeyList additionalMarkedKeys(additionalKeys.begin(), additionalKeys.end());
129 factorsToRemove, constrainedKeys, {}, additionalMarkedKeys);
133 "Bayes Tree After Update, Before Marginalization:");
134 std::cout <<
"END" << std::endl;
138 if (marginalizableKeys.size() > 0) {
140 marginalizableKeys.end());
149 std::cout <<
"END" << std::endl;
155 result.linearVariables = 0;
156 result.nonlinearVariables = 0;
160 std::cout <<
"IncrementalFixedLagSmoother::update() Finish" << std::endl;
179 if (marginalizableKeys.size() > 0) {
184 constrainedKeys->operator[](timestamp_key.second) = 1;
187 for(
Key key: marginalizableKeys) {
188 constrainedKeys->operator[](
key) = 0;
195 const std::string& label) {
200 std::cout << std::endl;
207 for(
Key key: factor->keys()) {
210 std::cout <<
" )" << std::endl;
216 std::cout << label << std::endl;
224 const std::string& label) {
225 std::cout << label << std::endl;
226 if (!
isam.roots().empty()) {
231 std::cout <<
"{Empty Tree}" << std::endl;
239 std::cout << indent <<
"P( ";
240 for(
Key key: clique->conditional()->frontals()) {
243 if (clique->conditional()->nrParents() > 0)
245 for(
Key key: clique->conditional()->parents()) {
248 std::cout <<
")" << std::endl;
void recursiveMarkAffectedKeys(const Key &key, const ISAM2Clique::shared_ptr &clique, std::set< Key > &additionalKeys)
void marginalizeLeaves(const FastList< Key > &leafKeys, FactorIndices *marginalFactorsIndices=nullptr, FactorIndices *deletedFactorsIndices=nullptr)
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.
virtual bool equals(const ISAM2 &other, double tol=1e-9) const
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
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 Fri Nov 1 2024 03:32:45