add HybridNonlinearFactor and nonlinear HybridFactorGraph
parent
852a9b9ef6
commit
2927d92a52
|
@ -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
|
|
@ -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
|
|
@ -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
|
|
@ -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);
|
||||
}
|
||||
/* ************************************************************************* */
|
Loading…
Reference in New Issue