diff --git a/include/ceres/covariance.h b/include/ceres/covariance.h new file mode 100644 index 000000000..1e9bb9fdb --- /dev/null +++ b/include/ceres/covariance.h @@ -0,0 +1,280 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2013 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_PUBLIC_COVARIANCE_H_ +#define CERES_PUBLIC_COVARIANCE_H_ + +#include +#include +#include "ceres/internal/port.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { + +class Problem; + +namespace internal { +class CovarianceImpl; +} // namespace internal + +// WARNINGS +// ======== +// +// 1. This is experimental code and the API WILL CHANGE before +// release. +// +// 2. WARNING: It is very easy to use this class incorrectly without +// understanding the underlying mathematics. Please read and +// understand the documentation completely before attempting to use +// this class. +// +// One way to assess the quality of the solution returned by a +// non-linear least squares solve is to analyze the covariance of the +// solution. +// +// Let us consider the non-linear regression problem +// +// y = f(x) + N(0, I) +// +// i.e., the observation y is a random non-linear function of the +// independent variable x with mean f(x) and identity covariance. Then +// the maximum likelihood estimate of x given observations y is the +// solution to the non-linear least squares problem: +// +// x* = arg min_x |f(x)|^2 +// +// And the covariance of x* is given by +// +// C(x*) = inverse[J'(x*)J(x*)] +// +// Here J(x*) is the Jacobian of f at x*. The above formula assumes +// that J(x*) has full column rank. +// +// If J(x*) is rank deficient, then the covariance matrix C(x*) is +// also rank deficient and is given by +// +// C(x*) = pseudoinverse[J'(x*)J(x*)] +// +// WARNING +// ======= +// +// Note that in the above, we assumed that the covariance +// matrix for y was identity. This is an important assumption. If this +// is not the case and we have +// +// y = f(x) + N(0, S) +// +// Where S is a positive semi-definite matrix denoting the covariance +// of y, then the maximum likelihood problem to be solved is +// +// x* = arg min_x f'(x) inverse[S] f(x) +// +// and the corresponding covariance estimate of x* is given by +// +// C(x*) = inverse[J'(x*) inverse[S] J(x*)] +// +// So, if it is the case that the observations being fitted to have a +// covariance matrix not equal to identity, then it is the user's +// responsibility that the corresponding cost functions are correctly +// scaled, e.g. in the above case the cost function for this problem +// should evaluate S^{-1/2} f(x) instead of just f(x), where S^{-1/2} +// is the inverse square root of the covariance matrix S. +// +// This class allows the user to evaluate the covariance for a +// non-linear least squares problem and provides random access to its +// blocks. The computation assumes that the CostFunctions compute +// residuals such that their covariance is identity. +// +// Since the computation of the covariance matrix involves computing +// the inverse of a potentially large matrix, this can involve a +// rather large amount of time and memory. However, it is usually the +// case that the user is only interested in a small part of the +// covariance matrix. Quite often just the block diagonal. This class +// allows the user to specify the parts of the covariance matrix that +// she is interested in and then uses this information to only compute +// and store those parts of the covariance matrix. +// +// Rank of the Jacobian +// ==================== +// As we noted above, if the jacobian is rank deficient, then the +// inverse of J'J is not defined and instead a pseudo inverse needs to +// be computed. +// +// The rank deficiency in J can be structural -- columns which are +// always known to be zero or numerical -- depending on the exact +// values in the Jacobian. This happens when the problem contains +// parameter blocks that are constant. This class correctly handles +// structural rank deficiency like that. +// +// Numerical rank deficiency, where the rank of the matrix cannot be +// predicted by its sparsity structure and requires looking at its +// numerical values is more complicated. Here again there are two +// cases. +// +// a. The rank deficiency arises from overparameterization. e.g., a +// four dimensional quaternion used to parameterize SO(3), which is +// a three dimensional manifold. In cases like this, the user should +// use an appropriate LocalParameterization. Not only will this lead +// to better numerical behaviour of the Solver, it will also expose +// the rank deficiency to the Covariance object so that it can +// handle it correctly. +// +// b. More general numerical rank deficiency in the Jacobian +// requires the computation of the so called Singular Value +// Decomposition (SVD) of J'J. We do not know how to do this for +// large sparse matrices efficiently. For small and moderate sized +// problems this is done using dense linear algebra. +// +// Gauge Invariance +// ---------------- +// In structure from motion (3D reconstruction) problems, the +// reconstruction is ambiguous upto a similarity transform. This is +// known as a Gauge Ambiguity. Handling Gauges correctly requires the +// use of SVD or custom inversion algorithms. For small problems the +// user can use the dense algorithm. For more details see Morris, +// Kanatani & Kanade's work the subject. +// +// Example Usage +// ============= +// +// double x[3]; +// double y[2]; +// +// Problem problem; +// problem.AddParameterBlock(x, 3); +// problem.AddParameterBlock(y, 2); +// +// +// +// Covariance::Options options; +// Covariance covariance(options); +// +// vector > covariance_blocks; +// covariance_blocks.push_back(make_pair(x, x)); +// covariance_blocks.push_back(make_pair(y, y)); +// covariance_blocks.push_back(make_pair(x, y)); +// +// CHECK(covariance.Compute(covariance_blocks, &problem)); +// +// double covariance_xx[3 * 3]; +// double covariance_yy[2 * 2]; +// double covariance_xy[3 * 2]; +// covariance.GetCovarianceBlock(x, x, covariance_xx) +// covariance.GetCovarianceBlock(y, y, covariance_yy) +// covariance.GetCovarianceBlock(x, y, covariance_xy) +// +class Covariance { + public: + struct Options { + Options() + : num_threads(1), +#ifndef CERES_NO_SUITESPARSE + use_dense_linear_algebra(false), +#else + use_dense_linear_algebra(true), +#endif + min_singular_value_threshold(1e-8), + null_space_rank(0), + apply_loss_function(true) { + } + + // Number of threads to be used for evaluating the Jacobian and + // estimation of covariance. + int num_threads; + + // Use Eigen's JacobiSVD algorithm to compute the covariance. This + // is a very accurate but slow algorithm. The up side is that it + // can handle numerically rank deficient jacobians. This option + // only makes sense for small to moderate sized problems. + bool use_dense_linear_algebra; + + // When use_dense_linear_algebra is true, singular values less + // than min_singular_value_threshold are set to zero. + double min_singular_value_threshold; + + // When use_dense_linear_algebra is true, the bottom + // null_space_rank singular values are set to zero. + int null_space_rank; + + // Even though the residual blocks in the problem may contain loss + // functions, setting apply_loss_function to false will turn off + // the application of the loss function to the output of the cost + // function and in turn its effect on the covariance. + // + // TODO(sameergaarwal): Expand this based on Jim's experiments. + bool apply_loss_function; + }; + + explicit Covariance(const Options& options); + + // Compute a part of the covariance matrix. + // + // The vector covariance_blocks, indexes into the covariance matrix + // block-wise using pairs of parameter blocks. This allows the + // covariance estimation algorithm to only compute and store these + // blocks. + // + // Since the covariance matrix is symmetric, if the user passes + // (block1, block2), then GetCovarianceBlock can be called with + // block1, block2 as well as block2, block1. + // + // covariance_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. + bool Compute( + const vector >& covariance_blocks, + Problem* problem); + + // Compute must be called before the first call to + // GetCovarianceBlock. covariance_block must point to a memory + // location that can store a parameter_block1_size x + // parameter_block2_size matrix. The returned covariance will be a + // row-major matrix. + // + // The pair OR the pair + // must have been present in + // the vector covariance_blocks when Compute was called. Otherwise + // GetCovarianceBlock will return false. + bool GetCovarianceBlock(const double* parameter_block1, + const double* parameter_block2, + double* covariance_block) const; + + private: + internal::scoped_ptr impl_; +}; + +} // namespace ceres + +#endif // CERES_PUBLIC_COVARIANCE_H_ diff --git a/include/ceres/problem.h b/include/ceres/problem.h index 33394ce0e..663616ddb 100644 --- a/include/ceres/problem.h +++ b/include/ceres/problem.h @@ -329,12 +329,12 @@ class Problem { int NumResiduals() const; // The size of the parameter block. - int ParameterBlockSize(double* values) const; + int ParameterBlockSize(const double* values) const; // The size of local parameterization for the parameter block. If // there is no local parameterization associated with this parameter // block, then ParameterBlockLocalSize = ParameterBlockSize. - int ParameterBlockLocalSize(double* values) const; + int ParameterBlockLocalSize(const double* values) const; // Fills the passed parameter_blocks vector with pointers to the // parameter blocks currently in the problem. After this call, @@ -423,6 +423,7 @@ class Problem { private: friend class Solver; + friend class Covariance; internal::scoped_ptr problem_impl_; CERES_DISALLOW_COPY_AND_ASSIGN(Problem); }; diff --git a/internal/ceres/CMakeLists.txt b/internal/ceres/CMakeLists.txt index 392d0586e..df67f6873 100644 --- a/internal/ceres/CMakeLists.txt +++ b/internal/ceres/CMakeLists.txt @@ -48,6 +48,8 @@ SET(CERES_INTERNAL_SRC conjugate_gradients_solver.cc coordinate_descent_minimizer.cc corrector.cc + covariance.cc + covariance_impl.cc cxsparse.cc dense_normal_cholesky_solver.cc dense_qr_solver.cc @@ -250,6 +252,7 @@ IF (${BUILD_TESTING} AND ${GFLAGS}) CERES_TEST(conditioned_cost_function) CERES_TEST(corrector) CERES_TEST(cost_function_to_functor) + CERES_TEST(covariance) CERES_TEST(dense_sparse_matrix) CERES_TEST(dynamic_autodiff_cost_function) CERES_TEST(evaluator) diff --git a/internal/ceres/covariance.cc b/internal/ceres/covariance.cc new file mode 100644 index 000000000..6cb52eed9 --- /dev/null +++ b/internal/ceres/covariance.cc @@ -0,0 +1,59 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2013 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/covariance.h" + +#include +#include +#include "ceres/covariance_impl.h" +#include "ceres/problem.h" +#include "ceres/problem_impl.h" + +namespace ceres { + +Covariance::Covariance(const Covariance::Options& options) { + impl_.reset(new internal::CovarianceImpl(options)); +} + +bool Covariance::Compute( + const vector >& covariance_blocks, + Problem* problem) { + return impl_->Compute(covariance_blocks, problem->problem_impl_.get()); +} + +bool Covariance::GetCovarianceBlock(const double* parameter_block1, + const double* parameter_block2, + double* covariance_block) const { + return impl_->GetCovarianceBlock(parameter_block1, + parameter_block2, + covariance_block); +} + +} // namespace ceres diff --git a/internal/ceres/covariance_impl.cc b/internal/ceres/covariance_impl.cc new file mode 100644 index 000000000..1f73ffd8e --- /dev/null +++ b/internal/ceres/covariance_impl.cc @@ -0,0 +1,535 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2013 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/covariance_impl.h" + +#include +#include +#include +#include "Eigen/SVD" +#include "ceres/compressed_row_sparse_matrix.h" +#include "ceres/covariance.h" +#include "ceres/crs_matrix.h" +#include "ceres/internal/eigen.h" +#include "ceres/map_util.h" +#include "ceres/parameter_block.h" +#include "ceres/problem_impl.h" +#include "ceres/suitesparse.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +typedef vector > CovarianceBlocks; + +CovarianceImpl::CovarianceImpl(const Covariance::Options& options) + : options_(options) { + evaluate_options_.num_threads = options.num_threads; + evaluate_options_.apply_loss_function = options.apply_loss_function; +} + +CovarianceImpl::~CovarianceImpl() { +} + +bool CovarianceImpl::Compute(const CovarianceBlocks& covariance_blocks, + ProblemImpl* problem) { + problem_ = problem; + + parameter_block_to_row_index_.clear(); + covariance_matrix_.reset(NULL); + + return (ComputeCovarianceSparsity(covariance_blocks, problem) && + ComputeCovarianceValues()); +} + +bool CovarianceImpl::GetCovarianceBlock(const double* original_parameter_block1, + const double* original_parameter_block2, + double* covariance_block) const { + // If either of the two parameter blocks is constant, then the + // covariance block is also zero. + if (constant_parameter_blocks_.count(original_parameter_block1) > 0 || + constant_parameter_blocks_.count(original_parameter_block2) > 0) { + const ProblemImpl::ParameterMap& parameter_map = problem_->parameter_map(); + ParameterBlock* block1 = + FindOrDie(parameter_map, + const_cast(original_parameter_block1)); + + ParameterBlock* block2 = + FindOrDie(parameter_map, + const_cast(original_parameter_block2)); + const int block1_size = block1->Size(); + const int block2_size = block2->Size(); + MatrixRef(covariance_block, block1_size, block2_size).setZero(); + return true; + } + + const double* parameter_block1 = original_parameter_block1; + const double* parameter_block2 = original_parameter_block2; + const bool transpose = parameter_block1 > parameter_block2; + if (transpose) { + std::swap(parameter_block1, parameter_block2); + } + + // Find where in the covariance matrix the block is located. + const int row_begin = + FindOrDie(parameter_block_to_row_index_, parameter_block1); + const int col_begin = + FindOrDie(parameter_block_to_row_index_, parameter_block2); + const int* rows = covariance_matrix_->rows(); + const int* cols = covariance_matrix_->cols(); + const int row_size = rows[row_begin + 1] - rows[row_begin]; + const int* cols_begin = cols + rows[row_begin]; + + // The only part that requires work is walking the compressed column + // vector to determine where the set of columns correspnding to the + // covariance block begin. + int offset = 0; + while (cols_begin[offset] != col_begin && offset < row_size) { + ++offset; + } + + if (offset == row_size) { + LOG(WARNING) << "Unable to find covariance block for " + << original_parameter_block1 << " " + << original_parameter_block2; + return false; + } + + const ProblemImpl::ParameterMap& parameter_map = problem_->parameter_map(); + ParameterBlock* block1 = + FindOrDie(parameter_map, const_cast(parameter_block1)); + ParameterBlock* block2 = + FindOrDie(parameter_map, const_cast(parameter_block2)); + const LocalParameterization* local_param1 = block1->local_parameterization(); + const LocalParameterization* local_param2 = block2->local_parameterization(); + const int block1_size = block1->Size(); + const int block1_local_size = block1->LocalSize(); + const int block2_size = block2->Size(); + const int block2_local_size = block2->LocalSize(); + + ConstMatrixRef cov(covariance_matrix_->values() + rows[row_begin], + block1_size, + row_size); + + // Fast path when there are no local parameterizations. + if (local_param1 == NULL && local_param2 == NULL) { + if (transpose) { + MatrixRef(covariance_block, block2_size, block1_size) = + cov.block(0, offset, block1_size, block2_size).transpose(); + } else { + MatrixRef(covariance_block, block1_size, block2_size) = + cov.block(0, offset, block1_size, block2_size); + } + return true; + } + + // If local parameterizations are used then the covariance that has + // been computed is in the tangent space and it needs to be lifted + // back to the ambient space. + // + // This is given by the formula + // + // C'_12 = J_1 C_12 J_2' + // + // Where C_12 is the local tangent space covariance for parameter + // blocks 1 and 2. J_1 and J_2 are respectively the local to global + // jacobians for parameter blocks 1 and 2. + // + // See Result 5.11 on page 142 of Hartley & Zisserman (2nd Edition) + // for a proof. + // + // TODO(sameeragarwal): Add caching of local parameterization, so + // that they are computed just once per parameter block. + Matrix block1_jacobian(block1_size, block1_local_size); + if (local_param1 == NULL) { + block1_jacobian.setIdentity(); + } else { + local_param1->ComputeJacobian(parameter_block1, block1_jacobian.data()); + } + + Matrix block2_jacobian(block2_size, block2_local_size); + // Fast path if the user is requesting a diagonal block. + if (parameter_block1 == parameter_block2) { + block2_jacobian = block1_jacobian; + } else { + if (local_param2 == NULL) { + block2_jacobian.setIdentity(); + } else { + local_param2->ComputeJacobian(parameter_block2, block2_jacobian.data()); + } + } + + if (transpose) { + MatrixRef(covariance_block, block2_size, block1_size) = + block2_jacobian * + cov.block(0, offset, block1_local_size, block2_local_size).transpose() * + block1_jacobian.transpose(); + } else { + MatrixRef(covariance_block, block1_size, block2_size) = + block1_jacobian * + cov.block(0, offset, block1_local_size, block2_local_size) * + block2_jacobian.transpose(); + } + + return true; +} + +// Determine the sparsity pattern of the covariance matrix based on +// the block pairs requested by the user. +bool CovarianceImpl::ComputeCovarianceSparsity( + const CovarianceBlocks& original_covariance_blocks, + ProblemImpl* problem) { + + // Determine an ordering for the parameter block, by sorting the + // parameter blocks by their pointers. + vector all_parameter_blocks; + problem->GetParameterBlocks(&all_parameter_blocks); + const ProblemImpl::ParameterMap& parameter_map = problem->parameter_map(); + constant_parameter_blocks_.clear(); + vector& active_parameter_blocks = evaluate_options_.parameter_blocks; + active_parameter_blocks.clear(); + for (int i = 0; i < all_parameter_blocks.size(); ++i) { + double* parameter_block = all_parameter_blocks[i]; + + ParameterBlock* block = FindOrDie(parameter_map, parameter_block); + if (block->IsConstant()) { + constant_parameter_blocks_.insert(parameter_block); + } else { + active_parameter_blocks.push_back(parameter_block); + } + } + + sort(active_parameter_blocks.begin(), active_parameter_blocks.end()); + + // Compute the number of rows. Map each parameter block to the + // first row corresponding to it in the covariance matrix using the + // ordering of parameter blocks just constructed. + int num_rows = 0; + parameter_block_to_row_index_.clear(); + for (int i = 0; i < active_parameter_blocks.size(); ++i) { + double* parameter_block = active_parameter_blocks[i]; + const int parameter_block_size = + problem->ParameterBlockLocalSize(parameter_block); + parameter_block_to_row_index_[parameter_block] = num_rows; + num_rows += parameter_block_size; + } + + // Compute the number of non-zeros in the covariance matrix. Along + // the way flip any covariance blocks which are in the lower + // triangular part of the matrix. + int num_nonzeros = 0; + CovarianceBlocks covariance_blocks; + for (int i = 0; i < original_covariance_blocks.size(); ++i) { + const pair& block_pair = + original_covariance_blocks[i]; + if (constant_parameter_blocks_.count(block_pair.first) > 0 || + constant_parameter_blocks_.count(block_pair.second) > 0) { + continue; + } + + int index1 = FindOrDie(parameter_block_to_row_index_, block_pair.first); + int index2 = FindOrDie(parameter_block_to_row_index_, block_pair.second); + const int size1 = problem->ParameterBlockLocalSize(block_pair.first); + const int size2 = problem->ParameterBlockLocalSize(block_pair.second); + num_nonzeros += size1 * size2; + + // Make sure we are constructing a block upper triangular matrix. + if (index1 > index2) { + covariance_blocks.push_back(make_pair(block_pair.second, + block_pair.first)); + } else { + covariance_blocks.push_back(block_pair); + } + } + + if (covariance_blocks.size() == 0) { + VLOG(2) << "No non-zero covariance blocks found"; + covariance_matrix_.reset(NULL); + return true; + } + + // Sort the block pairs. As a consequence we get the covariance + // blocks as they will occur in the CompressedRowSparseMatrix that + // will store the covariance. + sort(covariance_blocks.begin(), covariance_blocks.end()); + + // Fill the sparsity pattern of the covariance matrix. + covariance_matrix_.reset( + new CompressedRowSparseMatrix(num_rows, num_rows, num_nonzeros)); + + int* rows = covariance_matrix_->mutable_rows(); + int* cols = covariance_matrix_->mutable_cols(); + + // Iterate over parameter blocks and in turn over the rows of the + // covariance matrix. For each parameter block, look in the upper + // triangular part of the covariance matrix to see if there are any + // blocks requested by the user. If this is the case then fill out a + // set of compressed rows corresponding to this parameter block. + // + // The key thing that makes this loop work is the fact that the + // row/columns of the covariance matrix are ordered by the pointer + // values of the parameter blocks. Thus iterating over the keys of + // parameter_block_to_row_index_ corresponds to iterating over the + // rows of the covariance matrix in order. + int i = 0; // index into covariance_blocks. + int cursor = 0; // index into the covariance matrix. + for (map::const_iterator it = + parameter_block_to_row_index_.begin(); + it != parameter_block_to_row_index_.end(); + ++it) { + const double* row_block = it->first; + const int row_block_size = problem->ParameterBlockLocalSize(row_block); + int row_begin = it->second; + + // Iterate over the covariance blocks contained in this row block + // and count the number of columns in this row block. + int num_col_blocks = 0; + int num_columns = 0; + for (int j = i; j < covariance_blocks.size(); ++j, ++num_col_blocks) { + const pair& block_pair = + covariance_blocks[j]; + if (block_pair.first != row_block) { + break; + } + num_columns += problem->ParameterBlockLocalSize(block_pair.second); + } + + // Fill out all the compressed rows for this parameter block. + for (int r = 0; r < row_block_size; ++r) { + rows[row_begin + r] = cursor; + for (int c = 0; c < num_col_blocks; ++c) { + const double* col_block = covariance_blocks[i + c].second; + const int col_block_size = problem->ParameterBlockLocalSize(col_block); + int col_begin = FindOrDie(parameter_block_to_row_index_, col_block); + for (int k = 0; k < col_block_size; ++k) { + cols[cursor++] = col_begin++; + } + } + } + + i+= num_col_blocks; + } + + rows[num_rows] = cursor; + return true; +} + +bool CovarianceImpl::ComputeCovarianceValues() { + if (options_.use_dense_linear_algebra) { + return ComputeCovarianceValuesUsingEigen(); + } + +#ifndef CERES_NO_SUITESPARSE + return ComputeCovarianceValuesUsingSuiteSparse(); +#else + LOG(ERROR) << "Ceres compiled without SuiteSparse. " + << "Large scale covariance computation is not possible."; + return false; +#endif +} + +bool CovarianceImpl::ComputeCovarianceValuesUsingSuiteSparse() { +#ifndef CERES_NO_SUITESPARSE + if (covariance_matrix_.get() == NULL) { + // Nothing to do, all zeros covariance matrix. + return true; + } + + CRSMatrix jacobian; + problem_->Evaluate(evaluate_options_, NULL, NULL, NULL, &jacobian); + + // m is a transposed view of the Jacobian. + cholmod_sparse cholmod_jacobian_view; + cholmod_jacobian_view.nrow = jacobian.num_cols; + cholmod_jacobian_view.ncol = jacobian.num_rows; + cholmod_jacobian_view.nzmax = jacobian.values.size(); + cholmod_jacobian_view.nz = NULL; + cholmod_jacobian_view.p = reinterpret_cast(&jacobian.rows[0]); + cholmod_jacobian_view.i = reinterpret_cast(&jacobian.cols[0]); + cholmod_jacobian_view.x = reinterpret_cast(&jacobian.values[0]); + cholmod_jacobian_view.z = NULL; + cholmod_jacobian_view.stype = 0; // Matrix is not symmetric. + cholmod_jacobian_view.itype = CHOLMOD_INT; + cholmod_jacobian_view.xtype = CHOLMOD_REAL; + cholmod_jacobian_view.dtype = CHOLMOD_DOUBLE; + cholmod_jacobian_view.sorted = 1; + cholmod_jacobian_view.packed = 1; + + cholmod_factor* factor = ss_.AnalyzeCholesky(&cholmod_jacobian_view); + if (!ss_.Cholesky(&cholmod_jacobian_view, factor)) { + ss_.Free(factor); + LOG(WARNING) << "Cholesky factorization failed."; + return false; + } + + const int num_rows = covariance_matrix_->num_rows(); + const int* rows = covariance_matrix_->rows(); + const int* cols = covariance_matrix_->cols(); + double* values = covariance_matrix_->mutable_values(); + + cholmod_dense* rhs = ss_.CreateDenseVector(NULL, num_rows, num_rows); + double* rhs_x = reinterpret_cast(rhs->x); + + // The following loop exploits the fact that the i^th column of A^{-1} + // is given by the solution to the linear system + // + // A x = e_i + // + // where e_i is a vector with e(i) = 1 and all other entries zero. + // + // Since the covariance matrix is symmetric, the i^th row and column + // are equal. + // + // The ifdef separates two different version of SuiteSparse. Newer + // versions of SuiteSparse have the cholmod_solve2 function which + // re-uses memory across calls. +#if (SUITESPARSE_VERSION < 4002) + for (int r = 0; r < num_rows; ++r) { + int row_begin = rows[r]; + int row_end = rows[r + 1]; + if (row_end == row_begin) { + continue; + } + + rhs_x[r] = 1.0; + cholmod_dense* solution = ss_.Solve(factor, rhs); + double* solution_x = reinterpret_cast(solution->x); + for (int idx = row_begin; idx < row_end; ++idx) { + const int c = cols[idx]; + values[idx] = solution_x[c]; + } + ss_.Free(solution); + rhs_x[r] = 0.0; + } +#else + // TODO(sameeragarwal) There should be a more efficient way + // involving the use of Bset but I am unable to make it work right + // now. + cholmod_dense* solution = NULL; + cholmod_sparse* solution_set = NULL; + cholmod_dense* y_workspace = NULL; + cholmod_dense* e_workspace = NULL; + + for (int r = 0; r < num_rows; ++r) { + int row_begin = rows[r]; + int row_end = rows[r + 1]; + if (row_end == row_begin) { + continue; + } + + rhs_x[r] = 1.0; + + cholmod_solve2(CHOLMOD_A, + factor, + rhs, + NULL, + &solution, + &solution_set, + &y_workspace, + &e_workspace, + ss_.mutable_cc()); + + double* solution_x = reinterpret_cast(solution->x); + for (int idx = row_begin; idx < row_end; ++idx) { + const int c = cols[idx]; + values[idx] = solution_x[c]; + } + rhs_x[r] = 0.0; + } + + ss_.Free(solution); + ss_.Free(solution_set); + ss_.Free(y_workspace); + ss_.Free(e_workspace); + +#endif + + ss_.Free(rhs); + ss_.Free(factor); + return true; +#else + return false; +#endif +}; + +bool CovarianceImpl::ComputeCovarianceValuesUsingEigen() { + if (covariance_matrix_.get() == NULL) { + // Nothing to do, all zeros covariance matrix. + return true; + } + + CRSMatrix jacobian; + problem_->Evaluate(evaluate_options_, NULL, NULL, NULL, &jacobian); + Matrix dense_jacobian(jacobian.num_rows, jacobian.num_cols); + dense_jacobian.setZero(); + for (int r = 0; r < jacobian.num_rows; ++r) { + for (int idx = jacobian.rows[r]; idx < jacobian.rows[r + 1]; ++idx) { + const int c = jacobian.cols[idx]; + dense_jacobian(r, c) = jacobian.values[idx]; + } + } + + Eigen::JacobiSVD svd(dense_jacobian, + Eigen::ComputeThinU | Eigen::ComputeThinV); + Vector inverse_singular_values = svd.singularValues(); + + for (int i = 0; i < inverse_singular_values.rows(); ++i) { + if (inverse_singular_values[i] > options_.min_singular_value_threshold && + i < (inverse_singular_values.rows() - options_.null_space_rank)) { + inverse_singular_values[i] = + 1.0 / (inverse_singular_values[i] * inverse_singular_values[i]); + } else { + inverse_singular_values[i] = 0.0; + } + } + + Matrix dense_covariance = + svd.matrixV() * + inverse_singular_values.asDiagonal() * + svd.matrixV().transpose(); + + const int num_rows = covariance_matrix_->num_rows(); + const int* rows = covariance_matrix_->rows(); + const int* cols = covariance_matrix_->cols(); + double* values = covariance_matrix_->mutable_values(); + + for (int r = 0; r < num_rows; ++r) { + for (int idx = rows[r]; idx < rows[r + 1]; ++idx) { + const int c = cols[idx]; + values[idx] = dense_covariance(r, c); + } + } + return true; +}; + + + +} // namespace internal +} // namespace ceres diff --git a/internal/ceres/covariance_impl.h b/internal/ceres/covariance_impl.h new file mode 100644 index 000000000..596c73fad --- /dev/null +++ b/internal/ceres/covariance_impl.h @@ -0,0 +1,87 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2013 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_COVARIANCE_IMPL_H_ +#define CERES_INTERNAL_COVARIANCE_IMPL_H_ + +#include +#include +#include +#include +#include "ceres/covariance.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/problem_impl.h" +#include "ceres/suitesparse.h" + +namespace ceres { + +namespace internal { + +class CompressedRowSparseMatrix; + +class CovarianceImpl { + public: + explicit CovarianceImpl(const Covariance::Options& options); + ~CovarianceImpl(); + + bool Compute( + const vector >& covariance_blocks, + ProblemImpl* problem); + + bool GetCovarianceBlock(const double* parameter_block1, + const double* parameter_block2, + double* covariance_block) const; + + bool ComputeCovarianceSparsity( + const vector >& covariance_blocks, + ProblemImpl* problem); + + bool ComputeCovarianceValues(); + bool ComputeCovarianceValuesUsingSuiteSparse(); + bool ComputeCovarianceValuesUsingEigen(); + + const CompressedRowSparseMatrix* covariance_matrix() const { + return covariance_matrix_.get(); + } + + private: + ProblemImpl* problem_; + Covariance::Options options_; + Problem::EvaluateOptions evaluate_options_; + map parameter_block_to_row_index_; + set constant_parameter_blocks_; + scoped_ptr covariance_matrix_; + SuiteSparse ss_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_COVARIANCE_IMPL_H_ diff --git a/internal/ceres/covariance_test.cc b/internal/ceres/covariance_test.cc new file mode 100644 index 000000000..b74efdaec --- /dev/null +++ b/internal/ceres/covariance_test.cc @@ -0,0 +1,647 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2013 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/covariance.h" + +#include +#include +#include "ceres/compressed_row_sparse_matrix.h" +#include "ceres/cost_function.h" +#include "ceres/covariance_impl.h" +#include "ceres/local_parameterization.h" +#include "ceres/map_util.h" +#include "ceres/problem_impl.h" +#include "gtest/gtest.h" + +namespace ceres { +namespace internal { + +TEST(CovarianceImpl, ComputeCovarianceSparsity) { + double parameters[10]; + + double* block1 = parameters; + double* block2 = block1 + 1; + double* block3 = block2 + 2; + double* block4 = block3 + 3; + + ProblemImpl problem; + + // Add in random order + problem.AddParameterBlock(block1, 1); + problem.AddParameterBlock(block4, 4); + problem.AddParameterBlock(block3, 3); + problem.AddParameterBlock(block2, 2); + + // Sparsity pattern + // + // x 0 0 0 0 0 x x x x + // 0 x x x x x 0 0 0 0 + // 0 x x x x x 0 0 0 0 + // 0 0 0 x x x 0 0 0 0 + // 0 0 0 x x x 0 0 0 0 + // 0 0 0 x x x 0 0 0 0 + // 0 0 0 0 0 0 x x x x + // 0 0 0 0 0 0 x x x x + // 0 0 0 0 0 0 x x x x + // 0 0 0 0 0 0 x x x x + + int expected_rows[] = {0, 5, 10, 15, 18, 21, 24, 28, 32, 36, 40}; + int expected_cols[] = {0, 6, 7, 8, 9, + 1, 2, 3, 4, 5, + 1, 2, 3, 4, 5, + 3, 4, 5, + 3, 4, 5, + 3, 4, 5, + 6, 7, 8, 9, + 6, 7, 8, 9, + 6, 7, 8, 9, + 6, 7, 8, 9}; + + + vector > covariance_blocks; + covariance_blocks.push_back(make_pair(block1, block1)); + covariance_blocks.push_back(make_pair(block4, block4)); + covariance_blocks.push_back(make_pair(block2, block2)); + covariance_blocks.push_back(make_pair(block3, block3)); + covariance_blocks.push_back(make_pair(block2, block3)); + covariance_blocks.push_back(make_pair(block4, block1)); // reversed + + Covariance::Options options; + CovarianceImpl covariance_impl(options); + EXPECT_TRUE(covariance_impl + .ComputeCovarianceSparsity(covariance_blocks, &problem)); + + const CompressedRowSparseMatrix* crsm = covariance_impl.covariance_matrix(); + + EXPECT_EQ(crsm->num_rows(), 10); + EXPECT_EQ(crsm->num_cols(), 10); + EXPECT_EQ(crsm->num_nonzeros(), 40); + + const int* rows = crsm->rows(); + for (int r = 0; r < crsm->num_rows() + 1; ++r) { + EXPECT_EQ(rows[r], expected_rows[r]) + << r << " " + << rows[r] << " " + << expected_rows[r]; + } + + const int* cols = crsm->cols(); + for (int c = 0; c < crsm->num_nonzeros(); ++c) { + EXPECT_EQ(cols[c], expected_cols[c]) + << c << " " + << cols[c] << " " + << expected_cols[c]; + } +} + + +class UnaryCostFunction: public CostFunction { + public: + UnaryCostFunction(const int num_residuals, + const int16 parameter_block_size, + const double* jacobian) + : jacobian_(jacobian, jacobian + num_residuals * parameter_block_size) { + set_num_residuals(num_residuals); + mutable_parameter_block_sizes()->push_back(parameter_block_size); + } + + virtual bool Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const { + for (int i = 0; i < num_residuals(); ++i) { + residuals[i] = 1; + } + + if (jacobians == NULL) { + return true; + } + + if (jacobians[0] != NULL) { + copy(jacobian_.begin(), jacobian_.end(), jacobians[0]); + } + + return true; + } + + private: + vector jacobian_; +}; + + +class BinaryCostFunction: public CostFunction { + public: + BinaryCostFunction(const int num_residuals, + const int16 parameter_block1_size, + const int16 parameter_block2_size, + const double* jacobian1, + const double* jacobian2) + : jacobian1_(jacobian1, + jacobian1 + num_residuals * parameter_block1_size), + jacobian2_(jacobian2, + jacobian2 + num_residuals * parameter_block2_size) { + set_num_residuals(num_residuals); + mutable_parameter_block_sizes()->push_back(parameter_block1_size); + mutable_parameter_block_sizes()->push_back(parameter_block2_size); + } + + virtual bool Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const { + for (int i = 0; i < num_residuals(); ++i) { + residuals[i] = 2; + } + + if (jacobians == NULL) { + return true; + } + + if (jacobians[0] != NULL) { + copy(jacobian1_.begin(), jacobian1_.end(), jacobians[0]); + } + + if (jacobians[1] != NULL) { + copy(jacobian2_.begin(), jacobian2_.end(), jacobians[1]); + } + + return true; + } + + private: + vector jacobian1_; + vector jacobian2_; +}; + +// x_plus_delta = delta * x; +class PolynomialParameterization : public LocalParameterization { + public: + virtual ~PolynomialParameterization() {} + + virtual bool Plus(const double* x, + const double* delta, + double* x_plus_delta) const { + x_plus_delta[0] = delta[0] * x[0]; + x_plus_delta[1] = delta[0] * x[1]; + return true; + } + + virtual bool ComputeJacobian(const double* x, double* jacobian) const { + jacobian[0] = x[0]; + jacobian[1] = x[1]; + return true; + } + + virtual int GlobalSize() const { return 2; } + virtual int LocalSize() const { return 1; } +}; + +class CovarianceTest : public ::testing::Test { + protected: + virtual void SetUp() { + double* x = parameters_; + double* y = x + 2; + double* z = y + 3; + + x[0] = 1; + x[1] = 1; + y[0] = 2; + y[1] = 2; + y[2] = 2; + z[0] = 3; + + { + double jacobian[] = { 1.0, 0.0, 0.0, 1.0}; + problem_.AddResidualBlock(new UnaryCostFunction(2, 2, jacobian), NULL, x); + } + + { + double jacobian[] = { 2.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 2.0 }; + problem_.AddResidualBlock(new UnaryCostFunction(3, 3, jacobian), NULL, y); + } + + { + double jacobian = 5.0; + problem_.AddResidualBlock(new UnaryCostFunction(1, 1, &jacobian), NULL, z); + } + + { + double jacobian1[] = { 1.0, 2.0, 3.0 }; + double jacobian2[] = { -5.0, -6.0 }; + problem_.AddResidualBlock( + new BinaryCostFunction(1, 3, 2, jacobian1, jacobian2), + NULL, + y, + x); + } + + { + double jacobian1[] = {2.0 }; + double jacobian2[] = { 3.0, -2.0 }; + problem_.AddResidualBlock( + new BinaryCostFunction(1, 1, 2, jacobian1, jacobian2), + NULL, + z, + x); + } + + all_covariance_blocks_.push_back(make_pair(x, x)); + all_covariance_blocks_.push_back(make_pair(y, y)); + all_covariance_blocks_.push_back(make_pair(z, z)); + all_covariance_blocks_.push_back(make_pair(x, y)); + all_covariance_blocks_.push_back(make_pair(x, z)); + all_covariance_blocks_.push_back(make_pair(y, z)); + + column_bounds_[x] = make_pair(0, 2); + column_bounds_[y] = make_pair(2, 5); + column_bounds_[z] = make_pair(5, 6); + } + + void ComputeAndCompareCovarianceBlocks(const Covariance::Options& options, + const double* expected_covariance) { + // Generate all possible combination of block pairs and check if the + // covariance computation is correct. + for (int i = 1; i <= 64; ++i) { + vector > covariance_blocks; + if (i & 1) { + covariance_blocks.push_back(all_covariance_blocks_[0]); + } + + if (i & 2) { + covariance_blocks.push_back(all_covariance_blocks_[1]); + } + + if (i & 4) { + covariance_blocks.push_back(all_covariance_blocks_[2]); + } + + if (i & 8) { + covariance_blocks.push_back(all_covariance_blocks_[3]); + } + + if (i & 16) { + covariance_blocks.push_back(all_covariance_blocks_[4]); + } + + if (i & 32) { + covariance_blocks.push_back(all_covariance_blocks_[5]); + } + + Covariance covariance(options); + EXPECT_TRUE(covariance.Compute(covariance_blocks, &problem_)); + + for (int i = 0; i < covariance_blocks.size(); ++i) { + const double* block1 = covariance_blocks[i].first; + const double* block2 = covariance_blocks[i].second; + // block1, block2 + GetCovarianceBlockAndCompare(block1, block2, covariance, expected_covariance); + // block2, block1 + GetCovarianceBlockAndCompare(block2, block1, covariance, expected_covariance); + } + } + } + + void GetCovarianceBlockAndCompare(const double* block1, + const double* block2, + const Covariance& covariance, + const double* expected_covariance) { + const int row_begin = FindOrDie(column_bounds_, block1).first; + const int row_end = FindOrDie(column_bounds_, block1).second; + const int col_begin = FindOrDie(column_bounds_, block2).first; + const int col_end = FindOrDie(column_bounds_, block2).second; + + Matrix actual(row_end - row_begin, col_end - col_begin); + EXPECT_TRUE(covariance.GetCovarianceBlock(block1, + block2, + actual.data())); + + ConstMatrixRef expected(expected_covariance, 6, 6); + double diff_norm = (expected.block(row_begin, + col_begin, + row_end - row_begin, + col_end - col_begin) - actual).norm(); + diff_norm /= (row_end - row_begin) * (col_end - col_begin); + + const double kTolerance = 1e-5; + EXPECT_NEAR(diff_norm, 0.0, kTolerance) + << "rows: " << row_begin << " " << row_end << " " + << "cols: " << col_begin << " " << col_end << " " + << "\n\n expected: \n " << expected.block(row_begin, + col_begin, + row_end - row_begin, + col_end - col_begin) + << "\n\n actual: \n " << actual + << "\n\n full expected: \n" << expected; + } + + double parameters_[10]; + Problem problem_; + vector > all_covariance_blocks_; + map > column_bounds_; +}; + + +TEST_F(CovarianceTest, NormalBehavior) { + // J + // + // 1 0 0 0 0 0 + // 0 1 0 0 0 0 + // 0 0 2 0 0 0 + // 0 0 0 2 0 0 + // 0 0 0 0 2 0 + // 0 0 0 0 0 5 + // -5 -6 1 2 3 0 + // 3 -2 0 0 0 2 + + // J'J + // + // 35 24 -5 -10 -15 6 + // 24 41 -6 -12 -18 -4 + // -5 -6 5 2 3 0 + // -10 -12 2 8 6 0 + // -15 -18 3 6 13 0 + // 6 -4 0 0 0 29 + + // inv(J'J) computed using octave. + double expected_covariance[] = { + 7.0747e-02, -8.4923e-03, 1.6821e-02, 3.3643e-02, 5.0464e-02, -1.5809e-02, // NOLINT + -8.4923e-03, 8.1352e-02, 2.4758e-02, 4.9517e-02, 7.4275e-02, 1.2978e-02, // NOLINT + 1.6821e-02, 2.4758e-02, 2.4904e-01, -1.9271e-03, -2.8906e-03, -6.5325e-05, // NOLINT + 3.3643e-02, 4.9517e-02, -1.9271e-03, 2.4615e-01, -5.7813e-03, -1.3065e-04, // NOLINT + 5.0464e-02, 7.4275e-02, -2.8906e-03, -5.7813e-03, 2.4133e-01, -1.9598e-04, // NOLINT + -1.5809e-02, 1.2978e-02, -6.5325e-05, -1.3065e-04, -1.9598e-04, 3.9544e-02, // NOLINT + }; + + Covariance::Options options; + options.use_dense_linear_algebra = false; + ComputeAndCompareCovarianceBlocks(options, expected_covariance); + + options.use_dense_linear_algebra = true; + ComputeAndCompareCovarianceBlocks(options, expected_covariance); +} + + +TEST_F(CovarianceTest, ConstantParameterBlock) { + problem_.SetParameterBlockConstant(parameters_); + + // J + // + // 0 0 0 0 0 0 + // 0 0 0 0 0 0 + // 0 0 2 0 0 0 + // 0 0 0 2 0 0 + // 0 0 0 0 2 0 + // 0 0 0 0 0 5 + // 0 0 1 2 3 0 + // 0 0 0 0 0 2 + + // J'J + // + // 0 0 0 0 0 0 + // 0 0 0 0 0 0 + // 0 0 5 2 3 0 + // 0 0 2 8 6 0 + // 0 0 3 6 13 0 + // 0 0 0 0 0 29 + + // pinv(J'J) computed using octave. + double expected_covariance[] = { + 0, 0, 0, 0, 0, 0, // NOLINT + 0, 0, 0, 0, 0, 0, // NOLINT + 0, 0, 0.23611, -0.02778, -0.04167, -0.00000, // NOLINT + 0, 0, -0.02778, 0.19444, -0.08333, -0.00000, // NOLINT + 0, 0, -0.04167, -0.08333, 0.12500, -0.00000, // NOLINT + 0, 0, -0.00000, -0.00000, -0.00000, 0.03448 // NOLINT + }; + + Covariance::Options options; + options.use_dense_linear_algebra = false; + ComputeAndCompareCovarianceBlocks(options, expected_covariance); + + options.use_dense_linear_algebra = true; + ComputeAndCompareCovarianceBlocks(options, expected_covariance); +} + +TEST_F(CovarianceTest, LocalParameterization) { + double* x = parameters_; + double* y = x + 2; + + problem_.SetParameterization(x, new PolynomialParameterization); + + vector subset; + subset.push_back(2); + problem_.SetParameterization(y, new SubsetParameterization(3, subset)); + + // Raw Jacobian: J + // + // 1 0 0 0 0 0 + // 0 1 0 0 0 0 + // 0 0 2 0 0 0 + // 0 0 0 2 0 0 + // 0 0 0 0 0 0 + // 0 0 0 0 0 5 + // -5 -6 1 2 0 0 + // 3 -2 0 0 0 2 + + // Global to local jacobian: A + // + // + // 1 0 0 0 0 + // 1 0 0 0 0 + // 0 1 0 0 0 + // 0 0 1 0 0 + // 0 0 0 1 0 + // 0 0 0 0 1 + + // A * pinv((J*A)'*(J*A)) * A' + // Computed using octave. + double expected_covariance[] = { + 0.01766, 0.01766, 0.02158, 0.04316, 0.00000, -0.00122, + 0.01766, 0.01766, 0.02158, 0.04316, 0.00000, -0.00122, + 0.02158, 0.02158, 0.24860, -0.00281, 0.00000, -0.00149, + 0.04316, 0.04316, -0.00281, 0.24439, 0.00000, -0.00298, + 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, + -0.00122, -0.00122, -0.00149, -0.00298, 0.00000, 0.03457 + }; + + + Covariance::Options options; + options.use_dense_linear_algebra = false; + ComputeAndCompareCovarianceBlocks(options, expected_covariance); + + options.use_dense_linear_algebra = true; + ComputeAndCompareCovarianceBlocks(options, expected_covariance); +} + + +TEST_F(CovarianceTest, TruncatedRank) { + // J + // + // 1 0 0 0 0 0 + // 0 1 0 0 0 0 + // 0 0 2 0 0 0 + // 0 0 0 2 0 0 + // 0 0 0 0 2 0 + // 0 0 0 0 0 5 + // -5 -6 1 2 3 0 + // 3 -2 0 0 0 2 + + // J'J + // + // 35 24 -5 -10 -15 6 + // 24 41 -6 -12 -18 -4 + // -5 -6 5 2 3 0 + // -10 -12 2 8 6 0 + // -15 -18 3 6 13 0 + // 6 -4 0 0 0 29 + + // 3.4142 is the smallest eigen value of J'J. The following matrix + // was obtained by dropping the eigenvector corresponding to this + // eigenvalue. + double expected_covariance[] = { + 5.4135e-02, -3.5121e-02, 1.7257e-04, 3.4514e-04, 5.1771e-04, -1.6076e-02, + -3.5121e-02, 3.8667e-02, -1.9288e-03, -3.8576e-03, -5.7864e-03, 1.2549e-02, + 1.7257e-04, -1.9288e-03, 2.3235e-01, -3.5297e-02, -5.2946e-02, -3.3329e-04, + 3.4514e-04, -3.8576e-03, -3.5297e-02, 1.7941e-01, -1.0589e-01, -6.6659e-04, + 5.1771e-04, -5.7864e-03, -5.2946e-02, -1.0589e-01, 9.1162e-02, -9.9988e-04, + -1.6076e-02, 1.2549e-02, -3.3329e-04, -6.6659e-04, -9.9988e-04, 3.9539e-02 + }; + + Covariance::Options options; + options.use_dense_linear_algebra = true; + options.null_space_rank = 1; + ComputeAndCompareCovarianceBlocks(options, expected_covariance); + + options.use_dense_linear_algebra = true; + options.null_space_rank = 0; + options.min_singular_value_threshold = sqrt(3.5); + ComputeAndCompareCovarianceBlocks(options, expected_covariance); +} + +class RankDeficientCovarianceTest : public CovarianceTest { + protected: + virtual void SetUp() { + double* x = parameters_; + double* y = x + 2; + double* z = y + 3; + + { + double jacobian[] = { 1.0, 0.0, 0.0, 1.0}; + problem_.AddResidualBlock(new UnaryCostFunction(2, 2, jacobian), NULL, x); + } + + { + double jacobian[] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + problem_.AddResidualBlock(new UnaryCostFunction(3, 3, jacobian), NULL, y); + } + + { + double jacobian = 5.0; + problem_.AddResidualBlock(new UnaryCostFunction(1, 1, &jacobian), NULL, z); + } + + { + double jacobian1[] = { 0.0, 0.0, 0.0 }; + double jacobian2[] = { -5.0, -6.0 }; + problem_.AddResidualBlock( + new BinaryCostFunction(1, 3, 2, jacobian1, jacobian2), + NULL, + y, + x); + } + + { + double jacobian1[] = {2.0 }; + double jacobian2[] = { 3.0, -2.0 }; + problem_.AddResidualBlock( + new BinaryCostFunction(1, 1, 2, jacobian1, jacobian2), + NULL, + z, + x); + } + + all_covariance_blocks_.push_back(make_pair(x, x)); + all_covariance_blocks_.push_back(make_pair(y, y)); + all_covariance_blocks_.push_back(make_pair(z, z)); + all_covariance_blocks_.push_back(make_pair(x, y)); + all_covariance_blocks_.push_back(make_pair(x, z)); + all_covariance_blocks_.push_back(make_pair(y, z)); + + column_bounds_[x] = make_pair(0, 2); + column_bounds_[y] = make_pair(2, 5); + column_bounds_[z] = make_pair(5, 6); + } +}; + +TEST_F(RankDeficientCovarianceTest, MinSingularValueTolerance) { + // J + // + // 1 0 0 0 0 0 + // 0 1 0 0 0 0 + // 0 0 0 0 0 0 + // 0 0 0 0 0 0 + // 0 0 0 0 0 0 + // 0 0 0 0 0 5 + // -5 -6 0 0 0 0 + // 3 -2 0 0 0 2 + + // J'J + // + // 35 24 0 0 0 6 + // 24 41 0 0 0 -4 + // 0 0 0 0 0 0 + // 0 0 0 0 0 0 + // 0 0 0 0 0 0 + // 6 -4 0 0 0 29 + + // pinv(J'J) computed using octave. + double expected_covariance[] = { + 0.053998, -0.033145, 0.000000, 0.000000, 0.000000, -0.015744, + -0.033145, 0.045067, 0.000000, 0.000000, 0.000000, 0.013074, + 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, + 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, + 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, + -0.015744, 0.013074, 0.000000, 0.000000, 0.000000, 0.039543 + }; + + Covariance::Options options; + options.use_dense_linear_algebra = true; + ComputeAndCompareCovarianceBlocks(options, expected_covariance); + + options.null_space_rank = 1; + ComputeAndCompareCovarianceBlocks(options, expected_covariance); + + options.null_space_rank = 2; + ComputeAndCompareCovarianceBlocks(options, expected_covariance); + + options.null_space_rank = 3; + ComputeAndCompareCovarianceBlocks(options, expected_covariance); +} + +} // namespace internal +} // namespace ceres diff --git a/internal/ceres/problem.cc b/internal/ceres/problem.cc index b483932b2..403e96a3a 100644 --- a/internal/ceres/problem.cc +++ b/internal/ceres/problem.cc @@ -206,11 +206,11 @@ int Problem::NumResiduals() const { return problem_impl_->NumResiduals(); } -int Problem::ParameterBlockSize(double* parameter_block) const { +int Problem::ParameterBlockSize(const double* parameter_block) const { return problem_impl_->ParameterBlockSize(parameter_block); }; -int Problem::ParameterBlockLocalSize(double* parameter_block) const { +int Problem::ParameterBlockLocalSize(const double* parameter_block) const { return problem_impl_->ParameterBlockLocalSize(parameter_block); }; diff --git a/internal/ceres/problem_impl.cc b/internal/ceres/problem_impl.cc index 34c378575..89e939faa 100644 --- a/internal/ceres/problem_impl.cc +++ b/internal/ceres/problem_impl.cc @@ -711,13 +711,14 @@ int ProblemImpl::NumResiduals() const { return program_->NumResiduals(); } -int ProblemImpl::ParameterBlockSize(double* parameter_block) const { - return FindParameterBlockOrDie(parameter_block_map_, parameter_block)->Size(); +int ProblemImpl::ParameterBlockSize(const double* parameter_block) const { + return FindParameterBlockOrDie(parameter_block_map_, + const_cast(parameter_block))->Size(); }; -int ProblemImpl::ParameterBlockLocalSize(double* parameter_block) const { +int ProblemImpl::ParameterBlockLocalSize(const double* parameter_block) const { return FindParameterBlockOrDie(parameter_block_map_, - parameter_block)->LocalSize(); + const_cast(parameter_block))->LocalSize(); }; void ProblemImpl::GetParameterBlocks(vector* parameter_blocks) const { diff --git a/internal/ceres/problem_impl.h b/internal/ceres/problem_impl.h index 260938964..ace27f56b 100644 --- a/internal/ceres/problem_impl.h +++ b/internal/ceres/problem_impl.h @@ -139,8 +139,8 @@ class ProblemImpl { int NumResidualBlocks() const; int NumResiduals() const; - int ParameterBlockSize(double* parameter_block) const; - int ParameterBlockLocalSize(double* parameter_block) const; + int ParameterBlockSize(const double* parameter_block) const; + int ParameterBlockLocalSize(const double* parameter_block) const; void GetParameterBlocks(vector* parameter_blocks) const; const Program& program() const { return *program_; }