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>
31 
32 #include <vector>
33 
34 #pragma once
35 
36 namespace gtsam {
37 
41 
52  size_t n, std::function<Key(int)> keyFunc = X,
53  std::function<Key(int)> dKeyFunc = M) {
55 
56  hfg.add(JacobianFactor(keyFunc(1), I_3x3, Z_3x1));
57 
58  // keyFunc(1) to keyFunc(n+1)
59  for (size_t t = 1; t < n; t++) {
61  {keyFunc(t), keyFunc(t + 1)}, {{dKeyFunc(t), 2}},
62  {std::make_shared<JacobianFactor>(keyFunc(t), I_3x3, keyFunc(t + 1),
63  I_3x3, Z_3x1),
64  std::make_shared<JacobianFactor>(keyFunc(t), I_3x3, keyFunc(t + 1),
65  I_3x3, Vector3::Ones())}));
66 
67  if (t > 1) {
68  hfg.add(DecisionTreeFactor({{dKeyFunc(t - 1), 2}, {dKeyFunc(t), 2}},
69  "0 1 1 3"));
70  }
71  }
72 
73  return std::make_shared<HybridGaussianFactorGraph>(std::move(hfg));
74 }
75 
85 inline std::pair<KeyVector, std::vector<int>> makeBinaryOrdering(
86  std::vector<Key> &input) {
87  KeyVector new_order;
88 
89  std::vector<int> levels(input.size());
90  std::function<void(std::vector<Key>::iterator, std::vector<Key>::iterator,
91  int)>
92  bsg = [&bsg, &new_order, &levels, &input](
93  std::vector<Key>::iterator begin,
94  std::vector<Key>::iterator end, int lvl) {
95  if (std::distance(begin, end) > 1) {
96  std::vector<Key>::iterator pivot =
97  begin + std::distance(begin, end) / 2;
98 
99  new_order.push_back(*pivot);
100  levels[std::distance(input.begin(), pivot)] = lvl;
101  bsg(begin, pivot, lvl + 1);
102  bsg(pivot + 1, end, lvl + 1);
103  } else if (std::distance(begin, end) == 1) {
104  new_order.push_back(*begin);
105  levels[std::distance(input.begin(), begin)] = lvl;
106  }
107  };
108 
109  bsg(input.begin(), input.end(), 0);
110  std::reverse(new_order.begin(), new_order.end());
111 
112  return {new_order, levels};
113 }
114 
115 /* ***************************************************************************
116  */
118 
119 // Test fixture with switching network.
120 struct Switching {
121  size_t K;
126 
135  Switching(size_t K, double between_sigma = 1.0, double prior_sigma = 0.1,
136  std::vector<double> measurements = {},
137  std::string discrete_transition_prob = "1/2 3/2")
138  : K(K) {
139  using noiseModel::Isotropic;
140 
141  // Create DiscreteKeys for binary K modes.
142  for (size_t k = 0; k < K; k++) {
143  modes.emplace_back(M(k), 2);
144  }
145 
146  // If measurements are not provided, we just have the robot moving forward.
147  if (measurements.size() == 0) {
148  for (size_t k = 0; k < K; k++) {
149  measurements.push_back(k);
150  }
151  }
152 
153  // Create hybrid factor graph.
154  // Add a prior on X(0).
155  nonlinearFactorGraph.emplace_shared<PriorFactor<double>>(
156  X(0), measurements.at(0), Isotropic::Sigma(1, prior_sigma));
157 
158  // Add "motion models".
159  for (size_t k = 0; k < K - 1; k++) {
160  KeyVector keys = {X(k), X(k + 1)};
161  auto motion_models = motionModels(k, between_sigma);
162  std::vector<NonlinearFactor::shared_ptr> components;
163  for (auto &&f : motion_models) {
164  components.push_back(std::dynamic_pointer_cast<NonlinearFactor>(f));
165  }
166  nonlinearFactorGraph.emplace_shared<MixtureFactor>(
167  keys, DiscreteKeys{modes[k]}, components);
168  }
169 
170  // Add measurement factors
171  auto measurement_noise = Isotropic::Sigma(1, prior_sigma);
172  for (size_t k = 1; k < K; k++) {
173  nonlinearFactorGraph.emplace_shared<PriorFactor<double>>(
174  X(k), measurements.at(k), measurement_noise);
175  }
176 
177  // Add "mode chain"
178  addModeChain(&nonlinearFactorGraph, discrete_transition_prob);
179 
180  // Create the linearization point.
181  for (size_t k = 0; k < K; k++) {
182  linearizationPoint.insert<double>(X(k), static_cast<double>(k + 1));
183  }
184 
185  // The ground truth is robot moving forward
186  // and one less than the linearization point
187  linearizedFactorGraph = *nonlinearFactorGraph.linearize(linearizationPoint);
188  }
189 
190  // Create motion models for a given time step
191  static std::vector<MotionModel::shared_ptr> motionModels(size_t k,
192  double sigma = 1.0) {
193  auto noise_model = noiseModel::Isotropic::Sigma(1, sigma);
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 discrete_transition_prob = "1/2 3/2") {
209  fg->emplace_shared<DiscreteDistribution>(modes[0], "1/1");
210  for (size_t k = 0; k < K - 2; k++) {
211  auto parents = {modes[k]};
212  fg->emplace_shared<DiscreteConditional>(modes[k + 1], parents,
213  discrete_transition_prob);
214  }
215  }
216 
224  std::string discrete_transition_prob = "1/2 3/2") {
225  fg->emplace_shared<DiscreteDistribution>(modes[0], "1/1");
226  for (size_t k = 0; k < K - 2; k++) {
227  auto parents = {modes[k]};
228  fg->emplace_shared<DiscreteConditional>(modes[k + 1], parents,
229  discrete_transition_prob);
230  }
231  }
232 };
233 
234 } // namespace gtsam
void addModeChain(HybridGaussianFactorGraph *fg, std::string discrete_transition_prob="1/2 3/2")
Add "mode chain" to HybridGaussianFactorGraph from M(0) to M(K-2). E.g. if K=4, we want M0...
Definition: Switching.h:223
HybridGaussianFactorGraph::shared_ptr makeSwitchingChain(size_t n, std::function< Key(int)> keyFunc=X, std::function< Key(int)> dKeyFunc=M)
Create a switching system chain. A switching system is a continuous system which depends on a discret...
Definition: Switching.h:51
Key M(std::uint64_t j)
A set of GaussianFactors, indexed by a set of discrete keys.
Matrix< RealScalar, Dynamic, Dynamic > M
Definition: bench_gemm.cpp:51
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:85
static const Eigen::MatrixBase< Vector3 >::ConstantReturnType Z_3x1
Definition: Vector.h:46
Nonlinear hybrid factor graph that uses type erasure.
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:196
Switching(size_t K, double between_sigma=1.0, double prior_sigma=0.1, std::vector< double > measurements={}, std::string discrete_transition_prob="1/2 3/2")
Create with given number of time steps.
Definition: Switching.h:135
Implementation of a discrete conditional mixture factor. Implements a joint discrete-continuous facto...
int n
Double_ distance(const OrientedPlane3_ &p)
Key X(std::uint64_t j)
Nonlinear Mixture factor of continuous and discrete.
IsDerived< DERIVEDFACTOR > add(std::shared_ptr< DERIVEDFACTOR > factor)
add is a synonym for push_back.
Definition: FactorGraph.h:214
Key C(std::uint64_t j)
void addModeChain(HybridNonlinearFactorGraph *fg, std::string discrete_transition_prob="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
HybridGaussianFactorGraph linearizedFactorGraph
Definition: Switching.h:124
static std::vector< MotionModel::shared_ptr > motionModels(size_t k, double sigma=1.0)
Definition: Switching.h:191
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
DiscreteKeys modes
Definition: Switching.h:122
std::shared_ptr< This > shared_ptr
shared_ptr to This
Linearized Hybrid factor graph that uses type erasure.
traits
Definition: chartTesting.h:28
Values linearizationPoint
Definition: Switching.h:125
HybridNonlinearFactorGraph nonlinearFactorGraph
Definition: Switching.h:123
void reverse(const MatrixType &m)
static EIGEN_DEPRECATED const end_t end
std::shared_ptr< HybridGaussianFactorGraph > linearize(const Values &continuousValues) const
Linearize all the continuous factors in the HybridNonlinearFactorGraph.
static const double sigma
void insert(Key j, const Value &val)
Definition: Values.cpp:155
Implementation of a discrete conditional mixture factor.
Definition: MixtureFactor.h:47
const KeyVector keys
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
#define X
Definition: icosphere.cpp:20
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
Point2 t(10, 10)
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
Definition: NoiseModel.cpp:594
DiscreteKeys is a set of keys that can be assembled using the & operator.
Definition: DiscreteKey.h:41


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:36:34