-
Notifications
You must be signed in to change notification settings - Fork 764
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #19 from varunagrawal/feature/hybrid-wrap
- Loading branch information
Showing
10 changed files
with
178 additions
and
14 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,119 @@ | ||
//************************************************************************* | ||
// hybrid | ||
//************************************************************************* | ||
|
||
namespace gtsam { | ||
|
||
// #include <gtsam/inference/Key.h> | ||
// class gtsam::KeyVector; | ||
|
||
#include <gtsam/hybrid/DCFactor.h> | ||
#include <gtsam/slam/BetweenFactor.h> | ||
|
||
virtual class DCFactor {}; | ||
|
||
#include <gtsam/hybrid/DCMixtureFactor.h> | ||
template <T> | ||
virtual class DCMixtureFactor : gtsam::DCFactor { | ||
DCMixtureFactor(); | ||
DCMixtureFactor(const gtsam::KeyVector& keys, | ||
const gtsam::DiscreteKeys& discreteKeys, | ||
const std::vector<T*>& factors, bool normalized = false); | ||
}; | ||
|
||
typedef gtsam::DCMixtureFactor<gtsam::BetweenFactor<double>> | ||
DCMixtureFactorBetweenFactorDouble; | ||
|
||
#include <gtsam/hybrid/DCGaussianMixtureFactor.h> | ||
|
||
virtual class DCGaussianMixtureFactor : gtsam::DCFactor { | ||
DCGaussianMixtureFactor(); | ||
DCGaussianMixtureFactor( | ||
const gtsam::KeyVector& continuousKeys, | ||
const gtsam::DiscreteKeys& discreteKeys, | ||
const gtsam::DCGaussianMixtureFactor::Factors& factors); | ||
}; | ||
|
||
#include <gtsam/hybrid/GaussianMixture.h> | ||
|
||
virtual class GaussianMixture : gtsam::DCGaussianMixtureFactor { | ||
GaussianMixture(); | ||
GaussianMixture(size_t nrFrontals, const gtsam::KeyVector& continuousKeys, | ||
const gtsam::DiscreteKeys& discreteKeys, | ||
const gtsam::GaussianMixture::Conditionals& conditionals); | ||
}; | ||
|
||
#include <gtsam/hybrid/DCFactorGraph.h> | ||
|
||
class DCFactorGraph { | ||
DCFactorGraph(); | ||
gtsam::DiscreteKeys discreteKeys() const; | ||
}; | ||
|
||
#include <gtsam/hybrid/HybridFactorGraph.h> | ||
|
||
template <FG> | ||
virtual class HybridFactorGraph { | ||
HybridFactorGraph(); | ||
HybridFactorGraph(const FG& factorGraph, | ||
const gtsam::DiscreteFactorGraph& discreteGraph, | ||
const gtsam::DCFactorGraph& dcGraph); | ||
|
||
bool equals(const gtsam::HybridFactorGraph<FG>& other, | ||
double tol = 1e-9) const; | ||
void print(const std::string& str = "HybridFactorGraph", | ||
const gtsam::KeyFormatter& keyFormatter = | ||
gtsam::DefaultKeyFormatter) const; | ||
|
||
const gtsam::DiscreteFactorGraph& discreteGraph() const; | ||
const gtsam::DCFactorGraph& dcGraph() const; | ||
|
||
gtsam::DiscreteKeys discreteKeys() const; | ||
}; | ||
|
||
typedef gtsam::HybridFactorGraph<gtsam::NonlinearFactorGraph> | ||
HybridFactorGraphNonlinear; | ||
|
||
#include <gtsam/hybrid/NonlinearHybridFactorGraph.h> | ||
|
||
virtual class NonlinearHybridFactorGraph { | ||
NonlinearHybridFactorGraph(); | ||
NonlinearHybridFactorGraph(const gtsam::NonlinearFactorGraph& nonlinearGraph, | ||
const gtsam::DiscreteFactorGraph& discreteGraph, | ||
const gtsam::DCFactorGraph& dcGraph); | ||
|
||
const gtsam::NonlinearFactorGraph& nonlinearGraph() const; | ||
|
||
gtsam::GaussianHybridFactorGraph linearize( | ||
const gtsam::Values& continuousValues) const; | ||
|
||
size_t size() const; | ||
bool equals(const gtsam::NonlinearHybridFactorGraph& other, | ||
double tol = 1e-9) const; | ||
void print(const std::string& str = "NonlinearHybridFactorGraph", | ||
const gtsam::KeyFormatter& keyFormatter = | ||
gtsam::DefaultKeyFormatter) const; | ||
}; | ||
|
||
#include <gtsam/hybrid/GaussianHybridFactorGraph.h> | ||
|
||
class GaussianHybridFactorGraph { | ||
GaussianHybridFactorGraph(); | ||
GaussianHybridFactorGraph(const gtsam::GaussianFactorGraph& gaussianGraph, | ||
const gtsam::DiscreteFactorGraph& discreteGraph, | ||
const gtsam::DCFactorGraph& dcGraph); | ||
|
||
size_t size() const; | ||
void print(const std::string& str = "GaussianHybridFactorGraph", | ||
const gtsam::KeyFormatter& keyFormatter = | ||
gtsam::DefaultKeyFormatter) const; | ||
}; | ||
|
||
#include <gtsam/hybrid/IncrementalHybrid.h> | ||
|
||
class IncrementalHybrid { | ||
void update(gtsam::GaussianHybridFactorGraph graph, | ||
const gtsam::Ordering& ordering); | ||
}; | ||
|
||
} // namespace gtsam |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,12 @@ | ||
/* Please refer to: | ||
* https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html | ||
* These are required to save one copy operation on Python calls. | ||
* | ||
* NOTES | ||
* ================= | ||
* | ||
* `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11 | ||
* automatic STL binding, such that the raw objects can be accessed in Python. | ||
* Without this they will be automatically converted to a Python object, and all | ||
* mutations on Python side will not be reflected on C++. | ||
*/ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,12 @@ | ||
/* Please refer to: | ||
* https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html | ||
* These are required to save one copy operation on Python calls. | ||
* | ||
* NOTES | ||
* ================= | ||
* | ||
* `py::bind_vector` and similar machinery gives the std container a Python-like | ||
* interface, but without the `<pybind11/stl.h>` copying mechanism. Combined | ||
* with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python, | ||
* and saves one copy operation. | ||
*/ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,13 @@ | ||
import gtsam | ||
import numpy as np | ||
from gtsam import GaussianHybridFactorGraph | ||
from gtsam.utils.test_case import GtsamTestCase | ||
|
||
|
||
class TestHybridElimination(GtsamTestCase): | ||
def setUp(self) -> None: | ||
self.ghfg = GaussianHybridFactorGraph() | ||
|
||
def test_elimination(self): | ||
# Check if constructed correctly | ||
self.assertIsInstance(self.ghfg, GaussianHybridFactorGraph) |