Sparse covariance estimation.
Add a Covariance object to the API.
Given a Problem object and a set of parameter block pairs the
Covariance object computes a sparse covariance matrix corresponding
to those block pairs and provides random access to them.
Constant parameter blocks and parameter blocks with local parameterizations
are correctly handled.
Sparse and dense implementations are provided. With the dense implementation
rank deficient Jacobians can also be handled.
Parts of the code are threaded using OpenMP if available.
Change-Id: I5b49583b3d79579df3e0f334c22567acb23ed4ad
diff --git a/include/ceres/covariance.h b/include/ceres/covariance.h
new file mode 100644
index 0000000..1e9bb9f
--- /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 <utility>
+#include <vector>
+#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);
+// <Build Problem>
+// <Solve Problem>
+//
+// Covariance::Options options;
+// Covariance covariance(options);
+//
+// vector<pair<const double*, const double*> > 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<pair<const double*, const double*> >& 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 <parameter_block1, parameter_block2> OR the pair
+ // <parameter_block2, parameter_block1> 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<internal::CovarianceImpl> impl_;
+};
+
+} // namespace ceres
+
+#endif // CERES_PUBLIC_COVARIANCE_H_
diff --git a/include/ceres/problem.h b/include/ceres/problem.h
index 33394ce..663616d 100644
--- a/include/ceres/problem.h
+++ b/include/ceres/problem.h
@@ -329,12 +329,12 @@
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 @@
private:
friend class Solver;
+ friend class Covariance;
internal::scoped_ptr<internal::ProblemImpl> problem_impl_;
CERES_DISALLOW_COPY_AND_ASSIGN(Problem);
};
diff --git a/internal/ceres/CMakeLists.txt b/internal/ceres/CMakeLists.txt
index 392d058..df67f68 100644
--- a/internal/ceres/CMakeLists.txt
+++ b/internal/ceres/CMakeLists.txt
@@ -48,6 +48,8 @@
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 @@
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 0000000..6cb52ee
--- /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 <utility>
+#include <vector>
+#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<pair<const double*, const double*> >& 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 0000000..1f73ffd
--- /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 <algorithm>
+#include <utility>
+#include <vector>
+#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<pair<const double*, const double*> > 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<double*>(original_parameter_block1));
+
+ ParameterBlock* block2 =
+ FindOrDie(parameter_map,
+ const_cast<double*>(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<double*>(parameter_block1));
+ ParameterBlock* block2 =
+ FindOrDie(parameter_map, const_cast<double*>(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<double*> all_parameter_blocks;
+ problem->GetParameterBlocks(&all_parameter_blocks);
+ const ProblemImpl::ParameterMap& parameter_map = problem->parameter_map();
+ constant_parameter_blocks_.clear();
+ vector<double*>& 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<const double*, const double*>& 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 double*, int>::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<const double*, const double*>& 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<void*>(&jacobian.rows[0]);
+ cholmod_jacobian_view.i = reinterpret_cast<void*>(&jacobian.cols[0]);
+ cholmod_jacobian_view.x = reinterpret_cast<void*>(&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<double*>(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<double*>(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<double*>(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<Matrix> 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 0000000..596c73f
--- /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 <map>
+#include <set>
+#include <utility>
+#include <vector>
+#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<pair<const double*, const double*> >& covariance_blocks,
+ ProblemImpl* problem);
+
+ bool GetCovarianceBlock(const double* parameter_block1,
+ const double* parameter_block2,
+ double* covariance_block) const;
+
+ bool ComputeCovarianceSparsity(
+ const vector<pair<const double*, const double*> >& 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<const double*, int> parameter_block_to_row_index_;
+ set<const double*> constant_parameter_blocks_;
+ scoped_ptr<CompressedRowSparseMatrix> 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 0000000..b74efda
--- /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 <algorithm>
+#include <cmath>
+#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<pair<const double*, const double*> > 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<double> 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<double> jacobian1_;
+ vector<double> 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<pair<const double*, const double*> > 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<pair<const double*, const double*> > all_covariance_blocks_;
+ map<const double*, pair<int, int> > 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<int> 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 b483932..403e96a 100644
--- a/internal/ceres/problem.cc
+++ b/internal/ceres/problem.cc
@@ -206,11 +206,11 @@
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 34c3785..89e939f 100644
--- a/internal/ceres/problem_impl.cc
+++ b/internal/ceres/problem_impl.cc
@@ -711,13 +711,14 @@
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<double*>(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<double*>(parameter_block))->LocalSize();
};
void ProblemImpl::GetParameterBlocks(vector<double*>* parameter_blocks) const {
diff --git a/internal/ceres/problem_impl.h b/internal/ceres/problem_impl.h
index 2609389..ace27f5 100644
--- a/internal/ceres/problem_impl.h
+++ b/internal/ceres/problem_impl.h
@@ -139,8 +139,8 @@
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<double*>* parameter_blocks) const;
const Program& program() const { return *program_; }
