Add dynamic_sparsity option. The standard sparse normal Cholesky solver assumes a fixed sparsity pattern which is useful for a large number of problems presented to Ceres. However, some problems are symbolically dense but numerically sparse i.e. each residual is a function of a large number of parameters but at any given state the residual only depends on a sparse subset of them. For these class of problems it is faster to re-analyse the sparsity pattern of the jacobian at each iteration of the non-linear optimisation instead of including all of the zero entries in the step computation. The proposed solution adds the dynamic_sparsity option which can be used with SPARSE_NORMAL_CHOLESKY. A DynamicCompressedRowSparseMatrix type (which extends CompressedRowSparseMatrix) has been introduced which allows dynamic addition and removal of elements. A Finalize method is provided which then consolidates the matrix so that it can be used in place of a regular CompressedRowSparseMatrix. An associated jacobian writer has also been provided. Changes that were required to make this extension were adding the SetMaxNumNonZeros method to CompressedRowSparseMatrix and adding a JacobianFinalizer template parameter to the ProgramEvaluator. Change-Id: Ia5a8a9523fdae8d5b027bc35e70b4611ec2a8d01
diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index f388146..60e464d 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt
@@ -49,6 +49,9 @@ TARGET_LINK_LIBRARIES(curve_fitting_c m) ENDIF (NOT MSVC) +ADD_EXECUTABLE(ellipse_approximation ellipse_approximation.cc) +TARGET_LINK_LIBRARIES(ellipse_approximation ceres) + ADD_EXECUTABLE(robust_curve_fitting robust_curve_fitting.cc) TARGET_LINK_LIBRARIES(robust_curve_fitting ceres)
diff --git a/examples/ellipse_approximation.cc b/examples/ellipse_approximation.cc new file mode 100644 index 0000000..a5bbe02 --- /dev/null +++ b/examples/ellipse_approximation.cc
@@ -0,0 +1,451 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2014 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: richie.stebbing@gmail.com (Richard Stebbing) +// +// This fits points randomly distributed on an ellipse with an approximate +// line segment contour. This is done by jointly optimizing the control points +// of the line segment contour along with the preimage positions for the data +// points. The purpose of this example is to show an example use case for +// dynamic_sparsity, and how it can benefit problems which are numerically +// dense but dynamically sparse. + +#include <cmath> +#include <vector> +#include "ceres/ceres.h" +#include "glog/logging.h" + +// Data generated with the following Python code. +// import numpy as np +// np.random.seed(1337) +// t = np.linspace(0.0, 2.0 * np.pi, 212, endpoint=False) +// t += 2.0 * np.pi * 0.01 * np.random.randn(t.size) +// theta = np.deg2rad(15) +// a, b = np.cos(theta), np.sin(theta) +// R = np.array([[a, -b], +// [b, a]]) +// Y = np.dot(np.c_[4.0 * np.cos(t), np.sin(t)], R.T) + +const int kYRows = 212; +const int kYCols = 2; +const double kYData[kYRows * kYCols] = { + +3.871364e+00, +9.916027e-01, + +3.864003e+00, +1.034148e+00, + +3.850651e+00, +1.072202e+00, + +3.868350e+00, +1.014408e+00, + +3.796381e+00, +1.153021e+00, + +3.857138e+00, +1.056102e+00, + +3.787532e+00, +1.162215e+00, + +3.704477e+00, +1.227272e+00, + +3.564711e+00, +1.294959e+00, + +3.754363e+00, +1.191948e+00, + +3.482098e+00, +1.322725e+00, + +3.602777e+00, +1.279658e+00, + +3.585433e+00, +1.286858e+00, + +3.347505e+00, +1.356415e+00, + +3.220855e+00, +1.378914e+00, + +3.558808e+00, +1.297174e+00, + +3.403618e+00, +1.343809e+00, + +3.179828e+00, +1.384721e+00, + +3.054789e+00, +1.398759e+00, + +3.294153e+00, +1.366808e+00, + +3.247312e+00, +1.374813e+00, + +2.988547e+00, +1.404247e+00, + +3.114508e+00, +1.392698e+00, + +2.899226e+00, +1.409802e+00, + +2.533256e+00, +1.414778e+00, + +2.654773e+00, +1.415909e+00, + +2.565100e+00, +1.415313e+00, + +2.976456e+00, +1.405118e+00, + +2.484200e+00, +1.413640e+00, + +2.324751e+00, +1.407476e+00, + +1.930468e+00, +1.378221e+00, + +2.329017e+00, +1.407688e+00, + +1.760640e+00, +1.360319e+00, + +2.147375e+00, +1.396603e+00, + +1.741989e+00, +1.358178e+00, + +1.743859e+00, +1.358394e+00, + +1.557372e+00, +1.335208e+00, + +1.280551e+00, +1.295087e+00, + +1.429880e+00, +1.317546e+00, + +1.213485e+00, +1.284400e+00, + +9.168172e-01, +1.232870e+00, + +1.311141e+00, +1.299839e+00, + +1.231969e+00, +1.287382e+00, + +7.453773e-01, +1.200049e+00, + +6.151587e-01, +1.173683e+00, + +5.935666e-01, +1.169193e+00, + +2.538707e-01, +1.094227e+00, + +6.806136e-01, +1.187089e+00, + +2.805447e-01, +1.100405e+00, + +6.184807e-01, +1.174371e+00, + +1.170550e-01, +1.061762e+00, + +2.890507e-01, +1.102365e+00, + +3.834234e-01, +1.123772e+00, + +3.980161e-04, +1.033061e+00, + -3.651680e-01, +9.370367e-01, + -8.386351e-01, +7.987201e-01, + -8.105704e-01, +8.073702e-01, + -8.735139e-01, +7.878886e-01, + -9.913836e-01, +7.506100e-01, + -8.784011e-01, +7.863636e-01, + -1.181440e+00, +6.882566e-01, + -1.229556e+00, +6.720191e-01, + -1.035839e+00, +7.362765e-01, + -8.031520e-01, +8.096470e-01, + -1.539136e+00, +5.629549e-01, + -1.755423e+00, +4.817306e-01, + -1.337589e+00, +6.348763e-01, + -1.836966e+00, +4.499485e-01, + -1.913367e+00, +4.195617e-01, + -2.126467e+00, +3.314900e-01, + -1.927625e+00, +4.138238e-01, + -2.339862e+00, +2.379074e-01, + -1.881736e+00, +4.322152e-01, + -2.116753e+00, +3.356163e-01, + -2.255733e+00, +2.754930e-01, + -2.555834e+00, +1.368473e-01, + -2.770277e+00, +2.895711e-02, + -2.563376e+00, +1.331890e-01, + -2.826715e+00, -9.000818e-04, + -2.978191e+00, -8.457804e-02, + -3.115855e+00, -1.658786e-01, + -2.982049e+00, -8.678322e-02, + -3.307892e+00, -2.902083e-01, + -3.038346e+00, -1.194222e-01, + -3.190057e+00, -2.122060e-01, + -3.279086e+00, -2.705777e-01, + -3.322028e+00, -2.999889e-01, + -3.122576e+00, -1.699965e-01, + -3.551973e+00, -4.768674e-01, + -3.581866e+00, -5.032175e-01, + -3.497799e+00, -4.315203e-01, + -3.565384e+00, -4.885602e-01, + -3.699493e+00, -6.199815e-01, + -3.585166e+00, -5.061925e-01, + -3.758914e+00, -6.918275e-01, + -3.741104e+00, -6.689131e-01, + -3.688331e+00, -6.077239e-01, + -3.810425e+00, -7.689015e-01, + -3.791829e+00, -7.386911e-01, + -3.789951e+00, -7.358189e-01, + -3.823100e+00, -7.918398e-01, + -3.857021e+00, -8.727074e-01, + -3.858250e+00, -8.767645e-01, + -3.872100e+00, -9.563174e-01, + -3.864397e+00, -1.032630e+00, + -3.846230e+00, -1.081669e+00, + -3.834799e+00, -1.102536e+00, + -3.866684e+00, -1.022901e+00, + -3.808643e+00, -1.139084e+00, + -3.868840e+00, -1.011569e+00, + -3.791071e+00, -1.158615e+00, + -3.797999e+00, -1.151267e+00, + -3.696278e+00, -1.232314e+00, + -3.779007e+00, -1.170504e+00, + -3.622855e+00, -1.270793e+00, + -3.647249e+00, -1.259166e+00, + -3.655412e+00, -1.255042e+00, + -3.573218e+00, -1.291696e+00, + -3.638019e+00, -1.263684e+00, + -3.498409e+00, -1.317750e+00, + -3.304143e+00, -1.364970e+00, + -3.183001e+00, -1.384295e+00, + -3.202456e+00, -1.381599e+00, + -3.244063e+00, -1.375332e+00, + -3.233308e+00, -1.377019e+00, + -3.060112e+00, -1.398264e+00, + -3.078187e+00, -1.396517e+00, + -2.689594e+00, -1.415761e+00, + -2.947662e+00, -1.407039e+00, + -2.854490e+00, -1.411860e+00, + -2.660499e+00, -1.415900e+00, + -2.875955e+00, -1.410930e+00, + -2.675385e+00, -1.415848e+00, + -2.813155e+00, -1.413363e+00, + -2.417673e+00, -1.411512e+00, + -2.725461e+00, -1.415373e+00, + -2.148334e+00, -1.396672e+00, + -2.108972e+00, -1.393738e+00, + -2.029905e+00, -1.387302e+00, + -2.046214e+00, -1.388687e+00, + -2.057402e+00, -1.389621e+00, + -1.650250e+00, -1.347160e+00, + -1.806764e+00, -1.365469e+00, + -1.206973e+00, -1.283343e+00, + -8.029259e-01, -1.211308e+00, + -1.229551e+00, -1.286993e+00, + -1.101507e+00, -1.265754e+00, + -9.110645e-01, -1.231804e+00, + -1.110046e+00, -1.267211e+00, + -8.465274e-01, -1.219677e+00, + -7.594163e-01, -1.202818e+00, + -8.023823e-01, -1.211203e+00, + -3.732519e-01, -1.121494e+00, + -1.918373e-01, -1.079668e+00, + -4.671988e-01, -1.142253e+00, + -4.033645e-01, -1.128215e+00, + -1.920740e-01, -1.079724e+00, + -3.022157e-01, -1.105389e+00, + -1.652831e-01, -1.073354e+00, + +4.671625e-01, -9.085886e-01, + +5.940178e-01, -8.721832e-01, + +3.147557e-01, -9.508290e-01, + +6.383631e-01, -8.591867e-01, + +9.888923e-01, -7.514088e-01, + +7.076339e-01, -8.386023e-01, + +1.326682e+00, -6.386698e-01, + +1.149834e+00, -6.988221e-01, + +1.257742e+00, -6.624207e-01, + +1.492352e+00, -5.799632e-01, + +1.595574e+00, -5.421766e-01, + +1.240173e+00, -6.684113e-01, + +1.706612e+00, -5.004442e-01, + +1.873984e+00, -4.353002e-01, + +1.985633e+00, -3.902561e-01, + +1.722880e+00, -4.942329e-01, + +2.095182e+00, -3.447402e-01, + +2.018118e+00, -3.768991e-01, + +2.422702e+00, -1.999563e-01, + +2.370611e+00, -2.239326e-01, + +2.152154e+00, -3.205250e-01, + +2.525121e+00, -1.516499e-01, + +2.422116e+00, -2.002280e-01, + +2.842806e+00, +9.536372e-03, + +3.030128e+00, +1.146027e-01, + +2.888424e+00, +3.433444e-02, + +2.991609e+00, +9.226409e-02, + +2.924807e+00, +5.445844e-02, + +3.007772e+00, +1.015875e-01, + +2.781973e+00, -2.282382e-02, + +3.164737e+00, +1.961781e-01, + +3.237671e+00, +2.430139e-01, + +3.046123e+00, +1.240014e-01, + +3.414834e+00, +3.669060e-01, + +3.436591e+00, +3.833600e-01, + +3.626207e+00, +5.444311e-01, + +3.223325e+00, +2.336361e-01, + +3.511963e+00, +4.431060e-01, + +3.698380e+00, +6.187442e-01, + +3.670244e+00, +5.884943e-01, + +3.558833e+00, +4.828230e-01, + +3.661807e+00, +5.797689e-01, + +3.767261e+00, +7.030893e-01, + +3.801065e+00, +7.532650e-01, + +3.828523e+00, +8.024454e-01, + +3.840719e+00, +8.287032e-01, + +3.848748e+00, +8.485921e-01, + +3.865801e+00, +9.066551e-01, + +3.870983e+00, +9.404873e-01, + +3.870263e+00, +1.001884e+00, + +3.864462e+00, +1.032374e+00, + +3.870542e+00, +9.996121e-01, + +3.865424e+00, +1.028474e+00 +}; +ceres::ConstMatrixRef kY(kYData, kYRows, kYCols); + +class PointToLineSegmentContourCostFunction : public ceres::CostFunction { + public: + PointToLineSegmentContourCostFunction(const int num_segments, + const Eigen::Vector2d y) + : num_segments_(num_segments), y_(y) { + // The first parameter is the preimage position. + mutable_parameter_block_sizes()->push_back(1); + // The next parameters are the control points for the line segment contour. + for (int i = 0; i < num_segments_; ++i) { + mutable_parameter_block_sizes()->push_back(2); + } + set_num_residuals(2); + } + + virtual bool Evaluate(const double* const* x, + double* residuals, + double** jacobians) const { + // Convert the preimage position `t` into a segment index `i0` and the + // line segment interpolation parameter `u`. `i1` is the index of the next + // control point. + const double t = ModuloNumSegments(*x[0]); + CHECK_GE(t, 0.0); + CHECK_LT(t, num_segments_); + const int i0 = floor(t), i1 = (i0 + 1) % num_segments_; + const double u = t - i0; + + // Linearly interpolate between control points `i0` and `i1`. + residuals[0] = y_[0] - ((1.0 - u) * x[1 + i0][0] + u * x[1 + i1][0]); + residuals[1] = y_[1] - ((1.0 - u) * x[1 + i0][1] + u * x[1 + i1][1]); + + if (jacobians == NULL) { + return true; + } + + if (jacobians[0] != NULL) { + jacobians[0][0] = x[1 + i0][0] - x[1 + i1][0]; + jacobians[0][1] = x[1 + i0][1] - x[1 + i1][1]; + } + for (int i = 0; i < num_segments_; ++i) { + if (jacobians[i + 1] != NULL) { + ceres::MatrixRef(jacobians[i + 1], 2, 2).setZero(); + if (i == i0) { + jacobians[i + 1][0] = -(1.0 - u); + jacobians[i + 1][3] = -(1.0 - u); + } else if (i == i1) { + jacobians[i + 1][0] = -u; + jacobians[i + 1][3] = -u; + } + } + } + return true; + } + + static ceres::CostFunction* Create(const int num_segments, + const Eigen::Vector2d y) { + return new PointToLineSegmentContourCostFunction(num_segments, y); + } + + private: + inline double ModuloNumSegments(const double& t) const { + return t - num_segments_ * floor(t / num_segments_); + } + + const int num_segments_; + const Eigen::Vector2d y_; +}; + +struct EuclideanDistanceFunctor { + EuclideanDistanceFunctor(const double& sqrt_weight) + : sqrt_weight_(sqrt_weight) {} + + template <typename T> + bool operator()(const T* x0, const T* x1, T* residuals) const { + residuals[0] = T(sqrt_weight_) * (x0[0] - x1[0]); + residuals[1] = T(sqrt_weight_) * (x0[1] - x1[1]); + return true; + } + + static ceres::CostFunction* Create(const double& sqrt_weight) { + return new ceres::AutoDiffCostFunction<EuclideanDistanceFunctor, 2, 2, 2>( + new EuclideanDistanceFunctor(sqrt_weight)); + } + + private: + const double sqrt_weight_; +}; + +bool SolveWithFullReport(ceres::Solver::Options options, + ceres::Problem* problem, + bool dynamic_sparsity) { + options.dynamic_sparsity = dynamic_sparsity; + + ceres::Solver::Summary summary; + ceres::Solve(options, problem, &summary); + + std::cout << "####################" << std::endl; + std::cout << "dynamic_sparsity = " << dynamic_sparsity << std::endl; + std::cout << "####################" << std::endl; + std::cout << summary.FullReport() << std::endl; + + return summary.termination_type == ceres::CONVERGENCE; +} + +int main(int argc, char** argv) { + google::InitGoogleLogging(argv[0]); + + // Problem configuration. + const int num_segments = 151; + const double regularization_weight = 1e-2; + + // Eigen::MatrixXd is column major so we define our own MatrixXd which is + // row major. Eigen::VectorXd can be used directly. + typedef Eigen::Matrix<double, + Eigen::Dynamic, Eigen::Dynamic, + Eigen::RowMajor> MatrixXd; + using Eigen::VectorXd; + + // `X` is the matrix of control points which make up the contour of line + // segments. The number of control points is equal to the number of line + // segments because the contour is closed. + // + // Initialize `X` to points on the unit circle. + VectorXd w(num_segments + 1); + w.setLinSpaced(num_segments + 1, 0.0, 2.0 * M_PI); + w.conservativeResize(num_segments); + MatrixXd X(num_segments, 2); + X.col(0) = w.array().cos(); + X.col(1) = w.array().sin(); + + // Each data point has an associated preimage position on the line segment + // contour. For each data point we initialize the preimage positions to + // the index of the closest control point. + const int num_observations = kY.rows(); + VectorXd t(num_observations); + for (int i = 0; i < num_observations; ++i) { + (X.rowwise() - kY.row(i)).rowwise().squaredNorm().minCoeff(&t[i]); + } + + ceres::Problem problem; + + // For each data point add a residual which measures its distance to its + // corresponding position on the line segment contour. + std::vector<double*> parameter_blocks(1 + num_segments); + parameter_blocks[0] = NULL; + for (int i = 0; i < num_segments; ++i) { + parameter_blocks[i + 1] = X.data() + 2 * i; + } + for (int i = 0; i < num_observations; ++i) { + parameter_blocks[0] = &t[i]; + problem.AddResidualBlock( + PointToLineSegmentContourCostFunction::Create(num_segments, kY.row(i)), + NULL, + parameter_blocks); + } + + // Add regularization to minimize the length of the line segment contour. + for (int i = 0; i < num_segments; ++i) { + problem.AddResidualBlock( + EuclideanDistanceFunctor::Create(sqrt(regularization_weight)), + NULL, + X.data() + 2 * i, + X.data() + 2 * ((i + 1) % num_segments)); + } + + ceres::Solver::Options options; + options.max_num_iterations = 100; + options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY; + + // First, solve `X` and `t` jointly with dynamic_sparsity = true. + MatrixXd X0 = X; + VectorXd t0 = t; + CHECK(SolveWithFullReport(options, &problem, true)); + + // Second, solve with dynamic_sparsity = false. + X = X0; + t = t0; + CHECK(SolveWithFullReport(options, &problem, false)); + + return 0; +}
diff --git a/include/ceres/solver.h b/include/ceres/solver.h index c5bfadc..3d63951 100644 --- a/include/ceres/solver.h +++ b/include/ceres/solver.h
@@ -108,6 +108,7 @@ num_linear_solver_threads = 1; use_postordering = false; + dynamic_sparsity = false; min_linear_solver_iterations = 1; max_linear_solver_iterations = 500; eta = 1e-1; @@ -500,6 +501,21 @@ // matrix. Setting use_postordering to true enables this tradeoff. bool use_postordering; + // Some non-linear least squares problems are symbolically dense but + // numerically sparse. i.e. at any given state only a small number + // of jacobian entries are non-zero, but the position and number of + // non-zeros is different depending on the state. For these problems + // it can be useful to factorize the sparse jacobian at each solver + // iteration instead of including all of the zero entries in a single + // general factorization. + // + // If your problem does not have this property (or you do not know), + // then it is probably best to keep this false, otherwise it will + // likely lead to worse performance. + + // This settings affects the SPARSE_NORMAL_CHOLESKY solver. + bool dynamic_sparsity; + // Some non-linear least squares problems have additional // structure in the way the parameter blocks interact that it is // beneficial to modify the way the trust region step is computed.
diff --git a/internal/ceres/CMakeLists.txt b/internal/ceres/CMakeLists.txt index 87faf2b..a9ec1c2 100644 --- a/internal/ceres/CMakeLists.txt +++ b/internal/ceres/CMakeLists.txt
@@ -59,6 +59,8 @@ dense_sparse_matrix.cc detect_structure.cc dogleg_strategy.cc + dynamic_compressed_row_jacobian_writer.cc + dynamic_compressed_row_sparse_matrix.cc evaluator.cc file.cc gradient_checking_cost_function.cc @@ -220,6 +222,7 @@ CERES_TEST(covariance) CERES_TEST(dense_sparse_matrix) CERES_TEST(dynamic_autodiff_cost_function) + CERES_TEST(dynamic_compressed_row_sparse_matrix) CERES_TEST(dynamic_numeric_diff_cost_function) CERES_TEST(evaluator) CERES_TEST(gradient_checker)
diff --git a/internal/ceres/compressed_row_jacobian_writer.cc b/internal/ceres/compressed_row_jacobian_writer.cc index bbadb77..ed8db14 100644 --- a/internal/ceres/compressed_row_jacobian_writer.cc +++ b/internal/ceres/compressed_row_jacobian_writer.cc
@@ -40,6 +40,44 @@ namespace ceres { namespace internal { +void CompressedRowJacobianWriter::PopulateJacobianRowAndColumnBlockVectors( + const Program* program, CompressedRowSparseMatrix* jacobian) { + const vector<ParameterBlock*>& parameter_blocks = + program->parameter_blocks(); + vector<int>& col_blocks = *(jacobian->mutable_col_blocks()); + col_blocks.resize(parameter_blocks.size()); + for (int i = 0; i < parameter_blocks.size(); ++i) { + col_blocks[i] = parameter_blocks[i]->LocalSize(); + } + + const vector<ResidualBlock*>& residual_blocks = + program->residual_blocks(); + vector<int>& row_blocks = *(jacobian->mutable_row_blocks()); + row_blocks.resize(residual_blocks.size()); + for (int i = 0; i < residual_blocks.size(); ++i) { + row_blocks[i] = residual_blocks[i]->NumResiduals(); + } +} + +void CompressedRowJacobianWriter::GetOrderedParameterBlocks( + const Program* program, + int residual_id, + vector<pair<int, int> >* evaluated_jacobian_blocks) { + const ResidualBlock* residual_block = + program->residual_blocks()[residual_id]; + const int num_parameter_blocks = residual_block->NumParameterBlocks(); + + for (int j = 0; j < num_parameter_blocks; ++j) { + const ParameterBlock* parameter_block = + residual_block->parameter_blocks()[j]; + if (!parameter_block->IsConstant()) { + evaluated_jacobian_blocks->push_back( + make_pair(parameter_block->index(), j)); + } + } + sort(evaluated_jacobian_blocks->begin(), evaluated_jacobian_blocks->end()); +} + SparseMatrix* CompressedRowJacobianWriter::CreateJacobian() const { const vector<ResidualBlock*>& residual_blocks = program_->residual_blocks(); @@ -71,7 +109,7 @@ total_num_effective_parameters, num_jacobian_nonzeros + total_num_effective_parameters); - // At this stage, the CompressedSparseMatrix is an invalid state. But this + // At this stage, the CompressedRowSparseMatrix is an invalid state. But this // seems to be the only way to construct it without doing a memory copy. int* rows = jacobian->mutable_rows(); int* cols = jacobian->mutable_cols(); @@ -132,22 +170,7 @@ } CHECK_EQ(num_jacobian_nonzeros, rows[total_num_residuals]); - // Populate the row and column block vectors for use by block - // oriented ordering algorithms. This is useful when - // Solver::Options::use_block_amd = true. - const vector<ParameterBlock*>& parameter_blocks = - program_->parameter_blocks(); - vector<int>& col_blocks = *(jacobian->mutable_col_blocks()); - col_blocks.resize(parameter_blocks.size()); - for (int i = 0; i < parameter_blocks.size(); ++i) { - col_blocks[i] = parameter_blocks[i]->LocalSize(); - } - - vector<int>& row_blocks = *(jacobian->mutable_row_blocks()); - row_blocks.resize(residual_blocks.size()); - for (int i = 0; i < residual_blocks.size(); ++i) { - row_blocks[i] = residual_blocks[i]->NumResiduals(); - } + PopulateJacobianRowAndColumnBlockVectors(program_, jacobian); return jacobian; } @@ -164,25 +187,10 @@ const ResidualBlock* residual_block = program_->residual_blocks()[residual_id]; - const int num_parameter_blocks = residual_block->NumParameterBlocks(); const int num_residuals = residual_block->NumResiduals(); - // It is necessary to determine the order of the jacobian blocks before - // copying them into the CompressedRowSparseMatrix. Just because a cost - // function uses parameter blocks 1 after 2 in its arguments does not mean - // that the block 1 occurs before block 2 in the column layout of the - // jacobian. Thus, determine the order by sorting the jacobian blocks by their - // position in the state vector. vector<pair<int, int> > evaluated_jacobian_blocks; - for (int j = 0; j < num_parameter_blocks; ++j) { - const ParameterBlock* parameter_block = - residual_block->parameter_blocks()[j]; - if (!parameter_block->IsConstant()) { - evaluated_jacobian_blocks.push_back( - make_pair(parameter_block->index(), j)); - } - } - sort(evaluated_jacobian_blocks.begin(), evaluated_jacobian_blocks.end()); + GetOrderedParameterBlocks(program_, residual_id, &evaluated_jacobian_blocks); // Where in the current row does the jacobian for a parameter block begin. int col_pos = 0;
diff --git a/internal/ceres/compressed_row_jacobian_writer.h b/internal/ceres/compressed_row_jacobian_writer.h index c103165..935e650 100644 --- a/internal/ceres/compressed_row_jacobian_writer.h +++ b/internal/ceres/compressed_row_jacobian_writer.h
@@ -39,6 +39,7 @@ namespace ceres { namespace internal { +class CompressedRowSparseMatrix; class Program; class SparseMatrix; @@ -49,6 +50,36 @@ : program_(program) { } + // `PopulateJacobianRowAndColumnBlockVectors` sets `col_blocks` and + // `row_blocks` for a `CompressedRowSparseMatrix`, based on the parameter + // block sizes and residual sizes respectively from the program. This is + // useful when Solver::Options::use_block_amd = true; + // + // This function is static so that it is available to other jacobian + // writers which use `CompressedRowSparseMatrix` (or derived types). + // (Jacobian writers do not fall under any type hierarchy; they only + // have to provide an interface as specified in program_evaluator.h). + static void PopulateJacobianRowAndColumnBlockVectors( + const Program* program, + CompressedRowSparseMatrix* jacobian); + + // It is necessary to determine the order of the jacobian blocks before + // copying them into a `CompressedRowSparseMatrix` (or derived type). + // Just because a cost function uses parameter blocks 1 after 2 in its + // arguments does not mean that the block 1 occurs before block 2 in the + // column layout of the jacobian. Thus, `GetOrderedParameterBlocks` + // determines the order by sorting the jacobian blocks by their position in + // the state vector. + // + // This function is static so that it is available to other jacobian + // writers which use `CompressedRowSparseMatrix` (or derived types). + // (Jacobian writers do not fall under any type hierarchy; they only + // have to provide an interface as specified in program_evaluator.h). + static void GetOrderedParameterBlocks( + const Program* program, + int residual_id, + vector<pair<int, int> >* evaluated_jacobian_blocks); + // JacobianWriter interface. // Since the compressed row matrix has different layout than that assumed by
diff --git a/internal/ceres/compressed_row_sparse_matrix.cc b/internal/ceres/compressed_row_sparse_matrix.cc index bef98d6..7993ed6 100644 --- a/internal/ceres/compressed_row_sparse_matrix.cc +++ b/internal/ceres/compressed_row_sparse_matrix.cc
@@ -286,6 +286,13 @@ matrix->values.resize(matrix->rows[matrix->num_rows]); } +void CompressedRowSparseMatrix::SetMaxNumNonZeros(int num_nonzeros) { + CHECK_GE(num_nonzeros, 0); + + cols_.resize(num_nonzeros); + values_.resize(num_nonzeros); +} + void CompressedRowSparseMatrix::SolveLowerTriangularInPlace( double* solution) const { for (int r = 0; r < num_rows_; ++r) {
diff --git a/internal/ceres/compressed_row_sparse_matrix.h b/internal/ceres/compressed_row_sparse_matrix.h index 06b8689..a0ba7ee 100644 --- a/internal/ceres/compressed_row_sparse_matrix.h +++ b/internal/ceres/compressed_row_sparse_matrix.h
@@ -115,6 +115,9 @@ const vector<int>& col_blocks() const { return col_blocks_; } vector<int>* mutable_col_blocks() { return &col_blocks_; } + // Destructive array resizing method. + void SetMaxNumNonZeros(int num_nonzeros); + // Non-destructive array resizing method. void set_num_rows(const int num_rows) { num_rows_ = num_rows; } void set_num_cols(const int num_cols) { num_cols_ = num_cols; }
diff --git a/internal/ceres/dynamic_compressed_row_finalizer.h b/internal/ceres/dynamic_compressed_row_finalizer.h new file mode 100644 index 0000000..5e6b0d8 --- /dev/null +++ b/internal/ceres/dynamic_compressed_row_finalizer.h
@@ -0,0 +1,51 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2014 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: richie.stebbing@gmail.com (Richard Stebbing) + +#ifndef CERES_INTERNAL_DYNAMIC_COMPRESED_ROW_FINALIZER_H_ +#define CERES_INTERNAL_DYNAMIC_COMPRESED_ROW_FINALIZER_H_ + +#include "ceres/casts.h" +#include "ceres/dynamic_compressed_row_sparse_matrix.h" + +namespace ceres { +namespace internal { + +struct DynamicCompressedRowJacobianFinalizer { + void operator()(SparseMatrix* base_jacobian, int num_parameters) { + DynamicCompressedRowSparseMatrix* jacobian = + down_cast<DynamicCompressedRowSparseMatrix*>(base_jacobian); + jacobian->Finalize(num_parameters); + } +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_DYNAMIC_COMPRESED_ROW_FINALISER_H_
diff --git a/internal/ceres/dynamic_compressed_row_jacobian_writer.cc b/internal/ceres/dynamic_compressed_row_jacobian_writer.cc new file mode 100644 index 0000000..2f01617 --- /dev/null +++ b/internal/ceres/dynamic_compressed_row_jacobian_writer.cc
@@ -0,0 +1,107 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2014 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: richie.stebbing@gmail.com (Richard Stebbing) + +#include "ceres/compressed_row_jacobian_writer.h" +#include "ceres/dynamic_compressed_row_jacobian_writer.h" +#include "ceres/casts.h" +#include "ceres/dynamic_compressed_row_sparse_matrix.h" +#include "ceres/parameter_block.h" +#include "ceres/program.h" +#include "ceres/residual_block.h" + +namespace ceres { +namespace internal { + +ScratchEvaluatePreparer* +DynamicCompressedRowJacobianWriter::CreateEvaluatePreparers(int num_threads) { + return ScratchEvaluatePreparer::Create(*program_, num_threads); +} + +SparseMatrix* DynamicCompressedRowJacobianWriter::CreateJacobian() const { + // Initialize `jacobian` with zero number of `max_num_nonzeros`. + const int num_residuals = program_->NumResiduals(); + const int num_effective_parameters = program_->NumEffectiveParameters(); + + DynamicCompressedRowSparseMatrix* jacobian = + new DynamicCompressedRowSparseMatrix(num_residuals, + num_effective_parameters, + 0); + + CompressedRowJacobianWriter::PopulateJacobianRowAndColumnBlockVectors( + program_, jacobian); + + return jacobian; +} + +void DynamicCompressedRowJacobianWriter::Write(int residual_id, + int residual_offset, + double **jacobians, + SparseMatrix* base_jacobian) { + DynamicCompressedRowSparseMatrix* jacobian = + down_cast<DynamicCompressedRowSparseMatrix*>(base_jacobian); + + // Get the `residual_block` of interest. + const ResidualBlock* residual_block = + program_->residual_blocks()[residual_id]; + const int num_residuals = residual_block->NumResiduals(); + + vector<pair<int, int> > evaluated_jacobian_blocks; + CompressedRowJacobianWriter::GetOrderedParameterBlocks( + program_, residual_id, &evaluated_jacobian_blocks); + + // `residual_offset` is the residual row in the global jacobian. + // Empty the jacobian rows. + jacobian->ClearRows(residual_offset, num_residuals); + + // Iterate over each parameter block. + for (int i = 0; i < evaluated_jacobian_blocks.size(); ++i) { + const ParameterBlock* parameter_block = + program_->parameter_blocks()[evaluated_jacobian_blocks[i].first]; + const int parameter_block_jacobian_index = + evaluated_jacobian_blocks[i].second; + const int parameter_block_size = parameter_block->LocalSize(); + + // For each parameter block only insert its non-zero entries. + for (int r = 0; r < num_residuals; ++r) { + for (int c = 0; c < parameter_block_size; ++c) { + const double& v = jacobians[parameter_block_jacobian_index][ + r * parameter_block_size + c]; + // Only insert non-zero entries. + if (v != 0.0) { + jacobian->InsertEntry( + residual_offset + r, parameter_block->delta_offset() + c, v); + } + } + } + } +} + +} // namespace internal +} // namespace ceres
diff --git a/internal/ceres/dynamic_compressed_row_jacobian_writer.h b/internal/ceres/dynamic_compressed_row_jacobian_writer.h new file mode 100644 index 0000000..df9581b --- /dev/null +++ b/internal/ceres/dynamic_compressed_row_jacobian_writer.h
@@ -0,0 +1,83 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2014 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: richie.stebbing@gmail.com (Richard Stebbing) +// +// A jacobian writer that directly writes to dynamic compressed row sparse +// matrices. + +#ifndef CERES_INTERNAL_DYNAMIC_COMPRESSED_ROW_JACOBIAN_WRITER_H_ +#define CERES_INTERNAL_DYNAMIC_COMPRESSED_ROW_JACOBIAN_WRITER_H_ + +#include "ceres/evaluator.h" +#include "ceres/scratch_evaluate_preparer.h" + +namespace ceres { +namespace internal { + +class Program; +class SparseMatrix; + +class DynamicCompressedRowJacobianWriter { + public: + DynamicCompressedRowJacobianWriter(Evaluator::Options /* ignored */, + Program* program) + : program_(program) { + } + + // JacobianWriter interface. + + // The compressed row matrix has different layout than that assumed by + // the cost functions. The scratch space is therefore used to store + // the jacobians (including zeros) temporarily before only the non-zero + // entries are copied over to the larger jacobian in `Write`. + ScratchEvaluatePreparer* CreateEvaluatePreparers(int num_threads); + + // Return a `DynamicCompressedRowSparseMatrix` which is filled by + // `Write`. Note that `Finalize` must be called to make the + // `CompressedRowSparseMatrix` interface valid. + SparseMatrix* CreateJacobian() const; + + // Write only the non-zero jacobian entries for a residual block + // (specified by `residual_id`) into `base_jacobian`, starting at the row + // specifed by `residual_offset`. + // + // This method is thread-safe over residual blocks (each `residual_id`). + void Write(int residual_id, + int residual_offset, + double **jacobians, + SparseMatrix* base_jacobian); + + private: + Program* program_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_DYNAMIC_COMPRESSED_ROW_JACOBIAN_WRITER_H_
diff --git a/internal/ceres/dynamic_compressed_row_sparse_matrix.cc b/internal/ceres/dynamic_compressed_row_sparse_matrix.cc new file mode 100644 index 0000000..f285d52 --- /dev/null +++ b/internal/ceres/dynamic_compressed_row_sparse_matrix.cc
@@ -0,0 +1,107 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2014 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: richie.stebbing@gmail.com (Richard Stebbing) + +#include <cstring> +#include "ceres/dynamic_compressed_row_sparse_matrix.h" + +namespace ceres { +namespace internal { + +DynamicCompressedRowSparseMatrix::DynamicCompressedRowSparseMatrix( + int num_rows, + int num_cols, + int initial_max_num_nonzeros) + : CompressedRowSparseMatrix(num_rows, + num_cols, + initial_max_num_nonzeros) { + dynamic_cols_.resize(num_rows); + dynamic_values_.resize(num_rows); + } + +void DynamicCompressedRowSparseMatrix::InsertEntry(int row, + int col, + const double& value) { + CHECK_GE(row, 0); + CHECK_LT(row, num_rows()); + CHECK_GE(col, 0); + CHECK_LT(col, num_cols()); + dynamic_cols_[row].push_back(col); + dynamic_values_[row].push_back(value); +} + +void DynamicCompressedRowSparseMatrix::ClearRows(int row_start, + int num_rows) { + for (int r = 0; r < num_rows; ++r) { + const int i = row_start + r; + CHECK_GE(i, 0); + CHECK_LT(i, this->num_rows()); + dynamic_cols_[i].resize(0); + dynamic_values_[i].resize(0); + } +} + +void DynamicCompressedRowSparseMatrix::Finalize(int num_additional_elements) { + // `num_additional_elements` is provided as an argument so that additional + // storage can be reserved when it is known by the finalizer. + CHECK_GE(num_additional_elements, 0); + + // Count the number of non-zeros and resize `cols_` and `values_`. + int num_jacobian_nonzeros = 0; + for (int i = 0; i < dynamic_cols_.size(); ++i) { + num_jacobian_nonzeros += dynamic_cols_[i].size(); + } + + SetMaxNumNonZeros(num_jacobian_nonzeros + num_additional_elements); + + // Flatten `dynamic_cols_` into `cols_` and `dynamic_values_` + // into `values_`. + int index_into_values_and_cols = 0; + for (int i = 0; i < num_rows(); ++i) { + mutable_rows()[i] = index_into_values_and_cols; + const int num_nonzero_columns = dynamic_cols_[i].size(); + if (num_nonzero_columns > 0) { + memcpy(mutable_cols() + index_into_values_and_cols, + &dynamic_cols_[i][0], + dynamic_cols_[i].size() * sizeof(dynamic_cols_[0][0])); + memcpy(mutable_values() + index_into_values_and_cols, + &dynamic_values_[i][0], + dynamic_values_[i].size() * sizeof(dynamic_values_[0][0])); + index_into_values_and_cols += dynamic_cols_[i].size(); + } + } + mutable_rows()[num_rows()] = index_into_values_and_cols; + + CHECK_EQ(index_into_values_and_cols, num_jacobian_nonzeros) + << "Ceres bug: final index into values_ and cols_ should be equal to " + << "the number of jacobian nonzeros. Please contact the developers!"; +} + +} // namespace internal +} // namespace ceres
diff --git a/internal/ceres/dynamic_compressed_row_sparse_matrix.h b/internal/ceres/dynamic_compressed_row_sparse_matrix.h new file mode 100644 index 0000000..7a89a70 --- /dev/null +++ b/internal/ceres/dynamic_compressed_row_sparse_matrix.h
@@ -0,0 +1,99 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2014 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: richie.stebbing@gmail.com (Richard Stebbing) +// +// A compressed row sparse matrix that provides an extended interface to +// allow dynamic insertion of entries. This is provided for the use case +// where the sparsity structure and number of non-zero entries is dynamic. +// This flexibility is achieved by using an (internal) scratch space that +// allows independent insertion of entries into each row (thread-safe). +// Once insertion is complete, the `Finalize` method must be called to ensure +// that the underlying `CompressedRowSparseMatrix` is consistent. +// +// This should only be used if you really do need a dynamic sparsity pattern. + +#ifndef CERES_INTERNAL_DYNAMIC_COMPRESSED_ROW_SPARSE_MATRIX_H_ +#define CERES_INTERNAL_DYNAMIC_COMPRESSED_ROW_SPARSE_MATRIX_H_ + +#include "ceres/compressed_row_sparse_matrix.h" + +namespace ceres { +namespace internal { + +class DynamicCompressedRowSparseMatrix : public CompressedRowSparseMatrix { + public: + // Set the number of rows and columns for the underlyig + // `CompressedRowSparseMatrix` and set the initial number of maximum non-zero + // entries. Note that following the insertion of entries, when `Finalize` + // is called the number of non-zeros is determined and all internal + // structures are adjusted as required. If you know the upper limit on the + // number of non-zeros, then passing this value here can prevent future + // memory reallocations which may improve performance. Otherwise, if no + // upper limit is available a value of 0 is sufficient. + // + // Typical usage of this class is to define a new instance with a given + // number of rows, columns and maximum number of non-zero elements + // (if available). Next, entries are inserted at row and column positions + // using `InsertEntry`. Finally, once all elements have been inserted, + // `Finalize` must be called to make the underlying + // `CompressedRowSparseMatrix` consistent. + DynamicCompressedRowSparseMatrix(int num_rows, + int num_cols, + int initial_max_num_nonzeros); + + // Insert an entry at a given row and column position. This method is + // thread-safe across rows i.e. different threads can insert values + // simultaneously into different rows. It should be emphasised that this + // method always inserts a new entry and does not check for existing + // entries at the specified row and column position. Duplicate entries + // for a given row and column position will result in undefined + // behavior. + void InsertEntry(int row, int col, const double& value); + + // Clear all entries for rows, starting from row index `row_start` + // and proceeding for `num_rows`. + void ClearRows(int row_start, int num_rows); + + // Make the underlying internal `CompressedRowSparseMatrix` data structures + // consistent. Additional space for non-zero entries in the + // `CompressedRowSparseMatrix` can be reserved by specifying + // `num_additional_elements`. This is useful when it is known that rows will + // be appended to the `CompressedRowSparseMatrix` (e.g. appending a diagonal + // matrix to the jacobian) as it prevents need for future reallocation. + void Finalize(int num_additional_elements); + + private: + vector<vector<int> > dynamic_cols_; + vector<vector<double> > dynamic_values_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_DYNAMIC_COMPRESSED_ROW_SPARSE_MATRIX_H_
diff --git a/internal/ceres/dynamic_compressed_row_sparse_matrix_test.cc b/internal/ceres/dynamic_compressed_row_sparse_matrix_test.cc new file mode 100644 index 0000000..03bfcb6 --- /dev/null +++ b/internal/ceres/dynamic_compressed_row_sparse_matrix_test.cc
@@ -0,0 +1,217 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2014 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: richie.stebbing@gmail.com (Richard Stebbing) + +#include "ceres/dynamic_compressed_row_sparse_matrix.h" + +#include "ceres/casts.h" +#include "ceres/compressed_row_sparse_matrix.h" +#include "ceres/casts.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/linear_least_squares_problems.h" +#include "ceres/triplet_sparse_matrix.h" +#include "gtest/gtest.h" + +namespace ceres { +namespace internal { + +class DynamicCompressedRowSparseMatrixTest : public ::testing::Test { + protected: + virtual void SetUp() { + num_rows = 7; + num_cols = 4; + + // The number of additional elements reserved when `Finalize` is called + // should have no effect on the number of rows, columns or nonzeros. + // Set this to some nonzero value to be sure. + num_additional_elements = 13; + + expected_num_nonzeros = num_rows * num_cols - min(num_rows, num_cols); + + InitialiseDenseReference(); + InitialiseSparseMatrixReferences(); + + dcrsm.reset(new DynamicCompressedRowSparseMatrix(num_rows, + num_cols, + 0)); + } + + void Finalize() { + dcrsm->Finalize(num_additional_elements); + } + + void InitialiseDenseReference() { + dense.resize(num_rows, num_cols); + dense.setZero(); + int num_nonzeros = 0; + for (int i = 0; i < (num_rows * num_cols); ++i) { + const int r = i / num_cols, c = i % num_cols; + if (r != c) { + dense(r, c) = i + 1; + ++num_nonzeros; + } + } + ASSERT_EQ(num_nonzeros, expected_num_nonzeros); + } + + void InitialiseSparseMatrixReferences() { + std::vector<int> rows, cols; + std::vector<double> values; + for (int i = 0; i < (num_rows * num_cols); ++i) { + const int r = i / num_cols, c = i % num_cols; + if (r != c) { + rows.push_back(r); + cols.push_back(c); + values.push_back(i + 1); + } + } + ASSERT_EQ(values.size(), expected_num_nonzeros); + + tsm.reset(new TripletSparseMatrix(num_rows, + num_cols, + expected_num_nonzeros)); + std::copy(rows.begin(), rows.end(), tsm->mutable_rows()); + std::copy(cols.begin(), cols.end(), tsm->mutable_cols()); + std::copy(values.begin(), values.end(), tsm->mutable_values()); + tsm->set_num_nonzeros(values.size()); + + Matrix dense_from_tsm; + tsm->ToDenseMatrix(&dense_from_tsm); + ASSERT_TRUE((dense.array() == dense_from_tsm.array()).all()); + + crsm.reset(new CompressedRowSparseMatrix(*tsm)); + Matrix dense_from_crsm; + crsm->ToDenseMatrix(&dense_from_crsm); + ASSERT_TRUE((dense.array() == dense_from_crsm.array()).all()); + } + + void InsertNonZeroEntriesFromDenseReference() { + for (int r = 0; r < num_rows; ++r) { + for (int c = 0; c < num_cols; ++c) { + const double& v = dense(r, c); + if (v != 0.0) { + dcrsm->InsertEntry(r, c, v); + } + } + } + } + + void ExpectEmpty() { + EXPECT_EQ(dcrsm->num_rows(), num_rows); + EXPECT_EQ(dcrsm->num_cols(), num_cols); + EXPECT_EQ(dcrsm->num_nonzeros(), 0); + + Matrix dense_from_dcrsm; + dcrsm->ToDenseMatrix(&dense_from_dcrsm); + EXPECT_EQ(dense_from_dcrsm.rows(), num_rows); + EXPECT_EQ(dense_from_dcrsm.cols(), num_cols); + EXPECT_TRUE((dense_from_dcrsm.array() == 0.0).all()); + } + + void ExpectEqualToDenseReference() { + Matrix dense_from_dcrsm; + dcrsm->ToDenseMatrix(&dense_from_dcrsm); + EXPECT_TRUE((dense.array() == dense_from_dcrsm.array()).all()); + } + + void ExpectEqualToCompressedRowSparseMatrixReference() { + typedef Eigen::Map<const Eigen::VectorXi> ConstIntVectorRef; + + ConstIntVectorRef crsm_rows(crsm->rows(), crsm->num_rows() + 1); + ConstIntVectorRef dcrsm_rows(dcrsm->rows(), dcrsm->num_rows() + 1); + EXPECT_TRUE((crsm_rows.array() == dcrsm_rows.array()).all()); + + ConstIntVectorRef crsm_cols(crsm->cols(), crsm->num_nonzeros()); + ConstIntVectorRef dcrsm_cols(dcrsm->cols(), dcrsm->num_nonzeros()); + EXPECT_TRUE((crsm_cols.array() == dcrsm_cols.array()).all()); + + ConstVectorRef crsm_values(crsm->values(), crsm->num_nonzeros()); + ConstVectorRef dcrsm_values(dcrsm->values(), dcrsm->num_nonzeros()); + EXPECT_TRUE((crsm_values.array() == dcrsm_values.array()).all()); + } + + int num_rows; + int num_cols; + + int num_additional_elements; + + int expected_num_nonzeros; + + Matrix dense; + scoped_ptr<TripletSparseMatrix> tsm; + scoped_ptr<CompressedRowSparseMatrix> crsm; + + scoped_ptr<DynamicCompressedRowSparseMatrix> dcrsm; +}; + +TEST_F(DynamicCompressedRowSparseMatrixTest, Initialization) { + ExpectEmpty(); + + Finalize(); + ExpectEmpty(); +} + +TEST_F(DynamicCompressedRowSparseMatrixTest, InsertEntryAndFinalize) { + InsertNonZeroEntriesFromDenseReference(); + ExpectEmpty(); + + Finalize(); + ExpectEqualToDenseReference(); + ExpectEqualToCompressedRowSparseMatrixReference(); +} + +TEST_F(DynamicCompressedRowSparseMatrixTest, ClearRows) { + InsertNonZeroEntriesFromDenseReference(); + Finalize(); + ExpectEqualToDenseReference(); + ExpectEqualToCompressedRowSparseMatrixReference(); + + dcrsm->ClearRows(0, 0); + Finalize(); + ExpectEqualToDenseReference(); + ExpectEqualToCompressedRowSparseMatrixReference(); + + dcrsm->ClearRows(0, num_rows); + ExpectEqualToCompressedRowSparseMatrixReference(); + + Finalize(); + ExpectEmpty(); + + InsertNonZeroEntriesFromDenseReference(); + dcrsm->ClearRows(1, 2); + Finalize(); + dense.block(1, 0, 2, num_cols).setZero(); + ExpectEqualToDenseReference(); + + InitialiseDenseReference(); +} + +} // namespace internal +} // namespace ceres
diff --git a/internal/ceres/evaluator.cc b/internal/ceres/evaluator.cc index 31a4176..c94c62c 100644 --- a/internal/ceres/evaluator.cc +++ b/internal/ceres/evaluator.cc
@@ -35,6 +35,8 @@ #include "ceres/compressed_row_sparse_matrix.h" #include "ceres/crs_matrix.h" #include "ceres/dense_jacobian_writer.h" +#include "ceres/dynamic_compressed_row_finalizer.h" +#include "ceres/dynamic_compressed_row_jacobian_writer.h" #include "ceres/evaluator.h" #include "ceres/internal/port.h" #include "ceres/program_evaluator.h" @@ -63,9 +65,17 @@ BlockJacobianWriter>(options, program); case SPARSE_NORMAL_CHOLESKY: - return new ProgramEvaluator<ScratchEvaluatePreparer, - CompressedRowJacobianWriter>(options, - program); + if (options.dynamic_sparsity) { + return new ProgramEvaluator<ScratchEvaluatePreparer, + DynamicCompressedRowJacobianWriter, + DynamicCompressedRowJacobianFinalizer>( + options, program); + } else { + return new ProgramEvaluator<ScratchEvaluatePreparer, + CompressedRowJacobianWriter>(options, + program); + } + default: *error = "Invalid Linear Solver Type. Unable to create evaluator."; return NULL;
diff --git a/internal/ceres/evaluator.h b/internal/ceres/evaluator.h index 3d25462..8fc60b8 100644 --- a/internal/ceres/evaluator.h +++ b/internal/ceres/evaluator.h
@@ -61,11 +61,13 @@ Options() : num_threads(1), num_eliminate_blocks(-1), - linear_solver_type(DENSE_QR) {} + linear_solver_type(DENSE_QR), + dynamic_sparsity(false) {} int num_threads; int num_eliminate_blocks; LinearSolverType linear_solver_type; + bool dynamic_sparsity; }; static Evaluator* Create(const Options& options,
diff --git a/internal/ceres/evaluator_test.cc b/internal/ceres/evaluator_test.cc index ea24504..c0de3fc 100644 --- a/internal/ceres/evaluator_test.cc +++ b/internal/ceres/evaluator_test.cc
@@ -44,6 +44,7 @@ #include "ceres/program.h" #include "ceres/sized_cost_function.h" #include "ceres/sparse_matrix.h" +#include "ceres/stringprintf.h" #include "ceres/types.h" #include "gtest/gtest.h" @@ -91,18 +92,42 @@ } }; +struct EvaluatorTestOptions { + EvaluatorTestOptions(LinearSolverType linear_solver_type, + int num_eliminate_blocks, + bool dynamic_sparsity = false) + : linear_solver_type(linear_solver_type), + num_eliminate_blocks(num_eliminate_blocks), + dynamic_sparsity(dynamic_sparsity) {} + + LinearSolverType linear_solver_type; + int num_eliminate_blocks; + bool dynamic_sparsity; +}; + struct EvaluatorTest - : public ::testing::TestWithParam<pair<LinearSolverType, int> > { + : public ::testing::TestWithParam<EvaluatorTestOptions> { Evaluator* CreateEvaluator(Program* program) { // This program is straight from the ProblemImpl, and so has no index/offset // yet; compute it here as required by the evalutor implementations. program->SetParameterOffsetsAndIndex(); - VLOG(1) << "Creating evaluator with type: " << GetParam().first - << " and num_eliminate_blocks: " << GetParam().second; + if (VLOG_IS_ON(1)) { + string report; + StringAppendF(&report, "Creating evaluator with type: %d", + GetParam().linear_solver_type); + if (GetParam().linear_solver_type == SPARSE_NORMAL_CHOLESKY) { + StringAppendF(&report, ", dynamic_sparsity: %d", + GetParam().dynamic_sparsity); + } + StringAppendF(&report, " and num_eliminate_blocks: %d", + GetParam().num_eliminate_blocks); + VLOG(1) << report; + } Evaluator::Options options; - options.linear_solver_type = GetParam().first; - options.num_eliminate_blocks = GetParam().second; + options.linear_solver_type = GetParam().linear_solver_type; + options.num_eliminate_blocks = GetParam().num_eliminate_blocks; + options.dynamic_sparsity = GetParam().dynamic_sparsity; string error; return Evaluator::Create(options, program, &error); } @@ -517,23 +542,25 @@ INSTANTIATE_TEST_CASE_P( LinearSolvers, EvaluatorTest, - ::testing::Values(make_pair(DENSE_QR, 0), - make_pair(DENSE_SCHUR, 0), - make_pair(DENSE_SCHUR, 1), - make_pair(DENSE_SCHUR, 2), - make_pair(DENSE_SCHUR, 3), - make_pair(DENSE_SCHUR, 4), - make_pair(SPARSE_SCHUR, 0), - make_pair(SPARSE_SCHUR, 1), - make_pair(SPARSE_SCHUR, 2), - make_pair(SPARSE_SCHUR, 3), - make_pair(SPARSE_SCHUR, 4), - make_pair(ITERATIVE_SCHUR, 0), - make_pair(ITERATIVE_SCHUR, 1), - make_pair(ITERATIVE_SCHUR, 2), - make_pair(ITERATIVE_SCHUR, 3), - make_pair(ITERATIVE_SCHUR, 4), - make_pair(SPARSE_NORMAL_CHOLESKY, 0))); + ::testing::Values( + EvaluatorTestOptions(DENSE_QR, 0), + EvaluatorTestOptions(DENSE_SCHUR, 0), + EvaluatorTestOptions(DENSE_SCHUR, 1), + EvaluatorTestOptions(DENSE_SCHUR, 2), + EvaluatorTestOptions(DENSE_SCHUR, 3), + EvaluatorTestOptions(DENSE_SCHUR, 4), + EvaluatorTestOptions(SPARSE_SCHUR, 0), + EvaluatorTestOptions(SPARSE_SCHUR, 1), + EvaluatorTestOptions(SPARSE_SCHUR, 2), + EvaluatorTestOptions(SPARSE_SCHUR, 3), + EvaluatorTestOptions(SPARSE_SCHUR, 4), + EvaluatorTestOptions(ITERATIVE_SCHUR, 0), + EvaluatorTestOptions(ITERATIVE_SCHUR, 1), + EvaluatorTestOptions(ITERATIVE_SCHUR, 2), + EvaluatorTestOptions(ITERATIVE_SCHUR, 3), + EvaluatorTestOptions(ITERATIVE_SCHUR, 4), + EvaluatorTestOptions(SPARSE_NORMAL_CHOLESKY, 0, false), + EvaluatorTestOptions(SPARSE_NORMAL_CHOLESKY, 0, true))); // Simple cost function used to check if the evaluator is sensitive to // state changes.
diff --git a/internal/ceres/linear_solver.h b/internal/ceres/linear_solver.h index 8737c7c..f091bc5 100644 --- a/internal/ceres/linear_solver.h +++ b/internal/ceres/linear_solver.h
@@ -98,6 +98,7 @@ dense_linear_algebra_library_type(EIGEN), sparse_linear_algebra_library_type(SUITE_SPARSE), use_postordering(false), + dynamic_sparsity(false), min_num_iterations(1), max_num_iterations(1), num_threads(1), @@ -115,6 +116,7 @@ // See solver.h for information about this flag. bool use_postordering; + bool dynamic_sparsity; // Number of internal iterations that the solver uses. This // parameter only makes sense for iterative solvers like CG.
diff --git a/internal/ceres/program_evaluator.h b/internal/ceres/program_evaluator.h index 8aa2a39..a06dc8c 100644 --- a/internal/ceres/program_evaluator.h +++ b/internal/ceres/program_evaluator.h
@@ -97,7 +97,13 @@ namespace ceres { namespace internal { -template<typename EvaluatePreparer, typename JacobianWriter> +struct NullJacobianFinalizer { + void operator()(SparseMatrix* jacobian, int num_parameters) {} +}; + +template<typename EvaluatePreparer, + typename JacobianWriter, + typename JacobianFinalizer = NullJacobianFinalizer> class ProgramEvaluator : public Evaluator { public: ProgramEvaluator(const Evaluator::Options &options, Program* program) @@ -244,9 +250,10 @@ } if (!abort) { + const int num_parameters = program_->NumEffectiveParameters(); + // Sum the cost and gradient (if requested) from each thread. (*cost) = 0.0; - int num_parameters = program_->NumEffectiveParameters(); if (gradient != NULL) { VectorRef(gradient, num_parameters).setZero(); } @@ -257,6 +264,15 @@ VectorRef(evaluate_scratch_[i].gradient.get(), num_parameters); } } + + // Finalize the Jacobian if it is available. + // `num_parameters` is passed to the finalizer so that additional + // storage can be reserved for additional diagonal elements if + // necessary. + if (jacobian != NULL) { + JacobianFinalizer f; + f(jacobian, num_parameters); + } } return !abort; }
diff --git a/internal/ceres/solver_impl.cc b/internal/ceres/solver_impl.cc index e85ffbf..6ae2c90 100644 --- a/internal/ceres/solver_impl.cc +++ b/internal/ceres/solver_impl.cc
@@ -1183,7 +1183,8 @@ return transformed_program.release(); } - if (options->linear_solver_type == SPARSE_NORMAL_CHOLESKY) { + if (options->linear_solver_type == SPARSE_NORMAL_CHOLESKY && + !options->dynamic_sparsity) { if (!ReorderProgramForSparseNormalCholesky( options->sparse_linear_algebra_library_type, linear_solver_ordering, @@ -1305,6 +1306,7 @@ linear_solver_options.dense_linear_algebra_library_type = options->dense_linear_algebra_library_type; linear_solver_options.use_postordering = options->use_postordering; + linear_solver_options.dynamic_sparsity = options->dynamic_sparsity; // Ignore user's postordering preferences and force it to be true if // cholmod_camd is not available. This ensures that the linear @@ -1457,6 +1459,7 @@ ->second.size()) : 0; evaluator_options.num_threads = options.num_threads; + evaluator_options.dynamic_sparsity = options.dynamic_sparsity; return Evaluator::Create(evaluator_options, program, error); }
diff --git a/internal/ceres/sparse_normal_cholesky_solver.cc b/internal/ceres/sparse_normal_cholesky_solver.cc index 880adf7..07537e3 100644 --- a/internal/ceres/sparse_normal_cholesky_solver.cc +++ b/internal/ceres/sparse_normal_cholesky_solver.cc
@@ -56,13 +56,13 @@ options_(options) { } -SparseNormalCholeskySolver::~SparseNormalCholeskySolver() { +void SparseNormalCholeskySolver::FreeFactorization() { #ifndef CERES_NO_SUITESPARSE if (factor_ != NULL) { ss_.Free(factor_); factor_ = NULL; } -#endif +#endif // CERES_NO_SUITESPARSE #ifndef CERES_NO_CXSPARSE if (cxsparse_factor_ != NULL) { @@ -72,6 +72,10 @@ #endif // CERES_NO_CXSPARSE } +SparseNormalCholeskySolver::~SparseNormalCholeskySolver() { + FreeFactorization(); +} + LinearSolver::Summary SparseNormalCholeskySolver::SolveImpl( CompressedRowSparseMatrix* A, const double* b, @@ -150,13 +154,20 @@ event_logger.AddEvent("Setup"); // Compute symbolic factorization if not available. + if (options_.dynamic_sparsity) { + FreeFactorization(); + } if (cxsparse_factor_ == NULL) { if (options_.use_postordering) { cxsparse_factor_ = cxsparse_.BlockAnalyzeCholesky(AtA, A->col_blocks(), A->col_blocks()); } else { - cxsparse_factor_ = cxsparse_.AnalyzeCholeskyWithNaturalOrdering(AtA); + if (options_.dynamic_sparsity) { + cxsparse_factor_ = cxsparse_.AnalyzeCholesky(AtA); + } else { + cxsparse_factor_ = cxsparse_.AnalyzeCholeskyWithNaturalOrdering(AtA); + } } } event_logger.AddEvent("Analysis"); @@ -169,6 +180,7 @@ summary.termination_type = LINEAR_SOLVER_FAILURE; } event_logger.AddEvent("Solve"); + return summary; } #else @@ -198,6 +210,9 @@ cholmod_sparse lhs = ss_.CreateSparseMatrixTransposeView(A); event_logger.AddEvent("Setup"); + if (options_.dynamic_sparsity) { + FreeFactorization(); + } if (factor_ == NULL) { if (options_.use_postordering) { factor_ = ss_.BlockAnalyzeCholesky(&lhs, @@ -205,7 +220,11 @@ A->row_blocks(), &summary.message); } else { - factor_ = ss_.AnalyzeCholeskyWithNaturalOrdering(&lhs, &summary.message); + if (options_.dynamic_sparsity) { + factor_ = ss_.AnalyzeCholesky(&lhs, &summary.message); + } else { + factor_ = ss_.AnalyzeCholeskyWithNaturalOrdering(&lhs, &summary.message); + } } } event_logger.AddEvent("Analysis");
diff --git a/internal/ceres/sparse_normal_cholesky_solver.h b/internal/ceres/sparse_normal_cholesky_solver.h index adca08a..ea91534 100644 --- a/internal/ceres/sparse_normal_cholesky_solver.h +++ b/internal/ceres/sparse_normal_cholesky_solver.h
@@ -71,6 +71,8 @@ const LinearSolver::PerSolveOptions& options, double* rhs_and_solution); + void FreeFactorization(); + SuiteSparse ss_; // Cached factorization cholmod_factor* factor_;
diff --git a/internal/ceres/unsymmetric_linear_solver_test.cc b/internal/ceres/unsymmetric_linear_solver_test.cc index 624db68..412e611 100644 --- a/internal/ceres/unsymmetric_linear_solver_test.cc +++ b/internal/ceres/unsymmetric_linear_solver_test.cc
@@ -172,6 +172,15 @@ options.use_postordering = true; TestSolver(options); } + +TEST_F(UnsymmetricLinearSolverTest, + SparseNormalCholeskyUsingSuiteSparseDynamicSparsity) { + LinearSolver::Options options; + options.sparse_linear_algebra_library_type = SUITE_SPARSE; + options.type = SPARSE_NORMAL_CHOLESKY; + options.dynamic_sparsity = true; + TestSolver(options); +} #endif #ifndef CERES_NO_CXSPARSE @@ -192,6 +201,15 @@ options.use_postordering = true; TestSolver(options); } + +TEST_F(UnsymmetricLinearSolverTest, + SparseNormalCholeskyUsingCXSparseDynamicSparsity) { + LinearSolver::Options options; + options.sparse_linear_algebra_library_type = CX_SPARSE; + options.type = SPARSE_NORMAL_CHOLESKY; + options.dynamic_sparsity = true; + TestSolver(options); +} #endif } // namespace internal