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

all tests pass!!!

release/4.3a0
Varun Agrawal 2022-08-02 19:17:49 -04:00
parent ee124c33c3
commit b39c2316e6
1 changed files with 69 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

137
gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
View File

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