Skip to content

Commit

Permalink
PnP update (#64)
Browse files Browse the repository at this point in the history 
* Made covariance functions take const inputs

* Added function for getting full covariance matrix between two parameters

* Added covariance to PnP calibration

* Added print of covariance to unit test

* Updated printing of covariance matrices

* Updated cost function to use unnormalized axis angle

* Updated unit test to check covariance

* Reduced residual expectation for perturbed case

* Revised PnP unit test with test fixture

* Updated CI YAML to print CTest details on test failure

* Corrected 3D PnP optimization; added covariance calculation

* Updated PnP 3D unit tests

* Updated CI config

* CI fixup

* Modified expectation for final cost per observation for perturbed initial condition

* Updated names of variables

* Updated unit test to have expectations on mean and variance of optimization results for perturbed tests
  • Loading branch information
@marip8
marip8 authored Jul 13, 2020
1 parent 089b432 commit 530b7bd
Show file tree
Hide file tree
Showing 6 changed files with 244 additions and 132 deletions.
6 changes: 4 additions & 2 deletions .github/workflows/main.yml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand All @@ -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
Expand Down
41 changes: 30 additions & 11 deletions rct_optimizations/include/rct_optimizations/covariance_analysis.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand All @@ -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);


/**
Expand All @@ -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.
Expand All @@ -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.
Expand All @@ -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.
Expand All @@ -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:
Expand All @@ -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:
Expand All @@ -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
1 change: 1 addition & 0 deletions rct_optimizations/include/rct_optimizations/pnp.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
50 changes: 42 additions & 8 deletions rct_optimizations/src/rct_optimizations/covariance_analysis.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand All @@ -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);

Expand All @@ -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);

Expand All @@ -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);

Expand All @@ -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);

Expand All @@ -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
Loading

0 comments on commit 530b7bd

Please sign in to comment.