WIP: Covariance with local parameterization

rct_optimizations/include/rct_optimizations/covariance_analysis.h

@@ -35,7 +35,7 @@ struct DefaultCovarianceOptions : ceres::Covariance::Options
  * ...|
  * a_n|
  */
-Eigen::MatrixXd covToEigenCorr(double* cov, std::size_t num_vars);
+Eigen::MatrixXd covToEigenCorr(const double* cov, const std::size_t num_vars);
 
 /**
  * @brief Arrange the Ceres covariance results as an Eigen matrix.
@@ -51,7 +51,7 @@ Eigen::MatrixXd covToEigenCorr(double* cov, std::size_t num_vars);
  * ...|
  * a_n|
  */
-Eigen::MatrixXd covToEigenCov(double* cov, std::size_t num_vars);
+Eigen::MatrixXd covToEigenCov(const double* cov, const std::size_t num_vars);
 
 
 /**
@@ -74,26 +74,30 @@ Eigen::MatrixXd covToEigenCov(double* cov, std::size_t num_vars);
  * ... |
  * a_n1|
  */
-Eigen::MatrixXd covToEigenOffDiagCorr(double* cov_d1d1, std::size_t num_vars1, double* cov_d2d2, std::size_t num_vars2, double* cov_d1d2);
+Eigen::MatrixXd covToEigenOffDiagCorr(const double* cov_d1d1, const std::size_t num_vars1, const double* cov_d2d2, const std::size_t num_vars2, const double* cov_d1d2);
 
 /**
- * @brief Compute variance and covariance for a given problem and Pose6d parameter. Uses @ref computeDVCovariance.
+ * @brief Compute variance and covariance for a given problem and Eigen quaternion and translation vector parameters.
+ * It assumed that the quaternion utilizes a Ceres local parameterization to reduce its values to 3 instead of 4.
+ * Uses @ref computeDVCovariance.
  * @param The Ceres problem (after optimization).
  * @param pose Pose6d parameter
  * @param options Options to use when calculating covariance. Use default if not explicitly set by user.
- * @return A square matrix with size (6 x 6) containing variance (diagonal elements) and covariance (off-diagonal elements).
- * Given that @ref pose contains the parameters [x, y, z, rx, ry, rz], the output matrix will be arranged like this:
- *   |x|y|z|rx|ry|rz
+ * @return A square matrix with size (6 x 6) containing variance (diagonal elements) and covariance (off-diagonal
+ * elements). Given that @ref pose contains the parameters [x, y, z, rx, ry, rz], the output matrix will be arranged
+ * like this: |x|y|z|rx|ry|rz
  * --|--------------
  * x |
  * y |
  * z |
  * rx|
  * ry|
  * rz|
- * @throw CovarianceException if computation of covariance fails for any pair of parameter blocks, or if GetCovarianceBlock returns false.
+ * @throw CovarianceException if computation of covariance fails for any pair of parameter blocks, or if
+ * GetCovarianceBlock returns false.
  */
-Eigen::MatrixXd computePoseCovariance(ceres::Problem& problem, Pose6d& pose, const ceres::Covariance::Options& options = DefaultCovarianceOptions());
+Eigen::MatrixXd computePoseCovariance(ceres::Problem& problem, const Pose6d& pose,
+                                      const ceres::Covariance::Options& options = DefaultCovarianceOptions());
 
 /**
  * @brief Compute off-diagonal covariance between a Pose6d parameter and a double array parameter. Uses @ref computeDV2DVCovariance.
@@ -116,7 +120,7 @@ Eigen::MatrixXd computePoseCovariance(ceres::Problem& problem, Pose6d& pose, con
  * rz|
  * @throw CovarianceException if computation of covariance fails for any pair of parameter blocks, or if GetCovarianceBlock returns false.
  */
-Eigen::MatrixXd computePose2DVCovariance(ceres::Problem &problem, Pose6d &pose, double* dptr, std::size_t num_vars, const ceres::Covariance::Options& options = DefaultCovarianceOptions());
+Eigen::MatrixXd computePose2DVCovariance(ceres::Problem &problem, const Pose6d &pose, const double* dptr, const std::size_t num_vars, const ceres::Covariance::Options& options = DefaultCovarianceOptions());
 
 /**
  * @brief Compute off-diagonal covariance between two Pose6d parameters. Uses @ref computeDV2DVCovariance.
@@ -138,13 +142,13 @@ Eigen::MatrixXd computePose2DVCovariance(ceres::Problem &problem, Pose6d &pose,
  * rz1|
  * @throw CovarianceException if computation of covariance fails for any pair of parameter blocks, or if GetCovarianceBlock returns false.
  */
-Eigen::MatrixXd computePose2PoseCovariance(ceres::Problem &problem, Pose6d &p1, Pose6d &p2, const ceres::Covariance::Options& options = DefaultCovarianceOptions());
+Eigen::MatrixXd computePose2PoseCovariance(ceres::Problem &problem, const Pose6d &p1, const Pose6d &p2, const ceres::Covariance::Options& options = DefaultCovarianceOptions());
 
 /**
  * @brief Compute standard deviations and correlation coefficients for a given problem and parameter block. Uses @ref computeDV2DVCovariance.
  * @param problem The Ceres problem (after optimization).
  * @param dptr Ceres parameter block.
- * @param num_vars Number of parameters in dptr.
+ * @param num_vars Number of parameters in @ref dptr.
  * @param options Options to use when calculating covariance. Use default if not explicitly set by user.
  * @return A square matrix with size (num_vars x num_vars) containing standard deviations (diagonal elements) and correlation coefficients (off-diagonal elements).
  * Given that @ref dptr contains the parameters [a_1 ... a_n] where n = num_vars, the output matrix will be arranged like this:
@@ -156,15 +160,15 @@ Eigen::MatrixXd computePose2PoseCovariance(ceres::Problem &problem, Pose6d &p1,
  * a_n|
  * @throw CovarianceException if computation of covariance fails for any pair of parameter blocks, or if GetCovarianceBlock returns false.
  */
-Eigen::MatrixXd computeDVCovariance(ceres::Problem &problem, double *dptr, std::size_t num_vars, const ceres::Covariance::Options& options = DefaultCovarianceOptions());
+Eigen::MatrixXd computeDVCovariance(ceres::Problem &problem, const double* dptr, const std::size_t& num_vars, const ceres::Covariance::Options& options = DefaultCovarianceOptions());
 
 /**
  * @brief Compute off-diagonal covariance for a given problem and parameters.
  * @param problem The Ceres problem (after optimization).
  * @param dptr1 First parameter block.
- * @param num_vars1 Number of parameters in dptr1.
+ * @param num_vars1 Number of parameters in @ref dptr1.
  * @param dptr2 Second parameter block.
- * @param num_vars2 Number of parameters in dptr2.
+ * @param num_vars2 Number of parameters in @ref dptr2.
  * @return A matrix with size (num_vars1 x num_vars2) containing the off-diagonal covariance.
  * Given that @ref dptr1 contains the parameters [a_1, a_2 ... a_n1] where n1 = num_vars1 and @ref dptr2 contains the parameters [b_1, b_2 ... b_n2] where n2 = num_vars2,
  * the output matrix will be arranged like this:
@@ -176,6 +180,25 @@ Eigen::MatrixXd computeDVCovariance(ceres::Problem &problem, double *dptr, std::
  * a_n1|
  * @throw CovarianceException if computation of covariance fails for any pair of parameter blocks, or if GetCovarianceBlock returns false.
  */
-Eigen::MatrixXd computeDV2DVCovariance(ceres::Problem &problem, double* dptr1, std::size_t num_vars1, double* dptr2, std::size_t num_vars2, const ceres::Covariance::Options& options = DefaultCovarianceOptions());
+Eigen::MatrixXd computeDV2DVCovariance(ceres::Problem &problem, const double* dptr1, const std::size_t num_vars1, const double* dptr2, const std::size_t num_vars2, const ceres::Covariance::Options& options = DefaultCovarianceOptions());
+
+/**
+ * @brief Compute variance and covariance for a given problem and parameters. Uses @ref computeDVCovariance and @ref computeDV2DVCovariance.
+ * @param The Ceres problem (after optimization).
+ * @param dptr1 First parameter block
+ * @param num_vars1 Number of parameters in @ref dptr1
+ * @param dptr2 Second parameter block
+ * @param num_vars1 Number of parameters in @ref dptr2
+ * @param options Options to use when calculating covariance. Use default if not explicitly set by user.
+ * @return A square matrix with size (n x n), where n = num_vars1 + num_vars2, containing variance (diagonal elements) and covariance (off-diagonal elements).
+ * With input parameter blocks p1 and p2, the output matrix will be arranged like this:
+ *    |     p1    |     p2    |
+ * ---|-----------|-----------|
+ * p1 | C(p1, p1) | C(p1, p2) |
+ * p2 | C(p2, p1) | C(p2, p2) |
+ */
+Eigen::MatrixXd computeFullDV2DVCovariance(ceres::Problem& problem, const double* dptr1, const std::size_t num_vars1, const double* dptr2, const std::size_t num_vars2,
+                                           const ceres::Covariance::Options& options = DefaultCovarianceOptions());
+
 
 }  // namespace rct_optimizations

rct_optimizations/include/rct_optimizations/pnp.h

@@ -21,6 +21,7 @@ struct PnPResult
   double final_cost_per_obs;
 
   Eigen::Isometry3d camera_to_target;
+  Eigen::MatrixXd camera_to_target_covariance;
 };
 
 PnPResult optimize(const PnPProblem& params);

rct_optimizations/src/rct_optimizations/covariance_analysis.cpp

@@ -3,7 +3,7 @@
 namespace rct_optimizations
 {
 
-Eigen::MatrixXd covToEigenCorr(double* cov, std::size_t num_vars)
+Eigen::MatrixXd covToEigenCorr(const double* cov, const std::size_t num_vars)
 {
   Eigen::MatrixXd out(num_vars, num_vars);
 
@@ -29,7 +29,7 @@ Eigen::MatrixXd covToEigenCorr(double* cov, std::size_t num_vars)
   return out;
 }
 
-Eigen::MatrixXd covToEigenCov(double* cov, std::size_t num_vars)
+Eigen::MatrixXd covToEigenCov(const double* cov, const std::size_t num_vars)
 {
   Eigen::MatrixXd out(num_vars, num_vars);
 
@@ -43,7 +43,7 @@ Eigen::MatrixXd covToEigenCov(double* cov, std::size_t num_vars)
   return out;
 }
 
-Eigen::MatrixXd covToEigenOffDiagCorr(double* cov_d1d1, std::size_t num_vars1, double* cov_d2d2, std::size_t num_vars2, double* cov_d1d2)
+Eigen::MatrixXd covToEigenOffDiagCorr(const double* cov_d1d1, const std::size_t num_vars1, const double* cov_d2d2, const std::size_t num_vars2, const double* cov_d1d2)
 {
   Eigen::MatrixXd out(num_vars1, num_vars2);
 
@@ -63,22 +63,22 @@ Eigen::MatrixXd covToEigenOffDiagCorr(double* cov_d1d1, std::size_t num_vars1, d
   return out;
 }
 
-Eigen::MatrixXd computePoseCovariance(ceres::Problem& problem, Pose6d& pose, const ceres::Covariance::Options& options)
+Eigen::MatrixXd computePoseCovariance(ceres::Problem& problem, const Pose6d& pose, const ceres::Covariance::Options& options)
 {
   return computeDVCovariance(problem, pose.values.data(), 6, options);
 }
 
-Eigen::MatrixXd computePose2DVCovariance(ceres::Problem &problem, Pose6d &pose, double* dptr, std::size_t num_vars, const ceres::Covariance::Options& options)
+Eigen::MatrixXd computePose2DVCovariance(ceres::Problem &problem, const Pose6d &pose, const double* dptr, std::size_t num_vars, const ceres::Covariance::Options& options)
 {
   return computeDV2DVCovariance(problem, pose.values.data(), 6, dptr, num_vars, options);
 }
 
-Eigen::MatrixXd computePose2PoseCovariance(ceres::Problem &problem, Pose6d &p1, Pose6d &p2, const ceres::Covariance::Options& options)
+Eigen::MatrixXd computePose2PoseCovariance(ceres::Problem &problem, const Pose6d &p1, const Pose6d &p2, const ceres::Covariance::Options& options)
 {
   return computeDV2DVCovariance(problem, p1.values.data(), 6, p2.values.data(), 6, options);
 }
 
-Eigen::MatrixXd computeDVCovariance(ceres::Problem &problem, double *dptr, std::size_t num_vars, const ceres::Covariance::Options& options)
+Eigen::MatrixXd computeDVCovariance(ceres::Problem &problem, const double * dptr, const std::size_t& num_vars, const ceres::Covariance::Options& options)
 {
   ceres::Covariance covariance(options);
 
@@ -89,13 +89,13 @@ Eigen::MatrixXd computeDVCovariance(ceres::Problem &problem, double *dptr, std::
     throw CovarianceException("Could not compute covariance in computeDVCovariance()");
 
   double cov[num_vars*num_vars];
-  if(!covariance.GetCovarianceBlock(dptr, dptr, cov))
+  if(!covariance.GetCovarianceBlockInTangentSpace(dptr, dptr, cov))
     throw CovarianceException("GetCovarianceBlock failed in computeDVCovariance()");
 
   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, const ceres::Covariance::Options& options)
+Eigen::MatrixXd computeDV2DVCovariance(ceres::Problem &P, const double* dptr1, const std::size_t num_vars1, const double* dptr2, const std::size_t num_vars2, const ceres::Covariance::Options& options)
 {
   ceres::Covariance covariance(options);
 
@@ -108,12 +108,46 @@ Eigen::MatrixXd computeDV2DVCovariance(ceres::Problem &P, double* dptr1, std::si
     throw CovarianceException("Could not compute covariance in computeDV2DVCovariance()");
 
   double cov_d1d1[num_vars1*num_vars2], cov_d2d2[num_vars2*num_vars2], cov_d1d2[num_vars1*num_vars2];
-  if(!(covariance.GetCovarianceBlock(dptr1, dptr1, cov_d1d1) &&
-       covariance.GetCovarianceBlock(dptr2, dptr2, cov_d2d2) &&
-       covariance.GetCovarianceBlock(dptr1, dptr2, cov_d1d2)))
+  if(!(covariance.GetCovarianceBlockInTangentSpace(dptr1, dptr1, cov_d1d1) &&
+       covariance.GetCovarianceBlockInTangentSpace(dptr2, dptr2, cov_d2d2) &&
+       covariance.GetCovarianceBlockInTangentSpace(dptr1, dptr2, cov_d1d2)))
     throw CovarianceException("GetCovarianceBlock failed in computeDV2DVCovariance()");
 
   return covToEigenOffDiagCorr(cov_d1d1, num_vars1, cov_d2d2, num_vars2, cov_d1d2);
 }
 
+Eigen::MatrixXd computeFullDV2DVCovariance(ceres::Problem &problem,
+                                           const double *dptr1,
+                                           const std::size_t num_vars1,
+                                           const double *dptr2,
+                                           const std::size_t num_vars2,
+                                           const ceres::Covariance::Options &options)
+{
+  // Calculate the individual covariance matrices
+  // Covariance of parameter 1 with itself
+  Eigen::MatrixXd cov_p1 = computeDVCovariance(problem, dptr1, num_vars1, options);
+  // Covariance of parameter 2 with itself
+  Eigen::MatrixXd cov_p2 = computeDVCovariance(problem, dptr2, num_vars2, options);
+  // Covariance of parameter 1 with parameter 2
+  Eigen::MatrixXd cov_p1p2
+    = computeDV2DVCovariance(problem, dptr1, num_vars1, dptr2, num_vars2, options);
+
+  // Total covariance matrix
+  Eigen::MatrixXd cov;
+  const std::size_t n = num_vars1 + num_vars2;
+  cov.resize(n, n);
+
+  /*    |     P1    |     P2    |
+   * ---|-----------|-----------|
+   * P1 | C(p1, p1) | C(p1, p2) |
+   * P2 | C(p2, p1) | C(p2, p2) |
+   */
+  cov.block(0, 0, num_vars1, num_vars1) = cov_p1;
+  cov.block(num_vars1, num_vars1, num_vars2, num_vars2) = cov_p2;
+  cov.block(0, num_vars1, num_vars1, num_vars2) = cov_p1p2;
+  cov.block(num_vars1, 0, num_vars2, num_vars1) = cov_p1p2.transpose();
+
+  return cov;
+}
+
 }  // namespace rct_optimizations

rct_optimizations/src/rct_optimizations/pnp.cpp

@@ -1,6 +1,8 @@
 #include "rct_optimizations/pnp.h"
 #include "rct_optimizations/ceres_math_utilities.h"
 #include "rct_optimizations/local_parameterization.h"
+#include "rct_optimizations/covariance_analysis.h"
+
 #include <ceres/ceres.h>
 
 namespace
@@ -84,6 +86,7 @@ PnPResult optimize(const PnPProblem &params)
   result.initial_cost_per_obs = summary.initial_cost / summary.num_residuals;
   result.final_cost_per_obs = summary.final_cost / summary.num_residuals;
   result.camera_to_target = Eigen::Translation3d(t) * q;
+  result.camera_to_target_covariance = computeFullDV2DVCovariance(problem, t.data(), t.size(), q.coeffs().data(), q_param->LocalSize());
 
   return result;
 }

rct_optimizations/test/pnp_utest.cpp

@@ -5,18 +5,18 @@
 
 using namespace rct_optimizations;
 
+static const unsigned TARGET_ROWS = 10;
+static const unsigned TARGET_COLS = 14;
+static const double SPACING = 0.025;
+
 TEST(PNP_2D, PerfectInitialConditions)
 {
   test::Camera camera = test::makeKinectCamera();
-
-  unsigned target_rows = 5;
-  unsigned target_cols = 7;
-  double spacing = 0.025;
-  test::Target target(target_rows, target_cols, spacing);
+  test::Target target(TARGET_ROWS, TARGET_COLS, SPACING);
 
   Eigen::Isometry3d target_to_camera(Eigen::Isometry3d::Identity());
-  double x = static_cast<double>(target_rows - 1) * spacing / 2.0;
-  double y = static_cast<double>(target_cols - 1) * spacing / 2.0;
+  double x = static_cast<double>(TARGET_ROWS - 1) * SPACING / 2.0;
+  double y = static_cast<double>(TARGET_COLS - 1) * SPACING / 2.0;
   target_to_camera.translate(Eigen::Vector3d(x, y, 1.0));
   target_to_camera.rotate(Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX()));
 
@@ -31,20 +31,19 @@ TEST(PNP_2D, PerfectInitialConditions)
   EXPECT_TRUE(result.camera_to_target.isApprox(target_to_camera.inverse()));
   EXPECT_LT(result.initial_cost_per_obs, 1.0e-15);
   EXPECT_LT(result.final_cost_per_obs, 1.0e-15);
+
+  Eigen::IOFormat fmt(4, 0, " | ", "\n", "|", "|");
+  std::cout << "Covariance:\n" << result.camera_to_target_covariance.format(fmt) << std::endl;
 }
 
 TEST(PNP_2D, PerturbedInitialCondition)
 {
   test::Camera camera = test::makeKinectCamera();
-
-  unsigned target_rows = 5;
-  unsigned target_cols = 7;
-  double spacing = 0.025;
-  test::Target target(target_rows, target_cols, spacing);
+  test::Target target(TARGET_ROWS, TARGET_COLS, SPACING);
 
   Eigen::Isometry3d target_to_camera(Eigen::Isometry3d::Identity());
-  double x = static_cast<double>(target_rows) * spacing / 2.0;
-  double y = static_cast<double>(target_cols) * spacing / 2.0;
+  double x = static_cast<double>(TARGET_ROWS) * SPACING / 2.0;
+  double y = static_cast<double>(TARGET_COLS) * SPACING / 2.0;
   target_to_camera.translate(Eigen::Vector3d(x, y, 1.0));
   target_to_camera.rotate(Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX()));
 
@@ -61,20 +60,19 @@ TEST(PNP_2D, PerturbedInitialCondition)
   EXPECT_TRUE(result.converged);
   EXPECT_TRUE(result.camera_to_target.isApprox(target_to_camera.inverse(), 1.0e-8));
   EXPECT_LT(result.final_cost_per_obs, 1.0e-14);
+
+  Eigen::IOFormat fmt(4, 0, " | ", "\n", "|", "|");
+  std::cout << "Covariance:\n" << result.camera_to_target_covariance.format(fmt) << std::endl;
 }
 
 TEST(PNP_2D, BadIntrinsicParameters)
 {
   test::Camera camera = test::makeKinectCamera();
-
-  unsigned target_rows = 5;
-  unsigned target_cols = 7;
-  double spacing = 0.025;
-  test::Target target(target_rows, target_cols, spacing);
+  test::Target target(TARGET_ROWS, TARGET_COLS, SPACING);
 
   Eigen::Isometry3d target_to_camera(Eigen::Isometry3d::Identity());
-  double x = static_cast<double>(target_rows) * spacing / 2.0;
-  double y = static_cast<double>(target_cols) * spacing / 2.0;
+  double x = static_cast<double>(TARGET_ROWS) * SPACING / 2.0;
+  double y = static_cast<double>(TARGET_COLS) * SPACING / 2.0;
   target_to_camera.translate(Eigen::Vector3d(x, y, 1.0));
   target_to_camera.rotate(Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX()));
 
@@ -101,6 +99,9 @@ TEST(PNP_2D, BadIntrinsicParameters)
   EXPECT_TRUE(result.converged);
   EXPECT_FALSE(result.camera_to_target.isApprox(target_to_camera.inverse(), 1.0e-3));
   EXPECT_GT(result.final_cost_per_obs, 1.0e-3);
+
+  Eigen::IOFormat fmt(4, 0, " | ", "\n", "|", "|");
+  std::cout << "Covariance:\n" << result.camera_to_target_covariance.format(fmt) << std::endl;
 }
 
 int main(int argc, char **argv)