Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Better HybridNonlinear! #1851

Merged
merged 4 commits into from
Sep 29, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
20 changes: 14 additions & 6 deletions gtsam/hybrid/HybridNonlinearFactor.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@
*/

#include <gtsam/hybrid/HybridNonlinearFactor.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/nonlinear/NonlinearFactor.h>

#include <memory>
Expand All @@ -29,7 +30,7 @@ struct HybridNonlinearFactor::ConstructorHelper {
DiscreteKeys discreteKeys; // Discrete keys provided to the constructors
FactorValuePairs factorTree;

void copyOrCheckContinuousKeys(const NonlinearFactor::shared_ptr& factor) {
void copyOrCheckContinuousKeys(const NoiseModelFactor::shared_ptr& factor) {
if (!factor) return;
if (continuousKeys.empty()) {
continuousKeys = factor->keys();
Expand All @@ -40,7 +41,7 @@ struct HybridNonlinearFactor::ConstructorHelper {
}

ConstructorHelper(const DiscreteKey& discreteKey,
const std::vector<NonlinearFactor::shared_ptr>& factors)
const std::vector<NoiseModelFactor::shared_ptr>& factors)
: discreteKeys({discreteKey}) {
std::vector<NonlinearFactorValuePair> pairs;
// Extract continuous keys from the first non-null factor
Expand Down Expand Up @@ -78,7 +79,7 @@ HybridNonlinearFactor::HybridNonlinearFactor(const ConstructorHelper& helper)

HybridNonlinearFactor::HybridNonlinearFactor(
const DiscreteKey& discreteKey,
const std::vector<NonlinearFactor::shared_ptr>& factors)
const std::vector<NoiseModelFactor::shared_ptr>& factors)
: HybridNonlinearFactor(ConstructorHelper(discreteKey, factors)) {}

HybridNonlinearFactor::HybridNonlinearFactor(
Expand Down Expand Up @@ -158,8 +159,7 @@ bool HybridNonlinearFactor::equals(const HybridFactor& other,
// Ensure that this HybridNonlinearFactor and `f` have the same `factors_`.
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);
return a.first->equals(*b.first, tol) && (a.second == b.second);
};
if (!factors_.equals(f.factors_, compare)) return false;

Expand All @@ -185,7 +185,15 @@ std::shared_ptr<HybridGaussianFactor> HybridNonlinearFactor::linearize(
[continuousValues](
const std::pair<sharedFactor, double>& f) -> GaussianFactorValuePair {
auto [factor, val] = f;
return {factor->linearize(continuousValues), val};
if (auto gaussian = std::dynamic_pointer_cast<noiseModel::Gaussian>(
factor->noiseModel())) {
return {factor->linearize(continuousValues),
val + gaussian->negLogConstant()};
} else {
throw std::runtime_error(
"HybridNonlinearFactor: linearize() only supports NoiseModelFactors "
"with Gaussian (or derived) noise models.");
}
};

DecisionTree<Key, std::pair<GaussianFactor::shared_ptr, double>>
Expand Down
20 changes: 9 additions & 11 deletions gtsam/hybrid/HybridNonlinearFactor.h
Original file line number Diff line number Diff line change
Expand Up @@ -26,25 +26,23 @@
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Symbol.h>

#include <algorithm>
#include <cmath>
#include <limits>
#include <vector>

namespace gtsam {

/// Alias for a NonlinearFactor shared pointer and double scalar pair.
using NonlinearFactorValuePair = std::pair<NonlinearFactor::shared_ptr, double>;
/// Alias for a NoiseModelFactor shared pointer and double scalar pair.
using NonlinearFactorValuePair =
std::pair<NoiseModelFactor::shared_ptr, double>;

/**
* @brief Implementation of a discrete-conditioned hybrid factor.
*
* Implements a joint discrete-continuous factor where the discrete variable
* serves to "select" a hybrid component corresponding to a NonlinearFactor.
* serves to "select" a hybrid component corresponding to a NoiseModelFactor.
*
* This class stores all factors as HybridFactors which can then be typecast to
* one of (NonlinearFactor, GaussianFactor) which can then be checked to perform
* the correct operation.
* one of (NoiseModelFactor, GaussianFactor) which can then be checked to
* perform the correct operation.
*
* In factor graphs the error function typically returns 0.5*|h(x)-z|^2, i.e.,
* the negative log-likelihood for a Gaussian noise model.
Expand All @@ -62,11 +60,11 @@ class GTSAM_EXPORT HybridNonlinearFactor : public HybridFactor {
using Base = HybridFactor;
using This = HybridNonlinearFactor;
using shared_ptr = std::shared_ptr<HybridNonlinearFactor>;
using sharedFactor = std::shared_ptr<NonlinearFactor>;
using sharedFactor = std::shared_ptr<NoiseModelFactor>;

/**
* @brief typedef for DecisionTree which has Keys as node labels and
* pairs of NonlinearFactor & an arbitrary scalar as leaf nodes.
* pairs of NoiseModelFactor & an arbitrary scalar as leaf nodes.
*/
using FactorValuePairs = DecisionTree<Key, NonlinearFactorValuePair>;

Expand Down Expand Up @@ -95,7 +93,7 @@ class GTSAM_EXPORT HybridNonlinearFactor : public HybridFactor {
*/
HybridNonlinearFactor(
const DiscreteKey& discreteKey,
const std::vector<NonlinearFactor::shared_ptr>& factors);
const std::vector<NoiseModelFactor::shared_ptr>& factors);

/**
* @brief Construct a new HybridNonlinearFactor on a single discrete key,
Expand Down
6 changes: 3 additions & 3 deletions gtsam/hybrid/hybrid.i
Original file line number Diff line number Diff line change
Expand Up @@ -245,16 +245,16 @@ class HybridNonlinearFactorGraph {
class HybridNonlinearFactor : gtsam::HybridFactor {
HybridNonlinearFactor(
const gtsam::DiscreteKey& discreteKey,
const std::vector<gtsam::NonlinearFactor*>& factors);
const std::vector<gtsam::NoiseModelFactor*>& factors);

HybridNonlinearFactor(
const gtsam::DiscreteKey& discreteKey,
const std::vector<std::pair<gtsam::NonlinearFactor*, double>>& factors);
const std::vector<std::pair<gtsam::NoiseModelFactor*, double>>& factors);

HybridNonlinearFactor(
const gtsam::DiscreteKeys& discreteKeys,
const gtsam::DecisionTree<
gtsam::Key, std::pair<gtsam::NonlinearFactor*, double>>& factors);
gtsam::Key, std::pair<gtsam::NoiseModelFactor*, double>>& factors);

double error(const gtsam::Values& continuousValues,
const gtsam::DiscreteValues& discreteValues) const;
Expand Down
3 changes: 2 additions & 1 deletion gtsam/hybrid/tests/Switching.h
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,7 @@

#include "gtsam/linear/GaussianFactor.h"
#include "gtsam/linear/GaussianFactorGraph.h"
#include "gtsam/nonlinear/NonlinearFactor.h"

#pragma once

Expand Down Expand Up @@ -185,7 +186,7 @@ struct Switching {
}

// Create motion models for a given time step
static std::vector<NonlinearFactor::shared_ptr> motionModels(
static std::vector<NoiseModelFactor::shared_ptr> motionModels(
size_t k, double sigma = 1.0) {
auto noise_model = noiseModel::Isotropic::Sigma(1, sigma);
auto still =
Expand Down
3 changes: 2 additions & 1 deletion gtsam/hybrid/tests/testHybridBayesNet.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,7 @@

#include "Switching.h"
#include "TinyHybridExample.h"
#include "gtsam/nonlinear/NonlinearFactor.h"
dellaert marked this conversation as resolved.
Show resolved Hide resolved

// Include for test suite
#include <CppUnitLite/TestHarness.h>
Expand Down Expand Up @@ -389,7 +390,7 @@ TEST(HybridBayesNet, Sampling) {
std::make_shared<BetweenFactor<double>>(X(0), X(1), 1, noise_model);
nfg.emplace_shared<HybridNonlinearFactor>(
DiscreteKey(M(0), 2),
std::vector<NonlinearFactor::shared_ptr>{zero_motion, one_motion});
std::vector<NoiseModelFactor::shared_ptr>{zero_motion, one_motion});

DiscreteKey mode(M(0), 2);
nfg.emplace_shared<DiscreteDistribution>(mode, "1/1");
Expand Down
58 changes: 7 additions & 51 deletions gtsam/hybrid/tests/testHybridEstimation.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,7 @@
#include <bitset>

#include "Switching.h"
#include "gtsam/nonlinear/NonlinearFactor.h"
dellaert marked this conversation as resolved.
Show resolved Hide resolved

using namespace std;
using namespace gtsam;
Expand Down Expand Up @@ -435,8 +436,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<NonlinearFactor::shared_ptr> components = {zero_motion,
one_motion};
std::vector<NoiseModelFactor::shared_ptr> components = {zero_motion,
one_motion};
nfg.emplace_shared<HybridNonlinearFactor>(m, components);

return nfg;
Expand Down Expand Up @@ -526,49 +527,6 @@ TEST(HybridEstimation, CorrectnessViaSampling) {
}
}

/****************************************************************************/
/**
* Helper function to add the constant term corresponding to
* the difference in noise models.
*/
std::shared_ptr<HybridGaussianFactor> mixedVarianceFactor(
const HybridNonlinearFactor& mf, const Values& initial, const Key& mode,
double noise_tight, double noise_loose, size_t d, size_t tight_index) {
HybridGaussianFactor::shared_ptr gmf = mf.linearize(initial);

constexpr double log2pi = 1.8378770664093454835606594728112;
// logConstant will be of the tighter model
double logNormalizationConstant = log(1.0 / noise_tight);
double logConstant = -0.5 * d * log2pi + logNormalizationConstant;

auto func = [&](const Assignment<Key>& assignment,
const GaussianFactor::shared_ptr& gf) {
if (assignment.at(mode) != tight_index) {
double factor_log_constant = -0.5 * d * log2pi + log(1.0 / noise_loose);

GaussianFactorGraph _gfg;
_gfg.push_back(gf);
Vector c(d);
for (size_t i = 0; i < d; i++) {
c(i) = std::sqrt(2.0 * (logConstant - factor_log_constant));
}

_gfg.emplace_shared<JacobianFactor>(c);
return std::make_shared<JacobianFactor>(_gfg);
} else {
return dynamic_pointer_cast<JacobianFactor>(gf);
}
};
auto updated_components = gmf->factors().apply(func);
auto updated_pairs = HybridGaussianFactor::FactorValuePairs(
updated_components,
[](const GaussianFactor::shared_ptr& gf) -> GaussianFactorValuePair {
return {gf, 0.0};
});
return std::make_shared<HybridGaussianFactor>(gmf->discreteKeys(),
updated_pairs);
}

/****************************************************************************/
TEST(HybridEstimation, ModeSelection) {
HybridNonlinearFactorGraph graph;
Expand All @@ -588,15 +546,14 @@ 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<NonlinearFactor::shared_ptr> components = {model0, model1};
std::vector<NoiseModelFactor::shared_ptr> components = {model0, model1};

HybridNonlinearFactor mf({M(0), 2}, components);

initial.insert(X(0), 0.0);
initial.insert(X(1), 0.0);

auto gmf =
mixedVarianceFactor(mf, initial, M(0), noise_tight, noise_loose, d, 1);
auto gmf = mf.linearize(initial);
graph.add(gmf);

auto gfg = graph.linearize(initial);
Expand Down Expand Up @@ -676,15 +633,14 @@ 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<NonlinearFactor::shared_ptr> components = {model0, model1};
std::vector<NoiseModelFactor::shared_ptr> components = {model0, model1};

HybridNonlinearFactor mf({M(0), 2}, components);

initial.insert<Vector3>(X(0), Z_3x1);
initial.insert<Vector3>(X(1), Z_3x1);

auto gmf =
mixedVarianceFactor(mf, initial, M(0), noise_tight, noise_loose, d, 1);
auto gmf = mf.linearize(initial);
graph.add(gmf);

auto gfg = graph.linearize(initial);
Expand Down
Loading
Loading