diff --git a/rct_optimizations/include/rct_optimizations/covariance_analysis.h b/rct_optimizations/include/rct_optimizations/covariance_analysis.h index b9225801..c13e37c8 100644 --- a/rct_optimizations/include/rct_optimizations/covariance_analysis.h +++ b/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,16 +74,18 @@ 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 | @@ -91,9 +93,11 @@ Eigen::MatrixXd covToEigenOffDiagCorr(double* cov_d1d1, std::size_t num_vars1, d * 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 diff --git a/rct_optimizations/include/rct_optimizations/pnp.h b/rct_optimizations/include/rct_optimizations/pnp.h index 2d5b5828..8e39effe 100644 --- a/rct_optimizations/include/rct_optimizations/pnp.h +++ b/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); diff --git a/rct_optimizations/src/rct_optimizations/covariance_analysis.cpp b/rct_optimizations/src/rct_optimizations/covariance_analysis.cpp index d85eb59d..985d9cf4 100644 --- a/rct_optimizations/src/rct_optimizations/covariance_analysis.cpp +++ b/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 diff --git a/rct_optimizations/src/rct_optimizations/pnp.cpp b/rct_optimizations/src/rct_optimizations/pnp.cpp index 83b30c17..a964a215 100644 --- a/rct_optimizations/src/rct_optimizations/pnp.cpp +++ b/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 namespace @@ -84,6 +86,7 @@ PnPResult optimize(const PnPProblem ¶ms) 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; } diff --git a/rct_optimizations/test/pnp_utest.cpp b/rct_optimizations/test/pnp_utest.cpp index 0f28052d..12ccc23a 100644 --- a/rct_optimizations/test/pnp_utest.cpp +++ b/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(target_rows - 1) * spacing / 2.0; - double y = static_cast(target_cols - 1) * spacing / 2.0; + double x = static_cast(TARGET_ROWS - 1) * SPACING / 2.0; + double y = static_cast(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(target_rows) * spacing / 2.0; - double y = static_cast(target_cols) * spacing / 2.0; + double x = static_cast(TARGET_ROWS) * SPACING / 2.0; + double y = static_cast(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(target_rows) * spacing / 2.0; - double y = static_cast(target_cols) * spacing / 2.0; + double x = static_cast(TARGET_ROWS) * SPACING / 2.0; + double y = static_cast(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)