Add functions to evaluate covariance of optimization results

rct_examples/src/tools/camera_on_wrist_extrinsic.cpp

@@ -154,6 +154,11 @@ int main(int argc, char** argv)
   rct_ros_tools::printTransform(t, "Base", "Target", "BASE TO TARGET");
   rct_ros_tools::printNewLine();
 
+  std::cout << "Covariance (wrist to camera):\n" << opt_result.covariance_camera_mount_to_camera.matrix() << std::endl;
+  std::cout << "Covariance (base to target):\n" << opt_result.covariance_target_mount_to_target.matrix() << std::endl;
+  std::cout << "Covariance (w2c vs b2t):\n" << opt_result.covariance_tform_target_to_tform_camera.matrix() << std::endl;
+
+
   rct_ros_tools::printTitle("REPROJECTION ERROR");
   for (std::size_t i = 0; i < data_set.images.size(); ++i)
   {

rct_examples/src/tools/intrinsic_calibration.cpp

@@ -140,6 +140,8 @@ int main(int argc, char** argv)
   rct_ros_tools::printCameraDistortion(new_dist, "RCT Distortion");
   rct_ros_tools::printNewLine();
 
+  std::cout << "Covariance (intrinsic):\n" << opt_result.covariance_intr.matrix() << std::endl;
+
   // Also try the OpenCV cameraCalibrate function
   rct_ros_tools::printTitle("OpenCV Calibration");
   opencvCameraCalibration(problem_def.image_observations, data_set.images.front().size(),

rct_optimizations/CMakeLists.txt

@@ -7,6 +7,9 @@ find_package(Ceres REQUIRED)
 add_library(${PROJECT_NAME} SHARED
   # Utilities
   src/${PROJECT_NAME}/eigen_conversions.cpp
+  src/${PROJECT_NAME}/covariance_analysis.cpp
+  # Optimizations (Simple)
+  src/${PROJECT_NAME}/circle_fit.cpp
   # Optimizations (multiple cameras)
   src/${PROJECT_NAME}/extrinsic_multi_static_camera.cpp
   src/${PROJECT_NAME}/extrinsic_multi_static_camera_only.cpp

rct_optimizations/include/rct_optimizations/circle_fit.h

@@ -0,0 +1,84 @@
+#pragma once
+
+#include <vector>
+#include <Eigen/Dense>
+#include <rct_optimizations/types.h>
+
+namespace rct_optimizations
+{
+
+/**
+ * @brief Problem setup info for the circle fit optimization.
+ */
+struct CircleFitProblem
+{
+  /**
+   * @brief Observations around the edge of the circle. 2D points in the form (x, y).
+   */
+  std::vector<Eigen::Vector2d> observations;
+
+  /**
+   * @brief Estimate for the initial x-coordinate of the center of the circle.
+   */
+  double x_center_initial;
+
+  /**
+   * @brief Estimate for the initial y-coordinate of the center of the circle.
+   */
+  double y_center_initial;
+
+  /**
+   * @brief Estimate for the initial radius of the circle.
+   */
+  double radius_initial;
+};
+
+/**
+ * @brief Results for the circle fit optimization
+ */
+struct CircleFitResult
+{
+  /**
+   * @brief True if converged, False if any other result.
+   */
+  bool converged;
+
+  /**
+   * @brief Initial average error per observation.
+   */
+  double initial_cost_per_obs;
+
+  /**
+   * @brief Final average error per observation.
+   */
+  double final_cost_per_obs;
+
+  /**
+   * @brief The covariance matrix for the problem. A square 3x3 matrix giving covariance between each pair of elements. Order of elements is [x, y, radius].
+   */
+  Eigen::MatrixXd covariance;
+
+  /**
+   * @brief Calculated circle center x-coord.
+   */
+  double x_center;
+
+  /**
+   * @brief Calculated circle center y-coord.
+   */
+  double y_center;
+
+  /**
+   * @brief Calculated circle radius.
+   */
+  double radius;
+};
+
+/**
+ * @brief Function that solves the circle fit problem.
+ * @param Input observations and guesses.
+ * @return Output results.
+ */
+CircleFitResult optimize(const CircleFitProblem& params);
+
+}  // namespace rct_optimizations

rct_optimizations/include/rct_optimizations/covariance_analysis.h

@@ -0,0 +1,104 @@
+#pragma once
+
+#include <ceres/ceres.h>
+#include <Eigen/Dense>
+#include <iostream>
+#include <rct_optimizations/types.h>
+
+namespace rct_optimizations
+{
+
+/**
+ * @brief DefaultCovarianceOptions An instance of ceres::Covariance::Options with values better suited to the optimizations in this library.
+ */
+struct DefaultCovarianceOptions : ceres::Covariance::Options
+{
+  DefaultCovarianceOptions()
+  {
+    this->algorithm_type = ceres::DENSE_SVD;
+  }
+};
+
+/**
+ * @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 standard deviations (diagonal elements) and correlation coefficients (off-diagonal elements).
+ */
+Eigen::MatrixXd covToEigenCorr(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 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 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 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.
+ * @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).
+ */
+Eigen::MatrixXd computePoseCovariance(ceres::Problem& problem, Pose6d& pose, const ceres::Covariance::Options& options = DefaultCovarianceOptions());
+
+/**
+ * @brief Compute off-diagonal covariance between a Pose6d parameter and a double array parameter.
+ * @param The Ceres problem (after optimization).
+ * @param pose First Pose6d parameter.
+ * @param dptr Second double array parameter.
+ * @param num_vars Number of elements in dptr.
+ * @param options Options to use when calculating covariance. Use default if not explicitly set by user.
+ * @return A matrix with size (6 x num_vars) containing the off-diagonal covariance.
+ */
+Eigen::MatrixXd computePose2DVCovariance(ceres::Problem &problem, Pose6d &pose, double* dptr, std::size_t num_vars, const ceres::Covariance::Options& options = DefaultCovarianceOptions());
+
+/**
+ * @brief Compute off-diagonal covariance between two Pose6d parameters.
+ * @param The Ceres problem (after optimization).
+ * @param p1 First pose parameter.
+ * @param p2 Second pose 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 the off-diagonal covariance.
+ */
+Eigen::MatrixXd computePose2PoseCovariance(ceres::Problem &problem, Pose6d &p1, Pose6d &p2, const ceres::Covariance::Options& options = DefaultCovarianceOptions());
+
+/**
+ * @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.
+ * @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).
+ */
+Eigen::MatrixXd computeDVCovariance(ceres::Problem &problem, double *dptr, 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 dptr2 Second parameter block.
+ * @param num_vars2 Number of parameters in dptr2.
+ * @return A matrix with size (num_vars1 x num_vars2) containing the off-diagonal covariance.
+ */
+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());
+
+}  // namespace rct_optimizations

rct_optimizations/include/rct_optimizations/experimental/camera_intrinsic.h

@@ -26,6 +26,8 @@ struct IntrinsicEstimationResult
   std::array<double, 5> distortions;
 
   std::vector<Eigen::Isometry3d> target_transforms;
+
+  Eigen::MatrixXd covariance_intr;
 };
 
 IntrinsicEstimationResult optimize(const IntrinsicEstimationProblem& params);

rct_optimizations/include/rct_optimizations/extrinsic_hand_eye.h

@@ -45,6 +45,10 @@ struct ExtrinsicHandEyeResult
 
   Eigen::Isometry3d target_mount_to_target;
   Eigen::Isometry3d camera_mount_to_camera;
+
+  Eigen::MatrixXd covariance_target_mount_to_target;
+  Eigen::MatrixXd covariance_camera_mount_to_camera;
+  Eigen::MatrixXd covariance_tform_target_to_tform_camera;
 };
 
 ExtrinsicHandEyeResult optimize(const ExtrinsicHandEyeProblem2D3D &params);

rct_optimizations/include/rct_optimizations/types.h

@@ -124,6 +124,20 @@ using Observation2D3D = Observation<2, 3>;
 /** @brief Typedef for observations of 3D sensor to 3D target correspondences */
 using Observation3D3D = Observation<3, 3>;
 
+
+struct OptimizationException : public std::runtime_error
+{
+public:
+  OptimizationException(const std::string& what) : std::runtime_error(what) {}
+};
+
+
+struct CovarianceException : public std::runtime_error
+{
+public:
+  CovarianceException(const std::string& what) : std::runtime_error(what) {}
+};
+
 } // namespace rct_optimizations
 
 #endif

rct_optimizations/src/rct_optimizations/camera_intrinsic.cpp

@@ -3,6 +3,7 @@
 
 #include <rct_optimizations/ceres_math_utilities.h>
 #include <rct_optimizations/eigen_conversions.h>
+#include <rct_optimizations/covariance_analysis.h>
 
 #include <ceres/ceres.h>
 
@@ -244,5 +245,9 @@ rct_optimizations::optimize(const rct_optimizations::IntrinsicEstimationProblem&
   result.initial_cost_per_obs = summary.initial_cost / summary.num_residuals;
   result.final_cost_per_obs = summary.final_cost / summary.num_residuals;
 
+  // TODO: How to handle errors while calculating covariance? Possible for optimization to converge even though the covariance matrix is near rank deficient,
+  // which means the result contains (potentially) valid optimization results but partial or invalid covariance results.
+  result.covariance_intr = rct_optimizations::computeDVCovariance(problem, internal_intrinsics_data.data(), 9);
+
   return result;
 }

rct_optimizations/src/rct_optimizations/circle_fit.cpp

@@ -0,0 +1,112 @@
+#include <ceres/ceres.h>
+#include <rct_optimizations/circle_fit.h>
+#include <rct_optimizations/covariance_analysis.h>
+
+namespace
+{
+
+/**
+ * @brief Cost function for the error between a point and a circle.
+ */
+class CircleDistCost
+{
+public:
+  CircleDistCost(const double& x, const double& y)
+    : x_(x), y_(y)
+  {}
+
+
+  /**
+   * @brief One formulation of a residual function for error relative to the center of a circle.
+   * Presented to the solver as three size-1 double parameters.
+   * @param sample A double pointer which contains {x, y, r_sqrt}.
+   */
+  template<typename T>
+  bool operator()(const T* const sample,
+                  T* residual) const
+  {
+    T r = sample[2] * sample[2];
+
+    T x_pos = x_ - sample[0];
+    T y_pos = y_ - sample[1];
+
+    residual[0] = r * r - x_pos * x_pos - y_pos * y_pos;
+    return true;
+  }
+
+  /**
+   * @brief An alternative formulation of a residual function for error relative to the center of a circle.
+   * Here, the inputs (x, y, r_sqrt) are provided as separate parameters. This is mathematically identical to the previous version,
+   * but the number and size of the parameters are presented differently to the solver (one size-3 double parameter).
+   * @param x_sample The x-coordinate of the center of the circle.
+   * @param y_sample The y-coordinate of the center of the circle.
+   * @param r_sqrt_sample The square root of the radius of the circle.
+   */
+  template<typename T>
+  bool operator()(const T* const x_sample,
+                  const T* const y_sample,
+                  const T* const r_sqrt_sample,
+                  T* residual) const
+  {
+    T r = r_sqrt_sample * r_sqrt_sample;  // Using sqrt(radius) prevents the radius from ending up negative and improves numerical stability.
+
+    T x_pos = x_ - x_sample;
+    T y_pos = y_ - y_sample;
+
+    residual[0] = r * r - x_pos * x_pos - y_pos * y_pos;
+    return true;
+  }
+
+private:
+  double x_, y_;
+};
+
+}
+
+rct_optimizations::CircleFitResult
+rct_optimizations::optimize(const rct_optimizations::CircleFitProblem& params)
+{
+  double x = params.x_center_initial;
+  double y = params.y_center_initial;
+  double r = params.radius_initial;
+
+  double r_sqrt = sqrt(r);
+
+  std::vector<double> circle_params(3, 0.0);
+  circle_params[0] = x;
+  circle_params[1] = y;
+  circle_params[2] = r_sqrt;
+
+  rct_optimizations::CircleFitResult result;
+
+  ceres::Problem problem;
+
+  ceres::LossFunction* loss_fn = nullptr;
+
+  for (auto obs : params.observations)
+  {
+    auto* cost_fn = new CircleDistCost(obs.x(), obs.y());
+
+    auto* cost_block = new ceres::AutoDiffCostFunction<CircleDistCost, 1, 3>(cost_fn);
+    problem.AddResidualBlock(cost_block, loss_fn, circle_params.data());
+  }
+
+  ceres::Solver::Options options;
+  options.max_num_iterations = 500;
+  options.linear_solver_type = ceres::DENSE_QR;
+  ceres::Solver::Summary summary;
+  ceres::Solve(options, &problem, &summary);
+
+
+  std::cout << summary.BriefReport() << std::endl;
+
+  result.converged = summary.termination_type == ceres::TerminationType::CONVERGENCE;
+  result.x_center = circle_params[0];
+  result.y_center = circle_params[1];
+  result.radius = pow(circle_params[2], 2);
+  result.initial_cost_per_obs = summary.initial_cost / summary.num_residuals;
+  result.final_cost_per_obs = summary.final_cost / summary.num_residuals;
+  result.covariance = rct_optimizations::computeDVCovariance(problem, circle_params.data(), 3);
+
+  return result;
+}