move direct FG motion model test to testHybridMotionModel.cpp
Varun Agrawal
parent
f5f878e6fa
commit
01829381da
|
|
@ -194,164 +194,6 @@ TEST(HybridGaussianFactor, Error) {
|
|||
4.0, hybridFactor.error({continuousValues, discreteValues}), 1e-9);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
namespace test_direct_factor_graph {
|
||||
/**
|
||||
* @brief Create a Factor Graph by directly specifying all
|
||||
* the factors instead of creating conditionals first.
|
||||
* This way we can directly provide the likelihoods and
|
||||
* then perform linearization.
|
||||
*
|
||||
* @param values Initial values to linearize around.
|
||||
* @param means The means of the HybridGaussianFactor components.
|
||||
* @param sigmas The covariances of the HybridGaussianFactor components.
|
||||
* @param m1 The discrete key.
|
||||
* @return HybridGaussianFactorGraph
|
||||
*/
|
||||
static HybridGaussianFactorGraph CreateFactorGraph(
|
||||
const gtsam::Values &values, const std::vector<double> &means,
|
||||
const std::vector<double> &sigmas, DiscreteKey &m1,
|
||||
double measurement_noise = 1e-3) {
|
||||
auto model0 = noiseModel::Isotropic::Sigma(1, sigmas[0]);
|
||||
auto model1 = noiseModel::Isotropic::Sigma(1, sigmas[1]);
|
||||
auto prior_noise = noiseModel::Isotropic::Sigma(1, measurement_noise);
|
||||
|
||||
auto f0 =
|
||||
std::make_shared<BetweenFactor<double>>(X(0), X(1), means[0], model0)
|
||||
->linearize(values);
|
||||
auto f1 =
|
||||
std::make_shared<BetweenFactor<double>>(X(0), X(1), means[1], model1)
|
||||
->linearize(values);
|
||||
|
||||
// Create HybridGaussianFactor
|
||||
// We take negative since we want
|
||||
// the underlying scalar to be log(\sqrt(|2πΣ|))
|
||||
std::vector<GaussianFactorValuePair> factors{{f0, model0->negLogConstant()},
|
||||
{f1, model1->negLogConstant()}};
|
||||
HybridGaussianFactor motionFactor(m1, factors);
|
||||
|
||||
HybridGaussianFactorGraph hfg;
|
||||
hfg.push_back(motionFactor);
|
||||
|
||||
hfg.push_back(PriorFactor<double>(X(0), values.at<double>(X(0)), prior_noise)
|
||||
.linearize(values));
|
||||
|
||||
return hfg;
|
||||
}
|
||||
} // namespace test_direct_factor_graph
|
||||
|
||||
/* ************************************************************************* */
|
||||
/**
|
||||
* @brief Test components with differing means but the same covariances.
|
||||
* The factor graph is
|
||||
* *-X1-*-X2
|
||||
* |
|
||||
* M1
|
||||
*/
|
||||
TEST(HybridGaussianFactor, DifferentMeansFG) {
|
||||
using namespace test_direct_factor_graph;
|
||||
|
||||
DiscreteKey m1(M(1), 2);
|
||||
|
||||
Values values;
|
||||
double x1 = 0.0, x2 = 1.75;
|
||||
values.insert(X(0), x1);
|
||||
values.insert(X(1), x2);
|
||||
|
||||
std::vector<double> means = {0.0, 2.0}, sigmas = {1e-0, 1e-0};
|
||||
|
||||
HybridGaussianFactorGraph hfg = CreateFactorGraph(values, means, sigmas, m1);
|
||||
|
||||
{
|
||||
auto bn = hfg.eliminateSequential();
|
||||
HybridValues actual = bn->optimize();
|
||||
|
||||
HybridValues expected(
|
||||
VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(-1.75)}},
|
||||
DiscreteValues{{M(1), 0}});
|
||||
|
||||
EXPECT(assert_equal(expected, actual));
|
||||
|
||||
DiscreteValues dv0{{M(1), 0}};
|
||||
VectorValues cont0 = bn->optimize(dv0);
|
||||
double error0 = bn->error(HybridValues(cont0, dv0));
|
||||
// regression
|
||||
EXPECT_DOUBLES_EQUAL(0.69314718056, error0, 1e-9);
|
||||
|
||||
DiscreteValues dv1{{M(1), 1}};
|
||||
VectorValues cont1 = bn->optimize(dv1);
|
||||
double error1 = bn->error(HybridValues(cont1, dv1));
|
||||
EXPECT_DOUBLES_EQUAL(error0, error1, 1e-9);
|
||||
}
|
||||
|
||||
{
|
||||
auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-3);
|
||||
hfg.push_back(
|
||||
PriorFactor<double>(X(1), means[1], prior_noise).linearize(values));
|
||||
|
||||
auto bn = hfg.eliminateSequential();
|
||||
HybridValues actual = bn->optimize();
|
||||
|
||||
HybridValues expected(
|
||||
VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(0.25)}},
|
||||
DiscreteValues{{M(1), 1}});
|
||||
|
||||
EXPECT(assert_equal(expected, actual));
|
||||
|
||||
{
|
||||
DiscreteValues dv{{M(1), 0}};
|
||||
VectorValues cont = bn->optimize(dv);
|
||||
double error = bn->error(HybridValues(cont, dv));
|
||||
// regression
|
||||
EXPECT_DOUBLES_EQUAL(2.12692448787, error, 1e-9);
|
||||
}
|
||||
{
|
||||
DiscreteValues dv{{M(1), 1}};
|
||||
VectorValues cont = bn->optimize(dv);
|
||||
double error = bn->error(HybridValues(cont, dv));
|
||||
// regression
|
||||
EXPECT_DOUBLES_EQUAL(0.126928487854, error, 1e-9);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
/**
|
||||
* @brief Test components with differing covariances but the same means.
|
||||
* The factor graph is
|
||||
* *-X1-*-X2
|
||||
* |
|
||||
* M1
|
||||
*/
|
||||
TEST(HybridGaussianFactor, DifferentCovariancesFG) {
|
||||
using namespace test_direct_factor_graph;
|
||||
|
||||
DiscreteKey m1(M(1), 2);
|
||||
|
||||
Values values;
|
||||
double x1 = 1.0, x2 = 1.0;
|
||||
values.insert(X(0), x1);
|
||||
values.insert(X(1), x2);
|
||||
|
||||
std::vector<double> means = {0.0, 0.0}, sigmas = {1e2, 1e-2};
|
||||
|
||||
// Create FG with HybridGaussianFactor and prior on X1
|
||||
HybridGaussianFactorGraph fg = CreateFactorGraph(values, means, sigmas, m1);
|
||||
auto hbn = fg.eliminateSequential();
|
||||
|
||||
VectorValues cv;
|
||||
cv.insert(X(0), Vector1(0.0));
|
||||
cv.insert(X(1), Vector1(0.0));
|
||||
|
||||
DiscreteValues dv0{{M(1), 0}};
|
||||
DiscreteValues dv1{{M(1), 1}};
|
||||
|
||||
DiscreteConditional expected_m1(m1, "0.5/0.5");
|
||||
DiscreteConditional actual_m1 = *(hbn->at(2)->asDiscrete());
|
||||
|
||||
EXPECT(assert_equal(expected_m1, actual_m1));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
|
|
|
|||
|
|
@ -198,7 +198,7 @@ TEST(HybridGaussianFactorGraph, TwoStateModel2) {
|
|||
{VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(1.0)}},
|
||||
VectorValues{{X(0), Vector1(0.5)}, {X(1), Vector1(3.0)}}}) {
|
||||
vv.insert(given); // add measurements for HBN
|
||||
const auto& expectedDiscretePosterior = hbn.discretePosterior(vv);
|
||||
const auto &expectedDiscretePosterior = hbn.discretePosterior(vv);
|
||||
|
||||
// Equality of posteriors asserts that the factor graph is correct (same
|
||||
// ratios for all modes)
|
||||
|
|
@ -234,7 +234,7 @@ TEST(HybridGaussianFactorGraph, TwoStateModel2) {
|
|||
{VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(1.0)}},
|
||||
VectorValues{{X(0), Vector1(0.5)}, {X(1), Vector1(3.0)}}}) {
|
||||
vv.insert(given); // add measurements for HBN
|
||||
const auto& expectedDiscretePosterior = hbn.discretePosterior(vv);
|
||||
const auto &expectedDiscretePosterior = hbn.discretePosterior(vv);
|
||||
|
||||
// Equality of posteriors asserts that the factor graph is correct (same
|
||||
// ratios for all modes)
|
||||
|
|
@ -385,6 +385,164 @@ TEST(HybridGaussianFactorGraph, TwoStateModel4) {
|
|||
EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.002));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
namespace test_direct_factor_graph {
|
||||
/**
|
||||
* @brief Create a Factor Graph by directly specifying all
|
||||
* the factors instead of creating conditionals first.
|
||||
* This way we can directly provide the likelihoods and
|
||||
* then perform linearization.
|
||||
*
|
||||
* @param values Initial values to linearize around.
|
||||
* @param means The means of the HybridGaussianFactor components.
|
||||
* @param sigmas The covariances of the HybridGaussianFactor components.
|
||||
* @param m1 The discrete key.
|
||||
* @return HybridGaussianFactorGraph
|
||||
*/
|
||||
static HybridGaussianFactorGraph CreateFactorGraph(
|
||||
const gtsam::Values &values, const std::vector<double> &means,
|
||||
const std::vector<double> &sigmas, DiscreteKey &m1,
|
||||
double measurement_noise = 1e-3) {
|
||||
auto model0 = noiseModel::Isotropic::Sigma(1, sigmas[0]);
|
||||
auto model1 = noiseModel::Isotropic::Sigma(1, sigmas[1]);
|
||||
auto prior_noise = noiseModel::Isotropic::Sigma(1, measurement_noise);
|
||||
|
||||
auto f0 =
|
||||
std::make_shared<BetweenFactor<double>>(X(0), X(1), means[0], model0)
|
||||
->linearize(values);
|
||||
auto f1 =
|
||||
std::make_shared<BetweenFactor<double>>(X(0), X(1), means[1], model1)
|
||||
->linearize(values);
|
||||
|
||||
// Create HybridGaussianFactor
|
||||
// We take negative since we want
|
||||
// the underlying scalar to be log(\sqrt(|2πΣ|))
|
||||
std::vector<GaussianFactorValuePair> factors{{f0, model0->negLogConstant()},
|
||||
{f1, model1->negLogConstant()}};
|
||||
HybridGaussianFactor motionFactor(m1, factors);
|
||||
|
||||
HybridGaussianFactorGraph hfg;
|
||||
hfg.push_back(motionFactor);
|
||||
|
||||
hfg.push_back(PriorFactor<double>(X(0), values.at<double>(X(0)), prior_noise)
|
||||
.linearize(values));
|
||||
|
||||
return hfg;
|
||||
}
|
||||
} // namespace test_direct_factor_graph
|
||||
|
||||
/* ************************************************************************* */
|
||||
/**
|
||||
* @brief Test components with differing means but the same covariances.
|
||||
* The factor graph is
|
||||
* *-X1-*-X2
|
||||
* |
|
||||
* M1
|
||||
*/
|
||||
TEST(HybridGaussianFactorGraph, DifferentMeans) {
|
||||
using namespace test_direct_factor_graph;
|
||||
|
||||
DiscreteKey m1(M(1), 2);
|
||||
|
||||
Values values;
|
||||
double x1 = 0.0, x2 = 1.75;
|
||||
values.insert(X(0), x1);
|
||||
values.insert(X(1), x2);
|
||||
|
||||
std::vector<double> means = {0.0, 2.0}, sigmas = {1e-0, 1e-0};
|
||||
|
||||
HybridGaussianFactorGraph hfg = CreateFactorGraph(values, means, sigmas, m1);
|
||||
|
||||
{
|
||||
auto bn = hfg.eliminateSequential();
|
||||
HybridValues actual = bn->optimize();
|
||||
|
||||
HybridValues expected(
|
||||
VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(-1.75)}},
|
||||
DiscreteValues{{M(1), 0}});
|
||||
|
||||
EXPECT(assert_equal(expected, actual));
|
||||
|
||||
DiscreteValues dv0{{M(1), 0}};
|
||||
VectorValues cont0 = bn->optimize(dv0);
|
||||
double error0 = bn->error(HybridValues(cont0, dv0));
|
||||
// regression
|
||||
EXPECT_DOUBLES_EQUAL(0.69314718056, error0, 1e-9);
|
||||
|
||||
DiscreteValues dv1{{M(1), 1}};
|
||||
VectorValues cont1 = bn->optimize(dv1);
|
||||
double error1 = bn->error(HybridValues(cont1, dv1));
|
||||
EXPECT_DOUBLES_EQUAL(error0, error1, 1e-9);
|
||||
}
|
||||
|
||||
{
|
||||
auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-3);
|
||||
hfg.push_back(
|
||||
PriorFactor<double>(X(1), means[1], prior_noise).linearize(values));
|
||||
|
||||
auto bn = hfg.eliminateSequential();
|
||||
HybridValues actual = bn->optimize();
|
||||
|
||||
HybridValues expected(
|
||||
VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(0.25)}},
|
||||
DiscreteValues{{M(1), 1}});
|
||||
|
||||
EXPECT(assert_equal(expected, actual));
|
||||
|
||||
{
|
||||
DiscreteValues dv{{M(1), 0}};
|
||||
VectorValues cont = bn->optimize(dv);
|
||||
double error = bn->error(HybridValues(cont, dv));
|
||||
// regression
|
||||
EXPECT_DOUBLES_EQUAL(2.12692448787, error, 1e-9);
|
||||
}
|
||||
{
|
||||
DiscreteValues dv{{M(1), 1}};
|
||||
VectorValues cont = bn->optimize(dv);
|
||||
double error = bn->error(HybridValues(cont, dv));
|
||||
// regression
|
||||
EXPECT_DOUBLES_EQUAL(0.126928487854, error, 1e-9);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
/**
|
||||
* @brief Test components with differing covariances but the same means.
|
||||
* The factor graph is
|
||||
* *-X1-*-X2
|
||||
* |
|
||||
* M1
|
||||
*/
|
||||
TEST(HybridGaussianFactorGraph, DifferentCovariances) {
|
||||
using namespace test_direct_factor_graph;
|
||||
|
||||
DiscreteKey m1(M(1), 2);
|
||||
|
||||
Values values;
|
||||
double x1 = 1.0, x2 = 1.0;
|
||||
values.insert(X(0), x1);
|
||||
values.insert(X(1), x2);
|
||||
|
||||
std::vector<double> means = {0.0, 0.0}, sigmas = {1e2, 1e-2};
|
||||
|
||||
// Create FG with HybridGaussianFactor and prior on X1
|
||||
HybridGaussianFactorGraph fg = CreateFactorGraph(values, means, sigmas, m1);
|
||||
auto hbn = fg.eliminateSequential();
|
||||
|
||||
VectorValues cv;
|
||||
cv.insert(X(0), Vector1(0.0));
|
||||
cv.insert(X(1), Vector1(0.0));
|
||||
|
||||
DiscreteValues dv0{{M(1), 0}};
|
||||
DiscreteValues dv1{{M(1), 1}};
|
||||
|
||||
DiscreteConditional expected_m1(m1, "0.5/0.5");
|
||||
DiscreteConditional actual_m1 = *(hbn->at(2)->asDiscrete());
|
||||
|
||||
EXPECT(assert_equal(expected_m1, actual_m1));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
|
|
|
|||
Loading…
Reference in New Issue