gtsam 4.2.0
gtsam
HybridValues.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
18#pragma once
19
23#include <gtsam/inference/Key.h>
26
27#include <map>
28#include <string>
29#include <vector>
30
31namespace gtsam {
32
38class GTSAM_EXPORT HybridValues {
39 private:
41 VectorValues continuous_;
42
44 DiscreteValues discrete_;
45
47 Values nonlinear_;
48
49 public:
52
54 HybridValues() = default;
55
58 : continuous_(cv), discrete_(dv){};
59
62 const Values& v)
63 : continuous_(cv), discrete_(dv), nonlinear_(v){};
64
68
70 void print(const std::string& s = "HybridValues",
71 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
72 std::cout << s << ": \n";
73 continuous_.print(" Continuous",
74 keyFormatter); // print continuous components
75 discrete_.print(" Discrete", keyFormatter); // print discrete components
76 };
77
79 bool equals(const HybridValues& other, double tol = 1e-9) const {
80 return continuous_.equals(other.continuous_, tol) &&
81 discrete_.equals(other.discrete_, tol);
82 }
83
87
89 const VectorValues& continuous() const { return continuous_; }
90
92 const DiscreteValues& discrete() const { return discrete_; }
93
95 const Values& nonlinear() const { return nonlinear_; }
96
98 bool existsVector(Key j) { return continuous_.exists(j); };
99
101 bool existsDiscrete(Key j) { return (discrete_.find(j) != discrete_.end()); };
102
105 return nonlinear_.exists(j);
106 };
107
109 bool exists(Key j) {
110 return existsVector(j) || existsDiscrete(j) || existsNonlinear(j);
111 };
112
117 void insert(Key j, const Vector& value) { continuous_.insert(j, value); }
118
123 void insert(Key j, size_t value) { discrete_[j] = value; };
124
126 void insert_or_assign(Key j, const Vector& value) {
127 continuous_.insert_or_assign(j, value);
128 }
129
131 void insert_or_assign(Key j, size_t value) {
132 discrete_[j] = value;
133 }
134
138 continuous_.insert(values);
139 return *this;
140 }
141
145 discrete_.insert(values);
146 return *this;
147 }
148
151 HybridValues& insert(const Values& values) {
152 nonlinear_.insert(values);
153 return *this;
154 }
155
159 continuous_.insert(values.continuous());
160 discrete_.insert(values.discrete());
161 nonlinear_.insert(values.nonlinear());
162 return *this;
163 }
164
169 Vector& at(Key j) { return continuous_.at(j); };
170
175 size_t& atDiscrete(Key j) { return discrete_.at(j); };
176
182 continuous_.update(values);
183 return *this;
184 }
185
191 discrete_.update(values);
192 return *this;
193 }
194
200 continuous_.update(values.continuous());
201 discrete_.update(values.discrete());
202 return *this;
203 }
204
207 VectorValues measurements;
208 for (const auto& key : keys) {
209 measurements.insert(key, continuous_.at(key));
210 }
211 return measurements;
212 }
213
217
224 std::string html(
225 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
226 std::stringstream ss;
227 ss << this->continuous_.html(keyFormatter);
228 ss << this->discrete_.html(keyFormatter);
229 return ss.str();
230 };
231
233};
234
235// traits
236template <>
237struct traits<HybridValues> : public Testable<HybridValues> {};
238
239} // namespace gtsam
specialized key for discrete variables
An assignment from labels to a discrete value index (size_t)
Factor Graph Values.
A non-templated config holding any types of Manifold-group elements.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:100
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition: Key.h:35
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:151
A map from keys to values.
Definition: DiscreteValues.h:34
std::string html(const KeyFormatter &keyFormatter=DefaultKeyFormatter, const Names &names={}) const
Output as a html table.
Definition: DiscreteValues.cpp:104
DiscreteValues & update(const DiscreteValues &values)
For all key/value pairs in values, replace values with corresponding keys in this object with those i...
Definition: DiscreteValues.cpp:63
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
print required by Testable.
Definition: DiscreteValues.cpp:31
bool equals(const DiscreteValues &x, double tol=1e-9) const
equals required by Testable for unit testing.
Definition: DiscreteValues.cpp:40
HybridValues represents a collection of DiscreteValues and VectorValues.
Definition: HybridValues.h:38
void print(const std::string &s="HybridValues", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
print required by Testable for unit testing
Definition: HybridValues.h:70
void insert(Key j, const Vector &value)
Insert a vector value with key j.
Definition: HybridValues.h:117
HybridValues & insert(const DiscreteValues &values)
Insert all discrete values from values.
Definition: HybridValues.h:144
HybridValues(const VectorValues &cv, const DiscreteValues &dv, const Values &v)
Construct from all values types.
Definition: HybridValues.h:61
std::string html(const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Output as a html table.
Definition: HybridValues.h:224
VectorValues continuousSubset(const KeyVector &keys) const
Extract continuous values with given keys.
Definition: HybridValues.h:206
void insert_or_assign(Key j, const Vector &value)
insert_or_assign() , similar to Values.h
Definition: HybridValues.h:126
HybridValues & insert(const Values &values)
Insert all values from values.
Definition: HybridValues.h:151
bool equals(const HybridValues &other, double tol=1e-9) const
equals required by Testable for unit testing
Definition: HybridValues.h:79
HybridValues & update(const VectorValues &values)
For all key/value pairs in values, replace continuous values with corresponding keys in this object w...
Definition: HybridValues.h:181
const DiscreteValues & discrete() const
Return the discrete values.
Definition: HybridValues.h:92
bool existsDiscrete(Key j)
Check whether a variable with key j exists in DiscreteValues.
Definition: HybridValues.h:101
const VectorValues & continuous() const
Return the multi-dimensional vector values.
Definition: HybridValues.h:89
Vector & at(Key j)
Read/write access to the vector value with key j, throws std::out_of_range if j does not exist.
Definition: HybridValues.h:169
bool exists(Key j)
Check whether a variable with key j exists.
Definition: HybridValues.h:109
HybridValues & update(const DiscreteValues &values)
For all key/value pairs in values, replace discrete values with corresponding keys in this object wit...
Definition: HybridValues.h:190
HybridValues & insert(const HybridValues &values)
Insert all values from values.
Definition: HybridValues.h:158
HybridValues & insert(const VectorValues &values)
Insert all continuous values from values.
Definition: HybridValues.h:137
void insert(Key j, size_t value)
Insert a discrete value with key j.
Definition: HybridValues.h:123
const Values & nonlinear() const
Return the nonlinear values.
Definition: HybridValues.h:95
bool existsNonlinear(Key j)
Check whether a variable with key j exists in values.
Definition: HybridValues.h:104
HybridValues & update(const HybridValues &values)
For all key/value pairs in values, replace all values with corresponding keys in this object with tho...
Definition: HybridValues.h:199
HybridValues()=default
Default constructor creates an empty HybridValues.
bool existsVector(Key j)
Check whether a variable with key j exists in VectorValues.
Definition: HybridValues.h:98
HybridValues(const VectorValues &cv, const DiscreteValues &dv)
Construct from DiscreteValues and VectorValues.
Definition: HybridValues.h:57
size_t & atDiscrete(Key j)
Read/write access to the discrete value with key j, throws std::out_of_range if j does not exist.
Definition: HybridValues.h:175
void insert_or_assign(Key j, size_t value)
insert_or_assign() , similar to Values.h
Definition: HybridValues.h:131
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition: VectorValues.h:74
iterator insert(const std::pair< Key, Vector > &key_value)
Insert a vector value with key j.
Definition: VectorValues.cpp:91
void print(const std::string &str="VectorValues", const KeyFormatter &formatter=DefaultKeyFormatter) const
print required by Testable for unit testing
Definition: VectorValues.cpp:158
bool equals(const VectorValues &x, double tol=1e-9) const
equals required by Testable for unit testing
Definition: VectorValues.cpp:166
bool exists(Key j) const
Check whether a variable with key j exists.
Definition: VectorValues.h:133
std::string html(const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Output as a html table.
Definition: VectorValues.cpp:387
void insert_or_assign(Key j, const Vector &value)
insert_or_assign that mimics the STL map insert_or_assign - if the value already exists,...
Definition: VectorValues.h:219
Vector & at(Key j)
Read/write access to the vector value with key j, throws std::out_of_range if j does not exist,...
Definition: VectorValues.h:139
VectorValues & update(const VectorValues &values)
For all key/value pairs in values, replace values with corresponding keys in this class with those in...
Definition: VectorValues.cpp:101
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:65
void insert(Key j, const Value &val)
Add a variable with the given j, throws KeyAlreadyExists<J> if j is already present.
Definition: Values.cpp:157
bool exists(Key j) const
Check if a value exists with key j.
Definition: Values.cpp:94