Add method to compute covariance for multiple StateIDs at once

examples/include/FactorGraphUtils.h

@@ -113,13 +113,13 @@ IMUState getIMUState(ceres_nav::FactorGraph &graph, double timestamp,
 Eigen::Matrix<double, 15, 15>
 computeIMUCovariance(ceres_nav::FactorGraph &graph, double timestamp,
                      ProblemKeys keys) {
-  bool success_ext_pose =
-      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();
+  std::vector<StateID> state_ids = {StateID(keys.nav_state_key, timestamp),
+                                    StateID(keys.bias_state_key, timestamp)};
+  bool success = graph.computeCovariance(state_ids);
+  if (!success) {
+    LOG(ERROR) << "Failed to compute covariance for IMU state at timestamp: "
+               << timestamp;
+    return Eigen::Matrix<double, 15, 15>::Zero();
   }
 
   // Assemble covariance and return

include/lib/Covariance.h

@@ -1,16 +1,21 @@
 #pragma once
 
+#include "lib/StateId.h"
 #include <string>
+#include <vector>
 
 // Forward declarations
+namespace ceres_nav {
 class StateCollection;
+}
+
 namespace ceres {
 class Problem;
 }
 
-namespace ceres_nav {
-class StateID;
-}
+// namespace ceres_nav {
+// class StateID;
+// }
 
 namespace ceres_nav {
 
@@ -23,4 +28,12 @@ namespace ceres_nav {
  */
 bool calculateCovariance(ceres::Problem &graph, StateCollection &states,
                          const StateID &state_id);
+
+/**
+ * @brief Computes the covariance for a given set of states in the
+ * StateCollection using the provided Ceres Problem.
+ */
+bool calculateCovariance(ceres::Problem &graph, StateCollection &states,
+                         const std::vector<StateID> &state_ids);
+
 }; // namespace ceres_nav

include/lib/FactorGraph.h

@@ -139,6 +139,7 @@ class FactorGraph {
    * a particular timestamp.
    */
   bool computeCovariance(const StateID &state_id);
+  bool computeCovariance(const std::vector<StateID> &state_ids); 
 
   /**
    * @brief Gets the marginalization information for a set of states.

include/lib/StateId.h

@@ -1,5 +1,6 @@
 #pragma once
 
+#include <cmath>
 #include <optional>
 #include <string>
 

src/lib/Covariance.cpp

@@ -1,6 +1,5 @@
 #include "lib/Covariance.h"
 #include "lib/StateCollection.h"
-#include "lib/StateId.h"
 
 #include <ceres/ceres.h>
 #include <glog/logging.h>
@@ -10,73 +9,82 @@
 namespace ceres_nav {
 bool calculateCovariance(ceres::Problem &graph, StateCollection &states,
                          const StateID &state_id) {
+  const std::vector<StateID> state_ids = {state_id};
+  return calculateCovariance(graph, states, state_ids);
+}
+
+bool calculateCovariance(ceres::Problem &graph, StateCollection &states,
+                         const std::vector<StateID> &state_ids) {
+
+  std::vector<const double *> parameter_block_ptrs;
+  for (auto const &state_id : state_ids) {
+    if (!states.hasState(state_id)) {
+      LOG(ERROR) << "State with ID: " << state_id.toString()
+                 << " does not exist in the state collection.";
+      return false;
+    }
 
-  // Check if the state exists in the collection
-  if (!states.hasState(state_id)) {
-    LOG(ERROR) << "State with ID: " << state_id.toString()
-               << " does not exist in the state collection.";
-    return false;
+    parameter_block_ptrs.push_back(
+        states.getState(state_id)->estimatePointer());
   }
 
-  // Create a covariance object
+  // Create the covariance object
   ceres::Covariance::Options cov_options;
   cov_options.num_threads =
       static_cast<int>(std::thread::hardware_concurrency());
   cov_options.apply_loss_function = true;
   cov_options.algorithm_type = ceres::CovarianceAlgorithmType::SPARSE_QR;
   ceres::Covariance covariance(cov_options);
 
-  std::vector<const double *> parameter_block_ptrs;
-  parameter_block_ptrs.push_back(states.getState(state_id)->estimatePointer());
-
   // Try with sparse QR first
   if (covariance.Compute(parameter_block_ptrs, &graph)) {
-    if (states.getState(state_id)->getLocalParameterizationPointer() ==
-        nullptr) {
-      covariance.GetCovarianceBlock(
-          states.getState(state_id)->estimatePointer(),
-          states.getState(state_id)->estimatePointer(),
-          states.getState(state_id)->getCovariancePointer());
-    } else {
-      covariance.GetCovarianceBlockInTangentSpace(
-          states.getState(state_id)->estimatePointer(),
-          states.getState(state_id)->estimatePointer(),
-          states.getState(state_id)->getCovariancePointer());
+    for (auto const &state_id : state_ids) {
+      if (states.getState(state_id)->getLocalParameterizationPointer() ==
+          nullptr) {
+        covariance.GetCovarianceBlock(
+            states.getState(state_id)->estimatePointer(),
+            states.getState(state_id)->estimatePointer(),
+            states.getState(state_id)->getCovariancePointer());
+      } else {
+        covariance.GetCovarianceBlockInTangentSpace(
+            states.getState(state_id)->estimatePointer(),
+            states.getState(state_id)->estimatePointer(),
+            states.getState(state_id)->getCovariancePointer());
+      }
     }
     return true;
   } else {
-    LOG(ERROR) << "Sparse QR covariance computation failed for state: "
-               << state_id.toString();
+    LOG(ERROR) << "Sparse QR covariance computation failed for states.";
     if (!graph.NumParameterBlocks() > 100) {
-      LOG(ERROR) << "Covariance computation of " << state_id.toString()
-                 << " failed. No covariance computed!";
+      LOG(ERROR) << "Covariance computation failed. No covariance computed!";
       return false;
     }
 
-    LOG(ERROR) << "Jacobian related to state " << state_id.toString()
-               << " is not full rank. Computing with SVD...";
+    LOG(ERROR) << "Jacobian related to the requested states is not full rank. "
+                  "Computing with SVD...";
     cov_options.algorithm_type = ceres::CovarianceAlgorithmType::DENSE_SVD;
     cov_options.null_space_rank = -1;
     ceres::Covariance covariance_svd(cov_options);
 
     // Try to compute again
     if (covariance_svd.Compute(parameter_block_ptrs, &graph)) {
-      if (states.getState(state_id)->getLocalParameterizationPointer() ==
-          nullptr) {
-        covariance_svd.GetCovarianceBlock(
-            states.getState(state_id)->estimatePointer(),
-            states.getState(state_id)->estimatePointer(),
-            states.getState(state_id)->getCovariancePointer());
-      } else {
-        covariance_svd.GetCovarianceBlockInTangentSpace(
-            states.getState(state_id)->estimatePointer(),
-            states.getState(state_id)->estimatePointer(),
-            states.getState(state_id)->getCovariancePointer());
+      for (auto const &state_id : state_ids) {
+        if (states.getState(state_id)->getLocalParameterizationPointer() ==
+            nullptr) {
+          covariance_svd.GetCovarianceBlock(
+              states.getState(state_id)->estimatePointer(),
+              states.getState(state_id)->estimatePointer(),
+              states.getState(state_id)->getCovariancePointer());
+        } else {
+          covariance_svd.GetCovarianceBlockInTangentSpace(
+              states.getState(state_id)->estimatePointer(),
+              states.getState(state_id)->estimatePointer(),
+              states.getState(state_id)->getCovariancePointer());
+        }
       }
       return true;
     } else {
-      LOG(ERROR) << "Failed to compute covariance for state: "
-                 << state_id.toString();
+      LOG(ERROR) << "Failed to compute covariance for the requested states.";
       return false;
     }
   }

src/lib/FactorGraph.cpp

@@ -1,8 +1,8 @@
 #include "lib/FactorGraph.h"
 
-#include "lib/ParameterBlockBase.h"
 #include "lib/Covariance.h"
 #include "lib/Marginalization.h"
+#include "lib/ParameterBlockBase.h"
 
 #include "utils/VectorMath.h"
 #include "utils/VectorTypes.h"
@@ -234,6 +234,10 @@ bool FactorGraph::computeCovariance(const StateID &state_id) {
   return calculateCovariance(problem_, states_, state_id);
 }
 
+bool FactorGraph::computeCovariance(const std::vector<StateID> &state_ids) {
+  return calculateCovariance(problem_, states_, state_ids);
+}
+
 bool FactorGraph::marginalizeStates(std::vector<StateID> states_m) {
   std::map<StateID, Eigen::VectorXd> empty_map;
   return marginalizeStates(states_m, empty_map);