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 ConcurrentIncrementalSmoother.h
14 * @brief An iSAM2-based Smoother 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 * A Levenberg-Marquardt Batch Smoother that implements the Concurrent Filtering and Smoother interface.
29 */
30class GTSAM_UNSTABLE_EXPORT ConcurrentIncrementalSmoother : public virtual ConcurrentSmoother {
31
32public:
33 typedef std::shared_ptr<ConcurrentIncrementalSmoother> shared_ptr;
34 typedef ConcurrentSmoother Base; ///< typedef for base class
35
36 /** Meta information returned about the update */
37 struct Result {
38 size_t iterations; ///< The number of optimizer iterations performed
39 size_t nonlinearVariables; ///< The number of variables that can be relinearized
40 size_t linearVariables; ///< The number of variables that must keep a constant linearization point
41 double error; ///< The final factor graph error
42
43 /// Constructor
44 Result() : iterations(0), nonlinearVariables(0), linearVariables(0), error(0) {}
45
46 /// Getter methods
47 size_t getIterations() const { return iterations; }
48 size_t getNonlinearVariables() const { return nonlinearVariables; }
49 size_t getLinearVariables() const { return linearVariables; }
50 double getError() const { return error; }
51 };
52
53 /** Default constructor */
54 ConcurrentIncrementalSmoother(const ISAM2Params& parameters = ISAM2Params()) : isam2_(parameters) {}
55
56 /** Default destructor */
57 ~ConcurrentIncrementalSmoother() override {}
58
59 /** Implement a GTSAM standard 'print' function */
60 void print(const std::string& s = "Concurrent Incremental Smoother:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
61
62 /** Check if two Concurrent Smoothers are equal */
63 bool equals(const ConcurrentSmoother& rhs, double tol = 1e-9) const override;
64
65 /** Access the current set of factors */
66 const NonlinearFactorGraph& getFactors() const {
67 return isam2_.getFactorsUnsafe();
68 }
69
70 /** Access the current linearization point */
71 const Values& getLinearizationPoint() const {
72 return isam2_.getLinearizationPoint();
73 }
74
75 /** Access the current set of deltas to the linearization point */
76 const VectorValues& getDelta() const {
77 return isam2_.getDelta();
78 }
79
80 /** Compute the current best estimate of all variables and return a full Values structure.
81 * If only a single variable is needed, it may be faster to call calculateEstimate(const KEY&).
82 */
83 Values calculateEstimate() const {
84 return isam2_.calculateEstimate();
85 }
86
87 /** Compute the current best estimate of a single variable. This is generally faster than
88 * calling the no-argument version of calculateEstimate if only specific variables are needed.
89 * @param key
90 * @return
91 */
92 template<class VALUE>
93 VALUE calculateEstimate(Key key) const {
94 return isam2_.calculateEstimate<VALUE>(key);
95 }
96
97 /**
98 * Add new factors and variables to the smoother.
99 *
100 * Add new measurements, and optionally new variables, to the smoother.
101 * This runs a full step of the ISAM2 algorithm, relinearizing and updating
102 * the solution as needed, according to the wildfire and relinearize
103 * thresholds.
104 *
105 * @param newFactors The new factors to be added to the smoother
106 * @param newTheta Initialization points for new variables to be added to the smoother
107 * You must include here all new variables occuring in newFactors (which were not already
108 * in the smoother). There must not be any variables here that do not occur in newFactors,
109 * and additionally, variables that were already in the system must not be included here.
110 */
111 Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(),
112 const std::optional<FactorIndices>& removeFactorIndices = {});
113
114 /**
115 * Perform any required operations before the synchronization process starts.
116 * Called by 'synchronize'
117 */
118 void presync() override;
119
120 /**
121 * Populate the provided containers with factors that constitute the smoother branch summarization
122 * needed by the filter.
123 *
124 * @param summarizedFactors The summarized factors for the filter branch
125 */
126 void getSummarizedFactors(NonlinearFactorGraph& summarizedFactors, Values& separatorValues) override;
127
128 /**
129 * Apply the new smoother factors sent by the filter, and the updated version of the filter
130 * branch summarized factors.
131 *
132 * @param smootherFactors A set of new factors added to the smoother from the filter
133 * @param smootherValues Linearization points for any new variables
134 * @param summarizedFactors An updated version of the filter branch summarized factors
135 * @param rootValues The linearization point of the root variables
136 */
137 void synchronize(const NonlinearFactorGraph& smootherFactors, const Values& smootherValues,
138 const NonlinearFactorGraph& summarizedFactors, const Values& separatorValues) override;
139
140 /**
141 * Perform any required operations after the synchronization process finishes.
142 * Called by 'synchronize'
143 */
144 void postsync() override;
145
146protected:
147
148 ISAM2 isam2_; ///< iSAM2 inference engine
149
150 // Storage for information received from the filter during the last synchronization
151 NonlinearFactorGraph smootherFactors_; ///< New factors to be added to the smoother during the next update
152 Values smootherValues_; ///< New variables to be added to the smoother during the next update
153 NonlinearFactorGraph filterSummarizationFactors_; ///< New filter summarization factors to replace the existing filter summarization during the next update
154 Values separatorValues_; ///< The linearization points of the separator variables. These should not be changed during optimization.
155 FactorIndices filterSummarizationSlots_; ///< The slots in factor graph that correspond to the current filter summarization factors
156 bool synchronizationUpdatesAvailable_; ///< Flag indicating the currently stored synchronization updates have not been applied yet
157
158 // Storage for information to be sent to the filter
159 NonlinearFactorGraph smootherSummarization_; ///< A temporary holding place for calculated smoother summarization
160
161private:
162
163 /** Calculate the smoother marginal factors on the separator variables */
164 void updateSmootherSummarization();
165
166}; // ConcurrentBatchSmoother
167
168/// Typedef for Matlab wrapping
169typedef ConcurrentIncrementalSmoother::Result ConcurrentIncrementalSmootherResult;
170
171/// traits
172template<>
173struct traits<ConcurrentIncrementalSmoother> : public Testable<ConcurrentIncrementalSmoother> {
174};
175
176} // \ namespace gtsam
177