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

Address PR comments

release/4.3a0
Fan Jiang 2022-05-22 21:29:12 -07:00
parent 74af969f68
commit b215d3a377
10 changed files with 61 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
gtsam/hybrid/GaussianMixture.cpp → gtsam/hybrid/GaussianMixtureConditional.cpp
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */
/**
* @file GaussianMixture.cpp
* @file GaussianMixtureConditional.cpp
* @brief A hybrid conditional in the Conditional Linear Gaussian scheme
* @author Fan Jiang
* @author Varun Agrawal
@ -20,38 +20,40 @@
#include <gtsam/base/utilities.h>
#include <gtsam/discrete/DecisionTree-inl.h>
#include <gtsam/hybrid/GaussianMixture.h>
#include <gtsam/hybrid/GaussianMixtureConditional.h>
#include <gtsam/inference/Conditional-inst.h>
#include <gtsam/linear/GaussianFactorGraph.h>
namespace gtsam {
GaussianMixture::GaussianMixture(
GaussianMixtureConditional::GaussianMixtureConditional(
const KeyVector &continuousFrontals, const KeyVector &continuousParents,
const DiscreteKeys &discreteParents,
const GaussianMixture::Conditionals &conditionals)
const GaussianMixtureConditional::Conditionals &conditionals)
: BaseFactor(CollectKeys(continuousFrontals, continuousParents),
discreteParents),
BaseConditional(continuousFrontals.size()),
conditionals_(conditionals) {}
const GaussianMixture::Conditionals &GaussianMixture::conditionals() {
/* *******************************************************************************/
const GaussianMixtureConditional::Conditionals &GaussianMixtureConditional::conditionals() {
return conditionals_;
}
GaussianMixture GaussianMixture::FromConditionalList(
/* *******************************************************************************/
GaussianMixtureConditional GaussianMixtureConditional::FromConditionalList(
const KeyVector &continuousFrontals, const KeyVector &continuousParents,
const DiscreteKeys &discreteParents,
const std::vector<GaussianConditional::shared_ptr> &conditionalsList) {
Conditionals dt(discreteParents, conditionalsList);
return GaussianMixture(continuousFrontals, continuousParents, discreteParents,
return GaussianMixtureConditional(continuousFrontals, continuousParents, discreteParents,
dt);
}
/* *******************************************************************************/
GaussianMixture::Sum GaussianMixture::add(
const GaussianMixture::Sum &sum) const {
GaussianMixtureConditional::Sum GaussianMixtureConditional::add(
const GaussianMixtureConditional::Sum &sum) const {
using Y = GaussianFactorGraph;
auto add = [](const Y &graph1, const Y &graph2) {
auto result = graph1;
@ -63,7 +65,7 @@ GaussianMixture::Sum GaussianMixture::add(
}
/* *******************************************************************************/
GaussianMixture::Sum GaussianMixture::asGraph() const {
GaussianMixtureConditional::Sum GaussianMixtureConditional::asGraph() const {
auto lambda = [](const GaussianFactor::shared_ptr &factor) {
GaussianFactorGraph result;
result.push_back(factor);
@ -73,11 +75,12 @@ GaussianMixture::Sum GaussianMixture::asGraph() const {
}
/* TODO(fan): this (for Testable) is not implemented! */
bool GaussianMixture::equals(const HybridFactor &lf, double tol) const {
bool GaussianMixtureConditional::equals(const HybridFactor &lf, double tol) const {
return false;
}
void GaussianMixture::print(const std::string &s,
/* *******************************************************************************/
void GaussianMixtureConditional::print(const std::string &s,
const KeyFormatter &formatter) const {
std::cout << s << ": ";
if (isContinuous_) std::cout << "Cont. ";

34
gtsam/hybrid/GaussianMixture.h → gtsam/hybrid/GaussianMixtureConditional.h
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */
/**
* @file GaussianMixture.h
* @file GaussianMixtureConditional.h
* @brief A hybrid conditional in the Conditional Linear Gaussian scheme
* @author Fan Jiang
* @author Varun Agrawal
@ -25,13 +25,13 @@
#include <gtsam/linear/GaussianConditional.h>
namespace gtsam {
class GaussianMixture : public HybridFactor,
public Conditional<HybridFactor, GaussianMixture> {
class GaussianMixtureConditional : public HybridFactor,
public Conditional<HybridFactor, GaussianMixtureConditional> {
public:
using This = GaussianMixture;
using shared_ptr = boost::shared_ptr<GaussianMixture>;
using This = GaussianMixtureConditional;
using shared_ptr = boost::shared_ptr<GaussianMixtureConditional>;
using BaseFactor = HybridFactor;
using BaseConditional = Conditional<HybridFactor, GaussianMixture>;
using BaseConditional = Conditional<HybridFactor, GaussianMixtureConditional>;
using Conditionals = DecisionTree<Key, GaussianConditional::shared_ptr>;
@ -46,7 +46,7 @@ class GaussianMixture : public HybridFactor,
* @param discreteParents the discrete parents. Will be placed last.
* @param conditionals a decision tree of GaussianConditionals.
*/
GaussianMixture(const KeyVector &continuousFrontals,
GaussianMixtureConditional(const KeyVector &continuousFrontals,
const KeyVector &continuousParents,
const DiscreteKeys &discreteParents,
const Conditionals &conditionals);
@ -55,21 +55,35 @@ class GaussianMixture : public HybridFactor,
const Conditionals &conditionals();
/* *******************************************************************************/
/**
* @brief Combine Decision Trees
*/
Sum add(const Sum &sum) const;
/* *******************************************************************************/
/**
* @brief Convert a DecisionTree of factors into a DT of Gaussian FGs.
*/
Sum asGraph() const;
/**
* @brief Make a Gaussian Mixture from a list of Gaussian conditionals
*
* @param continuousFrontals The continuous frontal variables
* @param continuousParents The continuous parent variables
* @param discreteParents Discrete parents variables
* @param conditionals List of conditionals
*/
static This FromConditionalList(
const KeyVector &continuousFrontals, const KeyVector &continuousParents,
const DiscreteKeys &discreteParents,
const std::vector<GaussianConditional::shared_ptr> &conditionals);
/* TODO: this is only a stub */
bool equals(const HybridFactor &lf, double tol = 1e-9) const override;
/* print utility */
void print(
const std::string &s = "GaussianMixture\n",
const std::string &s = "GaussianMixtureConditional\n",
const KeyFormatter &formatter = DefaultKeyFormatter) const override;
};
} // namespace gtsam

4
gtsam/hybrid/GaussianMixtureFactor.cpp
View File

@ -34,6 +34,7 @@ bool GaussianMixtureFactor::equals(const HybridFactor &lf, double tol) const {
return false;
}
/* *******************************************************************************/
GaussianMixtureFactor GaussianMixtureFactor::FromFactorList(
const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys,
const std::vector<GaussianFactor::shared_ptr> &factorsList) {
@ -42,6 +43,8 @@ GaussianMixtureFactor GaussianMixtureFactor::FromFactorList(
return GaussianMixtureFactor(continuousKeys, discreteKeys, dt);
}
/* *******************************************************************************/
void GaussianMixtureFactor::print(const std::string &s,
const KeyFormatter &formatter) const {
HybridFactor::print(s, formatter);
@ -57,6 +60,7 @@ void GaussianMixtureFactor::print(const std::string &s,
});
}
/* *******************************************************************************/
const GaussianMixtureFactor::Factors &GaussianMixtureFactor::factors() {
return factors_;
}

2
gtsam/hybrid/GaussianMixtureFactor.h
View File

@ -55,10 +55,8 @@ class GaussianMixtureFactor : public HybridFactor {
const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys,
const std::vector<GaussianFactor::shared_ptr> &factors);
/* *******************************************************************************/
Sum add(const Sum &sum) const;
/* *******************************************************************************/
Sum wrappedFactors() const;
bool equals(const HybridFactor &lf, double tol = 1e-9) const override;

2
gtsam/hybrid/HybridBayesNet.h
View File

@ -20,8 +20,6 @@
#include <gtsam/hybrid/HybridConditional.h>
#include <gtsam/inference/BayesNet.h>
#include <iostream> // TODO!
namespace gtsam {
/**

2
gtsam/hybrid/HybridConditional.cpp
View File

@ -50,7 +50,7 @@ HybridConditional::HybridConditional(
}
HybridConditional::HybridConditional(
boost::shared_ptr<GaussianMixture> gaussianMixture)
boost::shared_ptr<GaussianMixtureConditional> gaussianMixture)
: BaseFactor(KeyVector(gaussianMixture->keys().begin(),
gaussianMixture->keys().begin() +
gaussianMixture->nrContinuous),

10
gtsam/hybrid/HybridConditional.h
View File

@ -18,7 +18,7 @@
#pragma once
#include <gtsam/discrete/DiscreteConditional.h>
#include <gtsam/hybrid/GaussianMixture.h>
#include <gtsam/hybrid/GaussianMixtureConditional.h>
#include <gtsam/hybrid/HybridFactor.h>
#include <gtsam/hybrid/HybridFactorGraph.h>
#include <gtsam/inference/Conditional.h>
@ -42,7 +42,7 @@ class HybridFactorGraph;
* As a type-erased variant of:
* - DiscreteConditional
* - GaussianConditional
* - GaussianMixture
* - GaussianMixtureConditional
*
* The reason why this is important is that `Conditional<T>` is a CRTP class.
* CRTP is static polymorphism such that all CRTP classes, while bearing the
@ -93,11 +93,11 @@ class GTSAM_EXPORT HybridConditional
HybridConditional(boost::shared_ptr<DiscreteConditional> discreteConditional);
HybridConditional(boost::shared_ptr<GaussianMixture> gaussianMixture);
HybridConditional(boost::shared_ptr<GaussianMixtureConditional> gaussianMixture);
GaussianMixture::shared_ptr asMixture() {
GaussianMixtureConditional::shared_ptr asMixture() {
if (!isHybrid_) throw std::invalid_argument("Not a mixture");
return boost::static_pointer_cast<GaussianMixture>(inner);
return boost::static_pointer_cast<GaussianMixtureConditional>(inner);
}
DiscreteConditional::shared_ptr asDiscreteConditional() {

10
gtsam/hybrid/HybridFactorGraph.cpp
View File

@ -22,7 +22,7 @@
#include <gtsam/discrete/Assignment.h>
#include <gtsam/discrete/DiscreteEliminationTree.h>
#include <gtsam/discrete/DiscreteFactorGraph.h>
#include <gtsam/hybrid/GaussianMixture.h>
#include <gtsam/hybrid/GaussianMixtureConditional.h>
#include <gtsam/hybrid/GaussianMixtureFactor.h>
#include <gtsam/hybrid/HybridConditional.h>
#include <gtsam/hybrid/HybridDiscreteFactor.h>
@ -104,7 +104,7 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
// Because of all these reasons, we need to think very carefully about how to
// implement the hybrid factors so that we do not get poor performance.
//
// The first thing is how to represent the GaussianMixture. A very possible
// The first thing is how to represent the GaussianMixtureConditional. A very possible
// scenario is that the incoming factors will have different levels of
// discrete keys. For example, imagine we are going to eliminate the fragment:
// $\phi(x1,c1,c2)$, $\phi(x1,c2,c3)$, which is perfectly valid. Now we will
@ -358,11 +358,11 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
auto pair = unzip(eliminationResults);
const GaussianMixture::Conditionals &conditionals = pair.first;
const GaussianMixtureConditional::Conditionals &conditionals = pair.first;
const GaussianMixtureFactor::Factors &separatorFactors = pair.second;
// Create the GaussianMixture from the conditionals
auto conditional = boost::make_shared<GaussianMixture>(
// Create the GaussianMixtureConditional from the conditionals
auto conditional = boost::make_shared<GaussianMixtureConditional>(
frontalKeys, keysOfSeparator, discreteSeparator, conditionals);
if (DEBUG) {

8
gtsam/hybrid/hybrid.i
View File

@ -38,16 +38,16 @@ class GaussianMixtureFactor : gtsam::HybridFactor {
gtsam::DefaultKeyFormatter) const;
};
#include <gtsam/hybrid/GaussianMixture.h>
class GaussianMixture : gtsam::HybridFactor {
static GaussianMixture FromConditionalList(
#include <gtsam/hybrid/GaussianMixtureConditional.h>
class GaussianMixtureConditional : gtsam::HybridFactor {
static GaussianMixtureConditional FromConditionalList(
const gtsam::KeyVector& continuousFrontals,
const gtsam::KeyVector& continuousParents,
const gtsam::DiscreteKeys& discreteParents,
const std::vector<gtsam::GaussianConditional::shared_ptr>&
conditionalsList);
void print(string s = "GaussianMixture\n",
void print(string s = "GaussianMixtureConditional\n",
const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
};

6
gtsam/hybrid/tests/testHybridFactorGraph.cpp
View File

@ -20,7 +20,7 @@
#include <gtsam/discrete/DecisionTreeFactor.h>
#include <gtsam/discrete/DiscreteKey.h>
#include <gtsam/discrete/DiscreteValues.h>
#include <gtsam/hybrid/GaussianMixture.h>
#include <gtsam/hybrid/GaussianMixtureConditional.h>
#include <gtsam/hybrid/GaussianMixtureFactor.h>
#include <gtsam/hybrid/HybridBayesNet.h>
#include <gtsam/hybrid/HybridBayesTree.h>
@ -79,8 +79,8 @@ TEST(HybridFactorGraph, creation) {
hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1)));
GaussianMixture clgc({X(0)}, {X(1)}, DiscreteKeys(DiscreteKey{C(0), 2}),
GaussianMixture::Conditionals(
GaussianMixtureConditional clgc({X(0)}, {X(1)}, DiscreteKeys(DiscreteKey{C(0), 2}),
GaussianMixtureConditional::Conditionals(
C(0),
boost::make_shared<GaussianConditional>(
X(0), Z_3x1, I_3x3, X(1), I_3x3),