add demarkers
parent
7bfa011886
commit
d60c4aca5a
|
|
@ -23,12 +23,12 @@ virtual class HybridConditional {
|
||||||
bool equals(const gtsam::HybridConditional& other, double tol = 1e-9) const;
|
bool equals(const gtsam::HybridConditional& other, double tol = 1e-9) const;
|
||||||
size_t nrFrontals() const;
|
size_t nrFrontals() const;
|
||||||
size_t nrParents() const;
|
size_t nrParents() const;
|
||||||
Factor* getInner();
|
Factor* inner();
|
||||||
};
|
};
|
||||||
|
|
||||||
#include <gtsam/hybrid/GaussianMixtureFactor.h>
|
#include <gtsam/hybrid/GaussianMixtureFactor.h>
|
||||||
class GaussianMixtureFactor : gtsam::HybridFactor {
|
class GaussianMixtureFactor : gtsam::HybridFactor {
|
||||||
static GaussianMixtureFactor FromFactorList(
|
static GaussianMixtureFactor FromFactors(
|
||||||
const gtsam::KeyVector& continuousKeys,
|
const gtsam::KeyVector& continuousKeys,
|
||||||
const gtsam::DiscreteKeys& discreteKeys,
|
const gtsam::DiscreteKeys& discreteKeys,
|
||||||
const std::vector<gtsam::GaussianFactor::shared_ptr>& factorsList);
|
const std::vector<gtsam::GaussianFactor::shared_ptr>& factorsList);
|
||||||
|
|
@ -40,7 +40,7 @@ class GaussianMixtureFactor : gtsam::HybridFactor {
|
||||||
|
|
||||||
#include <gtsam/hybrid/GaussianMixtureConditional.h>
|
#include <gtsam/hybrid/GaussianMixtureConditional.h>
|
||||||
class GaussianMixtureConditional : gtsam::HybridFactor {
|
class GaussianMixtureConditional : gtsam::HybridFactor {
|
||||||
static GaussianMixtureConditional FromConditionalList(
|
static GaussianMixtureConditional FromConditionals(
|
||||||
const gtsam::KeyVector& continuousFrontals,
|
const gtsam::KeyVector& continuousFrontals,
|
||||||
const gtsam::KeyVector& continuousParents,
|
const gtsam::KeyVector& continuousParents,
|
||||||
const gtsam::DiscreteKeys& discreteParents,
|
const gtsam::DiscreteKeys& discreteParents,
|
||||||
|
|
|
||||||
|
|
@ -90,6 +90,7 @@ TEST(HybridFactorGraph, creation) {
|
||||||
GTSAM_PRINT(clgc);
|
GTSAM_PRINT(clgc);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
TEST(HybridFactorGraph, eliminate) {
|
TEST(HybridFactorGraph, eliminate) {
|
||||||
HybridFactorGraph hfg;
|
HybridFactorGraph hfg;
|
||||||
|
|
||||||
|
|
@ -100,6 +101,7 @@ TEST(HybridFactorGraph, eliminate) {
|
||||||
EXPECT_LONGS_EQUAL(result.first->size(), 1);
|
EXPECT_LONGS_EQUAL(result.first->size(), 1);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
TEST(HybridFactorGraph, eliminateMultifrontal) {
|
TEST(HybridFactorGraph, eliminateMultifrontal) {
|
||||||
HybridFactorGraph hfg;
|
HybridFactorGraph hfg;
|
||||||
|
|
||||||
|
|
@ -116,6 +118,7 @@ TEST(HybridFactorGraph, eliminateMultifrontal) {
|
||||||
EXPECT_LONGS_EQUAL(result.second->size(), 1);
|
EXPECT_LONGS_EQUAL(result.second->size(), 1);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
TEST(HybridFactorGraph, eliminateFullSequentialEqualChance) {
|
TEST(HybridFactorGraph, eliminateFullSequentialEqualChance) {
|
||||||
HybridFactorGraph hfg;
|
HybridFactorGraph hfg;
|
||||||
|
|
||||||
|
|
@ -139,9 +142,8 @@ TEST(HybridFactorGraph, eliminateFullSequentialEqualChance) {
|
||||||
EXPECT_DOUBLES_EQUAL(0.6225, dc->operator()(dv), 1e-3);
|
EXPECT_DOUBLES_EQUAL(0.6225, dc->operator()(dv), 1e-3);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
TEST(HybridFactorGraph, eliminateFullSequentialSimple) {
|
TEST(HybridFactorGraph, eliminateFullSequentialSimple) {
|
||||||
std::cout << ">>>>>>>>>>>>>>\n";
|
|
||||||
|
|
||||||
HybridFactorGraph hfg;
|
HybridFactorGraph hfg;
|
||||||
|
|
||||||
DiscreteKey c1(C(1), 2);
|
DiscreteKey c1(C(1), 2);
|
||||||
|
|
@ -168,9 +170,8 @@ TEST(HybridFactorGraph, eliminateFullSequentialSimple) {
|
||||||
GTSAM_PRINT(*result);
|
GTSAM_PRINT(*result);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
TEST(HybridFactorGraph, eliminateFullMultifrontalSimple) {
|
TEST(HybridFactorGraph, eliminateFullMultifrontalSimple) {
|
||||||
std::cout << ">>>>>>>>>>>>>>\n";
|
|
||||||
|
|
||||||
HybridFactorGraph hfg;
|
HybridFactorGraph hfg;
|
||||||
|
|
||||||
DiscreteKey c1(C(1), 2);
|
DiscreteKey c1(C(1), 2);
|
||||||
|
|
@ -202,9 +203,8 @@ TEST(HybridFactorGraph, eliminateFullMultifrontalSimple) {
|
||||||
GTSAM_PRINT(*result->marginalFactor(C(2)));
|
GTSAM_PRINT(*result->marginalFactor(C(2)));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
TEST(HybridFactorGraph, eliminateFullMultifrontalCLG) {
|
TEST(HybridFactorGraph, eliminateFullMultifrontalCLG) {
|
||||||
std::cout << ">>>>>>>>>>>>>>\n";
|
|
||||||
|
|
||||||
HybridFactorGraph hfg;
|
HybridFactorGraph hfg;
|
||||||
|
|
||||||
DiscreteKey c(C(1), 2);
|
DiscreteKey c(C(1), 2);
|
||||||
|
|
@ -235,13 +235,12 @@ TEST(HybridFactorGraph, eliminateFullMultifrontalCLG) {
|
||||||
*/
|
*/
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
/*
|
/*
|
||||||
* This test is about how to assemble the Bayes Tree roots after we do partial
|
* This test is about how to assemble the Bayes Tree roots after we do partial
|
||||||
* elimination
|
* elimination
|
||||||
*/
|
*/
|
||||||
TEST(HybridFactorGraph, eliminateFullMultifrontalTwoClique) {
|
TEST(HybridFactorGraph, eliminateFullMultifrontalTwoClique) {
|
||||||
std::cout << ">>>>>>>>>>>>>>\n";
|
|
||||||
|
|
||||||
HybridFactorGraph hfg;
|
HybridFactorGraph hfg;
|
||||||
|
|
||||||
hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
|
hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
|
||||||
|
|
@ -308,6 +307,7 @@ TEST(HybridFactorGraph, eliminateFullMultifrontalTwoClique) {
|
||||||
*/
|
*/
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
// TODO(fan): make a graph like Varun's paper one
|
// TODO(fan): make a graph like Varun's paper one
|
||||||
TEST(HybridFactorGraph, Switching) {
|
TEST(HybridFactorGraph, Switching) {
|
||||||
auto N = 12;
|
auto N = 12;
|
||||||
|
|
@ -415,6 +415,7 @@ TEST(HybridFactorGraph, Switching) {
|
||||||
hbt->marginalFactor(C(11))->print("HBT: ");
|
hbt->marginalFactor(C(11))->print("HBT: ");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
// TODO(fan): make a graph like Varun's paper one
|
// TODO(fan): make a graph like Varun's paper one
|
||||||
TEST(HybridFactorGraph, SwitchingISAM) {
|
TEST(HybridFactorGraph, SwitchingISAM) {
|
||||||
auto N = 11;
|
auto N = 11;
|
||||||
|
|
@ -510,6 +511,7 @@ TEST(HybridFactorGraph, SwitchingISAM) {
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
TEST(HybridFactorGraph, SwitchingTwoVar) {
|
TEST(HybridFactorGraph, SwitchingTwoVar) {
|
||||||
const int N = 7;
|
const int N = 7;
|
||||||
auto hfg = makeSwitchingChain(N, X);
|
auto hfg = makeSwitchingChain(N, X);
|
||||||
|
|
@ -599,10 +601,6 @@ TEST(HybridFactorGraph, SwitchingTwoVar) {
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
int main() {
|
int main() {
|
||||||
#ifdef HYBRID_DEBUG
|
|
||||||
::signal(SIGSEGV, &my_signal_handler);
|
|
||||||
::signal(SIGBUS, &my_signal_handler);
|
|
||||||
#endif
|
|
||||||
TestResult tr;
|
TestResult tr;
|
||||||
return TestRegistry::runAllTests(tr);
|
return TestRegistry::runAllTests(tr);
|
||||||
}
|
}
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue