From 59fe7e15ae3521b0f283f461fa538e8c1956a1b2 Mon Sep 17 00:00:00 2001 From: Kurran Singh Date: Thu, 2 Sep 2021 17:20:49 -0400 Subject: [PATCH 1/5] outline of removing factors --- include/dcsam/DCSAM.h | 18 ++++++--- src/DCSAM.cpp | 18 ++++++--- tests/testDCSAM.cpp | 93 +++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 119 insertions(+), 10 deletions(-) diff --git a/include/dcsam/DCSAM.h b/include/dcsam/DCSAM.h index 57abbba..9e7886f 100644 --- a/include/dcsam/DCSAM.h +++ b/include/dcsam/DCSAM.h @@ -70,14 +70,20 @@ class DCSAM { * factors to add. * @param dcfg - a DCFactorGraph containing any joint discrete-continuous * factors to add. - * @param initialGuess - an initial guess for any new continuous keys that. - * appear in the updated factors (or if one wants to force override previously - * obtained continuous values). + * @param removeFactorIndices - indices of factors to remove from system + * @param initialGuessContinuous - an initial guess for any new continuous + * keys that appear in the updated factors (or if one wants to force override + * previously obtained continuous values). + * @param initialGuessDiscrete - an initial guess for any new discrete keys + * that appear in the updated factors (or if one wants to force override + * previously obtained discrete values). */ void update(const gtsam::NonlinearFactorGraph &graph, const gtsam::DiscreteFactorGraph &dfg, const DCFactorGraph &dcfg, const gtsam::Values &initialGuessContinuous = gtsam::Values(), - const DiscreteValues &initialGuessDiscrete = DiscreteValues()); + const DiscreteValues &initialGuessDiscrete = DiscreteValues(), + const gtsam::FactorIndices &removeFactorIndices = + gtsam::FactorIndices()); /** * A HybridFactorGraph is a container holding a NonlinearFactorGraph, a @@ -90,7 +96,9 @@ class DCSAM { */ void update(const HybridFactorGraph &hfg, const gtsam::Values &initialGuessContinuous = gtsam::Values(), - const DiscreteValues &initialGuessDiscrete = DiscreteValues()); + const DiscreteValues &initialGuessDiscrete = DiscreteValues(), + const gtsam::FactorIndices &removeFactorIndices = + gtsam::FactorIndices()); /** * Inline convenience function to allow "skipping" the initial guess for diff --git a/src/DCSAM.cpp b/src/DCSAM.cpp index a1eee7e..1b672df 100644 --- a/src/DCSAM.cpp +++ b/src/DCSAM.cpp @@ -30,8 +30,15 @@ void DCSAM::update(const gtsam::NonlinearFactorGraph &graph, const gtsam::DiscreteFactorGraph &dfg, const DCFactorGraph &dcfg, const gtsam::Values &initialGuessContinuous, - const DiscreteValues &initialGuessDiscrete) { - // First things first: combine currContinuous_ estimate with the new values + const DiscreteValues &initialGuessDiscrete, + const gtsam::FactorIndices &removeFactorIndices) { + + // First things first: get rid of factors that are to be removed so updates + // to follow take the removals into account + isam_.update(removeFactorIndices); + //dfg_.remov + + // Second things second: combine currContinuous_ estimate with the new values // from initialGuessContinuous to produce the full continuous variable state. for (const gtsam::Key k : initialGuessContinuous.keys()) { if (currContinuous_.exists(k)) @@ -90,9 +97,10 @@ void DCSAM::update(const gtsam::NonlinearFactorGraph &graph, void DCSAM::update(const HybridFactorGraph &hfg, const gtsam::Values &initialGuessContinuous, - const DiscreteValues &initialGuessDiscrete) { - update(hfg.nonlinearGraph(), hfg.discreteGraph(), hfg.dcGraph(), - initialGuessContinuous, initialGuessDiscrete); + const DiscreteValues &initialGuessDiscrete, + const gtsam::FactorIndices &removeFactorIndices) { + update(hfg.nonlinearGraph(), hfg.discreteGraph(), hfg.dcGraph(), + initialGuessContinuous, initialGuessDiscrete, removeFactorIndices); } void DCSAM::update() { diff --git a/tests/testDCSAM.cpp b/tests/testDCSAM.cpp index 625ce92..1a2376e 100644 --- a/tests/testDCSAM.cpp +++ b/tests/testDCSAM.cpp @@ -1344,6 +1344,99 @@ TEST(TestSuite, dcMaxMixture_semantic_slam) { EXPECT_EQ(mpeClassL1, 1); } +/** + * This is for testing the behavior of factor removal using isam.update() + * for continuous factors and DiscreteFactorGraph.remove() or .erase() + * for discrete factors. + */ +TEST(TestSuite, factor_removal) { + // Make an empty dc factor graph + DCFactorGraph dcfg; + + // We'll make a variable with 2 possible assignments + const size_t cardinality = 2; + gtsam::DiscreteKey dk(gtsam::Symbol('d', 1), cardinality); + + // Make a symbol for a single continuous variable and add to KeyVector + gtsam::Symbol x1 = gtsam::Symbol('x', 1); + gtsam::KeyVector keys; + keys.push_back(x1); + + // Make a factor for non-null hypothesis + const double loc = 0.0; + const double sigma1 = 1.0; + gtsam::noiseModel::Isotropic::shared_ptr prior_noise1 = + gtsam::noiseModel::Isotropic::Sigma(1, sigma1); + gtsam::PriorFactor f1(x1, loc, prior_noise1); + + // Make a factor for null hypothesis + const double sigmaNullHypo = 8.0; + gtsam::noiseModel::Isotropic::shared_ptr prior_noiseNullHypo = + gtsam::noiseModel::Isotropic::Sigma(1, sigmaNullHypo); + + gtsam::PriorFactor fNullHypo(x1, loc, prior_noiseNullHypo); + std::vector> factorComponents{f1, fNullHypo}; + + DCMixtureFactor> dcMixture(keys, dk, + factorComponents); + dcfg.push_back(dcMixture); + + gtsam::DiscreteKey dkTest = dcMixture.discreteKeys()[0]; + std::cout << "DK 1st: " << dkTest.first << std::endl; + std::cout << "DK 2nd: " << dkTest.second << std::endl; + + // Let's make an initial guess for the continuous values + gtsam::Values initialGuess; + double initVal = -2.5; + initialGuess.insert(x1, initVal); + + // We also need an initial guess for the discrete variables (this will only be + // used if it is needed by your factors), here it is ignored. + DiscreteValues initialGuessDiscrete; + initialGuessDiscrete[dk.first] = 0; + + // Let's make a discrete factor graph + gtsam::DiscreteFactorGraph dfg; + + // Pack DCMixture into a DCDiscreteFactor + for (auto& it : dcfg) { + DCDiscreteFactor dcDiscrete(it->discreteKeys()[0], it); + dfg.push_back(dcDiscrete); + } + + // Update continuous info + for (size_t j = 0; j < dfg.size(); j++) { + boost::shared_ptr dcDiscreteFactor = + boost::dynamic_pointer_cast(dfg[j]); + if (dcDiscreteFactor) { + dcDiscreteFactor->updateContinuous(initialGuess); + dcDiscreteFactor->updateDiscrete(initialGuessDiscrete); + } + } + + // Solve for discrete given continuous + gtsam::DiscreteFactor::sharedValues mostProbableEstimate = dfg.optimize(); + + // Get the most probable estimate + size_t mpeD = (*mostProbableEstimate).at(dk.first); + + // Get the marginals + gtsam::DiscreteMarginals newDiscreteMarginals(dfg); + gtsam::Vector newMargProbs = newDiscreteMarginals.marginalProbabilities(dk); + + // Ensure that the prediction is correct + EXPECT_EQ(mpeD, 1); + + // now let's try deleting things and see what happens! + dfg.remove(0); + EXPECT_EQ(dfg.size(), 0); + + + +} + + + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); From e65e6da467a3e38f1d3940581b40a25676029918 Mon Sep 17 00:00:00 2001 From: Kurran Singh Date: Fri, 3 Sep 2021 14:27:32 -0400 Subject: [PATCH 2/5] factor removal code and tests --- include/dcsam/DCSAM.h | 14 ++- src/DCSAM.cpp | 17 +-- tests/testDCSAM.cpp | 241 +++++++++++++++++++++++++++++++----------- 3 files changed, 198 insertions(+), 74 deletions(-) diff --git a/include/dcsam/DCSAM.h b/include/dcsam/DCSAM.h index 9e7886f..6681468 100644 --- a/include/dcsam/DCSAM.h +++ b/include/dcsam/DCSAM.h @@ -70,20 +70,24 @@ class DCSAM { * factors to add. * @param dcfg - a DCFactorGraph containing any joint discrete-continuous * factors to add. - * @param removeFactorIndices - indices of factors to remove from system * @param initialGuessContinuous - an initial guess for any new continuous * keys that appear in the updated factors (or if one wants to force override * previously obtained continuous values). * @param initialGuessDiscrete - an initial guess for any new discrete keys * that appear in the updated factors (or if one wants to force override * previously obtained discrete values). + * @param removeFactorIndices - indices of continuous factors to remove + * @param removeDiscreteFactorIndices - indices of discrete factors to remove + */ void update(const gtsam::NonlinearFactorGraph &graph, const gtsam::DiscreteFactorGraph &dfg, const DCFactorGraph &dcfg, const gtsam::Values &initialGuessContinuous = gtsam::Values(), const DiscreteValues &initialGuessDiscrete = DiscreteValues(), const gtsam::FactorIndices &removeFactorIndices = - gtsam::FactorIndices()); + gtsam::FactorIndices(), + const std::vector &removeDiscreteFactorIndices = + std::vector()); /** * A HybridFactorGraph is a container holding a NonlinearFactorGraph, a @@ -92,13 +96,15 @@ class DCSAM { * parameters: that is: * * update(hfg.nonlinearGraph(), hfg.discreteGraph(), hfg.dcGraph(), - * initialGuess); + * initialGuess, removeFactorIndices, removeDiscreteFactorIndices); */ void update(const HybridFactorGraph &hfg, const gtsam::Values &initialGuessContinuous = gtsam::Values(), const DiscreteValues &initialGuessDiscrete = DiscreteValues(), const gtsam::FactorIndices &removeFactorIndices = - gtsam::FactorIndices()); + gtsam::FactorIndices(), + const std::vector &removeDiscreteFactorIndices = + std::vector()); /** * Inline convenience function to allow "skipping" the initial guess for diff --git a/src/DCSAM.cpp b/src/DCSAM.cpp index 1b672df..45f51d0 100644 --- a/src/DCSAM.cpp +++ b/src/DCSAM.cpp @@ -31,14 +31,17 @@ void DCSAM::update(const gtsam::NonlinearFactorGraph &graph, const DCFactorGraph &dcfg, const gtsam::Values &initialGuessContinuous, const DiscreteValues &initialGuessDiscrete, - const gtsam::FactorIndices &removeFactorIndices) { + const gtsam::FactorIndices &removeFactorIndices, + const std::vector &removeDiscreteFactorIndices) { // First things first: get rid of factors that are to be removed so updates // to follow take the removals into account - isam_.update(removeFactorIndices); - //dfg_.remov + isam_.update(gtsam::NonlinearFactorGraph(), gtsam::Values(), removeFactorIndices); + for (auto& i : removeDiscreteFactorIndices) { + dfg_.remove(i); + } - // Second things second: combine currContinuous_ estimate with the new values + // Next: combine currContinuous_ estimate with the new values // from initialGuessContinuous to produce the full continuous variable state. for (const gtsam::Key k : initialGuessContinuous.keys()) { if (currContinuous_.exists(k)) @@ -98,9 +101,11 @@ void DCSAM::update(const gtsam::NonlinearFactorGraph &graph, void DCSAM::update(const HybridFactorGraph &hfg, const gtsam::Values &initialGuessContinuous, const DiscreteValues &initialGuessDiscrete, - const gtsam::FactorIndices &removeFactorIndices) { + const gtsam::FactorIndices &removeFactorIndices, + const std::vector &removeDiscreteFactorIndices) { update(hfg.nonlinearGraph(), hfg.discreteGraph(), hfg.dcGraph(), - initialGuessContinuous, initialGuessDiscrete, removeFactorIndices); + initialGuessContinuous, initialGuessDiscrete, + removeFactorIndices, removeDiscreteFactorIndices); } void DCSAM::update() { diff --git a/tests/testDCSAM.cpp b/tests/testDCSAM.cpp index 1a2376e..4e615b6 100644 --- a/tests/testDCSAM.cpp +++ b/tests/testDCSAM.cpp @@ -1345,94 +1345,207 @@ TEST(TestSuite, dcMaxMixture_semantic_slam) { } /** - * This is for testing the behavior of factor removal using isam.update() - * for continuous factors and DiscreteFactorGraph.remove() or .erase() - * for discrete factors. + * This is for testing the behavior of factor removal */ TEST(TestSuite, factor_removal) { - // Make an empty dc factor graph - DCFactorGraph dcfg; + // Make a factor graph + HybridFactorGraph hfg; - // We'll make a variable with 2 possible assignments - const size_t cardinality = 2; - gtsam::DiscreteKey dk(gtsam::Symbol('d', 1), cardinality); + // Values for initial guess + gtsam::Values initialGuess; + DiscreteValues initialGuessDiscrete; - // Make a symbol for a single continuous variable and add to KeyVector - gtsam::Symbol x1 = gtsam::Symbol('x', 1); - gtsam::KeyVector keys; - keys.push_back(x1); + gtsam::Symbol x0('x', 0); + gtsam::Symbol l1('l', 1); + gtsam::Symbol lc1('c', 1); + // Create a discrete key for landmark 1 class with cardinality 2. + gtsam::DiscreteKey lm1_class(lc1, 2); + gtsam::Pose2 pose0(0, 0, 0); + gtsam::Pose2 dx(1, 0, 0.78539816); + double prior_sigma = 0.1; + double meas_sigma = 1.0; + double circumradius = (std::sqrt(4 + 2 * std::sqrt(2))) / 2.0; + gtsam::Point2 landmark1(circumradius, circumradius); - // Make a factor for non-null hypothesis - const double loc = 0.0; - const double sigma1 = 1.0; - gtsam::noiseModel::Isotropic::shared_ptr prior_noise1 = - gtsam::noiseModel::Isotropic::Sigma(1, sigma1); - gtsam::PriorFactor f1(x1, loc, prior_noise1); + gtsam::noiseModel::Isotropic::shared_ptr prior_noise = + gtsam::noiseModel::Isotropic::Sigma(3, prior_sigma); + gtsam::noiseModel::Isotropic::shared_ptr prior_lm_noise = + gtsam::noiseModel::Isotropic::Sigma(2, prior_sigma); + gtsam::noiseModel::Isotropic::shared_ptr meas_noise = + gtsam::noiseModel::Isotropic::Sigma(3, meas_sigma); - // Make a factor for null hypothesis - const double sigmaNullHypo = 8.0; - gtsam::noiseModel::Isotropic::shared_ptr prior_noiseNullHypo = - gtsam::noiseModel::Isotropic::Sigma(1, sigmaNullHypo); + // 0.1 rad std on bearing, 10cm on range + gtsam::noiseModel::Isotropic::shared_ptr br_noise = + gtsam::noiseModel::Isotropic::Sigma(2, 0.1); - gtsam::PriorFactor fNullHypo(x1, loc, prior_noiseNullHypo); - std::vector> factorComponents{f1, fNullHypo}; + std::vector prior_lm1_class; + prior_lm1_class.push_back(0.9); + prior_lm1_class.push_back(0.1); - DCMixtureFactor> dcMixture(keys, dk, - factorComponents); - dcfg.push_back(dcMixture); + gtsam::PriorFactor p0(x0, pose0, prior_noise); + gtsam::PriorFactor pl1(l1, landmark1, prior_lm_noise); + DiscretePriorFactor plc1(lm1_class, prior_lm1_class); - gtsam::DiscreteKey dkTest = dcMixture.discreteKeys()[0]; - std::cout << "DK 1st: " << dkTest.first << std::endl; - std::cout << "DK 2nd: " << dkTest.second << std::endl; + initialGuess.insert(x0, pose0); + initialGuess.insert(l1, landmark1); + initialGuessDiscrete[lm1_class.first] = 0; - // Let's make an initial guess for the continuous values - gtsam::Values initialGuess; - double initVal = -2.5; - initialGuess.insert(x1, initVal); + hfg.push_nonlinear(p0); + hfg.push_nonlinear(pl1); + hfg.push_discrete(plc1); - // We also need an initial guess for the discrete variables (this will only be - // used if it is needed by your factors), here it is ignored. - DiscreteValues initialGuessDiscrete; - initialGuessDiscrete[dk.first] = 0; + // set up for landmark 2 + gtsam::Symbol l2('l', 2); + gtsam::Symbol lc2('c', 2); + // Create a discrete key for landmark 2 class with cardinality 2. + gtsam::DiscreteKey lm2_class(lc2, 2); + gtsam::Point2 landmark2(circumradius + .5, circumradius + 5); - // Let's make a discrete factor graph - gtsam::DiscreteFactorGraph dfg; + std::vector prior_lm2_class; + prior_lm2_class.push_back(0.1); + prior_lm2_class.push_back(0.9); - // Pack DCMixture into a DCDiscreteFactor - for (auto& it : dcfg) { - DCDiscreteFactor dcDiscrete(it->discreteKeys()[0], it); - dfg.push_back(dcDiscrete); - } + gtsam::PriorFactor pl2(l2, landmark2, prior_lm_noise); + DiscretePriorFactor plc2(lm2_class, prior_lm2_class); - // Update continuous info - for (size_t j = 0; j < dfg.size(); j++) { - boost::shared_ptr dcDiscreteFactor = - boost::dynamic_pointer_cast(dfg[j]); - if (dcDiscreteFactor) { - dcDiscreteFactor->updateContinuous(initialGuess); - dcDiscreteFactor->updateDiscrete(initialGuessDiscrete); + initialGuess.insert(l2, landmark2); + initialGuessDiscrete[lm2_class.first] = 1; + + hfg.push_nonlinear(pl2); + hfg.push_discrete(plc2); + + // Setup dcsam + DCSAM dcsam; + dcsam.update(hfg, initialGuess, initialGuessDiscrete); + + DCValues dcval_start = dcsam.calculateEstimate(); + std::cout << "Printing first values" << std::endl; + dcval_start.discrete.print(); + + hfg.clear(); + initialGuess.clear(); + initialGuessDiscrete.clear(); + + gtsam::Pose2 odom(pose0); + gtsam::Pose2 noise(0.01, 0.01, 0.01); + for (size_t i = 0; i < 7; i++) { + gtsam::Symbol xi('x', i); + gtsam::Symbol xj('x', i + 1); + + gtsam::Pose2 meas = dx * noise; + + gtsam::BetweenFactor bw(xi, xj, meas, meas_noise); + hfg.push_nonlinear(bw); + + // Add semantic bearing-range measurement to landmark in center + gtsam::Rot2 bearing1 = gtsam::Rot2::fromDegrees(67.5); + double range1 = circumradius; + + // For the first couple measurements, pick class=0, later pick class=1 + std::vector semantic_meas; + if (i < 2) { + semantic_meas.push_back(0.9); + semantic_meas.push_back(0.1); + } else { + semantic_meas.push_back(0.1); + semantic_meas.push_back(0.9); + } + + gtsam::DiscreteKeys dks({lm1_class, lm2_class}); + + // build mixture: dcmaxmixture should be picking the component for lm1 + SemanticBearingRangeFactor sbr1( + xi, l1, lm1_class, semantic_meas, bearing1, range1, br_noise); + SemanticBearingRangeFactor sbr2( + xi, l2, lm2_class, semantic_meas, bearing1, range1, br_noise); + DCMaxMixtureFactor> dcmmf( + {xi, l1, l2}, dks, {sbr1, sbr2}, {.5, .5}, false); + + hfg.push_dc(dcmmf); + odom = odom * meas; + initialGuess.insert(xj, odom); + dcsam.update(hfg, initialGuess); + DCValues dcvals = dcsam.calculateEstimate(); + + size_t mpeClassL1 = dcvals.discrete.at(lc1); + + // Plot poses and landmarks +#ifdef ENABLE_PLOTTING + std::vector xs, ys; + for (size_t j = 0; j < i + 2; j++) { + xs.push_back( + dcvals.continuous.at(gtsam::Symbol('x', j)).x()); + ys.push_back( + dcvals.continuous.at(gtsam::Symbol('x', j)).y()); } + + std::vector lmxs, lmys; + lmxs.push_back( + dcvals.continuous.at(gtsam::Symbol('l', 1)).x()); + lmys.push_back( + dcvals.continuous.at(gtsam::Symbol('l', 1)).y()); + + string color = (mpeClassL1 == 0) ? "b" : "orange"; + + plt::plot(xs, ys); + plt::scatter(lmxs, lmys, {{"color", color}}); + plt::show(); +#endif + + hfg.clear(); + initialGuess.clear(); } - // Solve for discrete given continuous - gtsam::DiscreteFactor::sharedValues mostProbableEstimate = dfg.optimize(); + gtsam::Symbol x7('x', 7); + gtsam::BetweenFactor bw(x0, x7, dx * noise, meas_noise); - // Get the most probable estimate - size_t mpeD = (*mostProbableEstimate).at(dk.first); + hfg.push_nonlinear(bw); + dcsam.update(hfg, initialGuess); - // Get the marginals - gtsam::DiscreteMarginals newDiscreteMarginals(dfg); - gtsam::Vector newMargProbs = newDiscreteMarginals.marginalProbabilities(dk); + DCValues dcvals = dcsam.calculateEstimate(); - // Ensure that the prediction is correct - EXPECT_EQ(mpeD, 1); + size_t mpeClassL1 = dcvals.discrete.at(lc1); - // now let's try deleting things and see what happens! - dfg.remove(0); - EXPECT_EQ(dfg.size(), 0); + // Plot the poses and landmarks +#ifdef ENABLE_PLOTTING + std::vector xs, ys; + for (size_t i = 0; i < 8; i++) { + xs.push_back(dcvals.continuous.at(gtsam::Symbol('x', i)).x()); + ys.push_back(dcvals.continuous.at(gtsam::Symbol('x', i)).y()); + } + std::vector lmxs, lmys; + lmxs.push_back( + dcvals.continuous.at(gtsam::Symbol('l', 1)).x()); + lmys.push_back( + dcvals.continuous.at(gtsam::Symbol('l', 1)).y()); + + string color = (mpeClassL1 == 0) ? "b" : "orange"; + + plt::plot(xs, ys); + plt::scatter(lmxs, lmys, {{"color", color}}); + plt::show(); +#endif + + EXPECT_EQ(mpeClassL1, 1); + // So far, this same as earlier example. Now let's start removing factors + // and see what happens. + EXPECT_EQ(dcsam.getDiscreteFactorGraph().size(), 18); + EXPECT_EQ(dcsam.getNonlinearFactorGraph().size(), 18); + + hfg.clear(); + initialGuess.clear(); + initialGuessDiscrete.clear(); + std::vector discreteRemovals{17}; + gtsam::FactorIndices removals{17}; + + dcsam.update(hfg, initialGuess, initialGuessDiscrete, removals, discreteRemovals); + + EXPECT_EQ(dcsam.getDiscreteFactorGraph().at(17), nullptr); + EXPECT_EQ(dcsam.getNonlinearFactorGraph().at(17), nullptr); } From f1e340bfbcc10272bbcdd20222c1f95d2128b08f Mon Sep 17 00:00:00 2001 From: Kurran Singh Date: Tue, 7 Sep 2021 11:05:24 -0400 Subject: [PATCH 3/5] use factor indices instead of size_t for discrete removal --- include/dcsam/DCSAM.h | 8 ++++---- src/DCSAM.cpp | 4 ++-- tests/testDCSAM.cpp | 4 +++- 3 files changed, 9 insertions(+), 7 deletions(-) diff --git a/include/dcsam/DCSAM.h b/include/dcsam/DCSAM.h index 6681468..d992c40 100644 --- a/include/dcsam/DCSAM.h +++ b/include/dcsam/DCSAM.h @@ -86,8 +86,8 @@ class DCSAM { const DiscreteValues &initialGuessDiscrete = DiscreteValues(), const gtsam::FactorIndices &removeFactorIndices = gtsam::FactorIndices(), - const std::vector &removeDiscreteFactorIndices = - std::vector()); + const gtsam::FactorIndices &removeDiscreteFactorIndices = + gtsam::FactorIndices()); /** * A HybridFactorGraph is a container holding a NonlinearFactorGraph, a @@ -103,8 +103,8 @@ class DCSAM { const DiscreteValues &initialGuessDiscrete = DiscreteValues(), const gtsam::FactorIndices &removeFactorIndices = gtsam::FactorIndices(), - const std::vector &removeDiscreteFactorIndices = - std::vector()); + const gtsam::FactorIndices &removeDiscreteFactorIndices = + gtsam::FactorIndices()); /** * Inline convenience function to allow "skipping" the initial guess for diff --git a/src/DCSAM.cpp b/src/DCSAM.cpp index 45f51d0..6620e7d 100644 --- a/src/DCSAM.cpp +++ b/src/DCSAM.cpp @@ -32,7 +32,7 @@ void DCSAM::update(const gtsam::NonlinearFactorGraph &graph, const gtsam::Values &initialGuessContinuous, const DiscreteValues &initialGuessDiscrete, const gtsam::FactorIndices &removeFactorIndices, - const std::vector &removeDiscreteFactorIndices) { + const gtsam::FactorIndices &removeDiscreteFactorIndices) { // First things first: get rid of factors that are to be removed so updates // to follow take the removals into account @@ -102,7 +102,7 @@ void DCSAM::update(const HybridFactorGraph &hfg, const gtsam::Values &initialGuessContinuous, const DiscreteValues &initialGuessDiscrete, const gtsam::FactorIndices &removeFactorIndices, - const std::vector &removeDiscreteFactorIndices) { + const gtsam::FactorIndices &removeDiscreteFactorIndices) { update(hfg.nonlinearGraph(), hfg.discreteGraph(), hfg.dcGraph(), initialGuessContinuous, initialGuessDiscrete, removeFactorIndices, removeDiscreteFactorIndices); diff --git a/tests/testDCSAM.cpp b/tests/testDCSAM.cpp index 4e615b6..b5d5af8 100644 --- a/tests/testDCSAM.cpp +++ b/tests/testDCSAM.cpp @@ -1539,13 +1539,15 @@ TEST(TestSuite, factor_removal) { hfg.clear(); initialGuess.clear(); initialGuessDiscrete.clear(); - std::vector discreteRemovals{17}; + gtsam::FactorIndices discreteRemovals{17}; gtsam::FactorIndices removals{17}; dcsam.update(hfg, initialGuess, initialGuessDiscrete, removals, discreteRemovals); EXPECT_EQ(dcsam.getDiscreteFactorGraph().at(17), nullptr); EXPECT_EQ(dcsam.getNonlinearFactorGraph().at(17), nullptr); + + } From 7e72e316f72af33969b7a9f2604899e764ceac95 Mon Sep 17 00:00:00 2001 From: Kurran Singh Date: Tue, 7 Sep 2021 15:19:12 -0400 Subject: [PATCH 4/5] better test case --- include/dcsam/DCSAM.h | 3 +- src/DCSAM.cpp | 7 ++- tests/testDCSAM.cpp | 128 ++++++++++-------------------------------- 3 files changed, 36 insertions(+), 102 deletions(-) diff --git a/include/dcsam/DCSAM.h b/include/dcsam/DCSAM.h index d992c40..3714d09 100644 --- a/include/dcsam/DCSAM.h +++ b/include/dcsam/DCSAM.h @@ -174,7 +174,8 @@ class DCSAM { */ void updateContinuousInfo(const DiscreteValues &discreteVals, const gtsam::NonlinearFactorGraph &newFactors, - const gtsam::Values &initialGuess); + const gtsam::Values &initialGuess, + const gtsam::FactorIndices &removeFactorIndices = gtsam::FactorIndices()); /** * Solve for discrete variables given continuous variables. Internally, calls diff --git a/src/DCSAM.cpp b/src/DCSAM.cpp index 6620e7d..de48d04 100644 --- a/src/DCSAM.cpp +++ b/src/DCSAM.cpp @@ -92,7 +92,8 @@ void DCSAM::update(const gtsam::NonlinearFactorGraph &graph, // Only the initialGuess needs to be provided for the continuous solver (not // the entire continuous state). - updateContinuousInfo(currDiscrete_, combined, initialGuessContinuous); + updateContinuousInfo(currDiscrete_, combined, initialGuessContinuous, + removeFactorIndices); currContinuous_ = isam_.calculateEstimate(); // Update discrete info from last solve and updateDiscrete(discreteCombined, currContinuous_, currDiscrete_); @@ -149,7 +150,8 @@ void DCSAM::updateContinuous() { // https://github.com/borglab/gtsam/pull/25/files#diff-277639578c861563a471b12776f86ad0b6317f61103adaae455e0cbe05899747R58 void DCSAM::updateContinuousInfo(const DiscreteValues &discreteVals, const gtsam::NonlinearFactorGraph &newFactors, - const gtsam::Values &initialGuess) { + const gtsam::Values &initialGuess, + const gtsam::FactorIndices &removeFactorIndices) { // ISAM2UpdateParams updateParams; // gtsam::FastMap newAffectedKeys; // for (size_t j = 0; j < dcContinuousFactors.size(); j++) { @@ -176,6 +178,7 @@ void DCSAM::updateContinuousInfo(const DiscreteValues &discreteVals, } } updateParams.newAffectedKeys = std::move(newAffectedKeys); + updateParams.removeFactorIndices = removeFactorIndices; // NOTE: I am not yet 100% sure this is the right way to handle this update. isam_.update(newFactors, initialGuess, updateParams); } diff --git a/tests/testDCSAM.cpp b/tests/testDCSAM.cpp index b5d5af8..460ab24 100644 --- a/tests/testDCSAM.cpp +++ b/tests/testDCSAM.cpp @@ -1388,32 +1388,12 @@ TEST(TestSuite, factor_removal) { initialGuess.insert(x0, pose0); initialGuess.insert(l1, landmark1); - initialGuessDiscrete[lm1_class.first] = 0; + initialGuessDiscrete[lm1_class.first] = 1; hfg.push_nonlinear(p0); hfg.push_nonlinear(pl1); hfg.push_discrete(plc1); - // set up for landmark 2 - gtsam::Symbol l2('l', 2); - gtsam::Symbol lc2('c', 2); - // Create a discrete key for landmark 2 class with cardinality 2. - gtsam::DiscreteKey lm2_class(lc2, 2); - gtsam::Point2 landmark2(circumradius + .5, circumradius + 5); - - std::vector prior_lm2_class; - prior_lm2_class.push_back(0.1); - prior_lm2_class.push_back(0.9); - - gtsam::PriorFactor pl2(l2, landmark2, prior_lm_noise); - DiscretePriorFactor plc2(lm2_class, prior_lm2_class); - - initialGuess.insert(l2, landmark2); - initialGuessDiscrete[lm2_class.first] = 1; - - hfg.push_nonlinear(pl2); - hfg.push_discrete(plc2); - // Setup dcsam DCSAM dcsam; dcsam.update(hfg, initialGuess, initialGuessDiscrete); @@ -1428,7 +1408,7 @@ TEST(TestSuite, factor_removal) { gtsam::Pose2 odom(pose0); gtsam::Pose2 noise(0.01, 0.01, 0.01); - for (size_t i = 0; i < 7; i++) { + for (size_t i = 0; i < 3; i++) { gtsam::Symbol xi('x', i); gtsam::Symbol xj('x', i + 1); @@ -1441,9 +1421,9 @@ TEST(TestSuite, factor_removal) { gtsam::Rot2 bearing1 = gtsam::Rot2::fromDegrees(67.5); double range1 = circumradius; - // For the first couple measurements, pick class=0, later pick class=1 + // For the first measurement, pick class=0, later pick class=1 std::vector semantic_meas; - if (i < 2) { + if (i < 1) { semantic_meas.push_back(0.9); semantic_meas.push_back(0.1); } else { @@ -1451,106 +1431,56 @@ TEST(TestSuite, factor_removal) { semantic_meas.push_back(0.9); } - gtsam::DiscreteKeys dks({lm1_class, lm2_class}); + gtsam::DiscreteKeys dks({lm1_class}); - // build mixture: dcmaxmixture should be picking the component for lm1 SemanticBearingRangeFactor sbr1( xi, l1, lm1_class, semantic_meas, bearing1, range1, br_noise); - SemanticBearingRangeFactor sbr2( - xi, l2, lm2_class, semantic_meas, bearing1, range1, br_noise); - DCMaxMixtureFactor> dcmmf( - {xi, l1, l2}, dks, {sbr1, sbr2}, {.5, .5}, false); - hfg.push_dc(dcmmf); + hfg.push_dc(sbr1); odom = odom * meas; initialGuess.insert(xj, odom); dcsam.update(hfg, initialGuess); DCValues dcvals = dcsam.calculateEstimate(); - size_t mpeClassL1 = dcvals.discrete.at(lc1); - - // Plot poses and landmarks -#ifdef ENABLE_PLOTTING - std::vector xs, ys; - for (size_t j = 0; j < i + 2; j++) { - xs.push_back( - dcvals.continuous.at(gtsam::Symbol('x', j)).x()); - ys.push_back( - dcvals.continuous.at(gtsam::Symbol('x', j)).y()); - } - - std::vector lmxs, lmys; - lmxs.push_back( - dcvals.continuous.at(gtsam::Symbol('l', 1)).x()); - lmys.push_back( - dcvals.continuous.at(gtsam::Symbol('l', 1)).y()); - - string color = (mpeClassL1 == 0) ? "b" : "orange"; - - plt::plot(xs, ys); - plt::scatter(lmxs, lmys, {{"color", color}}); - plt::show(); -#endif - hfg.clear(); initialGuess.clear(); } - - gtsam::Symbol x7('x', 7); - gtsam::BetweenFactor bw(x0, x7, dx * noise, meas_noise); - - hfg.push_nonlinear(bw); - dcsam.update(hfg, initialGuess); - + DCValues dcvals = dcsam.calculateEstimate(); - size_t mpeClassL1 = dcvals.discrete.at(lc1); - - // Plot the poses and landmarks -#ifdef ENABLE_PLOTTING - std::vector xs, ys; - for (size_t i = 0; i < 8; i++) { - xs.push_back(dcvals.continuous.at(gtsam::Symbol('x', i)).x()); - ys.push_back(dcvals.continuous.at(gtsam::Symbol('x', i)).y()); - } - - std::vector lmxs, lmys; - lmxs.push_back( - dcvals.continuous.at(gtsam::Symbol('l', 1)).x()); - lmys.push_back( - dcvals.continuous.at(gtsam::Symbol('l', 1)).y()); - - string color = (mpeClassL1 == 0) ? "b" : "orange"; - - plt::plot(xs, ys); - plt::scatter(lmxs, lmys, {{"color", color}}); - plt::show(); -#endif - EXPECT_EQ(mpeClassL1, 1); - - // So far, this same as earlier example. Now let's start removing factors - // and see what happens. - EXPECT_EQ(dcsam.getDiscreteFactorGraph().size(), 18); - EXPECT_EQ(dcsam.getNonlinearFactorGraph().size(), 18); + std::cout << "now let's remove factors! " << std::endl; + + // Now let's start removing factors and see what happens. + EXPECT_EQ(dcsam.getDiscreteFactorGraph().size(), 8); + EXPECT_EQ(dcsam.getNonlinearFactorGraph().size(), 8); hfg.clear(); initialGuess.clear(); initialGuessDiscrete.clear(); - gtsam::FactorIndices discreteRemovals{17}; - gtsam::FactorIndices removals{17}; + + // removing all discrete factors with high class 1 prob + gtsam::FactorIndices discreteRemovals{3,5,7}; + + // make sure continuous removal works as well + gtsam::FactorIndices removals{7}; + + // TODO(Kurran) why does removing continous factor 5 cause isam segfault? + // dcsam.getNonlinearFactorGraph().at(5)->print(); dcsam.update(hfg, initialGuess, initialGuessDiscrete, removals, discreteRemovals); - - EXPECT_EQ(dcsam.getDiscreteFactorGraph().at(17), nullptr); - EXPECT_EQ(dcsam.getNonlinearFactorGraph().at(17), nullptr); - -} + EXPECT_EQ(dcsam.getDiscreteFactorGraph().at(7), nullptr); + EXPECT_EQ(dcsam.getNonlinearFactorGraph().at(7), nullptr); + + dcvals = dcsam.calculateEstimate(); + + mpeClassL1 = dcvals.discrete.at(lc1); + EXPECT_EQ(mpeClassL1, 0); +} int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); From 385f26a2e5efa9d6aef2e3d0098240ddc472b01d Mon Sep 17 00:00:00 2001 From: Kurran Singh Date: Mon, 16 Oct 2023 13:53:27 -0400 Subject: [PATCH 5/5] bring up to date with newer gtsam version --- include/dcsam/DCDiscreteFactor.h | 11 + include/dcsam/DiscretePriorFactor.h | 11 + src/DCSAM.cpp | 4 +- tests/testDCSAM.cpp | 533 +++++++++++++++++++--------- 4 files changed, 395 insertions(+), 164 deletions(-) diff --git a/include/dcsam/DCDiscreteFactor.h b/include/dcsam/DCDiscreteFactor.h index ba25211..c8eaf75 100644 --- a/include/dcsam/DCDiscreteFactor.h +++ b/include/dcsam/DCDiscreteFactor.h @@ -141,6 +141,17 @@ class DCDiscreteFactor : public gtsam::DiscreteFactor { } return true; } + + std::string markdown(const gtsam::KeyFormatter& keyFormatter, + const Names& names) const override { + return toDecisionTreeFactor().markdown(keyFormatter, names); + } + + std::string html(const gtsam::KeyFormatter& keyFormatter, + const Names& names) const override { + return toDecisionTreeFactor().markdown(keyFormatter, names); + } + }; } // namespace dcsam diff --git a/include/dcsam/DiscretePriorFactor.h b/include/dcsam/DiscretePriorFactor.h index 88d7bdc..acef849 100644 --- a/include/dcsam/DiscretePriorFactor.h +++ b/include/dcsam/DiscretePriorFactor.h @@ -82,6 +82,17 @@ class DiscretePriorFactor : public gtsam::DiscreteFactor { size_t assignment = values.at(dk_.first); return probs_[assignment]; } + + std::string markdown(const gtsam::KeyFormatter& keyFormatter, + const Names& names) const override { + return toDecisionTreeFactor().markdown(keyFormatter, names); + } + + std::string html(const gtsam::KeyFormatter& keyFormatter, + const Names& names) const override { + return toDecisionTreeFactor().markdown(keyFormatter, names); + } + }; } // namespace dcsam diff --git a/src/DCSAM.cpp b/src/DCSAM.cpp index 7a440df..61ebd03 100644 --- a/src/DCSAM.cpp +++ b/src/DCSAM.cpp @@ -172,7 +172,7 @@ void DCSAM::updateContinuousInfo(const DiscreteValues &discreteVals, } DiscreteValues DCSAM::solveDiscrete() const { - DiscreteValues discreteVals = (*dfg_.optimize()); + DiscreteValues discreteVals = dfg_.optimize(); return discreteVals; } @@ -180,7 +180,7 @@ DCValues DCSAM::calculateEstimate() const { // NOTE: if we have these cached from solves, we could presumably just return // the cached values. gtsam::Values continuousVals = isam_.calculateEstimate(); - DiscreteValues discreteVals = (*dfg_.optimize()); + DiscreteValues discreteVals = dfg_.optimize(); DCValues dcValues(continuousVals, discreteVals); return dcValues; } diff --git a/tests/testDCSAM.cpp b/tests/testDCSAM.cpp index 91d590a..feb7c40 100644 --- a/tests/testDCSAM.cpp +++ b/tests/testDCSAM.cpp @@ -16,14 +16,14 @@ #include #include #include -#include +#include #include #include #include #ifdef ENABLE_PLOTTING -#include "matplotlibcpp.h" +#include #endif // Our custom DCSAM includes @@ -39,16 +39,11 @@ const double tol = 1e-7; -using namespace std; -using namespace dcsam; +/******************************************************************************/ #ifdef ENABLE_PLOTTING namespace plt = matplotlibcpp; -#endif - -/******************************************************************************/ -// TEMP just for plotting // thx @ github gist template std::vector linspace(T a, T b, size_t N) { @@ -59,6 +54,7 @@ std::vector linspace(T a, T b, size_t N) { for (x = xs.begin(), val = a; x != xs.end(); ++x, val += h) *x = val; return xs; } +#endif /* * Test simple DiscretePriorFactor. We construct a gtsam::DiscreteFactor factor @@ -80,14 +76,14 @@ TEST(TestSuite, discrete_prior_factor) { const std::vector probs{0.1, 0.9}; // Make a discrete prior factor and add it to the graph - DiscretePriorFactor dpf(dk, probs); + dcsam::DiscretePriorFactor dpf(dk, probs); dfg.push_back(dpf); // Solve - gtsam::DiscreteFactor::sharedValues mostProbableEstimate = dfg.optimize(); + dcsam::DiscreteValues mostProbableEstimate = dfg.optimize(); // Get the most probable estimate - size_t mpeD = (*mostProbableEstimate).at(dk.first); + const size_t mpeD = mostProbableEstimate.at(dk.first); // Get the marginals gtsam::DiscreteMarginals discreteMarginals(dfg); @@ -104,7 +100,7 @@ TEST(TestSuite, discrete_prior_factor) { } /* - * Test update-able SmartDiscretePriorFactor/ We construct a + * Test update-able SmartDiscretePriorFactor. We construct a * gtsam::DiscreteFactor factor graph with a single binary variable d1 and a * single custom SmartDiscretePriorFactor p(d1) specifying the distribution: * p(d1 = 0) = 0.1, p(d1 = 1) = 0.9. @@ -127,14 +123,14 @@ TEST(TestSuite, smart_discrete_prior_factor) { const std::vector probs{0.1, 0.9}; // Make a discrete prior factor and add it to the graph - SmartDiscretePriorFactor dpf(dk, probs); + dcsam::SmartDiscretePriorFactor dpf(dk, probs); dfg.push_back(dpf); // Solve - gtsam::DiscreteFactor::sharedValues mostProbableEstimate = dfg.optimize(); + dcsam::DiscreteValues mostProbableEstimate = dfg.optimize(); // Get the most probable estimate - size_t mpeD = (*mostProbableEstimate).at(dk.first); + size_t mpeD = mostProbableEstimate.at(dk.first); // EXPECT MPE = 1 (exactly the same as above test) EXPECT_EQ(mpeD, 1); @@ -145,15 +141,15 @@ TEST(TestSuite, smart_discrete_prior_factor) { // Update the factor const std::vector newProbs{0.9, 0.1}; - boost::shared_ptr smart = - boost::dynamic_pointer_cast(dfg[0]); + boost::shared_ptr smart = + boost::dynamic_pointer_cast(dfg[0]); if (smart) smart->updateProbs(newProbs); // Solve mostProbableEstimate = dfg.optimize(); // Get the most probable estimate - mpeD = (*mostProbableEstimate).at(dk.first); + mpeD = mostProbableEstimate.at(dk.first); // Get the new marginals gtsam::DiscreteMarginals newDiscreteMarginals(dfg); @@ -181,7 +177,7 @@ TEST(TestSuite, smart_discrete_prior_factor) { */ TEST(TestSuite, dcdiscrete_mixture) { // Make an empty discrete factor graph - DCFactorGraph dcfg; + dcsam::DCFactorGraph dcfg; // We'll make a variable with 2 possible assignments const size_t cardinality = 2; @@ -203,12 +199,12 @@ TEST(TestSuite, dcdiscrete_mixture) { const double sigmaNullHypo = 8.0; gtsam::noiseModel::Isotropic::shared_ptr prior_noiseNullHypo = gtsam::noiseModel::Isotropic::Sigma(1, sigmaNullHypo); - gtsam::PriorFactor fNullHypo(x1, loc, prior_noiseNullHypo); + std::vector> factorComponents{f1, fNullHypo}; - DCMixtureFactor> dcMixture(keys, dk, - factorComponents); + dcsam::DCMixtureFactor> dcMixture(keys, dk, + factorComponents); dcfg.push_back(dcMixture); gtsam::DiscreteKey dkTest = dcMixture.discreteKeys()[0]; @@ -222,7 +218,7 @@ TEST(TestSuite, dcdiscrete_mixture) { // We also need an initial guess for the discrete variables (this will only be // used if it is needed by your factors), here it is ignored. - DiscreteValues initialGuessDiscrete; + dcsam::DiscreteValues initialGuessDiscrete; initialGuessDiscrete[dk.first] = 0; // Let's make a discrete factor graph @@ -230,14 +226,14 @@ TEST(TestSuite, dcdiscrete_mixture) { // Pack DCMixture into a DCDiscreteFactor for (auto& it : dcfg) { - DCDiscreteFactor dcDiscrete(it->discreteKeys()[0], it); + dcsam::DCDiscreteFactor dcDiscrete(it->discreteKeys(), it); dfg.push_back(dcDiscrete); } // Update continuous info for (size_t j = 0; j < dfg.size(); j++) { - boost::shared_ptr dcDiscreteFactor = - boost::dynamic_pointer_cast(dfg[j]); + boost::shared_ptr dcDiscreteFactor = + boost::dynamic_pointer_cast(dfg[j]); if (dcDiscreteFactor) { dcDiscreteFactor->updateContinuous(initialGuess); dcDiscreteFactor->updateDiscrete(initialGuessDiscrete); @@ -245,10 +241,10 @@ TEST(TestSuite, dcdiscrete_mixture) { } // Solve for discrete given continuous - gtsam::DiscreteFactor::sharedValues mostProbableEstimate = dfg.optimize(); + dcsam::DiscreteValues mostProbableEstimate = dfg.optimize(); // Get the most probable estimate - size_t mpeD = (*mostProbableEstimate).at(dk.first); + const size_t mpeD = mostProbableEstimate.at(dk.first); // Get the marginals gtsam::DiscreteMarginals newDiscreteMarginals(dfg); @@ -275,7 +271,7 @@ TEST(TestSuite, dcdiscrete_mixture) { */ TEST(TestSuite, dccontinuous_mixture) { // Make an empty discrete factor graph - DCFactorGraph dcfg; + dcsam::DCFactorGraph dcfg; // We'll make a variable with 2 possible assignments const size_t cardinality = 2; @@ -301,19 +297,33 @@ TEST(TestSuite, dccontinuous_mixture) { gtsam::PriorFactor fNullHypo(x1, loc, prior_noiseNullHypo); std::vector> factorComponents{f1, fNullHypo}; - DCMixtureFactor> dcMixture(keys, dk, - factorComponents); + dcsam::DCMixtureFactor> dcMixture(keys, dk, + factorComponents); dcfg.push_back(dcMixture); gtsam::DiscreteKey dkTest = dcMixture.discreteKeys()[0]; std::cout << "DK 1st: " << dkTest.first << std::endl; std::cout << "DK 2nd: " << dkTest.second << std::endl; + // Test calculation of negative log probability + dcsam::DiscreteValues dv1, dvNH; + dv1[dk.first] = 0; + dvNH[dk.first] = 1; + gtsam::Values xvals; + xvals.insert(x1, 0.0); + const double negLogProb_1 = dcMixture.error(xvals, dv1); + const double negLogProb_NH = dcMixture.error(xvals, dvNH); + xvals.clear(); + + // As calculated in MATLAB using -log(normpdf(0,0,1)), -log(normpdf(0,0,8)) + EXPECT_NEAR(negLogProb_1, 0.9189, 1e-3); + EXPECT_NEAR(negLogProb_NH, 2.9984, 1e-3); + // Plot the cost functions for each hypothesis #ifdef ENABLE_PLOTTING // Query cost function std::vector xs = linspace(-5.0, 5.0, 50); - DiscreteValues dv1, dvNH; + dcsam::DiscreteValues dv1, dvNH; dv1[dk.first] = 0; dvNH[dk.first] = 1; std::vector errors1; @@ -326,8 +336,12 @@ TEST(TestSuite, dccontinuous_mixture) { xvals.clear(); } - plt::plot(xs, errors1); - plt::plot(xs, errorsNH); + plt::plot(xs, errors1, {{"label", "-log(p(x|d=0))"}}); + plt::plot(xs, errorsNH, {{"label", "-log(p(x|d=1))"}}); + plt::xlabel("Value of r.v. X"); + plt::ylabel("Negative Log-Likelihood"); + plt::grid(true); + plt::legend(); #endif // Let's make an initial guess for the continuous values @@ -341,13 +355,13 @@ TEST(TestSuite, dccontinuous_mixture) { std::vector initError1{dcMixture.error(initialGuess, dv1)}; std::vector initErrorNH{dcMixture.error(initialGuess, dvNH)}; - plt::scatter(initVec, initError1, {{"color", "r"}}); - plt::scatter(initVec, initErrorNH, {{"color", "r"}}); + plt::scatter(initVec, initError1, 15, {{"color", "r"}}); + plt::scatter(initVec, initErrorNH, 15, {{"color", "r"}}); #endif // We also need an initial guess for the discrete variables (this will only be // used if it is needed by your factors), here it is ignored. - DiscreteValues initialGuessDiscrete; + dcsam::DiscreteValues initialGuessDiscrete; initialGuessDiscrete[dk.first] = 0; // Let's make some factor graphs @@ -356,16 +370,16 @@ TEST(TestSuite, dccontinuous_mixture) { // Pack DCMixture into a DCContinuousFactor for (auto& it : dcfg) { - DCDiscreteFactor dcDiscrete(it); - DCContinuousFactor dcContinuous(it); + dcsam::DCDiscreteFactor dcDiscrete(it); + dcsam::DCContinuousFactor dcContinuous(it); dfg.push_back(dcDiscrete); graph.push_back(dcContinuous); } // Update continuous info inside DCDiscreteFactor for (size_t j = 0; j < dfg.size(); j++) { - boost::shared_ptr dcDiscreteFactor = - boost::dynamic_pointer_cast(dfg[j]); + boost::shared_ptr dcDiscreteFactor = + boost::dynamic_pointer_cast(dfg[j]); if (dcDiscreteFactor) { dcDiscreteFactor->updateContinuous(initialGuess); dcDiscreteFactor->updateDiscrete(initialGuessDiscrete); @@ -373,10 +387,10 @@ TEST(TestSuite, dccontinuous_mixture) { } // Solve for discrete given continuous - gtsam::DiscreteFactor::sharedValues mostProbableEstimate = dfg.optimize(); + dcsam::DiscreteValues mostProbableEstimate = dfg.optimize(); // Get the most probable estimate - size_t mpeD = (*mostProbableEstimate).at(dk.first); + size_t mpeD = mostProbableEstimate.at(dk.first); // From previous test we know this is == 1 EXPECT_EQ(mpeD, 1); @@ -387,10 +401,10 @@ TEST(TestSuite, dccontinuous_mixture) { // Update discrete info inside DCContinuousFactor for (size_t j = 0; j < graph.size(); j++) { - boost::shared_ptr dcContinuousFactor = - boost::dynamic_pointer_cast(graph[j]); + boost::shared_ptr dcContinuousFactor = + boost::dynamic_pointer_cast(graph[j]); if (dcContinuousFactor) - dcContinuousFactor->updateDiscrete((*mostProbableEstimate)); + dcContinuousFactor->updateDiscrete(mostProbableEstimate); } // Setup isam @@ -410,15 +424,15 @@ TEST(TestSuite, dccontinuous_mixture) { std::vector updatedError1{dcMixture.error(values, dv1)}; std::vector updatedErrorNH{dcMixture.error(values, dvNH)}; - plt::scatter(updatedVec, updatedError1, {{"color", "b"}}); - plt::scatter(updatedVec, updatedErrorNH, {{"color", "b"}}); + plt::scatter(updatedVec, updatedError1, 15, {{"color", "b"}}); + plt::scatter(updatedVec, updatedErrorNH, 15, {{"color", "b"}}); plt::show(); #endif // Now update the continuous info in the discrete solver for (size_t j = 0; j < dfg.size(); j++) { - boost::shared_ptr dcDiscreteFactor = - boost::dynamic_pointer_cast(dfg[j]); + boost::shared_ptr dcDiscreteFactor = + boost::dynamic_pointer_cast(dfg[j]); if (dcDiscreteFactor) dcDiscreteFactor->updateContinuous(values); // NOTE: we won't updateDiscrete explicitly here anymore, because we don't // need to. @@ -426,7 +440,7 @@ TEST(TestSuite, dccontinuous_mixture) { // Re-solve discrete to verify that output has switched mostProbableEstimate = dfg.optimize(); - mpeD = (*mostProbableEstimate).at(dk.first); + mpeD = mostProbableEstimate.at(dk.first); // Ensure that the prediction is correct EXPECT_EQ(mpeD, 0); @@ -464,11 +478,11 @@ TEST(TestSuite, simple_mixture_factor) { gtsam::PriorFactor fNullHypo(x1, loc, prior_noiseNullHypo); std::vector> factorComponents{f1, fNullHypo}; - DCMixtureFactor> dcMixture(keys, dk, - factorComponents); + dcsam::DCMixtureFactor> dcMixture(keys, dk, + factorComponents); // Make an empty hybrid factor graph - HybridFactorGraph hfg; + dcsam::HybridFactorGraph hfg; hfg.push_dc(dcMixture); @@ -479,7 +493,7 @@ TEST(TestSuite, simple_mixture_factor) { // Plot the cost functions for each hypothesis #ifdef ENABLE_PLOTTING std::vector xs = linspace(-5.0, 5.0, 50); - DiscreteValues dv1, dvNH; + dcsam::DiscreteValues dv1, dvNH; dv1[dk.first] = 0; dvNH[dk.first] = 1; std::vector errors1; @@ -492,8 +506,12 @@ TEST(TestSuite, simple_mixture_factor) { xvals.clear(); } - plt::plot(xs, errors1); - plt::plot(xs, errorsNH); + plt::plot(xs, errors1, {{"label", "-log(p(x|d=0))"}}); + plt::plot(xs, errorsNH, {{"label", "-log(p(x|d=1))"}}); + plt::xlabel("Value of r.v. X"); + plt::ylabel("Negative Log-Likelihood"); + plt::grid(true); + plt::legend(); #endif // Let's make an initial guess @@ -507,23 +525,23 @@ TEST(TestSuite, simple_mixture_factor) { std::vector initError1{dcMixture.error(initialGuess, dv1)}; std::vector initErrorNH{dcMixture.error(initialGuess, dvNH)}; - plt::scatter(initVec, initError1, {{"color", "r"}}); - plt::scatter(initVec, initErrorNH, {{"color", "r"}}); + plt::scatter(initVec, initError1, 15, {{"color", "r"}}); + plt::scatter(initVec, initErrorNH, 15, {{"color", "r"}}); #endif // We also need an initial guess for the discrete variables (this will only be // used if it is needed by your factors), here it is ignored. - DiscreteValues initialGuessDiscrete; + dcsam::DiscreteValues initialGuessDiscrete; initialGuessDiscrete[dk.first] = 0; // Let's make a solver - DCSAM dcsam; + dcsam::DCSAM dcsam; // Add the HybridFactorGraph to DCSAM dcsam.update(hfg, initialGuess, initialGuessDiscrete); // Solve - DCValues dcvals = dcsam.calculateEstimate(); + dcsam::DCValues dcvals = dcsam.calculateEstimate(); // Run another iteration dcsam.update(); @@ -537,17 +555,13 @@ TEST(TestSuite, simple_mixture_factor) { std::vector updatedError1{dcMixture.error(dcvals.continuous, dv1)}; std::vector updatedErrorNH{dcMixture.error(dcvals.continuous, dvNH)}; - plt::scatter(updatedVec, updatedError1, {{"color", "b"}}); - plt::scatter(updatedVec, updatedErrorNH, {{"color", "b"}}); - plt::show(); -#endif - -#ifdef ENABLE_PLOTTING + plt::scatter(updatedVec, updatedError1, 15, {{"color", "b"}}); + plt::scatter(updatedVec, updatedErrorNH, 15, {{"color", "b"}}); plt::show(); #endif // Ensure that the prediction is correct - size_t mpeD = dcvals.discrete.at(dk.first); + const size_t mpeD = dcvals.discrete.at(dk.first); EXPECT_EQ(mpeD, 0); } @@ -557,7 +571,7 @@ TEST(TestSuite, simple_mixture_factor) { */ TEST(TestSuite, simple_slam_batch) { // Make a hybrid factor graph - HybridFactorGraph graph; + dcsam::HybridFactorGraph graph; // Values for initial guess gtsam::Values initialGuess; @@ -565,8 +579,8 @@ TEST(TestSuite, simple_slam_batch) { gtsam::Symbol x0('x', 0); gtsam::Pose2 pose0(0, 0, 0); gtsam::Pose2 dx(1, 0, 0.78539816); - double prior_sigma = 0.1; - double meas_sigma = 1.0; + const double prior_sigma = 0.1; + const double meas_sigma = 1.0; gtsam::noiseModel::Isotropic::shared_ptr prior_noise = gtsam::noiseModel::Isotropic::Sigma(3, prior_sigma); @@ -579,7 +593,7 @@ TEST(TestSuite, simple_slam_batch) { graph.push_nonlinear(p0); // Setup dcsam - DCSAM dcsam; + dcsam::DCSAM dcsam; gtsam::Pose2 odom(pose0); gtsam::Pose2 noise(0.01, 0.01, 0.01); @@ -600,7 +614,7 @@ TEST(TestSuite, simple_slam_batch) { gtsam::BetweenFactor bw(x0, x7, dx * noise, meas_noise); dcsam.update(graph, initialGuess); - DCValues dcvals = dcsam.calculateEstimate(); + dcsam::DCValues dcvals = dcsam.calculateEstimate(); // Plot the robot positions #ifdef ENABLE_PLOTTING @@ -611,6 +625,7 @@ TEST(TestSuite, simple_slam_batch) { } plt::plot(xs, ys); + plt::grid(true); plt::show(); #endif @@ -623,7 +638,7 @@ TEST(TestSuite, simple_slam_batch) { */ TEST(TestSuite, simple_slam_incremental) { // Make a factor graph - HybridFactorGraph graph; + dcsam::HybridFactorGraph graph; // Values for initial guess gtsam::Values initialGuess; @@ -631,8 +646,8 @@ TEST(TestSuite, simple_slam_incremental) { gtsam::Symbol x0('x', 0); gtsam::Pose2 pose0(0, 0, 0); gtsam::Pose2 dx(1, 0, 0.78539816); - double prior_sigma = 0.1; - double meas_sigma = 1.0; + const double prior_sigma = 0.1; + const double meas_sigma = 1.0; gtsam::noiseModel::Isotropic::shared_ptr prior_noise = gtsam::noiseModel::Isotropic::Sigma(3, prior_sigma); @@ -645,7 +660,7 @@ TEST(TestSuite, simple_slam_incremental) { graph.push_nonlinear(p0); // Setup dcsam - DCSAM dcsam; + dcsam::DCSAM dcsam; dcsam.update(graph, initialGuess); graph.clear(); @@ -674,7 +689,7 @@ TEST(TestSuite, simple_slam_incremental) { gtsam::BetweenFactor bw(x0, x7, dx * noise, meas_noise); dcsam.update(graph, initialGuess); - DCValues dcvals = dcsam.calculateEstimate(); + dcsam::DCValues dcvals = dcsam.calculateEstimate(); // Plot the robot positions #ifdef ENABLE_PLOTTING @@ -684,6 +699,7 @@ TEST(TestSuite, simple_slam_incremental) { ys.push_back(dcvals.continuous.at(gtsam::Symbol('x', i)).y()); } plt::plot(xs, ys); + plt::grid(true); plt::show(); #endif @@ -697,10 +713,10 @@ TEST(TestSuite, simple_slam_incremental) { */ TEST(TestSuite, simple_discrete_dcsam) { // Create a DCSAM instance - DCSAM dcsam; + dcsam::DCSAM dcsam; // Make an empty hybrid factor graph - HybridFactorGraph hfg; + dcsam::HybridFactorGraph hfg; // We'll make a variable with 2 possible assignments const size_t cardinality = 2; @@ -708,21 +724,21 @@ TEST(TestSuite, simple_discrete_dcsam) { const std::vector probs{0.1, 0.9}; // Make a discrete prior factor and add it to the graph - DiscretePriorFactor dpf(dk, probs); + dcsam::DiscretePriorFactor dpf(dk, probs); hfg.push_discrete(dpf); // Initial guess for discrete values (only used in certain circumstances) - DiscreteValues initialGuessDiscrete; + dcsam::DiscreteValues initialGuessDiscrete; initialGuessDiscrete[dk.first] = 1; // Update DCSAM with the new factor dcsam.update(hfg, initialGuessDiscrete); // Solve - DCValues dcvals = dcsam.calculateEstimate(); + dcsam::DCValues dcvals = dcsam.calculateEstimate(); // Get the most probable estimate - size_t mpeD = dcvals.discrete.at(dk.first); + const size_t mpeD = dcvals.discrete.at(dk.first); // Ensure that the prediction is correct EXPECT_EQ(mpeD, 1); @@ -735,12 +751,12 @@ TEST(TestSuite, simple_discrete_dcsam) { */ TEST(TestSuite, simple_semantic_slam) { // Make a factor graph - HybridFactorGraph hfg; + dcsam::HybridFactorGraph hfg; // Values for initial guess gtsam::Values initialGuess; // Initial guess for discrete values (only used in certain circumstances) - DiscreteValues initialGuessDiscrete; + dcsam::DiscreteValues initialGuessDiscrete; gtsam::Symbol x0('x', 0); gtsam::Symbol l1('l', 1); @@ -749,9 +765,9 @@ TEST(TestSuite, simple_semantic_slam) { gtsam::DiscreteKey lm1_class(lc1, 2); gtsam::Pose2 pose0(0, 0, 0); gtsam::Pose2 dx(1, 0, 0.78539816); - double prior_sigma = 0.1; - double meas_sigma = 1.0; - double circumradius = (std::sqrt(4 + 2 * std::sqrt(2))) / 2.0; + const double prior_sigma = 0.1; + const double meas_sigma = 1.0; + const double circumradius = (std::sqrt(4 + 2 * std::sqrt(2))) / 2.0; gtsam::Point2 landmark1(circumradius, circumradius); gtsam::noiseModel::Isotropic::shared_ptr prior_noise = @@ -770,8 +786,8 @@ TEST(TestSuite, simple_semantic_slam) { prior_lm1_class.push_back(0.1); gtsam::PriorFactor p0(x0, pose0, prior_noise); - gtsam::PriorFactor pl1(l1, landmark1, prior_lm_noise); - DiscretePriorFactor plc1(lm1_class, prior_lm1_class); + // gtsam::PriorFactor pl1(l1, landmark1, prior_lm_noise); + dcsam::DiscretePriorFactor plc1(lm1_class, prior_lm1_class); initialGuess.insert(x0, pose0); initialGuess.insert(l1, landmark1); @@ -784,10 +800,10 @@ TEST(TestSuite, simple_semantic_slam) { hfg.push_discrete(plc1); // Setup dcsam - DCSAM dcsam; + dcsam::DCSAM dcsam; dcsam.update(hfg, initialGuess, initialGuessDiscrete); - DCValues dcval_start = dcsam.calculateEstimate(); + dcsam::DCValues dcval_start = dcsam.calculateEstimate(); hfg.clear(); initialGuess.clear(); @@ -806,7 +822,7 @@ TEST(TestSuite, simple_semantic_slam) { // Add bearing range measurement to landmark in center gtsam::Rot2 bearing1 = gtsam::Rot2::fromDegrees(67.5); - double range1 = circumradius; + const double range1 = circumradius; hfg.push_nonlinear(gtsam::BearingRangeFactor( xi, l1, bearing1, range1, br_noise)); @@ -820,16 +836,16 @@ TEST(TestSuite, simple_semantic_slam) { semantic_meas.push_back(0.1); semantic_meas.push_back(0.9); } - DiscretePriorFactor dpf(lm1_class, semantic_meas); + dcsam::DiscretePriorFactor dpf(lm1_class, semantic_meas); hfg.push_discrete(dpf); odom = odom * meas; initialGuess.insert(xj, odom); dcsam.update(hfg, initialGuess); - DCValues dcvals = dcsam.calculateEstimate(); + dcsam::DCValues dcvals = dcsam.calculateEstimate(); - size_t mpeClassL1 = dcvals.discrete.at(lc1); + const size_t mpeClassL1 = dcvals.discrete.at(lc1); // Plot poses and landmarks #ifdef ENABLE_PLOTTING @@ -847,10 +863,11 @@ TEST(TestSuite, simple_semantic_slam) { lmys.push_back( dcvals.continuous.at(gtsam::Symbol('l', 1)).y()); - string color = (mpeClassL1 == 0) ? "b" : "orange"; + std::string color = (mpeClassL1 == 0) ? "b" : "orange"; plt::plot(xs, ys); - plt::scatter(lmxs, lmys, {{"color", color}}); + plt::scatter(lmxs, lmys, 30, {{"color", color}}); + plt::grid(true); plt::show(); #endif @@ -864,9 +881,9 @@ TEST(TestSuite, simple_semantic_slam) { hfg.push_nonlinear(bw); dcsam.update(hfg, initialGuess); - DCValues dcvals = dcsam.calculateEstimate(); + dcsam::DCValues dcvals = dcsam.calculateEstimate(); - size_t mpeClassL1 = dcvals.discrete.at(lc1); + const size_t mpeClassL1 = dcvals.discrete.at(lc1); // Plot the poses and landmarks #ifdef ENABLE_PLOTTING @@ -882,10 +899,11 @@ TEST(TestSuite, simple_semantic_slam) { lmys.push_back( dcvals.continuous.at(gtsam::Symbol('l', 1)).y()); - string color = (mpeClassL1 == 0) ? "b" : "orange"; + std::string color = (mpeClassL1 == 0) ? "b" : "orange"; plt::plot(xs, ys); - plt::scatter(lmxs, lmys, {{"color", color}}); + plt::scatter(lmxs, lmys, 30, {{"color", color}}); + plt::grid(true); plt::show(); #endif @@ -902,12 +920,12 @@ TEST(TestSuite, simple_semantic_slam) { */ TEST(TestSuite, dcsam_initialization) { // Make a factor graph. - HybridFactorGraph hfg; + dcsam::HybridFactorGraph hfg; // Values for initial guess. gtsam::Values initialGuess; // Initial guess for discrete variables. - DiscreteValues initialGuessDiscrete; + dcsam::DiscreteValues initialGuessDiscrete; gtsam::Symbol x0('x', 0); gtsam::Symbol l1('l', 1); @@ -919,8 +937,8 @@ TEST(TestSuite, dcsam_initialization) { // Set up initial pose gtsam::Pose2 pose0(0, 0, 0); - double prior_sigma = 0.1; - double meas_sigma = 1.0; + const double prior_sigma = 0.1; + const double meas_sigma = 1.0; gtsam::Point2 landmark1(1.0, 1.0); /// Noise models for measurements and priors @@ -941,8 +959,8 @@ TEST(TestSuite, dcsam_initialization) { // Add prior factors to the graph and solve. gtsam::PriorFactor p0(x0, pose0, prior_noise); - gtsam::PriorFactor pl1(l1, landmark1, prior_lm_noise); - DiscretePriorFactor plc1(lm1_class, prior_lm1_class); + // gtsam::PriorFactor pl1(l1, landmark1, prior_lm_noise); + dcsam::DiscretePriorFactor plc1(lm1_class, prior_lm1_class); initialGuess.insert(x0, pose0); initialGuess.insert(l1, landmark1); @@ -953,10 +971,10 @@ TEST(TestSuite, dcsam_initialization) { hfg.push_discrete(plc1); // Setup dcsam - DCSAM dcsam; + dcsam::DCSAM dcsam; dcsam.update(hfg, initialGuess, initialGuessDiscrete); - DCValues dcval_start = dcsam.calculateEstimate(); + dcsam::DCValues dcval_start = dcsam.calculateEstimate(); std::cout << "Printing first values" << std::endl; dcval_start.discrete.print(); @@ -974,13 +992,13 @@ TEST(TestSuite, dcsam_initialization) { // landmark (x, y) = (1, 1) gtsam::Rot2 bearing = gtsam::Rot2::fromDegrees(45); double range = sqrt(2); - gtsam::BearingRangeFactor brfactor( - x0, l1, bearing, range, br_noise); + // gtsam::BearingRangeFactor brfactor( + // x0, l1, bearing, range, br_noise); // Set a semantic bearing-range factor up with BR measurement above and // semantic measurement equal to the landmark class prior. gtsam::KeyVector lm_keys{x0, l1}; - hfg.push_dc(SemanticBearingRangeFactor( + hfg.push_dc(dcsam::SemanticBearingRangeFactor( x0, l1, lm1_class, prior_lm1_class, bearing, range, br_noise)); // If DCSAM isn't initializing `x0` and `l1` properly for the new factor, this @@ -988,7 +1006,7 @@ TEST(TestSuite, dcsam_initialization) { dcsam.update(hfg); // Attempt a solve if we made it this far. - DCValues dcval_final = dcsam.calculateEstimate(); + dcsam::DCValues dcval_final = dcsam.calculateEstimate(); // If we made it here without an AssertionError, the test passed. EXPECT_EQ(true, true); @@ -1002,11 +1020,11 @@ TEST(TestSuite, dcsam_initialization) { */ TEST(TestSuite, bearing_range_semantic_slam) { // Make a factor graph - HybridFactorGraph hfg; + dcsam::HybridFactorGraph hfg; // Values for initial guess gtsam::Values initialGuess; - DiscreteValues initialGuessDiscrete; + dcsam::DiscreteValues initialGuessDiscrete; gtsam::Symbol x0('x', 0); gtsam::Symbol l1('l', 1); @@ -1015,9 +1033,9 @@ TEST(TestSuite, bearing_range_semantic_slam) { gtsam::DiscreteKey lm1_class(lc1, 2); gtsam::Pose2 pose0(0, 0, 0); gtsam::Pose2 dx(1, 0, 0.78539816); - double prior_sigma = 0.1; - double meas_sigma = 1.0; - double circumradius = (std::sqrt(4 + 2 * std::sqrt(2))) / 2.0; + const double prior_sigma = 0.1; + const double meas_sigma = 1.0; + const double circumradius = (std::sqrt(4 + 2 * std::sqrt(2))) / 2.0; gtsam::Point2 landmark1(circumradius, circumradius); gtsam::noiseModel::Isotropic::shared_ptr prior_noise = @@ -1037,7 +1055,7 @@ TEST(TestSuite, bearing_range_semantic_slam) { gtsam::PriorFactor p0(x0, pose0, prior_noise); gtsam::PriorFactor pl1(l1, landmark1, prior_lm_noise); - DiscretePriorFactor plc1(lm1_class, prior_lm1_class); + dcsam::DiscretePriorFactor plc1(lm1_class, prior_lm1_class); initialGuess.insert(x0, pose0); initialGuess.insert(l1, landmark1); @@ -1048,10 +1066,10 @@ TEST(TestSuite, bearing_range_semantic_slam) { hfg.push_discrete(plc1); // Setup dcsam - DCSAM dcsam; + dcsam::DCSAM dcsam; dcsam.update(hfg, initialGuess, initialGuessDiscrete); - DCValues dcval_start = dcsam.calculateEstimate(); + dcsam::DCValues dcval_start = dcsam.calculateEstimate(); std::cout << "Printing first values" << std::endl; dcval_start.discrete.print(); @@ -1072,7 +1090,7 @@ TEST(TestSuite, bearing_range_semantic_slam) { // Add semantic bearing-range measurement to landmark in center gtsam::Rot2 bearing1 = gtsam::Rot2::fromDegrees(67.5); - double range1 = circumradius; + const double range1 = circumradius; // For the first couple measurements, pick class=0, later pick class=1 std::vector semantic_meas; @@ -1084,16 +1102,16 @@ TEST(TestSuite, bearing_range_semantic_slam) { semantic_meas.push_back(0.9); } - hfg.push_dc(SemanticBearingRangeFactor( + hfg.push_dc(dcsam::SemanticBearingRangeFactor( xi, l1, lm1_class, semantic_meas, bearing1, range1, br_noise)); odom = odom * meas; initialGuess.insert(xj, odom); dcsam.update(hfg, initialGuess); - DCValues dcvals = dcsam.calculateEstimate(); + dcsam::DCValues dcvals = dcsam.calculateEstimate(); - size_t mpeClassL1 = dcvals.discrete.at(lc1); + const size_t mpeClassL1 = dcvals.discrete.at(lc1); // Plot poses and landmarks #ifdef ENABLE_PLOTTING @@ -1111,10 +1129,11 @@ TEST(TestSuite, bearing_range_semantic_slam) { lmys.push_back( dcvals.continuous.at(gtsam::Symbol('l', 1)).y()); - string color = (mpeClassL1 == 0) ? "b" : "orange"; + std::string color = (mpeClassL1 == 0) ? "b" : "orange"; plt::plot(xs, ys); - plt::scatter(lmxs, lmys, {{"color", color}}); + plt::scatter(lmxs, lmys, 30, {{"color", color}}); + plt::grid(true); plt::show(); #endif @@ -1128,9 +1147,9 @@ TEST(TestSuite, bearing_range_semantic_slam) { hfg.push_nonlinear(bw); dcsam.update(hfg, initialGuess); - DCValues dcvals = dcsam.calculateEstimate(); + dcsam::DCValues dcvals = dcsam.calculateEstimate(); - size_t mpeClassL1 = dcvals.discrete.at(lc1); + const size_t mpeClassL1 = dcvals.discrete.at(lc1); // Plot the poses and landmarks #ifdef ENABLE_PLOTTING @@ -1146,10 +1165,11 @@ TEST(TestSuite, bearing_range_semantic_slam) { lmys.push_back( dcvals.continuous.at(gtsam::Symbol('l', 1)).y()); - string color = (mpeClassL1 == 0) ? "b" : "orange"; + std::string color = (mpeClassL1 == 0) ? "b" : "orange"; plt::plot(xs, ys); - plt::scatter(lmxs, lmys, {{"color", color}}); + plt::scatter(lmxs, lmys, 30, {{"color", color}}); + plt::grid(true); plt::show(); #endif @@ -1163,11 +1183,11 @@ TEST(TestSuite, bearing_range_semantic_slam) { */ TEST(TestSuite, dcMaxMixture_semantic_slam) { // Make a factor graph - HybridFactorGraph hfg; + dcsam::HybridFactorGraph hfg; // Values for initial guess gtsam::Values initialGuess; - DiscreteValues initialGuessDiscrete; + dcsam::DiscreteValues initialGuessDiscrete; gtsam::Symbol x0('x', 0); gtsam::Symbol l1('l', 1); @@ -1176,9 +1196,9 @@ TEST(TestSuite, dcMaxMixture_semantic_slam) { gtsam::DiscreteKey lm1_class(lc1, 2); gtsam::Pose2 pose0(0, 0, 0); gtsam::Pose2 dx(1, 0, 0.78539816); - double prior_sigma = 0.1; - double meas_sigma = 1.0; - double circumradius = (std::sqrt(4 + 2 * std::sqrt(2))) / 2.0; + const double prior_sigma = 0.1; + const double meas_sigma = 1.0; + const double circumradius = (std::sqrt(4 + 2 * std::sqrt(2))) / 2.0; gtsam::Point2 landmark1(circumradius, circumradius); gtsam::noiseModel::Isotropic::shared_ptr prior_noise = @@ -1198,7 +1218,7 @@ TEST(TestSuite, dcMaxMixture_semantic_slam) { gtsam::PriorFactor p0(x0, pose0, prior_noise); gtsam::PriorFactor pl1(l1, landmark1, prior_lm_noise); - DiscretePriorFactor plc1(lm1_class, prior_lm1_class); + dcsam::DiscretePriorFactor plc1(lm1_class, prior_lm1_class); initialGuess.insert(x0, pose0); initialGuess.insert(l1, landmark1); @@ -1220,7 +1240,7 @@ TEST(TestSuite, dcMaxMixture_semantic_slam) { prior_lm2_class.push_back(0.9); gtsam::PriorFactor pl2(l2, landmark2, prior_lm_noise); - DiscretePriorFactor plc2(lm2_class, prior_lm2_class); + dcsam::DiscretePriorFactor plc2(lm2_class, prior_lm2_class); initialGuess.insert(l2, landmark2); initialGuessDiscrete[lm2_class.first] = 1; @@ -1229,10 +1249,10 @@ TEST(TestSuite, dcMaxMixture_semantic_slam) { hfg.push_discrete(plc2); // Setup dcsam - DCSAM dcsam; + dcsam::DCSAM dcsam; dcsam.update(hfg, initialGuess, initialGuessDiscrete); - DCValues dcval_start = dcsam.calculateEstimate(); + dcsam::DCValues dcval_start = dcsam.calculateEstimate(); std::cout << "Printing first values" << std::endl; dcval_start.discrete.print(); @@ -1268,20 +1288,21 @@ TEST(TestSuite, dcMaxMixture_semantic_slam) { gtsam::DiscreteKeys dks({lm1_class, lm2_class}); // build mixture: dcmaxmixture should be picking the component for lm1 - SemanticBearingRangeFactor sbr1( + dcsam::SemanticBearingRangeFactor sbr1( xi, l1, lm1_class, semantic_meas, bearing1, range1, br_noise); - SemanticBearingRangeFactor sbr2( + dcsam::SemanticBearingRangeFactor sbr2( xi, l2, lm2_class, semantic_meas, bearing1, range1, br_noise); - DCMaxMixtureFactor> + dcsam::DCMaxMixtureFactor< + dcsam::SemanticBearingRangeFactor> dcmmf({xi, l1, l2}, dks, {sbr1, sbr2}, {.5, .5}, false); hfg.push_dc(dcmmf); odom = odom * meas; initialGuess.insert(xj, odom); dcsam.update(hfg, initialGuess); - DCValues dcvals = dcsam.calculateEstimate(); + dcsam::DCValues dcvals = dcsam.calculateEstimate(); - size_t mpeClassL1 = dcvals.discrete.at(lc1); + const size_t mpeClassL1 = dcvals.discrete.at(lc1); // Plot poses and landmarks #ifdef ENABLE_PLOTTING @@ -1299,10 +1320,11 @@ TEST(TestSuite, dcMaxMixture_semantic_slam) { lmys.push_back( dcvals.continuous.at(gtsam::Symbol('l', 1)).y()); - string color = (mpeClassL1 == 0) ? "b" : "orange"; + std::string color = (mpeClassL1 == 0) ? "b" : "orange"; plt::plot(xs, ys); - plt::scatter(lmxs, lmys, {{"color", color}}); + plt::scatter(lmxs, lmys, 30, {{"color", color}}); + plt::grid(true); plt::show(); #endif @@ -1316,9 +1338,195 @@ TEST(TestSuite, dcMaxMixture_semantic_slam) { hfg.push_nonlinear(bw); dcsam.update(hfg, initialGuess); - DCValues dcvals = dcsam.calculateEstimate(); + dcsam::DCValues dcvals = dcsam.calculateEstimate(); - size_t mpeClassL1 = dcvals.discrete.at(lc1); + const size_t mpeClassL1 = dcvals.discrete.at(lc1); + + // Plot the poses and landmarks +#ifdef ENABLE_PLOTTING + std::vector xs, ys; + for (size_t i = 0; i < 8; i++) { + xs.push_back(dcvals.continuous.at(gtsam::Symbol('x', i)).x()); + ys.push_back(dcvals.continuous.at(gtsam::Symbol('x', i)).y()); + } + + std::vector lmxs, lmys; + lmxs.push_back( + dcvals.continuous.at(gtsam::Symbol('l', 1)).x()); + lmys.push_back( + dcvals.continuous.at(gtsam::Symbol('l', 1)).y()); + + std::string color = (mpeClassL1 == 0) ? "b" : "orange"; + + plt::plot(xs, ys); + plt::scatter(lmxs, lmys, 30, {{"color", color}}); + plt::grid(true); + plt::show(); +#endif + + EXPECT_EQ(mpeClassL1, 1); +} + +TEST(TestSuite, simple_dcemfactor) { + // Make a factor graph + dcsam::HybridFactorGraph hfg; + + // Values for initial guess + gtsam::Values initialGuess; + dcsam::DiscreteValues initialGuessDiscrete; + + gtsam::Symbol x0('x', 0); + gtsam::Symbol l1('l', 1); + gtsam::Symbol lc1('c', 1); + // Create a discrete key for landmark 1 class with cardinality 2. + gtsam::DiscreteKey lm1_class(lc1, 2); + gtsam::Pose2 pose0(0, 0, 0); + gtsam::Pose2 dx(1, 0, 0.78539816); + const double prior_sigma = 0.1; + const double meas_sigma = 1.0; + const double circumradius = (std::sqrt(4 + 2 * std::sqrt(2))) / 2.0; + gtsam::Point2 landmark1(circumradius, circumradius); + + gtsam::noiseModel::Isotropic::shared_ptr prior_noise = + gtsam::noiseModel::Isotropic::Sigma(3, prior_sigma); + gtsam::noiseModel::Isotropic::shared_ptr prior_lm_noise = + gtsam::noiseModel::Isotropic::Sigma(2, prior_sigma); + gtsam::noiseModel::Isotropic::shared_ptr meas_noise = + gtsam::noiseModel::Isotropic::Sigma(3, meas_sigma); + + // 0.1 rad std on bearing, 10cm on range + gtsam::noiseModel::Isotropic::shared_ptr br_noise = + gtsam::noiseModel::Isotropic::Sigma(2, 0.1); + + std::vector prior_lm1_class; + prior_lm1_class.push_back(0.9); + prior_lm1_class.push_back(0.1); + + gtsam::PriorFactor p0(x0, pose0, prior_noise); + gtsam::PriorFactor pl1(l1, landmark1, prior_lm_noise); + dcsam::DiscretePriorFactor plc1(lm1_class, prior_lm1_class); + + initialGuess.insert(x0, pose0); + initialGuess.insert(l1, landmark1); + initialGuessDiscrete[lm1_class.first] = 0; + + hfg.push_nonlinear(p0); + hfg.push_nonlinear(pl1); + hfg.push_discrete(plc1); + + // set up for landmark 2 + gtsam::Symbol l2('l', 2); + gtsam::Symbol lc2('c', 2); + // Create a discrete key for landmark 2 class with cardinality 2. + gtsam::DiscreteKey lm2_class(lc2, 2); + gtsam::Point2 landmark2(circumradius + .5, circumradius + 5); + + std::vector prior_lm2_class; + prior_lm2_class.push_back(0.1); + prior_lm2_class.push_back(0.9); + + gtsam::PriorFactor pl2(l2, landmark2, prior_lm_noise); + dcsam::DiscretePriorFactor plc2(lm2_class, prior_lm2_class); + + initialGuess.insert(l2, landmark2); + initialGuessDiscrete[lm2_class.first] = 1; + + hfg.push_nonlinear(pl2); + hfg.push_discrete(plc2); + + // Setup dcsam + dcsam::DCSAM dcsam; + dcsam.update(hfg, initialGuess, initialGuessDiscrete); + + dcsam::DCValues dcval_start = dcsam.calculateEstimate(); + std::cout << "Printing first values" << std::endl; + dcval_start.discrete.print(); + + hfg.clear(); + initialGuess.clear(); + initialGuessDiscrete.clear(); + + gtsam::Pose2 odom(pose0); + gtsam::Pose2 noise(0.01, 0.01, 0.01); + for (size_t i = 0; i < 7; i++) { + gtsam::Symbol xi('x', i); + gtsam::Symbol xj('x', i + 1); + + gtsam::Pose2 meas = dx * noise; + + gtsam::BetweenFactor bw(xi, xj, meas, meas_noise); + hfg.push_nonlinear(bw); + + // Add semantic bearing-range measurement to landmark in center + gtsam::Rot2 bearing1 = gtsam::Rot2::fromDegrees(67.5); + const double range1 = circumradius; + + // For the first couple measurements, pick class=0, later pick class=1 + std::vector semantic_meas; + if (i < 2) { + semantic_meas.push_back(0.9); + semantic_meas.push_back(0.1); + } else { + semantic_meas.push_back(0.1); + semantic_meas.push_back(0.9); + } + + gtsam::DiscreteKeys dks({lm1_class, lm2_class}); + + // build mixture: dcemfactor should be picking the component for lm1 + dcsam::SemanticBearingRangeFactor sbr1( + xi, l1, lm1_class, semantic_meas, bearing1, range1, br_noise); + dcsam::SemanticBearingRangeFactor sbr2( + xi, l2, lm2_class, semantic_meas, bearing1, range1, br_noise); + dcsam::DCEMFactor< + dcsam::SemanticBearingRangeFactor> + dcemf({xi, l1, l2}, dks, {sbr1, sbr2}, {.5, .5}, false); + + hfg.push_dc(dcemf); + odom = odom * meas; + initialGuess.insert(xj, odom); + dcsam.update(hfg, initialGuess); + dcsam::DCValues dcvals = dcsam.calculateEstimate(); + + const size_t mpeClassL1 = dcvals.discrete.at(lc1); + + // Plot poses and landmarks +#ifdef ENABLE_PLOTTING + std::vector xs, ys; + for (size_t j = 0; j < i + 2; j++) { + xs.push_back( + dcvals.continuous.at(gtsam::Symbol('x', j)).x()); + ys.push_back( + dcvals.continuous.at(gtsam::Symbol('x', j)).y()); + } + + std::vector lmxs, lmys; + lmxs.push_back( + dcvals.continuous.at(gtsam::Symbol('l', 1)).x()); + lmys.push_back( + dcvals.continuous.at(gtsam::Symbol('l', 1)).y()); + + std::string color = (mpeClassL1 == 0) ? "b" : "orange"; + + plt::plot(xs, ys); + plt::scatter(lmxs, lmys, 30, {{"color", color}}); + plt::grid(true); + plt::show(); +#endif + + hfg.clear(); + initialGuess.clear(); + } + + gtsam::Symbol x7('x', 7); + gtsam::BetweenFactor bw(x0, x7, dx * noise, meas_noise); + + hfg.push_nonlinear(bw); + dcsam.update(hfg, initialGuess); + + dcsam::DCValues dcvals = dcsam.calculateEstimate(); + + const size_t mpeClassL1 = dcvals.discrete.at(lc1); // Plot the poses and landmarks #ifdef ENABLE_PLOTTING @@ -1334,10 +1542,11 @@ TEST(TestSuite, dcMaxMixture_semantic_slam) { lmys.push_back( dcvals.continuous.at(gtsam::Symbol('l', 1)).y()); - string color = (mpeClassL1 == 0) ? "b" : "orange"; + std::string color = (mpeClassL1 == 0) ? "b" : "orange"; plt::plot(xs, ys); - plt::scatter(lmxs, lmys, {{"color", color}}); + plt::scatter(lmxs, lmys, 30, {{"color", color}}); + plt::grid(true); plt::show(); #endif @@ -1349,11 +1558,11 @@ TEST(TestSuite, dcMaxMixture_semantic_slam) { */ TEST(TestSuite, factor_removal) { // Make a factor graph - HybridFactorGraph hfg; + dcsam::HybridFactorGraph hfg; // Values for initial guess gtsam::Values initialGuess; - DiscreteValues initialGuessDiscrete; + dcsam::DiscreteValues initialGuessDiscrete; gtsam::Symbol x0('x', 0); gtsam::Symbol l1('l', 1); @@ -1384,7 +1593,7 @@ TEST(TestSuite, factor_removal) { gtsam::PriorFactor p0(x0, pose0, prior_noise); gtsam::PriorFactor pl1(l1, landmark1, prior_lm_noise); - DiscretePriorFactor plc1(lm1_class, prior_lm1_class); + dcsam::DiscretePriorFactor plc1(lm1_class, prior_lm1_class); initialGuess.insert(x0, pose0); initialGuess.insert(l1, landmark1); @@ -1395,10 +1604,10 @@ TEST(TestSuite, factor_removal) { hfg.push_discrete(plc1); // Setup dcsam - DCSAM dcsam; + dcsam::DCSAM dcsam; dcsam.update(hfg, initialGuess, initialGuessDiscrete); - DCValues dcval_start = dcsam.calculateEstimate(); + dcsam::DCValues dcval_start = dcsam.calculateEstimate(); std::cout << "Printing first values" << std::endl; dcval_start.discrete.print(); @@ -1433,20 +1642,20 @@ TEST(TestSuite, factor_removal) { gtsam::DiscreteKeys dks({lm1_class}); - SemanticBearingRangeFactor sbr1( + dcsam::SemanticBearingRangeFactor sbr1( xi, l1, lm1_class, semantic_meas, bearing1, range1, br_noise); hfg.push_dc(sbr1); odom = odom * meas; initialGuess.insert(xj, odom); dcsam.update(hfg, initialGuess); - DCValues dcvals = dcsam.calculateEstimate(); + dcsam::DCValues dcvals = dcsam.calculateEstimate(); hfg.clear(); initialGuess.clear(); } - DCValues dcvals = dcsam.calculateEstimate(); + dcsam::DCValues dcvals = dcsam.calculateEstimate(); size_t mpeClassL1 = dcvals.discrete.at(lc1); EXPECT_EQ(mpeClassL1, 1);