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

Fix call sites for HybridNonlinearFactor

release/4.3a0
Frank Dellaert 2024-09-26 14:51:10 -07:00
parent 2c12e685ea
commit bb4c3c95ab
7 changed files with 19 additions and 61 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -159,9 +159,8 @@ struct Switching {
// Add "motion models".
for (size_t k = 0; k < K - 1; k++) {
KeyVector keys = {X(k), X(k + 1)};
auto motion_models = motionModels(k, between_sigma);
nonlinearFactorGraph.emplace_shared<HybridNonlinearFactor>(keys, modes[k],
nonlinearFactorGraph.emplace_shared<HybridNonlinearFactor>(modes[k],
motion_models);
}

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

@ -388,7 +388,7 @@ TEST(HybridBayesNet, Sampling) {
auto one_motion =
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),
DiscreteKey(M(0), 2),
std::vector<NonlinearFactor::shared_ptr>{zero_motion, one_motion});
DiscreteKey mode(M(0), 2);

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

@ -437,8 +437,7 @@ static HybridNonlinearFactorGraph createHybridNonlinearFactorGraph() {
std::make_shared<BetweenFactor<double>>(X(0), X(1), 1, noise_model);
std::vector<NonlinearFactor::shared_ptr> components = {zero_motion,
one_motion};
nfg.emplace_shared<HybridNonlinearFactor>(KeyVector{X(0), X(1)}, m,
components);
nfg.emplace_shared<HybridNonlinearFactor>(m, components);
return nfg;
}
@ -591,9 +590,7 @@ TEST(HybridEstimation, ModeSelection) {
X(0), X(1), 0.0, noiseModel::Isotropic::Sigma(d, noise_tight));
std::vector<NonlinearFactor::shared_ptr> components = {model0, model1};
KeyVector keys = {X(0), X(1)};
DiscreteKey modes(M(0), 2);
HybridNonlinearFactor mf(keys, modes, components);
HybridNonlinearFactor mf({M(0), 2}, components);
initial.insert(X(0), 0.0);
initial.insert(X(1), 0.0);
@ -681,9 +678,7 @@ TEST(HybridEstimation, ModeSelection2) {
X(0), X(1), Z_3x1, noiseModel::Isotropic::Sigma(d, noise_tight));
std::vector<NonlinearFactor::shared_ptr> components = {model0, model1};
KeyVector keys = {X(0), X(1)};
DiscreteKey modes(M(0), 2);
HybridNonlinearFactor mf(keys, modes, components);
HybridNonlinearFactor mf({M(0), 2}, components);
initial.insert<Vector3>(X(0), Z_3x1);
initial.insert<Vector3>(X(1), Z_3x1);

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

@ -414,15 +414,13 @@ TEST(HybridGaussianISAM, NonTrivial) {
// Add odometry factor with discrete modes.
Pose2 odometry(1.0, 0.0, 0.0);
KeyVector contKeys = {W(0), W(1)};
auto noise_model = noiseModel::Isotropic::Sigma(3, 1.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);
fg.emplace_shared<HybridNonlinearFactor>(DiscreteKey(M(1), 2), components);
// Add equivalent of ImuFactor
fg.emplace_shared<BetweenFactor<Pose2>>(X(0), X(1), Pose2(1.0, 0.0, 0),
@ -454,14 +452,12 @@ TEST(HybridGaussianISAM, NonTrivial) {
/*************** Run Round 3 ***************/
// Add odometry factor with discrete modes.
contKeys = {W(1), W(2)};
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);
fg.emplace_shared<HybridNonlinearFactor>(DiscreteKey(M(2), 2), components);
// Add equivalent of ImuFactor
fg.emplace_shared<BetweenFactor<Pose2>>(X(1), X(2), Pose2(1.0, 0.0, 0),
@ -496,14 +492,12 @@ TEST(HybridGaussianISAM, NonTrivial) {
/*************** Run Round 4 ***************/
// Add odometry factor with discrete modes.
contKeys = {W(2), W(3)};
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);
fg.emplace_shared<HybridNonlinearFactor>(DiscreteKey(M(3), 2), components);
// Add equivalent of ImuFactor
fg.emplace_shared<BetweenFactor<Pose2>>(X(2), X(3), Pose2(1.0, 0.0, 0),

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

@ -60,14 +60,14 @@ auto f1 = std::make_shared<BetweenFactor<double>>(X(1), X(2), between1, model);
// Test simple to complex constructors...
TEST(HybridGaussianFactor, ConstructorVariants) {
using namespace test_constructor;
HybridNonlinearFactor fromFactors({X(1), X(2)}, m1, {f0, f1});
HybridNonlinearFactor fromFactors(m1, {f0, f1});
std::vector<NonlinearFactorValuePair> pairs{{f0, 0.0}, {f1, 0.0}};
HybridNonlinearFactor fromPairs({X(1), X(2)}, m1, pairs);
HybridNonlinearFactor fromPairs(m1, pairs);
assert_equal(fromFactors, fromPairs);
HybridNonlinearFactor::FactorValuePairs decisionTree({m1}, pairs);
HybridNonlinearFactor fromDecisionTree({X(1), X(2)}, {m1}, decisionTree);
HybridNonlinearFactor fromDecisionTree({m1}, decisionTree);
assert_equal(fromDecisionTree, fromPairs);
}
@ -75,7 +75,7 @@ TEST(HybridGaussianFactor, ConstructorVariants) {
// Test .print() output.
TEST(HybridNonlinearFactor, Printing) {
using namespace test_constructor;
HybridNonlinearFactor hybridFactor({X(1), X(2)}, {m1}, {f0, f1});
HybridNonlinearFactor hybridFactor({m1}, {f0, f1});
std::string expected =
R"(Hybrid [x1 x2; 1]
@ -101,7 +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);
return HybridNonlinearFactor({X(1), X(2)}, m1, {f0, f1});
return HybridNonlinearFactor(m1, {f0, f1});
}
/* ************************************************************************* */

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

@ -117,7 +117,6 @@ TEST(HybridNonlinearFactorGraph, Resize) {
/***************************************************************************/
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 = {
@ -139,8 +138,7 @@ TEST(HybridGaussianFactorGraph, Resize) {
auto discreteFactor = std::make_shared<DecisionTreeFactor>();
hnfg.push_back(discreteFactor);
auto dcFactor =
std::make_shared<HybridNonlinearFactor>(contKeys, m1, components);
auto dcFactor = std::make_shared<HybridNonlinearFactor>(m1, components);
hnfg.push_back(dcFactor);
Values linearizationPoint;
@ -156,26 +154,6 @@ TEST(HybridGaussianFactorGraph, Resize) {
EXPECT_LONGS_EQUAL(gfg.size(), 0);
}
/***************************************************************************
* Test that the HybridNonlinearFactor reports correctly if the number of
* 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>();
// Check for exception when number of continuous keys are under-specified.
THROWS_EXCEPTION(
std::make_shared<HybridNonlinearFactor>(KeyVector{X(0)}, m1, components));
// Check for exception when number of continuous keys are too many.
THROWS_EXCEPTION(std::make_shared<HybridNonlinearFactor>(
KeyVector{X(0), X(1), X(2)}, m1, components));
}
/*****************************************************************************
* Test push_back on HFG makes the correct distinction.
*/
@ -828,14 +806,12 @@ TEST(HybridNonlinearFactorGraph, DefaultDecisionTree) {
// Add odometry factor
Pose2 odometry(2.0, 0.0, 0.0);
KeyVector contKeys = {X(0), X(1)};
auto noise_model = noiseModel::Isotropic::Sigma(3, 1.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);
fg.emplace_shared<HybridNonlinearFactor>(DiscreteKey{M(1), 2}, motion_models);
// Add Range-Bearing measurements to from X0 to L0 and X1 to L1.
// create a noise model for the landmark measurements
@ -901,7 +877,7 @@ static HybridNonlinearFactorGraph CreateFactorGraph(
std::vector<NonlinearFactorValuePair> factors{{f0, model0->negLogConstant()},
{f1, model1->negLogConstant()}};
HybridNonlinearFactor mixtureFactor({X(0), X(1)}, m1, factors);
HybridNonlinearFactor mixtureFactor(m1, factors);
HybridNonlinearFactorGraph hfg;
hfg.push_back(mixtureFactor);

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

@ -433,15 +433,13 @@ TEST(HybridNonlinearISAM, NonTrivial) {
/*************** Run Round 2 ***************/
// Add odometry factor with discrete modes.
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<NonlinearFactor::shared_ptr> components{moving, still};
fg.emplace_shared<HybridNonlinearFactor>(
contKeys, gtsam::DiscreteKey(M(1), 2), components);
fg.emplace_shared<HybridNonlinearFactor>(DiscreteKey(M(1), 2), components);
// Add equivalent of ImuFactor
fg.emplace_shared<BetweenFactor<Pose2>>(X(0), X(1), Pose2(1.0, 0.0, 0),
@ -473,14 +471,12 @@ TEST(HybridNonlinearISAM, 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, still};
fg.emplace_shared<HybridNonlinearFactor>(
contKeys, gtsam::DiscreteKey(M(2), 2), components);
fg.emplace_shared<HybridNonlinearFactor>(DiscreteKey(M(2), 2), components);
// Add equivalent of ImuFactor
fg.emplace_shared<BetweenFactor<Pose2>>(X(1), X(2), Pose2(1.0, 0.0, 0),
@ -515,14 +511,12 @@ TEST(HybridNonlinearISAM, 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, still};
fg.emplace_shared<HybridNonlinearFactor>(
contKeys, gtsam::DiscreteKey(M(3), 2), components);
fg.emplace_shared<HybridNonlinearFactor>(DiscreteKey(M(3), 2), components);
// Add equivalent of ImuFactor
fg.emplace_shared<BetweenFactor<Pose2>>(X(2), X(3), Pose2(1.0, 0.0, 0),