Merge pull request #2254 from jaffarrell/BugFix_ImuFactorsExample

Bug fix imu factors example

Author: dellaert

examples/ImuFactorsExample.cpp

@@ -212,10 +212,20 @@ int main(int argc, char* argv[]) {
                       // exactly the same, so keeping this for simplicity.
 
   // All priors have been set up, now iterate through the data file.
-  while (file.good()) {
-    // Parse out first value
-    getline(file, value, ',');
-    int type = stoi(value.c_str());
+  std::string line;
+  int type{1000};
+  while (std::getline(file, line)) {
+    std::stringstream ss(line);
+    std::string value;
+
+    // Read value until comma. Skip to next line on failure to read.
+    if (!std::getline(ss, value, ',')) continue;
+    try {
+      type = std::stoi(value);
+    } catch (const std::invalid_argument& e) {
+      std::cerr << "Invalid integer in input: \"" << value << "\"\n";
+      continue;  // Or break, depending on desired behavior
+    }
 
     if (type == 0) {  // IMU measurement
       Vector6 imu;

gtsam/basis/tests/testFourier.cpp

@@ -22,10 +22,14 @@
 #include <gtsam/basis/Fourier.h>
 #include <gtsam/nonlinear/factorTesting.h>
 
-#if defined(__GNUC__) && !defined(__clang__)
+#if defined(__GNUC__) && !defined(__clang__) && (__GNUC__ >= 11)
 #pragma GCC diagnostic warning "-Wstringop-overread"
+#endif
+
+#if defined(__GNUC__) && !defined(__clang__)
 #pragma GCC diagnostic warning "-Warray-bounds"
 #endif
+
 using namespace std;
 using namespace gtsam;
 

gtsam/discrete/tests/testDecisionTree.cpp

@@ -29,9 +29,9 @@
 
 #include <iomanip>
 
-using std::vector;
-using std::string;
 using std::map;
+using std::string;
+using std::vector;
 using namespace gtsam;
 
 template <typename T>
@@ -54,7 +54,8 @@ struct CrazyDecisionTree : public DecisionTree<string, Crazy> {
     auto keyFormatter = [](const std::string& s) { return s; };
     auto valueFormatter = [](const Crazy& v) {
       std::stringstream ss;
-      ss << "{" << v.a << "," << std::setw(4) << std::setprecision(2) << v.b << "}";
+      ss << "{" << v.a << "," << std::setw(4) << std::setprecision(2) << v.b
+         << "}";
       return ss.str();
     };
     DecisionTree<string, Crazy>::print("", keyFormatter, valueFormatter);
@@ -103,9 +104,7 @@ struct DT : public DecisionTree<string, int> {
   /// print to stdout
   void print(const std::string& s = "") const {
     auto keyFormatter = [](const std::string& s) { return s; };
-    auto valueFormatter = [](const int& v) {
-      return std::to_string(v);
-    };
+    auto valueFormatter = [](const int& v) { return std::to_string(v); };
     std::cout << s;
     Base::print("", keyFormatter, valueFormatter);
   }
@@ -132,10 +131,10 @@ TEST(DecisionTree, ConstructorOrder) {
   string A("A"), B("B");
 
   const std::vector<int> ys1 = {1, 2, 3, 4};
-  DT tree1({{B, 2}, {A, 2}}, ys1); // faster version, as B is "higher" than A!
+  DT tree1({{B, 2}, {A, 2}}, ys1);  // faster version, as B is "higher" than A!
 
   const std::vector<int> ys2 = {1, 3, 2, 4};
-  DT tree2({{A, 2}, {B, 2}}, ys2); // slower version !
+  DT tree2({{A, 2}, {B, 2}}, ys2);  // slower version !
 
   // Both trees will be the same, tree is order from high to low labels.
   // Choice(B)
@@ -266,7 +265,8 @@ TEST(DecisionTree, Example) {
 }
 
 /* ************************************************************************** */
-// Test that we can create two trees out of one, using a function that returns a pair.
+// Test that we can create two trees out of one, using a function that returns a
+// pair.
 TEST(DecisionTree, Split) {
   // Create labels
   string A("A"), B("B");
@@ -276,11 +276,11 @@ TEST(DecisionTree, Split) {
 
   // Define a function that returns an int/bool pair
   auto split_function = [](const int& value) -> std::pair<int, bool> {
-    return {value*3, value*3 % 2 == 0};
+    return {value * 3, value * 3 % 2 == 0};
   };
 
   // Split the original tree into two new trees
-  auto [la,lb] = original.split<int,bool>(split_function);
+  auto [la, lb] = original.split<int, bool>(split_function);
 
   // Check the first resulting tree
   EXPECT_LONGS_EQUAL(3, la(Assignment<string>{{A, 0}, {B, 0}}));
@@ -295,7 +295,6 @@ TEST(DecisionTree, Split) {
   EXPECT(lb(Assignment<string>{{A, 1}, {B, 1}}));
 }
 
-
 /* ************************************************************************** */
 // Test that we can create a tree by modifying an rvalue.
 TEST(DecisionTree, Consume) {
@@ -305,7 +304,7 @@ TEST(DecisionTree, Consume) {
   // Create a decision tree
   DT original(A, DT(B, 1, 2), DT(B, 3, 4));
 
-  DT modified([](int i){return i*2;}, std::move(original));
+  DT modified([](int i) { return i * 2; }, std::move(original));
 
   // Check the first resulting tree
   EXPECT_LONGS_EQUAL(2, modified(Assignment<string>{{A, 0}, {B, 0}}));
@@ -319,7 +318,7 @@ TEST(DecisionTree, Consume) {
 
 /* ************************************************************************** */
 // test Conversion of values
-bool bool_of_int(const int& y) { return y != 0; };
+bool bool_of_int(const int& y) { return y != 0; }
 typedef DecisionTree<string, bool> StringBoolTree;
 
 TEST(DecisionTree, ConvertValuesOnly) {
@@ -333,7 +332,7 @@ TEST(DecisionTree, ConvertValuesOnly) {
   StringBoolTree f2(f1, bool_of_int);
 
   // Check a value
-  Assignment<string> x00 {{A, 0}, {B, 0}};
+  Assignment<string> x00{{A, 0}, {B, 0}};
   EXPECT(!f2(x00));
 }
 

gtsam/discrete/tests/testSerializationDiscrete.cpp

@@ -32,11 +32,11 @@ BOOST_CLASS_EXPORT_GUID(Tree, "gtsam_DecisionTreeStringInt")
 BOOST_CLASS_EXPORT_GUID(Tree::Leaf, "gtsam_DecisionTreeStringInt_Leaf")
 BOOST_CLASS_EXPORT_GUID(Tree::Choice, "gtsam_DecisionTreeStringInt_Choice")
 
-BOOST_CLASS_EXPORT_GUID(DecisionTreeFactor, "gtsam_DecisionTreeFactor");
-BOOST_CLASS_EXPORT_GUID(TableFactor, "gtsam_TableFactor");
+BOOST_CLASS_EXPORT_GUID(DecisionTreeFactor, "gtsam_DecisionTreeFactor")
+BOOST_CLASS_EXPORT_GUID(TableFactor, "gtsam_TableFactor")
 
 using ADT = AlgebraicDecisionTree<Key>;
-BOOST_CLASS_EXPORT_GUID(ADT, "gtsam_AlgebraicDecisionTree");
+BOOST_CLASS_EXPORT_GUID(ADT, "gtsam_AlgebraicDecisionTree")
 BOOST_CLASS_EXPORT_GUID(ADT::Leaf, "gtsam_AlgebraicDecisionTree_Leaf")
 BOOST_CLASS_EXPORT_GUID(ADT::Choice, "gtsam_AlgebraicDecisionTree_Choice")
 

gtsam/hybrid/tests/testGaussianMixture.cpp

@@ -47,7 +47,7 @@ DiscreteConditional::shared_ptr mixing =
 /// Gaussian density function
 double Gaussian(double mu, double sigma, double z) {
   return exp(-0.5 * pow((z - mu) / sigma, 2)) / sqrt(2 * M_PI * sigma * sigma);
-};
+}
 
 /**
  * Closed form computation of P(m=1|z).
@@ -59,7 +59,7 @@ double prob_m_z(double mu0, double mu1, double sigma0, double sigma1,
   const double p0 = 0.6 * Gaussian(mu0, sigma0, z);
   const double p1 = 0.4 * Gaussian(mu1, sigma1, z);
   return p1 / (p0 + p1);
-};
+}
 
 /*
  * Test a Gaussian Mixture Model P(m)p(z|m) with same sigma.

gtsam/hybrid/tests/testSerializationHybrid.cpp

@@ -40,44 +40,44 @@ using symbol_shorthand::Z;
 
 using namespace serializationTestHelpers;
 
-BOOST_CLASS_EXPORT_GUID(Factor, "gtsam_Factor");
-BOOST_CLASS_EXPORT_GUID(HybridFactor, "gtsam_HybridFactor");
-BOOST_CLASS_EXPORT_GUID(JacobianFactor, "gtsam_JacobianFactor");
-BOOST_CLASS_EXPORT_GUID(GaussianConditional, "gtsam_GaussianConditional");
-BOOST_CLASS_EXPORT_GUID(DiscreteConditional, "gtsam_DiscreteConditional");
-BOOST_CLASS_EXPORT_GUID(TableDistribution, "gtsam_TableDistribution");
-
-BOOST_CLASS_EXPORT_GUID(DecisionTreeFactor, "gtsam_DecisionTreeFactor");
+BOOST_CLASS_EXPORT_GUID(Factor, "gtsam_Factor")
+BOOST_CLASS_EXPORT_GUID(HybridFactor, "gtsam_HybridFactor")
+BOOST_CLASS_EXPORT_GUID(JacobianFactor, "gtsam_JacobianFactor")
+BOOST_CLASS_EXPORT_GUID(GaussianConditional, "gtsam_GaussianConditional")
+BOOST_CLASS_EXPORT_GUID(DiscreteConditional, "gtsam_DiscreteConditional")
+BOOST_CLASS_EXPORT_GUID(TableDistribution, "gtsam_TableDistribution")
+
+BOOST_CLASS_EXPORT_GUID(DecisionTreeFactor, "gtsam_DecisionTreeFactor")
 using ADT = AlgebraicDecisionTree<Key>;
-BOOST_CLASS_EXPORT_GUID(ADT, "gtsam_AlgebraicDecisionTree");
-BOOST_CLASS_EXPORT_GUID(ADT::Leaf, "gtsam_AlgebraicDecisionTree_Leaf");
+BOOST_CLASS_EXPORT_GUID(ADT, "gtsam_AlgebraicDecisionTree")
+BOOST_CLASS_EXPORT_GUID(ADT::Leaf, "gtsam_AlgebraicDecisionTree_Leaf")
 BOOST_CLASS_EXPORT_GUID(ADT::Choice, "gtsam_AlgebraicDecisionTree_Choice")
 
-BOOST_CLASS_EXPORT_GUID(HybridGaussianFactor, "gtsam_HybridGaussianFactor");
+BOOST_CLASS_EXPORT_GUID(HybridGaussianFactor, "gtsam_HybridGaussianFactor")
 BOOST_CLASS_EXPORT_GUID(HybridGaussianFactor::FactorValuePairs,
-                        "gtsam_HybridGaussianFactor_Factors");
+                        "gtsam_HybridGaussianFactor_Factors")
 BOOST_CLASS_EXPORT_GUID(HybridGaussianFactor::FactorValuePairs::Leaf,
-                        "gtsam_HybridGaussianFactor_Factors_Leaf");
+                        "gtsam_HybridGaussianFactor_Factors_Leaf")
 BOOST_CLASS_EXPORT_GUID(HybridGaussianFactor::FactorValuePairs::Choice,
-                        "gtsam_HybridGaussianFactor_Factors_Choice");
+                        "gtsam_HybridGaussianFactor_Factors_Choice")
 
 BOOST_CLASS_EXPORT_GUID(GaussianFactorGraphValuePair,
-                        "gtsam_GaussianFactorGraphValuePair");
+                        "gtsam_GaussianFactorGraphValuePair")
 BOOST_CLASS_EXPORT_GUID(HybridGaussianProductFactor,
-                        "gtsam_HybridGaussianProductFactor");
+                        "gtsam_HybridGaussianProductFactor")
 
 BOOST_CLASS_EXPORT_GUID(HybridGaussianConditional,
-                        "gtsam_HybridGaussianConditional");
+                        "gtsam_HybridGaussianConditional")
 BOOST_CLASS_EXPORT_GUID(HybridGaussianConditional::Conditionals,
-                        "gtsam_HybridGaussianConditional_Conditionals");
+                        "gtsam_HybridGaussianConditional_Conditionals")
 BOOST_CLASS_EXPORT_GUID(HybridGaussianConditional::Conditionals::Leaf,
-                        "gtsam_HybridGaussianConditional_Conditionals_Leaf");
+                        "gtsam_HybridGaussianConditional_Conditionals_Leaf")
 BOOST_CLASS_EXPORT_GUID(HybridGaussianConditional::Conditionals::Choice,
-                        "gtsam_HybridGaussianConditional_Conditionals_Choice");
+                        "gtsam_HybridGaussianConditional_Conditionals_Choice")
 // Needed since GaussianConditional::FromMeanAndStddev uses it
-BOOST_CLASS_EXPORT_GUID(noiseModel::Isotropic, "gtsam_noiseModel_Isotropic");
+BOOST_CLASS_EXPORT_GUID(noiseModel::Isotropic, "gtsam_noiseModel_Isotropic")
 
-BOOST_CLASS_EXPORT_GUID(HybridBayesNet, "gtsam_HybridBayesNet");
+BOOST_CLASS_EXPORT_GUID(HybridBayesNet, "gtsam_HybridBayesNet")
 
 /* ****************************************************************************/
 // Test HybridGaussianFactor serialization.

gtsam/sam/tests/testSerializationSam.cpp

@@ -39,8 +39,8 @@ constexpr double rangeMmeasurement(10.0);
 /* ************************************************************************* */
 // Export Noisemodels
 // See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html
-BOOST_CLASS_EXPORT(gtsam::noiseModel::Isotropic);
-BOOST_CLASS_EXPORT(gtsam::noiseModel::Unit);
+BOOST_CLASS_EXPORT(gtsam::noiseModel::Isotropic)
+BOOST_CLASS_EXPORT(gtsam::noiseModel::Unit)
 
 /* ************************************************************************* */
 TEST(SerializationSam, BearingFactor2D) {