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

Merge pull request #1843 from borglab/feature/easy_hnf 

Easy HNF constructors
release/4.3a0
Frank Dellaert 2024-09-24 11:25:22 -07:00 committed by GitHub
parent fd7df61d45 5aa5222edb
commit 6c97e4b641
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
12 changed files with 185 additions and 145 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

53
gtsam/hybrid/HybridNonlinearFactor.cpp
View File

@ -21,10 +21,57 @@
namespace gtsam {
/* *******************************************************************************/
HybridNonlinearFactor::HybridNonlinearFactor(const KeyVector& keys,
static void checkKeys(const KeyVector& continuousKeys,
std::vector<NonlinearFactorValuePair>& pairs) {
KeySet factor_keys_set;
for (const auto& pair : pairs) {
auto f = pair.first;
// Insert all factor continuous keys in the continuous keys set.
std::copy(f->keys().begin(), f->keys().end(),
std::inserter(factor_keys_set, factor_keys_set.end()));
}
KeySet continuous_keys_set(continuousKeys.begin(), continuousKeys.end());
if (continuous_keys_set != factor_keys_set) {
throw std::runtime_error(
"HybridNonlinearFactor: The specified continuous keys and the keys in "
"the factors do not match!");
}
}
/* *******************************************************************************/
HybridNonlinearFactor::HybridNonlinearFactor(
const KeyVector& continuousKeys, const DiscreteKey& discreteKey,
const std::vector<NonlinearFactor::shared_ptr>& factors)
: Base(continuousKeys, {discreteKey}) {
std::vector<NonlinearFactorValuePair> pairs;
for (auto&& f : factors) {
pairs.emplace_back(f, 0.0);
}
checkKeys(continuousKeys, pairs);
factors_ = FactorValuePairs({discreteKey}, pairs);
}
/* *******************************************************************************/
HybridNonlinearFactor::HybridNonlinearFactor(
const KeyVector& continuousKeys, const DiscreteKey& discreteKey,
const std::vector<NonlinearFactorValuePair>& factors)
: Base(continuousKeys, {discreteKey}) {
std::vector<NonlinearFactorValuePair> pairs;
KeySet continuous_keys_set(continuousKeys.begin(), continuousKeys.end());
KeySet factor_keys_set;
for (auto&& [f, val] : factors) {
pairs.emplace_back(f, val);
}
checkKeys(continuousKeys, pairs);
factors_ = FactorValuePairs({discreteKey}, pairs);
}
/* *******************************************************************************/
HybridNonlinearFactor::HybridNonlinearFactor(const KeyVector& continuousKeys,
const DiscreteKeys& discreteKeys,
const Factors& factors)
: Base(keys, discreteKeys), factors_(factors) {}
const FactorValuePairs& factors)
: Base(continuousKeys, discreteKeys), factors_(factors) {}
/* *******************************************************************************/
AlgebraicDecisionTree<Key> HybridNonlinearFactor::errorTree(

90
gtsam/hybrid/HybridNonlinearFactor.h
View File

@ -68,11 +68,11 @@ class GTSAM_EXPORT HybridNonlinearFactor : public HybridFactor {
* @brief typedef for DecisionTree which has Keys as node labels and
* pairs of NonlinearFactor & an arbitrary scalar as leaf nodes.
*/
using Factors = DecisionTree<Key, NonlinearFactorValuePair>;
using FactorValuePairs = DecisionTree<Key, NonlinearFactorValuePair>;
private:
/// Decision tree of Gaussian factors indexed by discrete keys.
Factors factors_;
/// Decision tree of nonlinear factors indexed by discrete keys.
FactorValuePairs factors_;
/// HybridFactor method implementation. Should not be used.
AlgebraicDecisionTree<Key> errorTree(
@ -82,62 +82,49 @@ class GTSAM_EXPORT HybridNonlinearFactor : public HybridFactor {
}
public:
/// Default constructor, mainly for serialization.
HybridNonlinearFactor() = default;
/**
* @brief Construct from Decision tree.
* @brief Construct a new HybridNonlinearFactor on a single discrete key,
* providing the factors for each mode m as a vector of factors ϕ_m(x).
* The value ϕ(x,m) for the factor is simply ϕ_m(x).
*
* @param keys Vector of keys for continuous factors.
* @param discreteKeys Vector of discrete keys.
* @param factors Decision tree with of shared factors.
* @param continuousKeys Vector of keys for continuous factors.
* @param discreteKey The discrete key for the "mode", indexing components.
* @param factors Vector of gaussian factors, one for each mode.
*/
HybridNonlinearFactor(const KeyVector& keys, const DiscreteKeys& discreteKeys,
const Factors& factors);
HybridNonlinearFactor(
const KeyVector& continuousKeys, const DiscreteKey& discreteKey,
const std::vector<NonlinearFactor::shared_ptr>& factors);
/**
* @brief Convenience constructor that generates the underlying factor
* decision tree for us.
* @brief Construct a new HybridNonlinearFactor on a single discrete key,
* including a scalar error value for each mode m. The factors and scalars are
* provided as a vector of pairs (ϕ_m(x), E_m).
* The value ϕ(x,m) for the factor is now ϕ_m(x) + E_m.
*
* Here it is important that the vector of factors has the correct number of
* elements based on the number of discrete keys and the cardinality of the
* 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 discreteKey The discrete key indexing each component factor.
* @param factors Vector of nonlinear factor and scalar pairs.
* Same size as the cardinality of discreteKey.
* @param continuousKeys Vector of keys for continuous factors.
* @param discreteKey The discrete key for the "mode", indexing components.
* @param factors Vector of gaussian factor-scalar pairs, one per mode.
*/
template <typename FACTOR>
HybridNonlinearFactor(
const KeyVector& keys, const DiscreteKey& discreteKey,
const std::vector<std::pair<std::shared_ptr<FACTOR>, double>>& factors)
: Base(keys, {discreteKey}) {
std::vector<NonlinearFactorValuePair> nonlinear_factors;
KeySet continuous_keys_set(keys.begin(), keys.end());
KeySet factor_keys_set;
for (auto&& [f, val] : factors) {
// Insert all factor continuous keys in the continuous keys set.
std::copy(f->keys().begin(), f->keys().end(),
std::inserter(factor_keys_set, factor_keys_set.end()));
if (auto nf = std::dynamic_pointer_cast<NonlinearFactor>(f)) {
nonlinear_factors.emplace_back(nf, val);
} else {
throw std::runtime_error(
"Factors passed into HybridNonlinearFactor need to be nonlinear!");
}
}
factors_ = Factors({discreteKey}, nonlinear_factors);
if (continuous_keys_set != factor_keys_set) {
throw std::runtime_error(
"The specified continuous keys and the keys in the factors don't "
"match!");
}
}
HybridNonlinearFactor(const KeyVector& continuousKeys,
const DiscreteKey& discreteKey,
const std::vector<NonlinearFactorValuePair>& factors);
/**
* @brief Construct a new HybridNonlinearFactor on a several discrete keys M,
* including a scalar error value for each assignment m. The factors and
* scalars are provided as a DecisionTree<Key> of pairs (ϕ_M(x), E_M).
* The value ϕ(x,M) for the factor is again ϕ_m(x) + E_m.
*
* @param continuousKeys A vector of keys representing continuous variables.
* @param discreteKeys Discrete variables and their cardinalities.
* @param factors The decision tree of nonlinear factor/scalar pairs.
*/
HybridNonlinearFactor(const KeyVector& continuousKeys,
const DiscreteKeys& discreteKeys,
const FactorValuePairs& factors);
/**
* @brief Compute error of the HybridNonlinearFactor as a tree.
*
@ -196,4 +183,9 @@ class GTSAM_EXPORT HybridNonlinearFactor : public HybridFactor {
const Values& continuousValues) const;
};
// traits
template <>
struct traits<HybridNonlinearFactor> : public Testable<HybridNonlinearFactor> {
};
} // namespace gtsam

10
gtsam/hybrid/hybrid.i
View File

@ -246,14 +246,18 @@ class HybridNonlinearFactorGraph {
#include <gtsam/hybrid/HybridNonlinearFactor.h>
class HybridNonlinearFactor : gtsam::HybridFactor {
HybridNonlinearFactor(
const gtsam::KeyVector& keys, const gtsam::DiscreteKeys& discreteKeys,
const gtsam::DecisionTree<
gtsam::Key, std::pair<gtsam::NonlinearFactor*, double>>& factors);
const gtsam::KeyVector& keys, const gtsam::DiscreteKey& discreteKey,
const std::vector<gtsam::NonlinearFactor*>& factors);
HybridNonlinearFactor(
const gtsam::KeyVector& keys, const gtsam::DiscreteKey& discreteKey,
const std::vector<std::pair<gtsam::NonlinearFactor*, double>>& factors);
HybridNonlinearFactor(
const gtsam::KeyVector& keys, const gtsam::DiscreteKeys& discreteKeys,
const gtsam::DecisionTree<
gtsam::Key, std::pair<gtsam::NonlinearFactor*, double>>& factors);
double error(const gtsam::Values& continuousValues,
const gtsam::DiscreteValues& discreteValues) const;

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

@ -161,13 +161,8 @@ struct Switching {
for (size_t k = 0; k < K - 1; k++) {
KeyVector keys = {X(k), X(k + 1)};
auto motion_models = motionModels(k, between_sigma);
std::vector<NonlinearFactorValuePair> components;
for (auto &&f : motion_models) {
components.push_back(
{std::dynamic_pointer_cast<NonlinearFactor>(f), 0.0});
}
nonlinearFactorGraph.emplace_shared<HybridNonlinearFactor>(keys, modes[k],
components);
motion_models);
}
// Add measurement factors
@ -191,8 +186,8 @@ struct Switching {
}
// Create motion models for a given time step
static std::vector<MotionModel::shared_ptr> motionModels(size_t k,
double sigma = 1.0) {
static std::vector<NonlinearFactor::shared_ptr> motionModels(
size_t k, double sigma = 1.0) {
auto noise_model = noiseModel::Isotropic::Sigma(1, sigma);
auto still =
std::make_shared<MotionModel>(X(k), X(k + 1), 0.0, noise_model),

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

@ -391,8 +391,7 @@ TEST(HybridBayesNet, Sampling) {
std::make_shared<BetweenFactor<double>>(X(0), X(1), 1, noise_model);
nfg.emplace_shared<HybridNonlinearFactor>(
KeyVector{X(0), X(1)}, DiscreteKey(M(0), 2),
std::vector<NonlinearFactorValuePair>{{zero_motion, 0.0},
{one_motion, 0.0}});
std::vector<NonlinearFactor::shared_ptr>{zero_motion, one_motion});
DiscreteKey mode(M(0), 2);
nfg.emplace_shared<DiscreteDistribution>(mode, "1/1");

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

@ -435,8 +435,8 @@ static HybridNonlinearFactorGraph createHybridNonlinearFactorGraph() {
std::make_shared<BetweenFactor<double>>(X(0), X(1), 0, noise_model);
const auto one_motion =
std::make_shared<BetweenFactor<double>>(X(0), X(1), 1, noise_model);
std::vector<NonlinearFactorValuePair> components = {{zero_motion, 0.0},
{one_motion, 0.0}};
std::vector<NonlinearFactor::shared_ptr> components = {zero_motion,
one_motion};
nfg.emplace_shared<HybridNonlinearFactor>(KeyVector{X(0), X(1)}, m,
components);
@ -566,10 +566,8 @@ std::shared_ptr<HybridGaussianFactor> mixedVarianceFactor(
[](const GaussianFactor::shared_ptr& gf) -> GaussianFactorValuePair {
return {gf, 0.0};
});
auto updated_gmf = std::make_shared<HybridGaussianFactor>(
return std::make_shared<HybridGaussianFactor>(
gmf->continuousKeys(), gmf->discreteKeys(), updated_pairs);
return updated_gmf;
}
/****************************************************************************/
@ -591,8 +589,7 @@ TEST(HybridEstimation, ModeSelection) {
X(0), X(1), 0.0, noiseModel::Isotropic::Sigma(d, noise_loose)),
model1 = std::make_shared<MotionModel>(
X(0), X(1), 0.0, noiseModel::Isotropic::Sigma(d, noise_tight));
std::vector<NonlinearFactorValuePair> components = {{model0, 0.0},
{model1, 0.0}};
std::vector<NonlinearFactor::shared_ptr> components = {model0, model1};
KeyVector keys = {X(0), X(1)};
DiscreteKey modes(M(0), 2);
@ -688,8 +685,7 @@ TEST(HybridEstimation, ModeSelection2) {
X(0), X(1), Z_3x1, noiseModel::Isotropic::Sigma(d, noise_loose)),
model1 = std::make_shared<BetweenFactor<Vector3>>(
X(0), X(1), Z_3x1, noiseModel::Isotropic::Sigma(d, noise_tight));
std::vector<NonlinearFactorValuePair> components = {{model0, 0.0},
{model1, 0.0}};
std::vector<NonlinearFactor::shared_ptr> components = {model0, model1};
KeyVector keys = {X(0), X(1)};
DiscreteKey modes(M(0), 2);

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

@ -73,7 +73,7 @@ TEST(HybridGaussianFactor, ConstructorVariants) {
HybridGaussianFactor fromFactors({X(1), X(2)}, m1, {f10, f11});
std::vector<GaussianFactorValuePair> pairs{{f10, 0.0}, {f11, 0.0}};
HybridGaussianFactor fromPairs({X(1), X(2)}, {m1}, pairs);
HybridGaussianFactor fromPairs({X(1), X(2)}, m1, pairs);
assert_equal(fromFactors, fromPairs);
HybridGaussianFactor::FactorValuePairs decisionTree({m1}, pairs);

31
gtsam/hybrid/tests/testHybridGaussianISAM.cpp
View File

@ -416,12 +416,11 @@ TEST(HybridGaussianISAM, NonTrivial) {
Pose2 odometry(1.0, 0.0, 0.0);
KeyVector contKeys = {W(0), W(1)};
auto noise_model = noiseModel::Isotropic::Sigma(3, 1.0);
auto still = std::make_shared<PlanarMotionModel>(W(0), W(1), Pose2(0, 0, 0),
noise_model),
moving = std::make_shared<PlanarMotionModel>(W(0), W(1), odometry,
noise_model);
std::vector<std::pair<PlanarMotionModel::shared_ptr, double>> components = {
{moving, 0.0}, {still, 0.0}};
std::vector<NonlinearFactor::shared_ptr> components;
components.emplace_back(
new PlanarMotionModel(W(0), W(1), odometry, noise_model)); // moving
components.emplace_back(
new PlanarMotionModel(W(0), W(1), Pose2(0, 0, 0), noise_model)); // still
fg.emplace_shared<HybridNonlinearFactor>(
contKeys, gtsam::DiscreteKey(M(1), 2), components);
@ -456,11 +455,11 @@ TEST(HybridGaussianISAM, NonTrivial) {
/*************** Run Round 3 ***************/
// Add odometry factor with discrete modes.
contKeys = {W(1), W(2)};
still = std::make_shared<PlanarMotionModel>(W(1), W(2), Pose2(0, 0, 0),
noise_model);
moving =
std::make_shared<PlanarMotionModel>(W(1), W(2), odometry, noise_model);
components = {{moving, 0.0}, {still, 0.0}};
components.clear();
components.emplace_back(
new PlanarMotionModel(W(1), W(2), odometry, noise_model)); // moving
components.emplace_back(
new PlanarMotionModel(W(1), W(2), Pose2(0, 0, 0), noise_model)); // still
fg.emplace_shared<HybridNonlinearFactor>(
contKeys, gtsam::DiscreteKey(M(2), 2), components);
@ -498,11 +497,11 @@ TEST(HybridGaussianISAM, NonTrivial) {
/*************** Run Round 4 ***************/
// Add odometry factor with discrete modes.
contKeys = {W(2), W(3)};
still = std::make_shared<PlanarMotionModel>(W(2), W(3), Pose2(0, 0, 0),
noise_model);
moving =
std::make_shared<PlanarMotionModel>(W(2), W(3), odometry, noise_model);
components = {{moving, 0.0}, {still, 0.0}};
components.clear();
components.emplace_back(
new PlanarMotionModel(W(2), W(3), odometry, noise_model)); // moving
components.emplace_back(
new PlanarMotionModel(W(2), W(3), Pose2(0, 0, 0), noise_model)); // still
fg.emplace_shared<HybridNonlinearFactor>(
contKeys, gtsam::DiscreteKey(M(3), 2), components);

47
gtsam/hybrid/tests/testHybridNonlinearFactor.cpp
View File

@ -43,24 +43,39 @@ TEST(HybridNonlinearFactor, Constructor) {
HybridNonlinearFactor::iterator it = factor.begin();
CHECK(it == factor.end());
}
/* ************************************************************************* */
namespace test_constructor {
DiscreteKey m1(1, 2);
double between0 = 0.0;
double between1 = 1.0;
Vector1 sigmas = Vector1(1.0);
auto model = noiseModel::Diagonal::Sigmas(sigmas, false);
auto f0 = std::make_shared<BetweenFactor<double>>(X(1), X(2), between0, model);
auto f1 = std::make_shared<BetweenFactor<double>>(X(1), X(2), between1, model);
} // namespace test_constructor
/* ************************************************************************* */
// Test simple to complex constructors...
TEST(HybridGaussianFactor, ConstructorVariants) {
using namespace test_constructor;
HybridNonlinearFactor fromFactors({X(1), X(2)}, m1, {f0, f1});
std::vector<NonlinearFactorValuePair> pairs{{f0, 0.0}, {f1, 0.0}};
HybridNonlinearFactor fromPairs({X(1), X(2)}, m1, pairs);
assert_equal(fromFactors, fromPairs);
HybridNonlinearFactor::FactorValuePairs decisionTree({m1}, pairs);
HybridNonlinearFactor fromDecisionTree({X(1), X(2)}, {m1}, decisionTree);
assert_equal(fromDecisionTree, fromPairs);
}
/* ************************************************************************* */
// Test .print() output.
TEST(HybridNonlinearFactor, Printing) {
DiscreteKey m1(1, 2);
double between0 = 0.0;
double between1 = 1.0;
Vector1 sigmas = Vector1(1.0);
auto model = noiseModel::Diagonal::Sigmas(sigmas, false);
auto f0 =
std::make_shared<BetweenFactor<double>>(X(1), X(2), between0, model);
auto f1 =
std::make_shared<BetweenFactor<double>>(X(1), X(2), between1, model);
std::vector<NonlinearFactorValuePair> factors{{f0, 0.0}, {f1, 0.0}};
HybridNonlinearFactor hybridFactor({X(1), X(2)}, {m1}, factors);
using namespace test_constructor;
HybridNonlinearFactor hybridFactor({X(1), X(2)}, {m1}, {f0, f1});
std::string expected =
R"(Hybrid [x1 x2; 1]
@ -86,9 +101,7 @@ static HybridNonlinearFactor getHybridNonlinearFactor() {
std::make_shared<BetweenFactor<double>>(X(1), X(2), between0, model);
auto f1 =
std::make_shared<BetweenFactor<double>>(X(1), X(2), between1, model);
std::vector<NonlinearFactorValuePair> factors{{f0, 0.0}, {f1, 0.0}};
return HybridNonlinearFactor({X(1), X(2)}, {m1}, factors);
return HybridNonlinearFactor({X(1), X(2)}, m1, {f0, f1});
}
/* ************************************************************************* */

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

@ -115,35 +115,40 @@ TEST(HybridNonlinearFactorGraph, Resize) {
EXPECT_LONGS_EQUAL(fg.size(), 0);
}
/***************************************************************************/
namespace test_motion {
KeyVector contKeys = {X(0), X(1)};
gtsam::DiscreteKey m1(M(1), 2);
auto noise_model = noiseModel::Isotropic::Sigma(1, 1.0);
std::vector<NonlinearFactor::shared_ptr> components = {
std::make_shared<MotionModel>(X(0), X(1), 0.0, noise_model),
std::make_shared<MotionModel>(X(0), X(1), 1.0, noise_model)};
} // namespace test_motion
/***************************************************************************
* Test that the resize method works correctly for a
* HybridGaussianFactorGraph.
*/
TEST(HybridGaussianFactorGraph, Resize) {
HybridNonlinearFactorGraph nhfg;
using namespace test_motion;
HybridNonlinearFactorGraph hnfg;
auto nonlinearFactor = std::make_shared<BetweenFactor<double>>(
X(0), X(1), 0.0, Isotropic::Sigma(1, 0.1));
nhfg.push_back(nonlinearFactor);
hnfg.push_back(nonlinearFactor);
auto discreteFactor = std::make_shared<DecisionTreeFactor>();
nhfg.push_back(discreteFactor);
hnfg.push_back(discreteFactor);
KeyVector contKeys = {X(0), X(1)};
auto noise_model = noiseModel::Isotropic::Sigma(1, 1.0);
auto still = std::make_shared<MotionModel>(X(0), X(1), 0.0, noise_model),
moving = std::make_shared<MotionModel>(X(0), X(1), 1.0, noise_model);
std::vector<std::pair<MotionModel::shared_ptr, double>> components = {
{still, 0.0}, {moving, 0.0}};
auto dcFactor = std::make_shared<HybridNonlinearFactor>(
contKeys, gtsam::DiscreteKey(M(1), 2), components);
nhfg.push_back(dcFactor);
auto dcFactor =
std::make_shared<HybridNonlinearFactor>(contKeys, m1, components);
hnfg.push_back(dcFactor);
Values linearizationPoint;
linearizationPoint.insert<double>(X(0), 0);
linearizationPoint.insert<double>(X(1), 1);
// Generate `HybridGaussianFactorGraph` by linearizing
HybridGaussianFactorGraph gfg = *nhfg.linearize(linearizationPoint);
HybridGaussianFactorGraph gfg = *hnfg.linearize(linearizationPoint);
EXPECT_LONGS_EQUAL(gfg.size(), 3);
@ -156,26 +161,19 @@ TEST(HybridGaussianFactorGraph, Resize) {
* continuous keys provided do not match the keys in the factors.
*/
TEST(HybridGaussianFactorGraph, HybridNonlinearFactor) {
using namespace test_motion;
auto nonlinearFactor = std::make_shared<BetweenFactor<double>>(
X(0), X(1), 0.0, Isotropic::Sigma(1, 0.1));
auto discreteFactor = std::make_shared<DecisionTreeFactor>();
auto noise_model = noiseModel::Isotropic::Sigma(1, 1.0);
auto still = std::make_shared<MotionModel>(X(0), X(1), 0.0, noise_model),
moving = std::make_shared<MotionModel>(X(0), X(1), 1.0, noise_model);
std::vector<std::pair<MotionModel::shared_ptr, double>> components = {
{still, 0.0}, {moving, 0.0}};
// Check for exception when number of continuous keys are under-specified.
KeyVector contKeys = {X(0)};
THROWS_EXCEPTION(std::make_shared<HybridNonlinearFactor>(
contKeys, gtsam::DiscreteKey(M(1), 2), components));
THROWS_EXCEPTION(
std::make_shared<HybridNonlinearFactor>(KeyVector{X(0)}, m1, components));
// Check for exception when number of continuous keys are too many.
contKeys = {X(0), X(1), X(2)};
THROWS_EXCEPTION(std::make_shared<HybridNonlinearFactor>(
contKeys, gtsam::DiscreteKey(M(1), 2), components));
KeyVector{X(0), X(1), X(2)}, m1, components));
}
/*****************************************************************************
@ -832,12 +830,10 @@ TEST(HybridNonlinearFactorGraph, DefaultDecisionTree) {
Pose2 odometry(2.0, 0.0, 0.0);
KeyVector contKeys = {X(0), X(1)};
auto noise_model = noiseModel::Isotropic::Sigma(3, 1.0);
auto still = std::make_shared<PlanarMotionModel>(X(0), X(1), Pose2(0, 0, 0),
noise_model),
moving = std::make_shared<PlanarMotionModel>(X(0), X(1), odometry,
noise_model);
std::vector<std::pair<PlanarMotionModel::shared_ptr, double>> motion_models =
{{still, 0.0}, {moving, 0.0}};
std::vector<NonlinearFactor::shared_ptr> motion_models = {
std::make_shared<PlanarMotionModel>(X(0), X(1), Pose2(0, 0, 0),
noise_model),
std::make_shared<PlanarMotionModel>(X(0), X(1), odometry, noise_model)};
fg.emplace_shared<HybridNonlinearFactor>(
contKeys, gtsam::DiscreteKey(M(1), 2), motion_models);

7
gtsam/hybrid/tests/testHybridNonlinearISAM.cpp
View File

@ -439,8 +439,7 @@ TEST(HybridNonlinearISAM, NonTrivial) {
noise_model),
moving = std::make_shared<PlanarMotionModel>(W(0), W(1), odometry,
noise_model);
std::vector<std::pair<PlanarMotionModel::shared_ptr, double>> components = {
{moving, 0.0}, {still, 0.0}};
std::vector<NonlinearFactor::shared_ptr> components{moving, still};
fg.emplace_shared<HybridNonlinearFactor>(
contKeys, gtsam::DiscreteKey(M(1), 2), components);
@ -479,7 +478,7 @@ TEST(HybridNonlinearISAM, NonTrivial) {
noise_model);
moving =
std::make_shared<PlanarMotionModel>(W(1), W(2), odometry, noise_model);
components = {{moving, 0.0}, {still, 0.0}};
components = {moving, still};
fg.emplace_shared<HybridNonlinearFactor>(
contKeys, gtsam::DiscreteKey(M(2), 2), components);
@ -521,7 +520,7 @@ TEST(HybridNonlinearISAM, NonTrivial) {
noise_model);
moving =
std::make_shared<PlanarMotionModel>(W(2), W(3), odometry, noise_model);
components = {{moving, 0.0}, {still, 0.0}};
components = {moving, still};
fg.emplace_shared<HybridNonlinearFactor>(
contKeys, gtsam::DiscreteKey(M(3), 2), components);

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

@ -83,7 +83,7 @@ TEST(HybridSerialization, HybridGaussianFactor) {
auto b1 = Matrix::Ones(2, 1);
auto f0 = std::make_shared<JacobianFactor>(X(0), A, b0);
auto f1 = std::make_shared<JacobianFactor>(X(0), A, b1);
std::vector<GaussianFactorValuePair> factors{{f0, 0.0}, {f1, 0.0}};
std::vector<GaussianFactor::shared_ptr> factors{f0, f1};
const HybridGaussianFactor factor(continuousKeys, discreteKey, factors);