Add ceres_nav namespace

examples/gps_imu_example.cpp

@@ -17,6 +17,7 @@
 #include "utils/Utils.h"
 
 namespace po = boost::program_options;
+using namespace ceres_nav;
 
 po::variables_map handle_args(int argc, const char *argv[]) {
   po::options_description options("Allowed options");
@@ -91,7 +92,7 @@ void runSlidingWindowEstimator(
   // Create the initial IMU increment
   IMUIncrement imu_increment(
       Q_ct, init_imu_state.gyroBias(), init_imu_state.accelBias(),
-      init_imu_state.timestamp(), gravity, lie_direction);
+      init_imu_state.timestamp(), gravity, lie_direction, state_rep);
   IMUState cur_imu_state = init_imu_state;
   double prev_gps_timestamp = gps_data[0].timestamp;
   std::vector<double> est_stamps;
@@ -186,7 +187,7 @@ void runFullBatchEstimator(
   size_t imu_idx = 0;
   IMUIncrement imu_increment(
       Q_ct, init_imu_state.gyroBias(), init_imu_state.accelBias(),
-      init_imu_state.timestamp(), gravity, lie_direction);
+      init_imu_state.timestamp(), gravity, lie_direction, state_rep);
 
   IMUState cur_imu_state = init_imu_state;
   double prev_gps_timestamp = gps_data[0].timestamp;

examples/include/FactorGraphUtils.h

@@ -21,35 +21,37 @@
 
 namespace factor_graph_utils {
 
+using namespace ceres_nav;
+
 struct ProblemKeys {
   std::string nav_state_key = "nav_state";
   std::string bias_state_key = "gyro_bias";
 };
 
 void addIMUState(
-    ceres_nav::FactorGraph &graph, const IMUState &imu_state,
+    FactorGraph &graph, const IMUState &imu_state,
     const LieDirection direction,
     ExtendedPoseRepresentation state_rep = ExtendedPoseRepresentation::SE23,
     ProblemKeys keys = ProblemKeys()) {
   // Create a new ExtendedPoseParameterBlock for the IMU state
   std::shared_ptr<ExtendedPoseParameterBlock> nav_state_block =
-      std::make_shared<ExtendedPoseParameterBlock>(imu_state.navState(),
-                                                   state_rep, "extended_pose", direction);
+      std::make_shared<ExtendedPoseParameterBlock>(
+          imu_state.navState(), state_rep, "extended_pose", direction);
   std::shared_ptr<ParameterBlock<6>> bias_block =
       std::make_shared<ParameterBlock<6>>(imu_state.bias());
 
   graph.addState(keys.nav_state_key, imu_state.timestamp(), nav_state_block);
   graph.addState(keys.bias_state_key, imu_state.timestamp(), bias_block);
 };
 
-void addPriorFactor(ceres_nav::FactorGraph &graph,
+void addPriorFactor(FactorGraph &graph,
                     const IMUState prior_imu_state,
                     const Eigen::Matrix<double, 15, 15> &prior_covariance,
                     LieDirection direction,
                     ExtendedPoseRepresentation state_rep, ProblemKeys keys) {
-  std::vector<ceres_nav::StateID> state_ids = {
-      ceres_nav::StateID(keys.nav_state_key, prior_imu_state.timestamp()),
-      ceres_nav::StateID(keys.bias_state_key, prior_imu_state.timestamp())};
+  std::vector<StateID> state_ids = {
+      StateID(keys.nav_state_key, prior_imu_state.timestamp()),
+      StateID(keys.bias_state_key, prior_imu_state.timestamp())};
 
   auto *factor =
       new IMUPriorFactor(prior_imu_state.navState(), prior_imu_state.bias(),
@@ -64,13 +66,13 @@ void addPreintegrationFactor(ceres_nav::FactorGraph &graph,
                              ProblemKeys keys = ProblemKeys()) {
   double start_stamp = imu_increment.start_stamp;
   double end_stamp = imu_increment.end_stamp;
-  std::vector<ceres_nav::StateID> state_ids = {
-      ceres_nav::StateID(keys.nav_state_key, start_stamp),
-      ceres_nav::StateID(keys.bias_state_key, start_stamp),
-      ceres_nav::StateID(keys.nav_state_key, end_stamp),
-      ceres_nav::StateID(keys.bias_state_key, end_stamp)};
+  std::vector<StateID> state_ids = {
+      StateID(keys.nav_state_key, start_stamp),
+      StateID(keys.bias_state_key, start_stamp),
+      StateID(keys.nav_state_key, end_stamp),
+      StateID(keys.bias_state_key, end_stamp)};
 
-  auto *factor = new IMUPreintegrationFactor(imu_increment, false, direction, state_rep);
+  auto *factor = new IMUPreintegrationFactor(imu_increment, false);
   graph.addFactor(state_ids, factor, start_stamp);
 }
 
@@ -82,12 +84,13 @@ void addGPSFactor(
     const LieDirection &direction, const Eigen::Matrix3d &covariance,
     ExtendedPoseRepresentation state_rep = ExtendedPoseRepresentation::SE23,
     ProblemKeys keys = ProblemKeys()) {
-  Eigen::Matrix3d sqrt_info = ceres_nav::computeSquareRootInformation(covariance);
+  Eigen::Matrix3d sqrt_info =
+      ceres_nav::computeSquareRootInformation(covariance);
 
-  std::vector<ceres_nav::StateID> state_ids = {
-      ceres_nav::StateID(keys.nav_state_key, gps_message.timestamp)};
-  auto factor =
-      new AbsolutePositionFactor(gps_message.measurement, direction, sqrt_info, state_rep);
+  std::vector<StateID> state_ids = {
+      StateID(keys.nav_state_key, gps_message.timestamp)};
+  auto factor = new AbsolutePositionFactor(gps_message.measurement, direction,
+                                           sqrt_info, state_rep);
   graph.addFactor(state_ids, factor, gps_message.timestamp);
 }
 
@@ -135,9 +138,9 @@ computeIMUCovariance(ceres_nav::FactorGraph &graph, double timestamp,
 
 void marginalizeIMUState(ceres_nav::FactorGraph &graph, double timestamp_marg,
                          ProblemKeys keys) {
-  std::vector<ceres_nav::StateID> state_ids_marg = {
-      ceres_nav::StateID(keys.nav_state_key, timestamp_marg),
-      ceres_nav::StateID(keys.bias_state_key, timestamp_marg)};
+  std::vector<StateID> state_ids_marg = {
+      StateID(keys.nav_state_key, timestamp_marg),
+      StateID(keys.bias_state_key, timestamp_marg)};
 
   graph.marginalizeStates(state_ids_marg);
 }

examples/include/GPSIMUExampleUtils.h

@@ -219,7 +219,7 @@ std::vector<IMUState> loadIMUStates(const std::string &fname) {
     Eigen::Vector3d accel_bias{values[14], values[15], values[16]};
 
     Eigen::Matrix<double, 5, 5> nav_state =
-        SE23::fromComponents(C_ab, velocity, position);
+        ceres_nav::SE23::fromComponents(C_ab, velocity, position);
     imu_states.push_back(IMUState(nav_state, gyro_bias, accel_bias, stamp));
   }
 
@@ -233,9 +233,9 @@ void propagateIMUState(IMUState &state, const IMUMessage &imu_msg, const Eigen::
   Eigen::Vector3d unbiased_gyro = imu_msg.gyro - state.gyroBias();
   Eigen::Vector3d unbiased_accel = imu_msg.accel - state.accelBias();
 
-  Eigen::Matrix<double, 5, 5> G = createGMatrix(gravity, dt);
+  Eigen::Matrix<double, 5, 5> G = ceres_nav::createGMatrix(gravity, dt);
   Eigen::Matrix<double, 5, 5> U =
-      createUMatrix(unbiased_gyro, unbiased_accel, dt);
+      ceres_nav::createUMatrix(unbiased_gyro, unbiased_accel, dt);
 
   Eigen::Matrix<double, 5, 5> prev_extended_pose = state.navState();
   Eigen::Matrix<double, 5, 5> next_extended_pose = G * prev_extended_pose * U;

include/factors/AbsolutePositionFactor.h

@@ -7,6 +7,8 @@
 #include "lib/ExtendedPoseParameterBlock.h"
 #include "lie/LieDirection.h"
 
+namespace ceres_nav {
+
 class AbsolutePositionFactor : public ceres::SizedCostFunction<3, 15> {
 public:
   Eigen::Vector3d meas;
@@ -29,3 +31,5 @@ class AbsolutePositionFactor : public ceres::SizedCostFunction<3, 15> {
   bool Evaluate(double const *const *parameters, double *residuals,
                 double **jacobians) const override;
 };
+
+} // namespace ceres_nav

include/factors/IMUPreintegrationFactor.h

@@ -7,6 +7,7 @@
 #include "imu/IMUIncrement.h"
 #include "imu/IMUPreintegrationHelper.h"
 
+namespace ceres_nav {
 class IMUPreintegrationFactor
     : public ceres::SizedCostFunction<15, 15, 6, 15, 6> {
 public:
@@ -29,9 +30,7 @@ class IMUPreintegrationFactor
    * time, and a continuous-time noise matrix.
    */
   IMUPreintegrationFactor(
-      IMUIncrement imu_increment_, bool use_group_jacobians_,
-      const LieDirection &direction_,
-      ExtendedPoseRepresentation pose_rep_ = ExtendedPoseRepresentation::SE23);
+      const IMUIncrement &imu_increment_, bool use_group_jacobians_);
 
   IMUPreintegrationFactor() = delete;
   /**
@@ -40,3 +39,5 @@ class IMUPreintegrationFactor
   virtual bool Evaluate(double const *const *parameters, double *residuals,
                         double **jacobians) const;
 };
+
+} // namespace ceres_nav

include/factors/IMUPriorFactor.h

@@ -5,6 +5,8 @@
 #include <Eigen/Dense>
 #include <ceres/ceres.h>
 
+namespace ceres_nav {
+
 class IMUPriorFactor : public ceres::SizedCostFunction<15, 15, 6> {
 public:
   /**
@@ -32,4 +34,6 @@ class IMUPriorFactor : public ceres::SizedCostFunction<15, 15, 6> {
   Eigen::Matrix<double, 15, 15> sqrt_info_;
   LieDirection direction_;
   ExtendedPoseRepresentation pose_rep_;
-};
+};
+
+}

include/factors/RelativeLandmarkFactor.h

@@ -6,6 +6,7 @@
 
 #include "lib/ExtendedPoseParameterBlock.h"
 
+namespace ceres_nav {
 /**
  * @brief A factor for relative landmark measurements of the form
  *    y = C_ab.T * (r_pw_a - r_zw_a),
@@ -40,4 +41,6 @@ class RelativeLandmarkFactor : public ceres::SizedCostFunction<3, 15> {
    */
   bool Evaluate(double const *const *parameters, double *residuals,
                 double **jacobians) const;
-};
+};
+
+} // namespace ceres_nav

include/factors/RelativePoseFactor.h

@@ -5,6 +5,8 @@
 
 #include "lie/LieDirection.h"
 
+namespace ceres_nav {
+
 class RelativePoseFactor : public ceres::SizedCostFunction<6, 12, 12> {
 public:
   Eigen::Matrix4d relative_pose_meas;
@@ -21,4 +23,6 @@ class RelativePoseFactor : public ceres::SizedCostFunction<6, 12, 12> {
    */
   virtual bool Evaluate(double const *const *parameters, double *residuals,
                         double **jacobians) const;
-};
+};
+
+} // namespace ceres_nav

include/imu/IMUHelper.h

@@ -2,17 +2,18 @@
 
 /*
  * Some helper functions for IMU preintegration.
-*/
+ */
 
 #include <Eigen/Dense>
 #include <vector>
 
+namespace ceres_nav {
 class IMUIncrement;
 class IMU;
 
 Eigen::Matrix<double, 5, 5> createGMatrix(const Eigen::Vector3d &gravity,
                                           double dt);
-                                          
+
 Eigen::Matrix3d createNMatrix(const Eigen::Vector3d &phi_vec);
 Eigen::Matrix<double, 5, 5> createUMatrix(const Eigen::Vector3d &omega,
                                           const Eigen::Vector3d &accel,
@@ -35,4 +36,5 @@ std::vector<IMU> getIMUBetweenTimes(const double &stamp_i,
                                     const std::vector<IMU> &imu_meas_vec);
 bool preintegrateBetweenTimes(IMUIncrement &rmi, const double &stamp_i,
                               const double &stamp_j,
-                              const std::vector<IMU> &imu_meas_vec);
+                              const std::vector<IMU> &imu_meas_vec);
+} // namespace ceres_name

include/imu/IMUIncrement.h

@@ -5,6 +5,8 @@
 #include <Eigen/Dense>
 #include <vector>
 
+namespace ceres_nav {
+
 class IMUIncrement {
 public:
   // Initial gyro and accel bias
@@ -90,3 +92,5 @@ class IMUIncrement {
       const Eigen::Vector3d &accel, Eigen::Matrix<double, 15, 15> &A_ct,
       Eigen::Matrix<double, 15, 12> &L_ct);
 };
+
+} // namespace ceres_nav

include/imu/IMUPreintegrationHelper.h

@@ -5,6 +5,7 @@
 
 #include "imu/IMUIncrement.h"
 
+namespace ceres_nav {
 /**
  * @brief Simple struct to hold the IMU state components.
  */
@@ -34,9 +35,7 @@ struct IMUStateHolder {
 class IMUPreintegrationHelper {
 public:
   IMUPreintegrationHelper(const IMUIncrement &imu_increment,
-                          bool use_group_jacobians,
-                          const LieDirection &direction,
-                          ExtendedPoseRepresentation pose_rep);
+                          bool use_group_jacobians);
 
   // Main method to compute Jacobians based on representation and direction
   std::vector<Eigen::Matrix<double, 15, 15>>
@@ -68,7 +67,7 @@ class IMUPreintegrationHelper {
 
   // Gets the covariance of the preintegrated measurement
   Eigen::Matrix<double, 15, 15> covariance() const { return rmi.covariance; }
-  
+
   double startStamp() const { return rmi.start_stamp; }
   double endStamp() const { return rmi.end_stamp; }
 
@@ -77,4 +76,6 @@ class IMUPreintegrationHelper {
   bool use_group_jacobians;
   LieDirection direction;
   ExtendedPoseRepresentation pose_rep;
-};
+};
+
+} // namespace ceres_nav

include/lib/ExtendedPoseParameterBlock.h

@@ -10,6 +10,8 @@
 #include "local_parameterizations/ExtendedPoseLocalParameterization.h"
 #include "local_parameterizations/SE23LocalParameterization.h"
 
+namespace ceres_nav {
+
 enum class ExtendedPoseRepresentation { SE23, Decoupled };
 
 /**
@@ -117,4 +119,6 @@ class ExtendedPoseParameterBlock : public ParameterBlock<15, 9> {
 
 protected:
   LieDirection direction_;
-};
+};
+
+} // namespace ceres_nav

include/lib/ParameterBlock.h

@@ -3,6 +3,8 @@
 #include "ParameterBlockBase.h"
 #include <Eigen/Dense>
 
+namespace ceres_nav {
+
 /**
  * @brief Templated parameter block for optimization in Ceres.
  *
@@ -81,4 +83,6 @@ class ParameterBlock : public ParameterBlockBase {
   // Storage for the parameter estimate and covariance
   Eigen::Matrix<double, Dim, 1> estimate_;
   Eigen::Matrix<double, MinimalDim, MinimalDim> covariance_;
-};
+};
+
+} // namespace ceres_nav

include/lib/ParameterBlockBase.h

@@ -4,6 +4,7 @@
 #include <ceres/ceres.h>
 #include <memory>
 
+namespace ceres_nav {
 /**
  * @brief Abstract base class for parameter blocks in
  * Ceres.
@@ -70,4 +71,6 @@ class ParameterBlockBase {
 
   // Local parameterization for this parameter block
   ceres::LocalParameterization *local_parameterization_ptr_;
-};
+};
+
+} // namespace ceres_nav