Go to the documentation of this file.
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") {
62 for (
size_t k = 1; k <
K; k++) {
64 std::vector<GaussianFactor::shared_ptr>
components;
73 transitionProbabilityTable));
77 return std::make_shared<HybridGaussianFactorGraph>(std::move(hfg));
90 std::vector<Key> &input) {
93 std::vector<int> levels(input.size());
94 std::function<void(std::vector<Key>::iterator, std::vector<Key>::iterator,
96 bsg = [&bsg, &new_order, &levels, &input](
97 std::vector<Key>::iterator begin,
98 std::vector<Key>::iterator
end,
int lvl) {
100 std::vector<Key>::iterator pivot =
103 new_order.push_back(*pivot);
105 bsg(begin, pivot, lvl + 1);
106 bsg(pivot + 1,
end, lvl + 1);
108 new_order.push_back(*begin);
113 bsg(input.begin(), input.end(), 0);
116 return {new_order, levels};
140 Switching(
size_t K,
double between_sigma = 1.0,
double prior_sigma = 0.1,
142 std::string transitionProbabilityTable =
"1/2 3/2")
147 for (
size_t k = 0; k <
K - 1; k++) {
148 modes.emplace_back(
M(k), 2);
153 for (
size_t k = 0; k <
K; k++) {
165 for (
size_t k = 0; k <
K - 1; k++) {
172 auto measurement_noise = Isotropic::Sigma(1, prior_sigma);
173 for (
size_t k = 1; k <
K; k++) {
182 for (
size_t k = 0; k <
K; k++) {
192 size_t k,
double sigma = 1.0) {
195 std::make_shared<MotionModel>(
X(k),
X(k + 1), 0.0,
noise_model),
197 std::make_shared<MotionModel>(
X(k),
X(k + 1), 1.0,
noise_model);
198 return {still, moving};
208 std::string transitionProbabilityTable =
"1/2 3/2") {
211 for (
size_t k = 0; k <
K - 2; k++) {
212 auto parents = {
modes[k]};
214 transitionProbabilityTable);
std::shared_ptr< This > shared_ptr
shared_ptr to This
Linear Factor Graph where all factors are Gaussians.
static const Eigen::MatrixBase< Vector3 >::ConstantReturnType Z_3x1
Nonlinear hybrid factor graph that uses type erasure.
typedef and functions to augment Eigen's MatrixXd
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
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.
DiscreteKeys is a set of keys that can be assembled using the & operator.
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
A set of GaussianFactors, indexed by a set of discrete keys.
HybridGaussianFactorGraph linearizedFactorGraph() const
Get the full linear factor graph.
HybridNonlinearFactorGraph nonlinearFactorGraph() const
Get all the nonlinear factors.
static const double sigma
ϕ(X(0)) .. ϕ(X(k),X(k+1)) .. ϕ(X(k);z_k) .. ϕ(M(0)) .. ϕ(M(K-3),M(K-2))
HybridGaussianFactorGraph linearUnaryFactors
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...
Non-linear factor base classes.
static std::vector< NoiseModelFactor::shared_ptr > motionModels(size_t k, double sigma=1.0)
A set of nonlinear factors indexed by a set of discrete keys.
HybridNonlinearFactorGraph modeChain
Linearized Hybrid factor graph that uses type erasure.
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.
std::vector< double > measurements
HybridGaussianFactorGraph linearBinaryFactors
Values linearizationPoint
Implementation of a discrete-conditioned hybrid factor. Implements a joint discrete-continuous factor...
void insert(Key j, const Value &val)
std::shared_ptr< HybridGaussianFactorGraph > linearize(const Values &continuousValues) const
Linearize all the continuous factors in the HybridNonlinearFactorGraph.
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
Double_ distance(const OrientedPlane3_ &p)
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,...
A factor with a quadratic error function - a Gaussian.
void reverse(const MatrixType &m)
std::vector< NoiseModelFactor::shared_ptr > components
static const EIGEN_DEPRECATED end_t end
HybridNonlinearFactorGraph binaryFactors
IsDerived< DERIVEDFACTOR > add(std::shared_ptr< DERIVEDFACTOR > factor)
add is a synonym for push_back.
NonlinearFactorGraph graph
std::uint64_t Key
Integer nonlinear key type.
HybridNonlinearFactorGraph unaryFactors
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Matrix< RealScalar, Dynamic, Dynamic > M
gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:05:21