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) {
57 
58  hfg.add(JacobianFactor(x(1), I_3x3, Z_3x1));
59 
60  // x(1) to x(n+1)
61  for (size_t k = 1; k < K; k++) {
62  DiscreteKeys dKeys{{m(k), 2}};
63  std::vector<GaussianFactor::shared_ptr> components;
64  components.emplace_back(
65  new JacobianFactor(x(k), I_3x3, x(k + 1), I_3x3, Z_3x1));
66  components.emplace_back(
67  new JacobianFactor(x(k), I_3x3, x(k + 1), I_3x3, Vector3::Ones()));
68  hfg.add(HybridGaussianFactor({m(k), 2}, components));
69 
70  if (k > 1) {
71  hfg.add(DecisionTreeFactor({{m(k - 1), 2}, {m(k), 2}}, "0 1 1 3"));
72  }
73  }
74 
75  return std::make_shared<HybridGaussianFactorGraph>(std::move(hfg));
76 }
77 
87 inline std::pair<KeyVector, std::vector<int>> makeBinaryOrdering(
88  std::vector<Key> &input) {
89  KeyVector new_order;
90 
91  std::vector<int> levels(input.size());
92  std::function<void(std::vector<Key>::iterator, std::vector<Key>::iterator,
93  int)>
94  bsg = [&bsg, &new_order, &levels, &input](
95  std::vector<Key>::iterator begin,
96  std::vector<Key>::iterator end, int lvl) {
97  if (std::distance(begin, end) > 1) {
98  std::vector<Key>::iterator pivot =
99  begin + std::distance(begin, end) / 2;
100 
101  new_order.push_back(*pivot);
102  levels[std::distance(input.begin(), pivot)] = lvl;
103  bsg(begin, pivot, lvl + 1);
104  bsg(pivot + 1, end, lvl + 1);
105  } else if (std::distance(begin, end) == 1) {
106  new_order.push_back(*begin);
107  levels[std::distance(input.begin(), begin)] = lvl;
108  }
109  };
110 
111  bsg(input.begin(), input.end(), 0);
112  std::reverse(new_order.begin(), new_order.end());
113 
114  return {new_order, levels};
115 }
116 
117 /* ****************************************************************************/
119 
120 // Test fixture with switching network.
122 struct Switching {
123  public:
124  size_t K;
129 
138  Switching(size_t K, double between_sigma = 1.0, double prior_sigma = 0.1,
139  std::vector<double> measurements = {},
140  std::string transitionProbabilityTable = "1/2 3/2")
141  : K(K) {
142  using noiseModel::Isotropic;
143 
144  // Create DiscreteKeys for K-1 binary modes.
145  for (size_t k = 0; k < K - 1; k++) {
146  modes.emplace_back(M(k), 2);
147  }
148 
149  // If measurements are not provided, we just have the robot moving forward.
150  if (measurements.size() == 0) {
151  for (size_t k = 0; k < K; k++) {
152  measurements.push_back(k);
153  }
154  }
155 
156  // Create hybrid factor graph.
157 
158  // Add a prior ϕ(X(0)) on X(0).
159  unaryFactors.emplace_shared<PriorFactor<double>>(
160  X(0), measurements.at(0), Isotropic::Sigma(1, prior_sigma));
161 
162  // Add "motion models" ϕ(X(k),X(k+1),M(k)).
163  for (size_t k = 0; k < K - 1; k++) {
164  auto motion_models = motionModels(k, between_sigma);
165  binaryFactors.emplace_shared<HybridNonlinearFactor>(modes[k],
166  motion_models);
167  }
168 
169  // Add measurement factors ϕ(X(k);z_k).
170  auto measurement_noise = Isotropic::Sigma(1, prior_sigma);
171  for (size_t k = 1; k < K; k++) {
172  unaryFactors.emplace_shared<PriorFactor<double>>(X(k), measurements.at(k),
173  measurement_noise);
174  }
175 
176  // Add "mode chain" ϕ(M(0)) ϕ(M(0),M(1)) ... ϕ(M(K-3),M(K-2))
177  modeChain = createModeChain(transitionProbabilityTable);
178 
179  // Create the linearization point.
180  for (size_t k = 0; k < K; k++) {
181  linearizationPoint.insert<double>(X(k), static_cast<double>(k + 1));
182  }
183 
186  }
187 
188  // Create motion models for a given time step
189  static std::vector<NoiseModelFactor::shared_ptr> motionModels(
190  size_t k, double sigma = 1.0) {
192  auto still =
193  std::make_shared<MotionModel>(X(k), X(k + 1), 0.0, noise_model),
194  moving =
195  std::make_shared<MotionModel>(X(k), X(k + 1), 1.0, noise_model);
196  return {still, moving};
197  }
198 
206  std::string transitionProbabilityTable = "1/2 3/2") {
208  chain.emplace_shared<DiscreteDistribution>(modes[0], "1/1");
209  for (size_t k = 0; k < K - 2; k++) {
210  auto parents = {modes[k]};
211  chain.emplace_shared<DiscreteConditional>(modes[k + 1], parents,
212  transitionProbabilityTable);
213  }
214  return chain;
215  }
216 
220  graph.push_back(linearUnaryFactors);
221  graph.push_back(linearBinaryFactors);
222  graph.push_back(modeChain);
223  return graph;
224  }
225 
229  graph.push_back(unaryFactors);
230  graph.push_back(binaryFactors);
231  graph.push_back(modeChain);
232  return graph;
233  }
234 };
235 
236 } // namespace gtsam
gtsam::HybridGaussianFactorGraph::shared_ptr
std::shared_ptr< This > shared_ptr
shared_ptr to This
Definition: HybridGaussianFactorGraph.h:119
gtsam::Switching::K
size_t K
Definition: Switching.h:124
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:87
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:218
gtsam::Switching::nonlinearFactorGraph
HybridNonlinearFactorGraph nonlinearFactorGraph() const
Get all the nonlinear factors.
Definition: Switching.h:227
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:122
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:127
gtsam::HybridGaussianFactorGraph
Definition: HybridGaussianFactorGraph.h:105
gtsam::makeSwitchingChain
HybridGaussianFactorGraph::shared_ptr makeSwitchingChain(size_t K, std::function< Key(int)> x=X, std::function< Key(int)> m=M)
Create a switching system chain. A switching system is a continuous system which depends on a discret...
Definition: Switching.h:54
Symbol.h
m
Matrix3f m
Definition: AngleAxis_mimic_euler.cpp:1
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:189
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:126
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:138
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:127
gtsam::Switching::linearizationPoint
Values linearizationPoint
Definition: Switching.h:128
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:125
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:205
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:126
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:126
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 Fri Nov 1 2024 03:36:54