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
12/**
13 * @file ConcurrentBatchFilter.h
14 * @brief An iSAM2-based Filter that implements the
15 * Concurrent Filtering and Smoothing interface.
16 * @author Stephen Williams
17 */
18
19// \callgraph
20#pragma once
21
22#include <gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h>
23#include <gtsam/nonlinear/ISAM2.h>
24
25namespace gtsam {
26
27/**
28 * An iSAM2-based Batch Filter that implements the Concurrent Filtering and Smoother interface.
29 */
30class GTSAM_UNSTABLE_EXPORT ConcurrentIncrementalFilter : public virtual ConcurrentFilter {
31
32public:
33
34 typedef std::shared_ptr<ConcurrentIncrementalFilter> shared_ptr;
35 typedef ConcurrentFilter Base; ///< typedef for base class
36
37 /** Meta information returned about the update */
38 struct Result {
39 size_t iterations; ///< The number of optimizer iterations performed
40 size_t nonlinearVariables; ///< The number of variables that can be relinearized
41 size_t linearVariables; ///< The number of variables that must keep a constant linearization point
42 size_t variablesReeliminated;
43 size_t variablesRelinearized;
44
45 /** The indices of the newly-added factors, in 1-to-1 correspondence with the
46 * factors passed as \c newFactors update(). These indices may be
47 * used later to refer to the factors in order to remove them.
48 */
49 FactorIndices newFactorsIndices;
50
51 double error; ///< The final factor graph error
52
53 /// Constructor
54 Result() : iterations(0), nonlinearVariables(0), linearVariables(0), error(0) {}
55
56 /// Getter methods
57 size_t getIterations() const { return iterations; }
58 size_t getNonlinearVariables() const { return nonlinearVariables; }
59 size_t getLinearVariables() const { return linearVariables; }
60 double getError() const { return error; }
61 };
62
63 /** Default constructor */
64 ConcurrentIncrementalFilter(const ISAM2Params& parameters = ISAM2Params()) : isam2_(parameters) {}
65
66 /** Default destructor */
67 ~ConcurrentIncrementalFilter() override {}
68
69 /** Implement a GTSAM standard 'print' function */
70 void print(const std::string& s = "Concurrent Incremental Filter:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
71
72 /** Check if two Concurrent Filters are equal */
73 bool equals(const ConcurrentFilter& rhs, double tol = 1e-9) const override;
74
75 /** Access the current set of factors */
76 const NonlinearFactorGraph& getFactors() const {
77 return isam2_.getFactorsUnsafe();
78 }
79
80 /** Access the current linearization point */
81 const ISAM2& getISAM2() const {
82 return isam2_;
83 }
84
85 /** Access the current linearization point */
86 const Values& getLinearizationPoint() const {
87 return isam2_.getLinearizationPoint();
88 }
89
90 /** Access the current set of deltas to the linearization point */
91 const VectorValues& getDelta() const {
92 return isam2_.getDelta();
93 }
94
95 /** Compute the current best estimate of all variables and return a full Values structure.
96 * If only a single variable is needed, it may be faster to call calculateEstimate(const KEY&).
97 */
98 Values calculateEstimate() const {
99 return isam2_.calculateEstimate();
100 }
101
102 /** Compute the current best estimate of a single variable. This is generally faster than
103 * calling the no-argument version of calculateEstimate if only specific variables are needed.
104 * @param key
105 * @return
106 */
107 template<class VALUE>
108 VALUE calculateEstimate(Key key) const {
109 return isam2_.calculateEstimate<VALUE>(key);
110 }
111
112 /**
113 * Add new factors and variables to the filter.
114 *
115 * Add new measurements, and optionally new variables, to the filter.
116 * This runs a full update step of the derived filter algorithm
117 *
118 * @param newFactors The new factors to be added to the smoother
119 * @param newTheta Initialization points for new variables to be added to the filter
120 * You must include here all new variables occurring in newFactors that were not already
121 * in the filter.
122 * @param keysToMove An optional set of keys to move from the filter to the smoother
123 * @param removeFactorIndices An optional set of indices corresponding to the factors you want to remove from the graph
124 */
125 Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(),
126 const std::optional<FastList<Key> >& keysToMove = {},
127 const std::optional< FactorIndices >& removeFactorIndices = {});
128
129 /**
130 * Perform any required operations before the synchronization process starts.
131 * Called by 'synchronize'
132 */
133 void presync() override;
134
135 /**
136 * Populate the provided containers with factors that constitute the filter branch summarization
137 * needed by the smoother. Also, linearization points for the new root clique must be provided.
138 *
139 * @param summarizedFactors The summarized factors for the filter branch
140 * @param rootValues The linearization points of the root clique variables
141 */
142 void getSummarizedFactors(NonlinearFactorGraph& filterSummarization, Values& filterSummarizationValues) override;
143
144 /**
145 * Populate the provided containers with factors being sent to the smoother from the filter. These
146 * may be original nonlinear factors, or factors encoding a summarization of the filter information.
147 * The specifics will be implementation-specific for a given filter.
148 *
149 * @param smootherFactors The new factors to be added to the smoother
150 * @param smootherValues The linearization points of any new variables
151 */
152 void getSmootherFactors(NonlinearFactorGraph& smootherFactors, Values& smootherValues) override;
153
154 /**
155 * Apply the updated version of the smoother branch summarized factors.
156 *
157 * @param summarizedFactors An updated version of the smoother branch summarized factors
158 */
159 void synchronize(const NonlinearFactorGraph& smootherSummarization, const Values& smootherSummarizationValues) override;
160
161 /**
162 * Perform any required operations after the synchronization process finishes.
163 * Called by 'synchronize'
164 */
165 void postsync() override;
166
167protected:
168
169 ISAM2 isam2_; ///< The iSAM2 inference engine
170
171 // ???
172 NonlinearFactorGraph previousSmootherSummarization_; ///< The smoother summarization on the old separator sent by the smoother during the last synchronization
173 FactorIndices currentSmootherSummarizationSlots_; ///< The slots in factor graph that correspond to the current smoother summarization on the current separator
174 NonlinearFactorGraph smootherShortcut_; ///< A set of conditional factors from the old separator to the current separator (recursively calculated during each filter update)
175
176 // Storage for information to be sent to the smoother
177 NonlinearFactorGraph smootherFactors_; ///< A temporary holding place for the set of full nonlinear factors being sent to the smoother
178 Values smootherValues_; ///< A temporary holding place for the linearization points of all keys being sent to the smoother
179
180private:
181
182 /** Traverse the iSAM2 Bayes Tree, inserting all descendants of the provided index/key into 'additionalKeys' */
183 static void RecursiveMarkAffectedKeys(const Key& key, const ISAM2Clique::shared_ptr& clique, std::set<Key>& additionalKeys);
184
185 /** Find the set of iSAM2 factors adjacent to 'keys' */
186 static FactorIndices FindAdjacentFactors(const ISAM2& isam2, const FastList<Key>& keys, const FactorIndices& factorsToIgnore);
187
188 /** Update the shortcut marginal between the current separator keys and the previous separator keys */
189 // TODO: Make this a static function
190 void updateShortcut(const NonlinearFactorGraph& removedFactors);
191
192 /** Calculate marginal factors on the current separator variables using just the information in the filter */
193 // TODO: Make this a static function
194 NonlinearFactorGraph calculateFilterSummarization() const;
195
196}; // ConcurrentBatchFilter
197
198/// Typedef for Matlab wrapping
199typedef ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilterResult;
200
201/// traits
202template<>
203struct traits<ConcurrentIncrementalFilter> : public Testable<ConcurrentIncrementalFilter> {
204};
205
206} //\ namespace gtsam
207