Fix examples using biasAccOmegaInt

examples/CombinedImuFactorsExample.cpp

@@ -45,10 +45,10 @@
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/slam/dataset.h>
 
+#include <cassert>
 #include <cstring>
 #include <fstream>
 #include <iostream>
-#include <cassert>
 
 using namespace gtsam;
 using namespace std;
@@ -108,8 +108,6 @@ std::shared_ptr<PreintegratedCombinedMeasurements::Params> imuParams() {
       I_3x3 * 1e-8;  // error committed in integrating position from velocities
   Matrix33 bias_acc_cov = I_3x3 * pow(accel_bias_rw_sigma, 2);
   Matrix33 bias_omega_cov = I_3x3 * pow(gyro_bias_rw_sigma, 2);
-  Matrix66 bias_acc_omega_init =
-      I_6x6 * 1e-5;  // error in the bias used for preintegration
 
   auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0);
   // PreintegrationBase params:
@@ -124,7 +122,12 @@ std::shared_ptr<PreintegratedCombinedMeasurements::Params> imuParams() {
   // PreintegrationCombinedMeasurements params:
   p->biasAccCovariance = bias_acc_cov;      // acc bias in continuous
   p->biasOmegaCovariance = bias_omega_cov;  // gyro bias in continuous
+
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
+  Matrix66 bias_acc_omega_init =
+      I_6x6 * 1e-5;  // error in the bias used for preintegration
   p->biasAccOmegaInt = bias_acc_omega_init;
+#endif
 
   return p;
 }

examples/ImuFactorsExample.cpp

@@ -48,8 +48,8 @@
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/slam/dataset.h>
 
-#include <cstring>
 #include <cassert>
+#include <cstring>
 #include <fstream>
 #include <iostream>
 
@@ -95,8 +95,6 @@ std::shared_ptr<PreintegratedCombinedMeasurements::Params> imuParams() {
       I_3x3 * 1e-8;  // error committed in integrating position from velocities
   Matrix33 bias_acc_cov = I_3x3 * pow(accel_bias_rw_sigma, 2);
   Matrix33 bias_omega_cov = I_3x3 * pow(gyro_bias_rw_sigma, 2);
-  Matrix66 bias_acc_omega_init =
-      I_6x6 * 1e-5;  // error in the bias used for preintegration
 
   auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0);
   // PreintegrationBase params:
@@ -111,7 +109,11 @@ std::shared_ptr<PreintegratedCombinedMeasurements::Params> imuParams() {
   // PreintegrationCombinedMeasurements params:
   p->biasAccCovariance = bias_acc_cov;      // acc bias in continuous
   p->biasOmegaCovariance = bias_omega_cov;  // gyro bias in continuous
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
+  Matrix66 bias_acc_omega_init =
+      I_6x6 * 1e-5;  // error in the bias used for preintegration
   p->biasAccOmegaInt = bias_acc_omega_init;
+#endif
 
   return p;
 }

gtsam/constrained/ConstrainedOptimizer.h

@@ -1,8 +1,12 @@
 /* ----------------------------------------------------------------------------
- * GTDynamics Copyright 2020-2021, Georgia Tech Research Corporation,
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
  * See LICENSE for the license information
+
  * -------------------------------------------------------------------------- */
 
 /**

gtsam/constrained/PenaltyOptimizer.cpp

@@ -1,8 +1,12 @@
 /* ----------------------------------------------------------------------------
- * GTDynamics Copyright 2020, Georgia Tech Research Corporation,
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
  * See LICENSE for the license information
+
  * -------------------------------------------------------------------------- */
 
 /**

gtsam/constrained/tests/testAugmentedLagrangianOptimizer.cpp

@@ -1,8 +1,12 @@
 /* ----------------------------------------------------------------------------
- * GTDynamics Copyright 2020, Georgia Tech Research Corporation,
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
  * See LICENSE for the license information
+
  * -------------------------------------------------------------------------- */
 
 /**

gtsam/constrained/tests/testPenaltyOptimizer.cpp

@@ -1,8 +1,12 @@
 /* ----------------------------------------------------------------------------
- * GTDynamics Copyright 2020, Georgia Tech Research Corporation,
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
  * See LICENSE for the license information
+
  * -------------------------------------------------------------------------- */
 
 /**

gtsam_unstable/examples/ISAM2_SmartFactorStereo_IMU.cpp

@@ -20,9 +20,9 @@
 
 using namespace std;
 using namespace gtsam;
-using symbol_shorthand::X;
-using symbol_shorthand::V;
 using symbol_shorthand::B;
+using symbol_shorthand::V;
+using symbol_shorthand::X;
 
 struct IMUHelper {
   IMUHelper() {
@@ -57,11 +57,13 @@ struct IMUHelper {
     p->biasAccCovariance = I_3x3 * pow(0.00002, 2.0);  // acc bias in continuous
     p->biasOmegaCovariance =
         I_3x3 * pow(0.001, 2.0);  // gyro bias in continuous
+
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
     p->biasAccOmegaInt = Matrix::Identity(6, 6) * 1e-5;
+#endif
 
     // body to IMU rotation
-    Rot3 iRb(0.036129, -0.998727, 0.035207,
-             0.045417, -0.033553, -0.998404,
+    Rot3 iRb(0.036129, -0.998727, 0.035207, 0.045417, -0.033553, -0.998404,
              0.998315, 0.037670, 0.044147);
 
     // body to IMU translation (meters)
@@ -133,8 +135,7 @@ int main(int argc, char* argv[]) {
   initialEstimate.insert(X(0), Pose3::Identity());
 
   // Bias prior
-  graph.addPrior(B(1), imu.priorImuBias,
-                                               imu.biasNoiseModel);
+  graph.addPrior(B(1), imu.priorImuBias, imu.biasNoiseModel);
   initialEstimate.insert(B(0), imu.priorImuBias);
 
   // Velocity prior - assume stationary