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

Merge branch 'hybrid/hybrid-factor-graph' into hybrid/pruning

release/4.3a0
Varun Agrawal 2022-07-10 13:46:54 -04:00
parent f1a1f1cd2a 91da5209fe
commit 34298c497d
25 changed files with 689 additions and 201 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
gtsam/discrete/DecisionTree-inl.h
View File

@ -291,10 +291,7 @@ namespace gtsam {
}
os << "\"" << this->id() << "\" -> \"" << branch->id() << "\"";
if (B == 2) {
if (i == 0) os << " [style=dashed]";
if (i > 1) os << " [style=bold]";
}
if (B == 2 && i == 0) os << " [style=dashed]";
os << std::endl;
branch->dot(os, labelFormatter, valueFormatter, showZero);
}

2
gtsam/discrete/DecisionTree.h
View File

@ -220,7 +220,7 @@ namespace gtsam {
/// @{
/// Make virtual
virtual ~DecisionTree() {}
virtual ~DecisionTree() = default;
/// Check if tree is empty.
bool empty() const { return !root_; }

4
gtsam/discrete/DiscreteFactorGraph.h
View File

@ -209,6 +209,10 @@ class GTSAM_EXPORT DiscreteFactorGraph
/// @}
}; // \ DiscreteFactorGraph
std::pair<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr> //
EliminateForMPE(const DiscreteFactorGraph& factors,
const Ordering& frontalKeys);
/// traits
template <>
struct traits<DiscreteFactorGraph> : public Testable<DiscreteFactorGraph> {};

2
gtsam/hybrid/GaussianMixtureFactor.h
View File

@ -108,7 +108,7 @@ class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor {
bool equals(const HybridFactor &lf, double tol = 1e-9) const override;
void print(
const std::string &s = "HybridFactor\n",
const std::string &s = "GaussianMixtureFactor\n",
const KeyFormatter &formatter = DefaultKeyFormatter) const override;
/// @}

23
gtsam/hybrid/HybridBayesNet.cpp
View File

@ -10,6 +10,7 @@
* @file HybridBayesNet.cpp
* @brief A bayes net of Gaussian Conditionals indexed by discrete keys.
* @author Fan Jiang
* @author Varun Agrawal
* @date January 2022
*/
@ -106,4 +107,26 @@ HybridBayesNet HybridBayesNet::prune(
return prunedBayesNetFragment;
}
/* ************************************************************************* */
GaussianMixture::shared_ptr HybridBayesNet::atGaussian(size_t i) const {
return boost::dynamic_pointer_cast<GaussianMixture>(factors_.at(i)->inner());
}
/* ************************************************************************* */
DiscreteConditional::shared_ptr HybridBayesNet::atDiscrete(size_t i) const {
return boost::dynamic_pointer_cast<DiscreteConditional>(
factors_.at(i)->inner());
}
/* ************************************************************************* */
GaussianBayesNet HybridBayesNet::choose(
const DiscreteValues &assignment) const {
GaussianBayesNet gbn;
for (size_t idx = 0; idx < size(); idx++) {
GaussianMixture gm = *this->atGaussian(idx);
gbn.push_back(gm(assignment));
}
return gbn;
}
} // namespace gtsam

28
gtsam/hybrid/HybridBayesNet.h
View File

@ -20,6 +20,7 @@
#include <gtsam/discrete/DecisionTreeFactor.h>
#include <gtsam/hybrid/HybridConditional.h>
#include <gtsam/inference/BayesNet.h>
#include <gtsam/linear/GaussianBayesNet.h>
namespace gtsam {
@ -36,10 +37,35 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet<HybridConditional> {
using sharedConditional = boost::shared_ptr<ConditionalType>;
/** Construct empty bayes net */
HybridBayesNet() : Base() {}
HybridBayesNet() = default;
/// Prune the Hybrid Bayes Net given the discrete decision tree.
HybridBayesNet prune(
const DecisionTreeFactor::shared_ptr &discreteFactor) const;
/// Add HybridConditional to Bayes Net
using Base::add;
/// Add a discrete conditional to the Bayes Net.
void add(const DiscreteKey &key, const std::string &table) {
push_back(
HybridConditional(boost::make_shared<DiscreteConditional>(key, table)));
}
/// Get a specific Gaussian mixture by index `i`.
GaussianMixture::shared_ptr atGaussian(size_t i) const;
/// Get a specific discrete conditional by index `i`.
DiscreteConditional::shared_ptr atDiscrete(size_t i) const;
/**
* @brief Get the Gaussian Bayes Net which corresponds to a specific discrete
* value assignment.
*
* @param assignment The discrete value assignment for the discrete keys.
* @return GaussianBayesNet
*/
GaussianBayesNet choose(const DiscreteValues &assignment) const;
};
} // namespace gtsam

3
gtsam/hybrid/HybridFactor.h
View File

@ -52,9 +52,6 @@ class GTSAM_EXPORT HybridFactor : public Factor {
protected:
// Set of DiscreteKeys for this factor.
DiscreteKeys discreteKeys_;
// For bookkeeping
KeyVector continuousKeys_;
/// Record continuous keys for book-keeping
KeyVector continuousKeys_;

140
gtsam/hybrid/HybridFactorGraph.h Normal file
View File

@ -0,0 +1,140 @@
/* ----------------------------------------------------------------------------
* 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 Hybrid factor graph base class that uses type erasure
* @author Varun Agrawal
* @date May 28, 2022
*/
#pragma once
#include <gtsam/discrete/DiscreteFactor.h>
#include <gtsam/hybrid/HybridDiscreteFactor.h>
#include <gtsam/hybrid/HybridFactor.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/inference/Ordering.h>
#include <boost/format.hpp>
namespace gtsam {
using SharedFactor = boost::shared_ptr<Factor>;
/**
* Hybrid Factor Graph
* -----------------------
* This is the base 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
protected:
/// Check if FACTOR type is derived from DiscreteFactor.
template <typename FACTOR>
using IsDiscrete = typename std::enable_if<
std::is_base_of<DiscreteFactor, FACTOR>::value>::type;
/// Check if FACTOR type is derived from HybridFactor.
template <typename FACTOR>
using IsHybrid = typename std::enable_if<
std::is_base_of<HybridFactor, FACTOR>::value>::type;
public:
/// @name Constructors
/// @{
/// Default constructor
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 Base::operator[];
using Base::add;
using Base::push_back;
using Base::resize;
/**
* Add a discrete factor *pointer* to the internal discrete graph
* @param discreteFactor - boost::shared_ptr to the factor to add
*/
template <typename FACTOR>
IsDiscrete<FACTOR> push_discrete(
const boost::shared_ptr<FACTOR>& discreteFactor) {
Base::push_back(boost::make_shared<HybridDiscreteFactor>(discreteFactor));
}
/**
* Add a discrete-continuous (Hybrid) factor *pointer* to the graph
* @param hybridFactor - boost::shared_ptr to the factor to add
*/
template <typename FACTOR>
IsHybrid<FACTOR> push_hybrid(const boost::shared_ptr<FACTOR>& hybridFactor) {
Base::push_back(hybridFactor);
}
/// delete emplace_shared.
template <class FACTOR, class... Args>
void emplace_shared(Args&&... args) = delete;
/// Construct a factor and add (shared pointer to it) to factor graph.
template <class FACTOR, class... Args>
IsDiscrete<FACTOR> emplace_discrete(Args&&... args) {
auto factor = boost::allocate_shared<FACTOR>(
Eigen::aligned_allocator<FACTOR>(), std::forward<Args>(args)...);
push_discrete(factor);
}
/// Construct a factor and add (shared pointer to it) to factor graph.
template <class FACTOR, class... Args>
IsHybrid<FACTOR> emplace_hybrid(Args&&... args) {
auto factor = boost::allocate_shared<FACTOR>(
Eigen::aligned_allocator<FACTOR>(), std::forward<Args>(args)...);
push_hybrid(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<DiscreteFactor>(sharedFactor)) {
push_discrete(p);
}
if (auto p = boost::dynamic_pointer_cast<HybridFactor>(sharedFactor)) {
push_hybrid(p);
}
}
};
} // namespace gtsam

60
gtsam/hybrid/HybridGaussianFactorGraph.h
View File

@ -19,6 +19,8 @@
#pragma once
#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>
@ -53,10 +55,9 @@ struct EliminationTraits<HybridGaussianFactorGraph> {
typedef HybridBayesNet
BayesNetType; ///< Type of Bayes net from sequential elimination
typedef HybridEliminationTree
EliminationTreeType; ///< Type of elimination tree
typedef HybridBayesTree BayesTreeType; ///< Type of Bayes tree
typedef HybridJunctionTree
JunctionTreeType; ///< Type of Junction tree
EliminationTreeType; ///< Type of elimination tree
typedef HybridBayesTree BayesTreeType; ///< Type of Bayes tree
typedef HybridJunctionTree JunctionTreeType; ///< Type of Junction tree
/// The default dense elimination function
static std::pair<boost::shared_ptr<ConditionalType>,
boost::shared_ptr<FactorType> >
@ -72,10 +73,16 @@ struct EliminationTraits<HybridGaussianFactorGraph> {
* Everything inside needs to be hybrid factor or hybrid conditional.
*/
class GTSAM_EXPORT HybridGaussianFactorGraph
: public FactorGraph<HybridFactor>,
: public HybridFactorGraph,
public EliminateableFactorGraph<HybridGaussianFactorGraph> {
protected:
/// Check if FACTOR type is derived from GaussianFactor.
template <typename FACTOR>
using IsGaussian = typename std::enable_if<
std::is_base_of<GaussianFactor, FACTOR>::value>::type;
public:
using Base = FactorGraph<HybridFactor>;
using Base = HybridFactorGraph;
using This = HybridGaussianFactorGraph; ///< this class
using BaseEliminateable =
EliminateableFactorGraph<This>; ///< for elimination
@ -100,7 +107,13 @@ class GTSAM_EXPORT HybridGaussianFactorGraph
/// @}
using FactorGraph::add;
using Base::empty;
using Base::reserve;
using Base::size;
using Base::operator[];
using Base::add;
using Base::push_back;
using Base::resize;
/// Add a Jacobian factor to the factor graph.
void add(JacobianFactor&& factor);
@ -113,6 +126,39 @@ class GTSAM_EXPORT HybridGaussianFactorGraph
/// Add a DecisionTreeFactor as a shared ptr.
void add(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);
}
}
};
} // namespace gtsam

138
gtsam/hybrid/tests/Switching.h
View File

@ -18,17 +18,135 @@
#include <gtsam/base/Matrix.h>
#include <gtsam/discrete/DecisionTreeFactor.h>
#include <gtsam/discrete/DiscreteDistribution.h>
#include <gtsam/hybrid/GaussianMixtureFactor.h>
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#pragma once
using gtsam::symbol_shorthand::C;
using gtsam::symbol_shorthand::X;
namespace gtsam {
using symbol_shorthand::C;
using symbol_shorthand::M;
using symbol_shorthand::X;
/* ****************************************************************************/
// Test fixture with switching network.
// TODO(Varun) Currently this is only linear. We need to add nonlinear support
// and then update to
// https://github.com/borglab/gtsam/pull/973/files#diff-58c02b3b197ebf731694946e87762d252e9eaa2f5c6c4ba22d618085b321ca23
struct Switching {
size_t K;
DiscreteKeys modes;
HybridGaussianFactorGraph linearizedFactorGraph;
Values linearizationPoint;
using MotionModel = BetweenFactor<double>;
// using MotionMixture = MixtureFactor<MotionModel>;
/// Create with given number of time steps.
Switching(size_t K, double between_sigma = 1.0, double prior_sigma = 0.1)
: K(K) {
// Create DiscreteKeys for binary K modes, modes[0] will not be used.
modes = addDiscreteModes(K);
// Create hybrid factor graph.
// Add a prior on X(1).
auto prior = boost::make_shared<JacobianFactor>(
X(1), Matrix11::Ones() / prior_sigma, Vector1::Zero());
linearizedFactorGraph.push_back(prior);
// Add "motion models".
linearizedFactorGraph = addMotionModels(K);
// Add measurement factors
for (size_t k = 1; k <= K; k++) {
linearizedFactorGraph.emplace_gaussian<JacobianFactor>(
X(k), Matrix11::Ones() / 0.1, Vector1::Zero());
}
// Add "mode chain"
linearizedFactorGraph = addModeChain(linearizedFactorGraph);
// Create the linearization point.
for (size_t k = 1; k <= K; k++) {
linearizationPoint.insert<double>(X(k), static_cast<double>(k));
}
}
/// Create DiscreteKeys for K binary modes.
DiscreteKeys addDiscreteModes(size_t K) {
DiscreteKeys m;
for (size_t k = 0; k <= K; k++) {
m.emplace_back(M(k), 2);
}
return m;
}
/// Helper function to add motion models for each [k, k+1] interval.
HybridGaussianFactorGraph addMotionModels(size_t K) {
HybridGaussianFactorGraph hgfg;
for (size_t k = 1; k < K; k++) {
auto keys = {X(k), X(k + 1)};
auto components = motionModels(k);
hgfg.emplace_hybrid<GaussianMixtureFactor>(keys, DiscreteKeys{modes[k]},
components);
}
return hgfg;
}
// Create motion models for a given time step
static std::vector<GaussianFactor::shared_ptr> motionModels(
size_t k, double sigma = 1.0) {
auto noise_model = noiseModel::Isotropic::Sigma(1, sigma);
auto still = boost::make_shared<JacobianFactor>(
X(k), -Matrix11::Ones() / sigma, X(k + 1),
Matrix11::Ones() / sigma, Vector1::Zero()),
moving = boost::make_shared<JacobianFactor>(
X(k), -Matrix11::Ones() / sigma, X(k + 1),
Matrix11::Ones() / sigma, -Vector1::Ones() / sigma);
return {boost::dynamic_pointer_cast<GaussianFactor>(still),
boost::dynamic_pointer_cast<GaussianFactor>(moving)};
}
// // Add "mode chain" to NonlinearHybridFactorGraph
// void addModeChain(HybridNonlinearFactorGraph& fg) {
// auto prior = boost::make_shared<DiscreteDistribution>(modes[1], "1/1");
// fg.push_discrete(prior);
// for (size_t k = 1; k < K - 1; k++) {
// auto parents = {modes[k]};
// auto conditional = boost::make_shared<DiscreteConditional>(
// modes[k + 1], parents, "1/2 3/2");
// fg.push_discrete(conditional);
// }
// }
// Add "mode chain" to GaussianHybridFactorGraph
HybridGaussianFactorGraph addModeChain(HybridGaussianFactorGraph& fg) {
auto prior = boost::make_shared<DiscreteDistribution>(modes[1], "1/1");
fg.push_discrete(prior);
for (size_t k = 1; k < K - 1; k++) {
auto parents = {modes[k]};
auto conditional = boost::make_shared<DiscreteConditional>(
modes[k + 1], parents, "1/2 3/2");
fg.push_discrete(conditional);
}
return fg;
}
};
/**
* @brief Create a switching system chain. A switching system is a continuous
* system which depends on a discrete mode at each time step of the chain.
*
* @param n The number of chain elements.
* @param keyFunc The functional to help specify the continuous key.
* @param dKeyFunc The functional to help specify the discrete key.
* @return HybridGaussianFactorGraph::shared_ptr
*/
inline HybridGaussianFactorGraph::shared_ptr makeSwitchingChain(
size_t n, std::function<Key(int)> keyFunc = X,
std::function<Key(int)> dKeyFunc = C) {
@ -54,9 +172,19 @@ inline HybridGaussianFactorGraph::shared_ptr makeSwitchingChain(
return boost::make_shared<HybridGaussianFactorGraph>(std::move(hfg));
}
/**
* @brief Return the ordering as a binary tree such that all parent nodes are
* above their children.
*
* This will result in a nested dissection Bayes tree after elimination.
*
* @param input The original ordering.
* @return std::pair<KeyVector, std::vector<int>>
*/
inline std::pair<KeyVector, std::vector<int>> makeBinaryOrdering(
std::vector<Key> &input) {
std::vector<Key>& input) {
KeyVector new_order;
std::vector<int> levels(input.size());
std::function<void(std::vector<Key>::iterator, std::vector<Key>::iterator,
int)>
@ -79,7 +207,7 @@ inline std::pair<KeyVector, std::vector<int>> makeBinaryOrdering(
bsg(input.begin(), input.end(), 0);
std::reverse(new_order.begin(), new_order.end());
// std::reverse(levels.begin(), levels.end());
return {new_order, levels};
}

226
gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp
View File

@ -58,26 +58,30 @@ using gtsam::symbol_shorthand::X;
using gtsam::symbol_shorthand::Y;
/* ************************************************************************* */
TEST(HybridGaussianFactorGraph, creation) {
HybridConditional test;
TEST(HybridGaussianFactorGraph, Creation) {
HybridConditional conditional;
HybridGaussianFactorGraph hfg;
hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1)));
hfg.add(HybridGaussianFactor(JacobianFactor(X(0), I_3x3, Z_3x1)));
GaussianMixture clgc(
{X(0)}, {X(1)}, DiscreteKeys(DiscreteKey{C(0), 2}),
GaussianMixture::Conditionals(
C(0),
boost::make_shared<GaussianConditional>(X(0), Z_3x1, I_3x3, X(1),
I_3x3),
boost::make_shared<GaussianConditional>(X(0), Vector3::Ones(), I_3x3,
X(1), I_3x3)));
GTSAM_PRINT(clgc);
// Define a gaussian mixture conditional P(x0|x1, c0) and add it to the factor
// graph
GaussianMixture gm({X(0)}, {X(1)}, DiscreteKeys(DiscreteKey{C(0), 2}),
GaussianMixture::Conditionals(
C(0),
boost::make_shared<GaussianConditional>(
X(0), Z_3x1, I_3x3, X(1), I_3x3),
boost::make_shared<GaussianConditional>(
X(0), Vector3::Ones(), I_3x3, X(1), I_3x3)));
hfg.add(gm);
EXPECT_LONGS_EQUAL(2, hfg.size());
}
/* ************************************************************************* */
TEST(HybridGaussianFactorGraph, eliminate) {
TEST(HybridGaussianFactorGraph, EliminateSequential) {
// Test elimination of a single variable.
HybridGaussianFactorGraph hfg;
hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1)));
@ -88,11 +92,13 @@ TEST(HybridGaussianFactorGraph, eliminate) {
}
/* ************************************************************************* */
TEST(HybridGaussianFactorGraph, eliminateMultifrontal) {
TEST(HybridGaussianFactorGraph, EliminateMultifrontal) {
// Test multifrontal elimination
HybridGaussianFactorGraph hfg;
DiscreteKey c(C(1), 2);
// Add priors on x0 and c1
hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
hfg.add(HybridDiscreteFactor(DecisionTreeFactor(c, {2, 8})));
@ -110,9 +116,12 @@ TEST(HybridGaussianFactorGraph, eliminateFullSequentialEqualChance) {
DiscreteKey c1(C(1), 2);
// Add prior on x0
hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
// Add factor between x0 and x1
hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
// Add a gaussian mixture factor ϕ(x1, c1)
DecisionTree<Key, GaussianFactor::shared_ptr> dt(
C(1), boost::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1),
boost::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones()));
@ -123,9 +132,9 @@ TEST(HybridGaussianFactorGraph, eliminateFullSequentialEqualChance) {
hfg.eliminateSequential(Ordering::ColamdConstrainedLast(hfg, {C(1)}));
auto dc = result->at(2)->asDiscreteConditional();
DiscreteValues dv;
dv[C(1)] = 0;
EXPECT_DOUBLES_EQUAL(0.6225, dc->operator()(dv), 1e-3);
DiscreteValues mode;
mode[C(1)] = 0;
EXPECT_DOUBLES_EQUAL(0.6225, (*dc)(mode), 1e-3);
}
/* ************************************************************************* */
@ -134,26 +143,26 @@ TEST(HybridGaussianFactorGraph, eliminateFullSequentialSimple) {
DiscreteKey c1(C(1), 2);
// Add prior on x0
hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
// Add factor between x0 and x1
hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
DecisionTree<Key, GaussianFactor::shared_ptr> dt(
C(1), boost::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1),
boost::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones()));
hfg.add(GaussianMixtureFactor({X(1)}, {c1}, dt));
// hfg.add(GaussianMixtureFactor({X(0)}, {c1}, dt));
// Discrete probability table for c1
hfg.add(HybridDiscreteFactor(DecisionTreeFactor(c1, {2, 8})));
// Joint discrete probability table for c1, c2
hfg.add(HybridDiscreteFactor(
DecisionTreeFactor({{C(1), 2}, {C(2), 2}}, "1 2 3 4")));
// hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(2), 2}, {C(3), 2}}, "1
// 2 3 4"))); hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(3), 2},
// {C(1), 2}}, "1 2 2 1")));
auto result = hfg.eliminateSequential(
Ordering::ColamdConstrainedLast(hfg, {C(1), C(2)}));
GTSAM_PRINT(*result);
EXPECT_LONGS_EQUAL(4, result->size());
}
/* ************************************************************************* */
@ -165,28 +174,19 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalSimple) {
hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
// DecisionTree<Key, GaussianFactor::shared_ptr> dt(
// C(1), boost::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1),
// boost::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones()));
// hfg.add(GaussianMixtureFactor({X(1)}, {c1}, dt));
hfg.add(GaussianMixtureFactor::FromFactors(
{X(1)}, {{C(1), 2}},
{boost::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1),
boost::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones())}));
// hfg.add(GaussianMixtureFactor({X(0)}, {c1}, dt));
hfg.add(DecisionTreeFactor(c1, {2, 8}));
hfg.add(DecisionTreeFactor({{C(1), 2}, {C(2), 2}}, "1 2 3 4"));
// hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(2), 2}, {C(3), 2}}, "1
// 2 3 4"))); hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(3), 2},
// {C(1), 2}}, "1 2 2 1")));
auto result = hfg.eliminateMultifrontal(
Ordering::ColamdConstrainedLast(hfg, {C(1), C(2)}));
GTSAM_PRINT(*result);
GTSAM_PRINT(*result->marginalFactor(C(2)));
// GTSAM_PRINT(*result);
// GTSAM_PRINT(*result->marginalFactor(C(2)));
}
/* ************************************************************************* */
@ -195,30 +195,28 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalCLG) {
DiscreteKey c(C(1), 2);
// Prior on x0
hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
// Factor between x0-x1
hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
// Decision tree with different modes on x1
DecisionTree<Key, GaussianFactor::shared_ptr> dt(
C(1), boost::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1),
boost::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones()));
// Hybrid factor P(x1|c1)
hfg.add(GaussianMixtureFactor({X(1)}, {c}, dt));
// Prior factor on c1
hfg.add(HybridDiscreteFactor(DecisionTreeFactor(c, {2, 8})));
// hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(1), 2}, {C(2), 2}}, "1
// 2 3 4")));
// Get a constrained ordering keeping c1 last
auto ordering_full = Ordering::ColamdConstrainedLast(hfg, {C(1)});
// Returns a Hybrid Bayes Tree with distribution P(x0|x1)P(x1|c1)P(c1)
HybridBayesTree::shared_ptr hbt = hfg.eliminateMultifrontal(ordering_full);
GTSAM_PRINT(*hbt);
/*
Explanation: the Junction tree will need to reeliminate to get to the marginal
on X(1), which is not possible because it involves eliminating discrete before
continuous. The solution to this, however, is in Murphy02. TLDR is that this
is 1. expensive and 2. inexact. neverless it is doable. And I believe that we
should do this.
*/
EXPECT_LONGS_EQUAL(3, hbt->size());
}
/* ************************************************************************* */
@ -233,10 +231,6 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalTwoClique) {
hfg.add(JacobianFactor(X(1), I_3x3, X(2), -I_3x3, Z_3x1));
{
// DecisionTree<Key, GaussianFactor::shared_ptr> dt(
// C(0), boost::make_shared<JacobianFactor>(X(0), I_3x3, Z_3x1),
// boost::make_shared<JacobianFactor>(X(0), I_3x3, Vector3::Ones()));
hfg.add(GaussianMixtureFactor::FromFactors(
{X(0)}, {{C(0), 2}},
{boost::make_shared<JacobianFactor>(X(0), I_3x3, Z_3x1),
@ -249,7 +243,6 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalTwoClique) {
hfg.add(GaussianMixtureFactor({X(2)}, {{C(1), 2}}, dt1));
}
// hfg.add(HybridDiscreteFactor(DecisionTreeFactor(c, {2, 8})));
hfg.add(HybridDiscreteFactor(
DecisionTreeFactor({{C(1), 2}, {C(2), 2}}, "1 2 3 4")));
@ -273,26 +266,37 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalTwoClique) {
auto ordering_full =
Ordering::ColamdConstrainedLast(hfg, {C(0), C(1), C(2), C(3)});
GTSAM_PRINT(ordering_full);
HybridBayesTree::shared_ptr hbt;
HybridGaussianFactorGraph::shared_ptr remaining;
std::tie(hbt, remaining) = hfg.eliminatePartialMultifrontal(ordering_full);
GTSAM_PRINT(*hbt);
// GTSAM_PRINT(*hbt);
// GTSAM_PRINT(*remaining);
GTSAM_PRINT(*remaining);
hbt->dot(std::cout);
// hbt->marginalFactor(X(1))->print("HBT: ");
/*
Explanation: the Junction tree will need to reeliminate to get to the marginal
on X(1), which is not possible because it involves eliminating discrete before
continuous. The solution to this, however, is in Murphy02. TLDR is that this
is 1. expensive and 2. inexact. neverless it is doable. And I believe that we
should do this.
(Fan) Explanation: the Junction tree will need to reeliminate to get to the
marginal on X(1), which is not possible because it involves eliminating
discrete before continuous. The solution to this, however, is in Murphy02.
TLDR is that this is 1. expensive and 2. inexact. nevertheless it is doable.
And I believe that we should do this.
*/
}
void dotPrint(const HybridGaussianFactorGraph::shared_ptr &hfg,
const HybridBayesTree::shared_ptr &hbt,
const Ordering &ordering) {
DotWriter dw;
dw.positionHints['c'] = 2;
dw.positionHints['x'] = 1;
std::cout << hfg->dot(DefaultKeyFormatter, dw);
std::cout << "\n";
hbt->dot(std::cout);
std::cout << "\n";
std::cout << hfg->eliminateSequential(ordering)->dot(DefaultKeyFormatter, dw);
}
/* ************************************************************************* */
// TODO(fan): make a graph like Varun's paper one
TEST(HybridGaussianFactorGraph, Switching) {
@ -326,9 +330,6 @@ TEST(HybridGaussianFactorGraph, Switching) {
for (auto &l : lvls) {
l = -l;
}
std::copy(lvls.begin(), lvls.end(),
std::ostream_iterator<int>(std::cout, ","));
std::cout << "\n";
}
{
std::vector<int> naturalC(N - 1);
@ -342,63 +343,19 @@ TEST(HybridGaussianFactorGraph, Switching) {
// std::copy(ordC.begin(), ordC.end(), std::back_inserter(ordering));
std::tie(ndC, lvls) = makeBinaryOrdering(ordC);
std::copy(ndC.begin(), ndC.end(), std::back_inserter(ordering));
std::copy(lvls.begin(), lvls.end(),
std::ostream_iterator<int>(std::cout, ","));
}
auto ordering_full = Ordering(ordering);
// auto ordering_full =
// Ordering();
// for (int i = 1; i <= 9; i++) {
// ordering_full.push_back(X(i));
// }
// for (int i = 1; i < 9; i++) {
// ordering_full.push_back(C(i));
// }
// auto ordering_full =
// Ordering(KeyVector{X(1), X(4), X(2), X(6), X(9), X(8), X(3), X(7),
// X(5),
// C(1), C(2), C(3), C(4), C(5), C(6), C(7), C(8)});
// GTSAM_PRINT(*hfg);
GTSAM_PRINT(ordering_full);
// GTSAM_PRINT(ordering_full);
HybridBayesTree::shared_ptr hbt;
HybridGaussianFactorGraph::shared_ptr remaining;
std::tie(hbt, remaining) = hfg->eliminatePartialMultifrontal(ordering_full);
// GTSAM_PRINT(*hbt);
// GTSAM_PRINT(*remaining);
{
DotWriter dw;
dw.positionHints['c'] = 2;
dw.positionHints['x'] = 1;
std::cout << hfg->dot(DefaultKeyFormatter, dw);
std::cout << "\n";
hbt->dot(std::cout);
}
{
DotWriter dw;
// dw.positionHints['c'] = 2;
// dw.positionHints['x'] = 1;
std::cout << "\n";
std::cout << hfg->eliminateSequential(ordering_full)
->dot(DefaultKeyFormatter, dw);
}
/*
Explanation: the Junction tree will need to reeliminate to get to the marginal
on X(1), which is not possible because it involves eliminating discrete before
continuous. The solution to this, however, is in Murphy02. TLDR is that this
is 1. expensive and 2. inexact. neverless it is doable. And I believe that we
should do this.
*/
hbt->marginalFactor(C(11))->print("HBT: ");
// hbt->marginalFactor(C(11))->print("HBT: ");
}
/* ************************************************************************* */
@ -434,9 +391,6 @@ TEST(HybridGaussianFactorGraph, SwitchingISAM) {
for (auto &l : lvls) {
l = -l;
}
std::copy(lvls.begin(), lvls.end(),
std::ostream_iterator<int>(std::cout, ","));
std::cout << "\n";
}
{
std::vector<int> naturalC(N - 1);
@ -450,40 +404,16 @@ TEST(HybridGaussianFactorGraph, SwitchingISAM) {
// std::copy(ordC.begin(), ordC.end(), std::back_inserter(ordering));
std::tie(ndC, lvls) = makeBinaryOrdering(ordC);
std::copy(ndC.begin(), ndC.end(), std::back_inserter(ordering));
std::copy(lvls.begin(), lvls.end(),
std::ostream_iterator<int>(std::cout, ","));
}
auto ordering_full = Ordering(ordering);
// GTSAM_PRINT(*hfg);
GTSAM_PRINT(ordering_full);
// GTSAM_PRINT(ordering_full);
HybridBayesTree::shared_ptr hbt;
HybridGaussianFactorGraph::shared_ptr remaining;
std::tie(hbt, remaining) = hfg->eliminatePartialMultifrontal(ordering_full);
// GTSAM_PRINT(*hbt);
// GTSAM_PRINT(*remaining);
{
DotWriter dw;
dw.positionHints['c'] = 2;
dw.positionHints['x'] = 1;
std::cout << hfg->dot(DefaultKeyFormatter, dw);
std::cout << "\n";
hbt->dot(std::cout);
}
{
DotWriter dw;
// dw.positionHints['c'] = 2;
// dw.positionHints['x'] = 1;
std::cout << "\n";
std::cout << hfg->eliminateSequential(ordering_full)
->dot(DefaultKeyFormatter, dw);
}
auto new_fg = makeSwitchingChain(12);
auto isam = HybridGaussianISAM(*hbt);
@ -492,13 +422,14 @@ TEST(HybridGaussianFactorGraph, SwitchingISAM) {
factorGraph.push_back(new_fg->at(new_fg->size() - 2));
factorGraph.push_back(new_fg->at(new_fg->size() - 1));
isam.update(factorGraph);
std::cout << isam.dot();
isam.marginalFactor(C(11))->print();
// std::cout << isam.dot();
// isam.marginalFactor(C(11))->print();
}
}
/* ************************************************************************* */
TEST(HybridGaussianFactorGraph, SwitchingTwoVar) {
// TODO(Varun) Actually test something!
TEST_DISABLED(HybridGaussianFactorGraph, SwitchingTwoVar) {
const int N = 7;
auto hfg = makeSwitchingChain(N, X);
hfg->push_back(*makeSwitchingChain(N, Y, D));
@ -517,21 +448,6 @@ TEST(HybridGaussianFactorGraph, SwitchingTwoVar) {
ordX.emplace_back(Y(i));
}
// {
// KeyVector ndX;
// std::vector<int> lvls;
// std::tie(ndX, lvls) = makeBinaryOrdering(naturalX);
// std::copy(ndX.begin(), ndX.end(), std::back_inserter(ordering));
// std::copy(lvls.begin(), lvls.end(),
// std::ostream_iterator<int>(std::cout, ","));
// std::cout << "\n";
// for (size_t i = 0; i < N; i++) {
// ordX.emplace_back(X(ndX[i]));
// ordX.emplace_back(Y(ndX[i]));
// }
// }
for (size_t i = 1; i <= N - 1; i++) {
ordX.emplace_back(C(i));
}

93
gtsam/hybrid/tests/testHybridBayesNet.cpp Normal file
View File

@ -0,0 +1,93 @@
/* ----------------------------------------------------------------------------
* 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 testHybridBayesNet.cpp
* @brief Unit tests for HybridBayesNet
* @author Varun Agrawal
* @author Fan Jiang
* @author Frank Dellaert
* @date December 2021
*/
#include <gtsam/hybrid/HybridBayesNet.h>
#include "Switching.h"
// Include for test suite
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
using noiseModel::Isotropic;
using symbol_shorthand::M;
using symbol_shorthand::X;
static const DiscreteKey Asia(0, 2);
/* ****************************************************************************/
// Test creation
TEST(HybridBayesNet, Creation) {
HybridBayesNet bayesNet;
bayesNet.add(Asia, "99/1");
DiscreteConditional expected(Asia, "99/1");
CHECK(bayesNet.atDiscrete(0));
auto& df = *bayesNet.atDiscrete(0);
EXPECT(df.equals(expected));
}
/* ****************************************************************************/
// Test choosing an assignment of conditionals
TEST(HybridBayesNet, Choose) {
Switching s(4);
Ordering ordering;
for (auto&& kvp : s.linearizationPoint) {
ordering += kvp.key;
}
HybridBayesNet::shared_ptr hybridBayesNet;
HybridGaussianFactorGraph::shared_ptr remainingFactorGraph;
std::tie(hybridBayesNet, remainingFactorGraph) =
s.linearizedFactorGraph.eliminatePartialSequential(ordering);
DiscreteValues assignment;
assignment[M(1)] = 1;
assignment[M(2)] = 1;
assignment[M(3)] = 0;
GaussianBayesNet gbn = hybridBayesNet->choose(assignment);
EXPECT_LONGS_EQUAL(4, gbn.size());
EXPECT(assert_equal(*(*boost::dynamic_pointer_cast<GaussianMixture>(
hybridBayesNet->atGaussian(0)))(assignment),
*gbn.at(0)));
EXPECT(assert_equal(*(*boost::dynamic_pointer_cast<GaussianMixture>(
hybridBayesNet->atGaussian(1)))(assignment),
*gbn.at(1)));
EXPECT(assert_equal(*(*boost::dynamic_pointer_cast<GaussianMixture>(
hybridBayesNet->atGaussian(2)))(assignment),
*gbn.at(2)));
EXPECT(assert_equal(*(*boost::dynamic_pointer_cast<GaussianMixture>(
hybridBayesNet->atGaussian(3)))(assignment),
*gbn.at(3)));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

2
gtsam/inference/FactorGraph.h
View File

@ -361,7 +361,7 @@ class FactorGraph {
* less than the original, factors at the end will be removed. If the new
* size is larger than the original, null factors will be appended.
*/
void resize(size_t size) { factors_.resize(size); }
virtual void resize(size_t size) { factors_.resize(size); }
/** delete factor without re-arranging indexes by inserting a nullptr pointer
*/

11
gtsam/inference/Ordering.cpp
View File

@ -283,6 +283,17 @@ void Ordering::print(const std::string& str,
cout.flush();
}
/* ************************************************************************* */
Ordering::This& Ordering::operator+=(KeyVector& keys) {
this->insert(this->end(), keys.begin(), keys.end());
return *this;
}
/* ************************************************************************* */
bool Ordering::contains(const Key& key) const {
return std::find(this->begin(), this->end(), key) != this->end();
}
/* ************************************************************************* */
bool Ordering::equals(const Ordering& other, double tol) const {
return (*this) == other;

18
gtsam/inference/Ordering.h
View File

@ -70,7 +70,23 @@ public:
boost::assign_detail::call_push_back<This>(*this))(key);
}
/// Invert (not reverse) the ordering - returns a map from key to order position
/**
* @brief Append new keys to the ordering as `ordering += keys`.
*
* @param key
* @return The ordering variable with appended keys.
*/
This& operator+=(KeyVector& keys);
/// Check if key exists in ordering.
bool contains(const Key& key) const;
/**
* @brief Invert (not reverse) the ordering - returns a map from key to order
* position.
*
* @return FastMap<Key, size_t>
*/
FastMap<Key, size_t> invert() const;
/// @name Fill-reducing Orderings @{

26
gtsam/inference/tests/testOrdering.cpp
View File

@ -199,6 +199,32 @@ TEST(Ordering, csr_format_3) {
EXPECT(adjExpected == adjAcutal);
}
/* ************************************************************************* */
TEST(Ordering, AppendVector) {
using symbol_shorthand::X;
Ordering actual;
KeyVector keys = {X(0), X(1), X(2)};
actual += keys;
Ordering expected;
expected += X(0);
expected += X(1);
expected += X(2);
EXPECT(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST(Ordering, Contains) {
using symbol_shorthand::X;
Ordering ordering;
ordering += X(0);
ordering += X(1);
ordering += X(2);
EXPECT(ordering.contains(X(1)));
EXPECT(!ordering.contains(X(4)));
}
/* ************************************************************************* */
#ifdef GTSAM_SUPPORT_NESTED_DISSECTION
TEST(Ordering, csr_format_4) {

2
gtsam/linear/GaussianBayesNet.h
View File

@ -45,7 +45,7 @@ namespace gtsam {
/// @name Standard Constructors
/// @{
/** Construct empty factor graph */
/** Construct empty bayes net */
GaussianBayesNet() {}
/** Construct from iterator over conditionals */

4
gtsam/linear/GaussianConditional.cpp
View File

@ -15,10 +15,10 @@
* @author Christian Potthast, Frank Dellaert
*/
#include <gtsam/linear/linearExceptions.h>
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/Sampler.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/linearExceptions.h>
#include <boost/format.hpp>
#ifdef __GNUC__

1
gtsam/linear/GaussianConditional.h
View File

@ -24,6 +24,7 @@
#include <gtsam/global_includes.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/inference/Conditional.h>
#include <gtsam/inference/Conditional-inst.h>
#include <gtsam/linear/VectorValues.h>
#include <random> // for std::mt19937_64

29
gtsam/sfm/TranslationRecovery.cpp
View File

@ -119,21 +119,24 @@ void TranslationRecovery::addPrior(
graph->emplace_shared<PriorFactor<Point3>>(edge->key1(), Point3(0, 0, 0),
priorNoiseModel);
// Add between factors for optional relative translations.
for (auto edge : betweenTranslations) {
graph->emplace_shared<BetweenFactor<Point3>>(
edge.key1(), edge.key2(), edge.measured(), edge.noiseModel());
}
// Add a scale prior only if no other between factors were added.
if (betweenTranslations.empty()) {
graph->emplace_shared<PriorFactor<Point3>>(
edge->key2(), scale * edge->measured().point3(), edge->noiseModel());
return;
}
// Add between factors for optional relative translations.
for (auto prior_edge : betweenTranslations) {
graph->emplace_shared<BetweenFactor<Point3>>(
prior_edge.key1(), prior_edge.key2(), prior_edge.measured(),
prior_edge.noiseModel());
}
}
Values TranslationRecovery::initializeRandomly(
const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations,
const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
std::mt19937 *rng, const Values &initialValues) const {
uniform_real_distribution<double> randomVal(-1, 1);
// Create a lambda expression that checks whether value exists and randomly
@ -155,14 +158,20 @@ Values TranslationRecovery::initializeRandomly(
insert(edge.key1());
insert(edge.key2());
}
// There may be nodes in betweenTranslations that do not have a measurement.
for (auto edge : betweenTranslations) {
insert(edge.key1());
insert(edge.key2());
}
return initial;
}
Values TranslationRecovery::initializeRandomly(
const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations,
const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
const Values &initialValues) const {
return initializeRandomly(relativeTranslations, &kRandomNumberGenerator,
initialValues);
return initializeRandomly(relativeTranslations, betweenTranslations,
&kRandomNumberGenerator, initialValues);
}
Values TranslationRecovery::run(
@ -187,8 +196,8 @@ Values TranslationRecovery::run(
&graph);
// Uses initial values from params if provided.
Values initial =
initializeRandomly(nonzeroRelativeTranslations, initialValues);
Values initial = initializeRandomly(
nonzeroRelativeTranslations, nonzeroBetweenTranslations, initialValues);
// If there are no valid edges, but zero-distance edges exist, initialize one
// of the nodes in a connected component of zero-distance edges.

14
gtsam/sfm/TranslationRecovery.h
View File

@ -112,12 +112,15 @@ class TranslationRecovery {
*
* @param relativeTranslations unit translation directions between
* translations to be estimated
* @param betweenTranslations relative translations (with scale) between 2
* points in world coordinate frame known a priori.
* @param rng random number generator
* @param intialValues (optional) initial values from a prior
* @return Values
*/
Values initializeRandomly(
const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations,
const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
std::mt19937 *rng, const Values &initialValues = Values()) const;
/**
@ -125,11 +128,14 @@ class TranslationRecovery {
*
* @param relativeTranslations unit translation directions between
* translations to be estimated
* @param betweenTranslations relative translations (with scale) between 2
* points in world coordinate frame known a priori.
* @param initialValues (optional) initial values from a prior
* @return Values
*/
Values initializeRandomly(
const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations,
const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
const Values &initialValues = Values()) const;
/**
@ -137,11 +143,15 @@ class TranslationRecovery {
*
* @param relativeTranslations the relative translations, in world coordinate
* frames, vector of BinaryMeasurements of Unit3, where each key of a
* measurement is a point in 3D.
* measurement is a point in 3D. If a relative translation magnitude is zero,
* it is treated as a hard same-point constraint (the result of all nodes
* connected by a zero-magnitude edge will be the same).
* @param scale scale for first relative translation which fixes gauge.
* The scale is only used if betweenTranslations is empty.
* @param betweenTranslations relative translations (with scale) between 2
* points in world coordinate frame known a priori.
* points in world coordinate frame known a priori. Unlike
* relativeTranslations, zero-magnitude betweenTranslations are not treated as
* hard constraints.
* @param initialValues intial values for optimization. Initializes randomly
* if not provided.
* @return Values

5
python/gtsam/examples/Pose3ISAM2Example.py
View File

@ -34,7 +34,10 @@ def report_on_progress(graph: gtsam.NonlinearFactorGraph, current_estimate: gtsa
# Plot the newly updated iSAM2 inference.
fig = plt.figure(0)
axes = fig.gca(projection='3d')
if not fig.axes:
axes = fig.add_subplot(projection='3d')
else:
axes = fig.axes[0]
plt.cla()
i = 1

5
python/gtsam/examples/VisualISAM2Example.py
View File

@ -33,7 +33,10 @@ def visual_ISAM2_plot(result):
fignum = 0
fig = plt.figure(fignum)
axes = fig.gca(projection='3d')
if not fig.axes:
axes = fig.add_subplot(projection='3d')
else:
axes = fig.axes[0]
plt.cla()
# Plot points

19
python/gtsam/utils/plot.py
View File

@ -333,7 +333,10 @@ def plot_point3(
"""
fig = plt.figure(fignum)
axes = fig.gca(projection='3d')
if not fig.axes:
axes = fig.add_subplot(projection='3d')
else:
axes = fig.axes[0]
plot_point3_on_axes(axes, point, linespec, P)
axes.set_xlabel(axis_labels[0])
@ -388,7 +391,7 @@ def plot_3d_points(fignum,
fig = plt.figure(fignum)
fig.suptitle(title)
fig.canvas.set_window_title(title.lower())
fig.canvas.manager.set_window_title(title.lower())
def plot_pose3_on_axes(axes, pose, axis_length=0.1, P=None, scale=1):
@ -490,7 +493,10 @@ def plot_trajectory(
axis_labels (iterable[string]): List of axis labels to set.
"""
fig = plt.figure(fignum)
axes = fig.gca(projection='3d')
if not fig.axes:
axes = fig.add_subplot(projection='3d')
else:
axes = fig.axes[0]
axes.set_xlabel(axis_labels[0])
axes.set_ylabel(axis_labels[1])
@ -522,7 +528,7 @@ def plot_trajectory(
plot_pose3_on_axes(axes, pose, P=covariance, axis_length=scale)
fig.suptitle(title)
fig.canvas.set_window_title(title.lower())
fig.canvas.manager.set_window_title(title.lower())
def plot_incremental_trajectory(fignum: int,
@ -545,7 +551,10 @@ def plot_incremental_trajectory(fignum: int,
Used to create animation effect.
"""
fig = plt.figure(fignum)
axes = fig.gca(projection='3d')
if not fig.axes:
axes = fig.add_subplot(projection='3d')
else:
axes = fig.axes[0]
poses = gtsam.utilities.allPose3s(values)
keys = gtsam.KeyVector(poses.keys())

30
tests/testTranslationRecovery.cpp
View File

@ -323,6 +323,36 @@ TEST(TranslationRecovery, ThreePosesWithOneHardConstraint) {
EXPECT(assert_equal(Point3(2, 0, 0), result.at<Point3>(1), 1e-4));
EXPECT(assert_equal(Point3(1, -1, 0), result.at<Point3>(3), 1e-4));
}
TEST(TranslationRecovery, NodeWithBetweenFactorAndNoMeasurements) {
// Checks that valid results are obtained when a between translation edge is
// provided with a node that does not have any other relative translations.
Values poses;
poses.insert<Pose3>(0, Pose3(Rot3(), Point3(0, 0, 0)));
poses.insert<Pose3>(1, Pose3(Rot3(), Point3(2, 0, 0)));
poses.insert<Pose3>(3, Pose3(Rot3(), Point3(1, -1, 0)));
poses.insert<Pose3>(4, Pose3(Rot3(), Point3(1, 2, 1)));
auto relativeTranslations = TranslationRecovery::SimulateMeasurements(
poses, {{0, 1}, {0, 3}, {1, 3}});
std::vector<BinaryMeasurement<Point3>> betweenTranslations;
betweenTranslations.emplace_back(0, 1, Point3(2, 0, 0),
noiseModel::Constrained::All(3, 1e2));
// Node 4 only has this between translation prior, no relative translations.
betweenTranslations.emplace_back(0, 4, Point3(1, 2, 1));
TranslationRecovery algorithm;
auto result =
algorithm.run(relativeTranslations, /*scale=*/0.0, betweenTranslations);
// Check result
EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-4));
EXPECT(assert_equal(Point3(2, 0, 0), result.at<Point3>(1), 1e-4));
EXPECT(assert_equal(Point3(1, -1, 0), result.at<Point3>(3), 1e-4));
EXPECT(assert_equal(Point3(1, 2, 1), result.at<Point3>(4), 1e-4));
}
/* ************************************************************************* */
int main() {
TestResult tr;