all tests pass!!!
Varun Agrawal
parent
ee124c33c3
commit
b39c2316e6
|
|
@ -476,9 +476,9 @@ TEST(HybridFactorGraph, Full_Elimination) {
|
|||
->equals(*discreteBayesNet.at(1)));
|
||||
}
|
||||
|
||||
/*
|
||||
****************************************************************************/
|
||||
// Test printing
|
||||
/****************************************************************************
|
||||
* Test printing
|
||||
*/
|
||||
TEST(HybridFactorGraph, Printing) {
|
||||
Switching self(3);
|
||||
|
||||
|
|
@ -663,80 +663,81 @@ factor 2: Hybrid P( x3 | m1 m2)
|
|||
EXPECT(assert_print_equal(expected_hybridBayesNet, *hybridBayesNet));
|
||||
}
|
||||
|
||||
// /* *************************************************************************
|
||||
// */
|
||||
// // Simple PlanarSLAM example test with 2 poses and 2 landmarks (each pose
|
||||
// // connects to 1 landmark) to expose issue with default decision tree
|
||||
// creation
|
||||
// // in hybrid elimination. The hybrid factor is between the poses X0 and X1.
|
||||
// The
|
||||
// // issue arises if we eliminate a landmark variable first since it is not
|
||||
// // connected to a DCFactor.
|
||||
// TEST(HybridFactorGraph, DefaultDecisionTree) {
|
||||
// HybridNonlinearFactorGraph fg;
|
||||
/****************************************************************************
|
||||
* Simple PlanarSLAM example test with 2 poses and 2 landmarks (each pose
|
||||
* connects to 1 landmark) to expose issue with default decision tree creation
|
||||
* in hybrid elimination. The hybrid factor is between the poses X0 and X1.
|
||||
* The issue arises if we eliminate a landmark variable first since it is not
|
||||
* connected to a HybridFactor.
|
||||
*/
|
||||
TEST(HybridFactorGraph, DefaultDecisionTree) {
|
||||
HybridNonlinearFactorGraph fg;
|
||||
|
||||
// // Add a prior on pose x1 at the origin. A prior factor consists of a mean
|
||||
// and
|
||||
// // a noise model (covariance matrix)
|
||||
// Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin
|
||||
// auto priorNoise = noiseModel::Diagonal::Sigmas(
|
||||
// Vector3(0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta
|
||||
// fg.emplace_nonlinear<PriorFactor<Pose2>>(X(0), prior, priorNoise);
|
||||
// Add a prior on pose x1 at the origin.
|
||||
// A prior factor consists of a mean and a noise model (covariance matrix)
|
||||
Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin
|
||||
auto priorNoise = noiseModel::Diagonal::Sigmas(
|
||||
Vector3(0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta
|
||||
fg.emplace_nonlinear<PriorFactor<Pose2>>(X(0), prior, priorNoise);
|
||||
|
||||
// using PlanarMotionModel = BetweenFactor<Pose2>;
|
||||
using PlanarMotionModel = BetweenFactor<Pose2>;
|
||||
|
||||
// // Add odometry factor
|
||||
// Pose2 odometry(2.0, 0.0, 0.0);
|
||||
// KeyVector contKeys = {X(0), X(1)};
|
||||
// auto noise_model = noiseModel::Isotropic::Sigma(3, 1.0);
|
||||
// auto still = boost::make_shared<PlanarMotionModel>(X(0), X(1), Pose2(0, 0,
|
||||
// 0),
|
||||
// noise_model),
|
||||
// moving = boost::make_shared<PlanarMotionModel>(X(0), X(1), odometry,
|
||||
// noise_model);
|
||||
// std::vector<PlanarMotionModel::shared_ptr> components = {still, moving};
|
||||
// auto dcFactor = boost::make_shared<DCMixtureFactor<PlanarMotionModel>>(
|
||||
// contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components);
|
||||
// fg.push_back(dcFactor);
|
||||
// Add odometry factor
|
||||
Pose2 odometry(2.0, 0.0, 0.0);
|
||||
KeyVector contKeys = {X(0), X(1)};
|
||||
auto noise_model = noiseModel::Isotropic::Sigma(3, 1.0);
|
||||
auto still = boost::make_shared<PlanarMotionModel>(X(0), X(1), Pose2(0, 0,
|
||||
0),
|
||||
noise_model),
|
||||
moving = boost::make_shared<PlanarMotionModel>(X(0), X(1), odometry,
|
||||
noise_model);
|
||||
std::vector<PlanarMotionModel::shared_ptr> motion_models = {still, moving};
|
||||
//TODO(Varun) Make a templated constructor for MixtureFactor which does this?
|
||||
std::vector<NonlinearFactor::shared_ptr> components;
|
||||
for (auto&& f : motion_models) {
|
||||
components.push_back(boost::dynamic_pointer_cast<NonlinearFactor>(f));
|
||||
}
|
||||
fg.emplace_hybrid<MixtureFactor>(
|
||||
contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components);
|
||||
|
||||
// // Add Range-Bearing measurements to from X0 to L0 and X1 to L1.
|
||||
// // create a noise model for the landmark measurements
|
||||
// auto measurementNoise = noiseModel::Diagonal::Sigmas(
|
||||
// Vector2(0.1, 0.2)); // 0.1 rad std on bearing, 20cm on range
|
||||
// // create the measurement values - indices are (pose id, landmark id)
|
||||
// Rot2 bearing11 = Rot2::fromDegrees(45), bearing22 = Rot2::fromDegrees(90);
|
||||
// double range11 = std::sqrt(4.0 + 4.0), range22 = 2.0;
|
||||
// Add Range-Bearing measurements to from X0 to L0 and X1 to L1.
|
||||
// create a noise model for the landmark measurements
|
||||
auto measurementNoise = noiseModel::Diagonal::Sigmas(
|
||||
Vector2(0.1, 0.2)); // 0.1 rad std on bearing, 20cm on range
|
||||
// create the measurement values - indices are (pose id, landmark id)
|
||||
Rot2 bearing11 = Rot2::fromDegrees(45), bearing22 = Rot2::fromDegrees(90);
|
||||
double range11 = std::sqrt(4.0 + 4.0), range22 = 2.0;
|
||||
|
||||
// // Add Bearing-Range factors
|
||||
// fg.emplace_nonlinear<BearingRangeFactor<Pose2, Point2>>(
|
||||
// X(0), L(0), bearing11, range11, measurementNoise);
|
||||
// fg.emplace_nonlinear<BearingRangeFactor<Pose2, Point2>>(
|
||||
// X(1), L(1), bearing22, range22, measurementNoise);
|
||||
// Add Bearing-Range factors
|
||||
fg.emplace_nonlinear<BearingRangeFactor<Pose2, Point2>>(
|
||||
X(0), L(0), bearing11, range11, measurementNoise);
|
||||
fg.emplace_nonlinear<BearingRangeFactor<Pose2, Point2>>(
|
||||
X(1), L(1), bearing22, range22, measurementNoise);
|
||||
|
||||
// // Create (deliberately inaccurate) initial estimate
|
||||
// Values initialEstimate;
|
||||
// initialEstimate.insert(X(0), Pose2(0.5, 0.0, 0.2));
|
||||
// initialEstimate.insert(X(1), Pose2(2.3, 0.1, -0.2));
|
||||
// initialEstimate.insert(L(0), Point2(1.8, 2.1));
|
||||
// initialEstimate.insert(L(1), Point2(4.1, 1.8));
|
||||
// Create (deliberately inaccurate) initial estimate
|
||||
Values initialEstimate;
|
||||
initialEstimate.insert(X(0), Pose2(0.5, 0.0, 0.2));
|
||||
initialEstimate.insert(X(1), Pose2(2.3, 0.1, -0.2));
|
||||
initialEstimate.insert(L(0), Point2(1.8, 2.1));
|
||||
initialEstimate.insert(L(1), Point2(4.1, 1.8));
|
||||
|
||||
// // We want to eliminate variables not connected to DCFactors first.
|
||||
// Ordering ordering;
|
||||
// ordering += L(0);
|
||||
// ordering += L(1);
|
||||
// ordering += X(0);
|
||||
// ordering += X(1);
|
||||
// We want to eliminate variables not connected to DCFactors first.
|
||||
Ordering ordering;
|
||||
ordering += L(0);
|
||||
ordering += L(1);
|
||||
ordering += X(0);
|
||||
ordering += X(1);
|
||||
|
||||
// HybridGaussianFactorGraph linearized = fg.linearize(initialEstimate);
|
||||
// gtsam::HybridBayesNet::shared_ptr hybridBayesNet;
|
||||
// gtsam::HybridGaussianFactorGraph::shared_ptr remainingFactorGraph;
|
||||
HybridGaussianFactorGraph linearized = fg.linearize(initialEstimate);
|
||||
gtsam::HybridBayesNet::shared_ptr hybridBayesNet;
|
||||
gtsam::HybridGaussianFactorGraph::shared_ptr remainingFactorGraph;
|
||||
|
||||
// // This should NOT fail
|
||||
// std::tie(hybridBayesNet, remainingFactorGraph) =
|
||||
// linearized.eliminatePartialSequential(ordering);
|
||||
// EXPECT_LONGS_EQUAL(4, hybridBayesNet->size());
|
||||
// EXPECT_LONGS_EQUAL(1, remainingFactorGraph->size());
|
||||
// }
|
||||
// This should NOT fail
|
||||
std::tie(hybridBayesNet, remainingFactorGraph) =
|
||||
linearized.eliminatePartialSequential(ordering);
|
||||
EXPECT_LONGS_EQUAL(4, hybridBayesNet->size());
|
||||
EXPECT_LONGS_EQUAL(1, remainingFactorGraph->size());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
|
|
|
|||
Loading…
Reference in New Issue