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

Remove GaussianFactor wrapper (easy!)

release/4.3a0
Frank Dellaert 2023-01-06 23:41:08 -08:00
parent 8d96b3efb9
commit 4e8dc6e34b
8 changed files with 10 additions and 310 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

70
gtsam/hybrid/HybridGaussianFactor.cpp
View File

@ -1,70 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 HybridGaussianFactor.cpp
* @date Mar 11, 2022
* @author Fan Jiang
*/
#include <gtsam/hybrid/HybridGaussianFactor.h>
#include <gtsam/hybrid/HybridValues.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/JacobianFactor.h>
#include <boost/make_shared.hpp>
namespace gtsam {
/* ************************************************************************* */
HybridGaussianFactor::HybridGaussianFactor(
const boost::shared_ptr<GaussianFactor> &ptr)
: Base(ptr->keys()), inner_(ptr) {}
HybridGaussianFactor::HybridGaussianFactor(
boost::shared_ptr<GaussianFactor> &&ptr)
: Base(ptr->keys()), inner_(std::move(ptr)) {}
HybridGaussianFactor::HybridGaussianFactor(JacobianFactor &&jf)
: Base(jf.keys()),
inner_(boost::make_shared<JacobianFactor>(std::move(jf))) {}
HybridGaussianFactor::HybridGaussianFactor(HessianFactor &&hf)
: Base(hf.keys()),
inner_(boost::make_shared<HessianFactor>(std::move(hf))) {}
/* ************************************************************************* */
bool HybridGaussianFactor::equals(const HybridFactor &other, double tol) const {
const This *e = dynamic_cast<const This *>(&other);
if (e == nullptr) return false;
if (!Base::equals(*e, tol)) return false;
return inner_ ? (e->inner_ ? inner_->equals(*(e->inner_), tol) : false)
: !(e->inner_);
}
/* ************************************************************************* */
void HybridGaussianFactor::print(const std::string &s,
const KeyFormatter &formatter) const {
HybridFactor::print(s, formatter);
if (inner_) {
inner_->print("\n", formatter);
} else {
std::cout << "\nGaussian: nullptr" << std::endl;
}
};
/* ************************************************************************ */
double HybridGaussianFactor::error(const HybridValues &values) const {
return inner_->error(values.continuous());
}
/* ************************************************************************ */
} // namespace gtsam

123
gtsam/hybrid/HybridGaussianFactor.h
View File

@ -1,123 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 HybridGaussianFactor.h
* @date Mar 11, 2022
* @author Fan Jiang
*/
#pragma once
#include <gtsam/hybrid/HybridFactor.h>
#include <gtsam/linear/GaussianFactor.h>
namespace gtsam {
// Forward declarations
class JacobianFactor;
class HessianFactor;
class HybridValues;
/**
* A HybridGaussianFactor is a layer over GaussianFactor so that we do not have
* a diamond inheritance i.e. an extra factor type that inherits from both
* HybridFactor and GaussianFactor.
*
* @ingroup hybrid
*/
class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
private:
GaussianFactor::shared_ptr inner_;
public:
using Base = HybridFactor;
using This = HybridGaussianFactor;
using shared_ptr = boost::shared_ptr<This>;
/// @name Constructors
/// @{
/// Default constructor - for serialization.
HybridGaussianFactor() = default;
/**
* Constructor from shared_ptr of GaussianFactor.
* Example:
* auto ptr = boost::make_shared<JacobianFactor>(...);
* HybridGaussianFactor factor(ptr);
*/
explicit HybridGaussianFactor(const boost::shared_ptr<GaussianFactor> &ptr);
/**
* Forwarding constructor from shared_ptr of GaussianFactor.
* Examples:
* HybridGaussianFactor factor = boost::make_shared<JacobianFactor>(...);
* HybridGaussianFactor factor(boost::make_shared<JacobianFactor>(...));
*/
explicit HybridGaussianFactor(boost::shared_ptr<GaussianFactor> &&ptr);
/**
* Forwarding constructor from rvalue reference of JacobianFactor.
*
* Examples:
* HybridGaussianFactor factor = JacobianFactor(...);
* HybridGaussianFactor factor(JacobianFactor(...));
*/
explicit HybridGaussianFactor(JacobianFactor &&jf);
/**
* Forwarding constructor from rvalue reference of JacobianFactor.
*
* Examples:
* HybridGaussianFactor factor = HessianFactor(...);
* HybridGaussianFactor factor(HessianFactor(...));
*/
explicit HybridGaussianFactor(HessianFactor &&hf);
/// @}
/// @name Testable
/// @{
/// Check equality.
virtual bool equals(const HybridFactor &lf, double tol) const override;
/// GTSAM print utility.
void print(
const std::string &s = "HybridGaussianFactor\n",
const KeyFormatter &formatter = DefaultKeyFormatter) const override;
/// @}
/// @name Standard Interface
/// @{
/// Return pointer to the internal Gaussian factor.
GaussianFactor::shared_ptr inner() const { return inner_; }
/// Return the error of the underlying Gaussian factor.
double error(const HybridValues &values) const override;
/// @}
private:
/** Serialization function */
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar &BOOST_SERIALIZATION_NVP(inner_);
}
};
// traits
template <>
struct traits<HybridGaussianFactor> : public Testable<HybridGaussianFactor> {};
} // namespace gtsam

40
gtsam/hybrid/HybridGaussianFactorGraph.cpp
View File

@ -28,7 +28,6 @@
#include <gtsam/hybrid/HybridConditional.h>
#include <gtsam/hybrid/HybridEliminationTree.h>
#include <gtsam/hybrid/HybridFactor.h>
#include <gtsam/hybrid/HybridGaussianFactor.h>
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
#include <gtsam/hybrid/HybridJunctionTree.h>
#include <gtsam/inference/EliminateableFactorGraph-inst.h>
@ -112,8 +111,6 @@ GaussianFactorGraphTree HybridGaussianFactorGraph::assembleGraphTree() const {
// TODO(dellaert): in C++20, we can use std::visit.
continue;
}
} else if (auto gf = dynamic_pointer_cast<HybridGaussianFactor>(f)) {
result = addGaussian(result, gf->inner());
} else if (dynamic_pointer_cast<DecisionTreeFactor>(f)) {
// Don't do anything for discrete-only factors
// since we want to eliminate continuous values only.
@ -136,8 +133,8 @@ continuousElimination(const HybridGaussianFactorGraph &factors,
const Ordering &frontalKeys) {
GaussianFactorGraph gfg;
for (auto &f : factors) {
if (auto hgf = dynamic_pointer_cast<HybridGaussianFactor>(f)) {
gfg.push_back(hgf->inner());
if (auto gf = dynamic_pointer_cast<GaussianFactor>(f)) {
gfg.push_back(gf);
} else if (auto orphan = dynamic_pointer_cast<OrphanWrapper>(f)) {
// Ignore orphaned clique.
// TODO(dellaert): is this correct? If so explain here.
@ -151,8 +148,7 @@ continuousElimination(const HybridGaussianFactorGraph &factors,
}
auto result = EliminatePreferCholesky(gfg, frontalKeys);
return {boost::make_shared<HybridConditional>(result.first),
boost::make_shared<HybridGaussianFactor>(result.second)};
return {boost::make_shared<HybridConditional>(result.first), result.second};
}
/* ************************************************************************ */
@ -428,28 +424,6 @@ EliminateHybrid(const HybridGaussianFactorGraph &factors,
}
}
/* ************************************************************************ */
void HybridGaussianFactorGraph::add(JacobianFactor &&factor) {
FactorGraph::add(boost::make_shared<HybridGaussianFactor>(std::move(factor)));
}
/* ************************************************************************ */
void HybridGaussianFactorGraph::add(
const boost::shared_ptr<JacobianFactor> &factor) {
FactorGraph::add(boost::make_shared<HybridGaussianFactor>(factor));
}
/* ************************************************************************ */
void HybridGaussianFactorGraph::add(DecisionTreeFactor &&factor) {
FactorGraph::add(std::move(factor));
}
/* ************************************************************************ */
void HybridGaussianFactorGraph::add(
const DecisionTreeFactor::shared_ptr &factor) {
FactorGraph::add(factor);
}
/* ************************************************************************ */
const Ordering HybridGaussianFactorGraph::getHybridOrdering() const {
const KeySet discrete_keys = discreteKeySet();
@ -472,19 +446,13 @@ AlgebraicDecisionTree<Key> HybridGaussianFactorGraph::error(
if (auto gaussianMixture = dynamic_pointer_cast<GaussianMixtureFactor>(f)) {
// Compute factor error and add it.
error_tree = error_tree + gaussianMixture->error(continuousValues);
} else if (auto hybridGaussianFactor =
dynamic_pointer_cast<HybridGaussianFactor>(f)) {
} else if (auto gaussian = dynamic_pointer_cast<GaussianFactor>(f)) {
// If continuous only, get the (double) error
// and add it to the error_tree
GaussianFactor::shared_ptr gaussian = hybridGaussianFactor->inner();
// Compute the error of the gaussian factor.
double error = gaussian->error(continuousValues);
// Add the gaussian factor error to every leaf of the error tree.
error_tree = error_tree.apply(
[error](double leaf_value) { return leaf_value + error; });
} else if (dynamic_pointer_cast<DecisionTreeFactor>(f)) {
// If factor at `idx` is discrete-only, we skip.
continue;

59
gtsam/hybrid/HybridGaussianFactorGraph.h
View File

@ -21,7 +21,6 @@
#include <gtsam/hybrid/GaussianMixtureFactor.h>
#include <gtsam/hybrid/HybridFactor.h>
#include <gtsam/hybrid/HybridFactorGraph.h>
#include <gtsam/hybrid/HybridGaussianFactor.h>
#include <gtsam/inference/EliminateableFactorGraph.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/inference/Ordering.h>
@ -117,59 +116,6 @@ class GTSAM_EXPORT HybridGaussianFactorGraph
HybridGaussianFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph)
: Base(graph) {}
/// @}
/// @name Adding factors.
/// @{
using Base::add;
using Base::push_back;
using Base::reserve;
/// Add a Jacobian factor to the factor graph.
void add(JacobianFactor&& factor);
/// Add a Jacobian factor as a shared ptr.
void add(const 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(const boost::shared_ptr<DecisionTreeFactor>& factor);
/**
* Add a gaussian factor *pointer* to the internal gaussian factor graph
* @param gaussianFactor - boost::shared_ptr to the factor to add
*/
template <typename FACTOR>
IsGaussian<FACTOR> push_gaussian(
const boost::shared_ptr<FACTOR>& gaussianFactor) {
Base::push_back(boost::make_shared<HybridGaussianFactor>(gaussianFactor));
}
/// Construct a factor and add (shared pointer to it) to factor graph.
template <class FACTOR, class... Args>
IsGaussian<FACTOR> emplace_gaussian(Args&&... args) {
auto factor = boost::allocate_shared<FACTOR>(
Eigen::aligned_allocator<FACTOR>(), std::forward<Args>(args)...);
push_gaussian(factor);
}
/**
* @brief Add a single factor shared pointer to the hybrid factor graph.
* Dynamically handles the factor type and assigns it to the correct
* underlying container.
*
* @param sharedFactor The factor to add to this factor graph.
*/
void push_back(const SharedFactor& sharedFactor) {
if (auto p = boost::dynamic_pointer_cast<GaussianFactor>(sharedFactor)) {
push_gaussian(p);
} else {
Base::push_back(sharedFactor);
}
}
/// @}
/// @name Testable
/// @{
@ -184,11 +130,6 @@ class GTSAM_EXPORT HybridGaussianFactorGraph
/// @name Standard Interface
/// @{
using Base::empty;
using Base::size;
using Base::operator[];
using Base::resize;
/**
* @brief Compute error for each discrete assignment,
* and return as a tree.

3
gtsam/hybrid/HybridNonlinearFactorGraph.cpp
View File

@ -65,8 +65,7 @@ HybridGaussianFactorGraph::shared_ptr HybridNonlinearFactorGraph::linearize(
linearFG->push_back(gmf);
} else if (auto nlf = dynamic_pointer_cast<NonlinearFactor>(f)) {
const GaussianFactor::shared_ptr& gf = nlf->linearize(continuousValues);
const auto hgf = boost::make_shared<HybridGaussianFactor>(gf);
linearFG->push_back(hgf);
linearFG->push_back(gf);
} else if (dynamic_pointer_cast<DecisionTreeFactor>(f)) {
// If discrete-only: doesn't need linearization.
linearFG->push_back(f);

5
gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp
View File

@ -27,7 +27,6 @@
#include <gtsam/hybrid/HybridBayesTree.h>
#include <gtsam/hybrid/HybridConditional.h>
#include <gtsam/hybrid/HybridFactor.h>
#include <gtsam/hybrid/HybridGaussianFactor.h>
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
#include <gtsam/hybrid/HybridGaussianISAM.h>
#include <gtsam/hybrid/HybridValues.h>
@ -64,7 +63,7 @@ TEST(HybridGaussianFactorGraph, Creation) {
HybridGaussianFactorGraph hfg;
hfg.add(HybridGaussianFactor(JacobianFactor(X(0), I_3x3, Z_3x1)));
hfg.emplace_shared<JacobianFactor>(X(0), I_3x3, Z_3x1);
// Define a gaussian mixture conditional P(x0|x1, c0) and add it to the factor
// graph
@ -85,7 +84,7 @@ TEST(HybridGaussianFactorGraph, EliminateSequential) {
// Test elimination of a single variable.
HybridGaussianFactorGraph hfg;
hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1)));
hfg.emplace_shared<JacobianFactor>(0, I_3x3, Z_3x1);
auto result = hfg.eliminatePartialSequential(KeyVector{0});

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

@ -496,8 +496,7 @@ TEST(HybridFactorGraph, Printing) {
string expected_hybridFactorGraph = R"(
size: 7
factor 0: Continuous [x0]
factor 0:
A[x0] = [
10
]
@ -549,15 +548,13 @@ factor 2: Hybrid [x1 x2; m1]{
No noise model
}
factor 3: Continuous [x1]
factor 3:
A[x1] = [
10
]
b = [ -10 ]
No noise model
factor 4: Continuous [x2]
factor 4:
A[x2] = [
10
]

11
gtsam/hybrid/tests/testSerializationHybrid.cpp
View File

@ -23,7 +23,6 @@
#include <gtsam/hybrid/HybridBayesNet.h>
#include <gtsam/hybrid/HybridBayesTree.h>
#include <gtsam/hybrid/HybridConditional.h>
#include <gtsam/hybrid/HybridGaussianFactor.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/GaussianConditional.h>
@ -72,16 +71,6 @@ BOOST_CLASS_EXPORT_GUID(noiseModel::Isotropic, "gtsam_noiseModel_Isotropic");
BOOST_CLASS_EXPORT_GUID(HybridBayesNet, "gtsam_HybridBayesNet");
/* ****************************************************************************/
// Test HybridGaussianFactor serialization.
TEST(HybridSerialization, HybridGaussianFactor) {
const HybridGaussianFactor factor(JacobianFactor(X(0), I_3x3, Z_3x1));
EXPECT(equalsObj<HybridGaussianFactor>(factor));
EXPECT(equalsXML<HybridGaussianFactor>(factor));
EXPECT(equalsBinary<HybridGaussianFactor>(factor));
}
/* ****************************************************************************/
// Test GaussianMixtureFactor serialization.
TEST(HybridSerialization, GaussianMixtureFactor) {