Switching.h
Go to the documentation of this file.
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 Switching.h
14  * @date Mar 11, 2022
15  * @author Varun Agrawal
16  * @author Fan Jiang
17  */
18 
19 #include <gtsam/base/Matrix.h>
26 #include <gtsam/inference/Symbol.h>
34 
35 #include <vector>
36 
37 #pragma once
38 
39 namespace gtsam {
40 
44 
55  size_t K, std::function<Key(int)> x = X, std::function<Key(int)> m = M,
56  const std::string &transitionProbabilityTable = "0 1 1 3") {
58 
59  hfg.add(JacobianFactor(x(1), I_3x3, Z_3x1));
60 
61  // x(1) to x(n+1)
62  for (size_t k = 1; k < K; k++) {
63  DiscreteKeys dKeys{{m(k), 2}};
64  std::vector<GaussianFactor::shared_ptr> components;
65  components.emplace_back(
66  new JacobianFactor(x(k), I_3x3, x(k + 1), I_3x3, Z_3x1));
67  components.emplace_back(
68  new JacobianFactor(x(k), I_3x3, x(k + 1), I_3x3, Vector3::Ones()));
69  hfg.add(HybridGaussianFactor({m(k), 2}, components));
70 
71  if (k > 1) {
72  hfg.add(DecisionTreeFactor({{m(k - 1), 2}, {m(k), 2}},
73  transitionProbabilityTable));
74  }
75  }
76 
77  return std::make_shared<HybridGaussianFactorGraph>(std::move(hfg));
78 }
79 
89 inline std::pair<KeyVector, std::vector<int>> makeBinaryOrdering(
90  std::vector<Key> &input) {
91  KeyVector new_order;
92 
93  std::vector<int> levels(input.size());
94  std::function<void(std::vector<Key>::iterator, std::vector<Key>::iterator,
95  int)>
96  bsg = [&bsg, &new_order, &levels, &input](
97  std::vector<Key>::iterator begin,
98  std::vector<Key>::iterator end, int lvl) {
99  if (std::distance(begin, end) > 1) {
100  std::vector<Key>::iterator pivot =
101  begin + std::distance(begin, end) / 2;
102 
103  new_order.push_back(*pivot);
104  levels[std::distance(input.begin(), pivot)] = lvl;
105  bsg(begin, pivot, lvl + 1);
106  bsg(pivot + 1, end, lvl + 1);
107  } else if (std::distance(begin, end) == 1) {
108  new_order.push_back(*begin);
109  levels[std::distance(input.begin(), begin)] = lvl;
110  }
111  };
112 
113  bsg(input.begin(), input.end(), 0);
114  std::reverse(new_order.begin(), new_order.end());
115 
116  return {new_order, levels};
117 }
118 
119 /* ****************************************************************************/
121 
122 // Test fixture with switching network.
124 struct Switching {
125  public:
126  size_t K;
131 
140  Switching(size_t K, double between_sigma = 1.0, double prior_sigma = 0.1,
141  std::vector<double> measurements = {},
142  std::string transitionProbabilityTable = "1/2 3/2")
143  : K(K) {
144  using noiseModel::Isotropic;
145 
146  // Create DiscreteKeys for K-1 binary modes.
147  for (size_t k = 0; k < K - 1; k++) {
148  modes.emplace_back(M(k), 2);
149  }
150 
151  // If measurements are not provided, we just have the robot moving forward.
152  if (measurements.size() == 0) {
153  for (size_t k = 0; k < K; k++) {
154  measurements.push_back(k);
155  }
156  }
157 
158  // Create hybrid factor graph.
159 
160  // Add a prior ϕ(X(0)) on X(0).
161  unaryFactors.emplace_shared<PriorFactor<double>>(
162  X(0), measurements.at(0), Isotropic::Sigma(1, prior_sigma));
163 
164  // Add "motion models" ϕ(X(k),X(k+1),M(k)).
165  for (size_t k = 0; k < K - 1; k++) {
166  auto motion_models = motionModels(k, between_sigma);
167  binaryFactors.emplace_shared<HybridNonlinearFactor>(modes[k],
168  motion_models);
169  }
170 
171  // Add measurement factors ϕ(X(k);z_k).
172  auto measurement_noise = Isotropic::Sigma(1, prior_sigma);
173  for (size_t k = 1; k < K; k++) {
174  unaryFactors.emplace_shared<PriorFactor<double>>(X(k), measurements.at(k),
175  measurement_noise);
176  }
177 
178  // Add "mode chain" ϕ(M(0)) ϕ(M(0),M(1)) ... ϕ(M(K-3),M(K-2))
179  modeChain = createModeChain(transitionProbabilityTable);
180 
181  // Create the linearization point.
182  for (size_t k = 0; k < K; k++) {
183  linearizationPoint.insert<double>(X(k), static_cast<double>(k + 1));
184  }
185 
188  }
189 
190  // Create motion models for a given time step
191  static std::vector<NoiseModelFactor::shared_ptr> motionModels(
192  size_t k, double sigma = 1.0) {
194  auto still =
195  std::make_shared<MotionModel>(X(k), X(k + 1), 0.0, noise_model),
196  moving =
197  std::make_shared<MotionModel>(X(k), X(k + 1), 1.0, noise_model);
198  return {still, moving};
199  }
200 
208  std::string transitionProbabilityTable = "1/2 3/2") {
210  chain.emplace_shared<DiscreteDistribution>(modes[0], "1/1");
211  for (size_t k = 0; k < K - 2; k++) {
212  auto parents = {modes[k]};
213  chain.emplace_shared<DiscreteConditional>(modes[k + 1], parents,
214  transitionProbabilityTable);
215  }
216  return chain;
217  }
218 
222  graph.push_back(linearUnaryFactors);
223  graph.push_back(linearBinaryFactors);
224  graph.push_back(modeChain);
225  return graph;
226  }
227 
231  graph.push_back(unaryFactors);
232  graph.push_back(binaryFactors);
233  graph.push_back(modeChain);
234  return graph;
235  }
236 };
237 
238 } // namespace gtsam
gtsam::HybridGaussianFactorGraph::shared_ptr
std::shared_ptr< This > shared_ptr
shared_ptr to This
Definition: HybridGaussianFactorGraph.h:120
gtsam::Switching::K
size_t K
Definition: Switching.h:126
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
gtsam::DecisionTreeFactor
Definition: DecisionTreeFactor.h:44
gtsam::Z_3x1
static const Eigen::MatrixBase< Vector3 >::ConstantReturnType Z_3x1
Definition: Vector.h:46
HybridNonlinearFactorGraph.h
Nonlinear hybrid factor graph that uses type erasure.
gtsam::symbol_shorthand::M
Key M(std::uint64_t j)
Definition: inference/Symbol.h:160
gtsam::HybridNonlinearFactorGraph
Definition: HybridNonlinearFactorGraph.h:33
gtsam::DiscreteDistribution
Definition: DiscreteDistribution.h:33
Matrix.h
typedef and functions to augment Eigen's MatrixXd
x
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
Definition: gnuplot_common_settings.hh:12
gtsam::JacobianFactor
Definition: JacobianFactor.h:91
gtsam::makeBinaryOrdering
std::pair< KeyVector, std::vector< int > > makeBinaryOrdering(std::vector< Key > &input)
Return the ordering as a binary tree such that all parent nodes are above their children.
Definition: Switching.h:89
X
#define X
Definition: icosphere.cpp:20
gtsam::DiscreteKeys
DiscreteKeys is a set of keys that can be assembled using the & operator.
Definition: DiscreteKey.h:41
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:92
HybridGaussianFactor.h
A set of GaussianFactors, indexed by a set of discrete keys.
test_motion::noise_model
auto noise_model
Definition: testHybridNonlinearFactorGraph.cpp:119
gtsam::Switching::linearizedFactorGraph
HybridGaussianFactorGraph linearizedFactorGraph() const
Get the full linear factor graph.
Definition: Switching.h:220
gtsam::Switching::nonlinearFactorGraph
HybridNonlinearFactorGraph nonlinearFactorGraph() const
Get all the nonlinear factors.
Definition: Switching.h:229
sampling::sigma
static const double sigma
Definition: testGaussianBayesNet.cpp:170
gtsam::Switching
ϕ(X(0)) .. ϕ(X(k),X(k+1)) .. ϕ(X(k);z_k) .. ϕ(M(0)) .. ϕ(M(K-3),M(K-2))
Definition: Switching.h:124
gtsam::symbol_shorthand::X
Key X(std::uint64_t j)
Definition: inference/Symbol.h:171
PriorFactor.h
BetweenFactor.h
gtsam::noiseModel::Isotropic
Definition: NoiseModel.h:541
gtsam::Switching::linearUnaryFactors
HybridGaussianFactorGraph linearUnaryFactors
Definition: Switching.h:129
gtsam::HybridGaussianFactorGraph
Definition: HybridGaussianFactorGraph.h:106
Symbol.h
m
Matrix3f m
Definition: AngleAxis_mimic_euler.cpp:1
gtsam::makeSwitchingChain
HybridGaussianFactorGraph::shared_ptr makeSwitchingChain(size_t K, std::function< Key(int)> x=X, std::function< Key(int)> m=M, const std::string &transitionProbabilityTable="0 1 1 3")
Create a switching system chain. A switching system is a continuous system which depends on a discret...
Definition: Switching.h:54
JacobianFactor.h
NonlinearFactor.h
Non-linear factor base classes.
gtsam::Switching::motionModels
static std::vector< NoiseModelFactor::shared_ptr > motionModels(size_t k, double sigma=1.0)
Definition: Switching.h:191
HybridNonlinearFactor.h
A set of nonlinear factors indexed by a set of discrete keys.
gtsam::DiscreteConditional
Definition: DiscreteConditional.h:37
gtsam::Switching::modeChain
HybridNonlinearFactorGraph modeChain
Definition: Switching.h:128
HybridGaussianFactorGraph.h
Linearized Hybrid factor graph that uses type erasure.
gtsam
traits
Definition: SFMdata.h:40
gtsam::Switching::Switching
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.
Definition: Switching.h:140
NoiseModel.h
K
#define K
Definition: igam.h:8
estimation_fixture::measurements
std::vector< double > measurements
Definition: testHybridEstimation.cpp:51
DiscreteDistribution.h
gtsam::Values
Definition: Values.h:65
gtsam::Switching::linearBinaryFactors
HybridGaussianFactorGraph linearBinaryFactors
Definition: Switching.h:129
gtsam::Switching::linearizationPoint
Values linearizationPoint
Definition: Switching.h:130
gtsam::HybridGaussianFactor
Implementation of a discrete-conditioned hybrid factor. Implements a joint discrete-continuous factor...
Definition: HybridGaussianFactor.h:60
gtsam::Values::insert
void insert(Key j, const Value &val)
Definition: Values.cpp:155
gtsam::HybridNonlinearFactorGraph::linearize
std::shared_ptr< HybridGaussianFactorGraph > linearize(const Values &continuousValues) const
Linearize all the continuous factors in the HybridNonlinearFactorGraph.
Definition: HybridNonlinearFactorGraph.cpp:139
gtsam::Switching::modes
DiscreteKeys modes
Definition: Switching.h:127
gtsam::noiseModel::Isotropic::Sigma
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
Definition: NoiseModel.cpp:624
gtsam::distance
Double_ distance(const OrientedPlane3_ &p)
Definition: slam/expressions.h:117
gtsam::Switching::createModeChain
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,...
Definition: Switching.h:207
GaussianFactor.h
A factor with a quadratic error function - a Gaussian.
reverse
void reverse(const MatrixType &m)
Definition: array_reverse.cpp:16
test_motion::components
std::vector< NoiseModelFactor::shared_ptr > components
Definition: testHybridNonlinearFactorGraph.cpp:120
Eigen::placeholders::end
static const EIGEN_DEPRECATED end_t end
Definition: IndexedViewHelper.h:181
gtsam::Switching::binaryFactors
HybridNonlinearFactorGraph binaryFactors
Definition: Switching.h:128
gtsam::FactorGraph::add
IsDerived< DERIVEDFACTOR > add(std::shared_ptr< DERIVEDFACTOR > factor)
add is a synonym for push_back.
Definition: FactorGraph.h:171
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::Switching::unaryFactors
HybridNonlinearFactorGraph unaryFactors
Definition: Switching.h:128
DecisionTreeFactor.h
gtsam::FactorGraph::emplace_shared
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:153
gtsam::BetweenFactor
Definition: BetweenFactor.h:40
M
Matrix< RealScalar, Dynamic, Dynamic > M
Definition: bench_gemm.cpp:51
gtsam::symbol_shorthand::C
Key C(std::uint64_t j)
Definition: inference/Symbol.h:150


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:05:21