Use mask only to determine tangent space in covariance calculation

Jmeyer1292 · Mar 4, 2022 · c3ee0c2 · c3ee0c2
1 parent 66c15a5
commit c3ee0c2
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 17 deletions.
diff --git a/rct_optimizations/include/rct_optimizations/covariance_analysis.h b/rct_optimizations/include/rct_optimizations/covariance_analysis.h
@@ -38,7 +38,6 @@ Eigen::MatrixXd computeCorrelationsFromCovariance(const Eigen::MatrixXd& covaria
 
 /**
  * @brief Compute all covariance results for a Ceres optimization problem. Labels results with generic names.
- * @details This excludes all parameters set to be constant during the solve
  * @param problem The Ceres problem (after optimization).
  * @param param_masks Map of the parameter block pointer and the indices of the parameters within that block to be excluded from the covariance calculation
  * @param options ceres::Covariance::Options to use when calculating covariance.
@@ -50,7 +49,6 @@ CovarianceResult computeCovariance(ceres::Problem &problem,
 
 /**
  * @brief Compute covariance results for the specified parameter blocks in a Ceres optimization problem. Labels results with generic names.
- * @details This excludes all parameters set to be constant during the solve
  * @param problem The Ceres problem (after optimization).
  * @param parameter_blocks Specific parameter blocks to compute covariance between.
  * @param param_masks Map of the parameter block pointer and the indices of the parameters within that block to be excluded from the covariance calculation
@@ -64,7 +62,6 @@ CovarianceResult computeCovariance(ceres::Problem &problem,
 
 /**
  * @brief Compute all covariance results for a Ceres optimization problem and label them with the provided names.
- * @details This excludes all parameters set to be constant during the solve
  * @param problem The Ceres problem (after optimization).
  * @param parameter_names Labels for all optimization parameters in the problem.
  * @param param_masks Map of the parameter block pointer and the indices of the parameters within that block to be excluded from the covariance calculation
@@ -78,7 +75,6 @@ CovarianceResult computeCovariance(ceres::Problem &problem,
 
 /**
  * @brief Compute covariance results for specified parameter blocks in a Ceres optimization problem and label them with the provided names.
- * @details This excludes all parameters set to be constant during the solve
  * @param problem The Ceres problem (after optimization).
  * @param parameter_blocks Specific parameter blocks for which covariance will be calculated.
  * @param parameter_names Labels for optimization parameters in the specified blocks.

diff --git a/rct_optimizations/src/rct_optimizations/covariance_analysis.cpp b/rct_optimizations/src/rct_optimizations/covariance_analysis.cpp
@@ -158,21 +158,18 @@ CovarianceResult computeCovariance(ceres::Problem &problem,
     }
 
     // Extract tangent space
-    if (!problem.IsParameterBlockConstant(const_cast<double*>(b)))
-    {
-      std::vector<int> masks;
-      auto it = param_masks.find(b);
-      if (it != param_masks.end())
-        masks = it->second;
+    std::vector<int> masks;
+    auto it = param_masks.find(b);
+    if (it != param_masks.end())
+      masks = it->second;
 
-      const std::vector<std::string>& label = param_names.at(b);
-      for (std::size_t i = 0; i < static_cast<std::size_t>(block_size); ++i)
+    const std::vector<std::string>& label = param_names.at(b);
+    for (std::size_t i = 0; i < static_cast<std::size_t>(block_size); ++i)
+    {
+      if (std::find(masks.begin(), masks.end(), i) == masks.end())
       {
-        if (std::find(masks.begin(), masks.end(), i) == masks.end())
-        {
-          tangent_space_indices.push_back(n_params_in_selected + static_cast<Eigen::Index>(i));
-          tangent_space_labels.push_back(label.at(i));
-        }
+        tangent_space_indices.push_back(n_params_in_selected + static_cast<Eigen::Index>(i));
+        tangent_space_labels.push_back(label.at(i));
       }
     }