diff --git a/include/dcsam/DCSAM.h b/include/dcsam/DCSAM.h index 686ca2d..9975489 100644 --- a/include/dcsam/DCSAM.h +++ b/include/dcsam/DCSAM.h @@ -70,14 +70,24 @@ 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 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 DiscreteValues &initialGuessDiscrete = DiscreteValues(), + const gtsam::FactorIndices &removeFactorIndices = + gtsam::FactorIndices(), + const gtsam::FactorIndices &removeDiscreteFactorIndices = + gtsam::FactorIndices()); /** * A HybridFactorGraph is a container holding a NonlinearFactorGraph, a @@ -86,11 +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 DiscreteValues &initialGuessDiscrete = DiscreteValues(), + const gtsam::FactorIndices &removeFactorIndices = + gtsam::FactorIndices(), + const gtsam::FactorIndices &removeDiscreteFactorIndices = + gtsam::FactorIndices()); /** * Inline convenience function to allow "skipping" the initial guess for @@ -160,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 4dd913a..4848ab4 100644 --- a/src/DCSAM.cpp +++ b/src/DCSAM.cpp @@ -31,8 +31,18 @@ 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, + 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 + isam_.update(gtsam::NonlinearFactorGraph(), gtsam::Values(), removeFactorIndices); + for (auto& i : removeDiscreteFactorIndices) { + dfg_.remove(i); + } + + // 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)) @@ -92,7 +102,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_); @@ -100,9 +111,12 @@ 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, + const gtsam::FactorIndices &removeDiscreteFactorIndices) { + update(hfg.nonlinearGraph(), hfg.discreteGraph(), hfg.dcGraph(), + initialGuessContinuous, initialGuessDiscrete, + removeFactorIndices, removeDiscreteFactorIndices); } void DCSAM::update() { @@ -138,7 +152,9 @@ void DCSAM::updateContinuous() { void DCSAM::updateContinuousInfo(const DiscreteValues &discreteVals, const gtsam::NonlinearFactorGraph &newFactors, - const gtsam::Values &initialGuess) { + const gtsam::Values &initialGuess, + const gtsam::FactorIndices &removeFactorIndices) { + gtsam::ISAM2UpdateParams updateParams; gtsam::FastMap newAffectedKeys; for (size_t j = 0; j < dcContinuousFactors_.size(); j++) { @@ -150,6 +166,8 @@ 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 0a4f3f5..feb7c40 100644 --- a/tests/testDCSAM.cpp +++ b/tests/testDCSAM.cpp @@ -1553,6 +1553,144 @@ TEST(TestSuite, simple_dcemfactor) { EXPECT_EQ(mpeClassL1, 1); } +/** + * This is for testing the behavior of factor removal + */ +TEST(TestSuite, factor_removal) { + // 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); + 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); + + 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] = 1; + + hfg.push_nonlinear(p0); + hfg.push_nonlinear(pl1); + hfg.push_discrete(plc1); + + // 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 < 3; 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 measurement, pick class=0, later pick class=1 + std::vector semantic_meas; + if (i < 1) { + 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}); + + 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); + dcsam::DCValues dcvals = dcsam.calculateEstimate(); + + hfg.clear(); + initialGuess.clear(); + } + + dcsam::DCValues dcvals = dcsam.calculateEstimate(); + size_t mpeClassL1 = dcvals.discrete.at(lc1); + EXPECT_EQ(mpeClassL1, 1); + + 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(); + + // 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{5}; + + // 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(5), nullptr); + EXPECT_EQ(dcsam.getNonlinearFactorGraph().at(5), nullptr); + + dcvals = dcsam.calculateEstimate(); + + mpeClassL1 = dcvals.discrete.at(lc1); + + EXPECT_EQ(mpeClassL1, 0); + +} + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS();