12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Removed emplace_hybrid, can be done with emplace_shared now...

release/4.3a0
Frank Dellaert 2023-01-07 08:15:45 -08:00
parent 84037e173f
commit e6fe9093af
5 changed files with 8 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
gtsam/hybrid/HybridFactorGraph.h
View File

@ -45,17 +45,6 @@ class HybridFactorGraph : public FactorGraph<Factor> {
using Values = gtsam::Values; ///< backwards compatibility
using Indices = KeyVector; ///> map from keys to values
protected:
/// Check if FACTOR type is derived from DiscreteFactor.
template <typename FACTOR>
using IsDiscrete = typename std::enable_if<
std::is_base_of<DiscreteFactor, FACTOR>::value>::type;
/// Check if FACTOR type is derived from HybridFactor.
template <typename FACTOR>
using IsHybrid = typename std::enable_if<
std::is_base_of<HybridFactor, FACTOR>::value>::type;
public:
/// @name Constructors
/// @{
@ -72,38 +61,8 @@ class HybridFactorGraph : public FactorGraph<Factor> {
HybridFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph) : Base(graph) {}
/// @}
// Allow use of selected FactorGraph methods:
using Base::empty;
using Base::reserve;
using Base::size;
using Base::operator[];
using Base::add;
using Base::push_back;
using Base::resize;
/**
* Add a discrete-continuous (Hybrid) factor *pointer* to the graph
* @param hybridFactor - boost::shared_ptr to the factor to add
*/
template <typename FACTOR>
IsHybrid<FACTOR> push_hybrid(const boost::shared_ptr<FACTOR>& hybridFactor) {
Base::push_back(hybridFactor);
}
/// Construct a discrete factor and add shared pointer to the factor graph.
template <class FACTOR, class... Args>
IsDiscrete<FACTOR> emplace_discrete(Args&&... args) {
emplace_shared<FACTOR>(std::forward<Args>(args)...);
}
/// Construct a factor and add (shared pointer to it) to factor graph.
template <class FACTOR, class... Args>
IsHybrid<FACTOR> emplace_hybrid(Args&&... args) {
auto factor = boost::allocate_shared<FACTOR>(
Eigen::aligned_allocator<FACTOR>(), std::forward<Args>(args)...);
push_hybrid(factor);
}
/// @name Extra methods to inspect discrete/continuous keys.
/// @{
/// Get all the discrete keys in the factor graph.
DiscreteKeys discreteKeys() const;
@ -116,6 +75,8 @@ class HybridFactorGraph : public FactorGraph<Factor> {
/// Get all the continuous keys in the factor graph.
const KeySet continuousKeySet() const;
/// @}
};
} // namespace gtsam

2
gtsam/hybrid/tests/Switching.h
View File

@ -163,7 +163,7 @@ struct Switching {
for (auto &&f : motion_models) {
components.push_back(boost::dynamic_pointer_cast<NonlinearFactor>(f));
}
nonlinearFactorGraph.emplace_hybrid<MixtureFactor>(
nonlinearFactorGraph.emplace_shared<MixtureFactor>(
keys, DiscreteKeys{modes[k]}, components);
}

2
gtsam/hybrid/tests/testHybridBayesNet.cpp
View File

@ -337,7 +337,7 @@ TEST(HybridBayesNet, Sampling) {
boost::make_shared<BetweenFactor<double>>(X(0), X(1), 1, noise_model);
std::vector<NonlinearFactor::shared_ptr> factors = {zero_motion, one_motion};
nfg.emplace_shared<PriorFactor<double>>(X(0), 0.0, noise_model);
nfg.emplace_hybrid<MixtureFactor>(
nfg.emplace_shared<MixtureFactor>(
KeyVector{X(0), X(1)}, DiscreteKeys{DiscreteKey(M(0), 2)}, factors);
DiscreteKey mode(M(0), 2);

2
gtsam/hybrid/tests/testHybridEstimation.cpp
View File

@ -416,7 +416,7 @@ static HybridNonlinearFactorGraph createHybridNonlinearFactorGraph() {
boost::make_shared<BetweenFactor<double>>(X(0), X(1), 0, noise_model);
const auto one_motion =
boost::make_shared<BetweenFactor<double>>(X(0), X(1), 1, noise_model);
nfg.emplace_hybrid<MixtureFactor>(
nfg.emplace_shared<MixtureFactor>(
KeyVector{X(0), X(1)}, DiscreteKeys{m},
std::vector<NonlinearFactor::shared_ptr>{zero_motion, one_motion});

4
gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
View File

@ -368,8 +368,6 @@ TEST(HybridGaussianElimination, EliminateHybrid_2_Variable) {
CHECK(discreteFactor);
EXPECT_LONGS_EQUAL(1, discreteFactor->discreteKeys().size());
EXPECT(discreteFactor->root_->isLeaf() == false);
// TODO(Varun) Test emplace_discrete
}
/****************************************************************************
@ -687,7 +685,7 @@ TEST(HybridFactorGraph, DefaultDecisionTree) {
moving = boost::make_shared<PlanarMotionModel>(X(0), X(1), odometry,
noise_model);
std::vector<PlanarMotionModel::shared_ptr> motion_models = {still, moving};
fg.emplace_hybrid<MixtureFactor>(
fg.emplace_shared<MixtureFactor>(
contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, motion_models);
// Add Range-Bearing measurements to from X0 to L0 and X1 to L1.