Skip to content

Commit

Permalink
Switching to NoiseModelFactor
Browse files Browse the repository at this point in the history
  • Loading branch information
dellaert committed Sep 28, 2024
1 parent 71655cc commit 8907ca7
Show file tree
Hide file tree
Showing 8 changed files with 43 additions and 77 deletions.
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 Gaussian factors.");
}
};

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
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"

// 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"

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
3 changes: 2 additions & 1 deletion gtsam/hybrid/tests/testHybridGaussianISAM.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,7 @@
#include <numeric>

#include "Switching.h"
#include "gtsam/nonlinear/NonlinearFactor.h"

// Include for test suite
#include <CppUnitLite/TestHarness.h>
Expand Down Expand Up @@ -415,7 +416,7 @@ TEST(HybridGaussianISAM, NonTrivial) {
// Add odometry factor with discrete modes.
Pose2 odometry(1.0, 0.0, 0.0);
auto noise_model = noiseModel::Isotropic::Sigma(3, 1.0);
std::vector<NonlinearFactor::shared_ptr> components;
std::vector<NoiseModelFactor::shared_ptr> components;
components.emplace_back(
new PlanarMotionModel(W(0), W(1), odometry, noise_model)); // moving
components.emplace_back(
Expand Down
10 changes: 5 additions & 5 deletions gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,7 @@
#include <numeric>

#include "Switching.h"
#include "gtsam/nonlinear/NonlinearFactor.h"

// Include for test suite
#include <CppUnitLite/TestHarness.h>
Expand Down Expand Up @@ -119,7 +120,7 @@ TEST(HybridNonlinearFactorGraph, Resize) {
namespace test_motion {
gtsam::DiscreteKey m1(M(1), 2);
auto noise_model = noiseModel::Isotropic::Sigma(1, 1.0);
std::vector<NonlinearFactor::shared_ptr> components = {
std::vector<NoiseModelFactor::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
Expand Down Expand Up @@ -207,7 +208,7 @@ TEST(HybridNonlinearFactorGraph, PushBack) {
factors.emplace_shared<PriorFactor<Pose2>>(1, Pose2(1, 0, 0), noise);
factors.emplace_shared<PriorFactor<Pose2>>(2, Pose2(2, 0, 0), noise);
// TODO(Varun) This does not currently work. It should work once HybridFactor
// becomes a base class of NonlinearFactor.
// becomes a base class of NoiseModelFactor.
// hnfg.push_back(factors.begin(), factors.end());

// EXPECT_LONGS_EQUAL(3, hnfg.size());
Expand Down Expand Up @@ -807,7 +808,7 @@ TEST(HybridNonlinearFactorGraph, DefaultDecisionTree) {
// Add odometry factor
Pose2 odometry(2.0, 0.0, 0.0);
auto noise_model = noiseModel::Isotropic::Sigma(3, 1.0);
std::vector<NonlinearFactor::shared_ptr> motion_models = {
std::vector<NoiseModelFactor::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)};
Expand Down Expand Up @@ -874,8 +875,7 @@ static HybridNonlinearFactorGraph CreateFactorGraph(
// Create HybridNonlinearFactor
// We take negative since we want
// the underlying scalar to be log(\sqrt(|2πΣ|))
std::vector<NonlinearFactorValuePair> factors{{f0, model0->negLogConstant()},
{f1, model1->negLogConstant()}};
std::vector<NonlinearFactorValuePair> factors{{f0, 0.0}, {f1, 0.0}};

HybridNonlinearFactor mixtureFactor(m1, factors);

Expand Down
3 changes: 2 additions & 1 deletion gtsam/hybrid/tests/testHybridNonlinearISAM.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,7 @@
#include <numeric>

#include "Switching.h"
#include "gtsam/nonlinear/NonlinearFactor.h"

// Include for test suite
#include <CppUnitLite/TestHarness.h>
Expand Down Expand Up @@ -438,7 +439,7 @@ TEST(HybridNonlinearISAM, NonTrivial) {
noise_model),
moving = std::make_shared<PlanarMotionModel>(W(0), W(1), odometry,
noise_model);
std::vector<NonlinearFactor::shared_ptr> components{moving, still};
std::vector<NoiseModelFactor::shared_ptr> components{moving, still};
fg.emplace_shared<HybridNonlinearFactor>(DiscreteKey(M(1), 2), components);

// Add equivalent of ImuFactor
Expand Down

0 comments on commit 8907ca7

Please sign in to comment.