PnP update

.github/workflows/main.yml

@@ -16,7 +16,8 @@ jobs:
              ADDITIONAL_DEBS: git,
              VERBOSE_TESTS: true,
              TARGET_CMAKE_ARGS: "-DRCT_BUILD_TESTS=ON",
-             AFTER_SCRIPT: 'catkin build -w $target_ws --no-deps --verbose rct_optimizations --make-args test'}
+             CTEST_OUTPUT_ON_FAILURE: true,
+             AFTER_SCRIPT: "cd $target_ws/build/rct_optimizations && ctest -V"}
           - {BADGE: xenial,
              OS_NAME: ubuntu,
              OS_CODE_NAME: xenial,
@@ -25,7 +26,8 @@ jobs:
              ADDITIONAL_DEBS: git,
              VERBOSE_TESTS: true,
              TARGET_CMAKE_ARGS: "-DRCT_BUILD_TESTS=ON",
-             AFTER_SCRIPT: 'catkin build -w $target_ws --no-deps --verbose rct_optimizations --make-args test'}
+             CTEST_OUTPUT_ON_FAILURE: true,
+             AFTER_SCRIPT: 'cd $target_ws/build/rct_optimizations && ctest -V'}
     runs-on: ubuntu-latest
     steps:
       - uses: actions/checkout@v1

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,7 +74,7 @@ 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.
@@ -93,7 +93,7 @@ Eigen::MatrixXd covToEigenOffDiagCorr(double* cov_d1d1, std::size_t num_vars1, d
  * rz|
  * @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 +116,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 +138,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 +156,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 +176,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

@@ -28,6 +28,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);
 
@@ -95,7 +95,7 @@ Eigen::MatrixXd computeDVCovariance(ceres::Problem &problem, double *dptr, std::
   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);
 
@@ -116,4 +116,38 @@ Eigen::MatrixXd computeDV2DVCovariance(ceres::Problem &P, double* dptr1, std::si
   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,6 @@
 #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>
 
@@ -17,16 +17,16 @@ struct SolvePnPCostFunc
   }
 
   template<typename T>
-  bool operator()(const T *const target_q, const T *const target_t, T *const residual) const
+  bool operator()(const T *const target_aa, const T *const target_t, T *const residual) const
   {
     using Isometry3 = Eigen::Transform<T, 3, Eigen::Isometry>;
     using Vector3 = Eigen::Matrix<T, 3, 1>;
     using Vector2 = Eigen::Matrix<T, 2, 1>;
 
-    Eigen::Map<const Eigen::Quaternion<T>> q(target_q);
+    Eigen::Map<const Vector3> aa(target_aa);
     Eigen::Map<const Vector3> t(target_t);
     Isometry3 camera_to_target = Isometry3::Identity();
-    camera_to_target = Eigen::Translation<T, 3>(t) * q;
+    camera_to_target = Eigen::Translation<T, 3>(t) * Eigen::AngleAxis<T>(aa.norm(), aa.normalized());
 
     // Transform points into camera coordinates
     Vector3 camera_pt = camera_to_target * in_target_.cast<T>();
@@ -53,16 +53,15 @@ struct SolvePnPCostFunc3D
   {}
 
   template<typename T>
-  bool operator()(const T *const target_q, const T *const target_t, T *const residual) const
+  bool operator()(const T *const target_aa, const T *const target_t, T *const residual) const
   {
-
     using Isometry3 = Eigen::Transform<T, 3, Eigen::Isometry>;
     using Vector3 = Eigen::Matrix<T, 3, 1>;
 
-    Eigen::Map<const Eigen::Quaternion<T>> q(target_q);
+    Eigen::Map<const Vector3> aa(target_aa);
     Eigen::Map<const Vector3> t(target_t);
     Isometry3 camera_to_target = Isometry3::Identity();
-    camera_to_target = Eigen::Translation<T, 3>(t) * q;
+    camera_to_target = Eigen::Translation<T, 3>(t) * Eigen::AngleAxis<T>(aa.norm(), aa.normalized());
 
     // Transform points into camera coordinates
     Vector3 camera_pt = camera_to_target * in_target_.cast<T>();
@@ -85,31 +84,24 @@ namespace rct_optimizations
 PnPResult optimize(const PnPProblem &params)
 {
   // Create the optimization variables from the input guess
-  Eigen::Quaterniond q(params.camera_to_target_guess.rotation());
+  Eigen::AngleAxisd rot(params.camera_to_target_guess.rotation());
+  Eigen::Vector3d aa = rot.angle() * rot.axis();
   Eigen::Vector3d t(params.camera_to_target_guess.translation());
 
   ceres::Problem problem;
 
   // For each 3D point seen in the 2D image
-  for (std::size_t i = 0; i < params.correspondences.size(); ++i)
+  for (const auto& corr : params.correspondences)
   {
-    // Define
-    const auto &img_obs = params.correspondences[i].in_image;
-    const auto &point_in_target = params.correspondences[i].in_target;
-
     // Allocate Ceres data structures - ownership is taken by the ceres
     // Problem data structure
-    auto *cost_fn = new SolvePnPCostFunc(params.intr, point_in_target, img_obs);
+    auto *cost_fn = new SolvePnPCostFunc(params.intr, corr.in_target, corr.in_image);
 
-    auto *cost_block = new ceres::AutoDiffCostFunction<SolvePnPCostFunc, 2, 4, 3>(cost_fn);
+    auto *cost_block = new ceres::AutoDiffCostFunction<SolvePnPCostFunc, 2, 3, 3>(cost_fn);
 
-    problem.AddResidualBlock(cost_block, nullptr, q.coeffs().data(), t.data());
+    problem.AddResidualBlock(cost_block, nullptr, aa.data(), t.data());
   }
 
-  ceres::LocalParameterization *q_param
-    = new ceres::AutoDiffLocalParameterization<EigenQuaternionPlus, 4, 3>();
-  problem.SetParameterization(q.coeffs().data(), q_param);
-
   ceres::Solver::Summary summary;
   ceres::Solver::Options options;
   ceres::Solve(options, &problem, &summary);
@@ -118,46 +110,52 @@ PnPResult optimize(const PnPProblem &params)
   result.converged = summary.termination_type == ceres::CONVERGENCE;
   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 = Eigen::Translation3d(t) * Eigen::AngleAxisd(aa.norm(), aa.normalized());
+  result.camera_to_target_covariance = computeFullDV2DVCovariance(problem,
+                                                                  t.data(),
+                                                                  t.size(),
+                                                                  aa.data(),
+                                                                  aa.size());
 
   return result;
 }
 
 PnPResult optimize(const rct_optimizations::PnPProblem3D& params)
 {
   // Create the optimization variables from the input guess
-  Eigen::Quaterniond q(params.camera_to_target_guess.rotation());
+  Eigen::AngleAxisd rot(params.camera_to_target_guess.rotation());
+  Eigen::Vector3d aa(rot.angle() * rot.axis());
   Eigen::Vector3d t(params.camera_to_target_guess.translation());
 
   ceres::Problem problem;
 
   //only one loop, for correspondences
-  for (std::size_t i = 0; i < params.correspondences.size(); ++i) // For each 3D point seen in the 3D image
+  for (const auto& corr : params.correspondences) // For each 3D point seen in the 3D image
   {
-    // Define
-    const auto& img_obs = params.correspondences[i].in_image;
-    const auto& point_in_target = params.correspondences[i].in_target;
-
     // Allocate Ceres data structures - ownership is taken by the ceres
     // Problem data structure
 
-    auto* cost_fn = new SolvePnPCostFunc3D(point_in_target, img_obs);
+    auto* cost_fn = new SolvePnPCostFunc3D(corr.in_target, corr.in_image);
 
-    auto* cost_block = new ceres::AutoDiffCostFunction<SolvePnPCostFunc3D, 2, 4, 3>(cost_fn);
+    auto* cost_block = new ceres::AutoDiffCostFunction<SolvePnPCostFunc3D, 3, 3, 3>(cost_fn);
 
-    problem.AddResidualBlock(cost_block, nullptr, q.coeffs().data(), t.data());
+    problem.AddResidualBlock(cost_block, nullptr, aa.data(), t.data());
   }
 
   ceres::Solver::Options options;
-  //options.function_tolerance = 1e-7;
   ceres::Solver::Summary summary;
   ceres::Solve(options, &problem, &summary);
 
   PnPResult result;
   result.converged = summary.termination_type == ceres::CONVERGENCE;
   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 = Eigen::Translation3d(t) * Eigen::AngleAxisd(aa.norm(), aa.normalized());
+  result.camera_to_target_covariance = computeFullDV2DVCovariance(problem,
+                                                                  t.data(),
+                                                                  t.size(),
+                                                                  aa.data(),
+                                                                  aa.size());
 
   return result;
 }