diff --git a/examples/nist.cc b/examples/nist.cc index f12dbf331..394458614 100644 --- a/examples/nist.cc +++ b/examples/nist.cc @@ -99,6 +99,9 @@ DEFINE_string(linear_solver, "dense_qr", "Options are: sparse_cholesky, dense_qr, dense_normal_cholesky " "and cgnr"); +DEFINE_string(dense_linear_algebra_library, + "eigen", + "Options are: eigen and lapack."); DEFINE_string(preconditioner, "jacobi", "Options are: identity, jacobi"); DEFINE_string(line_search, "wolfe", @@ -466,6 +469,9 @@ void SetMinimizerOptions(ceres::Solver::Options* options) { &options->minimizer_type)); CHECK(ceres::StringToLinearSolverType(CERES_GET_FLAG(FLAGS_linear_solver), &options->linear_solver_type)); + CHECK(StringToDenseLinearAlgebraLibraryType( + CERES_GET_FLAG(FLAGS_dense_linear_algebra_library), + &options->dense_linear_algebra_library_type)); CHECK(ceres::StringToPreconditionerType(CERES_GET_FLAG(FLAGS_preconditioner), &options->preconditioner_type)); CHECK(ceres::StringToTrustRegionStrategyType( diff --git a/internal/ceres/CMakeLists.txt b/internal/ceres/CMakeLists.txt index 5d9e0abf7..957d476ea 100644 --- a/internal/ceres/CMakeLists.txt +++ b/internal/ceres/CMakeLists.txt @@ -74,6 +74,7 @@ set(CERES_INTERNAL_SRC cxsparse.cc dense_cholesky.cc dense_normal_cholesky_solver.cc + dense_qr.cc dense_qr_solver.cc dense_sparse_matrix.cc detect_structure.cc @@ -97,7 +98,6 @@ set(CERES_INTERNAL_SRC iterative_refiner.cc iterative_schur_complement_solver.cc levenberg_marquardt_strategy.cc - lapack.cc line_search.cc line_search_direction.cc line_search_minimizer.cc @@ -455,6 +455,7 @@ if (BUILD_TESTING AND GFLAGS) ceres_test(cubic_interpolation) ceres_test(dense_linear_solver) ceres_test(dense_cholesky) + ceres_test(dense_qr) ceres_test(dense_sparse_matrix) ceres_test(detect_structure) ceres_test(dogleg_strategy) diff --git a/internal/ceres/dense_jacobian_writer.h b/internal/ceres/dense_jacobian_writer.h index 1d7187c58..efecf35d0 100644 --- a/internal/ceres/dense_jacobian_writer.h +++ b/internal/ceres/dense_jacobian_writer.h @@ -59,8 +59,8 @@ class DenseJacobianWriter { } SparseMatrix* CreateJacobian() const { - return new DenseSparseMatrix( - program_->NumResiduals(), program_->NumEffectiveParameters(), true); + return new DenseSparseMatrix(program_->NumResiduals(), + program_->NumEffectiveParameters()); } void Write(int residual_id, @@ -86,10 +86,10 @@ class DenseJacobianWriter { ConstMatrixRef parameter_jacobian( jacobians[j], num_residuals, parameter_block_size); - dense_jacobian->mutable_matrix().block(residual_offset, - parameter_block->delta_offset(), - num_residuals, - parameter_block_size) = + dense_jacobian->mutable_matrix()->block(residual_offset, + parameter_block->delta_offset(), + num_residuals, + parameter_block_size) = parameter_jacobian; } } diff --git a/internal/ceres/dense_linear_solver_benchmark.cc b/internal/ceres/dense_linear_solver_benchmark.cc index fa0d03400..b8f1e0010 100644 --- a/internal/ceres/dense_linear_solver_benchmark.cc +++ b/internal/ceres/dense_linear_solver_benchmark.cc @@ -43,10 +43,8 @@ template Solve(&lhs, rhs.data(), per_solve_options, solution.data()); EXPECT_EQ(summary.termination_type, LINEAR_SOLVER_SUCCESS); - // If solving for the regularized solution, add the diagonal to the - // matrix. This makes subsequent computations simpler. - if (testing::get<2>(param)) { - lhs.AppendDiagonal(problem->D.get()); - }; + Vector normal_rhs = lhs.matrix().transpose() * rhs.head(num_rows); + Matrix normal_lhs = lhs.matrix().transpose() * lhs.matrix(); - Vector tmp = Vector::Zero(num_rows + num_cols); - lhs.RightMultiply(solution.data(), tmp.data()); - Vector actual_normal_rhs = Vector::Zero(num_cols); - lhs.LeftMultiply(tmp.data(), actual_normal_rhs.data()); + if (regularized) { + ConstVectorRef diagonal(problem->D.get(), num_cols); + normal_lhs += diagonal.array().square().matrix().asDiagonal(); + } - Vector expected_normal_rhs = Vector::Zero(num_cols); - lhs.LeftMultiply(rhs.data(), expected_normal_rhs.data()); - const double residual = (expected_normal_rhs - actual_normal_rhs).norm() / - expected_normal_rhs.norm(); + Vector actual_normal_rhs = normal_lhs * solution; - EXPECT_NEAR(residual, 0.0, 10 * std::numeric_limits::epsilon()) - << "\nexpected: " << expected_normal_rhs.transpose() + const double normalized_residual = + (normal_rhs - actual_normal_rhs).norm() / normal_rhs.norm(); + + EXPECT_NEAR( + normalized_residual, 0.0, 10 * std::numeric_limits::epsilon()) + << "\nexpected: " << normal_rhs.transpose() << "\nactual: " << actual_normal_rhs.transpose(); } diff --git a/internal/ceres/dense_normal_cholesky_solver.cc b/internal/ceres/dense_normal_cholesky_solver.cc index 73e5bf82c..b208d589d 100644 --- a/internal/ceres/dense_normal_cholesky_solver.cc +++ b/internal/ceres/dense_normal_cholesky_solver.cc @@ -57,7 +57,6 @@ LinearSolver::Summary DenseNormalCholeskySolver::SolveImpl( const int num_rows = A->num_rows(); const int num_cols = A->num_cols(); - ConstColMajorMatrixRef Aref = A->matrix(); Matrix lhs(num_cols, num_cols); lhs.setZero(); @@ -68,10 +67,10 @@ LinearSolver::Summary DenseNormalCholeskySolver::SolveImpl( // Using rankUpdate instead of GEMM, exposes the fact that its the // same matrix being multiplied with itself and that the product is // symmetric. - lhs.selfadjointView().rankUpdate(Aref.transpose()); + lhs.selfadjointView().rankUpdate(A->matrix().transpose()); // rhs = A'b - Vector rhs = Aref.transpose() * ConstVectorRef(b, num_rows); + Vector rhs = A->matrix().transpose() * ConstVectorRef(b, num_rows); if (per_solve_options.D != NULL) { ConstVectorRef D(per_solve_options.D, num_cols); diff --git a/internal/ceres/dense_qr.cc b/internal/ceres/dense_qr.cc new file mode 100644 index 000000000..9c0b24e91 --- /dev/null +++ b/internal/ceres/dense_qr.cc @@ -0,0 +1,297 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2022 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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/dense_qr.h" + +#include +#include + +#ifndef CERES_NO_LAPACK + +// LAPACK routines for solving a linear least squares problem using QR +// factorization. This is done in three stages: +// +// A * x = b +// Q * R * x = b (dgeqrf) +// R * x = Q' * b (dormqr) +// x = R^{-1} * Q'* b (dtrtrs) + +// clang-format off + +// Compute the QR factorization of a. +// +// a is an m x n column major matrix (Denoted by "A" in the above description) +// lda is the leading dimension of a. lda >= max(1, num_rows) +// tau is an array of size min(m,n). It contains the scalar factors of the +// elementary reflectors. +// work is an array of size max(1,lwork). On exit, if info=0, work[0] contains +// the optimal size of work. +// +// if lwork >= 1 it is the size of work. If lwork = -1, then a workspace query is assumed. +// dgeqrf computes the optimal size of the work array and returns it as work[0]. +// +// info = 0, successful exit. +// info < 0, if info = -i, then the i^th argument had illegal value. +extern "C" void dgeqrf_(const int* m, const int* n, double* a, const int* lda, + double* tau, double* work, const int* lwork, int* info); + +// Apply Q or Q' to b. +// +// b is a m times n column major matrix. +// size = 'L' applies Q or Q' on the left, size = 'R' applies Q or Q' on the right. +// trans = 'N', applies Q, trans = 'T', applies Q'. +// k is the number of elementary reflectors whose product defines the matrix Q. +// If size = 'L', m >= k >= 0 and if side = 'R', n >= k >= 0. +// a is an lda x k column major matrix containing the reflectors as returned by dgeqrf. +// ldb is the leading dimension of b. +// work is an array of size max(1, lwork) +// lwork if positive is the size of work. If lwork = -1, then a +// workspace query is assumed. +// +// info = 0, successful exit. +// info < 0, if info = -i, then the i^th argument had illegal value. +extern "C" void dormqr_(const char* side, const char* trans, const int* m, + const int* n ,const int* k, double* a, const int* lda, + double* tau, double* b, const int* ldb, double* work, + const int* lwork, int* info); + +// Solve a triangular system of the form A * x = b +// +// uplo = 'U', A is upper triangular. uplo = 'L' is lower triangular. +// trans = 'N', 'T', 'C' specifies the form - A, A^T, A^H. +// DIAG = 'N', A is not unit triangular. 'U' is unit triangular. +// n is the order of the matrix A. +// nrhs number of columns of b. +// a is a column major lda x n. +// b is a column major matrix of ldb x nrhs +// +// info = 0 succesful. +// = -i < 0 i^th argument is an illegal value. +// = i > 0, i^th diagonal element of A is zero. +extern "C" void dtrtrs_(const char* uplo, const char* trans, const char* diag, + const int* n, const int* nrhs, double* a, const int* lda, + double* b, const int* ldb, int* info); +// clang-format on + +#endif + +namespace ceres { +namespace internal { + +std::unique_ptr DenseQR::Create(const LinearSolver::Options& options) { + std::unique_ptr dense_qr; + + switch (options.dense_linear_algebra_library_type) { + case EIGEN: + dense_qr = std::make_unique(); + break; + + case LAPACK: +#ifndef CERES_NO_LAPACK + dense_qr = std::make_unique(); + break; +#else + LOG(FATAL) << "Ceres was compiled without support for LAPACK."; +#endif + + default: + LOG(FATAL) << "Unknown dense linear algebra library type : " + << DenseLinearAlgebraLibraryTypeToString( + options.dense_linear_algebra_library_type); + } + return dense_qr; +} + +LinearSolverTerminationType DenseQR::FactorAndSolve(int num_rows, + int num_cols, + double* lhs, + const double* rhs, + double* solution, + std::string* message) { + LinearSolverTerminationType termination_type = + Factorize(num_rows, num_cols, lhs, message); + if (termination_type == LINEAR_SOLVER_SUCCESS) { + termination_type = Solve(rhs, solution, message); + } + return termination_type; +} + +LinearSolverTerminationType EigenDenseQR::Factorize(int num_rows, + int num_cols, + double* lhs, + std::string* message) { + Eigen::Map m(lhs, num_rows, num_cols); + qr_ = std::make_unique(m); + *message = "Success."; + return LINEAR_SOLVER_SUCCESS; +} + +LinearSolverTerminationType EigenDenseQR::Solve(const double* rhs, + double* solution, + std::string* message) { + VectorRef(solution, qr_->cols()) = + qr_->solve(ConstVectorRef(rhs, qr_->rows())); + *message = "Success."; + return LINEAR_SOLVER_SUCCESS; +} + +#ifndef CERES_NO_LAPACK +LinearSolverTerminationType LAPACKDenseQR::Factorize(int num_rows, + int num_cols, + double* lhs, + std::string* message) { + int lwork = -1; + double work_size; + int info = 0; + + // Compute the size of the temporary workspace needed to compute the QR + // factorization in the dgeqrf call below. + dgeqrf_(&num_rows, + &num_cols, + lhs_, + &num_rows, + tau_.data(), + &work_size, + &lwork, + &info); + if (info < 0) { + LOG(FATAL) << "Congratulations, you found a bug in Ceres." + << "Please report it." + << "LAPACK::dgels fatal error." + << "Argument: " << -info << " is invalid."; + } + + lhs_ = lhs; + num_rows_ = num_rows; + num_cols_ = num_cols; + + lwork = static_cast(work_size); + + if (work_.size() < lwork) { + work_.resize(lwork); + } + if (tau_.size() < num_cols) { + tau_.resize(num_cols); + } + + if (q_transpose_rhs_.size() < num_rows) { + q_transpose_rhs_.resize(num_rows); + } + + // Factorize the lhs_ using the workspace that we just constructed above. + dgeqrf_(&num_rows, + &num_cols, + lhs_, + &num_rows, + tau_.data(), + work_.data(), + &lwork, + &info); + + if (info < 0) { + LOG(FATAL) << "Congratulations, you found a bug in Ceres." + << "Please report it. dgeqrf fatal error." + << "Argument: " << -info << " is invalid."; + } + + termination_type_ = LINEAR_SOLVER_SUCCESS; + *message = "Success."; + return termination_type_; +} + +LinearSolverTerminationType LAPACKDenseQR::Solve(const double* rhs, + double* solution, + std::string* message) { + if (termination_type_ != LINEAR_SOLVER_SUCCESS) { + *message = "QR factorization failed and solve called."; + return termination_type_; + } + + std::copy_n(rhs, num_rows_, q_transpose_rhs_.data()); + + const char side = 'L'; + char trans = 'T'; + const int num_c_cols = 1; + const int lwork = work_.size(); + int info = 0; + dormqr_(&side, + &trans, + &num_rows_, + &num_c_cols, + &num_cols_, + lhs_, + &num_rows_, + tau_.data(), + q_transpose_rhs_.data(), + &num_rows_, + work_.data(), + &lwork, + &info); + if (info < 0) { + LOG(FATAL) << "Congratulations, you found a bug in Ceres." + << "Please report it. dormr fatal error." + << "Argument: " << -info << " is invalid."; + } + + const char uplo = 'U'; + trans = 'N'; + const char diag = 'N'; + dtrtrs_(&uplo, + &trans, + &diag, + &num_cols_, + &num_c_cols, + lhs_, + &num_rows_, + q_transpose_rhs_.data(), + &num_rows_, + &info); + + if (info < 0) { + LOG(FATAL) << "Congratulations, you found a bug in Ceres." + << "Please report it. dormr fatal error." + << "Argument: " << -info << " is invalid."; + } else if (info > 0) { + *message = + "QR factorization failure. The factorization is not full rank. R has " + "zeros on the diagonal."; + termination_type_ = LINEAR_SOLVER_FAILURE; + } else { + std::copy_n(q_transpose_rhs_.data(), num_cols_, solution); + termination_type_ = LINEAR_SOLVER_SUCCESS; + } + + return termination_type_; +} + +#endif // CERES_NO_LAPACK + +} // namespace internal +} // namespace ceres diff --git a/internal/ceres/dense_qr.h b/internal/ceres/dense_qr.h new file mode 100644 index 000000000..93d890ba0 --- /dev/null +++ b/internal/ceres/dense_qr.h @@ -0,0 +1,143 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2022 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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_DENSE_QR_H_ +#define CERES_INTERNAL_DENSE_QR_H_ + +// This include must come before any #ifndef check on Ceres compile options. +// clang-format off +#include "ceres/internal/port.h" +// clang-format on + +#include +#include + +#include "Eigen/Dense" +#include "ceres/internal/eigen.h" +#include "ceres/linear_solver.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +// An interface that abstracts away the internal details of various dense linear +// algebra libraries and offers a simple API for solving dense linear systems +// using a QR factorization. +class CERES_EXPORT_INTERNAL DenseQR { + public: + static std::unique_ptr Create(const LinearSolver::Options& options); + + virtual ~DenseQR() = default; + + // Computes the QR factorization of the given matrix. + // + // The input matrix lhs is assumed to be a column-major num_rows x num_cols + // matrix. + // + // The input matrix lhs may be modified by the implementation to store the + // factorization, irrespective of whether the factorization succeeds or not. + // As a result it is the user's responsibility to ensure that lhs is valid + // when Solve is called. + virtual LinearSolverTerminationType Factorize(int num_rows, + int num_cols, + double* lhs, + std::string* message) = 0; + + // Computes the solution to the equation + // + // lhs * solution = rhs + // + // Calling Solve without calling Factorize is undefined behaviour. It is the + // user's responsibility to ensure that the input matrix lhs passed to + // Factorize has not been freed/modified when Solve is called. + virtual LinearSolverTerminationType Solve(const double* rhs, + double* solution, + std::string* message) = 0; + + // Convenience method which combines a call to Factorize and Solve. Solve is + // only called if Factorize returns LINEAR_SOLVER_SUCCESS. + // + // The input matrix lhs may be modified by the implementation to store the + // factorization, irrespective of whether the method succeeds or not. It is + // the user's responsibility to ensure that lhs is valid if and when Solve is + // called again after this call. + LinearSolverTerminationType FactorAndSolve(int num_rows, + int num_cols, + double* lhs, + const double* rhs, + double* solution, + std::string* message); +}; + +class CERES_EXPORT_INTERNAL EigenDenseQR : public DenseQR { + public: + ~EigenDenseQR() override = default; + + LinearSolverTerminationType Factorize(int num_rows, + int num_cols, + double* lhs, + std::string* message) override; + LinearSolverTerminationType Solve(const double* rhs, + double* solution, + std::string* message) override; + + private: + using QRType = Eigen::HouseholderQR>; + std::unique_ptr qr_; +}; + +#ifndef CERES_NO_LAPACK +class CERES_EXPORT_INTERNAL LAPACKDenseQR : public DenseQR { + public: + ~LAPACKDenseQR() override = default; + + LinearSolverTerminationType Factorize(int num_rows, + int num_cols, + double* lhs, + std::string* message) override; + LinearSolverTerminationType Solve(const double* rhs, + double* solution, + std::string* message) override; + + private: + double* lhs_ = nullptr; + int num_rows_; + int num_cols_; + LinearSolverTerminationType termination_type_ = LINEAR_SOLVER_FATAL_ERROR; + Vector work_; + Vector tau_; + Vector q_transpose_rhs_; +}; +#endif // CERES_NO_LAPACK + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_DENSE_QR_H_ diff --git a/internal/ceres/dense_qr_solver.cc b/internal/ceres/dense_qr_solver.cc index 44388f30a..24cb25abd 100644 --- a/internal/ceres/dense_qr_solver.cc +++ b/internal/ceres/dense_qr_solver.cc @@ -33,9 +33,9 @@ #include #include "Eigen/Dense" +#include "ceres/dense_qr.h" #include "ceres/dense_sparse_matrix.h" #include "ceres/internal/eigen.h" -#include "ceres/lapack.h" #include "ceres/linear_solver.h" #include "ceres/types.h" #include "ceres/wall_time.h" @@ -44,124 +44,40 @@ namespace ceres { namespace internal { DenseQRSolver::DenseQRSolver(const LinearSolver::Options& options) - : options_(options) { - work_.resize(1); -} + : options_(options), dense_qr_(DenseQR::Create(options)) {} LinearSolver::Summary DenseQRSolver::SolveImpl( DenseSparseMatrix* A, const double* b, const LinearSolver::PerSolveOptions& per_solve_options, double* x) { - if (options_.dense_linear_algebra_library_type == EIGEN) { - return SolveUsingEigen(A, b, per_solve_options, x); - } else { - return SolveUsingLAPACK(A, b, per_solve_options, x); - } -} - -LinearSolver::Summary DenseQRSolver::SolveUsingLAPACK( - DenseSparseMatrix* A, - const double* b, - const LinearSolver::PerSolveOptions& per_solve_options, - double* x) { EventLogger event_logger("DenseQRSolver::Solve"); const int num_rows = A->num_rows(); const int num_cols = A->num_cols(); + const int num_augmented_rows = + num_rows + ((per_solve_options.D != nullptr) ? num_cols : 0); - if (per_solve_options.D != NULL) { - // Temporarily append a diagonal block to the A matrix, but undo - // it before returning the matrix to the user. - A->AppendDiagonal(per_solve_options.D); + if (lhs_.rows() != num_augmented_rows || lhs_.cols() != num_cols) { + lhs_.resize(num_augmented_rows, num_cols); + rhs_.resize(num_augmented_rows); } - // TODO(sameeragarwal): Since we are copying anyways, the diagonal - // can be appended to the matrix instead of doing it on A. - lhs_ = A->matrix(); - - if (per_solve_options.D != NULL) { - // Undo the modifications to the matrix A. - A->RemoveDiagonal(); - } - - // rhs = [b;0] to account for the additional rows in the lhs. - if (rhs_.rows() != lhs_.rows()) { - rhs_.resize(lhs_.rows()); - } - rhs_.setZero(); + lhs_.topRows(num_rows) = A->matrix(); rhs_.head(num_rows) = ConstVectorRef(b, num_rows); - if (work_.rows() == 1) { - const int work_size = - LAPACK::EstimateWorkSizeForQR(lhs_.rows(), lhs_.cols()); - VLOG(3) << "Working memory for Dense QR factorization: " - << work_size * sizeof(double); - work_.resize(work_size); + if (num_rows != num_augmented_rows) { + lhs_.bottomRows(num_cols) = + ConstVectorRef(per_solve_options.D, num_cols).asDiagonal(); + rhs_.tail(num_cols).setZero(); } LinearSolver::Summary summary; + summary.termination_type = dense_qr_->FactorAndSolve( + lhs_.rows(), lhs_.cols(), lhs_.data(), rhs_.data(), x, &summary.message); summary.num_iterations = 1; - summary.termination_type = LAPACK::SolveInPlaceUsingQR(lhs_.rows(), - lhs_.cols(), - lhs_.data(), - work_.rows(), - work_.data(), - rhs_.data(), - &summary.message); - event_logger.AddEvent("Solve"); - if (summary.termination_type == LINEAR_SOLVER_SUCCESS) { - VectorRef(x, num_cols) = rhs_.head(num_cols); - } - - event_logger.AddEvent("TearDown"); - return summary; -} - -LinearSolver::Summary DenseQRSolver::SolveUsingEigen( - DenseSparseMatrix* A, - const double* b, - const LinearSolver::PerSolveOptions& per_solve_options, - double* x) { - EventLogger event_logger("DenseQRSolver::Solve"); - - const int num_rows = A->num_rows(); - const int num_cols = A->num_cols(); - - if (per_solve_options.D != NULL) { - // Temporarily append a diagonal block to the A matrix, but undo - // it before returning the matrix to the user. - A->AppendDiagonal(per_solve_options.D); - } - - // rhs = [b;0] to account for the additional rows in the lhs. - const int augmented_num_rows = - num_rows + ((per_solve_options.D != NULL) ? num_cols : 0); - if (rhs_.rows() != augmented_num_rows) { - rhs_.resize(augmented_num_rows); - rhs_.setZero(); - } - rhs_.head(num_rows) = ConstVectorRef(b, num_rows); - event_logger.AddEvent("Setup"); - - // Solve the system. - VectorRef(x, num_cols) = A->matrix().householderQr().solve(rhs_); event_logger.AddEvent("Solve"); - if (per_solve_options.D != NULL) { - // Undo the modifications to the matrix A. - A->RemoveDiagonal(); - } - - // We always succeed, since the QR solver returns the best solution - // it can. It is the job of the caller to determine if the solution - // is good enough or not. - LinearSolver::Summary summary; - summary.num_iterations = 1; - summary.termination_type = LINEAR_SOLVER_SUCCESS; - summary.message = "Success."; - - event_logger.AddEvent("TearDown"); return summary; } diff --git a/internal/ceres/dense_qr_solver.h b/internal/ceres/dense_qr_solver.h index 980243bd6..927dc32b1 100644 --- a/internal/ceres/dense_qr_solver.h +++ b/internal/ceres/dense_qr_solver.h @@ -32,6 +32,9 @@ #ifndef CERES_INTERNAL_DENSE_QR_SOLVER_H_ #define CERES_INTERNAL_DENSE_QR_SOLVER_H_ +#include + +#include "ceres/dense_qr.h" #include "ceres/internal/eigen.h" #include "ceres/internal/port.h" #include "ceres/linear_solver.h" @@ -105,7 +108,7 @@ class CERES_EXPORT_INTERNAL DenseQRSolver : public DenseSparseMatrixSolver { const LinearSolver::Options options_; ColMajorMatrix lhs_; Vector rhs_; - Vector work_; + std::unique_ptr dense_qr_; }; } // namespace internal diff --git a/internal/ceres/dense_qr_test.cc b/internal/ceres/dense_qr_test.cc new file mode 100644 index 000000000..c71f40efe --- /dev/null +++ b/internal/ceres/dense_qr_test.cc @@ -0,0 +1,114 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2022 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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/dense_qr.h" + +#include +#include +#include +#include + +#include "Eigen/Dense" +#include "ceres/internal/eigen.h" +#include "ceres/linear_solver.h" +#include "glog/logging.h" +#include "gmock/gmock.h" +#include "gtest/gtest.h" + +namespace ceres { +namespace internal { + +typedef DenseLinearAlgebraLibraryType Param; + +std::string ParamInfoToString(testing::TestParamInfo info) { + return DenseLinearAlgebraLibraryTypeToString(info.param); +} + +class DenseQRTest : public ::testing::TestWithParam {}; + +TEST_P(DenseQRTest, FactorAndSolve) { + // TODO(sameeragarwal): Convert these tests into type parameterized tests so + // that we can test the single and double precision solvers. + + using Scalar = double; + using MatrixType = Eigen::Matrix; + using VectorType = Eigen::Matrix; + + LinearSolver::Options options; + options.dense_linear_algebra_library_type = GetParam(); + std::unique_ptr dense_qr = DenseQR::Create(options); + + const int kNumTrials = 10; + const int kMinNumCols = 1; + const int kMaxNumCols = 10; + const int kMinRowsFactor = 1; + const int kMaxRowsFactor = 3; + for (int num_cols = kMinNumCols; num_cols < kMaxNumCols; ++num_cols) { + for (int num_rows = kMinRowsFactor * num_cols; + num_rows < kMaxRowsFactor * num_cols; + ++num_rows) { + for (int trial = 0; trial < kNumTrials; ++trial) { + MatrixType lhs = MatrixType::Random(num_rows, num_cols); + Vector x = VectorType::Random(num_cols); + Vector rhs = lhs * x; + Vector actual = Vector::Random(num_cols); + + LinearSolver::Summary summary; + summary.termination_type = dense_qr->FactorAndSolve(num_rows, + num_cols, + lhs.data(), + rhs.data(), + actual.data(), + &summary.message); + EXPECT_EQ(summary.termination_type, LINEAR_SOLVER_SUCCESS); + EXPECT_NEAR((x - actual).norm() / x.norm(), + 0.0, + std::numeric_limits::epsilon() * 100) + << "\nexpected: " << x.transpose() + << "\nactual : " << actual.transpose(); + } + } + } +} + +#ifndef CERES_NO_LAPACK +INSTANTIATE_TEST_SUITE_P(_, + DenseQRTest, + ::testing::Values(EIGEN, LAPACK), + ParamInfoToString); +#else +INSTANTIATE_TEST_SUITE_P(_, + DenseQRTest, + ::testing::Values(EIGEN), + ParamInfoToString); +#endif + +} // namespace internal +} // namespace ceres diff --git a/internal/ceres/dense_sparse_matrix.cc b/internal/ceres/dense_sparse_matrix.cc index 53207fe30..80ea6585d 100644 --- a/internal/ceres/dense_sparse_matrix.cc +++ b/internal/ceres/dense_sparse_matrix.cc @@ -41,28 +41,10 @@ namespace ceres { namespace internal { DenseSparseMatrix::DenseSparseMatrix(int num_rows, int num_cols) - : has_diagonal_appended_(false), has_diagonal_reserved_(false) { - m_.resize(num_rows, num_cols); - m_.setZero(); -} - -DenseSparseMatrix::DenseSparseMatrix(int num_rows, - int num_cols, - bool reserve_diagonal) - : has_diagonal_appended_(false), has_diagonal_reserved_(reserve_diagonal) { - if (reserve_diagonal) { - // Allocate enough space for the diagonal. - m_.resize(num_rows + num_cols, num_cols); - } else { - m_.resize(num_rows, num_cols); - } - m_.setZero(); -} + : m_(Matrix(num_rows, num_cols)) {} DenseSparseMatrix::DenseSparseMatrix(const TripletSparseMatrix& m) - : m_(Eigen::MatrixXd::Zero(m.num_rows(), m.num_cols())), - has_diagonal_appended_(false), - has_diagonal_reserved_(false) { + : m_(Matrix::Zero(m.num_rows(), m.num_cols())) { const double* values = m.values(); const int* rows = m.rows(); const int* cols = m.cols(); @@ -73,8 +55,7 @@ DenseSparseMatrix::DenseSparseMatrix(const TripletSparseMatrix& m) } } -DenseSparseMatrix::DenseSparseMatrix(const ColMajorMatrix& m) - : m_(m), has_diagonal_appended_(false), has_diagonal_reserved_(false) {} +DenseSparseMatrix::DenseSparseMatrix(const Matrix& m) : m_(m) {} void DenseSparseMatrix::SetZero() { m_.setZero(); } @@ -96,72 +77,22 @@ void DenseSparseMatrix::ScaleColumns(const double* scale) { } void DenseSparseMatrix::ToDenseMatrix(Matrix* dense_matrix) const { - *dense_matrix = m_.block(0, 0, num_rows(), num_cols()); + *dense_matrix = m_; } -void DenseSparseMatrix::AppendDiagonal(double* d) { - CHECK(!has_diagonal_appended_); - if (!has_diagonal_reserved_) { - ColMajorMatrix tmp = m_; - m_.resize(m_.rows() + m_.cols(), m_.cols()); - m_.setZero(); - m_.block(0, 0, tmp.rows(), tmp.cols()) = tmp; - has_diagonal_reserved_ = true; - } - - m_.bottomLeftCorner(m_.cols(), m_.cols()) = - ConstVectorRef(d, m_.cols()).asDiagonal(); - has_diagonal_appended_ = true; -} - -void DenseSparseMatrix::RemoveDiagonal() { - CHECK(has_diagonal_appended_); - has_diagonal_appended_ = false; - // Leave the diagonal reserved. -} - -int DenseSparseMatrix::num_rows() const { - if (has_diagonal_reserved_ && !has_diagonal_appended_) { - return m_.rows() - m_.cols(); - } - return m_.rows(); -} +int DenseSparseMatrix::num_rows() const { return m_.rows(); } int DenseSparseMatrix::num_cols() const { return m_.cols(); } -int DenseSparseMatrix::num_nonzeros() const { - if (has_diagonal_reserved_ && !has_diagonal_appended_) { - return (m_.rows() - m_.cols()) * m_.cols(); - } - return m_.rows() * m_.cols(); -} +int DenseSparseMatrix::num_nonzeros() const { return m_.rows() * m_.cols(); } -ConstColMajorMatrixRef DenseSparseMatrix::matrix() const { - return ConstColMajorMatrixRef( - m_.data(), - ((has_diagonal_reserved_ && !has_diagonal_appended_) - ? m_.rows() - m_.cols() - : m_.rows()), - m_.cols(), - Eigen::Stride(m_.rows(), 1)); -} +const Matrix& DenseSparseMatrix::matrix() const { return m_; } -ColMajorMatrixRef DenseSparseMatrix::mutable_matrix() { - return ColMajorMatrixRef(m_.data(), - ((has_diagonal_reserved_ && !has_diagonal_appended_) - ? m_.rows() - m_.cols() - : m_.rows()), - m_.cols(), - Eigen::Stride(m_.rows(), 1)); -} +Matrix* DenseSparseMatrix::mutable_matrix() { return &m_; } void DenseSparseMatrix::ToTextFile(FILE* file) const { CHECK(file != nullptr); - const int active_rows = (has_diagonal_reserved_ && !has_diagonal_appended_) - ? (m_.rows() - m_.cols()) - : m_.rows(); - - for (int r = 0; r < active_rows; ++r) { + for (int r = 0; r < m_.rows(); ++r) { for (int c = 0; c < m_.cols(); ++c) { fprintf(file, "% 10d % 10d %17f\n", r, c, m_(r, c)); } diff --git a/internal/ceres/dense_sparse_matrix.h b/internal/ceres/dense_sparse_matrix.h index 94064b3ed..e2f4a6e91 100644 --- a/internal/ceres/dense_sparse_matrix.h +++ b/internal/ceres/dense_sparse_matrix.h @@ -48,12 +48,10 @@ class CERES_EXPORT_INTERNAL DenseSparseMatrix : public SparseMatrix { // Build a matrix with the same content as the TripletSparseMatrix // m. This assumes that m does not have any repeated entries. explicit DenseSparseMatrix(const TripletSparseMatrix& m); - explicit DenseSparseMatrix(const ColMajorMatrix& m); - + explicit DenseSparseMatrix(const Matrix& m); DenseSparseMatrix(int num_rows, int num_cols); - DenseSparseMatrix(int num_rows, int num_cols, bool reserve_diagonal); - virtual ~DenseSparseMatrix() {} + virtual ~DenseSparseMatrix() = default; // SparseMatrix interface. void SetZero() final; @@ -69,37 +67,11 @@ class CERES_EXPORT_INTERNAL DenseSparseMatrix : public SparseMatrix { const double* values() const final { return m_.data(); } double* mutable_values() final { return m_.data(); } - ConstColMajorMatrixRef matrix() const; - ColMajorMatrixRef mutable_matrix(); - - // Only one diagonal can be appended at a time. The diagonal is appended to - // as a new set of rows, e.g. - // - // Original matrix: - // - // x x x - // x x x - // x x x - // - // After append diagonal (1, 2, 3): - // - // x x x - // x x x - // x x x - // 1 0 0 - // 0 2 0 - // 0 0 3 - // - // Calling RemoveDiagonal removes the block. It is a fatal error to append a - // diagonal to a matrix that already has an appended diagonal, and it is also - // a fatal error to remove a diagonal from a matrix that has none. - void AppendDiagonal(double* d); - void RemoveDiagonal(); + const Matrix& matrix() const; + Matrix* mutable_matrix(); private: - ColMajorMatrix m_; - bool has_diagonal_appended_; - bool has_diagonal_reserved_; + Matrix m_; }; } // namespace internal diff --git a/internal/ceres/lapack.cc b/internal/ceres/lapack.cc deleted file mode 100644 index 7ce00a5c2..000000000 --- a/internal/ceres/lapack.cc +++ /dev/null @@ -1,132 +0,0 @@ -// Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. -// http://ceres-solver.org/ -// -// 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/lapack.h" - -#include "ceres/internal/port.h" -#include "ceres/linear_solver.h" -#include "glog/logging.h" - -#ifndef CERES_NO_LAPACK -extern "C" void dgels_(char* uplo, - int* m, - int* n, - int* nrhs, - double* a, - int* lda, - double* b, - int* ldb, - double* work, - int* lwork, - int* info); -#endif - -namespace ceres { -namespace internal { - -int LAPACK::EstimateWorkSizeForQR(int num_rows, int num_cols) { -#ifdef CERES_NO_LAPACK - LOG(FATAL) << "Ceres was built without a LAPACK library."; - return -1; -#else - char trans = 'N'; - int nrhs = 1; - int lwork = -1; - double work; - int info = 0; - dgels_(&trans, - &num_rows, - &num_cols, - &nrhs, - NULL, - &num_rows, - NULL, - &num_rows, - &work, - &lwork, - &info); - - if (info < 0) { - LOG(FATAL) << "Congratulations, you found a bug in Ceres." - << "Please report it." - << "LAPACK::dgels fatal error." - << "Argument: " << -info << " is invalid."; - } - return static_cast(work); -#endif -} - -LinearSolverTerminationType LAPACK::SolveInPlaceUsingQR( - int num_rows, - int num_cols, - const double* in_lhs, - int work_size, - double* work, - double* rhs_and_solution, - std::string* message) { -#ifdef CERES_NO_LAPACK - LOG(FATAL) << "Ceres was built without a LAPACK library."; - return LINEAR_SOLVER_FATAL_ERROR; -#else - char trans = 'N'; - int m = num_rows; - int n = num_cols; - int nrhs = 1; - int lda = num_rows; - int ldb = num_rows; - int info = 0; - double* lhs = const_cast(in_lhs); - - dgels_(&trans, - &m, - &n, - &nrhs, - lhs, - &lda, - rhs_and_solution, - &ldb, - work, - &work_size, - &info); - - if (info < 0) { - LOG(FATAL) << "Congratulations, you found a bug in Ceres." - << "Please report it." - << "LAPACK::dgels fatal error." - << "Argument: " << -info << " is invalid."; - } - - *message = "Success."; - return LINEAR_SOLVER_SUCCESS; -#endif -} - -} // namespace internal -} // namespace ceres diff --git a/internal/ceres/lapack.h b/internal/ceres/lapack.h deleted file mode 100644 index 5c5bf8bf8..000000000 --- a/internal/ceres/lapack.h +++ /dev/null @@ -1,101 +0,0 @@ -// Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. -// http://ceres-solver.org/ -// -// 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_LAPACK_H_ -#define CERES_INTERNAL_LAPACK_H_ - -#include - -#include "ceres/internal/port.h" -#include "ceres/linear_solver.h" - -namespace ceres { -namespace internal { - -class LAPACK { - public: - // Solve - // - // lhs * solution = rhs - // - // using a Cholesky factorization. Here - // lhs is a symmetric positive definite matrix. It is assumed to be - // column major and only the lower triangular part of the matrix is - // referenced. - // - // This function uses the LAPACK dpotrf and dpotrs routines. - // - // The return value and the message string together describe whether - // the solver terminated successfully or not and if so, what was the - // reason for failure. - static LinearSolverTerminationType SolveInPlaceUsingCholesky( - int num_rows, - const double* lhs, - double* rhs_and_solution, - std::string* message); - - // The SolveUsingQR function requires a buffer for its temporary - // computation. This function given the size of the lhs matrix will - // return the size of the buffer needed. - static int EstimateWorkSizeForQR(int num_rows, int num_cols); - - // Solve - // - // lhs * solution = rhs - // - // using a dense QR factorization. lhs is an arbitrary (possibly - // rectangular) matrix with full column rank. - // - // work is an array of size work_size that this routine uses for its - // temporary storage. The optimal size of this array can be obtained - // by calling EstimateWorkSizeForQR. - // - // When calling, rhs_and_solution contains the rhs, and upon return - // the first num_col entries are the solution. - // - // This function uses the LAPACK dgels routine. - // - // The return value and the message string together describe whether - // the solver terminated successfully or not and if so, what was the - // reason for failure. - static LinearSolverTerminationType SolveInPlaceUsingQR( - int num_rows, - int num_cols, - const double* lhs, - int work_size, - double* work, - double* rhs_and_solution, - std::string* message); -}; - -} // namespace internal -} // namespace ceres - -#endif // CERES_INTERNAL_LAPACK_H_ diff --git a/internal/ceres/trust_region_minimizer_test.cc b/internal/ceres/trust_region_minimizer_test.cc index cad30eb3a..8f544d608 100644 --- a/internal/ceres/trust_region_minimizer_test.cc +++ b/internal/ceres/trust_region_minimizer_test.cc @@ -131,7 +131,7 @@ class PowellEvaluator2 : public Evaluator { dense_jacobian = down_cast(jacobian); dense_jacobian->SetZero(); - ColMajorMatrixRef jacobian_matrix = dense_jacobian->mutable_matrix(); + Matrix& jacobian_matrix = *(dense_jacobian->mutable_matrix()); CHECK_EQ(jacobian_matrix.cols(), num_active_cols_); int column_index = 0;