From ccf6a272b419f03aab761977fd8ff34837b58dec Mon Sep 17 00:00:00 2001 From: "michael.ripperger@swri.org" Date: Thu, 11 Jun 2020 19:09:40 -0500 Subject: [PATCH 01/10] Made covariance functions take const inputs --- .../rct_optimizations/covariance_analysis.h | 17 +++++++++-------- .../rct_optimizations/covariance_analysis.cpp | 16 ++++++++-------- 2 files changed, 17 insertions(+), 16 deletions(-) diff --git a/rct_optimizations/include/rct_optimizations/covariance_analysis.h b/rct_optimizations/include/rct_optimizations/covariance_analysis.h index b9225801..95c2d27f 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,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,8 @@ 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 +117,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,7 +139,7 @@ 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. @@ -156,7 +157,7 @@ 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. @@ -176,6 +177,6 @@ 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()); } // namespace rct_optimizations diff --git a/rct_optimizations/src/rct_optimizations/covariance_analysis.cpp b/rct_optimizations/src/rct_optimizations/covariance_analysis.cpp index d85eb59d..7a0fb66b 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); @@ -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); From 96ed095d80d5b8eefa639721b3dc670424f231a9 Mon Sep 17 00:00:00 2001 From: "michael.ripperger@swri.org" Date: Thu, 11 Jun 2020 19:10:36 -0500 Subject: [PATCH 02/10] Updated to get covariance block in the locally parameterized variable space --- .../src/rct_optimizations/covariance_analysis.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/rct_optimizations/src/rct_optimizations/covariance_analysis.cpp b/rct_optimizations/src/rct_optimizations/covariance_analysis.cpp index 7a0fb66b..ff6ecf86 100644 --- a/rct_optimizations/src/rct_optimizations/covariance_analysis.cpp +++ b/rct_optimizations/src/rct_optimizations/covariance_analysis.cpp @@ -89,7 +89,7 @@ Eigen::MatrixXd computeDVCovariance(ceres::Problem &problem, const double * dptr 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); @@ -108,9 +108,9 @@ Eigen::MatrixXd computeDV2DVCovariance(ceres::Problem &P, const double* dptr1, c 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); From 8a977f30fb80f5a3c861d1a815ab3fbb315495ae Mon Sep 17 00:00:00 2001 From: "michael.ripperger@swri.org" Date: Thu, 11 Jun 2020 19:11:49 -0500 Subject: [PATCH 03/10] Added function for getting covariance of Eigen quaternion and translation --- .../rct_optimizations/covariance_analysis.h | 24 ++++++++++++++ .../rct_optimizations/covariance_analysis.cpp | 33 +++++++++++++++++++ 2 files changed, 57 insertions(+) diff --git a/rct_optimizations/include/rct_optimizations/covariance_analysis.h b/rct_optimizations/include/rct_optimizations/covariance_analysis.h index 95c2d27f..4a89400a 100644 --- a/rct_optimizations/include/rct_optimizations/covariance_analysis.h +++ b/rct_optimizations/include/rct_optimizations/covariance_analysis.h @@ -96,6 +96,30 @@ Eigen::MatrixXd covToEigenOffDiagCorr(const double* cov_d1d1, const std::size_t Eigen::MatrixXd computePoseCovariance(ceres::Problem& problem, const Pose6d& pose, const ceres::Covariance::Options& options = DefaultCovarianceOptions()); +/** + * @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 t the pose translation parameter + * @param q the pose quaternion 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 + * --|-------------- + * x | + * y | + * z | + * rx| + * ry| + * 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, const Eigen::Vector3d& t, Eigen::Quaterniond& q, + const ceres::Covariance::Options& options = DefaultCovarianceOptions()); + /** * @brief Compute off-diagonal covariance between a Pose6d parameter and a double array parameter. Uses @ref computeDV2DVCovariance. * @param The Ceres problem (after optimization). diff --git a/rct_optimizations/src/rct_optimizations/covariance_analysis.cpp b/rct_optimizations/src/rct_optimizations/covariance_analysis.cpp index ff6ecf86..b8ee5944 100644 --- a/rct_optimizations/src/rct_optimizations/covariance_analysis.cpp +++ b/rct_optimizations/src/rct_optimizations/covariance_analysis.cpp @@ -68,6 +68,39 @@ Eigen::MatrixXd computePoseCovariance(ceres::Problem& problem, const Pose6d& pos return computeDVCovariance(problem, pose.values.data(), 6, options); } +Eigen::MatrixXd computePoseCovariance(ceres::Problem& problem, const Eigen::Vector3d& t, Eigen::Quaterniond& q, + const ceres::Covariance::Options& options) +{ + const ceres::LocalParameterization* q_loc_param = problem.GetParameterization(q.coeffs().data()); + if (q_loc_param->LocalSize() >= q.coeffs().size()) + throw CovarianceException("Locally parameterized size of the quaternion is not smaller than its original size"); + + // Calculate the individual covariance matrices + // Covariance of locally parameterized quaternion with itself + Eigen::MatrixXd q_cov = computeDVCovariance(problem, q.coeffs().data(), q_loc_param->LocalSize()); + // Covariance of position with itself + Eigen::MatrixXd t_cov = computeDVCovariance(problem, t.data(), t.size()); + // Covariance of locally parameterized quaternion covariance with position + Eigen::MatrixXd qt_cov = + computeDV2DVCovariance(problem, q.coeffs().data(), q_loc_param->LocalSize(), t.data(), t.size()); + + Eigen::Index n = q_loc_param->LocalSize() + t.size(); + Eigen::MatrixXd cov; + cov.resize(n, n); + /* Total covariance matrix + * Q P + * Q | C(q,q) | C(q, p) | + * P | C(p,q) | C(p, p) | + */ + cov.resize(6, 6); + cov.block<3, 3>(0, 0) = q_cov; + cov.block<3, 3>(3, 3) = t_cov; + cov.block<3, 3>(0, 3) = qt_cov; + cov.block<3, 3>(3, 0) = qt_cov.transpose(); + + return cov; +} + 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); From fc27555a0ad89acea5454c41d95d3d15289ace23 Mon Sep 17 00:00:00 2001 From: "michael.ripperger@swri.org" Date: Thu, 11 Jun 2020 19:12:50 -0500 Subject: [PATCH 04/10] Added covariance to PnP calibration --- rct_optimizations/include/rct_optimizations/pnp.h | 1 + rct_optimizations/src/rct_optimizations/pnp.cpp | 3 +++ 2 files changed, 4 insertions(+) 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/pnp.cpp b/rct_optimizations/src/rct_optimizations/pnp.cpp index 83b30c17..debd2ea2 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 = computePoseCovariance(problem, t, q); return result; } From 152d61347c7025f5f36d3267b00899dacdc331c3 Mon Sep 17 00:00:00 2001 From: "michael.ripperger@swri.org" Date: Thu, 11 Jun 2020 19:14:43 -0500 Subject: [PATCH 05/10] Added print of covariance to unit test --- rct_optimizations/test/pnp_utest.cpp | 43 ++++++++++++++-------------- 1 file changed, 22 insertions(+), 21 deletions(-) diff --git a/rct_optimizations/test/pnp_utest.cpp b/rct_optimizations/test/pnp_utest.cpp index 0f28052d..d4bdf0c5 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) From 75439ad6c31855ef1bf480eb0b055a0e5eed04b2 Mon Sep 17 00:00:00 2001 From: "michael.ripperger@swri.org" Date: Thu, 11 Jun 2020 19:42:17 -0500 Subject: [PATCH 06/10] Updated covariance matrix to match documentation --- .../rct_optimizations/covariance_analysis.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/rct_optimizations/src/rct_optimizations/covariance_analysis.cpp b/rct_optimizations/src/rct_optimizations/covariance_analysis.cpp index b8ee5944..569d64c8 100644 --- a/rct_optimizations/src/rct_optimizations/covariance_analysis.cpp +++ b/rct_optimizations/src/rct_optimizations/covariance_analysis.cpp @@ -81,22 +81,22 @@ Eigen::MatrixXd computePoseCovariance(ceres::Problem& problem, const Eigen::Vect // Covariance of position with itself Eigen::MatrixXd t_cov = computeDVCovariance(problem, t.data(), t.size()); // Covariance of locally parameterized quaternion covariance with position - Eigen::MatrixXd qt_cov = - computeDV2DVCovariance(problem, q.coeffs().data(), q_loc_param->LocalSize(), t.data(), t.size()); + Eigen::MatrixXd tq_cov = + computeDV2DVCovariance(problem, t.data(), t.size(), q.coeffs().data(), q_loc_param->LocalSize()); Eigen::Index n = q_loc_param->LocalSize() + t.size(); Eigen::MatrixXd cov; cov.resize(n, n); /* Total covariance matrix - * Q P - * Q | C(q,q) | C(q, p) | - * P | C(p,q) | C(p, p) | + * T Q + * T | C(t, t) | C(t, q) | + * Q | C(q, t) | C(q, q) | */ cov.resize(6, 6); - cov.block<3, 3>(0, 0) = q_cov; - cov.block<3, 3>(3, 3) = t_cov; - cov.block<3, 3>(0, 3) = qt_cov; - cov.block<3, 3>(3, 0) = qt_cov.transpose(); + cov.block<3, 3>(0, 0) = t_cov; + cov.block<3, 3>(3, 3) = q_cov; + cov.block<3, 3>(0, 3) = tq_cov; + cov.block<3, 3>(3, 0) = tq_cov.transpose(); return cov; } From f7cfccf6cd4d4ab463dac064145f0f4f25f7b943 Mon Sep 17 00:00:00 2001 From: "michael.ripperger@swri.org" Date: Thu, 11 Jun 2020 19:43:02 -0500 Subject: [PATCH 07/10] Updated printing of covariance matrices --- rct_optimizations/test/pnp_utest.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/rct_optimizations/test/pnp_utest.cpp b/rct_optimizations/test/pnp_utest.cpp index d4bdf0c5..12ccc23a 100644 --- a/rct_optimizations/test/pnp_utest.cpp +++ b/rct_optimizations/test/pnp_utest.cpp @@ -32,7 +32,7 @@ TEST(PNP_2D, PerfectInitialConditions) 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", "[", "]"); + Eigen::IOFormat fmt(4, 0, " | ", "\n", "|", "|"); std::cout << "Covariance:\n" << result.camera_to_target_covariance.format(fmt) << std::endl; } @@ -61,7 +61,7 @@ TEST(PNP_2D, PerturbedInitialCondition) 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", "[", "]"); + Eigen::IOFormat fmt(4, 0, " | ", "\n", "|", "|"); std::cout << "Covariance:\n" << result.camera_to_target_covariance.format(fmt) << std::endl; } @@ -100,7 +100,7 @@ TEST(PNP_2D, BadIntrinsicParameters) 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", "[", "]"); + Eigen::IOFormat fmt(4, 0, " | ", "\n", "|", "|"); std::cout << "Covariance:\n" << result.camera_to_target_covariance.format(fmt) << std::endl; } From 3c823c43881ecccca16f8bc16e2ac7dc0a3e2bc8 Mon Sep 17 00:00:00 2001 From: mripperger Date: Wed, 24 Jun 2020 14:45:39 -0500 Subject: [PATCH 08/10] Updated covariance for Eigen pose --- .../src/rct_optimizations/covariance_analysis.cpp | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/rct_optimizations/src/rct_optimizations/covariance_analysis.cpp b/rct_optimizations/src/rct_optimizations/covariance_analysis.cpp index 569d64c8..07830e22 100644 --- a/rct_optimizations/src/rct_optimizations/covariance_analysis.cpp +++ b/rct_optimizations/src/rct_optimizations/covariance_analysis.cpp @@ -72,27 +72,26 @@ Eigen::MatrixXd computePoseCovariance(ceres::Problem& problem, const Eigen::Vect const ceres::Covariance::Options& options) { const ceres::LocalParameterization* q_loc_param = problem.GetParameterization(q.coeffs().data()); - if (q_loc_param->LocalSize() >= q.coeffs().size()) + + if (q_loc_param->LocalSize() != 3) throw CovarianceException("Locally parameterized size of the quaternion is not smaller than its original size"); // Calculate the individual covariance matrices // Covariance of locally parameterized quaternion with itself - Eigen::MatrixXd q_cov = computeDVCovariance(problem, q.coeffs().data(), q_loc_param->LocalSize()); + Eigen::MatrixXd q_cov = computeDVCovariance(problem, q.coeffs().data(), q_loc_param->LocalSize(), options); // Covariance of position with itself - Eigen::MatrixXd t_cov = computeDVCovariance(problem, t.data(), t.size()); + Eigen::MatrixXd t_cov = computeDVCovariance(problem, t.data(), t.size(), options); // Covariance of locally parameterized quaternion covariance with position Eigen::MatrixXd tq_cov = - computeDV2DVCovariance(problem, t.data(), t.size(), q.coeffs().data(), q_loc_param->LocalSize()); + computeDV2DVCovariance(problem, t.data(), t.size(), q.coeffs().data(), q_loc_param->LocalSize(), options); - Eigen::Index n = q_loc_param->LocalSize() + t.size(); Eigen::MatrixXd cov; - cov.resize(n, n); + cov.resize(6, 6); /* Total covariance matrix * T Q * T | C(t, t) | C(t, q) | * Q | C(q, t) | C(q, q) | */ - cov.resize(6, 6); cov.block<3, 3>(0, 0) = t_cov; cov.block<3, 3>(3, 3) = q_cov; cov.block<3, 3>(0, 3) = tq_cov; From 481ada80df502ffbe9af79356aba1ebd03999db0 Mon Sep 17 00:00:00 2001 From: mripperger Date: Thu, 25 Jun 2020 11:58:46 -0500 Subject: [PATCH 09/10] Pose covariance function fixup --- .../rct_optimizations/covariance_analysis.h | 50 +++++++------- .../rct_optimizations/covariance_analysis.cpp | 66 ++++++++++--------- 2 files changed, 58 insertions(+), 58 deletions(-) diff --git a/rct_optimizations/include/rct_optimizations/covariance_analysis.h b/rct_optimizations/include/rct_optimizations/covariance_analysis.h index 4a89400a..c13e37c8 100644 --- a/rct_optimizations/include/rct_optimizations/covariance_analysis.h +++ b/rct_optimizations/include/rct_optimizations/covariance_analysis.h @@ -76,33 +76,12 @@ Eigen::MatrixXd covToEigenCov(const double* cov, const std::size_t num_vars); */ 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. - * @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 - * --|-------------- - * x | - * y | - * z | - * rx| - * ry| - * 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, const Pose6d& pose, - const ceres::Covariance::Options& options = DefaultCovarianceOptions()); - /** * @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 t the pose translation parameter - * @param q the pose quaternion parameter + * @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 @@ -117,7 +96,7 @@ Eigen::MatrixXd computePoseCovariance(ceres::Problem& problem, const Pose6d& pos * @throw CovarianceException if computation of covariance fails for any pair of parameter blocks, or if * GetCovarianceBlock returns false. */ -Eigen::MatrixXd computePoseCovariance(ceres::Problem& problem, const Eigen::Vector3d& t, Eigen::Quaterniond& q, +Eigen::MatrixXd computePoseCovariance(ceres::Problem& problem, const Pose6d& pose, const ceres::Covariance::Options& options = DefaultCovarianceOptions()); /** @@ -169,7 +148,7 @@ Eigen::MatrixXd computePose2PoseCovariance(ceres::Problem &problem, const Pose6d * @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: @@ -187,9 +166,9 @@ Eigen::MatrixXd computeDVCovariance(ceres::Problem &problem, const double* dptr, * @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: @@ -203,4 +182,23 @@ Eigen::MatrixXd computeDVCovariance(ceres::Problem &problem, const double* dptr, */ 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/src/rct_optimizations/covariance_analysis.cpp b/rct_optimizations/src/rct_optimizations/covariance_analysis.cpp index 07830e22..985d9cf4 100644 --- a/rct_optimizations/src/rct_optimizations/covariance_analysis.cpp +++ b/rct_optimizations/src/rct_optimizations/covariance_analysis.cpp @@ -68,38 +68,6 @@ Eigen::MatrixXd computePoseCovariance(ceres::Problem& problem, const Pose6d& pos return computeDVCovariance(problem, pose.values.data(), 6, options); } -Eigen::MatrixXd computePoseCovariance(ceres::Problem& problem, const Eigen::Vector3d& t, Eigen::Quaterniond& q, - const ceres::Covariance::Options& options) -{ - const ceres::LocalParameterization* q_loc_param = problem.GetParameterization(q.coeffs().data()); - - if (q_loc_param->LocalSize() != 3) - throw CovarianceException("Locally parameterized size of the quaternion is not smaller than its original size"); - - // Calculate the individual covariance matrices - // Covariance of locally parameterized quaternion with itself - Eigen::MatrixXd q_cov = computeDVCovariance(problem, q.coeffs().data(), q_loc_param->LocalSize(), options); - // Covariance of position with itself - Eigen::MatrixXd t_cov = computeDVCovariance(problem, t.data(), t.size(), options); - // Covariance of locally parameterized quaternion covariance with position - Eigen::MatrixXd tq_cov = - computeDV2DVCovariance(problem, t.data(), t.size(), q.coeffs().data(), q_loc_param->LocalSize(), options); - - Eigen::MatrixXd cov; - cov.resize(6, 6); - /* Total covariance matrix - * T Q - * T | C(t, t) | C(t, q) | - * Q | C(q, t) | C(q, q) | - */ - cov.block<3, 3>(0, 0) = t_cov; - cov.block<3, 3>(3, 3) = q_cov; - cov.block<3, 3>(0, 3) = tq_cov; - cov.block<3, 3>(3, 0) = tq_cov.transpose(); - - return cov; -} - 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); @@ -148,4 +116,38 @@ Eigen::MatrixXd computeDV2DVCovariance(ceres::Problem &P, const double* dptr1, c 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 From 44a6911d747421812d7c1d807f3d0f6ee4f078db Mon Sep 17 00:00:00 2001 From: mripperger Date: Thu, 25 Jun 2020 11:59:09 -0500 Subject: [PATCH 10/10] Added covariance to unit test fixup --- rct_optimizations/src/rct_optimizations/pnp.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rct_optimizations/src/rct_optimizations/pnp.cpp b/rct_optimizations/src/rct_optimizations/pnp.cpp index debd2ea2..a964a215 100644 --- a/rct_optimizations/src/rct_optimizations/pnp.cpp +++ b/rct_optimizations/src/rct_optimizations/pnp.cpp @@ -86,7 +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 = computePoseCovariance(problem, t, q); + result.camera_to_target_covariance = computeFullDV2DVCovariance(problem, t.data(), t.size(), q.coeffs().data(), q_param->LocalSize()); return result; }