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

Apply easy constructor

release/4.3a0
Frank Dellaert 2024-09-23 14:49:10 -07:00
parent 7c07d802a3
commit 5aa5222edb
7 changed files with 85 additions and 88 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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);

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);