add HybridNonlinearFactor and nonlinear HybridFactorGraph

release/4.3a0
Varun Agrawal 2022-05-28 15:43:37 -04:00
parent 852a9b9ef6
commit 2927d92a52
4 changed files with 263 additions and 0 deletions

View File

@ -0,0 +1,108 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file HybridFactorGraph.h
* @brief Nonlinear hybrid factor graph that uses type erasure
* @author Varun Agrawal
* @date May 28, 2022
*/
#pragma once
#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
#include <gtsam/hybrid/HybridFactor.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/inference/Ordering.h>
namespace gtsam {
/**
* Hybrid Factor Graph
* -----------------------
* This is the non-linear version of a hybrid factor graph.
* Everything inside needs to be hybrid factor or hybrid conditional.
*/
class HybridFactorGraph : public FactorGraph<HybridFactor> {
public:
using Base = FactorGraph<HybridFactor>;
using This = HybridFactorGraph; ///< this class
using shared_ptr = boost::shared_ptr<This>; ///< shared_ptr to This
using Values = gtsam::Values; ///< backwards compatibility
using Indices = KeyVector; ///> map from keys to values
/// @name Constructors
/// @{
HybridFactorGraph() = default;
/**
* Implicit copy/downcast constructor to override explicit template container
* constructor. In BayesTree this is used for:
* `cachedSeparatorMarginal_.reset(*separatorMarginal)`
* */
template <class DERIVEDFACTOR>
HybridFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph) : Base(graph) {}
/// @}
// Allow use of selected FactorGraph methods:
using Base::empty;
using Base::reserve;
using Base::size;
using FactorGraph::add;
using Base::operator[];
/// Add a Jacobian factor to the factor graph.
void add(JacobianFactor&& factor);
/// Add a Jacobian factor as a shared ptr.
void add(boost::shared_ptr<JacobianFactor> factor);
/// Add a DecisionTreeFactor to the factor graph.
void add(DecisionTreeFactor&& factor);
/// Add a DecisionTreeFactor as a shared ptr.
void add(boost::shared_ptr<DecisionTreeFactor> factor);
/**
* @brief Linearize all the continuous factors in the
* NonlinearHybridFactorGraph.
*
* @param continuousValues: Dictionary of continuous values.
* @return GaussianHybridFactorGraph::shared_ptr
*/
GaussianHybridFactorGraph::shared_ptr linearize(
const Values& continuousValues) const {
// create an empty linear FG
GaussianHybridFactorGraph::shared_ptr linearFG =
boost::make_shared<GaussianHybridFactorGraph>();
linearFG->reserve(size());
// linearize all factors
for (const sharedFactor& factor : factors_) {
if (factor) {
if (auto nf = boost::dynamic_pointer_cast<NonlinearFactor>) {
(*linearFG) += factor->linearize(linearizationPoint);
} else if (auto hf = boost::dynamic_pointer_cast<HybridFactor>) {
if (hf->isContinuous()) {
}
}
} else
(*linearFG) += GaussianFactor::shared_ptr();
}
}
};
} // namespace gtsam

View File

@ -0,0 +1,45 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file HybridNonlinearFactor.cpp
* @date May 28, 2022
* @author Varun Agrawal
*/
#include <gtsam/hybrid/HybridNonlinearFactor.h>
#include <boost/make_shared.hpp>
namespace gtsam {
/* ************************************************************************* */
HybridNonlinearFactor::HybridNonlinearFactor(NonlinearFactor::shared_ptr other)
: Base(other->keys()), inner_(other) {}
/* ************************************************************************* */
HybridNonlinearFactor::HybridNonlinearFactor(NonlinearFactor &&nf)
: Base(nf.keys()),
inner_(boost::make_shared<NonlinearFactor>(std::move(nf))) {}
/* ************************************************************************* */
bool HybridNonlinearFactor::equals(const HybridFactor &lf, double tol) const {
return Base(lf, tol);
}
/* ************************************************************************* */
void HybridNonlinearFactor::print(const std::string &s,
const KeyFormatter &formatter) const {
HybridFactor::print(s, formatter);
inner_->print("inner: ", formatter);
};
} // namespace gtsam

View File

@ -0,0 +1,60 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file HybridNonlinearFactor.h
* @date May 28, 2022
* @author Varun Agrawal
*/
#pragma once
#include <gtsam/hybrid/HybridFactor.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
namespace gtsam {
/**
* A HybridNonlinearFactor is a layer over NonlinearFactor so that we do not
* have a diamond inheritance.
*/
class HybridNonlinearFactor : public HybridFactor {
private:
NonlinearFactor::shared_ptr inner_;
public:
using Base = HybridFactor;
using This = HybridNonlinearFactor;
using shared_ptr = boost::shared_ptr<This>;
// Explicit conversion from a shared ptr of GF
explicit HybridNonlinearFactor(NonlinearFactor::shared_ptr other);
// Forwarding constructor from concrete NonlinearFactor
explicit HybridNonlinearFactor(NonlinearFactor &&jf);
public:
/// @name Testable
/// @{
/// Check equality.
virtual bool equals(const HybridFactor &lf, double tol) const override;
/// GTSAM print utility.
void print(
const std::string &s = "HybridNonlinearFactor\n",
const KeyFormatter &formatter = DefaultKeyFormatter) const override;
/// @}
NonlinearFactor::shared_ptr inner() const { return inner_; }
};
} // namespace gtsam

View File

@ -0,0 +1,50 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file testHybridFactorGraph.cpp
* @brief Unit tests for HybridFactorGraph
* @author Varun Agrawal
* @date May 2021
*/
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/utilities.h>
#include <gtsam/hybrid/HybridFactorGraph.h>
/* ****************************************************************************
* Test that any linearizedFactorGraph gaussian factors are appended to the
* existing gaussian factor graph in the hybrid factor graph.
*/
TEST(HybridFactorGraph, GaussianFactorGraph) {
// Initialize the hybrid factor graph
HybridFactorGraph fg;
// Add a simple prior factor to the nonlinear factor graph
fg.emplace_shared<PriorFactor<double>>(X(0), 0, Isotropic::Sigma(1, 0.1));
// Add a linear factor to the nonlinear factor graph
fg.add(X(0), I_1x1, Vector1(5));
// Linearization point
Values linearizationPoint;
linearizationPoint.insert<double>(X(0), 0);
GaussianHybridFactorGraph ghfg = fg.linearize(linearizationPoint);
// ghfg.push_back(ghfg.gaussianGraph().begin(), ghfg.gaussianGraph().end());
// EXPECT_LONGS_EQUAL(2, dcmfg.gaussianGraph().size());
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */