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

update HybridNonlinearFactor to accept a tree of nonlinear factors and arbitrary scalars

release/4.3a0
Varun Agrawal 2024-09-13 15:20:27 -04:00
parent 8da15fd3e0
commit 4343b3aadc
8 changed files with 67 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

64
gtsam/hybrid/HybridNonlinearFactor.h
View File

@ -53,9 +53,9 @@ class HybridNonlinearFactor : public HybridFactor {
/**
* @brief typedef for DecisionTree which has Keys as node labels and
* NonlinearFactor as leaf nodes.
* pairs of NonlinearFactor & an arbitrary scalar as leaf nodes.
*/
using Factors = DecisionTree<Key, sharedFactor>;
using Factors = DecisionTree<Key, std::pair<sharedFactor, double>>;
private:
/// Decision tree of Gaussian factors indexed by discrete keys.
@ -90,25 +90,27 @@ class HybridNonlinearFactor : public HybridFactor {
* Will be typecast to NonlinearFactor shared pointers.
* @param keys Vector of keys for continuous factors.
* @param discreteKeys Vector of discrete keys.
* @param factors Vector of nonlinear factors.
* @param factors Vector of nonlinear factor and scalar pairs.
* @param normalized Flag indicating if the factor error is already
* normalized.
*/
template <typename FACTOR>
HybridNonlinearFactor(const KeyVector& keys, const DiscreteKeys& discreteKeys,
const std::vector<std::shared_ptr<FACTOR>>& factors,
bool normalized = false)
HybridNonlinearFactor(
const KeyVector& keys, const DiscreteKeys& discreteKeys,
const std::vector<std::pair<std::shared_ptr<FACTOR>, double>>& factors,
bool normalized = false)
: Base(keys, discreteKeys), normalized_(normalized) {
std::vector<NonlinearFactor::shared_ptr> nonlinear_factors;
std::vector<std::pair<NonlinearFactor::shared_ptr, double>>
nonlinear_factors;
KeySet continuous_keys_set(keys.begin(), keys.end());
KeySet factor_keys_set;
for (auto&& f : factors) {
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.push_back(nf);
nonlinear_factors.push_back(std::make_pair(nf, val));
} else {
throw std::runtime_error(
"Factors passed into HybridNonlinearFactor need to be nonlinear!");
@ -133,9 +135,11 @@ class HybridNonlinearFactor : public HybridFactor {
*/
AlgebraicDecisionTree<Key> errorTree(const Values& continuousValues) const {
// functor to convert from sharedFactor to double error value.
auto errorFunc = [continuousValues](const sharedFactor& factor) {
return factor->error(continuousValues);
};
auto errorFunc =
[continuousValues](const std::pair<sharedFactor, double>& f) {
auto [factor, val] = f;
return factor->error(continuousValues) + val;
};
DecisionTree<Key, double> result(factors_, errorFunc);
return result;
}
@ -150,12 +154,10 @@ class HybridNonlinearFactor : public HybridFactor {
double error(const Values& continuousValues,
const DiscreteValues& discreteValues) const {
// Retrieve the factor corresponding to the assignment in discreteValues.
auto factor = factors_(discreteValues);
auto [factor, val] = factors_(discreteValues);
// Compute the error for the selected factor
const double factorError = factor->error(continuousValues);
if (normalized_) return factorError;
return factorError + this->nonlinearFactorLogNormalizingConstant(
factor, continuousValues);
return factorError + val;
}
/**
@ -175,7 +177,7 @@ class HybridNonlinearFactor : public HybridFactor {
*/
size_t dim() const {
const auto assignments = DiscreteValues::CartesianProduct(discreteKeys_);
auto factor = factors_(assignments.at(0));
auto [factor, val] = factors_(assignments.at(0));
return factor->dim();
}
@ -189,9 +191,11 @@ class HybridNonlinearFactor : public HybridFactor {
std::cout << (s.empty() ? "" : s + " ");
Base::print("", keyFormatter);
std::cout << "\nHybridNonlinearFactor\n";
auto valueFormatter = [](const sharedFactor& v) {
if (v) {
return "Nonlinear factor on " + std::to_string(v->size()) + " keys";
auto valueFormatter = [](const std::pair<sharedFactor, double>& v) {
auto [factor, val] = v;
if (factor) {
return "Nonlinear factor on " + std::to_string(factor->size()) +
" keys";
} else {
return std::string("nullptr");
}
@ -211,8 +215,10 @@ class HybridNonlinearFactor : public HybridFactor {
static_cast<const HybridNonlinearFactor&>(other));
// Ensure that this HybridNonlinearFactor and `f` have the same `factors_`.
auto compare = [tol](const sharedFactor& a, const sharedFactor& b) {
return traits<NonlinearFactor>::Equals(*a, *b, tol);
auto compare = [tol](const std::pair<sharedFactor, double>& a,
const std::pair<sharedFactor, double>& b) {
return traits<NonlinearFactor>::Equals(*a.first, *b.first, tol) &&
(a.second == b.second);
};
if (!factors_.equals(f.factors_, compare)) return false;
@ -230,7 +236,7 @@ class HybridNonlinearFactor : public HybridFactor {
GaussianFactor::shared_ptr linearize(
const Values& continuousValues,
const DiscreteValues& discreteValues) const {
auto factor = factors_(discreteValues);
auto [factor, val] = factors_(discreteValues);
return factor->linearize(continuousValues);
}
@ -238,12 +244,14 @@ class HybridNonlinearFactor : public HybridFactor {
std::shared_ptr<HybridGaussianFactor> linearize(
const Values& continuousValues) const {
// functional to linearize each factor in the decision tree
auto linearizeDT = [continuousValues](const sharedFactor& factor) {
return factor->linearize(continuousValues);
};
auto linearizeDT =
[continuousValues](const std::pair<sharedFactor, double>& f) {
auto [factor, val] = f;
return {factor->linearize(continuousValues), val};
};
DecisionTree<Key, GaussianFactor::shared_ptr> linearized_factors(
factors_, linearizeDT);
DecisionTree<Key, std::pair<GaussianFactor::shared_ptr, double>>
linearized_factors(factors_, linearizeDT);
return std::make_shared<HybridGaussianFactor>(
continuousKeys_, discreteKeys_, linearized_factors);

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

@ -159,9 +159,10 @@ 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<NonlinearFactor::shared_ptr> components;
std::vector<std::pair<NonlinearFactor::shared_ptr, double>> components;
for (auto &&f : motion_models) {
components.push_back(std::dynamic_pointer_cast<NonlinearFactor>(f));
components.push_back(
{std::dynamic_pointer_cast<NonlinearFactor>(f), 0.0});
}
nonlinearFactorGraph.emplace_shared<HybridNonlinearFactor>(
keys, DiscreteKeys{modes[k]}, components);

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

@ -387,7 +387,8 @@ TEST(HybridBayesNet, Sampling) {
std::make_shared<BetweenFactor<double>>(X(0), X(1), 0, noise_model);
auto one_motion =
std::make_shared<BetweenFactor<double>>(X(0), X(1), 1, noise_model);
std::vector<NonlinearFactor::shared_ptr> factors = {zero_motion, one_motion};
std::vector<std::pair<NonlinearFactor::shared_ptr, double>> factors = {
{zero_motion, 0.0}, {one_motion, 0.0}};
nfg.emplace_shared<PriorFactor<double>>(X(0), 0.0, noise_model);
nfg.emplace_shared<HybridNonlinearFactor>(
KeyVector{X(0), X(1)}, DiscreteKeys{DiscreteKey(M(0), 2)}, factors);

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

@ -435,9 +435,10 @@ 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);
nfg.emplace_shared<HybridNonlinearFactor>(
KeyVector{X(0), X(1)}, DiscreteKeys{m},
std::vector<NonlinearFactor::shared_ptr>{zero_motion, one_motion});
std::vector<std::pair<NonlinearFactor::shared_ptr, double>> components = {
{zero_motion, 0.0}, {one_motion, 0.0}};
nfg.emplace_shared<HybridNonlinearFactor>(KeyVector{X(0), X(1)},
DiscreteKeys{m}, components);
return nfg;
}
@ -589,7 +590,8 @@ TEST(HybridEstimation, ModeSelection) {
model1 = std::make_shared<MotionModel>(
X(0), X(1), 0.0, noiseModel::Isotropic::Sigma(d, noise_tight));
std::vector<NonlinearFactor::shared_ptr> components = {model0, model1};
std::vector<std::pair<NonlinearFactor::shared_ptr, double>> components = {
{model0, 0.0}, {model1, 0.0}};
KeyVector keys = {X(0), X(1)};
HybridNonlinearFactor mf(keys, modes, components);
@ -680,7 +682,8 @@ TEST(HybridEstimation, ModeSelection2) {
model1 = std::make_shared<BetweenFactor<Vector3>>(
X(0), X(1), Z_3x1, noiseModel::Isotropic::Sigma(d, noise_tight));
std::vector<NonlinearFactor::shared_ptr> components = {model0, model1};
std::vector<std::pair<NonlinearFactor::shared_ptr, double>> components = {
{model0, 0.0}, {model1, 0.0}};
KeyVector keys = {X(0), X(1)};
HybridNonlinearFactor mf(keys, modes, components);

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

@ -420,7 +420,8 @@ TEST(HybridGaussianISAM, NonTrivial) {
noise_model),
moving = std::make_shared<PlanarMotionModel>(W(0), W(1), odometry,
noise_model);
std::vector<PlanarMotionModel::shared_ptr> components = {moving, still};
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);
fg.push_back(mixtureFactor);
@ -460,7 +461,7 @@ TEST(HybridGaussianISAM, NonTrivial) {
noise_model);
moving =
std::make_shared<PlanarMotionModel>(W(1), W(2), odometry, noise_model);
components = {moving, still};
components = {{moving, 0.0}, {still, 0.0}};
mixtureFactor = std::make_shared<HybridNonlinearFactor>(
contKeys, DiscreteKeys{gtsam::DiscreteKey(M(2), 2)}, components);
fg.push_back(mixtureFactor);
@ -503,7 +504,7 @@ TEST(HybridGaussianISAM, NonTrivial) {
noise_model);
moving =
std::make_shared<PlanarMotionModel>(W(2), W(3), odometry, noise_model);
components = {moving, still};
components = {{moving, 0.0}, {still, 0.0}};
mixtureFactor = std::make_shared<HybridNonlinearFactor>(
contKeys, DiscreteKeys{gtsam::DiscreteKey(M(3), 2)}, components);
fg.push_back(mixtureFactor);

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

@ -58,7 +58,8 @@ TEST(HybridNonlinearFactor, Printing) {
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<NonlinearFactor::shared_ptr> factors{f0, f1};
std::vector<std::pair<NonlinearFactor::shared_ptr, double>> factors{
{f0, 0.0}, {f1, 0.0}};
HybridNonlinearFactor mixtureFactor({X(1), X(2)}, {m1}, factors);
@ -86,7 +87,8 @@ 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<NonlinearFactor::shared_ptr> factors{f0, f1};
std::vector<std::pair<NonlinearFactor::shared_ptr, double>> factors{
{f0, 0.0}, {f1, 0.0}};
return HybridNonlinearFactor({X(1), X(2)}, {m1}, factors);
}

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

@ -131,7 +131,8 @@ TEST(HybridGaussianFactorGraph, Resize) {
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<MotionModel::shared_ptr> components = {still, moving};
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);
nhfg.push_back(dcFactor);
@ -162,7 +163,8 @@ TEST(HybridGaussianFactorGraph, HybridNonlinearFactor) {
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<MotionModel::shared_ptr> components = {still, moving};
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)};
@ -801,7 +803,8 @@ TEST(HybridFactorGraph, DefaultDecisionTree) {
noise_model),
moving = std::make_shared<PlanarMotionModel>(X(0), X(1), odometry,
noise_model);
std::vector<PlanarMotionModel::shared_ptr> motion_models = {still, moving};
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);

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

@ -439,7 +439,8 @@ TEST(HybridNonlinearISAM, NonTrivial) {
noise_model),
moving = std::make_shared<PlanarMotionModel>(W(0), W(1), odometry,
noise_model);
std::vector<PlanarMotionModel::shared_ptr> components = {moving, still};
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);
fg.push_back(mixtureFactor);
@ -479,7 +480,7 @@ TEST(HybridNonlinearISAM, NonTrivial) {
noise_model);
moving =
std::make_shared<PlanarMotionModel>(W(1), W(2), odometry, noise_model);
components = {moving, still};
components = {{moving, 0.0}, {still, 0.0}};
mixtureFactor = std::make_shared<HybridNonlinearFactor>(
contKeys, DiscreteKeys{gtsam::DiscreteKey(M(2), 2)}, components);
fg.push_back(mixtureFactor);
@ -522,7 +523,7 @@ TEST(HybridNonlinearISAM, NonTrivial) {
noise_model);
moving =
std::make_shared<PlanarMotionModel>(W(2), W(3), odometry, noise_model);
components = {moving, still};
components = {{moving, 0.0}, {still, 0.0}};
mixtureFactor = std::make_shared<HybridNonlinearFactor>(
contKeys, DiscreteKeys{gtsam::DiscreteKey(M(3), 2)}, components);
fg.push_back(mixtureFactor);