ϕ(X(0)) .. ϕ(X(k),X(k+1)) .. ϕ(X(k);z_k) .. ϕ(M(0)) .. ϕ(M(K-3),M(K-2)) More...
#include <Switching.h>
Public Member Functions | |
HybridNonlinearFactorGraph | createModeChain (std::string transitionProbabilityTable="1/2 3/2") |
Add "mode chain" to HybridNonlinearFactorGraph from M(0) to M(K-2). E.g. if K=4, we want M0, M1 and M2. More... | |
HybridGaussianFactorGraph | linearizedFactorGraph () const |
Get the full linear factor graph. More... | |
HybridNonlinearFactorGraph | nonlinearFactorGraph () const |
Get all the nonlinear factors. More... | |
Switching (size_t K, double between_sigma=1.0, double prior_sigma=0.1, std::vector< double > measurements={}, std::string transitionProbabilityTable="1/2 3/2") | |
Create with given number of time steps. More... | |
Static Public Member Functions | |
static std::vector< NoiseModelFactor::shared_ptr > | motionModels (size_t k, double sigma=1.0) |
ϕ(X(0)) .. ϕ(X(k),X(k+1)) .. ϕ(X(k);z_k) .. ϕ(M(0)) .. ϕ(M(K-3),M(K-2))
Definition at line 124 of file Switching.h.
|
inline |
Create with given number of time steps.
K | The total number of timesteps. |
between_sigma | The stddev between poses. |
prior_sigma | The stddev on priors (also used for measurements). |
measurements | Vector of measurements for each timestep. |
Definition at line 140 of file Switching.h.
|
inline |
Add "mode chain" to HybridNonlinearFactorGraph from M(0) to M(K-2). E.g. if K=4, we want M0, M1 and M2.
fg | The factor graph to which the mode chain is added. |
Definition at line 207 of file Switching.h.
|
inline |
Get the full linear factor graph.
Definition at line 220 of file Switching.h.
|
inlinestatic |
Definition at line 191 of file Switching.h.
|
inline |
Get all the nonlinear factors.
Definition at line 229 of file Switching.h.
HybridNonlinearFactorGraph gtsam::Switching::binaryFactors |
Definition at line 128 of file Switching.h.
size_t gtsam::Switching::K |
Definition at line 126 of file Switching.h.
HybridGaussianFactorGraph gtsam::Switching::linearBinaryFactors |
Definition at line 129 of file Switching.h.
Values gtsam::Switching::linearizationPoint |
Definition at line 130 of file Switching.h.
HybridGaussianFactorGraph gtsam::Switching::linearUnaryFactors |
Definition at line 129 of file Switching.h.
HybridNonlinearFactorGraph gtsam::Switching::modeChain |
Definition at line 128 of file Switching.h.
DiscreteKeys gtsam::Switching::modes |
Definition at line 127 of file Switching.h.
HybridNonlinearFactorGraph gtsam::Switching::unaryFactors |
Definition at line 128 of file Switching.h.