Modify StateID to handle timestamp rounding

examples/CMakeLists.txt

@@ -9,7 +9,7 @@ set(sources
     include/GPSIMUExampleUtils.h
 )
 
-# # Create executable
+# Create executable
 add_executable(gps_imu_example ${sources})
 target_include_directories(gps_imu_example PRIVATE include)
 

examples/gps_imu_example.cpp

@@ -126,7 +126,7 @@ void runSlidingWindowEstimator(
                                      R_gps, state_rep, keys);
     // If we've reached the window size, optimize the graph and marginalize the
     // oldest state
-    if (graph.getStates().getNumStatesForType(keys.nav_state_key) >=
+    if (graph.getStates().getNumberOfStatesForType(keys.nav_state_key) >=
         window_size) {
       graph.solve(options);
       ceres::Solver::Summary summary = graph.getSolverSummary();

examples/include/FactorGraphUtils.h

@@ -29,8 +29,7 @@ struct ProblemKeys {
 };
 
 void addIMUState(
-    FactorGraph &graph, const IMUState &imu_state,
-    const LieDirection direction,
+    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
@@ -40,12 +39,13 @@ void addIMUState(
   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);
+  StateID nav_state_id(keys.nav_state_key, imu_state.timestamp());
+  StateID bias_state_id(keys.bias_state_key, imu_state.timestamp());
+  graph.addState(nav_state_id, nav_state_block);
+  graph.addState(bias_state_id, bias_block);
 };
 
-void addPriorFactor(FactorGraph &graph,
-                    const IMUState prior_imu_state,
+void addPriorFactor(FactorGraph &graph, const IMUState prior_imu_state,
                     const Eigen::Matrix<double, 15, 15> &prior_covariance,
                     LieDirection direction,
                     ExtendedPoseRepresentation state_rep, ProblemKeys keys) {
@@ -66,11 +66,10 @@ 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<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)};
+  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);
   graph.addFactor(state_ids, factor, start_stamp);
@@ -97,11 +96,11 @@ void addGPSFactor(
 IMUState getIMUState(ceres_nav::FactorGraph &graph, double timestamp,
                      ProblemKeys keys = ProblemKeys()) {
   std::shared_ptr<ExtendedPoseParameterBlock> nav_state =
-      graph.getStates().getState<ExtendedPoseParameterBlock>(keys.nav_state_key,
-                                                             timestamp);
+      graph.getStates().getState<ExtendedPoseParameterBlock>(
+          StateID(keys.nav_state_key, timestamp));
   std::shared_ptr<ParameterBlock<6>> bias =
-      graph.getStates().getState<ParameterBlock<6>>(keys.bias_state_key,
-                                                    timestamp);
+      graph.getStates().getState<ParameterBlock<6>>(
+          StateID(keys.bias_state_key, timestamp));
   if (!nav_state || !bias) {
     throw std::runtime_error("IMU state not found for timestamp: " +
                              std::to_string(timestamp));
@@ -115,8 +114,9 @@ Eigen::Matrix<double, 15, 15>
 computeIMUCovariance(ceres_nav::FactorGraph &graph, double timestamp,
                      ProblemKeys keys) {
   bool success_ext_pose =
-      graph.computeCovariance(keys.nav_state_key, timestamp);
-  bool success_bias = graph.computeCovariance(keys.bias_state_key, timestamp);
+      graph.computeCovariance(StateID(keys.nav_state_key, timestamp));
+  bool success_bias =
+      graph.computeCovariance(StateID(keys.bias_state_key, timestamp));
 
   if (!success_ext_pose || !success_bias) {
     return Eigen::Matrix<double, 15, 15>::Identity();
@@ -125,13 +125,13 @@ computeIMUCovariance(ceres_nav::FactorGraph &graph, double timestamp,
   // Assemble covariance and return
   Eigen::Matrix<double, 15, 15> covariance =
       Eigen::Matrix<double, 15, 15>::Zero();
-  covariance.block<9, 9>(0, 0) =
-      graph.getStates()
-          .getState<ExtendedPoseParameterBlock>(keys.nav_state_key, timestamp)
-          ->getCovariance();
+  covariance.block<9, 9>(0, 0) = graph.getStates()
+                                     .getState<ExtendedPoseParameterBlock>(
+                                         StateID(keys.nav_state_key, timestamp))
+                                     ->getCovariance();
   covariance.block<6, 6>(9, 9) =
       graph.getStates()
-          .getState<ParameterBlock<6>>(keys.bias_state_key, timestamp)
+          .getState<ParameterBlock<6>>(StateID(keys.bias_state_key, timestamp))
           ->getCovariance();
   return covariance;
 }
@@ -143,8 +143,8 @@ void marginalizeIMUState(ceres_nav::FactorGraph &graph, double timestamp_marg,
       StateID(keys.bias_state_key, timestamp_marg)};
 
   graph.marginalizeStates(state_ids_marg);
-  // FactorGraph::LastMarginalizationInfo marg_info = graph.getLastMarginalizationInfo();
-  // marg_info.print();
+  // FactorGraph::LastMarginalizationInfo marg_info =
+  // graph.getLastMarginalizationInfo(); marg_info.print();
 }
 
 } // namespace factor_graph_utils

examples/python/run_gps_imu_fusion.py

@@ -444,8 +444,8 @@ def evaluate_imu_states(
     )
     executable_path = os.path.join(cur_dir, "../../build/examples/gps_imu_example")
 
-    config.lie_direction = "right"
-    config.state_representation = "decoupled"
+    config.lie_direction = "left" # left or right
+    config.state_representation = "SE23" # SE23 or decoupled
 
     # Generate data an run the example
     data_fpaths = generate_and_save_data(config, save_dir)

include/lib/Covariance.h

@@ -2,13 +2,25 @@
 
 #include <string>
 
-// Forward declarations 
+// Forward declarations
 class StateCollection;
 namespace ceres {
-    class Problem;
+class Problem;
 }
 
 namespace ceres_nav {
+class StateID;
+}
+
+namespace ceres_nav {
+
+/**
+ * @brief Computes the covariance for a given state in the StateCollection
+ * using the provided Ceres Problem.
+ *
+ * It is assumed that the state exists in both the StateCollection and the Ceres
+ * problem.
+ */
 bool calculateCovariance(ceres::Problem &graph, StateCollection &states,
-                         const std::string &key, double timestamp);
+                         const StateID &state_id);
 }; // namespace ceres_nav

include/lib/FactorGraph.h

@@ -16,6 +16,12 @@
 
 namespace ceres_nav {
 
+struct FactorInfo {
+  ceres::ResidualBlockId residual_block_id;
+  std::vector<StateID> connected_states;
+  double timestamp;
+};
+
 class FactorGraph {
 public:
   using StatePtr = std::shared_ptr<ParameterBlockBase>;
@@ -31,14 +37,11 @@ class FactorGraph {
   FactorGraph(ceres::Solver::Options solver_options);
 
   /**
-   * @brief Adds a state to the problem with a particular name and
-   * timestamp
-   */
-  void addState(const std::string &name, double timestamp,
-                std::shared_ptr<ParameterBlockBase> state);
-
-  /**
-   * @brief Adds a state to the problem with a particular StateID
+   * @brief Adds a state to the problem with a particular StateID.
+   *
+   * @param state_id The StateID for the state to add
+   * @param state A shared pointer to the ParameterBlockBase object containing
+   * the estimate of the state.
    */
   void addState(const StateID &state_id,
                 std::shared_ptr<ParameterBlockBase> state);
@@ -56,7 +59,22 @@ class FactorGraph {
                  ceres::LossFunction *loss_function = nullptr);
 
   /**
-   * @brief Solves the optimization problem using the current solver options.
+   * @brief Adds a factor to the problem, and additionally returns
+   * information about the added factor via the FactorInfo struct.
+   */
+  bool addFactor(const std::vector<StateID> &state_ids,
+                 ceres::CostFunction *cost_function, double stamp,
+                 FactorInfo &info,
+                 ceres::LossFunction *loss_function = nullptr);
+
+  /**
+   * @brief Removes a factor from the problem given its FactorInfo.
+   */
+  bool removeFactor(const FactorInfo &info);
+
+  /**
+   * @brief Solves the optimization problem using the current solver
+   * options.
    */
   void solve();
 
@@ -65,7 +83,9 @@ class FactorGraph {
    */
   void solve(ceres::Solver::Options Options);
 
-  /** Get information about the internal Ceres problem. */
+  /**
+   * @brief Gets the
+   */
   bool getStatePointers(const std::vector<StateID> &StateIDs,
                         std::vector<double *> &state_ptrs) const;
   /**
@@ -86,30 +106,10 @@ class FactorGraph {
                        std::vector<ceres::ResidualBlockId> &factors_m,
                        std::vector<ceres::ResidualBlockId> &factors_r) const;
 
-  /**
-   * @brief Removes a timestamped state from the problem.
-   */
-  void removeState(const std::string &name, double timestamp);
-
-  /**
-   * @brief Removes a state from the problem given a StateID.
-   */
-  void removeState(const StateID &state_id);
-
-  /**
-   * @brief Sets a state as constant in the optimization problem.
-   */
-  void setConstant(const std::string &name, double timestamp);
-
-  /**
-   * @brief Checks if a state is constant in the optimization problem.
-   */
-  bool isConstant(const std::string &name, double timestamp);
-
-  /**
-   * @brief Sets a state as variable in the optimization problem.
-   */
-  void setVariable(const std::string &name, double timestamp);
+  // Control whether a state is constant or variable
+  void setConstant(const StateID &state_id);
+  void setVariable(const StateID &state_id);
+  bool isConstant(const StateID &state_id) const;
 
   /**
    * @brief Marginalizes out a set of states from the problem
@@ -126,11 +126,19 @@ class FactorGraph {
       std::vector<StateID> states_m,
       const std::map<StateID, Eigen::VectorXd> &linearization_points);
 
+  /**
+   * @brief Directly removes a state from the problem given a StateID.
+   *
+   * WARNING: this does not properly marginalize out the state, and should be
+   * used with caution!
+   */
+  void removeState(const StateID &state_id);
+
   /**
    * @brief Computes the covariance of a state with a given name at
    * a particular timestamp.
    */
-  bool computeCovariance(const std::string &name, double timestamp);
+  bool computeCovariance(const StateID &state_id);
 
   /**
    * @brief Gets the marginalization information for a set of states.