Merge pull request #38 from jungseokhong/main

address the issue of duplicating discrete factors
MarineRoboticsGroup · Jul 25, 2024 · a493b3d · a493b3d
2 parents b7f6229 + 0d36698
commit a493b3d
Show file tree

Hide file tree

Showing 2 changed files with 111 additions and 1 deletion.
diff --git a/src/DCSAM.cpp b/src/DCSAM.cpp
@@ -95,7 +95,7 @@ void DCSAM::update(const gtsam::NonlinearFactorGraph &graph,
   updateContinuousInfo(currDiscrete_, combined, initialGuessContinuous);
   currContinuous_ = isam_.calculateEstimate();
   // Update discrete info from last solve and
-  updateDiscrete(discreteCombined, currContinuous_, currDiscrete_);
+  updateDiscreteInfo(currContinuous_, currDiscrete_);
 }
 
 void DCSAM::update(const HybridFactorGraph &hfg,

diff --git a/tests/testDCSAM.cpp b/tests/testDCSAM.cpp
@@ -1553,6 +1553,116 @@ TEST(TestSuite, simple_dcemfactor) {
   EXPECT_EQ(mpeClassL1, 1);
 }
 
+/**
+ * This is for testing the behavior of duplicating discrete factors 
+ */
+TEST(TestSuite, factor_duplicate) {
+   // 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<double> prior_lm1_class;
+  prior_lm1_class.push_back(0.9);
+  prior_lm1_class.push_back(0.1);
+
+  gtsam::PriorFactor<gtsam::Pose2> p0(x0, pose0, prior_noise);
+  gtsam::PriorFactor<gtsam::Point2> 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<gtsam::Pose2> 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<double> 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<gtsam::Pose2, gtsam::Point2> 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, 0);
+  EXPECT_EQ(dcsam.getDiscreteFactorGraph().size(), 4);
+  EXPECT_EQ(dcsam.getNonlinearFactorGraph().size(), 8);
+
+}
+
 int main(int argc, char** argv) {
   testing::InitGoogleTest(&argc, argv);
   return RUN_ALL_TESTS();