diff --git a/include/ceres/covariance.h b/include/ceres/covariance.h index dd20dc36b..dd87868fc 100644 --- a/include/ceres/covariance.h +++ b/include/ceres/covariance.h @@ -357,6 +357,28 @@ class CERES_EXPORT Covariance { const double*> >& covariance_blocks, Problem* problem); + // Compute a part of the covariance matrix. + // + // The vector parameter_blocks contains the parameter blocks that + // are used for computing the covariance matrix. From this vector + // all covariance pairs are generated. This allows the covariance + // estimation algorithm to only compute and store these blocks. + // + // parameter_blocks cannot contain duplicates. Bad things will + // happen if they do. + // + // Note that the list of covariance_blocks is only used to determine + // what parts of the covariance matrix are computed. The full + // Jacobian is used to do the computation, i.e. they do not have an + // impact on what part of the Jacobian is used for computation. + // + // The return value indicates the success or failure of the + // covariance computation. Please see the documentation for + // Covariance::Options for more on the conditions under which this + // function returns false. + bool Compute(const std::vector& parameter_blocks, + Problem* problem); + // Return the block of the cross-covariance matrix corresponding to // parameter_block1 and parameter_block2. // @@ -394,6 +416,40 @@ class CERES_EXPORT Covariance { const double* parameter_block2, double* covariance_block) const; + // Return the covariance matrix corresponding to all parameter_blocks. + // + // Compute must be called before calling GetCovarianceMatrix and all + // parameter_blocks must have been present in the vector + // parameters_blocks when Compute was called. Otherwise + // GetCovarianceMatrix returns false. + // + // covariance_matrix must point to a memory location that can store + // the size of the covariance matrix. The size of the covariance + // matrix corresponds the sum of the sizes of the individual + // parameter blocks. The covariance matrix will be a row-major + // matrix. + bool GetCovarianceMatrix(const std::vector& parameter_blocks, + double* covariance_matrix); + + // Return the covariance matrix corresponding to parameter_blocks + // in the tangent space if a local parameterization is associated + // with one of the parameter blocks else returns the covariance + // matrix in the ambient space. + // + // Compute must be called before calling GetCovarianceMatrix and all + // parameter_blocks must have been present in the vector + // parameters_blocks when Compute was called. Otherwise + // GetCovarianceMatrix returns false. + // + // covariance_matrix must point to a memory location that can store + // the size of the covariance matrix. The size of the covariance + // matrix corresponds the sum of the sizes of the individual + // parameter blocks. The covariance matrix will be a row-major + // matrix. + bool GetCovarianceMatrixInTangentSpace( + const std::vector& parameter_blocks, + double* covariance_matrix); + private: internal::scoped_ptr impl_; }; diff --git a/internal/ceres/covariance.cc b/internal/ceres/covariance.cc index 690847945..cb280a368 100644 --- a/internal/ceres/covariance.cc +++ b/internal/ceres/covariance.cc @@ -38,6 +38,7 @@ namespace ceres { +using std::make_pair; using std::pair; using std::vector; @@ -54,6 +55,12 @@ bool Covariance::Compute( return impl_->Compute(covariance_blocks, problem->problem_impl_.get()); } +bool Covariance::Compute( + const vector& parameter_blocks, + Problem* problem) { + return impl_->Compute(parameter_blocks, problem->problem_impl_.get()); +} + bool Covariance::GetCovarianceBlock(const double* parameter_block1, const double* parameter_block2, double* covariance_block) const { @@ -73,4 +80,20 @@ bool Covariance::GetCovarianceBlockInTangentSpace( covariance_block); } +bool Covariance::GetCovarianceMatrix( + const vector& parameter_blocks, + double* covariance_matrix) { + return impl_->GetCovarianceMatrixInTangentOrAmbientSpace(parameter_blocks, + true, // ambient + covariance_matrix); +} + +bool Covariance::GetCovarianceMatrixInTangentSpace( + const std::vector& parameter_blocks, + double *covariance_matrix) { + return impl_->GetCovarianceMatrixInTangentOrAmbientSpace(parameter_blocks, + false, // tangent + covariance_matrix); +} + } // namespace ceres diff --git a/internal/ceres/covariance_impl.cc b/internal/ceres/covariance_impl.cc index ff65c77f0..198a13e4f 100644 --- a/internal/ceres/covariance_impl.cc +++ b/internal/ceres/covariance_impl.cc @@ -36,6 +36,8 @@ #include #include +#include +#include #include #include @@ -63,6 +65,7 @@ namespace internal { using std::make_pair; using std::map; using std::pair; +using std::sort; using std::swap; using std::vector; @@ -88,8 +91,37 @@ CovarianceImpl::CovarianceImpl(const Covariance::Options& options) CovarianceImpl::~CovarianceImpl() { } +template void CheckForDuplicates(vector blocks) { + sort(blocks.begin(), blocks.end()); + typename vector::iterator it = std::adjacent_find(blocks.begin(), blocks + .end()); + if (it != blocks.end()) { + // In case there are duplicates, we search for their location. + map > blocks_map; + for (int i=0; i < blocks.size(); ++i) { + blocks_map[blocks[i]].push_back(i); + } + std::ostringstream duplicates; + while (it != blocks.end()) { + duplicates << "("; + for (int i=0; i < blocks_map[*it].size()-1; ++i) { + duplicates << blocks_map[*it][i] << ", "; + } + duplicates << blocks_map[*it].back() << ")"; + it = std::adjacent_find(it + 1, blocks.end()); + if (it < blocks.end()) { + duplicates << " and "; + } + } + CHECK(false) << "Covariance::Compute called with duplicate blocks at " + << "indices " + << duplicates.str(); + } +} + bool CovarianceImpl::Compute(const CovarianceBlocks& covariance_blocks, ProblemImpl* problem) { + CheckForDuplicates >(covariance_blocks); problem_ = problem; parameter_block_to_row_index_.clear(); covariance_matrix_.reset(NULL); @@ -99,6 +131,25 @@ bool CovarianceImpl::Compute(const CovarianceBlocks& covariance_blocks, return is_valid_; } +bool CovarianceImpl::Compute(const vector& parameter_blocks, + ProblemImpl* problem) { + CheckForDuplicates(parameter_blocks); + vector > covariance_blocks; + for (int i = 0; i < parameter_blocks.size(); ++i) { + for (int j = i; j < parameter_blocks.size(); ++j) { + covariance_blocks.push_back(make_pair(parameter_blocks[i], + parameter_blocks[j])); + } + } + problem_ = problem; + parameter_block_to_row_index_.clear(); + covariance_matrix_.reset(NULL); + is_valid_ = (ComputeCovarianceSparsity(covariance_blocks, problem) && + ComputeCovarianceValues()); + is_computed_ = true; + return is_valid_; +} + bool CovarianceImpl::GetCovarianceBlockInTangentOrAmbientSpace( const double* original_parameter_block1, const double* original_parameter_block2, @@ -250,6 +301,83 @@ bool CovarianceImpl::GetCovarianceBlockInTangentOrAmbientSpace( return true; } +bool CovarianceImpl::GetCovarianceMatrixInTangentOrAmbientSpace( + const vector& parameters, + bool lift_covariance_to_ambient_space, + double* covariance_matrix) const { + CHECK(is_computed_) + << "Covariance::GetCovarianceMatrix called before Covariance::Compute"; + CHECK(is_valid_) + << "Covariance::GetCovarianceMatrix called when Covariance::Compute " + << "returned false."; + + const ProblemImpl::ParameterMap& parameter_map = problem_->parameter_map(); + // For OpenMP compatibility we need to define these vectors in advance + vector parameter_sizes; + vector cum_parameter_size; + parameter_sizes.reserve(parameters.size()); + cum_parameter_size.resize(parameters.size() + 1); + cum_parameter_size[0] = 0; + for (int i = 0; i < parameters.size(); ++i) { + ParameterBlock* block = FindOrDie(parameter_map, + const_cast(parameters[i])); + if (lift_covariance_to_ambient_space) { + parameter_sizes.push_back(block->Size()); + } else { + parameter_sizes.push_back(block->LocalSize()); + } + } + std::partial_sum(parameter_sizes.begin(), parameter_sizes.end(), + cum_parameter_size.begin() + 1); + const int max_covariance_block_size = *std::max_element( + parameter_sizes.begin(), + parameter_sizes.end()); + const int covariance_size = cum_parameter_size.back(); + + // Assemble the blocks in the covariance matrix. + MatrixRef covariance(covariance_matrix, covariance_size, covariance_size); + const int num_threads = options_.num_threads; + scoped_array workspace(new double[num_threads * + max_covariance_block_size * max_covariance_block_size]); + + bool success = true; +#pragma omp parallel for num_threads (num_threads) schedule(dynamic) collapse(2) + for (int i = 0; i < parameters.size(); ++i) { + for (int j = 0; j < parameters.size(); ++j) { + // The second loop can't start from j = i for compatibility with OpenMP + // collapse command. The conditional serves as a workaround + if (j >= i) { + int covariance_row_idx = cum_parameter_size[i]; + int covariance_col_idx = cum_parameter_size[j]; + int size_i = parameter_sizes[i]; + int size_j = parameter_sizes[j]; +#ifdef CERES_USE_OPENMP + int thread_id = omp_get_thread_num(); +#else + int thread_id = 0; +#endif + double* covariance_block = workspace.get() + thread_id * + max_covariance_block_size * max_covariance_block_size; + if (!GetCovarianceBlockInTangentOrAmbientSpace( + parameters[i], + parameters[j], + lift_covariance_to_ambient_space, + covariance_block)) { + success = false; + } + covariance.block(covariance_row_idx, covariance_col_idx, size_i, + size_j) = MatrixRef(covariance_block, size_i, size_j); + if (i != j) { + covariance.block(covariance_col_idx, covariance_row_idx, size_j, + size_i) = MatrixRef(covariance_block, size_i, + size_j).transpose(); + } + } + } + } + return success; +} + // Determine the sparsity pattern of the covariance matrix based on // the block pairs requested by the user. bool CovarianceImpl::ComputeCovarianceSparsity( diff --git a/internal/ceres/covariance_impl.h b/internal/ceres/covariance_impl.h index eb0cd0406..a3f0761f5 100644 --- a/internal/ceres/covariance_impl.h +++ b/internal/ceres/covariance_impl.h @@ -55,12 +55,21 @@ class CovarianceImpl { const double*> >& covariance_blocks, ProblemImpl* problem); + bool Compute( + const std::vector& parameter_blocks, + ProblemImpl* problem); + bool GetCovarianceBlockInTangentOrAmbientSpace( const double* parameter_block1, const double* parameter_block2, bool lift_covariance_to_ambient_space, double* covariance_block) const; + bool GetCovarianceMatrixInTangentOrAmbientSpace( + const std::vector& parameters, + bool lift_covariance_to_ambient_space, + double *covariance_matrix) const; + bool ComputeCovarianceSparsity( const std::vector >& covariance_blocks, diff --git a/internal/ceres/covariance_test.cc b/internal/ceres/covariance_test.cc index 6b90d6ef6..63f85c4f1 100644 --- a/internal/ceres/covariance_test.cc +++ b/internal/ceres/covariance_test.cc @@ -963,6 +963,120 @@ TEST_F(CovarianceTest, TruncatedRank) { } } +TEST_F(CovarianceTest, DenseCovarianceMatrixFromSetOfParameters) { + Covariance::Options options; + Covariance covariance(options); + double* x = parameters_; + double* y = x + 2; + double* z = y + 3; + vector parameter_blocks; + parameter_blocks.push_back(x); + parameter_blocks.push_back(y); + parameter_blocks.push_back(z); + covariance.Compute(parameter_blocks, &problem_); + double expected_covariance[36]; + covariance.GetCovarianceMatrix(parameter_blocks, expected_covariance); + +#ifndef CERES_NO_SUITESPARSE + options.algorithm_type = SUITE_SPARSE_QR; + ComputeAndCompareCovarianceBlocks(options, expected_covariance); +#endif + + options.algorithm_type = DENSE_SVD; + ComputeAndCompareCovarianceBlocks(options, expected_covariance); + + options.algorithm_type = EIGEN_SPARSE_QR; + ComputeAndCompareCovarianceBlocks(options, expected_covariance); +} + +TEST_F(CovarianceTest, DenseCovarianceMatrixFromSetOfParametersThreaded) { + Covariance::Options options; + options.num_threads = 4; + Covariance covariance(options); + double* x = parameters_; + double* y = x + 2; + double* z = y + 3; + vector parameter_blocks; + parameter_blocks.push_back(x); + parameter_blocks.push_back(y); + parameter_blocks.push_back(z); + covariance.Compute(parameter_blocks, &problem_); + double expected_covariance[36]; + covariance.GetCovarianceMatrix(parameter_blocks, expected_covariance); + +#ifndef CERES_NO_SUITESPARSE + options.algorithm_type = SUITE_SPARSE_QR; + ComputeAndCompareCovarianceBlocks(options, expected_covariance); +#endif + + options.algorithm_type = DENSE_SVD; + ComputeAndCompareCovarianceBlocks(options, expected_covariance); + + options.algorithm_type = EIGEN_SPARSE_QR; + ComputeAndCompareCovarianceBlocks(options, expected_covariance); +} + +TEST_F(CovarianceTest, DenseCovarianceMatrixFromSetOfParametersInTangentSpace) { + Covariance::Options options; + Covariance covariance(options); + double* x = parameters_; + double* y = x + 2; + double* z = y + 3; + + problem_.SetParameterization(x, new PolynomialParameterization); + + vector subset; + subset.push_back(2); + problem_.SetParameterization(y, new SubsetParameterization(3, subset)); + + local_column_bounds_[x] = make_pair(0, 1); + local_column_bounds_[y] = make_pair(1, 3); + local_column_bounds_[z] = make_pair(3, 4); + + vector parameter_blocks; + parameter_blocks.push_back(x); + parameter_blocks.push_back(y); + parameter_blocks.push_back(z); + covariance.Compute(parameter_blocks, &problem_); + double expected_covariance[16]; + covariance.GetCovarianceMatrixInTangentSpace(parameter_blocks, + expected_covariance); + +#ifndef CERES_NO_SUITESPARSE + options.algorithm_type = SUITE_SPARSE_QR; + ComputeAndCompareCovarianceBlocksInTangentSpace(options, expected_covariance); +#endif + + options.algorithm_type = DENSE_SVD; + ComputeAndCompareCovarianceBlocksInTangentSpace(options, expected_covariance); + + options.algorithm_type = EIGEN_SPARSE_QR; + ComputeAndCompareCovarianceBlocksInTangentSpace(options, expected_covariance); +} + +TEST_F(CovarianceTest, ComputeCovarianceFailure) { + Covariance::Options options; + Covariance covariance(options); + double* x = parameters_; + double* y = x + 2; + vector parameter_blocks; + parameter_blocks.push_back(x); + parameter_blocks.push_back(x); + parameter_blocks.push_back(y); + parameter_blocks.push_back(y); + EXPECT_DEATH_IF_SUPPORTED(covariance.Compute(parameter_blocks, &problem_), + "Covariance::Compute called with duplicate blocks " + "at indices \\(0, 1\\) and \\(2, 3\\)"); + vector > covariance_blocks; + covariance_blocks.push_back(make_pair(x, x)); + covariance_blocks.push_back(make_pair(x, x)); + covariance_blocks.push_back(make_pair(y, y)); + covariance_blocks.push_back(make_pair(y, y)); + EXPECT_DEATH_IF_SUPPORTED(covariance.Compute(covariance_blocks, &problem_), + "Covariance::Compute called with duplicate blocks " + "at indices \\(0, 1\\) and \\(2, 3\\)"); +} + class RankDeficientCovarianceTest : public CovarianceTest { protected: virtual void SetUp() {