Rename covariance functions for clarity and fix errors in docs

rct_optimizations/include/rct_optimizations/covariance_analysis.h

@@ -9,26 +9,35 @@ namespace rct_optimizations
 {
 
 /**
- * @brief Calculate the covariance matrix given a double array covariance.
+ * @brief Calculate standard deviations and correlation coefficients given a double array covariance. Each diagonal element is the standard deviation of
+ * a parameter, and its units match the units of that parameter. Each off-diagonal element is the correlation coefficients of a pair of parameters, which
+ * is unitless and in the range [-1, 1].
  * @param cov Points to a double array containing covariance results from Ceres.
  * @param num_vars Number of elements in cov.
- * @return A square matrix with size (num_vars x num_vars) containing variance (diagonal elements) and covariance (off-diagonal elements).
+ * @return A square matrix with size (num_vars x num_vars) containing standard deviations (diagonal elements) and correlation coefficients (off-diagonal elements).
  */
-Eigen::MatrixXd diagScaledCovarianceToMatrix(double* cov, std::size_t num_vars);
+Eigen::MatrixXd covToEigenCorr(double* cov, std::size_t num_vars);
 
-Eigen::MatrixXd diagCovarianceToMatrix(double* cov, std::size_t num_vars);
+/**
+ * @brief Arrange the covariance results from Ceres as an Eigen matrix.
+ * @param cov Points to a double array containing covariance results from Ceres.
+ * @param num_vars Number of elements in cov.
+ * @return A square matrix with size (num_vars x num_vars) containing variance (diagonal elements) and covariance (off-diagonal elements).
+ */
+Eigen::MatrixXd covToEigenCov(double* cov, std::size_t num_vars);
 
 
 /**
- * @brief Calculate the off-diagonal covariance matrix given covariances.
+ * @brief Calculate the off-diagonal correlation matrix given Ceres covariances.
  * @param cov_d1d1 Pointer to a double array containing covariance for the first parameter block.
  * @param num_vars1 Number of elements in cov_d1d1
  * @param cov_d2d2 Pointer to a double array containing covariance for the second parameter block.
  * @param num_vars2 Number of elements in cov_d1d2
- * @param cov_d1d2 Pointer to a double array containing covariance for the first parameter block relative to the second parameter block. Assumed to contain (num_vars1 x num_vars2 elements).
+ * @param cov_d1d2 Pointer to a double array containing correlation coefficients for the first parameter block relative to the second parameter block.
+ * Assumed to contain (num_vars1 x num_vars2 elements).
  * @return A matrix with size (num_vars1 x num_vars2) containing the off-diagonal covariance.
  */
-Eigen::MatrixXd offDiagCovarianceToMatrix(double* cov_d1d1, std::size_t num_vars1, double* cov_d2d2, std::size_t num_vars2, double* cov_d1d2);
+Eigen::MatrixXd covToEigenOffDiagCorr(double* cov_d1d1, std::size_t num_vars1, double* cov_d2d2, std::size_t num_vars2, double* cov_d1d2);
 
 /**
  * @brief Compute variance and covariance for a given problem and Pose6d parameter.
@@ -58,11 +67,11 @@ Eigen::MatrixXd computePose2DVCovariance(ceres::Problem &problem, Pose6d &pose,
 Eigen::MatrixXd computePose2PoseCovariance(ceres::Problem &problem, Pose6d &p1, Pose6d &p2);
 
 /**
- * @brief Compute variance and covariance for a given problem and parameter block.
+ * @brief Compute standard deviations and correlation coefficients for a given problem and parameter block.
  * @param problem The Ceres problem (after optimization).
  * @param dptr Ceres parameter block.
  * @param num_vars Number of parameters in dptr.
- * @return A square matrix with size (num_vars x num_vars) containing variance (diagonal elements) and covariance (off-diagonal elements).
+ * @return A square matrix with size (num_vars x num_vars) containing standard deviations (diagonal elements) and correlation coefficients (off-diagonal elements).
  */
 Eigen::MatrixXd computeDVCovariance(ceres::Problem &problem, double *dptr, std::size_t num_vars);
 

rct_optimizations/src/rct_optimizations/covariance_analysis.cpp

@@ -3,37 +3,33 @@
 namespace rct_optimizations
 {
 
-Eigen::MatrixXd diagScaledCovarianceToMatrix(double* cov, std::size_t num_vars)
+Eigen::MatrixXd covToEigenCorr(double* cov, std::size_t num_vars)
 {
   Eigen::MatrixXd out(num_vars, num_vars);
 
   for (std::size_t i = 0; i < num_vars; i++)
   {
-    double sigma_i = sqrt(fabs(cov[i * num_vars + i]));
+    double sigma_i = sqrt(fabs(cov[i * num_vars + i]));  // standard deviation at the diagonal element in row i
+
     for (std::size_t j = 0; j < num_vars; j++)
     {
-      double sigma_j = sqrt(fabs(cov[j * num_vars + j]));
-      double value;
+      double sigma_j = sqrt(fabs(cov[j * num_vars + j]));  // standard deviation at the diagonal element in column j
 
-      if (i == j)
-        value = sigma_i;
+      if (i == j)  // out(i,j) is a diagonal element
+        out(static_cast<Eigen::Index>(i), static_cast<Eigen::Index>(j)) = sigma_i;
       else
       {
-        if (sigma_i < std::numeric_limits<double>::epsilon())
-          sigma_i = 1;
+        if (sigma_i < std::numeric_limits<double>::epsilon()) sigma_i = 1;
+        if (sigma_j < std::numeric_limits<double>::epsilon()) sigma_j = 1;
 
-        if (sigma_j < std::numeric_limits<double>::epsilon())
-          sigma_j = 1;
-        value = cov[i * num_vars + j] / (sigma_i * sigma_j);
+        out(static_cast<Eigen::Index>(i), static_cast<Eigen::Index>(j)) = cov[i * num_vars + j] / (sigma_i * sigma_j);
       }
-
-      out(static_cast<Eigen::Index>(i), static_cast<Eigen::Index>(j)) = value;
     }
   }
   return out;
 }
 
-Eigen::MatrixXd diagCovarianceToMatrix(double* cov, std::size_t num_vars)
+Eigen::MatrixXd covToEigenCov(double* cov, std::size_t num_vars)
 {
   Eigen::MatrixXd out(num_vars, num_vars);
 
@@ -47,27 +43,23 @@ Eigen::MatrixXd diagCovarianceToMatrix(double* cov, std::size_t num_vars)
   return out;
 }
 
-Eigen::MatrixXd offDiagCovarianceToMatrix(double* cov_d1d1, std::size_t num_vars1, double* cov_d2d2, std::size_t num_vars2, double* cov_d1d2)
+Eigen::MatrixXd covToEigenOffDiagCorr(double* cov_d1d1, std::size_t num_vars1, double* cov_d2d2, std::size_t num_vars2, double* cov_d1d2)
 {
   Eigen::MatrixXd out(num_vars1, num_vars2);
 
   for (std::size_t i = 0; i < num_vars1; i++)
   {
-    double sigma_i = sqrt(fabs(cov_d1d1[i * num_vars1 + i]));
+    double sigma_i = sqrt(fabs(cov_d1d1[i * num_vars1 + i]));  // standard deviation at the diagonal element in row i of cov_d1d1
     for (std::size_t j = 0; j < num_vars2; j++)
     {
-      double sigma_j = sqrt(fabs(cov_d2d2[j * num_vars2 + j]));
+      double sigma_j = sqrt(fabs(cov_d2d2[j * num_vars2 + j]));  // standard deviation at the diagonal element in column j of cov_d2d2
 
-      if (sigma_i < std::numeric_limits<double>::epsilon())
-        sigma_i = 1;
-
-      if (sigma_j < std::numeric_limits<double>::epsilon())
-        sigma_j = 1;
+      if (sigma_i < std::numeric_limits<double>::epsilon()) sigma_i = 1;
+      if (sigma_j < std::numeric_limits<double>::epsilon()) sigma_j = 1;
 
       out(static_cast<Eigen::Index>(i), static_cast<Eigen::Index>(j)) = cov_d1d2[i * num_vars1 + j] / (sigma_i * sigma_j);
     }
   }
-
   return out;
 }
 
@@ -105,7 +97,7 @@ Eigen::MatrixXd computeDVCovariance(ceres::Problem &problem, double *dptr, std::
   if(!covariance.GetCovarianceBlock(dptr, dptr, cov))
     throw CovarianceException("GetCovarianceBlock failed in computeDVCovariance()");
 
-  return diagCovarianceToMatrix(cov, num_vars);
+  return covToEigenCorr(cov, num_vars);
 }
 
 Eigen::MatrixXd computeDV2DVCovariance(ceres::Problem &P, double* dptr1, std::size_t num_vars1, double* dptr2, std::size_t num_vars2)
@@ -130,7 +122,7 @@ Eigen::MatrixXd computeDV2DVCovariance(ceres::Problem &P, double* dptr1, std::si
        covariance.GetCovarianceBlock(dptr1, dptr2, cov_d1d2)))
     throw CovarianceException("GetCovarianceBlock failed in computeDV2DVCovariance()");
 
-  return offDiagCovarianceToMatrix(cov_d1d1, num_vars1, cov_d2d2, num_vars2, cov_d1d2);
+  return covToEigenOffDiagCorr(cov_d1d1, num_vars1, cov_d2d2, num_vars2, cov_d1d2);
 }
 
 }  // namespace rct_optimizations