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

update API to only allow a single DiscreteKey and vector of size the same as the discrete key cardinality

release/4.3a0
Varun Agrawal 2024-09-17 13:37:44 -04:00
parent ef2ffd4115
commit 091352806b
9 changed files with 32 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/hybrid/HybridGaussianFactor.h
View File

@ -96,15 +96,15 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
* GaussianFactor shared pointers.
*
* @param continuousKeys Vector of keys for continuous factors.
* @param discreteKeys Vector of discrete keys.
* @param discreteKey The discrete key to index each component.
* @param factors Vector of gaussian factor shared pointers
* and arbitrary scalars.
* and arbitrary scalars. Same size as the cardinality of discreteKey.
*/
HybridGaussianFactor(const KeyVector &continuousKeys,
const DiscreteKeys &discreteKeys,
const DiscreteKey &discreteKey,
const std::vector<GaussianFactorValuePair> &factors)
: HybridGaussianFactor(continuousKeys, discreteKeys,
FactorValuePairs(discreteKeys, factors)) {}
: HybridGaussianFactor(continuousKeys, {discreteKey},
FactorValuePairs({discreteKey}, factors)) {}
/// @}
/// @name Testable

9
gtsam/hybrid/HybridNonlinearFactor.h
View File

@ -92,17 +92,18 @@ class HybridNonlinearFactor : public HybridFactor {
* @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 discreteKeys Vector of discrete keys.
* @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 normalized Flag indicating if the factor error is already
* normalized.
*/
template <typename FACTOR>
HybridNonlinearFactor(
const KeyVector& keys, const DiscreteKeys& discreteKeys,
const KeyVector& keys, const DiscreteKey& discreteKey,
const std::vector<std::pair<std::shared_ptr<FACTOR>, double>>& factors,
bool normalized = false)
: Base(keys, discreteKeys), normalized_(normalized) {
: Base(keys, {discreteKey}), normalized_(normalized) {
std::vector<NonlinearFactorValuePair> nonlinear_factors;
KeySet continuous_keys_set(keys.begin(), keys.end());
KeySet factor_keys_set;
@ -118,7 +119,7 @@ class HybridNonlinearFactor : public HybridFactor {
"Factors passed into HybridNonlinearFactor need to be nonlinear!");
}
}
factors_ = Factors(discreteKeys, nonlinear_factors);
factors_ = Factors({discreteKey}, nonlinear_factors);
if (continuous_keys_set != factor_keys_set) {
throw std::runtime_error(

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

@ -168,8 +168,8 @@ struct Switching {
components.push_back(
{std::dynamic_pointer_cast<NonlinearFactor>(f), 0.0});
}
nonlinearFactorGraph.emplace_shared<HybridNonlinearFactor>(
keys, DiscreteKeys{modes[k]}, components);
nonlinearFactorGraph.emplace_shared<HybridNonlinearFactor>(keys, modes[k],
components);
}
// Add measurement factors

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

@ -437,8 +437,8 @@ static HybridNonlinearFactorGraph createHybridNonlinearFactorGraph() {
std::make_shared<BetweenFactor<double>>(X(0), X(1), 1, noise_model);
std::vector<NonlinearFactorValuePair> components = {{zero_motion, 0.0},
{one_motion, 0.0}};
nfg.emplace_shared<HybridNonlinearFactor>(KeyVector{X(0), X(1)},
DiscreteKeys{m}, components);
nfg.emplace_shared<HybridNonlinearFactor>(KeyVector{X(0), X(1)}, m,
components);
return nfg;
}
@ -583,9 +583,6 @@ TEST(HybridEstimation, ModeSelection) {
graph.emplace_shared<PriorFactor<double>>(X(0), 0.0, measurement_model);
graph.emplace_shared<PriorFactor<double>>(X(1), 0.0, measurement_model);
DiscreteKeys modes;
modes.emplace_back(M(0), 2);
// The size of the noise model
size_t d = 1;
double noise_tight = 0.5, noise_loose = 5.0;
@ -594,11 +591,11 @@ 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}};
KeyVector keys = {X(0), X(1)};
DiscreteKey modes(M(0), 2);
HybridNonlinearFactor mf(keys, modes, components);
initial.insert(X(0), 0.0);
@ -617,7 +614,7 @@ TEST(HybridEstimation, ModeSelection) {
/**************************************************************/
HybridBayesNet bn;
const DiscreteKey mode{M(0), 2};
const DiscreteKey mode(M(0), 2);
bn.push_back(
GaussianConditional::sharedMeanAndStddev(Z(0), -I_1x1, X(0), Z_1x1, 0.1));
@ -648,7 +645,7 @@ TEST(HybridEstimation, ModeSelection2) {
double noise_tight = 0.5, noise_loose = 5.0;
HybridBayesNet bn;
const DiscreteKey mode{M(0), 2};
const DiscreteKey mode(M(0), 2);
bn.push_back(
GaussianConditional::sharedMeanAndStddev(Z(0), -I_3x3, X(0), Z_3x1, 0.1));
@ -679,18 +676,15 @@ TEST(HybridEstimation, ModeSelection2) {
graph.emplace_shared<PriorFactor<Vector3>>(X(0), Z_3x1, measurement_model);
graph.emplace_shared<PriorFactor<Vector3>>(X(1), Z_3x1, measurement_model);
DiscreteKeys modes;
modes.emplace_back(M(0), 2);
auto model0 = std::make_shared<BetweenFactor<Vector3>>(
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}};
KeyVector keys = {X(0), X(1)};
DiscreteKey modes(M(0), 2);
HybridNonlinearFactor mf(keys, modes, components);
initial.insert<Vector3>(X(0), Z_3x1);

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

@ -181,7 +181,7 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalSimple) {
std::vector<GaussianFactorValuePair> factors = {
{std::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1), 0.0},
{std::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones()), 0.0}};
hfg.add(HybridGaussianFactor({X(1)}, {{M(1), 2}}, factors));
hfg.add(HybridGaussianFactor({X(1)}, {M(1), 2}, factors));
hfg.add(DecisionTreeFactor(m1, {2, 8}));
// TODO(Varun) Adding extra discrete variable not connected to continuous
@ -241,7 +241,7 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalTwoClique) {
std::vector<GaussianFactorValuePair> factors = {
{std::make_shared<JacobianFactor>(X(0), I_3x3, Z_3x1), 0.0},
{std::make_shared<JacobianFactor>(X(0), I_3x3, Vector3::Ones()), 0.0}};
hfg.add(HybridGaussianFactor({X(0)}, {{M(0), 2}}, factors));
hfg.add(HybridGaussianFactor({X(0)}, {M(0), 2}, factors));
DecisionTree<Key, GaussianFactorValuePair> dt1(
M(1), {std::make_shared<JacobianFactor>(X(2), I_3x3, Z_3x1), 0.0},

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

@ -423,7 +423,7 @@ TEST(HybridGaussianISAM, NonTrivial) {
std::vector<std::pair<PlanarMotionModel::shared_ptr, double>> components = {
{moving, 0.0}, {still, 0.0}};
auto mixtureFactor = std::make_shared<HybridNonlinearFactor>(
contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components);
contKeys, gtsam::DiscreteKey(M(1), 2), components);
fg.push_back(mixtureFactor);
// Add equivalent of ImuFactor
@ -463,7 +463,7 @@ TEST(HybridGaussianISAM, NonTrivial) {
std::make_shared<PlanarMotionModel>(W(1), W(2), odometry, noise_model);
components = {{moving, 0.0}, {still, 0.0}};
mixtureFactor = std::make_shared<HybridNonlinearFactor>(
contKeys, DiscreteKeys{gtsam::DiscreteKey(M(2), 2)}, components);
contKeys, gtsam::DiscreteKey(M(2), 2), components);
fg.push_back(mixtureFactor);
// Add equivalent of ImuFactor
@ -506,7 +506,7 @@ TEST(HybridGaussianISAM, NonTrivial) {
std::make_shared<PlanarMotionModel>(W(2), W(3), odometry, noise_model);
components = {{moving, 0.0}, {still, 0.0}};
mixtureFactor = std::make_shared<HybridNonlinearFactor>(
contKeys, DiscreteKeys{gtsam::DiscreteKey(M(3), 2)}, components);
contKeys, gtsam::DiscreteKey(M(3), 2), components);
fg.push_back(mixtureFactor);
// Add equivalent of ImuFactor

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

@ -135,7 +135,7 @@ TEST(HybridGaussianFactorGraph, Resize) {
std::vector<std::pair<MotionModel::shared_ptr, double>> components = {
{still, 0.0}, {moving, 0.0}};
auto dcFactor = std::make_shared<HybridNonlinearFactor>(
contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components);
contKeys, gtsam::DiscreteKey(M(1), 2), components);
nhfg.push_back(dcFactor);
Values linearizationPoint;
@ -170,12 +170,12 @@ TEST(HybridGaussianFactorGraph, HybridNonlinearFactor) {
// Check for exception when number of continuous keys are under-specified.
KeyVector contKeys = {X(0)};
THROWS_EXCEPTION(std::make_shared<HybridNonlinearFactor>(
contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components));
contKeys, gtsam::DiscreteKey(M(1), 2), 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, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components));
contKeys, gtsam::DiscreteKey(M(1), 2), components));
}
/*****************************************************************************
@ -807,7 +807,7 @@ TEST(HybridFactorGraph, DefaultDecisionTree) {
std::vector<std::pair<PlanarMotionModel::shared_ptr, double>> motion_models =
{{still, 0.0}, {moving, 0.0}};
fg.emplace_shared<HybridNonlinearFactor>(
contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, motion_models);
contKeys, gtsam::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

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

@ -442,7 +442,7 @@ TEST(HybridNonlinearISAM, NonTrivial) {
std::vector<std::pair<PlanarMotionModel::shared_ptr, double>> components = {
{moving, 0.0}, {still, 0.0}};
auto mixtureFactor = std::make_shared<HybridNonlinearFactor>(
contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components);
contKeys, gtsam::DiscreteKey(M(1), 2), components);
fg.push_back(mixtureFactor);
// Add equivalent of ImuFactor
@ -482,7 +482,7 @@ TEST(HybridNonlinearISAM, NonTrivial) {
std::make_shared<PlanarMotionModel>(W(1), W(2), odometry, noise_model);
components = {{moving, 0.0}, {still, 0.0}};
mixtureFactor = std::make_shared<HybridNonlinearFactor>(
contKeys, DiscreteKeys{gtsam::DiscreteKey(M(2), 2)}, components);
contKeys, gtsam::DiscreteKey(M(2), 2), components);
fg.push_back(mixtureFactor);
// Add equivalent of ImuFactor
@ -525,7 +525,7 @@ TEST(HybridNonlinearISAM, NonTrivial) {
std::make_shared<PlanarMotionModel>(W(2), W(3), odometry, noise_model);
components = {{moving, 0.0}, {still, 0.0}};
mixtureFactor = std::make_shared<HybridNonlinearFactor>(
contKeys, DiscreteKeys{gtsam::DiscreteKey(M(3), 2)}, components);
contKeys, gtsam::DiscreteKey(M(3), 2), components);
fg.push_back(mixtureFactor);
// Add equivalent of ImuFactor

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

@ -76,7 +76,7 @@ BOOST_CLASS_EXPORT_GUID(HybridBayesNet, "gtsam_HybridBayesNet");
// Test HybridGaussianFactor serialization.
TEST(HybridSerialization, HybridGaussianFactor) {
KeyVector continuousKeys{X(0)};
DiscreteKeys discreteKeys{{M(0), 2}};
DiscreteKey discreteKey{M(0), 2};
auto A = Matrix::Zero(2, 1);
auto b0 = Matrix::Zero(2, 1);
@ -85,7 +85,7 @@ TEST(HybridSerialization, HybridGaussianFactor) {
auto f1 = std::make_shared<JacobianFactor>(X(0), A, b1);
std::vector<GaussianFactorValuePair> factors{{f0, 0.0}, {f1, 0.0}};
const HybridGaussianFactor factor(continuousKeys, discreteKeys, factors);
const HybridGaussianFactor factor(continuousKeys, discreteKey, factors);
EXPECT(equalsObj<HybridGaussianFactor>(factor));
EXPECT(equalsXML<HybridGaussianFactor>(factor));