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

use templetized constructor for MixtureFactor

release/4.3a0
Varun Agrawal 2022-08-10 18:37:24 -04:00
parent fbceda3e56
commit 0e4db30713
2 changed files with 19 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
gtsam/hybrid/MixtureFactor.h
View File

@ -86,17 +86,26 @@ class MixtureFactor : public HybridFactor {
* elements based on the number of discrete keys and the cardinality of the * elements based on the number of discrete keys and the cardinality of the
* keys, so that the decision tree is constructed appropriately. * keys, so that the decision tree is constructed appropriately.
* *
* @tparam FACTOR The type of the factor shared pointers being passed in. Will
* be typecast to NonlinearFactor shared pointers.
* @param keys Vector of keys for continuous factors. * @param keys Vector of keys for continuous factors.
* @param discreteKeys Vector of discrete keys. * @param discreteKeys Vector of discrete keys.
* @param factors Vector of shared pointers to factors. * @param factors Vector of shared pointers to factors.
* @param normalized Flag indicating if the factor error is already * @param normalized Flag indicating if the factor error is already
* normalized. * normalized.
*/ */
template <typename FACTOR>
MixtureFactor(const KeyVector& keys, const DiscreteKeys& discreteKeys, MixtureFactor(const KeyVector& keys, const DiscreteKeys& discreteKeys,
const std::vector<sharedFactor>& factors, const std::vector<boost::shared_ptr<FACTOR>>& factors,
bool normalized = false) bool normalized = false)
: MixtureFactor(keys, discreteKeys, Factors(discreteKeys, factors), : Base(keys, discreteKeys), normalized_(normalized) {
normalized) {} std::vector<NonlinearFactor::shared_ptr> nonlinear_factors;
for (auto&& f : factors) {
nonlinear_factors.push_back(
boost::dynamic_pointer_cast<NonlinearFactor>(f));
}
factors_ = Factors(discreteKeys, nonlinear_factors);
}
~MixtureFactor() = default; ~MixtureFactor() = default;

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

@ -105,9 +105,7 @@ TEST(HybridGaussianFactorGraph, Resize) {
auto still = boost::make_shared<MotionModel>(X(0), X(1), 0.0, noise_model), auto still = boost::make_shared<MotionModel>(X(0), X(1), 0.0, noise_model),
moving = boost::make_shared<MotionModel>(X(0), X(1), 1.0, noise_model); moving = boost::make_shared<MotionModel>(X(0), X(1), 1.0, noise_model);
// TODO(Varun) This is declared as NonlinearFactor instead of MotionModel, aka std::vector<MotionModel::shared_ptr> components = {still, moving};
// not clear!!
std::vector<NonlinearFactor::shared_ptr> components = {still, moving};
auto dcFactor = boost::make_shared<MixtureFactor>( auto dcFactor = boost::make_shared<MixtureFactor>(
contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components); contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components);
nhfg.push_back(dcFactor); nhfg.push_back(dcFactor);
@ -431,14 +429,15 @@ TEST(HybridFactorGraph, Full_Elimination) {
linearizedFactorGraph.eliminatePartialSequential(ordering); linearizedFactorGraph.eliminatePartialSequential(ordering);
DiscreteFactorGraph discrete_fg; DiscreteFactorGraph discrete_fg;
//TODO(Varun) Make this a function of HybridGaussianFactorGraph? // TODO(Varun) Make this a function of HybridGaussianFactorGraph?
for(HybridFactor::shared_ptr& factor: (*remainingFactorGraph_partial)) { for (HybridFactor::shared_ptr& factor : (*remainingFactorGraph_partial)) {
auto df = dynamic_pointer_cast<HybridDiscreteFactor>(factor); auto df = dynamic_pointer_cast<HybridDiscreteFactor>(factor);
discrete_fg.push_back(df->inner()); discrete_fg.push_back(df->inner());
} }
ordering.clear(); ordering.clear();
for (size_t k = 1; k < self.K; k++) ordering += M(k); for (size_t k = 1; k < self.K; k++) ordering += M(k);
discreteBayesNet = *discrete_fg.eliminateSequential(ordering, EliminateForMPE); discreteBayesNet =
*discrete_fg.eliminateSequential(ordering, EliminateForMPE);
} }
// Create ordering. // Create ordering.
@ -678,19 +677,13 @@ TEST(HybridFactorGraph, DefaultDecisionTree) {
Pose2 odometry(2.0, 0.0, 0.0); Pose2 odometry(2.0, 0.0, 0.0);
KeyVector contKeys = {X(0), X(1)}; KeyVector contKeys = {X(0), X(1)};
auto noise_model = noiseModel::Isotropic::Sigma(3, 1.0); auto noise_model = noiseModel::Isotropic::Sigma(3, 1.0);
auto still = boost::make_shared<PlanarMotionModel>(X(0), X(1), Pose2(0, 0, auto still = boost::make_shared<PlanarMotionModel>(X(0), X(1), Pose2(0, 0, 0),
0),
noise_model), noise_model),
moving = boost::make_shared<PlanarMotionModel>(X(0), X(1), odometry, moving = boost::make_shared<PlanarMotionModel>(X(0), X(1), odometry,
noise_model); noise_model);
std::vector<PlanarMotionModel::shared_ptr> motion_models = {still, moving}; std::vector<PlanarMotionModel::shared_ptr> motion_models = {still, moving};
//TODO(Varun) Make a templated constructor for MixtureFactor which does this?
std::vector<NonlinearFactor::shared_ptr> components;
for (auto&& f : motion_models) {
components.push_back(boost::dynamic_pointer_cast<NonlinearFactor>(f));
}
fg.emplace_hybrid<MixtureFactor>( fg.emplace_hybrid<MixtureFactor>(
contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components); contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, motion_models);
// Add Range-Bearing measurements to from X0 to L0 and X1 to L1. // Add Range-Bearing measurements to from X0 to L0 and X1 to L1.
// create a noise model for the landmark measurements // create a noise model for the landmark measurements