Skip to content

Commit 05a4b7a

Browse files
varunagrawalvarunagrawal
committed
Merge branch 'develop' into direct-hybrid-fg
2 parents 9dc29e0 + 31e2f54 commit 05a4b7a

9 files changed

+321
-184
lines changed

‎.github/workflows/build-python.yml

-7
Original file line numberDiff line numberDiff line change
@@ -29,7 +29,6 @@ jobs:
2929
name:
3030
[
3131
ubuntu-20.04-gcc-9,
32-
ubuntu-20.04-gcc-9-tbb,
3332
ubuntu-20.04-clang-9,
3433
macos-12-xcode-14.2,
3534
macos-14-xcode-15.4,
@@ -44,12 +43,6 @@ jobs:
4443
compiler: gcc
4544
version: "9"
4645

47-
- name: ubuntu-20.04-gcc-9-tbb
48-
os: ubuntu-20.04
49-
compiler: gcc
50-
version: "9"
51-
flag: tbb
52-
5346
- name: ubuntu-20.04-clang-9
5447
os: ubuntu-20.04
5548
compiler: clang

‎gtsam/hybrid/HybridBayesNet.h

+30-14
Original file line numberDiff line numberDiff line change
@@ -70,20 +70,6 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet<HybridConditional> {
7070
factors_.push_back(conditional);
7171
}
7272

73-
/**
74-
* Preferred: add a conditional directly using a pointer.
75-
*
76-
* Examples:
77-
* hbn.emplace_back(new GaussianMixture(...)));
78-
* hbn.emplace_back(new GaussianConditional(...)));
79-
* hbn.emplace_back(new DiscreteConditional(...)));
80-
*/
81-
template <class Conditional>
82-
void emplace_back(Conditional *conditional) {
83-
factors_.push_back(std::make_shared<HybridConditional>(
84-
std::shared_ptr<Conditional>(conditional)));
85-
}
86-
8773
/**
8874
* Add a conditional using a shared_ptr, using implicit conversion to
8975
* a HybridConditional.
@@ -101,6 +87,36 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet<HybridConditional> {
10187
std::make_shared<HybridConditional>(std::move(conditional)));
10288
}
10389

90+
/**
91+
* @brief Add a conditional to the Bayes net.
92+
* Implicitly convert to a HybridConditional.
93+
*
94+
* E.g.
95+
* hbn.push_back(std::make_shared<DiscreteConditional>(m, "1/1"));
96+
*
97+
* @tparam CONDITIONAL Type of conditional. This is shared_ptr version.
98+
* @param conditional The conditional as a shared pointer.
99+
*/
100+
template <class CONDITIONAL>
101+
void push_back(const std::shared_ptr<CONDITIONAL> &conditional) {
102+
factors_.push_back(std::make_shared<HybridConditional>(conditional));
103+
}
104+
105+
/**
106+
* Preferred: Emplace a conditional directly using arguments.
107+
*
108+
* Examples:
109+
* hbn.emplace_shared<GaussianMixture>(...)));
110+
* hbn.emplace_shared<GaussianConditional>(...)));
111+
* hbn.emplace_shared<DiscreteConditional>(...)));
112+
*/
113+
template <class CONDITIONAL, class... Args>
114+
void emplace_shared(Args &&...args) {
115+
auto cond = std::allocate_shared<CONDITIONAL>(
116+
Eigen::aligned_allocator<CONDITIONAL>(), std::forward<Args>(args)...);
117+
factors_.push_back(std::make_shared<HybridConditional>(std::move(cond)));
118+
}
119+
104120
/**
105121
* @brief Get the Gaussian Bayes Net which corresponds to a specific discrete
106122
* value assignment.

‎gtsam/hybrid/HybridValues.cpp

+167
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,167 @@
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 HybridValues.cpp
14+
* @author Varun Agrawal
15+
* @date August 2024
16+
*/
17+
18+
#include <gtsam/discrete/DiscreteValues.h>
19+
#include <gtsam/hybrid/HybridValues.h>
20+
#include <gtsam/inference/Key.h>
21+
#include <gtsam/linear/VectorValues.h>
22+
#include <gtsam/nonlinear/Values.h>
23+
24+
namespace gtsam {
25+
26+
/* ************************************************************************* */
27+
HybridValues::HybridValues(const VectorValues& cv, const DiscreteValues& dv)
28+
: continuous_(cv), discrete_(dv) {}
29+
30+
/* ************************************************************************* */
31+
HybridValues::HybridValues(const VectorValues& cv, const DiscreteValues& dv,
32+
const Values& v)
33+
: continuous_(cv), discrete_(dv), nonlinear_(v) {}
34+
35+
/* ************************************************************************* */
36+
void HybridValues::print(const std::string& s,
37+
const KeyFormatter& keyFormatter) const {
38+
std::cout << s << ": \n";
39+
continuous_.print(" Continuous",
40+
keyFormatter); // print continuous components
41+
discrete_.print(" Discrete", keyFormatter); // print discrete components
42+
}
43+
44+
/* ************************************************************************* */
45+
bool HybridValues::equals(const HybridValues& other, double tol) const {
46+
return continuous_.equals(other.continuous_, tol) &&
47+
discrete_.equals(other.discrete_, tol);
48+
}
49+
50+
/* ************************************************************************* */
51+
const VectorValues& HybridValues::continuous() const { return continuous_; }
52+
53+
/* ************************************************************************* */
54+
const DiscreteValues& HybridValues::discrete() const { return discrete_; }
55+
56+
/* ************************************************************************* */
57+
const Values& HybridValues::nonlinear() const { return nonlinear_; }
58+
59+
/* ************************************************************************* */
60+
bool HybridValues::existsVector(Key j) { return continuous_.exists(j); }
61+
62+
/* ************************************************************************* */
63+
bool HybridValues::existsDiscrete(Key j) {
64+
return (discrete_.find(j) != discrete_.end());
65+
}
66+
67+
/* ************************************************************************* */
68+
bool HybridValues::existsNonlinear(Key j) { return nonlinear_.exists(j); }
69+
70+
/* ************************************************************************* */
71+
bool HybridValues::exists(Key j) {
72+
return existsVector(j) || existsDiscrete(j) || existsNonlinear(j);
73+
}
74+
75+
/* ************************************************************************* */
76+
HybridValues HybridValues::retract(const VectorValues& delta) const {
77+
HybridValues updated(continuous_, discrete_, nonlinear_.retract(delta));
78+
return updated;
79+
}
80+
81+
/* ************************************************************************* */
82+
void HybridValues::insert(Key j, const Vector& value) {
83+
continuous_.insert(j, value);
84+
}
85+
86+
/* ************************************************************************* */
87+
void HybridValues::insert(Key j, size_t value) { discrete_[j] = value; }
88+
89+
/* ************************************************************************* */
90+
void HybridValues::insert_or_assign(Key j, const Vector& value) {
91+
continuous_.insert_or_assign(j, value);
92+
}
93+
94+
/* ************************************************************************* */
95+
void HybridValues::insert_or_assign(Key j, size_t value) {
96+
discrete_[j] = value;
97+
}
98+
99+
/* ************************************************************************* */
100+
HybridValues& HybridValues::insert(const VectorValues& values) {
101+
continuous_.insert(values);
102+
return *this;
103+
}
104+
105+
/* ************************************************************************* */
106+
HybridValues& HybridValues::insert(const DiscreteValues& values) {
107+
discrete_.insert(values);
108+
return *this;
109+
}
110+
111+
/* ************************************************************************* */
112+
HybridValues& HybridValues::insert(const Values& values) {
113+
nonlinear_.insert(values);
114+
return *this;
115+
}
116+
117+
/* ************************************************************************* */
118+
HybridValues& HybridValues::insert(const HybridValues& values) {
119+
continuous_.insert(values.continuous());
120+
discrete_.insert(values.discrete());
121+
nonlinear_.insert(values.nonlinear());
122+
return *this;
123+
}
124+
125+
/* ************************************************************************* */
126+
Vector& HybridValues::at(Key j) { return continuous_.at(j); }
127+
128+
/* ************************************************************************* */
129+
size_t& HybridValues::atDiscrete(Key j) { return discrete_.at(j); }
130+
131+
/* ************************************************************************* */
132+
HybridValues& HybridValues::update(const VectorValues& values) {
133+
continuous_.update(values);
134+
return *this;
135+
}
136+
137+
/* ************************************************************************* */
138+
HybridValues& HybridValues::update(const DiscreteValues& values) {
139+
discrete_.update(values);
140+
return *this;
141+
}
142+
143+
/* ************************************************************************* */
144+
HybridValues& HybridValues::update(const HybridValues& values) {
145+
continuous_.update(values.continuous());
146+
discrete_.update(values.discrete());
147+
return *this;
148+
}
149+
150+
/* ************************************************************************* */
151+
VectorValues HybridValues::continuousSubset(const KeyVector& keys) const {
152+
VectorValues measurements;
153+
for (const auto& key : keys) {
154+
measurements.insert(key, continuous_.at(key));
155+
}
156+
return measurements;
157+
}
158+
159+
/* ************************************************************************* */
160+
std::string HybridValues::html(const KeyFormatter& keyFormatter) const {
161+
std::stringstream ss;
162+
ss << this->continuous_.html(keyFormatter);
163+
ss << this->discrete_.html(keyFormatter);
164+
return ss.str();
165+
}
166+
167+
} // namespace gtsam

0 commit comments

Comments
 (0)