diff options
author | Sergey Sharybin <sergey.vfx@gmail.com> | 2012-06-10 19:27:41 +0400 |
---|---|---|
committer | Sergey Sharybin <sergey.vfx@gmail.com> | 2012-06-10 19:27:41 +0400 |
commit | 59ef51aa2753895442aaf2d8e4ef8ff923be3973 (patch) | |
tree | 8a6e6472f44bfe937154d6536669117bd57c6359 /extern/libmv/third_party/ceres/internal | |
parent | 39e591d3d04015b84fe5f12e7ef5af6507df92af (diff) |
Initial Ceres integration into Blender
Currently only put sources of Ceres library into extern/libmv/third_party and
setup CMake and SCons building systems.
Integration details:
- Even CMake build files are not re-used from Ceres's trunk: they're using some
automatic stuff detection like glog, pthreads, protobuf and so and it's not
so clear how to re-use that files without modifications.
And IMO it's easier if build files are getting re-generated automatically to
match Blender-specific setup rather than keeping changes made locally in
Blender in sync when re-bundling Ceres library. Especially in case when it's
already needed to support SCons build system.
- Integrated only actual sources, all tests were stripped. Probably it'll be nice
to have them, but they'll need clear integration with current module test stuff
in Blender.
- Suitesparse was disabled. It'll help a lot having it, but there are some difficulties
making cholmod working fine on windows. Would be added in future
- collections_port.cc was also stripped. It's not used by Ceres's upstream and
it gives compilation error (undefined uint32 -- looks like namespace issue).
- Currently all schur eliminators are included. Not sure if it makes sense,
also not sure if it makes sense having them switchable on and off -- IMO better
to have single configuration which works and does not require special tweaks
after everything was set up.
To bundle updated version of Ceres:
- Go to extern/libmv/third_party/ceres folder
- Run ./bundle.sh
This will checkout fresh Ceres snapshot of Windows branch (which is currently
most interesting from integration into Blender POV), apply all patches listed
in patches/series and copy needed files into Blender's working copy. This will
also re-generate CMake/SCons build rules.
If you'll need extra files from Ceres repository which are not present in
Blender, you'll need to copy them manually and then run ./mkfiles.sh from
extern/libmv/third_party/ceres folder which will update list of files used
by Blender.
Thanks to Leir Mierle and Sameer Agarwal (and all others who helped developing
Ceres) this library and thanks to Keir Mierle with help integrating it into Blender!
Diffstat (limited to 'extern/libmv/third_party/ceres/internal')
128 files changed, 19300 insertions, 0 deletions
diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_evaluate_preparer.cc b/extern/libmv/third_party/ceres/internal/ceres/block_evaluate_preparer.cc new file mode 100644 index 00000000000..05e63eb560b --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/block_evaluate_preparer.cc @@ -0,0 +1,73 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 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: keir@google.com (Keir Mierle) + +#include "ceres/block_evaluate_preparer.h" + +#include <vector> +#include "ceres/block_sparse_matrix.h" +#include "ceres/casts.h" +#include "ceres/parameter_block.h" +#include "ceres/residual_block.h" +#include "ceres/sparse_matrix.h" + +namespace ceres { +namespace internal { + +void BlockEvaluatePreparer::Init(int** jacobian_layout) { + jacobian_layout_ = jacobian_layout; +} + +// Point the jacobian blocks directly into the block sparse matrix. +void BlockEvaluatePreparer::Prepare(const ResidualBlock* residual_block, + int residual_block_index, + SparseMatrix* jacobian, + double** jacobians) const { + CHECK(jacobian != NULL); + double* jacobian_values = + down_cast<BlockSparseMatrix*>(jacobian)->mutable_values(); + + const int* jacobian_block_offset = jacobian_layout_[residual_block_index]; + const int num_parameter_blocks = residual_block->NumParameterBlocks(); + for (int j = 0; j < num_parameter_blocks; ++j) { + if (!residual_block->parameter_blocks()[j]->IsConstant()) { + jacobians[j] = jacobian_values + *jacobian_block_offset; + + // The jacobian_block_offset can't be indexed with 'j' since the code + // that creates the layout strips out any blocks for inactive + // parameters. Instead, bump the pointer for active parameters only. + jacobian_block_offset++; + } else { + jacobians[j] = NULL; + } + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_evaluate_preparer.h b/extern/libmv/third_party/ceres/internal/ceres/block_evaluate_preparer.h new file mode 100644 index 00000000000..a7869311e6e --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/block_evaluate_preparer.h @@ -0,0 +1,67 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 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: keir@google.com (Keir Mierle) +// +// A evaluate preparer which puts jacobian the evaluated jacobian blocks +// directly into their final resting place in an overall block sparse matrix. +// The evaluator takes care to avoid evaluating the jacobian for fixed +// parameters. + +#ifndef CERES_INTERNAL_BLOCK_EVALUATE_PREPARER_H_ +#define CERES_INTERNAL_BLOCK_EVALUATE_PREPARER_H_ + +namespace ceres { +namespace internal { + +class ResidualBlock; +class SparseMatrix; + +class BlockEvaluatePreparer { + public: + // Using Init() instead of a constructor allows for allocating this structure + // with new[]. This is because C++ doesn't allow passing arguments to objects + // constructed with new[] (as opposed to plain 'new'). + void Init(int** jacobian_layout); + + // EvaluatePreparer interface + + // Point the jacobian blocks directly into the block sparse matrix. + void Prepare(const ResidualBlock* residual_block, + int residual_block_index, + SparseMatrix* jacobian, + double** jacobians) const; + + private: + int const* const* jacobian_layout_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_BLOCK_EVALUATE_PREPARER_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_jacobi_preconditioner.cc b/extern/libmv/third_party/ceres/internal/ceres/block_jacobi_preconditioner.cc new file mode 100644 index 00000000000..1a5001f9c71 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/block_jacobi_preconditioner.cc @@ -0,0 +1,136 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2012 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: keir@google.com (Keir Mierle) + +#include "ceres/block_jacobi_preconditioner.h" + +#include "Eigen/Cholesky" +#include "ceres/block_sparse_matrix.h" +#include "ceres/block_structure.h" +#include "ceres/casts.h" +#include "ceres/integral_types.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +BlockJacobiPreconditioner::BlockJacobiPreconditioner( + const LinearOperator& A) + : block_structure_( + *(down_cast<const BlockSparseMatrix*>(&A)->block_structure())), + num_rows_(A.num_rows()) { + // Calculate the amount of storage needed. + int storage_needed = 0; + for (int c = 0; c < block_structure_.cols.size(); ++c) { + int size = block_structure_.cols[c].size; + storage_needed += size * size; + } + + // Size the offsets and storage. + blocks_.resize(block_structure_.cols.size()); + block_storage_.resize(storage_needed); + + // Put pointers to the storage in the offsets. + double* block_cursor = &block_storage_[0]; + for (int c = 0; c < block_structure_.cols.size(); ++c) { + int size = block_structure_.cols[c].size; + blocks_[c] = block_cursor; + block_cursor += size * size; + } +} + +BlockJacobiPreconditioner::~BlockJacobiPreconditioner() { +} + +void BlockJacobiPreconditioner::Update(const LinearOperator& matrix, const double* D) { + const BlockSparseMatrix& A = *(down_cast<const BlockSparseMatrix*>(&matrix)); + const CompressedRowBlockStructure* bs = A.block_structure(); + + // Compute the diagonal blocks by block inner products. + std::fill(block_storage_.begin(), block_storage_.end(), 0.0); + for (int r = 0; r < bs->rows.size(); ++r) { + const int row_block_size = bs->rows[r].block.size; + const vector<Cell>& cells = bs->rows[r].cells; + const double* row_values = A.RowBlockValues(r); + for (int c = 0; c < cells.size(); ++c) { + const int col_block_size = bs->cols[cells[c].block_id].size; + ConstMatrixRef m(row_values + cells[c].position, + row_block_size, + col_block_size); + + MatrixRef(blocks_[cells[c].block_id], + col_block_size, + col_block_size).noalias() += m.transpose() * m; + + // TODO(keir): Figure out when the below expression is actually faster + // than doing the full rank update. The issue is that for smaller sizes, + // the rankUpdate() function is slower than the full product done above. + // + // On the typical bundling problems, the above product is ~5% faster. + // + // MatrixRef(blocks_[cells[c].block_id], + // col_block_size, + // col_block_size).selfadjointView<Eigen::Upper>().rankUpdate(m); + // + } + } + + // Add the diagonal and invert each block. + for (int c = 0; c < bs->cols.size(); ++c) { + const int size = block_structure_.cols[c].size; + const int position = block_structure_.cols[c].position; + MatrixRef block(blocks_[c], size, size); + + if (D != NULL) { + block.diagonal() += ConstVectorRef(D + position, size).array().square().matrix(); + } + + block = block.selfadjointView<Eigen::Upper>() + .ldlt() + .solve(Matrix::Identity(size, size)); + } +} + +void BlockJacobiPreconditioner::RightMultiply(const double* x, double* y) const { + for (int c = 0; c < block_structure_.cols.size(); ++c) { + const int size = block_structure_.cols[c].size; + const int position = block_structure_.cols[c].position; + ConstMatrixRef D(blocks_[c], size, size); + ConstVectorRef x_block(x + position, size); + VectorRef y_block(y + position, size); + y_block += D * x_block; + } +} + +void BlockJacobiPreconditioner::LeftMultiply(const double* x, double* y) const { + RightMultiply(x, y); +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_jacobi_preconditioner.h b/extern/libmv/third_party/ceres/internal/ceres/block_jacobi_preconditioner.h new file mode 100644 index 00000000000..91cfeddb688 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/block_jacobi_preconditioner.h @@ -0,0 +1,84 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2012 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: keir@google.com (Keir Mierle) + +#ifndef CERES_INTERNAL_BLOCK_JACOBI_PRECONDITIONER_H_ +#define CERES_INTERNAL_BLOCK_JACOBI_PRECONDITIONER_H_ + +#include <vector> +#include "ceres/linear_operator.h" + +namespace ceres { +namespace internal { + +class CompressedRowBlockStructure; +class LinearOperator; +class SparseMatrix; + +// A block Jacobi preconditioner. This is intended for use with conjugate +// gradients, or other iterative symmetric solvers. To use the preconditioner, +// create one by passing a BlockSparseMatrix as the linear operator "A" to the +// constructor. This fixes the sparsity pattern to the pattern of the matrix +// A^TA. +// +// Before each use of the preconditioner in a solve with conjugate gradients, +// update the matrix by running Update(A, D). The values of the matrix A are +// inspected to construct the preconditioner. The vector D is applied as the +// D^TD diagonal term. +class BlockJacobiPreconditioner : public LinearOperator { + public: + // A must remain valid while the BlockJacobiPreconditioner is. + BlockJacobiPreconditioner(const LinearOperator& A); + virtual ~BlockJacobiPreconditioner(); + + // Update the preconditioner with the values found in A. The sparsity pattern + // must match that of the A passed to the constructor. D is a vector that + // must have the same number of rows as A, and is applied as a diagonal in + // addition to the block diagonals of A. + void Update(const LinearOperator& A, const double* D); + + // LinearOperator interface. + virtual void RightMultiply(const double* x, double* y) const; + virtual void LeftMultiply(const double* x, double* y) const; + virtual int num_rows() const { return num_rows_; } + virtual int num_cols() const { return num_rows_; } + + private: + std::vector<double*> blocks_; + std::vector<double> block_storage_; + int num_rows_; + + // The block structure of the matrix this preconditioner is for (e.g. J). + const CompressedRowBlockStructure& block_structure_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_BLOCK_JACOBI_PRECONDITIONER_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_jacobian_writer.cc b/extern/libmv/third_party/ceres/internal/ceres/block_jacobian_writer.cc new file mode 100644 index 00000000000..52a58bb43a6 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/block_jacobian_writer.cc @@ -0,0 +1,209 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 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: keir@google.com (Keir Mierle) + +#include "ceres/block_jacobian_writer.h" + +#include "ceres/block_evaluate_preparer.h" +#include "ceres/block_sparse_matrix.h" +#include "ceres/parameter_block.h" +#include "ceres/program.h" +#include "ceres/residual_block.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/port.h" +#include "ceres/internal/scoped_ptr.h" + +namespace ceres { +namespace internal { +namespace { + +// Given the residual block ordering, build a lookup table to determine which +// per-parameter jacobian goes where in the overall program jacobian. +// +// Since we expect to use a Schur type linear solver to solve the LM step, take +// extra care to place the E blocks and the F blocks contiguously. E blocks are +// the first num_eliminate_blocks parameter blocks as indicated by the parameter +// block ordering. The remaining parameter blocks are the F blocks. +// +// TODO(keir): Consider if we should use a boolean for each parameter block +// instead of num_eliminate_blocks. +void BuildJacobianLayout(const Program& program, + int num_eliminate_blocks, + vector<int*>* jacobian_layout, + vector<int>* jacobian_layout_storage) { + const vector<ResidualBlock*>& residual_blocks = program.residual_blocks(); + + // Iterate over all the active residual blocks and determine how many E blocks + // are there. This will determine where the F blocks start in the jacobian + // matrix. Also compute the number of jacobian blocks. + int f_block_pos = 0; + int num_jacobian_blocks = 0; + for (int i = 0; i < residual_blocks.size(); ++i) { + ResidualBlock* residual_block = residual_blocks[i]; + const int num_residuals = residual_block->NumResiduals(); + const int num_parameter_blocks = residual_block->NumParameterBlocks(); + + // Advance f_block_pos over each E block for this residual. + for (int j = 0; j < num_parameter_blocks; ++j) { + ParameterBlock* parameter_block = residual_block->parameter_blocks()[j]; + if (!parameter_block->IsConstant()) { + // Only count blocks for active parameters. + num_jacobian_blocks++; + if (parameter_block->index() < num_eliminate_blocks) { + f_block_pos += num_residuals * parameter_block->LocalSize(); + } + } + } + } + + // We now know that the E blocks are laid out starting at zero, and the F + // blocks are laid out starting at f_block_pos. Iterate over the residual + // blocks again, and this time fill the jacobian_layout array with the + // position information. + + jacobian_layout->resize(program.NumResidualBlocks()); + jacobian_layout_storage->resize(num_jacobian_blocks); + + int e_block_pos = 0; + int* jacobian_pos = &(*jacobian_layout_storage)[0]; + for (int i = 0; i < residual_blocks.size(); ++i) { + const ResidualBlock* residual_block = residual_blocks[i]; + const int num_residuals = residual_block->NumResiduals(); + const int num_parameter_blocks = residual_block->NumParameterBlocks(); + + (*jacobian_layout)[i] = jacobian_pos; + for (int j = 0; j < num_parameter_blocks; ++j) { + ParameterBlock* parameter_block = residual_block->parameter_blocks()[j]; + const int parameter_block_index = parameter_block->index(); + if (parameter_block->IsConstant()) { + continue; + } + const int jacobian_block_size = + num_residuals * parameter_block->LocalSize(); + if (parameter_block_index < num_eliminate_blocks) { + *jacobian_pos = e_block_pos; + e_block_pos += jacobian_block_size; + } else { + *jacobian_pos = f_block_pos; + f_block_pos += jacobian_block_size; + } + jacobian_pos++; + } + } +} + +} // namespace + +BlockJacobianWriter::BlockJacobianWriter(const Evaluator::Options& options, + Program* program) + : program_(program) { + CHECK_GE(options.num_eliminate_blocks, 0) + << "num_eliminate_blocks must be greater than 0."; + + BuildJacobianLayout(*program, + options.num_eliminate_blocks, + &jacobian_layout_, + &jacobian_layout_storage_); +} + +// Create evaluate prepareres that point directly into the final jacobian. This +// makes the final Write() a nop. +BlockEvaluatePreparer* BlockJacobianWriter::CreateEvaluatePreparers( + int num_threads) { + BlockEvaluatePreparer* preparers = new BlockEvaluatePreparer[num_threads]; + for (int i = 0; i < num_threads; i++) { + preparers[i].Init(&jacobian_layout_[0]); + } + return preparers; +} + +SparseMatrix* BlockJacobianWriter::CreateJacobian() const { + CompressedRowBlockStructure* bs = new CompressedRowBlockStructure; + + const vector<ParameterBlock*>& parameter_blocks = + program_->parameter_blocks(); + + // Construct the column blocks. + bs->cols.resize(parameter_blocks.size()); + for (int i = 0, cursor = 0; i < parameter_blocks.size(); ++i) { + CHECK_NE(parameter_blocks[i]->index(), -1); + CHECK(!parameter_blocks[i]->IsConstant()); + bs->cols[i].size = parameter_blocks[i]->LocalSize(); + bs->cols[i].position = cursor; + cursor += bs->cols[i].size; + } + + // Construct the cells in each row. + const vector<ResidualBlock*>& residual_blocks = + program_->residual_blocks(); + int row_block_position = 0; + bs->rows.resize(residual_blocks.size()); + for (int i = 0; i < residual_blocks.size(); ++i) { + const ResidualBlock* residual_block = residual_blocks[i]; + CompressedRow* row = &bs->rows[i]; + + row->block.size = residual_block->NumResiduals(); + row->block.position = row_block_position; + row_block_position += row->block.size; + + // Size the row by the number of active parameters in this residual. + const int num_parameter_blocks = residual_block->NumParameterBlocks(); + int num_active_parameter_blocks = 0; + for (int j = 0; j < num_parameter_blocks; ++j) { + if (residual_block->parameter_blocks()[j]->index() != -1) { + num_active_parameter_blocks++; + } + } + row->cells.resize(num_active_parameter_blocks); + + // Add layout information for the active parameters in this row. + for (int j = 0, k = 0; j < num_parameter_blocks; ++j) { + const ParameterBlock* parameter_block = + residual_block->parameter_blocks()[j]; + if (!parameter_block->IsConstant()) { + Cell& cell = row->cells[k]; + cell.block_id = parameter_block->index(); + cell.position = jacobian_layout_[i][k]; + + // Only increment k for active parameters, since there is only layout + // information for active parameters. + k++; + } + } + + sort(row->cells.begin(), row->cells.end(), CellLessThan); + } + + BlockSparseMatrix* jacobian = new BlockSparseMatrix(bs); + CHECK_NOTNULL(jacobian); + return jacobian; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_jacobian_writer.h b/extern/libmv/third_party/ceres/internal/ceres/block_jacobian_writer.h new file mode 100644 index 00000000000..140c7211129 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/block_jacobian_writer.h @@ -0,0 +1,127 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 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: keir@google.com (Keir Mierle) +// +// A jacobian writer that writes to block sparse matrices. The "writer" name is +// misleading, since the Write() operation on the block jacobian writer does not +// write anything. Instead, the Prepare() method on the BlockEvaluatePreparers +// makes a jacobians array which has direct pointers into the block sparse +// jacobian. When the cost function is evaluated, the jacobian blocks get placed +// directly in their final location. + +#ifndef CERES_INTERNAL_BLOCK_JACOBIAN_WRITER_H_ +#define CERES_INTERNAL_BLOCK_JACOBIAN_WRITER_H_ + +#include <vector> +#include "ceres/evaluator.h" +#include "ceres/internal/port.h" + +namespace ceres { +namespace internal { + +class BlockEvaluatePreparer; +class Program; +class SparseMatrix; + +class BlockJacobianWriter { + public: + BlockJacobianWriter(const Evaluator::Options& options, + Program* program); + + // JacobianWriter interface. + + // Create evaluate prepareres that point directly into the final jacobian. + // This makes the final Write() a nop. + BlockEvaluatePreparer* CreateEvaluatePreparers(int num_threads); + + SparseMatrix* CreateJacobian() const; + + void Write(int /* residual_id */, + int /* residual_offset */, + double** /* jacobians */, + SparseMatrix* /* jacobian */) { + // This is a noop since the blocks were written directly into their final + // position by the outside evaluate call, thanks to the jacobians array + // prepared by the BlockEvaluatePreparers. + } + + private: + Program* program_; + + // Stores the position of each residual / parameter jacobian. + // + // The block sparse matrix that this writer writes to is stored as a set of + // contiguos dense blocks, one after each other; see BlockSparseMatrix. The + // "double* values_" member of the block sparse matrix contains all of these + // blocks. Given a pointer to the first element of a block and the size of + // that block, it's possible to write to it. + // + // In the case of a block sparse jacobian, the jacobian writer needs a way to + // find the offset in the values_ array of each residual/parameter jacobian + // block. + // + // That is the purpose of jacobian_layout_. + // + // In particular, jacobian_layout_[i][j] is the offset in the values_ array of + // the derivative of residual block i with respect to the parameter block at + // active argument position j. + // + // The active qualifier means that non-active parameters do not count. Care + // must be taken when indexing into jacobian_layout_ to account for this. + // Consider a single residual example: + // + // r(x, y, z) + // + // with r in R^3, x in R^4, y in R^2, and z in R^5. + // Take y as a constant (non-active) parameter. + // Take r as residual number 0. + // + // In this case, the active arguments are only (x, z), so the active argument + // position for x is 0, and the active argument position for z is 1. This is + // similar to thinking of r as taking only 2 parameters: + // + // r(x, z) + // + // There are only 2 jacobian blocks: dr/dx and dr/dz. jacobian_layout_ would + // have the following contents: + // + // jacobian_layout_[0] = { 0, 12 } + // + // which indicates that dr/dx is located at values_[0], and dr/dz is at + // values_[12]. See BlockEvaluatePreparer::Prepare()'s comments about 'j'. + vector<int*> jacobian_layout_; + + // The pointers in jacobian_layout_ point directly into this vector. + vector<int> jacobian_layout_storage_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_BLOCK_JACOBIAN_WRITER_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_random_access_dense_matrix.cc b/extern/libmv/third_party/ceres/internal/ceres/block_random_access_dense_matrix.cc new file mode 100644 index 00000000000..2afaf5e2ea2 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/block_random_access_dense_matrix.cc @@ -0,0 +1,83 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/block_random_access_dense_matrix.h" + +#include <vector> +#include <glog/logging.h> +#include "ceres/internal/eigen.h" +#include "ceres/internal/scoped_ptr.h" + +namespace ceres { +namespace internal { + +BlockRandomAccessDenseMatrix::BlockRandomAccessDenseMatrix( + const vector<int>& blocks) { + block_layout_.resize(blocks.size(), 0); + num_rows_ = 0; + for (int i = 0; i < blocks.size(); ++i) { + block_layout_[i] = num_rows_; + num_rows_ += blocks[i]; + } + + values_.reset(new double[num_rows_ * num_rows_]); + CHECK_NOTNULL(values_.get()); + cell_info_.values = values_.get(); + SetZero(); +} + +// Assume that the user does not hold any locks on any cell blocks +// when they are calling SetZero. +BlockRandomAccessDenseMatrix::~BlockRandomAccessDenseMatrix() { +} + +CellInfo* BlockRandomAccessDenseMatrix::GetCell(const int row_block_id, + const int col_block_id, + int* row, + int* col, + int* row_stride, + int* col_stride) { + *row = block_layout_[row_block_id]; + *col = block_layout_[col_block_id]; + *row_stride = num_rows_; + *col_stride = num_rows_; + return &cell_info_; +} + +// Assume that the user does not hold any locks on any cell blocks +// when they are calling SetZero. +void BlockRandomAccessDenseMatrix::SetZero() { + if (num_rows_) { + VectorRef(values_.get(), num_rows_ * num_rows_).setZero(); + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_random_access_dense_matrix.h b/extern/libmv/third_party/ceres/internal/ceres/block_random_access_dense_matrix.h new file mode 100644 index 00000000000..3a0096209f7 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/block_random_access_dense_matrix.h @@ -0,0 +1,98 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_BLOCK_RANDOM_ACCESS_DENSE_MATRIX_H_ +#define CERES_INTERNAL_BLOCK_RANDOM_ACCESS_DENSE_MATRIX_H_ + +#include "ceres/block_random_access_matrix.h" + +#include <vector> + +#include "ceres/internal/macros.h" +#include "ceres/internal/port.h" +#include "ceres/internal/scoped_ptr.h" + +namespace ceres { +namespace internal { + +// A square block random accessible matrix with the same row and +// column block structure. All cells are stored in the same single +// array, so that its also accessible as a dense matrix of size +// num_rows x num_cols. +// +// This class is NOT thread safe. Since all n^2 cells are stored, +// GetCell never returns NULL for any (row_block_id, col_block_id) +// pair. +// +// ReturnCell is a nop. +class BlockRandomAccessDenseMatrix : public BlockRandomAccessMatrix { + public: + // blocks is a vector of block sizes. The resulting matrix has + // blocks.size() * blocks.size() cells. + explicit BlockRandomAccessDenseMatrix(const vector<int>& blocks); + + // The destructor is not thread safe. It assumes that no one is + // modifying any cells when the matrix is being destroyed. + virtual ~BlockRandomAccessDenseMatrix(); + + // BlockRandomAccessMatrix interface. + virtual CellInfo* GetCell(int row_block_id, + int col_block_id, + int* row, + int* col, + int* row_stride, + int* col_stride); + + // This is not a thread safe method, it assumes that no cell is + // locked. + virtual void SetZero(); + + // Since the matrix is square with the same row and column block + // structure, num_rows() = num_cols(). + virtual int num_rows() const { return num_rows_; } + virtual int num_cols() const { return num_rows_; } + + // The underlying matrix storing the cells. + const double* values() const { return values_.get(); } + double* mutable_values() { return values_.get(); } + + private: + CellInfo cell_info_; + int num_rows_; + vector<int> block_layout_; + scoped_array<double> values_; + + DISALLOW_COPY_AND_ASSIGN(BlockRandomAccessDenseMatrix); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_BLOCK_RANDOM_ACCESS_DENSE_MATRIX_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_random_access_matrix.cc b/extern/libmv/third_party/ceres/internal/ceres/block_random_access_matrix.cc new file mode 100644 index 00000000000..58fe4a10de3 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/block_random_access_matrix.cc @@ -0,0 +1,40 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/block_random_access_matrix.h" + +namespace ceres { +namespace internal { + +BlockRandomAccessMatrix::~BlockRandomAccessMatrix() { +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_random_access_matrix.h b/extern/libmv/third_party/ceres/internal/ceres/block_random_access_matrix.h new file mode 100644 index 00000000000..f398af3be87 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/block_random_access_matrix.h @@ -0,0 +1,132 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Interface for matrices that allow block based random access. + +#ifndef CERES_INTERNAL_BLOCK_RANDOM_ACCESS_MATRIX_H_ +#define CERES_INTERNAL_BLOCK_RANDOM_ACCESS_MATRIX_H_ + +#include "ceres/mutex.h" + +namespace ceres { +namespace internal { + +// A matrix implementing the BlockRandomAccessMatrix interface is a +// matrix whose rows and columns are divided into blocks. For example +// the matrix A: +// +// 3 4 5 +// A = 5 [c_11 c_12 c_13] +// 4 [c_21 c_22 c_23] +// +// has row blocks of size 5 and 4, and column blocks of size 3, 4 and +// 5. It has six cells corresponding to the six row-column block +// combinations. +// +// BlockRandomAccessMatrix objects provide access to cells c_ij using +// the GetCell method. when a cell is present, GetCell will return a +// CellInfo object containing a pointer to an array which contains the +// cell as a submatrix and a mutex that guards this submatrix. If the +// user is accessing the matrix concurrently, it is his responsibility +// to use the mutex to exclude other writers from writing to the cell +// concurrently. +// +// There is no requirement that all cells be present, i.e. the matrix +// itself can be block sparse. When a cell is not present, the GetCell +// method will return a NULL pointer. +// +// There is no requirement about how the cells are stored beyond that +// form a dense submatrix of a larger dense matrix. Like everywhere +// else in Ceres, RowMajor storage assumed. +// +// Example usage: +// +// BlockRandomAccessMatrix* A = new BlockRandomAccessMatrixSubClass(...) +// +// int row, col, row_stride, col_stride; +// CellInfo* cell = A->GetCell(row_block_id, col_block_id, +// &row, &col, +// &row_stride, &col_stride); +// +// if (cell != NULL) { +// MatrixRef m(cell->values, row_stride, col_stride); +// MutexLock l(&cell->m); +// m.block(row, col, row_block_size, col_block_size) = ... +// } + +// Structure to carry a pointer to the array containing a cell and the +// Mutex guarding it. +struct CellInfo { + CellInfo() + : values(NULL) { + } + + explicit CellInfo(double* ptr) + : values(ptr) { + } + + double* values; + Mutex m; +}; + +class BlockRandomAccessMatrix { + public: + virtual ~BlockRandomAccessMatrix(); + + // If the cell (row_block_id, col_block_id) is present, then return + // a CellInfo with a pointer to the dense matrix containing it, + // otherwise return NULL. The dense matrix containing this cell has + // size row_stride, col_stride and the cell is located at position + // (row, col) within this matrix. + // + // The size of the cell is row_block_size x col_block_size is + // assumed known to the caller. row_block_size less than or equal to + // row_stride and col_block_size is upper bounded by col_stride. + virtual CellInfo* GetCell(int row_block_id, + int col_block_id, + int* row, + int* col, + int* row_stride, + int* col_stride) = 0; + + // Zero out the values of the array. The structure of the matrix + // (size and sparsity) is preserved. + virtual void SetZero() = 0; + + // Number of scalar rows and columns in the matrix, i.e the sum of + // all row blocks and column block sizes respectively. + virtual int num_rows() const = 0; + virtual int num_cols() const = 0; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_BLOCK_RANDOM_ACCESS_MATRIX_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_random_access_sparse_matrix.cc b/extern/libmv/third_party/ceres/internal/ceres/block_random_access_sparse_matrix.cc new file mode 100644 index 00000000000..c496fcd13de --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/block_random_access_sparse_matrix.cc @@ -0,0 +1,158 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/block_random_access_sparse_matrix.h" + +#include <algorithm> +#include <set> +#include <utility> +#include <vector> +#include <glog/logging.h> +#include "ceres/mutex.h" +#include "ceres/triplet_sparse_matrix.h" +#include "ceres/internal/port.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +BlockRandomAccessSparseMatrix::BlockRandomAccessSparseMatrix( + const vector<int>& blocks, + const set<pair<int, int> >& block_pairs) + : kMaxRowBlocks(10 * 1000 * 1000), + blocks_(blocks) { + CHECK_LT(blocks.size(), kMaxRowBlocks); + + // Build the row/column layout vector and count the number of scalar + // rows/columns. + int num_cols = 0; + vector<int> col_layout; + for (int i = 0; i < blocks_.size(); ++i) { + col_layout.push_back(num_cols); + num_cols += blocks_[i]; + } + + // Count the number of scalar non-zero entries and build the layout + // object for looking into the values array of the + // TripletSparseMatrix. + int num_nonzeros = 0; + for (set<pair<int, int> >::const_iterator it = block_pairs.begin(); + it != block_pairs.end(); + ++it) { + const int row_block_size = blocks_[it->first]; + const int col_block_size = blocks_[it->second]; + num_nonzeros += row_block_size * col_block_size; + } + + VLOG(1) << "Matrix Size [" << num_cols + << "," << num_cols + << "] " << num_nonzeros; + + tsm_.reset(new TripletSparseMatrix(num_cols, num_cols, num_nonzeros)); + tsm_->set_num_nonzeros(num_nonzeros); + int* rows = tsm_->mutable_rows(); + int* cols = tsm_->mutable_cols(); + double* values = tsm_->mutable_values(); + + int pos = 0; + for (set<pair<int, int> >::const_iterator it = block_pairs.begin(); + it != block_pairs.end(); + ++it) { + const int row_block_size = blocks_[it->first]; + const int col_block_size = blocks_[it->second]; + layout_[IntPairToLong(it->first, it->second)] = + new CellInfo(values + pos); + pos += row_block_size * col_block_size; + } + + // Fill the sparsity pattern of the underlying matrix. + for (set<pair<int, int> >::const_iterator it = block_pairs.begin(); + it != block_pairs.end(); + ++it) { + const int row_block_id = it->first; + const int col_block_id = it->second; + const int row_block_size = blocks_[row_block_id]; + const int col_block_size = blocks_[col_block_id]; + int pos = + layout_[IntPairToLong(row_block_id, col_block_id)]->values - values; + for (int r = 0; r < row_block_size; ++r) { + for (int c = 0; c < col_block_size; ++c, ++pos) { + rows[pos] = col_layout[row_block_id] + r; + cols[pos] = col_layout[col_block_id] + c; + values[pos] = 1.0; + DCHECK_LT(rows[pos], tsm_->num_rows()); + DCHECK_LT(cols[pos], tsm_->num_rows()); + } + } + } +} + +// Assume that the user does not hold any locks on any cell blocks +// when they are calling SetZero. +BlockRandomAccessSparseMatrix::~BlockRandomAccessSparseMatrix() { + for (LayoutType::iterator it = layout_.begin(); + it != layout_.end(); + ++it) { + delete it->second; + } +} + +CellInfo* BlockRandomAccessSparseMatrix::GetCell(int row_block_id, + int col_block_id, + int* row, + int* col, + int* row_stride, + int* col_stride) { + const LayoutType::iterator it = + layout_.find(IntPairToLong(row_block_id, col_block_id)); + if (it == layout_.end()) { + return NULL; + } + + // Each cell is stored contiguously as its own little dense matrix. + *row = 0; + *col = 0; + *row_stride = blocks_[row_block_id]; + *col_stride = blocks_[col_block_id]; + return it->second; +} + +// Assume that the user does not hold any locks on any cell blocks +// when they are calling SetZero. +void BlockRandomAccessSparseMatrix::SetZero() { + if (tsm_->num_nonzeros()) { + VectorRef(tsm_->mutable_values(), + tsm_->num_nonzeros()).setZero(); + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_random_access_sparse_matrix.h b/extern/libmv/third_party/ceres/internal/ceres/block_random_access_sparse_matrix.h new file mode 100644 index 00000000000..12613c3daa0 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/block_random_access_sparse_matrix.h @@ -0,0 +1,109 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_BLOCK_RANDOM_ACCESS_SPARSE_MATRIX_H_ +#define CERES_INTERNAL_BLOCK_RANDOM_ACCESS_SPARSE_MATRIX_H_ + +#include <set> +#include <vector> +#include <utility> +#include "ceres/mutex.h" +#include "ceres/block_random_access_matrix.h" +#include "ceres/collections_port.h" +#include "ceres/triplet_sparse_matrix.h" +#include "ceres/internal/macros.h" +#include "ceres/internal/port.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +// A threaf safe square block sparse implementation of +// BlockRandomAccessMatrix. Internally a TripletSparseMatrix is used +// for doing the actual storage. This class augments this matrix with +// an unordered_map that allows random read/write access. +class BlockRandomAccessSparseMatrix : public BlockRandomAccessMatrix { + public: + // blocks is an array of block sizes. block_pairs is a set of + // <row_block_id, col_block_id> pairs to identify the non-zero cells + // of this matrix. + BlockRandomAccessSparseMatrix(const vector<int>& blocks, + const set<pair<int, int> >& block_pairs); + + // The destructor is not thread safe. It assumes that no one is + // modifying any cells when the matrix is being destroyed. + virtual ~BlockRandomAccessSparseMatrix(); + + // BlockRandomAccessMatrix Interface. + virtual CellInfo* GetCell(int row_block_id, + int col_block_id, + int* row, + int* col, + int* row_stride, + int* col_stride); + + // This is not a thread safe method, it assumes that no cell is + // locked. + virtual void SetZero(); + virtual bool IsThreadSafe() const { return true; } + + // Since the matrix is square, num_rows() == num_cols(). + virtual int num_rows() const { return tsm_->num_rows(); } + virtual int num_cols() const { return tsm_->num_cols(); } + + // Access to the underlying matrix object. + const TripletSparseMatrix* matrix() const { return tsm_.get(); } + TripletSparseMatrix* mutable_matrix() { return tsm_.get(); } + + private: + long int IntPairToLong(int a, int b) { + return a * kMaxRowBlocks + b; + } + + const int kMaxRowBlocks; + // row/column block sizes. + const vector<int> blocks_; + + // A mapping from <row_block_id, col_block_id> to the position in + // the values array of tsm_ where the block is stored. + typedef HashMap<long int, CellInfo* > LayoutType; + LayoutType layout_; + + // The underlying matrix object which actually stores the cells. + scoped_ptr<TripletSparseMatrix> tsm_; + + DISALLOW_COPY_AND_ASSIGN(BlockRandomAccessSparseMatrix); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_BLOCK_RANDOM_ACCESS_SPARSE_MATRIX_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_sparse_matrix.cc b/extern/libmv/third_party/ceres/internal/ceres/block_sparse_matrix.cc new file mode 100644 index 00000000000..7dd395e2975 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/block_sparse_matrix.cc @@ -0,0 +1,286 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/block_sparse_matrix.h" + +#include <cstddef> +#include <algorithm> +#include <vector> +#include <glog/logging.h> +#include "ceres/block_structure.h" +#include "ceres/matrix_proto.h" +#include "ceres/triplet_sparse_matrix.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +BlockSparseMatrix::~BlockSparseMatrix() {} + +BlockSparseMatrix::BlockSparseMatrix( + CompressedRowBlockStructure* block_structure) + : num_rows_(0), + num_cols_(0), + num_nonzeros_(0), + values_(NULL), + block_structure_(block_structure) { + CHECK_NOTNULL(block_structure_.get()); + + // Count the number of columns in the matrix. + for (int i = 0; i < block_structure_->cols.size(); ++i) { + num_cols_ += block_structure_->cols[i].size; + } + + // Count the number of non-zero entries and the number of rows in + // the matrix. + for (int i = 0; i < block_structure_->rows.size(); ++i) { + int row_block_size = block_structure_->rows[i].block.size; + num_rows_ += row_block_size; + + const vector<Cell>& cells = block_structure_->rows[i].cells; + for (int j = 0; j < cells.size(); ++j) { + int col_block_id = cells[j].block_id; + int col_block_size = block_structure_->cols[col_block_id].size; + num_nonzeros_ += col_block_size * row_block_size; + } + } + + CHECK_GE(num_rows_, 0); + CHECK_GE(num_cols_, 0); + CHECK_GE(num_nonzeros_, 0); + VLOG(2) << "Allocating values array with " + << num_nonzeros_ * sizeof(double) << " bytes."; // NOLINT + values_.reset(new double[num_nonzeros_]); + CHECK_NOTNULL(values_.get()); +} + +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS +BlockSparseMatrix::BlockSparseMatrix(const SparseMatrixProto& outer_proto) { + CHECK(outer_proto.has_block_matrix()); + + const BlockSparseMatrixProto& proto = outer_proto.block_matrix(); + CHECK(proto.has_num_rows()); + CHECK(proto.has_num_cols()); + CHECK_EQ(proto.num_nonzeros(), proto.values_size()); + + num_rows_ = proto.num_rows(); + num_cols_ = proto.num_cols(); + num_nonzeros_ = proto.num_nonzeros(); + + // Copy out the values into *this. + values_.reset(new double[num_nonzeros_]); + for (int i = 0; i < proto.num_nonzeros(); ++i) { + values_[i] = proto.values(i); + } + + // Create the block structure according to the proto. + block_structure_.reset(new CompressedRowBlockStructure); + ProtoToBlockStructure(proto.block_structure(), block_structure_.get()); +} +#endif + +void BlockSparseMatrix::SetZero() { + fill(values_.get(), values_.get() + num_nonzeros_, 0.0); +} + +void BlockSparseMatrix::RightMultiply(const double* x, double* y) const { + CHECK_NOTNULL(x); + CHECK_NOTNULL(y); + + for (int i = 0; i < block_structure_->rows.size(); ++i) { + int row_block_pos = block_structure_->rows[i].block.position; + int row_block_size = block_structure_->rows[i].block.size; + VectorRef yref(y + row_block_pos, row_block_size); + const vector<Cell>& cells = block_structure_->rows[i].cells; + for (int j = 0; j < cells.size(); ++j) { + int col_block_id = cells[j].block_id; + int col_block_size = block_structure_->cols[col_block_id].size; + int col_block_pos = block_structure_->cols[col_block_id].position; + ConstVectorRef xref(x + col_block_pos, col_block_size); + MatrixRef m(values_.get() + cells[j].position, + row_block_size, col_block_size); + yref += m.lazyProduct(xref); + } + } +} + +void BlockSparseMatrix::LeftMultiply(const double* x, double* y) const { + CHECK_NOTNULL(x); + CHECK_NOTNULL(y); + + for (int i = 0; i < block_structure_->rows.size(); ++i) { + int row_block_pos = block_structure_->rows[i].block.position; + int row_block_size = block_structure_->rows[i].block.size; + const ConstVectorRef xref(x + row_block_pos, row_block_size); + const vector<Cell>& cells = block_structure_->rows[i].cells; + for (int j = 0; j < cells.size(); ++j) { + int col_block_id = cells[j].block_id; + int col_block_size = block_structure_->cols[col_block_id].size; + int col_block_pos = block_structure_->cols[col_block_id].position; + VectorRef yref(y + col_block_pos, col_block_size); + MatrixRef m(values_.get() + cells[j].position, + row_block_size, col_block_size); + yref += m.transpose().lazyProduct(xref); + } + } +} + +void BlockSparseMatrix::SquaredColumnNorm(double* x) const { + CHECK_NOTNULL(x); + VectorRef(x, num_cols_).setZero(); + for (int i = 0; i < block_structure_->rows.size(); ++i) { + int row_block_size = block_structure_->rows[i].block.size; + const vector<Cell>& cells = block_structure_->rows[i].cells; + for (int j = 0; j < cells.size(); ++j) { + int col_block_id = cells[j].block_id; + int col_block_size = block_structure_->cols[col_block_id].size; + int col_block_pos = block_structure_->cols[col_block_id].position; + const MatrixRef m(values_.get() + cells[j].position, + row_block_size, col_block_size); + VectorRef(x + col_block_pos, col_block_size) += m.colwise().squaredNorm(); + } + } +} + +void BlockSparseMatrix::ScaleColumns(const double* scale) { + CHECK_NOTNULL(scale); + + for (int i = 0; i < block_structure_->rows.size(); ++i) { + int row_block_size = block_structure_->rows[i].block.size; + const vector<Cell>& cells = block_structure_->rows[i].cells; + for (int j = 0; j < cells.size(); ++j) { + int col_block_id = cells[j].block_id; + int col_block_size = block_structure_->cols[col_block_id].size; + int col_block_pos = block_structure_->cols[col_block_id].position; + MatrixRef m(values_.get() + cells[j].position, + row_block_size, col_block_size); + m *= ConstVectorRef(scale + col_block_pos, col_block_size).asDiagonal(); + } + } +} + +void BlockSparseMatrix::ToDenseMatrix(Matrix* dense_matrix) const { + CHECK_NOTNULL(dense_matrix); + + dense_matrix->resize(num_rows_, num_cols_); + dense_matrix->setZero(); + Matrix& m = *dense_matrix; + + for (int i = 0; i < block_structure_->rows.size(); ++i) { + int row_block_pos = block_structure_->rows[i].block.position; + int row_block_size = block_structure_->rows[i].block.size; + const vector<Cell>& cells = block_structure_->rows[i].cells; + for (int j = 0; j < cells.size(); ++j) { + int col_block_id = cells[j].block_id; + int col_block_size = block_structure_->cols[col_block_id].size; + int col_block_pos = block_structure_->cols[col_block_id].position; + int jac_pos = cells[j].position; + m.block(row_block_pos, col_block_pos, row_block_size, col_block_size) + += MatrixRef(values_.get() + jac_pos, row_block_size, col_block_size); + } + } +} + +void BlockSparseMatrix::ToTripletSparseMatrix( + TripletSparseMatrix* matrix) const { + CHECK_NOTNULL(matrix); + + matrix->Reserve(num_nonzeros_); + matrix->Resize(num_rows_, num_cols_); + matrix->SetZero(); + + for (int i = 0; i < block_structure_->rows.size(); ++i) { + int row_block_pos = block_structure_->rows[i].block.position; + int row_block_size = block_structure_->rows[i].block.size; + const vector<Cell>& cells = block_structure_->rows[i].cells; + for (int j = 0; j < cells.size(); ++j) { + int col_block_id = cells[j].block_id; + int col_block_size = block_structure_->cols[col_block_id].size; + int col_block_pos = block_structure_->cols[col_block_id].position; + int jac_pos = cells[j].position; + for (int r = 0; r < row_block_size; ++r) { + for (int c = 0; c < col_block_size; ++c, ++jac_pos) { + matrix->mutable_rows()[jac_pos] = row_block_pos + r; + matrix->mutable_cols()[jac_pos] = col_block_pos + c; + matrix->mutable_values()[jac_pos] = values_[jac_pos]; + } + } + } + } + matrix->set_num_nonzeros(num_nonzeros_); +} + +// Return a pointer to the block structure. We continue to hold +// ownership of the object though. +const CompressedRowBlockStructure* BlockSparseMatrix::block_structure() + const { + return block_structure_.get(); +} + +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS +void BlockSparseMatrix::ToProto(SparseMatrixProto* outer_proto) const { + outer_proto->Clear(); + + BlockSparseMatrixProto* proto = outer_proto->mutable_block_matrix(); + proto->set_num_rows(num_rows_); + proto->set_num_cols(num_cols_); + proto->set_num_nonzeros(num_nonzeros_); + for (int i = 0; i < num_nonzeros_; ++i) { + proto->add_values(values_[i]); + } + BlockStructureToProto(*block_structure_, proto->mutable_block_structure()); +} +#endif + +void BlockSparseMatrix::ToTextFile(FILE* file) const { + CHECK_NOTNULL(file); + for (int i = 0; i < block_structure_->rows.size(); ++i) { + const int row_block_pos = block_structure_->rows[i].block.position; + const int row_block_size = block_structure_->rows[i].block.size; + const vector<Cell>& cells = block_structure_->rows[i].cells; + for (int j = 0; j < cells.size(); ++j) { + const int col_block_id = cells[j].block_id; + const int col_block_size = block_structure_->cols[col_block_id].size; + const int col_block_pos = block_structure_->cols[col_block_id].position; + int jac_pos = cells[j].position; + for (int r = 0; r < row_block_size; ++r) { + for (int c = 0; c < col_block_size; ++c) { + fprintf(file, "% 10d % 10d %17f\n", + row_block_pos + r, + col_block_pos + c, + values_[jac_pos++]); + } + } + } + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_sparse_matrix.h b/extern/libmv/third_party/ceres/internal/ceres/block_sparse_matrix.h new file mode 100644 index 00000000000..f71446e8f58 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/block_sparse_matrix.h @@ -0,0 +1,144 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Implementation of the SparseMatrix interface for block sparse +// matrices. + +#ifndef CERES_INTERNAL_BLOCK_SPARSE_MATRIX_H_ +#define CERES_INTERNAL_BLOCK_SPARSE_MATRIX_H_ + +#include "ceres/block_structure.h" +#include "ceres/sparse_matrix.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/macros.h" +#include "ceres/internal/scoped_ptr.h" + +namespace ceres { +namespace internal { + +class SparseMatrixProto; +class TripletSparseMatrix; + +// A further extension of the SparseMatrix interface to support block-oriented +// matrices. The key addition is the RowBlockValues() accessor, which enables +// the lazy block sparse matrix implementation. +class BlockSparseMatrixBase : public SparseMatrix { + public: + BlockSparseMatrixBase() {} + virtual ~BlockSparseMatrixBase() {} + + // Convert this matrix into a triplet sparse matrix. + virtual void ToTripletSparseMatrix(TripletSparseMatrix* matrix) const = 0; + + // Returns a pointer to the block structure. Does not transfer + // ownership. + virtual const CompressedRowBlockStructure* block_structure() const = 0; + + // Returns a pointer to a row of the matrix. The returned array is only valid + // until the next call to RowBlockValues. The caller does not own the result. + // + // The returned array is laid out such that cells on the specified row are + // contiguous in the returned array, though neighbouring cells in row order + // may not be contiguous in the row values. The cell values for cell + // (row_block, cell_block) are found at offset + // + // block_structure()->rows[row_block].cells[cell_block].position + // + virtual const double* RowBlockValues(int row_block_index) const = 0; + + private: + DISALLOW_COPY_AND_ASSIGN(BlockSparseMatrixBase); +}; + +// This class implements the SparseMatrix interface for storing and +// manipulating block sparse matrices. The block structure is stored +// in the CompressedRowBlockStructure object and one is needed to +// initialize the matrix. For details on how the blocks structure of +// the matrix is stored please see the documentation +// +// internal/ceres/block_structure.h +// +class BlockSparseMatrix : public BlockSparseMatrixBase { + public: + // Construct a block sparse matrix with a fully initialized + // CompressedRowBlockStructure objected. The matrix takes over + // ownership of this object and destroys it upon destruction. + // + // TODO(sameeragarwal): Add a function which will validate legal + // CompressedRowBlockStructure objects. + explicit BlockSparseMatrix(CompressedRowBlockStructure* block_structure); + + // Construct a block sparse matrix from a protocol buffer. +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS + explicit BlockSparseMatrix(const SparseMatrixProto& proto); +#endif + + BlockSparseMatrix(); + virtual ~BlockSparseMatrix(); + + // Implementation of SparseMatrix interface. + virtual void SetZero(); + virtual void RightMultiply(const double* x, double* y) const; + virtual void LeftMultiply(const double* x, double* y) const; + virtual void SquaredColumnNorm(double* x) const; + virtual void ScaleColumns(const double* scale); + virtual void ToDenseMatrix(Matrix* dense_matrix) const; +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS + virtual void ToProto(SparseMatrixProto* proto) const; +#endif + virtual void ToTextFile(FILE* file) const; + + virtual int num_rows() const { return num_rows_; } + virtual int num_cols() const { return num_cols_; } + virtual int num_nonzeros() const { return num_nonzeros_; } + virtual const double* values() const { return values_.get(); } + virtual double* mutable_values() { return values_.get(); } + + // Implementation of BlockSparseMatrixBase interface. + virtual void ToTripletSparseMatrix(TripletSparseMatrix* matrix) const; + virtual const CompressedRowBlockStructure* block_structure() const; + virtual const double* RowBlockValues(int row_block_index) const { + return values_.get(); + } + + private: + int num_rows_; + int num_cols_; + int max_num_nonzeros_; + int num_nonzeros_; + scoped_array<double> values_; + scoped_ptr<CompressedRowBlockStructure> block_structure_; + DISALLOW_COPY_AND_ASSIGN(BlockSparseMatrix); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_BLOCK_SPARSE_MATRIX_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_structure.cc b/extern/libmv/third_party/ceres/internal/ceres/block_structure.cc new file mode 100644 index 00000000000..5add4f3b94d --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/block_structure.cc @@ -0,0 +1,92 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/block_structure.h" +#include "ceres/matrix_proto.h" + +namespace ceres { +namespace internal { + +bool CellLessThan(const Cell& lhs, const Cell& rhs) { + return (lhs.block_id < rhs.block_id); +} + +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS +void ProtoToBlockStructure(const BlockStructureProto &proto, + CompressedRowBlockStructure *block_structure) { + // Decode the column blocks. + block_structure->cols.resize(proto.cols_size()); + for (int i = 0; i < proto.cols_size(); ++i) { + block_structure->cols[i].size = proto.cols(i).size(); + block_structure->cols[i].position = + proto.cols(i).position(); + } + // Decode the row structure. + block_structure->rows.resize(proto.rows_size()); + for (int i = 0; i < proto.rows_size(); ++i) { + const CompressedRowProto &row = proto.rows(i); + block_structure->rows[i].block.size = row.block().size(); + block_structure->rows[i].block.position = row.block().position(); + + // Copy the cells within the row. + block_structure->rows[i].cells.resize(row.cells_size()); + for (int j = 0; j < row.cells_size(); ++j) { + const CellProto &cell = row.cells(j); + block_structure->rows[i].cells[j].block_id = cell.block_id(); + block_structure->rows[i].cells[j].position = cell.position(); + } + } +} + +void BlockStructureToProto(const CompressedRowBlockStructure &block_structure, + BlockStructureProto *proto) { + // Encode the column blocks. + for (int i = 0; i < block_structure.cols.size(); ++i) { + BlockProto *block = proto->add_cols(); + block->set_size(block_structure.cols[i].size); + block->set_position(block_structure.cols[i].position); + } + // Encode the row structure. + for (int i = 0; i < block_structure.rows.size(); ++i) { + CompressedRowProto *row = proto->add_rows(); + BlockProto *block = row->mutable_block(); + block->set_size(block_structure.rows[i].block.size); + block->set_position(block_structure.rows[i].block.position); + for (int j = 0; j < block_structure.rows[i].cells.size(); ++j) { + CellProto *cell = row->add_cells(); + cell->set_block_id(block_structure.rows[i].cells[j].block_id); + cell->set_position(block_structure.rows[i].cells[j].position); + } + } +} +#endif + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_structure.h b/extern/libmv/third_party/ceres/internal/ceres/block_structure.h new file mode 100644 index 00000000000..f509067d216 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/block_structure.h @@ -0,0 +1,105 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Block structure objects are used to carry information about the +// dense block structure of sparse matrices. The BlockSparseMatrix +// object uses the BlockStructure objects to keep track of the matrix +// structure and operate upon it. This allows us to use more cache +// friendly block oriented linear algebra operations on the matrix +// instead of accessing it one scalar entry at a time. + +#ifndef CERES_INTERNAL_BLOCK_STRUCTURE_H_ +#define CERES_INTERNAL_BLOCK_STRUCTURE_H_ + +#include <vector> +#include "ceres/internal/port.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +class BlockStructureProto; + +typedef int16 BlockSize; + +struct Block { + Block() : size(-1), position(-1) {} + Block(int size_, int position_) : size(size_), position(position_) {} + + BlockSize size; + int position; // Position along the row/column. +}; + +struct Cell { + Cell() : block_id(-1), position(-1) {} + Cell(int block_id_, int position_) + : block_id(block_id_), position(position_) {} + + // Column or row block id as the case maybe. + int block_id; + // Where in the values array of the jacobian is this cell located. + int position; +}; + +// Order cell by their block_id; +bool CellLessThan(const Cell& lhs, const Cell& rhs); + +struct CompressedList { + Block block; + vector<Cell> cells; +}; + +typedef CompressedList CompressedRow; +typedef CompressedList CompressedColumn; + +struct CompressedRowBlockStructure { + vector<Block> cols; + vector<CompressedRow> rows; +}; + +struct CompressedColumnBlockStructure { + vector<Block> rows; + vector<CompressedColumn> cols; +}; + +// Deserialize the given block structure proto to the given block structure. +// Destroys previous contents of block_structure. +void ProtoToBlockStructure(const BlockStructureProto &proto, + CompressedRowBlockStructure *block_structure); + +// Serialize the given block structure to the given proto. Destroys previous +// contents of proto. +void BlockStructureToProto(const CompressedRowBlockStructure &block_structure, + BlockStructureProto *proto); + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_BLOCK_STRUCTURE_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/canonical_views_clustering.cc b/extern/libmv/third_party/ceres/internal/ceres/canonical_views_clustering.cc new file mode 100644 index 00000000000..53190ada6fc --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/canonical_views_clustering.cc @@ -0,0 +1,238 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 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: David Gallup (dgallup@google.com) +// Sameer Agarwal (sameeragarwal@google.com) + +#include "ceres/canonical_views_clustering.h" + +#include <glog/logging.h> +#include "ceres/graph.h" +#include "ceres/collections_port.h" +#include "ceres/map_util.h" +#include "ceres/internal/macros.h" + +namespace ceres { +namespace internal { + +typedef HashMap<int, int> IntMap; +typedef HashSet<int> IntSet; + +class CanonicalViewsClustering { + public: + CanonicalViewsClustering() {} + + // Compute the canonical views clustering of the vertices of the + // graph. centers will contain the vertices that are the identified + // as the canonical views/cluster centers, and membership is a map + // from vertices to cluster_ids. The i^th cluster center corresponds + // to the i^th cluster. It is possible depending on the + // configuration of the clustering algorithm that some of the + // vertices may not be assigned to any cluster. In this case they + // are assigned to a cluster with id = kInvalidClusterId. + void ComputeClustering(const Graph<int>& graph, + const CanonicalViewsClusteringOptions& options, + vector<int>* centers, + IntMap* membership); + + private: + void FindValidViews(IntSet* valid_views) const; + double ComputeClusteringQualityDifference(const int candidate, + const vector<int>& centers) const; + void UpdateCanonicalViewAssignments(const int canonical_view); + void ComputeClusterMembership(const vector<int>& centers, + IntMap* membership) const; + + CanonicalViewsClusteringOptions options_; + const Graph<int>* graph_; + // Maps a view to its representative canonical view (its cluster + // center). + IntMap view_to_canonical_view_; + // Maps a view to its similarity to its current cluster center. + HashMap<int, double> view_to_canonical_view_similarity_; + DISALLOW_COPY_AND_ASSIGN(CanonicalViewsClustering); +}; + +void ComputeCanonicalViewsClustering( + const Graph<int>& graph, + const CanonicalViewsClusteringOptions& options, + vector<int>* centers, + IntMap* membership) { + time_t start_time = time(NULL); + CanonicalViewsClustering cv; + cv.ComputeClustering(graph, options, centers, membership); + VLOG(2) << "Canonical views clustering time (secs): " + << time(NULL) - start_time; +} + +// Implementation of CanonicalViewsClustering +void CanonicalViewsClustering::ComputeClustering( + const Graph<int>& graph, + const CanonicalViewsClusteringOptions& options, + vector<int>* centers, + IntMap* membership) { + options_ = options; + CHECK_NOTNULL(centers)->clear(); + CHECK_NOTNULL(membership)->clear(); + graph_ = &graph; + + IntSet valid_views; + FindValidViews(&valid_views); + while (valid_views.size() > 0) { + // Find the next best canonical view. + double best_difference = -std::numeric_limits<double>::max(); + int best_view = 0; + + // TODO(sameeragarwal): Make this loop multi-threaded. + for (IntSet::const_iterator view = valid_views.begin(); + view != valid_views.end(); + ++view) { + const double difference = + ComputeClusteringQualityDifference(*view, *centers); + if (difference > best_difference) { + best_difference = difference; + best_view = *view; + } + } + + CHECK_GT(best_difference, -std::numeric_limits<double>::max()); + + // Add canonical view if quality improves, or if minimum is not + // yet met, otherwise break. + if ((best_difference <= 0) && + (centers->size() >= options_.min_views)) { + break; + } + + centers->push_back(best_view); + valid_views.erase(best_view); + UpdateCanonicalViewAssignments(best_view); + } + + ComputeClusterMembership(*centers, membership); +} + +// Return the set of vertices of the graph which have valid vertex +// weights. +void CanonicalViewsClustering::FindValidViews( + IntSet* valid_views) const { + const IntSet& views = graph_->vertices(); + for (IntSet::const_iterator view = views.begin(); + view != views.end(); + ++view) { + if (graph_->VertexWeight(*view) != Graph<int>::InvalidWeight()) { + valid_views->insert(*view); + } + } +} + +// Computes the difference in the quality score if 'candidate' were +// added to the set of canonical views. +double CanonicalViewsClustering::ComputeClusteringQualityDifference( + const int candidate, + const vector<int>& centers) const { + // View score. + double difference = + options_.view_score_weight * graph_->VertexWeight(candidate); + + // Compute how much the quality score changes if the candidate view + // was added to the list of canonical views and its nearest + // neighbors became members of its cluster. + const IntSet& neighbors = graph_->Neighbors(candidate); + for (IntSet::const_iterator neighbor = neighbors.begin(); + neighbor != neighbors.end(); + ++neighbor) { + const double old_similarity = + FindWithDefault(view_to_canonical_view_similarity_, *neighbor, 0.0); + const double new_similarity = graph_->EdgeWeight(*neighbor, candidate); + if (new_similarity > old_similarity) { + difference += new_similarity - old_similarity; + } + } + + // Number of views penalty. + difference -= options_.size_penalty_weight; + + // Orthogonality. + for (int i = 0; i < centers.size(); ++i) { + difference -= options_.similarity_penalty_weight * + graph_->EdgeWeight(centers[i], candidate); + } + + return difference; +} + +// Reassign views if they're more similar to the new canonical view. +void CanonicalViewsClustering::UpdateCanonicalViewAssignments( + const int canonical_view) { + const IntSet& neighbors = graph_->Neighbors(canonical_view); + for (IntSet::const_iterator neighbor = neighbors.begin(); + neighbor != neighbors.end(); + ++neighbor) { + const double old_similarity = + FindWithDefault(view_to_canonical_view_similarity_, *neighbor, 0.0); + const double new_similarity = + graph_->EdgeWeight(*neighbor, canonical_view); + if (new_similarity > old_similarity) { + view_to_canonical_view_[*neighbor] = canonical_view; + view_to_canonical_view_similarity_[*neighbor] = new_similarity; + } + } +} + +// Assign a cluster id to each view. +void CanonicalViewsClustering::ComputeClusterMembership( + const vector<int>& centers, + IntMap* membership) const { + CHECK_NOTNULL(membership)->clear(); + + // The i^th cluster has cluster id i. + IntMap center_to_cluster_id; + for (int i = 0; i < centers.size(); ++i) { + center_to_cluster_id[centers[i]] = i; + } + + static const int kInvalidClusterId = -1; + + const IntSet& views = graph_->vertices(); + for (IntSet::const_iterator view = views.begin(); + view != views.end(); + ++view) { + IntMap::const_iterator it = + view_to_canonical_view_.find(*view); + int cluster_id = kInvalidClusterId; + if (it != view_to_canonical_view_.end()) { + cluster_id = FindOrDie(center_to_cluster_id, it->second); + } + + InsertOrDie(membership, *view, cluster_id); + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/canonical_views_clustering.h b/extern/libmv/third_party/ceres/internal/ceres/canonical_views_clustering.h new file mode 100644 index 00000000000..2d1eb403995 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/canonical_views_clustering.h @@ -0,0 +1,133 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// An implementation of the Canonical Views clustering algorithm from +// "Scene Summarization for Online Image Collections", Ian Simon, Noah +// Snavely, Steven M. Seitz, ICCV 2007. +// +// More details can be found at +// http://grail.cs.washington.edu/projects/canonview/ +// +// Ceres uses this algorithm to perform view clustering for +// constructing visibility based preconditioners. + +#ifndef CERES_INTERNAL_CANONICAL_VIEWS_CLUSTERING_H_ +#define CERES_INTERNAL_CANONICAL_VIEWS_CLUSTERING_H_ + +#include <vector> + +#include <glog/logging.h> +#include "ceres/collections_port.h" +#include "ceres/graph.h" +#include "ceres/map_util.h" +#include "ceres/internal/macros.h" + +namespace ceres { +namespace internal { + +class CanonicalViewsClusteringOptions; + +// Compute a partitioning of the vertices of the graph using the +// canonical views clustering algorithm. +// +// In the following we will use the terms vertices and views +// interchangably. Given a weighted Graph G(V,E), the canonical views +// of G are the the set of vertices that best "summarize" the content +// of the graph. If w_ij i s the weight connecting the vertex i to +// vertex j, and C is the set of canonical views. Then the objective +// of the canonical views algorithm is +// +// E[C] = sum_[i in V] max_[j in C] w_ij +// - size_penalty_weight * |C| +// - similarity_penalty_weight * sum_[i in C, j in C, j > i] w_ij +// +// alpha is the size penalty that penalizes large number of canonical +// views. +// +// beta is the similarity penalty that penalizes canonical views that +// are too similar to other canonical views. +// +// Thus the canonical views algorithm tries to find a canonical view +// for each vertex in the graph which best explains it, while trying +// to minimize the number of canonical views and the overlap between +// them. +// +// We further augment the above objective function by allowing for per +// vertex weights, higher weights indicating a higher preference for +// being chosen as a canonical view. Thus if w_i is the vertex weight +// for vertex i, the objective function is then +// +// E[C] = sum_[i in V] max_[j in C] w_ij +// - size_penalty_weight * |C| +// - similarity_penalty_weight * sum_[i in C, j in C, j > i] w_ij +// + view_score_weight * sum_[i in C] w_i +// +// centers will contain the vertices that are the identified +// as the canonical views/cluster centers, and membership is a map +// from vertices to cluster_ids. The i^th cluster center corresponds +// to the i^th cluster. +// +// It is possible depending on the configuration of the clustering +// algorithm that some of the vertices may not be assigned to any +// cluster. In this case they are assigned to a cluster with id = -1; +void ComputeCanonicalViewsClustering( + const Graph<int>& graph, + const CanonicalViewsClusteringOptions& options, + vector<int>* centers, + HashMap<int, int>* membership); + +struct CanonicalViewsClusteringOptions { + CanonicalViewsClusteringOptions() + : min_views(3), + size_penalty_weight(5.75), + similarity_penalty_weight(100.0), + view_score_weight(0.0) { + } + // The minimum number of canonical views to compute. + int min_views; + + // Penalty weight for the number of canonical views. A higher + // number will result in fewer canonical views. + double size_penalty_weight; + + // Penalty weight for the diversity (orthogonality) of the + // canonical views. A higher number will encourage less similar + // canonical views. + double similarity_penalty_weight; + + // Weight for per-view scores. Lower weight places less + // confidence in the view scores. + double view_score_weight; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_CANONICAL_VIEWS_CLUSTERING_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/casts.h b/extern/libmv/third_party/ceres/internal/ceres/casts.h new file mode 100644 index 00000000000..99cf2186cc7 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/casts.h @@ -0,0 +1,108 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 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: keir@google.com (Keir Mierle) + +#ifndef CERES_INTERNAL_CASTS_H_ +#define CERES_INTERNAL_CASTS_H_ + +#include <cassert> +#include <cstddef> // For NULL. + +namespace ceres { + +// Identity metafunction. +template <class T> +struct identity_ { + typedef T type; +}; + +// Use implicit_cast as a safe version of static_cast or const_cast +// for implicit conversions. For example: +// - Upcasting in a type hierarchy. +// - Performing arithmetic conversions (int32 to int64, int to double, etc.). +// - Adding const or volatile qualifiers. +// +// In general, implicit_cast can be used to convert this code +// To to = from; +// DoSomething(to); +// to this +// DoSomething(implicit_cast<To>(from)); +// +// base::identity_ is used to make a non-deduced context, which +// forces all callers to explicitly specify the template argument. +template<typename To> +inline To implicit_cast(typename identity_<To>::type to) { + return to; +} + +// This version of implicit_cast is used when two template arguments +// are specified. It's obsolete and should not be used. +template<typename To, typename From> +inline To implicit_cast(typename identity_<From>::type const &f) { + return f; +} + +// When you upcast (that is, cast a pointer from type Foo to type +// SuperclassOfFoo), it's fine to use implicit_cast<>, since upcasts +// always succeed. When you downcast (that is, cast a pointer from +// type Foo to type SubclassOfFoo), static_cast<> isn't safe, because +// how do you know the pointer is really of type SubclassOfFoo? It +// could be a bare Foo, or of type DifferentSubclassOfFoo. Thus, +// when you downcast, you should use this macro. In debug mode, we +// use dynamic_cast<> to double-check the downcast is legal (we die +// if it's not). In normal mode, we do the efficient static_cast<> +// instead. Thus, it's important to test in debug mode to make sure +// the cast is legal! +// This is the only place in the code we should use dynamic_cast<>. +// In particular, you SHOULDN'T be using dynamic_cast<> in order to +// do RTTI (eg code like this: +// if (dynamic_cast<Subclass1>(foo)) HandleASubclass1Object(foo); +// if (dynamic_cast<Subclass2>(foo)) HandleASubclass2Object(foo); +// You should design the code some other way not to need this. + +template<typename To, typename From> // use like this: down_cast<T*>(foo); +inline To down_cast(From* f) { // so we only accept pointers + // Ensures that To is a sub-type of From *. This test is here only + // for compile-time type checking, and has no overhead in an + // optimized build at run-time, as it will be optimized away + // completely. + + // TODO(csilvers): This should use COMPILE_ASSERT. + if (false) { + implicit_cast<From*, To>(NULL); + } + + // uses RTTI in dbg and fastbuild. asserts are disabled in opt builds. + assert(f == NULL || dynamic_cast<To>(f) != NULL); // NOLINT + return static_cast<To>(f); +} + +} // namespace ceres + +#endif // CERES_INTERNAL_CASTS_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/cgnr_linear_operator.h b/extern/libmv/third_party/ceres/internal/ceres/cgnr_linear_operator.h new file mode 100644 index 00000000000..f32d8d95c19 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/cgnr_linear_operator.h @@ -0,0 +1,120 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2012 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: keir@google.com (Keir Mierle) + +#ifndef CERES_INTERNAL_CGNR_LINEAR_OPERATOR_H_ +#define CERES_INTERNAL_CGNR_LINEAR_OPERATOR_H_ + +#include <algorithm> +#include "ceres/linear_operator.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +class SparseMatrix; + +// A linear operator which takes a matrix A and a diagonal vector D and +// performs products of the form +// +// (A^T A + D^T D)x +// +// This is used to implement iterative general sparse linear solving with +// conjugate gradients, where A is the Jacobian and D is a regularizing +// parameter. A brief proof that D^T D is the correct regularizer: +// +// Given a regularized least squares problem: +// +// min ||Ax - b||^2 + ||Dx||^2 +// x +// +// First expand into matrix notation: +// +// (Ax - b)^T (Ax - b) + xD^TDx +// +// Then multiply out to get: +// +// = xA^TAx - 2b^T Ax + b^Tb + xD^TDx +// +// Take the derivative: +// +// 0 = 2A^TAx - 2A^T b + 2 D^TDx +// 0 = A^TAx - A^T b + D^TDx +// 0 = (A^TA + D^TD)x - A^T b +// +// Thus, the symmetric system we need to solve for CGNR is +// +// Sx = z +// +// with S = A^TA + D^TD +// and z = A^T b +// +// Note: This class is not thread safe, since it uses some temporary storage. +class CgnrLinearOperator : public LinearOperator { + public: + CgnrLinearOperator(const LinearOperator& A, const double *D) + : A_(A), D_(D), z_(new double[A.num_rows()]) { + } + virtual ~CgnrLinearOperator() {} + + virtual void RightMultiply(const double* x, double* y) const { + std::fill(z_.get(), z_.get() + A_.num_rows(), 0.0); + + // z = Ax + A_.RightMultiply(x, z_.get()); + + // y = y + Atz + A_.LeftMultiply(z_.get(), y); + + // y = y + DtDx + if (D_ != NULL) { + int n = A_.num_cols(); + VectorRef(y, n).array() += ConstVectorRef(D_, n).array().square() * + ConstVectorRef(x, n).array(); + } + } + + virtual void LeftMultiply(const double* x, double* y) const { + RightMultiply(x, y); + } + + virtual int num_rows() const { return A_.num_cols(); } + virtual int num_cols() const { return A_.num_cols(); } + + private: + const LinearOperator& A_; + const double* D_; + scoped_array<double> z_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_CGNR_LINEAR_OPERATOR_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/cgnr_solver.cc b/extern/libmv/third_party/ceres/internal/ceres/cgnr_solver.cc new file mode 100644 index 00000000000..ccc8026f9f7 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/cgnr_solver.cc @@ -0,0 +1,80 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2012 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: keir@google.com (Keir Mierle) + +#include "ceres/cgnr_solver.h" + +#include "glog/logging.h" +#include "ceres/linear_solver.h" +#include "ceres/cgnr_linear_operator.h" +#include "ceres/conjugate_gradients_solver.h" +#include "ceres/block_jacobi_preconditioner.h" + +namespace ceres { +namespace internal { + +CgnrSolver::CgnrSolver(const LinearSolver::Options& options) + : options_(options), + jacobi_preconditioner_(NULL) { +} + +LinearSolver::Summary CgnrSolver::Solve( + LinearOperator* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x) { + // Form z = Atb. + scoped_array<double> z(new double[A->num_cols()]); + std::fill(z.get(), z.get() + A->num_cols(), 0.0); + A->LeftMultiply(b, z.get()); + + // Precondition if necessary. + LinearSolver::PerSolveOptions cg_per_solve_options = per_solve_options; + if (options_.preconditioner_type == JACOBI) { + if (jacobi_preconditioner_.get() == NULL) { + jacobi_preconditioner_.reset(new BlockJacobiPreconditioner(*A)); + } + jacobi_preconditioner_->Update(*A, per_solve_options.D); + cg_per_solve_options.preconditioner = jacobi_preconditioner_.get(); + } else if (options_.preconditioner_type != IDENTITY) { + LOG(FATAL) << "CGNR only supports IDENTITY and JACOBI preconditioners."; + } + + // Solve (AtA + DtD)x = z (= Atb). + std::fill(x, x + A->num_cols(), 0.0); + CgnrLinearOperator lhs(*A, per_solve_options.D); + ConjugateGradientsSolver conjugate_gradient_solver(options_); + return conjugate_gradient_solver.Solve(&lhs, + z.get(), + cg_per_solve_options, + x); +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/cgnr_solver.h b/extern/libmv/third_party/ceres/internal/ceres/cgnr_solver.h new file mode 100644 index 00000000000..dd36f99006b --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/cgnr_solver.h @@ -0,0 +1,66 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2012 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: keir@google.com (Keir Mierle) + +#ifndef CERES_INTERNAL_CGNR_SOLVER_H_ +#define CERES_INTERNAL_CGNR_SOLVER_H_ + +#include "ceres/internal/scoped_ptr.h" +#include "ceres/linear_solver.h" + +namespace ceres { +namespace internal { + +class BlockJacobiPreconditioner; + +// A conjugate gradients on the normal equations solver. This directly solves +// for the solution to +// +// (A^T A + D^T D)x = A^T b +// +// as required for solving for x in the least squares sense. Currently only +// block diagonal preconditioning is supported. +class CgnrSolver : public LinearSolver { + public: + explicit CgnrSolver(const LinearSolver::Options& options); + virtual Summary Solve(LinearOperator* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x); + + private: + const LinearSolver::Options options_; + scoped_ptr<BlockJacobiPreconditioner> jacobi_preconditioner_; + DISALLOW_COPY_AND_ASSIGN(CgnrSolver); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_CGNR_SOLVER_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/collections_port.h b/extern/libmv/third_party/ceres/internal/ceres/collections_port.h new file mode 100644 index 00000000000..e125f3fffcd --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/collections_port.h @@ -0,0 +1,141 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 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: keir@google.com (Keir Mierle) +// +// Portable HashMap and HashSet, and a specialized overload for hashing pairs. + +#ifndef CERES_INTERNAL_COLLECTIONS_PORT_H_ +#define CERES_INTERNAL_COLLECTIONS_PORT_H_ + +#if defined(_MSC_VER) && _MSC_VER <= 1600 +#include <unordered_map> +#include <unordered_set> +#else +#include <tr1/unordered_map> +#include <tr1/unordered_set> +#endif +#include <utility> +#include "ceres/integral_types.h" +#include "ceres/internal/port.h" + +namespace ceres { +namespace internal { + +template<typename K, typename V> +struct HashMap : tr1::unordered_map<K, V> {}; + +template<typename K> +struct HashSet : tr1::unordered_set<K> {}; + +#ifdef _WIN32 +#define GG_LONGLONG(x) x##I64 +#define GG_ULONGLONG(x) x##UI64 +#else +#define GG_LONGLONG(x) x##LL +#define GG_ULONGLONG(x) x##ULL +#endif + +// The hash function is due to Bob Jenkins (see +// http://burtleburtle.net/bob/hash/index.html). Each mix takes 36 instructions, +// in 18 cycles if you're lucky. On x86 architectures, this requires 45 +// instructions in 27 cycles, if you're lucky. +// +// 32bit version +inline void hash_mix(uint32& a, uint32& b, uint32& c) { + a -= b; a -= c; a ^= (c>>13); + b -= c; b -= a; b ^= (a<<8); + c -= a; c -= b; c ^= (b>>13); + a -= b; a -= c; a ^= (c>>12); + b -= c; b -= a; b ^= (a<<16); + c -= a; c -= b; c ^= (b>>5); + a -= b; a -= c; a ^= (c>>3); + b -= c; b -= a; b ^= (a<<10); + c -= a; c -= b; c ^= (b>>15); +} + +// 64bit version +inline void hash_mix(uint64& a, uint64& b, uint64& c) { + a -= b; a -= c; a ^= (c>>43); + b -= c; b -= a; b ^= (a<<9); + c -= a; c -= b; c ^= (b>>8); + a -= b; a -= c; a ^= (c>>38); + b -= c; b -= a; b ^= (a<<23); + c -= a; c -= b; c ^= (b>>5); + a -= b; a -= c; a ^= (c>>35); + b -= c; b -= a; b ^= (a<<49); + c -= a; c -= b; c ^= (b>>11); +} + +inline uint32 Hash32NumWithSeed(uint32 num, uint32 c) { + // The golden ratio; an arbitrary value. + uint32 b = 0x9e3779b9UL; + hash_mix(num, b, c); + return c; +} + +inline uint64 Hash64NumWithSeed(uint64 num, uint64 c) { + // More of the golden ratio. + uint64 b = GG_ULONGLONG(0xe08c1d668b756f82); + hash_mix(num, b, c); + return c; +} + +} // namespace internal +} // namespace ceres + +// Since on some platforms this is a doubly-nested namespace (std::tr1) and +// others it is not, the entire namespace line must be in a macro. +CERES_HASH_NAMESPACE_START + +// The outrageously annoying specializations below are for portability reasons. +// In short, it's not possible to have two overloads of hash<pair<T1, T2> + +// Hasher for STL pairs. Requires hashers for both members to be defined. +template<typename T> +struct hash<pair<T, T> > { + size_t operator()(const pair<T, T>& p) const { + size_t h1 = hash<T>()(p.first); + size_t h2 = hash<T>()(p.second); + // The decision below is at compile time + return (sizeof(h1) <= sizeof(ceres::internal::uint32)) ? + ceres::internal::Hash32NumWithSeed(h1, h2) : + ceres::internal::Hash64NumWithSeed(h1, h2); + } + // Less than operator for MSVC. + bool operator()(const pair<T, T>& a, + const pair<T, T>& b) const { + return a < b; + } + static const size_t bucket_size = 4; // These are required by MSVC + static const size_t min_buckets = 8; // 4 and 8 are defaults. +}; + +CERES_HASH_NAMESPACE_END + +#endif // CERES_INTERNAL_COLLECTIONS_PORT_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/compressed_row_jacobian_writer.cc b/extern/libmv/third_party/ceres/internal/ceres/compressed_row_jacobian_writer.cc new file mode 100644 index 00000000000..aa883b7d353 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/compressed_row_jacobian_writer.cc @@ -0,0 +1,201 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 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: keir@google.com (Keir Mierle) + +#include "ceres/compressed_row_jacobian_writer.h" + +#include "ceres/casts.h" +#include "ceres/compressed_row_sparse_matrix.h" +#include "ceres/parameter_block.h" +#include "ceres/program.h" +#include "ceres/residual_block.h" +#include "ceres/scratch_evaluate_preparer.h" + +namespace ceres { +namespace internal { + +SparseMatrix* CompressedRowJacobianWriter::CreateJacobian() const { + const vector<ResidualBlock*>& residual_blocks = + program_->residual_blocks(); + + int total_num_residuals = program_->NumResiduals(); + int total_num_effective_parameters = program_->NumEffectiveParameters(); + + // Count the number of jacobian nonzeros. + int num_jacobian_nonzeros = 0; + for (int i = 0; i < residual_blocks.size(); ++i) { + ResidualBlock* residual_block = residual_blocks[i]; + const int num_residuals = residual_block->NumResiduals(); + const int num_parameter_blocks = residual_block->NumParameterBlocks(); + for (int j = 0; j < num_parameter_blocks; ++j) { + ParameterBlock* parameter_block = residual_block->parameter_blocks()[j]; + if (!parameter_block->IsConstant()) { + num_jacobian_nonzeros += num_residuals * parameter_block->LocalSize(); + } + } + } + + // Allocate storage for the jacobian with some extra space at the end. + // Allocate more space than needed to store the jacobian so that when the LM + // algorithm adds the diagonal, no reallocation is necessary. This reduces + // peak memory usage significantly. + CompressedRowSparseMatrix* jacobian = + new CompressedRowSparseMatrix( + total_num_residuals, + total_num_effective_parameters, + num_jacobian_nonzeros + total_num_effective_parameters); + + // At this stage, the CompressedSparseMatrix 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(); + int row_pos = 0; + rows[0] = 0; + for (int i = 0; i < residual_blocks.size(); ++i) { + const ResidualBlock* residual_block = residual_blocks[i]; + const int num_parameter_blocks = residual_block->NumParameterBlocks(); + + // Count the number of derivatives for a row of this residual block and + // build a list of active parameter block indices. + int num_derivatives = 0; + vector<int> parameter_indices; + for (int j = 0; j < num_parameter_blocks; ++j) { + ParameterBlock* parameter_block = residual_block->parameter_blocks()[j]; + if (!parameter_block->IsConstant()) { + parameter_indices.push_back(parameter_block->index()); + num_derivatives += parameter_block->LocalSize(); + } + } + + // Sort the parameters by their position in the state vector. + sort(parameter_indices.begin(), parameter_indices.end()); + CHECK(unique(parameter_indices.begin(), parameter_indices.end()) == + parameter_indices.end()) + << "Ceres internal error: " + << "Duplicate parameter blocks detected in a cost function. " + << "This should never happen. Please report this to " + << "the Ceres developers."; + + // Update the row indices. + const int num_residuals = residual_block->NumResiduals(); + for (int j = 0; j < num_residuals; ++j) { + rows[row_pos + j + 1] = rows[row_pos + j] + num_derivatives; + } + + // Iterate over parameter blocks in the order which they occur in the + // parameter vector. This code mirrors that in Write(), where jacobian + // values are updated. + int col_pos = 0; + for (int j = 0; j < parameter_indices.size(); ++j) { + ParameterBlock* parameter_block = + program_->parameter_blocks()[parameter_indices[j]]; + const int parameter_block_size = parameter_block->LocalSize(); + + for (int r = 0; r < num_residuals; ++r) { + // This is the position in the values array of the jacobian where this + // row of the jacobian block should go. + const int column_block_begin = rows[row_pos + r] + col_pos; + + for (int c = 0; c < parameter_block_size; ++c) { + cols[column_block_begin + c] = parameter_block->delta_offset() + c; + } + } + col_pos += parameter_block_size; + } + row_pos += num_residuals; + } + + CHECK_EQ(num_jacobian_nonzeros, rows[total_num_residuals]); + return jacobian; +} + +void CompressedRowJacobianWriter::Write(int residual_id, + int residual_offset, + double **jacobians, + SparseMatrix* base_jacobian) { + CompressedRowSparseMatrix* jacobian = + down_cast<CompressedRowSparseMatrix*>(base_jacobian); + + double* jacobian_values = jacobian->mutable_values(); + const int* jacobian_rows = jacobian->rows(); + + 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()); + + // Where in the current row does the jacobian for a parameter block begin. + int col_pos = 0; + + // Iterate over the jacobian blocks in increasing order of their + // positions in the reduced parameter vector. + for (int i = 0; i < evaluated_jacobian_blocks.size(); ++i) { + const ParameterBlock* parameter_block = + program_->parameter_blocks()[evaluated_jacobian_blocks[i].first]; + const int argument = evaluated_jacobian_blocks[i].second; + const int parameter_block_size = parameter_block->LocalSize(); + + // Copy one row of the jacobian block at a time. + for (int r = 0; r < num_residuals; ++r) { + // Position of the r^th row of the current jacobian block. + const double* block_row_begin = + jacobians[argument] + r * parameter_block_size; + + // Position in the values array of the jacobian where this + // row of the jacobian block should go. + double* column_block_begin = + jacobian_values + jacobian_rows[residual_offset + r] + col_pos; + + copy(block_row_begin, + block_row_begin + parameter_block_size, + column_block_begin); + } + col_pos += parameter_block_size; + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/compressed_row_jacobian_writer.h b/extern/libmv/third_party/ceres/internal/ceres/compressed_row_jacobian_writer.h new file mode 100644 index 00000000000..c103165eaf1 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/compressed_row_jacobian_writer.h @@ -0,0 +1,75 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 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: keir@google.com (Keir Mierle) +// +// A jacobian writer that directly writes to compressed row sparse matrices. + +#ifndef CERES_INTERNAL_COMPRESSED_ROW_JACOBIAN_WRITER_H_ +#define CERES_INTERNAL_COMPRESSED_ROW_JACOBIAN_WRITER_H_ + +#include "ceres/evaluator.h" +#include "ceres/scratch_evaluate_preparer.h" + +namespace ceres { +namespace internal { + +class Program; +class SparseMatrix; + +class CompressedRowJacobianWriter { + public: + CompressedRowJacobianWriter(Evaluator::Options /* ignored */, + Program* program) + : program_(program) { + } + + // JacobianWriter interface. + + // Since the compressed row matrix has different layout than that assumed by + // the cost functions, use scratch space to store the jacobians temporarily + // then copy them over to the larger jacobian in the Write() function. + ScratchEvaluatePreparer* CreateEvaluatePreparers(int num_threads) { + return ScratchEvaluatePreparer::Create(*program_, num_threads); + } + + SparseMatrix* CreateJacobian() const; + + void Write(int residual_id, + int residual_offset, + double **jacobians, + SparseMatrix* base_jacobian); + + private: + Program* program_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_COMPRESSED_ROW_JACOBIAN_WRITER_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/compressed_row_sparse_matrix.cc b/extern/libmv/third_party/ceres/internal/ceres/compressed_row_sparse_matrix.cc new file mode 100644 index 00000000000..95edf5396af --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/compressed_row_sparse_matrix.cc @@ -0,0 +1,334 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/compressed_row_sparse_matrix.h" + +#include <algorithm> +#include <vector> +#include "ceres/matrix_proto.h" +#include "ceres/internal/port.h" + +namespace ceres { +namespace internal { +namespace { + +// Helper functor used by the constructor for reordering the contents +// of a TripletSparseMatrix. +struct RowColLessThan { + RowColLessThan(const int* rows, const int* cols) + : rows(rows), cols(cols) { + } + + bool operator()(const int x, const int y) const { + if (rows[x] == rows[y]) { + return (cols[x] < cols[y]); + } + return (rows[x] < rows[y]); + } + + const int* rows; + const int* cols; +}; + +} // namespace + +// This constructor gives you a semi-initialized CompressedRowSparseMatrix. +CompressedRowSparseMatrix::CompressedRowSparseMatrix(int num_rows, + int num_cols, + int max_num_nonzeros) { + num_rows_ = num_rows; + num_cols_ = num_cols; + max_num_nonzeros_ = max_num_nonzeros; + + VLOG(1) << "# of rows: " << num_rows_ << " # of columns: " << num_cols_ + << " max_num_nonzeros: " << max_num_nonzeros_ + << ". Allocating " << (num_rows_ + 1) * sizeof(int) + // NOLINT + max_num_nonzeros_ * sizeof(int) + // NOLINT + max_num_nonzeros_ * sizeof(double); // NOLINT + + rows_.reset(new int[num_rows_ + 1]); + cols_.reset(new int[max_num_nonzeros_]); + values_.reset(new double[max_num_nonzeros_]); + + fill(rows_.get(), rows_.get() + num_rows_ + 1, 0); + fill(cols_.get(), cols_.get() + max_num_nonzeros_, 0); + fill(values_.get(), values_.get() + max_num_nonzeros_, 0); +} + +CompressedRowSparseMatrix::CompressedRowSparseMatrix( + const TripletSparseMatrix& m) { + num_rows_ = m.num_rows(); + num_cols_ = m.num_cols(); + max_num_nonzeros_ = m.max_num_nonzeros(); + + // index is the list of indices into the TripletSparseMatrix m. + vector<int> index(m.num_nonzeros(), 0); + for (int i = 0; i < m.num_nonzeros(); ++i) { + index[i] = i; + } + + // Sort index such that the entries of m are ordered by row and ties + // are broken by column. + sort(index.begin(), index.end(), RowColLessThan(m.rows(), m.cols())); + + VLOG(1) << "# of rows: " << num_rows_ << " # of columns: " << num_cols_ + << " max_num_nonzeros: " << max_num_nonzeros_ + << ". Allocating " << (num_rows_ + 1) * sizeof(int) + // NOLINT + max_num_nonzeros_ * sizeof(int) + // NOLINT + max_num_nonzeros_ * sizeof(double); // NOLINT + + rows_.reset(new int[num_rows_ + 1]); + cols_.reset(new int[max_num_nonzeros_]); + values_.reset(new double[max_num_nonzeros_]); + + // rows_ = 0 + fill(rows_.get(), rows_.get() + num_rows_ + 1, 0); + + // Copy the contents of the cols and values array in the order given + // by index and count the number of entries in each row. + for (int i = 0; i < m.num_nonzeros(); ++i) { + const int idx = index[i]; + ++rows_[m.rows()[idx] + 1]; + cols_[i] = m.cols()[idx]; + values_[i] = m.values()[idx]; + } + + // Find the cumulative sum of the row counts. + for (int i = 1; i < num_rows_ + 1; ++i) { + rows_[i] += rows_[i-1]; + } + + CHECK_EQ(num_nonzeros(), m.num_nonzeros()); +} + +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS +CompressedRowSparseMatrix::CompressedRowSparseMatrix( + const SparseMatrixProto& outer_proto) { + CHECK(outer_proto.has_compressed_row_matrix()); + + const CompressedRowSparseMatrixProto& proto = + outer_proto.compressed_row_matrix(); + + num_rows_ = proto.num_rows(); + num_cols_ = proto.num_cols(); + + rows_.reset(new int[proto.rows_size()]); + cols_.reset(new int[proto.cols_size()]); + values_.reset(new double[proto.values_size()]); + + for (int i = 0; i < proto.rows_size(); ++i) { + rows_[i] = proto.rows(i); + } + + CHECK_EQ(proto.rows_size(), num_rows_ + 1); + CHECK_EQ(proto.cols_size(), proto.values_size()); + CHECK_EQ(proto.cols_size(), rows_[num_rows_]); + + for (int i = 0; i < proto.cols_size(); ++i) { + cols_[i] = proto.cols(i); + values_[i] = proto.values(i); + } + + max_num_nonzeros_ = proto.cols_size(); +} +#endif + +CompressedRowSparseMatrix::CompressedRowSparseMatrix(const double* diagonal, + int num_rows) { + CHECK_NOTNULL(diagonal); + + num_rows_ = num_rows; + num_cols_ = num_rows; + max_num_nonzeros_ = num_rows; + + rows_.reset(new int[num_rows_ + 1]); + cols_.reset(new int[num_rows_]); + values_.reset(new double[num_rows_]); + + rows_[0] = 0; + for (int i = 0; i < num_rows_; ++i) { + cols_[i] = i; + values_[i] = diagonal[i]; + rows_[i + 1] = i + 1; + } + + CHECK_EQ(num_nonzeros(), num_rows); +} + +CompressedRowSparseMatrix::~CompressedRowSparseMatrix() { +} + +void CompressedRowSparseMatrix::SetZero() { + fill(values_.get(), values_.get() + num_nonzeros(), 0.0); +} + +void CompressedRowSparseMatrix::RightMultiply(const double* x, + double* y) const { + CHECK_NOTNULL(x); + CHECK_NOTNULL(y); + + for (int r = 0; r < num_rows_; ++r) { + for (int idx = rows_[r]; idx < rows_[r + 1]; ++idx) { + y[r] += values_[idx] * x[cols_[idx]]; + } + } +} + +void CompressedRowSparseMatrix::LeftMultiply(const double* x, double* y) const { + CHECK_NOTNULL(x); + CHECK_NOTNULL(y); + + for (int r = 0; r < num_rows_; ++r) { + for (int idx = rows_[r]; idx < rows_[r + 1]; ++idx) { + y[cols_[idx]] += values_[idx] * x[r]; + } + } +} + +void CompressedRowSparseMatrix::SquaredColumnNorm(double* x) const { + CHECK_NOTNULL(x); + + fill(x, x + num_cols_, 0.0); + for (int idx = 0; idx < rows_[num_rows_]; ++idx) { + x[cols_[idx]] += values_[idx] * values_[idx]; + } +} + +void CompressedRowSparseMatrix::ScaleColumns(const double* scale) { + CHECK_NOTNULL(scale); + + for (int idx = 0; idx < rows_[num_rows_]; ++idx) { + values_[idx] *= scale[cols_[idx]]; + } +} + +void CompressedRowSparseMatrix::ToDenseMatrix(Matrix* dense_matrix) const { + CHECK_NOTNULL(dense_matrix); + dense_matrix->resize(num_rows_, num_cols_); + dense_matrix->setZero(); + + for (int r = 0; r < num_rows_; ++r) { + for (int idx = rows_[r]; idx < rows_[r + 1]; ++idx) { + (*dense_matrix)(r, cols_[idx]) = values_[idx]; + } + } +} + +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS +void CompressedRowSparseMatrix::ToProto(SparseMatrixProto* outer_proto) const { + CHECK_NOTNULL(outer_proto); + + outer_proto->Clear(); + CompressedRowSparseMatrixProto* proto + = outer_proto->mutable_compressed_row_matrix(); + + proto->set_num_rows(num_rows_); + proto->set_num_cols(num_cols_); + + for (int r = 0; r < num_rows_ + 1; ++r) { + proto->add_rows(rows_[r]); + } + + for (int idx = 0; idx < rows_[num_rows_]; ++idx) { + proto->add_cols(cols_[idx]); + proto->add_values(values_[idx]); + } +} +#endif + +void CompressedRowSparseMatrix::DeleteRows(int delta_rows) { + CHECK_GE(delta_rows, 0); + CHECK_LE(delta_rows, num_rows_); + + int new_num_rows = num_rows_ - delta_rows; + + num_rows_ = new_num_rows; + int* new_rows = new int[num_rows_ + 1]; + copy(rows_.get(), rows_.get() + num_rows_ + 1, new_rows); + rows_.reset(new_rows); +} + +void CompressedRowSparseMatrix::AppendRows(const CompressedRowSparseMatrix& m) { + CHECK_EQ(m.num_cols(), num_cols_); + + // Check if there is enough space. If not, then allocate new arrays + // to hold the combined matrix and copy the contents of this matrix + // into it. + if (max_num_nonzeros_ < num_nonzeros() + m.num_nonzeros()) { + int new_max_num_nonzeros = num_nonzeros() + m.num_nonzeros(); + + VLOG(1) << "Reallocating " << sizeof(int) * new_max_num_nonzeros; // NOLINT + + int* new_cols = new int[new_max_num_nonzeros]; + copy(cols_.get(), cols_.get() + max_num_nonzeros_, new_cols); + cols_.reset(new_cols); + + double* new_values = new double[new_max_num_nonzeros]; + copy(values_.get(), values_.get() + max_num_nonzeros_, new_values); + values_.reset(new_values); + + max_num_nonzeros_ = new_max_num_nonzeros; + } + + // Copy the contents of m into this matrix. + copy(m.cols(), m.cols() + m.num_nonzeros(), cols_.get() + num_nonzeros()); + copy(m.values(), + m.values() + m.num_nonzeros(), + values_.get() + num_nonzeros()); + + // Create the new rows array to hold the enlarged matrix. + int* new_rows = new int[num_rows_ + m.num_rows() + 1]; + // The first num_rows_ entries are the same + copy(rows_.get(), rows_.get() + num_rows_, new_rows); + + // new_rows = [rows_, m.row() + rows_[num_rows_]] + fill(new_rows + num_rows_, + new_rows + num_rows_ + m.num_rows() + 1, + rows_[num_rows_]); + + for (int r = 0; r < m.num_rows() + 1; ++r) { + new_rows[num_rows_ + r] += m.rows()[r]; + } + + rows_.reset(new_rows); + num_rows_ += m.num_rows(); +} + +void CompressedRowSparseMatrix::ToTextFile(FILE* file) const { + CHECK_NOTNULL(file); + for (int r = 0; r < num_rows_; ++r) { + for (int idx = rows_[r]; idx < rows_[r + 1]; ++idx) { + fprintf(file, "% 10d % 10d %17f\n", r, cols_[idx], values_[idx]); + } + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/compressed_row_sparse_matrix.h b/extern/libmv/third_party/ceres/internal/ceres/compressed_row_sparse_matrix.h new file mode 100644 index 00000000000..9a39d28e111 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/compressed_row_sparse_matrix.h @@ -0,0 +1,129 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_COMPRESSED_ROW_SPARSE_MATRIX_H_ +#define CERES_INTERNAL_COMPRESSED_ROW_SPARSE_MATRIX_H_ + +#include <glog/logging.h> +#include "ceres/sparse_matrix.h" +#include "ceres/triplet_sparse_matrix.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/macros.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +class SparseMatrixProto; + +class CompressedRowSparseMatrix : public SparseMatrix { + public: + // Build a matrix with the same content as the TripletSparseMatrix + // m. TripletSparseMatrix objects are easier to construct + // incrementally, so we use them to initialize SparseMatrix + // objects. + // + // We assume that m does not have any repeated entries. + explicit CompressedRowSparseMatrix(const TripletSparseMatrix& m); +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS + explicit CompressedRowSparseMatrix(const SparseMatrixProto& proto); +#endif + + // Use this constructor only if you know what you are doing. This + // creates a "blank" matrix with the appropriate amount of memory + // allocated. However, the object itself is in an inconsistent state + // as the rows and cols matrices do not match the values of + // num_rows, num_cols and max_num_nonzeros. + // + // The use case for this constructor is that when the user knows the + // size of the matrix to begin with and wants to update the layout + // manually, instead of going via the indirect route of first + // constructing a TripletSparseMatrix, which leads to more than + // double the peak memory usage. + CompressedRowSparseMatrix(int num_rows, + int num_cols, + int max_num_nonzeros); + + // Build a square sparse diagonal matrix with num_rows rows and + // columns. The diagonal m(i,i) = diagonal(i); + CompressedRowSparseMatrix(const double* diagonal, int num_rows); + + virtual ~CompressedRowSparseMatrix(); + + // SparseMatrix interface. + virtual void SetZero(); + virtual void RightMultiply(const double* x, double* y) const; + virtual void LeftMultiply(const double* x, double* y) const; + virtual void SquaredColumnNorm(double* x) const; + virtual void ScaleColumns(const double* scale); + + virtual void ToDenseMatrix(Matrix* dense_matrix) const; +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS + virtual void ToProto(SparseMatrixProto* proto) const; +#endif + virtual void ToTextFile(FILE* file) const; + virtual int num_rows() const { return num_rows_; } + virtual int num_cols() const { return num_cols_; } + virtual int num_nonzeros() const { return rows_[num_rows_]; } + virtual const double* values() const { return values_.get(); } + virtual double* mutable_values() { return values_.get(); } + + // Delete the bottom delta_rows. + // num_rows -= delta_rows + void DeleteRows(int delta_rows); + + // Append the contents of m to the bottom of this matrix. m must + // have the same number of columns as this matrix. + void AppendRows(const CompressedRowSparseMatrix& m); + + // Low level access methods that expose the structure of the matrix. + const int* cols() const { return cols_.get(); } + int* mutable_cols() { return cols_.get(); } + + const int* rows() const { return rows_.get(); } + int* mutable_rows() { return rows_.get(); } + + private: + scoped_array<int> cols_; + scoped_array<int> rows_; + scoped_array<double> values_; + + int num_rows_; + int num_cols_; + + int max_num_nonzeros_; + + DISALLOW_COPY_AND_ASSIGN(CompressedRowSparseMatrix); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_COMPRESSED_ROW_SPARSE_MATRIX_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/conditioned_cost_function.cc b/extern/libmv/third_party/ceres/internal/ceres/conditioned_cost_function.cc new file mode 100644 index 00000000000..ca80bfb9c9d --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/conditioned_cost_function.cc @@ -0,0 +1,130 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 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: wjr@google.com (William Rucklidge) +// +// This file contains the implementation of the conditioned cost function. + +#include "ceres/conditioned_cost_function.h" + +#include <cstddef> + +#include <glog/logging.h> +#include "ceres/stl_util.h" +#include "ceres/internal/eigen.h" +#include "ceres/types.h" + +namespace ceres { + +// This cost function has the same dimensions (parameters, residuals) as +// the one it's wrapping. +ConditionedCostFunction::ConditionedCostFunction( + CostFunction* wrapped_cost_function, + const vector<CostFunction*>& conditioners, + Ownership ownership) + : wrapped_cost_function_(wrapped_cost_function), + conditioners_(conditioners), + ownership_(ownership) { + // Set up our dimensions. + set_num_residuals(wrapped_cost_function_->num_residuals()); + *mutable_parameter_block_sizes() = + wrapped_cost_function_->parameter_block_sizes(); + + // Sanity-check the conditioners' dimensions. + CHECK_EQ(wrapped_cost_function_->num_residuals(), conditioners_.size()); + for (int i = 0; i < wrapped_cost_function_->num_residuals(); i++) { + if (conditioners[i]) { + CHECK_EQ(1, conditioners[i]->num_residuals()); + CHECK_EQ(1, conditioners[i]->parameter_block_sizes().size()); + CHECK_EQ(1, conditioners[i]->parameter_block_sizes()[0]); + } + } +} + +ConditionedCostFunction::~ConditionedCostFunction() { + if (ownership_ == TAKE_OWNERSHIP) { + STLDeleteElements(&conditioners_); + } else { + wrapped_cost_function_.release(); + } +} + +bool ConditionedCostFunction::Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const { + bool success = wrapped_cost_function_->Evaluate(parameters, residuals, + jacobians); + if (!success) { + return false; + } + + for (int r = 0; r < wrapped_cost_function_->num_residuals(); r++) { + // On output, we want to have + // residuals[r] = conditioners[r](wrapped_residuals[r]) + // For parameter block i, column c, + // jacobians[i][r*parameter_block_size_[i] + c] = + // = d residual[r] / d parameters[i][c] + // = conditioners[r]'(wrapped_residuals[r]) * + // d wrapped_residuals[r] / d parameters[i][c] + if (conditioners_[r]) { + double conditioner_derivative; + double* conditioner_derivative_pointer = &conditioner_derivative; + double** conditioner_derivative_pointer2 = + &conditioner_derivative_pointer; + if (!jacobians) { + conditioner_derivative_pointer2 = NULL; + } + + double unconditioned_residual = residuals[r]; + double* parameter_pointer = &unconditioned_residual; + success = conditioners_[r]->Evaluate(¶meter_pointer, + &residuals[r], + conditioner_derivative_pointer2); + if (!success) { + return false; + } + + if (jacobians) { + for (int i = 0; + i < wrapped_cost_function_->parameter_block_sizes().size(); + i++) { + if (jacobians[i]) { + int parameter_block_size = + wrapped_cost_function_->parameter_block_sizes()[i]; + VectorRef jacobian_row(jacobians[i] + r * parameter_block_size, + parameter_block_size, 1); + jacobian_row *= conditioner_derivative; + } + } + } + } + } + return true; +} + +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/conjugate_gradients_solver.cc b/extern/libmv/third_party/ceres/internal/ceres/conjugate_gradients_solver.cc new file mode 100644 index 00000000000..75f9e043fa5 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/conjugate_gradients_solver.cc @@ -0,0 +1,233 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// A preconditioned conjugate gradients solver +// (ConjugateGradientsSolver) for positive semidefinite linear +// systems. +// +// We have also augmented the termination criterion used by this +// solver to support not just residual based termination but also +// termination based on decrease in the value of the quadratic model +// that CG optimizes. + +#include "ceres/conjugate_gradients_solver.h" + +#include <cmath> +#include <cstddef> +#include <glog/logging.h> +#include "ceres/linear_operator.h" +#include "ceres/internal/eigen.h" +#include "ceres/types.h" +#include "ceres/jet.h" + +namespace ceres { +namespace internal { +namespace { + +bool IsZeroOrInfinity(double x) { + return ((x == 0.0) || (isinf(x))); +} + +// Constant used in the MATLAB implementation ~ 2 * eps. +const double kEpsilon = 2.2204e-16; + +} // namespace + +ConjugateGradientsSolver::ConjugateGradientsSolver( + const LinearSolver::Options& options) + : options_(options) { +} + +LinearSolver::Summary ConjugateGradientsSolver::Solve( + LinearOperator* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x) { + CHECK_NOTNULL(A); + CHECK_NOTNULL(x); + CHECK_NOTNULL(b); + CHECK_EQ(A->num_rows(), A->num_cols()); + + LinearSolver::Summary summary; + summary.termination_type = MAX_ITERATIONS; + summary.num_iterations = 0; + + int num_cols = A->num_cols(); + VectorRef xref(x, num_cols); + ConstVectorRef bref(b, num_cols); + + double norm_b = bref.norm(); + if (norm_b == 0.0) { + xref.setZero(); + summary.termination_type = TOLERANCE; + return summary; + } + + Vector r(num_cols); + Vector p(num_cols); + Vector z(num_cols); + Vector tmp(num_cols); + + double tol_r = per_solve_options.r_tolerance * norm_b; + + tmp.setZero(); + A->RightMultiply(x, tmp.data()); + r = bref - tmp; + double norm_r = r.norm(); + + if (norm_r <= tol_r) { + summary.termination_type = TOLERANCE; + return summary; + } + + double rho = 1.0; + + // Initial value of the quadratic model Q = x'Ax - 2 * b'x. + double Q0 = -1.0 * xref.dot(bref + r); + + for (summary.num_iterations = 1; + summary.num_iterations < options_.max_num_iterations; + ++summary.num_iterations) { + VLOG(2) << "cg iteration " << summary.num_iterations; + + // Apply preconditioner + if (per_solve_options.preconditioner != NULL) { + z.setZero(); + per_solve_options.preconditioner->RightMultiply(r.data(), z.data()); + } else { + z = r; + } + + double last_rho = rho; + rho = r.dot(z); + + if (IsZeroOrInfinity(rho)) { + LOG(ERROR) << "Numerical failure. rho = " << rho; + summary.termination_type = FAILURE; + break; + }; + + if (summary.num_iterations == 1) { + p = z; + } else { + double beta = rho / last_rho; + if (IsZeroOrInfinity(beta)) { + LOG(ERROR) << "Numerical failure. beta = " << beta; + summary.termination_type = FAILURE; + break; + } + p = z + beta * p; + } + + Vector& q = z; + q.setZero(); + A->RightMultiply(p.data(), q.data()); + double pq = p.dot(q); + + if ((pq <= 0) || isinf(pq)) { + LOG(ERROR) << "Numerical failure. pq = " << pq; + summary.termination_type = FAILURE; + break; + } + + double alpha = rho / pq; + if (isinf(alpha)) { + LOG(ERROR) << "Numerical failure. alpha " << alpha; + summary.termination_type = FAILURE; + break; + } + + xref = xref + alpha * p; + + // Ideally we would just use the update r = r - alpha*q to keep + // track of the residual vector. However this estimate tends to + // drift over time due to round off errors. Thus every + // residual_reset_period iterations, we calculate the residual as + // r = b - Ax. We do not do this every iteration because this + // requires an additional matrix vector multiply which would + // double the complexity of the CG algorithm. + if (summary.num_iterations % options_.residual_reset_period == 0) { + tmp.setZero(); + A->RightMultiply(x, tmp.data()); + r = bref - tmp; + } else { + r = r - alpha * q; + } + + // Quadratic model based termination. + // Q1 = x'Ax - 2 * b' x. + double Q1 = -1.0 * xref.dot(bref + r); + + // For PSD matrices A, let + // + // Q(x) = x'Ax - 2b'x + // + // be the cost of the quadratic function defined by A and b. Then, + // the solver terminates at iteration i if + // + // i * (Q(x_i) - Q(x_i-1)) / Q(x_i) < q_tolerance. + // + // This termination criterion is more useful when using CG to + // solve the Newton step. This particular convergence test comes + // from Stephen Nash's work on truncated Newton + // methods. References: + // + // 1. Stephen G. Nash & Ariela Sofer, Assessing A Search + // Direction Within A Truncated Newton Method, Operation + // Research Letters 9(1990) 219-221. + // + // 2. Stephen G. Nash, A Survey of Truncated Newton Methods, + // Journal of Computational and Applied Mathematics, + // 124(1-2), 45-59, 2000. + // + double zeta = summary.num_iterations * (Q1 - Q0) / Q1; + VLOG(2) << "Q termination: zeta " << zeta + << " " << per_solve_options.q_tolerance; + if (zeta < per_solve_options.q_tolerance) { + summary.termination_type = TOLERANCE; + break; + } + Q0 = Q1; + + // Residual based termination. + norm_r = r. norm(); + VLOG(2) << "R termination: norm_r " << norm_r + << " " << tol_r; + if (norm_r <= tol_r) { + summary.termination_type = TOLERANCE; + break; + } + } + + return summary; +}; + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/conjugate_gradients_solver.h b/extern/libmv/third_party/ceres/internal/ceres/conjugate_gradients_solver.h new file mode 100644 index 00000000000..57f99e31db7 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/conjugate_gradients_solver.h @@ -0,0 +1,74 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Preconditioned Conjugate Gradients based solver for positive +// semidefinite linear systems. + +#ifndef CERES_INTERNAL_CONJUGATE_GRADIENTS_SOLVER_H_ +#define CERES_INTERNAL_CONJUGATE_GRADIENTS_SOLVER_H_ + +#include "ceres/linear_solver.h" +#include "ceres/internal/macros.h" + +namespace ceres { +namespace internal { + +class LinearOperator; + +// This class implements the now classical Conjugate Gradients +// algorithm of Hestenes & Stiefel for solving postive semidefinite +// linear sytems. Optionally it can use a preconditioner also to +// reduce the condition number of the linear system and improve the +// convergence rate. Modern references for Conjugate Gradients are the +// books by Yousef Saad and Trefethen & Bau. This implementation of CG +// has been augmented with additional termination tests that are +// needed for forcing early termination when used as part of an +// inexact Newton solver. +// +// For more details see the documentation for +// LinearSolver::PerSolveOptions::r_tolerance and +// LinearSolver::PerSolveOptions::q_tolerance in linear_solver.h. +class ConjugateGradientsSolver : public LinearSolver { + public: + explicit ConjugateGradientsSolver(const LinearSolver::Options& options); + virtual Summary Solve(LinearOperator* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x); + + private: + const LinearSolver::Options options_; + DISALLOW_COPY_AND_ASSIGN(ConjugateGradientsSolver); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_CONJUGATE_GRADIENTS_SOLVER_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/corrector.cc b/extern/libmv/third_party/ceres/internal/ceres/corrector.cc new file mode 100644 index 00000000000..4ca2c6f6c86 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/corrector.cc @@ -0,0 +1,125 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/corrector.h" + +#include <cstddef> +#include <cmath> +#include <glog/logging.h> +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +Corrector::Corrector(double sq_norm, const double rho[3]) { + CHECK_GE(sq_norm, 0.0); + CHECK_GT(rho[1], 0.0); + sqrt_rho1_ = sqrt(rho[1]); + + // If sq_norm = 0.0, the correction becomes trivial, the residual + // and the jacobian are scaled by the squareroot of the derivative + // of rho. Handling this case explicitly avoids the divide by zero + // error that would occur below. + // + // The case where rho'' < 0 also gets special handling. Technically + // it shouldn't, and the computation of the scaling should proceed + // as below, however we found in experiments that applying the + // curvature correction when rho'' < 0, which is the case when we + // are in the outlier region slows down the convergence of the + // algorithm significantly. + // + // Thus, we have divided the action of the robustifier into two + // parts. In the inliner region, we do the full second order + // correction which re-wights the gradient of the function by the + // square root of the derivative of rho, and the Gauss-Newton + // Hessian gets both the scaling and the rank-1 curvature + // correction. Normaly, alpha is upper bounded by one, but with this + // change, alpha is bounded above by zero. + // + // Empirically we have observed that the full Triggs correction and + // the clamped correction both start out as very good approximations + // to the loss function when we are in the convex part of the + // function, but as the function starts transitioning from convex to + // concave, the Triggs approximation diverges more and more and + // ultimately becomes linear. The clamped Triggs model however + // remains quadratic. + // + // The reason why the Triggs approximation becomes so poor is + // because the curvature correction that it applies to the gauss + // newton hessian goes from being a full rank correction to a rank + // deficient correction making the inversion of the Hessian fraught + // with all sorts of misery and suffering. + // + // The clamped correction retains its quadratic nature and inverting it + // is always well formed. + if ((sq_norm == 0.0) || (rho[2] <= 0.0)) { + residual_scaling_ = sqrt_rho1_; + alpha_sq_norm_ = 0.0; + return; + } + + // Calculate the smaller of the two solutions to the equation + // + // 0.5 * alpha^2 - alpha - rho'' / rho' * z'z = 0. + // + // Start by calculating the discriminant D. + const double D = 1.0 + 2.0 * sq_norm*rho[2] / rho[1]; + + // Since both rho[1] and rho[2] are guaranteed to be positive at + // this point, we know that D > 1.0. + + const double alpha = 1.0 - sqrt(D); + + // Calculate the constants needed by the correction routines. + residual_scaling_ = sqrt_rho1_ / (1 - alpha); + alpha_sq_norm_ = alpha / sq_norm; +} + +void Corrector::CorrectResiduals(int nrow, double* residuals) { + DCHECK(residuals != NULL); + VectorRef r_ref(residuals, nrow); + // Equation 11 in BANS. + r_ref *= residual_scaling_; +} + +void Corrector::CorrectJacobian(int nrow, int ncol, + double* residuals, double* jacobian) { + DCHECK(residuals != NULL); + DCHECK(jacobian != NULL); + ConstVectorRef r_ref(residuals, nrow); + MatrixRef j_ref(jacobian, nrow, ncol); + + // Equation 11 in BANS. + j_ref = sqrt_rho1_ * (j_ref - alpha_sq_norm_ * + r_ref * (r_ref.transpose() * j_ref)); +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/corrector.h b/extern/libmv/third_party/ceres/internal/ceres/corrector.h new file mode 100644 index 00000000000..9914641cb01 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/corrector.h @@ -0,0 +1,88 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Class definition for the object that is responsible for applying a +// second order correction to the Gauss-Newton based on the ideas in +// BANS by Triggs et al. + +#ifndef CERES_INTERNAL_CORRECTOR_H_ +#define CERES_INTERNAL_CORRECTOR_H_ + +namespace ceres { +namespace internal { + +// Corrector is responsible for applying the second order correction +// to the residual and jacobian of a least squares problem based on a +// radial robust loss. +// +// The key idea here is to look at the expressions for the robustified +// gauss newton approximation and then take its squareroot to get the +// corresponding corrections to the residual and jacobian. For the +// full expressions see Eq. 10 and 11 in BANS by Triggs et al. +class Corrector { + public: + // The constructor takes the squared norm, the value, the first and + // second derivatives of the LossFunction. It precalculates some of + // the constants that are needed to apply the correction. The + // correction constant alpha is constrained to be smaller than 1, if + // it becomes larger than 1, then it will reverse the sign of the + // residual and the correction. If alpha is equal to 1 will result + // in a divide by zero error. Thus we constrain alpha to be upper + // bounded by 1 - epsilon_. + // + // rho[1] needs to be positive. The constructor will crash if this + // condition is not met. + // + // In practical use CorrectJacobian should always be called before + // CorrectResidual, because the jacobian correction depends on the + // value of the uncorrected residual values. + explicit Corrector(double sq_norm, const double rho[3]); + + // residuals *= sqrt(rho[1]) / (1 - alpha) + void CorrectResiduals(int nrow, double* residuals); + + // jacobian = sqrt(rho[1]) * jacobian - + // sqrt(rho[1]) * alpha / sq_norm * residuals residuals' * jacobian. + // + // The method assumes that the jacobian has row-major storage. It is + // the caller's responsibility to ensure that the pointer to + // jacobian is not null. + void CorrectJacobian(int nrow, int ncol, + double* residuals, double* jacobian); + + private: + double sqrt_rho1_; + double residual_scaling_; + double alpha_sq_norm_; +}; +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_CORRECTOR_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/dense_jacobian_writer.h b/extern/libmv/third_party/ceres/internal/ceres/dense_jacobian_writer.h new file mode 100644 index 00000000000..1177b83a556 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/dense_jacobian_writer.h @@ -0,0 +1,110 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 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: keir@google.com (Keir Mierle) +// +// A jacobian writer that writes to dense Eigen matrices. + +#ifndef CERES_INTERNAL_DENSE_JACOBIAN_WRITER_H_ +#define CERES_INTERNAL_DENSE_JACOBIAN_WRITER_H_ + +#include "ceres/casts.h" +#include "ceres/dense_sparse_matrix.h" +#include "ceres/parameter_block.h" +#include "ceres/program.h" +#include "ceres/residual_block.h" +#include "ceres/scratch_evaluate_preparer.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +class DenseJacobianWriter { + public: + DenseJacobianWriter(Evaluator::Options /* ignored */, + Program* program) + : program_(program) { + } + + // JacobianWriter interface. + + // Since the dense matrix has different layout than that assumed by the cost + // functions, use scratch space to store the jacobians temporarily then copy + // them over to the larger jacobian later. + ScratchEvaluatePreparer* CreateEvaluatePreparers(int num_threads) { + return ScratchEvaluatePreparer::Create(*program_, num_threads); + } + + SparseMatrix* CreateJacobian() const { + return new DenseSparseMatrix(program_->NumResiduals(), + program_->NumEffectiveParameters()); + } + + void Write(int residual_id, + int residual_offset, + double **jacobians, + SparseMatrix* jacobian) { + DenseSparseMatrix* dense_jacobian; + if (jacobian != NULL) { + dense_jacobian = down_cast<DenseSparseMatrix*>(jacobian); + } + const ResidualBlock* residual_block = + program_->residual_blocks()[residual_id]; + int num_parameter_blocks = residual_block->NumParameterBlocks(); + int num_residuals = residual_block->NumResiduals(); + + // Now copy the jacobians for each parameter into the dense jacobian matrix. + for (int j = 0; j < num_parameter_blocks; ++j) { + ParameterBlock* parameter_block = residual_block->parameter_blocks()[j]; + + // If the parameter block is fixed, then there is nothing to do. + if (parameter_block->IsConstant()) { + continue; + } + + int parameter_block_size = parameter_block->LocalSize(); + MatrixRef 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) = parameter_jacobian; + } + } + + private: + Program* program_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_DENSE_JACOBIAN_WRITER_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/dense_qr_solver.cc b/extern/libmv/third_party/ceres/internal/ceres/dense_qr_solver.cc new file mode 100644 index 00000000000..328505404d7 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/dense_qr_solver.cc @@ -0,0 +1,93 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/dense_qr_solver.h" + +#include <cstddef> + +#include "Eigen/Dense" +#include "ceres/linear_solver.h" +#include "ceres/triplet_sparse_matrix.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +DenseQRSolver::DenseQRSolver(const LinearSolver::Options& options) + : options_(options) {} + +LinearSolver::Summary DenseQRSolver::SolveImpl( + DenseSparseMatrix* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x) { + const int num_rows = A->num_rows(); + const int num_cols = A->num_cols(); + VLOG(2) << "DenseQRSolver: " + << num_rows << " x " << num_cols << " system."; + + 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. + Vector rhs(num_rows + ((per_solve_options.D !=NULL) ? num_cols : 0)); + rhs.setZero(); + rhs.head(num_rows) = ConstVectorRef(b, num_rows); + + // Solve the system. + VectorRef(x, num_cols) = A->matrix().colPivHouseholderQr().solve(rhs); + + VLOG(3) << "A:\n" << A->matrix(); + VLOG(3) << "x:\n" << VectorRef(x, num_cols); + VLOG(3) << "b:\n" << rhs; + VLOG(3) << "error: " << (A->matrix() * VectorRef(x, num_cols) - rhs).norm(); + + + 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 = TOLERANCE; + return summary; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/dense_qr_solver.h b/extern/libmv/third_party/ceres/internal/ceres/dense_qr_solver.h new file mode 100644 index 00000000000..990c8d445eb --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/dense_qr_solver.h @@ -0,0 +1,99 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Solve dense rectangular systems Ax = b using the QR factoriztion. +#ifndef CERES_INTERNAL_DENSE_QR_SOLVER_H_ +#define CERES_INTERNAL_DENSE_QR_SOLVER_H_ + +#include "ceres/linear_solver.h" +#include "ceres/internal/macros.h" + +namespace ceres { +namespace internal { + +class DenseSparseMatrix; + +// This class implements the LinearSolver interface for solving +// rectangular/unsymmetric (well constrained) linear systems of the +// form +// +// Ax = b +// +// Since there does not usually exist a solution that satisfies these +// equations, the solver instead solves the linear least squares +// problem +// +// min_x |Ax - b|^2 +// +// The solution strategy is based on computing the QR decomposition of +// A, i.e. +// +// A = QR +// +// Where Q is an orthonormal matrix and R is an upper triangular +// matrix. Then +// +// Ax = b +// QRx = b +// Q'QRx = Q'b +// Rx = Q'b +// x = R^{-1} Q'b +// +// If the PerSolveOptions struct has a non-null array D, then the +// augmented/regularized linear system +// +// [ A ]x = [b] +// [ diag(D) ] [0] +// +// is solved. +// +// This class uses the dense QR factorization routines from the Eigen +// library. This solver always returns a solution, it is the user's +// responsibility to judge if the solution is good enough for their +// purposes. +class DenseQRSolver: public DenseSparseMatrixSolver { + public: + explicit DenseQRSolver(const LinearSolver::Options& options); + + private: + virtual LinearSolver::Summary SolveImpl( + DenseSparseMatrix* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x); + + const LinearSolver::Options options_; + DISALLOW_COPY_AND_ASSIGN(DenseQRSolver); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_DENSE_QR_SOLVER_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/dense_sparse_matrix.cc b/extern/libmv/third_party/ceres/internal/ceres/dense_sparse_matrix.cc new file mode 100644 index 00000000000..5d392ba6c3b --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/dense_sparse_matrix.cc @@ -0,0 +1,197 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 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: keir@google.com (Keir Mierle) + +#include "ceres/dense_sparse_matrix.h" + +#include <algorithm> +#include "ceres/matrix_proto.h" +#include "ceres/triplet_sparse_matrix.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/port.h" + +namespace ceres { +namespace internal { + +DenseSparseMatrix::DenseSparseMatrix(int num_rows, int num_cols) + : has_diagonal_appended_(false), + has_diagonal_reserved_(false) { + // Allocate enough space for the diagonal. + m_.resize(num_rows, num_cols); + m_.setZero(); +} + +DenseSparseMatrix::DenseSparseMatrix(const TripletSparseMatrix& m) + : m_(Eigen::MatrixXd::Zero(m.num_rows(), m.num_cols())), + has_diagonal_appended_(false), + has_diagonal_reserved_(false) { + const double *values = m.values(); + const int *rows = m.rows(); + const int *cols = m.cols(); + int num_nonzeros = m.num_nonzeros(); + + for (int i = 0; i < num_nonzeros; ++i) { + m_(rows[i], cols[i]) += values[i]; + } +} + +DenseSparseMatrix::DenseSparseMatrix(const Matrix& m) + : m_(m), + has_diagonal_appended_(false), + has_diagonal_reserved_(false) { +} + +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS +DenseSparseMatrix::DenseSparseMatrix(const SparseMatrixProto& outer_proto) + : m_(Eigen::MatrixXd::Zero( + outer_proto.dense_matrix().num_rows(), + outer_proto.dense_matrix().num_cols())), + has_diagonal_appended_(false), + has_diagonal_reserved_(false) { + const DenseSparseMatrixProto& proto = outer_proto.dense_matrix(); + for (int i = 0; i < m_.rows(); ++i) { + for (int j = 0; j < m_.cols(); ++j) { + m_(i, j) = proto.values(m_.cols() * i + j); + } + } +} +#endif + +void DenseSparseMatrix::SetZero() { + m_.setZero(); +} + +void DenseSparseMatrix::RightMultiply(const double* x, double* y) const { + VectorRef(y, num_rows()) += matrix() * ConstVectorRef(x, num_cols()); +} + +void DenseSparseMatrix::LeftMultiply(const double* x, double* y) const { + VectorRef(y, num_cols()) += + matrix().transpose() * ConstVectorRef(x, num_rows()); +} + +void DenseSparseMatrix::SquaredColumnNorm(double* x) const { + VectorRef(x, num_cols()) = m_.colwise().squaredNorm(); +} + +void DenseSparseMatrix::ScaleColumns(const double* scale) { + m_ *= ConstVectorRef(scale, num_cols()).asDiagonal(); +} + +void DenseSparseMatrix::ToDenseMatrix(Matrix* dense_matrix) const { + *dense_matrix = m_; +} + +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS +void DenseSparseMatrix::ToProto(SparseMatrixProto* outer_proto) const { + CHECK(!has_diagonal_appended_) << "Not supported."; + outer_proto->Clear(); + DenseSparseMatrixProto* proto = outer_proto->mutable_dense_matrix(); + + proto->set_num_rows(num_rows()); + proto->set_num_cols(num_cols()); + + int num_nnz = num_nonzeros(); + for (int i = 0; i < num_nnz; ++i) { + proto->add_values(m_.data()[i]); + } +} +#endif + +void DenseSparseMatrix::AppendDiagonal(double *d) { + CHECK(!has_diagonal_appended_); + if (!has_diagonal_reserved_) { + Matrix 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_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(); +} + +ConstAlignedMatrixRef DenseSparseMatrix::matrix() const { + if (has_diagonal_reserved_ && !has_diagonal_appended_) { + return ConstAlignedMatrixRef( + m_.data(), m_.rows() - m_.cols(), m_.cols()); + } + return ConstAlignedMatrixRef(m_.data(), m_.rows(), m_.cols()); +} + +AlignedMatrixRef DenseSparseMatrix::mutable_matrix() { + if (has_diagonal_reserved_ && !has_diagonal_appended_) { + return AlignedMatrixRef( + m_.data(), m_.rows() - m_.cols(), m_.cols()); + } + return AlignedMatrixRef(m_.data(), m_.rows(), m_.cols()); +} + +void DenseSparseMatrix::ToTextFile(FILE* file) const { + CHECK_NOTNULL(file); + 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 c = 0; c < m_.cols(); ++c) { + fprintf(file, "% 10d % 10d %17f\n", r, c, m_(r, c)); + } + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/dense_sparse_matrix.h b/extern/libmv/third_party/ceres/internal/ceres/dense_sparse_matrix.h new file mode 100644 index 00000000000..416c2143c2c --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/dense_sparse_matrix.h @@ -0,0 +1,116 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 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: keir@google.com (Keir Mierle) +// +// A dense matrix implemented under the SparseMatrix interface. + +#ifndef CERES_INTERNAL_DENSE_SPARSE_MATRIX_H_ +#define CERES_INTERNAL_DENSE_SPARSE_MATRIX_H_ + +#include <glog/logging.h> +#include "ceres/sparse_matrix.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/macros.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +class SparseMatrixProto; +class TripletSparseMatrix; + +class DenseSparseMatrix : public SparseMatrix { + public: + // 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 Matrix& m); +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS + explicit DenseSparseMatrix(const SparseMatrixProto& proto); +#endif + + DenseSparseMatrix(int num_rows, int num_cols); + + virtual ~DenseSparseMatrix() {} + + // SparseMatrix interface. + virtual void SetZero(); + virtual void RightMultiply(const double* x, double* y) const; + virtual void LeftMultiply(const double* x, double* y) const; + virtual void SquaredColumnNorm(double* x) const; + virtual void ScaleColumns(const double* scale); + virtual void ToDenseMatrix(Matrix* dense_matrix) const; +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS + virtual void ToProto(SparseMatrixProto* proto) const; +#endif + virtual void ToTextFile(FILE* file) const; + virtual int num_rows() const; + virtual int num_cols() const; + virtual int num_nonzeros() const; + virtual const double* values() const { return m_.data(); } + virtual double* mutable_values() { return m_.data(); } + + ConstAlignedMatrixRef matrix() const; + AlignedMatrixRef 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(); + + private: + Matrix m_; + bool has_diagonal_appended_; + bool has_diagonal_reserved_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_DENSE_SPARSE_MATRIX_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/detect_structure.cc b/extern/libmv/third_party/ceres/internal/ceres/detect_structure.cc new file mode 100644 index 00000000000..e9755043bab --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/detect_structure.cc @@ -0,0 +1,114 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include <glog/logging.h> +#include "ceres/detect_structure.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +void DetectStructure(const CompressedRowBlockStructure& bs, + const int num_eliminate_blocks, + int* row_block_size, + int* e_block_size, + int* f_block_size) { + const int num_row_blocks = bs.rows.size(); + *row_block_size = 0; + *e_block_size = 0; + *f_block_size = 0; + + // Iterate over row blocks of the matrix, checking if row_block, + // e_block or f_block sizes remain constant. + for (int r = 0; r < num_row_blocks; ++r) { + const CompressedRow& row = bs.rows[r]; + // We do not care about the sizes of the blocks in rows which do + // not contain e_blocks. + if (row.cells.front().block_id >= num_eliminate_blocks) { + break; + } + const int e_block_id = row.cells.front().block_id; + + if (*row_block_size == 0) { + *row_block_size = row.block.size; + } else if (*row_block_size != Eigen::Dynamic && + *row_block_size != row.block.size) { + *row_block_size = Eigen::Dynamic; + VLOG(2) << "Dynamic row block size because the block size changed from " + << *row_block_size << " to " + << row.block.size; + } + + + if (*e_block_size == 0) { + *e_block_size = bs.cols[e_block_id].size; + } else if (*e_block_size != Eigen::Dynamic && + *e_block_size != bs.cols[e_block_id].size) { + *e_block_size = Eigen::Dynamic; + VLOG(2) << "Dynamic e block size because the block size changed from " + << *e_block_size << " to " + << bs.cols[e_block_id].size; + } + + if (*f_block_size == 0) { + if (row.cells.size() > 1) { + const int f_block_id = row.cells[1].block_id; + *f_block_size = bs.cols[f_block_id].size; + } + } else if (*f_block_size != Eigen::Dynamic) { + for (int c = 1; c < row.cells.size(); ++c) { + if (*f_block_size != bs.cols[row.cells[c].block_id].size) { + *f_block_size = Eigen::Dynamic; + VLOG(2) << "Dynamic f block size because the block size " + << "changed from " << *f_block_size << " to " + << bs.cols[row.cells[c].block_id].size; + break; + } + } + } + + const bool is_everything_dynamic = (*row_block_size == Eigen::Dynamic && + *e_block_size == Eigen::Dynamic && + *f_block_size == Eigen::Dynamic); + if (is_everything_dynamic) { + break; + } + } + + CHECK_NE(*row_block_size, 0) << "No rows found"; + CHECK_NE(*e_block_size, 0) << "No e type blocks found"; + VLOG(1) << "Schur complement static structure <" + << *row_block_size << "," + << *e_block_size << "," + << *f_block_size << ">."; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/detect_structure.h b/extern/libmv/third_party/ceres/internal/ceres/detect_structure.h new file mode 100644 index 00000000000..8af4f236690 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/detect_structure.h @@ -0,0 +1,63 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_DETECT_STRUCTURE_H_ +#define CERES_INTERNAL_DETECT_STRUCTURE_H_ + +#include "ceres/block_structure.h" + +namespace ceres { +namespace internal { + +// Detect static blocks in the problem sparsity. For rows containing +// e_blocks, we are interested in detecting if the size of the row +// blocks, e_blocks and the f_blocks remain constant. If they do, then +// we can use template specialization to improve the performance of +// the block level linear algebra operations used by the +// SchurEliminator. +// +// If a block size is not constant, we return Eigen::Dynamic as the +// value. This just means that the eliminator uses dynamically sized +// linear algebra operations rather than static operations whose size +// is known as compile time. +// +// For more details about e_blocks and f_blocks, see +// schur_complement.h. This information is used to initialized an +// appropriate template specialization of SchurEliminator. +void DetectStructure(const CompressedRowBlockStructure& bs, + const int num_eliminate_blocks, + int* row_block_size, + int* e_block_size, + int* f_block_size); + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_DETECT_STRUCTURE_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/evaluator.cc b/extern/libmv/third_party/ceres/internal/ceres/evaluator.cc new file mode 100644 index 00000000000..ea05aefec8c --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/evaluator.cc @@ -0,0 +1,71 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 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: keir@google.com (Keir Mierle) + +#include <glog/logging.h> +#include "ceres/evaluator.h" +#include "ceres/block_evaluate_preparer.h" +#include "ceres/block_jacobian_writer.h" +#include "ceres/compressed_row_jacobian_writer.h" +#include "ceres/scratch_evaluate_preparer.h" +#include "ceres/dense_jacobian_writer.h" +#include "ceres/program_evaluator.h" + +namespace ceres { +namespace internal { + +Evaluator::~Evaluator() {} + +Evaluator* Evaluator::Create(const Evaluator::Options& options, + Program* program, + string* error) { + switch (options.linear_solver_type) { + case DENSE_QR: + return new ProgramEvaluator<ScratchEvaluatePreparer, + DenseJacobianWriter>(options, + program); + case DENSE_SCHUR: + case SPARSE_SCHUR: + case ITERATIVE_SCHUR: + case CGNR: + return new ProgramEvaluator<BlockEvaluatePreparer, + BlockJacobianWriter>(options, + program); + case SPARSE_NORMAL_CHOLESKY: + return new ProgramEvaluator<ScratchEvaluatePreparer, + CompressedRowJacobianWriter>(options, + program); + default: + *error = "Invalid Linear Solver Type. Unable to create evaluator."; + return NULL; + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/evaluator.h b/extern/libmv/third_party/ceres/internal/ceres/evaluator.h new file mode 100644 index 00000000000..adefdd26660 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/evaluator.h @@ -0,0 +1,129 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// keir@google.com (Keir Mierle) + +#ifndef CERES_INTERNAL_EVALUATOR_H_ +#define CERES_INTERNAL_EVALUATOR_H_ + +#include <string> +#include "ceres/internal/port.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +class Program; +class SparseMatrix; + +// The Evaluator interface offers a way to interact with a least squares cost +// function that is useful for an optimizer that wants to minimize the least +// squares objective. This insulates the optimizer from issues like Jacobian +// storage, parameterization, etc. +class Evaluator { + public: + virtual ~Evaluator(); + + struct Options { + Options() + : num_threads(1), + num_eliminate_blocks(-1), + linear_solver_type(DENSE_QR) {} + + int num_threads; + int num_eliminate_blocks; + LinearSolverType linear_solver_type; + }; + + static Evaluator* Create(const Options& options, + Program* program, + string* error); + + // Build and return a sparse matrix for storing and working with the Jacobian + // of the objective function. The jacobian has dimensions + // NumEffectiveParameters() by NumParameters(), and is typically extremely + // sparse. Since the sparsity pattern of the Jacobian remains constant over + // the lifetime of the optimization problem, this method is used to + // instantiate a SparseMatrix object with the appropriate sparsity structure + // (which can be an expensive operation) and then reused by the optimization + // algorithm and the various linear solvers. + // + // It is expected that the classes implementing this interface will be aware + // of their client's requirements for the kind of sparse matrix storage and + // layout that is needed for an efficient implementation. For example + // CompressedRowOptimizationProblem creates a compressed row representation of + // the jacobian for use with CHOLMOD, where as BlockOptimizationProblem + // creates a BlockSparseMatrix representation of the jacobian for use in the + // Schur complement based methods. + virtual SparseMatrix* CreateJacobian() const = 0; + + // Evaluate the cost function for the given state. Returns the cost, + // residuals, and jacobian in the corresponding arguments. Both residuals and + // jacobian are optional; to avoid computing them, pass NULL. + // + // If non-NULL, the Jacobian must have a suitable sparsity pattern; only the + // values array of the jacobian is modified. + // + // state is an array of size NumParameters(), cost is a pointer to a single + // double, and residuals is an array of doubles of size NumResiduals(). + virtual bool Evaluate(const double* state, + double* cost, + double* residuals, + SparseMatrix* jacobian) = 0; + + // Make a change delta (of size NumEffectiveParameters()) to state (of size + // NumParameters()) and store the result in state_plus_delta. + // + // In the case that there are no parameterizations used, this is equivalent to + // + // state_plus_delta[i] = state[i] + delta[i] ; + // + // however, the mapping is more complicated in the case of parameterizations + // like quaternions. This is the same as the "Plus()" operation in + // local_parameterization.h, but operating over the entire state vector for a + // problem. + virtual bool Plus(const double* state, + const double* delta, + double* state_plus_delta) const = 0; + + // The number of parameters in the optimization problem. + virtual int NumParameters() const = 0; + + // This is the effective number of parameters that the optimizer may adjust. + // This applies when there are parameterizations on some of the parameters. + virtual int NumEffectiveParameters() const = 0; + + // The number of residuals in the optimization problem. + virtual int NumResiduals() const = 0; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_EVALUATOR_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/file.cc b/extern/libmv/third_party/ceres/internal/ceres/file.cc new file mode 100644 index 00000000000..5fc9d220861 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/file.cc @@ -0,0 +1,93 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 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: keir@google.com (Keir Mierle) +// +// Really simple file IO. + +#include <cstdio> +#include <glog/logging.h> + +namespace ceres { +namespace internal { + +using std::string; + +void WriteStringToFileOrDie(const string &data, const string &filename) { + FILE* file_descriptor = fopen(filename.c_str(), "wb"); + if (!file_descriptor) { + LOG(FATAL) << "Couldn't write to file: " << filename; + } + fwrite(data.c_str(), 1, data.size(), file_descriptor); + fclose(file_descriptor); +} + +void ReadFileToStringOrDie(const string &filename, string *data) { + FILE* file_descriptor = file_descriptor = fopen(filename.c_str(), "r"); + + if (!file_descriptor) { + LOG(FATAL) << "Couldn't read file: " << filename; + } + + // Resize the input buffer appropriately. + fseek(file_descriptor, 0L, SEEK_END); + int num_bytes = ftell(file_descriptor); + data->resize(num_bytes); + + // Read the data. + fseek(file_descriptor, 0L, SEEK_SET); + int num_read = fread(&((*data)[0]), + sizeof((*data)[0]), + num_bytes, + file_descriptor); + if (num_read != num_bytes) { + LOG(FATAL) << "Couldn't read all of " << filename + << "expected bytes: " << num_bytes * sizeof((*data)[0]) + << "actual bytes: " << num_read; + } + fclose(file_descriptor); +} + +string JoinPath(const string& dirname, const string& basename) { +#ifdef _WIN32 + static const char separator = '\\'; +#else + static const char separator = '/'; +#endif // _WIN32 + + if ((!basename.empty() && basename[0] == separator) || dirname.empty()) { + return basename; + } else if (dirname[dirname.size() - 1] == separator) { + return dirname + basename; + } else { + return dirname + string(&separator, 1) + basename; + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/file.h b/extern/libmv/third_party/ceres/internal/ceres/file.h new file mode 100644 index 00000000000..4741d650646 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/file.h @@ -0,0 +1,52 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 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: keir@google.com (Keir Mierle) +// +// Simple file IO support. This is a portability shim. + +#ifndef CERES_INTERNAL_FILE_H_ +#define CERES_INTERNAL_FILE_H_ + +#include <string> +#include "ceres/internal/port.h" + +namespace ceres { +namespace internal { + +void WriteStringToFileOrDie(const string &data, const string &filename); +void ReadFileToStringOrDie(const string &filename, string *data); + +// Join two path components, adding a slash if necessary. If basename is an +// absolute path then JoinPath ignores dirname and simply returns basename. +string JoinPath(const string& dirname, const string& basename); + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_FILE_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_2_2.cc b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_2_2.cc new file mode 100644 index 00000000000..5529386e485 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_2_2.cc @@ -0,0 +1,53 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specializations.py. +// Editing it manually is not recommended. + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<2, 2, 2>; + +} // namespace internal +} // namespace ceres + diff --git a/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_2_3.cc b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_2_3.cc new file mode 100644 index 00000000000..fd7af95192e --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_2_3.cc @@ -0,0 +1,53 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specializations.py. +// Editing it manually is not recommended. + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<2, 2, 3>; + +} // namespace internal +} // namespace ceres + diff --git a/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_2_4.cc b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_2_4.cc new file mode 100644 index 00000000000..109483e9fc0 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_2_4.cc @@ -0,0 +1,53 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specializations.py. +// Editing it manually is not recommended. + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<2, 2, 4>; + +} // namespace internal +} // namespace ceres + diff --git a/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_2_d.cc b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_2_d.cc new file mode 100644 index 00000000000..b93e82fe2fa --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_2_d.cc @@ -0,0 +1,53 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specializations.py. +// Editing it manually is not recommended. + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<2, 2, Dynamic>; + +} // namespace internal +} // namespace ceres + diff --git a/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_3_3.cc b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_3_3.cc new file mode 100644 index 00000000000..86352c07304 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_3_3.cc @@ -0,0 +1,53 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specializations.py. +// Editing it manually is not recommended. + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<2, 3, 3>; + +} // namespace internal +} // namespace ceres + diff --git a/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_3_4.cc b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_3_4.cc new file mode 100644 index 00000000000..200df7f5931 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_3_4.cc @@ -0,0 +1,53 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specializations.py. +// Editing it manually is not recommended. + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<2, 3, 4>; + +} // namespace internal +} // namespace ceres + diff --git a/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_3_9.cc b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_3_9.cc new file mode 100644 index 00000000000..1fda3434bef --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_3_9.cc @@ -0,0 +1,53 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specializations.py. +// Editing it manually is not recommended. + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<2, 3, 9>; + +} // namespace internal +} // namespace ceres + diff --git a/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_3_d.cc b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_3_d.cc new file mode 100644 index 00000000000..385cd2d70c9 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_3_d.cc @@ -0,0 +1,53 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specializations.py. +// Editing it manually is not recommended. + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<2, 3, Dynamic>; + +} // namespace internal +} // namespace ceres + diff --git a/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_4_3.cc b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_4_3.cc new file mode 100644 index 00000000000..7b15d6366ac --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_4_3.cc @@ -0,0 +1,53 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specializations.py. +// Editing it manually is not recommended. + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<2, 4, 3>; + +} // namespace internal +} // namespace ceres + diff --git a/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_4_4.cc b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_4_4.cc new file mode 100644 index 00000000000..29a610d743e --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_4_4.cc @@ -0,0 +1,53 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specializations.py. +// Editing it manually is not recommended. + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<2, 4, 4>; + +} // namespace internal +} // namespace ceres + diff --git a/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_4_d.cc b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_4_d.cc new file mode 100644 index 00000000000..a3bc4dc6f83 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_4_d.cc @@ -0,0 +1,53 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specializations.py. +// Editing it manually is not recommended. + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<2, 4, Dynamic>; + +} // namespace internal +} // namespace ceres + diff --git a/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_4_4_2.cc b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_4_4_2.cc new file mode 100644 index 00000000000..f71a4f62944 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_4_4_2.cc @@ -0,0 +1,53 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specializations.py. +// Editing it manually is not recommended. + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<4, 4, 2>; + +} // namespace internal +} // namespace ceres + diff --git a/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_4_4_3.cc b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_4_4_3.cc new file mode 100644 index 00000000000..52259fb1a67 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_4_4_3.cc @@ -0,0 +1,53 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specializations.py. +// Editing it manually is not recommended. + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<4, 4, 3>; + +} // namespace internal +} // namespace ceres + diff --git a/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_4_4_4.cc b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_4_4_4.cc new file mode 100644 index 00000000000..775424e6c8f --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_4_4_4.cc @@ -0,0 +1,53 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specializations.py. +// Editing it manually is not recommended. + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<4, 4, 4>; + +} // namespace internal +} // namespace ceres + diff --git a/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_4_4_d.cc b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_4_4_d.cc new file mode 100644 index 00000000000..97cde594059 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_4_4_d.cc @@ -0,0 +1,53 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specializations.py. +// Editing it manually is not recommended. + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<4, 4, Dynamic>; + +} // namespace internal +} // namespace ceres + diff --git a/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_d_d_d.cc b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_d_d_d.cc new file mode 100644 index 00000000000..4cba32e26c8 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_d_d_d.cc @@ -0,0 +1,53 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specializations.py. +// Editing it manually is not recommended. + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<Dynamic, Dynamic, Dynamic>; + +} // namespace internal +} // namespace ceres + diff --git a/extern/libmv/third_party/ceres/internal/ceres/gradient_checking_cost_function.cc b/extern/libmv/third_party/ceres/internal/ceres/gradient_checking_cost_function.cc new file mode 100644 index 00000000000..abba40824ef --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/gradient_checking_cost_function.cc @@ -0,0 +1,308 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 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: keir@google.com (Keir Mierle) + +#include "ceres/gradient_checking_cost_function.h" + +#include <algorithm> +#include <cmath> +#include <numeric> +#include <string> +#include <vector> + +#include <glog/logging.h> +#include "ceres/parameter_block.h" +#include "ceres/problem_impl.h" +#include "ceres/program.h" +#include "ceres/residual_block.h" +#include "ceres/runtime_numeric_diff_cost_function.h" +#include "ceres/stringprintf.h" +#include "ceres/cost_function.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/problem.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { +namespace { + +// True if x and y have an absolute relative difference less than +// relative_precision and false otherwise. Stores the relative and absolute +// difference in relative/absolute_error if non-NULL. +bool IsClose(double x, double y, double relative_precision, + double *relative_error, + double *absolute_error) { + double local_absolute_error; + double local_relative_error; + if (!absolute_error) { + absolute_error = &local_absolute_error; + } + if (!relative_error) { + relative_error = &local_relative_error; + } + *absolute_error = fabs(x - y); + *relative_error = *absolute_error / max(fabs(x), fabs(y)); + if (x == 0 || y == 0) { + // If x or y is exactly zero, then relative difference doesn't have any + // meaning. Take the absolute difference instead. + *relative_error = *absolute_error; + } + return fabs(*relative_error) < fabs(relative_precision); +} + +class GradientCheckingCostFunction : public CostFunction { + public: + GradientCheckingCostFunction(const CostFunction* function, + double relative_step_size, + double relative_precision, + const string& extra_info) + : function_(function), + finite_diff_cost_function_( + CreateRuntimeNumericDiffCostFunction(function, + CENTRAL, + relative_step_size)), + relative_precision_(relative_precision), + extra_info_(extra_info) { + *mutable_parameter_block_sizes() = function->parameter_block_sizes(); + set_num_residuals(function->num_residuals()); + } + + virtual ~GradientCheckingCostFunction() { } + + virtual bool Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const { + if (!jacobians) { + // Nothing to check in this case; just forward. + return function_->Evaluate(parameters, residuals, NULL); + } + + int num_residuals = function_->num_residuals(); + + // Make space for the jacobians of the two methods. + const vector<int16>& block_sizes = function_->parameter_block_sizes(); + vector<Matrix> term_jacobians(block_sizes.size()); + vector<Matrix> finite_difference_jacobians(block_sizes.size()); + vector<double*> term_jacobian_pointers(block_sizes.size()); + vector<double*> finite_difference_jacobian_pointers(block_sizes.size()); + for (int i = 0; i < block_sizes.size(); i++) { + term_jacobians[i].resize(num_residuals, block_sizes[i]); + term_jacobian_pointers[i] = term_jacobians[i].data(); + finite_difference_jacobians[i].resize(num_residuals, block_sizes[i]); + finite_difference_jacobian_pointers[i] = + finite_difference_jacobians[i].data(); + } + + // Evaluate the derivative using the user supplied code. + if (!function_->Evaluate(parameters, + residuals, + &term_jacobian_pointers[0])) { + LOG(WARNING) << "Function evaluation failed."; + return false; + } + + // Evaluate the derivative using numeric derivatives. + finite_diff_cost_function_->Evaluate( + parameters, + residuals, + &finite_difference_jacobian_pointers[0]); + + // See if any elements have relative error larger than the threshold. + int num_bad_jacobian_components = 0; + double worst_relative_error = 0; + + // Accumulate the error message for all the jacobians, since it won't get + // output if there are no bad jacobian components. + string m; + for (int k = 0; k < block_sizes.size(); k++) { + // Copy the original jacobian blocks into the jacobians array. + if (jacobians[k] != NULL) { + MatrixRef(jacobians[k], + term_jacobians[k].rows(), + term_jacobians[k].cols()) = term_jacobians[k]; + } + + StringAppendF(&m, + "========== " + "Jacobian for " "block %d: (%ld by %ld)) " + "==========\n", + k, + term_jacobians[k].rows(), + term_jacobians[k].cols()); + // The funny spacing creates appropriately aligned column headers. + m += " block row col user dx/dy num diff dx/dy " + "abs error relative error parameter residual\n"; + + for (int i = 0; i < term_jacobians[k].rows(); i++) { + for (int j = 0; j < term_jacobians[k].cols(); j++) { + double term_jacobian = term_jacobians[k](i, j); + double finite_jacobian = finite_difference_jacobians[k](i, j); + double relative_error, absolute_error; + bool bad_jacobian_entry = + !IsClose(term_jacobian, + finite_jacobian, + relative_precision_, + &relative_error, + &absolute_error); + worst_relative_error = std::max(worst_relative_error, + relative_error); + + StringAppendF(&m, "%6d %4d %4d %17g %17g %17g %17g %17g %17g", + k, i, j, + term_jacobian, finite_jacobian, + absolute_error, relative_error, + parameters[k][j], + residuals[i]); + + if (bad_jacobian_entry) { + num_bad_jacobian_components++; + StringAppendF( + &m, " ------ (%d,%d,%d) Relative error worse than %g", + k, i, j, relative_precision_); + } + m += "\n"; + } + } + } + + // Since there were some bad errors, dump comprehensive debug info. + if (num_bad_jacobian_components) { + string header = StringPrintf("Detected %d bad jacobian component(s). " + "Worst relative error was %g.\n", + num_bad_jacobian_components, + worst_relative_error); + if (!extra_info_.empty()) { + header += "Extra info for this residual: " + extra_info_ + "\n"; + } + LOG(WARNING) << "\n" << header << m; + } + return true; + } + + private: + const CostFunction* function_; + internal::scoped_ptr<CostFunction> finite_diff_cost_function_; + double relative_precision_; + string extra_info_; +}; + +} // namespace + +CostFunction *CreateGradientCheckingCostFunction( + const CostFunction *cost_function, + double relative_step_size, + double relative_precision, + const string& extra_info) { + return new GradientCheckingCostFunction(cost_function, + relative_step_size, + relative_precision, + extra_info); +} + +ProblemImpl* CreateGradientCheckingProblemImpl(ProblemImpl* problem_impl, + double relative_step_size, + double relative_precision) { + // We create new CostFunctions by wrapping the original CostFunction + // in a gradient checking CostFunction. So its okay for the + // ProblemImpl to take ownership of it and destroy it. The + // LossFunctions and LocalParameterizations are reused and since + // they are owned by problem_impl, gradient_checking_problem_impl + // should not take ownership of it. + Problem::Options gradient_checking_problem_options; + gradient_checking_problem_options.cost_function_ownership = TAKE_OWNERSHIP; + gradient_checking_problem_options.loss_function_ownership = + DO_NOT_TAKE_OWNERSHIP; + gradient_checking_problem_options.local_parameterization_ownership = + DO_NOT_TAKE_OWNERSHIP; + + ProblemImpl* gradient_checking_problem_impl = new ProblemImpl( + gradient_checking_problem_options); + + Program* program = problem_impl->mutable_program(); + + // For every ParameterBlock in problem_impl, create a new parameter + // block with the same local parameterization and constancy. + const vector<ParameterBlock*>& parameter_blocks = program->parameter_blocks(); + for (int i = 0; i < parameter_blocks.size(); ++i) { + ParameterBlock* parameter_block = parameter_blocks[i]; + gradient_checking_problem_impl->AddParameterBlock( + parameter_block->mutable_user_state(), + parameter_block->Size(), + parameter_block->mutable_local_parameterization()); + + if (parameter_block->IsConstant()) { + gradient_checking_problem_impl->SetParameterBlockConstant( + parameter_block->mutable_user_state()); + } + } + + // For every ResidualBlock in problem_impl, create a new + // ResidualBlock by wrapping its CostFunction inside a + // GradientCheckingCostFunction. + const vector<ResidualBlock*>& residual_blocks = program->residual_blocks(); + for (int i = 0; i < residual_blocks.size(); ++i) { + ResidualBlock* residual_block = residual_blocks[i]; + + // Build a human readable string which identifies the + // ResidualBlock. This is used by the GradientCheckingCostFunction + // when logging debugging information. + string extra_info = StringPrintf( + "Residual block id %d; depends on parameters [", i); + vector<double*> parameter_blocks; + for (int j = 0; j < residual_block->NumParameterBlocks(); ++j) { + ParameterBlock* parameter_block = residual_block->parameter_blocks()[j]; + parameter_blocks.push_back(parameter_block->mutable_user_state()); + StringAppendF(&extra_info, "%p", parameter_block->mutable_user_state()); + extra_info += (j < residual_block->NumParameterBlocks() - 1) ? ", " : "]"; + } + + // Wrap the original CostFunction in a GradientCheckingCostFunction. + CostFunction* gradient_checking_cost_function = + CreateGradientCheckingCostFunction(residual_block->cost_function(), + relative_step_size, + relative_precision, + extra_info); + + // The const_cast is necessary because + // ProblemImpl::AddResidualBlock can potentially take ownership of + // the LossFunction, but in this case we are guaranteed that this + // will not be the case, so this const_cast is harmless. + gradient_checking_problem_impl->AddResidualBlock( + gradient_checking_cost_function, + const_cast<LossFunction*>(residual_block->loss_function()), + parameter_blocks); + } + + return gradient_checking_problem_impl; +} + + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/gradient_checking_cost_function.h b/extern/libmv/third_party/ceres/internal/ceres/gradient_checking_cost_function.h new file mode 100644 index 00000000000..d49c8e6c244 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/gradient_checking_cost_function.h @@ -0,0 +1,85 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 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: keir@google.com (Keir Mierle) + +#ifndef CERES_INTERNAL_GRADIENT_CHECKING_COST_FUNCTION_H_ +#define CERES_INTERNAL_GRADIENT_CHECKING_COST_FUNCTION_H_ + +#include <string> + +#include "ceres/cost_function.h" + +namespace ceres { +namespace internal { + +class ProblemImpl; + +// Creates a CostFunction that checks the jacobians that cost_function computes +// with finite differences. Bad results are logged; required precision is +// controlled by relative_precision and the numeric differentiation step size is +// controlled with relative_step_size. See solver.h for a better explanation of +// relative_step_size. Caller owns result. +// +// The condition enforced is that +// +// (J_actual(i, j) - J_numeric(i, j)) +// ------------------------------------ < relative_precision +// max(J_actual(i, j), J_numeric(i, j)) +// +// where J_actual(i, j) is the jacobian as computed by the supplied cost +// function (by the user) and J_numeric is the jacobian as computed by finite +// differences. +// +// Note: This is quite inefficient and is intended only for debugging. +CostFunction* CreateGradientCheckingCostFunction( + const CostFunction* cost_function, + double relative_step_size, + double relative_precision, + const string& extra_info); + +// Create a new ProblemImpl object from the input problem_impl, where +// each CostFunctions in problem_impl are wrapped inside a +// GradientCheckingCostFunctions. This gives us a ProblemImpl object +// which checks its derivatives against estimates from numeric +// differentiation everytime a ResidualBlock is evaluated. +// +// relative_step_size and relative_precision are parameters to control +// the numeric differentiation and the relative tolerance between the +// jacobian computed by the CostFunctions in problem_impl and +// jacobians obtained by numerically differentiating them. For more +// details see the documentation for +// CreateGradientCheckingCostFunction above. +ProblemImpl* CreateGradientCheckingProblemImpl(ProblemImpl* problem_impl, + double relative_step_size, + double relative_precision); + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_GRADIENT_CHECKING_COST_FUNCTION_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/graph.h b/extern/libmv/third_party/ceres/internal/ceres/graph.h new file mode 100644 index 00000000000..fd7a224f0aa --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/graph.h @@ -0,0 +1,138 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_GRAPH_H_ +#define CERES_INTERNAL_GRAPH_H_ + +#include <limits> +#include <glog/logging.h> +#include "ceres/integral_types.h" +#include "ceres/map_util.h" +#include "ceres/collections_port.h" +#include "ceres/internal/macros.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +// A weighted undirected graph templated over the vertex ids. Vertex +// should be hashable and comparable. +template <typename Vertex> +class Graph { + public: + Graph() {} + + // Add a weighted vertex. If the vertex already exists in the graph, + // its weight is set to the new weight. + void AddVertex(const Vertex& vertex, double weight) { + if (vertices_.find(vertex) == vertices_.end()) { + vertices_.insert(vertex); + edges_[vertex] = HashSet<Vertex>(); + } + vertex_weights_[vertex] = weight; + } + + // Uses weight = 1.0. If vertex already exists, its weight is set to + // 1.0. + void AddVertex(const Vertex& vertex) { + AddVertex(vertex, 1.0); + } + + // Add a weighted edge between the vertex1 and vertex2. Calling + // AddEdge on a pair of vertices which do not exist in the graph yet + // will result in undefined behavior. + // + // It is legal to call this method repeatedly for the same set of + // vertices. + void AddEdge(const Vertex& vertex1, const Vertex& vertex2, double weight) { + DCHECK(vertices_.find(vertex1) != vertices_.end()); + DCHECK(vertices_.find(vertex2) != vertices_.end()); + + if (edges_[vertex1].insert(vertex2).second) { + edges_[vertex2].insert(vertex1); + } + + if (vertex1 < vertex2) { + edge_weights_[make_pair(vertex1, vertex2)] = weight; + } else { + edge_weights_[make_pair(vertex2, vertex1)] = weight; + } + } + + // Uses weight = 1.0. + void AddEdge(const Vertex& vertex1, const Vertex& vertex2) { + AddEdge(vertex1, vertex2, 1.0); + } + + // Calling VertexWeight on a vertex not in the graph will result in + // undefined behavior. + double VertexWeight(const Vertex& vertex) const { + return FindOrDie(vertex_weights_, vertex); + } + + // Calling EdgeWeight on a pair of vertices where either one of the + // vertices is not present in the graph will result in undefined + // behaviour. If there is no edge connecting vertex1 and vertex2, + // the edge weight is zero. + double EdgeWeight(const Vertex& vertex1, const Vertex& vertex2) const { + if (vertex1 < vertex2) { + return FindWithDefault(edge_weights_, make_pair(vertex1, vertex2), 0.0); + } else { + return FindWithDefault(edge_weights_, make_pair(vertex2, vertex1), 0.0); + } + } + + // Calling Neighbors on a vertex not in the graph will result in + // undefined behaviour. + const HashSet<Vertex>& Neighbors(const Vertex& vertex) const { + return FindOrDie(edges_, vertex); + } + + const HashSet<Vertex>& vertices() const { + return vertices_; + } + + static double InvalidWeight() { + return std::numeric_limits<double>::quiet_NaN(); + }; + + private: + HashSet<Vertex> vertices_; + HashMap<Vertex, double> vertex_weights_; + HashMap<Vertex, HashSet<Vertex> > edges_; + HashMap<pair<Vertex, Vertex>, double> edge_weights_; + + DISALLOW_COPY_AND_ASSIGN(Graph); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_GRAPH_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/graph_algorithms.h b/extern/libmv/third_party/ceres/internal/ceres/graph_algorithms.h new file mode 100644 index 00000000000..3b42d936336 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/graph_algorithms.h @@ -0,0 +1,270 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Various algorithms that operate on undirected graphs. + +#ifndef CERES_INTERNAL_GRAPH_ALGORITHMS_H_ +#define CERES_INTERNAL_GRAPH_ALGORITHMS_H_ + +#include <vector> +#include <glog/logging.h> +#include "ceres/collections_port.h" +#include "ceres/graph.h" + +namespace ceres { +namespace internal { + +// Compare two vertices of a graph by their degrees. +template <typename Vertex> +class VertexDegreeLessThan { + public: + explicit VertexDegreeLessThan(const Graph<Vertex>& graph) + : graph_(graph) {} + + bool operator()(const Vertex& lhs, const Vertex& rhs) const { + if (graph_.Neighbors(lhs).size() == graph_.Neighbors(rhs).size()) { + return lhs < rhs; + } + return graph_.Neighbors(lhs).size() < graph_.Neighbors(rhs).size(); + } + + private: + const Graph<Vertex>& graph_; +}; + +// Order the vertices of a graph using its (approximately) largest +// independent set, where an independent set of a graph is a set of +// vertices that have no edges connecting them. The maximum +// independent set problem is NP-Hard, but there are effective +// approximation algorithms available. The implementation here uses a +// breadth first search that explores the vertices in order of +// increasing degree. The same idea is used by Saad & Li in "MIQR: A +// multilevel incomplete QR preconditioner for large sparse +// least-squares problems", SIMAX, 2007. +// +// Given a undirected graph G(V,E), the algorithm is a greedy BFS +// search where the vertices are explored in increasing order of their +// degree. The output vector ordering contains elements of S in +// increasing order of their degree, followed by elements of V - S in +// increasing order of degree. The return value of the function is the +// cardinality of S. +template <typename Vertex> +int IndependentSetOrdering(const Graph<Vertex>& graph, + vector<Vertex>* ordering) { + const HashSet<Vertex>& vertices = graph.vertices(); + const int num_vertices = vertices.size(); + + CHECK_NOTNULL(ordering); + ordering->clear(); + ordering->reserve(num_vertices); + + // Colors for labeling the graph during the BFS. + const char kWhite = 0; + const char kGrey = 1; + const char kBlack = 2; + + // Mark all vertices white. + HashMap<Vertex, char> vertex_color; + vector<Vertex> vertex_queue; + for (typename HashSet<Vertex>::const_iterator it = vertices.begin(); + it != vertices.end(); + ++it) { + vertex_color[*it] = kWhite; + vertex_queue.push_back(*it); + } + + + sort(vertex_queue.begin(), vertex_queue.end(), + VertexDegreeLessThan<Vertex>(graph)); + + // Iterate over vertex_queue. Pick the first white vertex, add it + // to the independent set. Mark it black and its neighbors grey. + for (int i = 0; i < vertex_queue.size(); ++i) { + const Vertex& vertex = vertex_queue[i]; + if (vertex_color[vertex] != kWhite) { + continue; + } + + ordering->push_back(vertex); + vertex_color[vertex] = kBlack; + const HashSet<Vertex>& neighbors = graph.Neighbors(vertex); + for (typename HashSet<Vertex>::const_iterator it = neighbors.begin(); + it != neighbors.end(); + ++it) { + vertex_color[*it] = kGrey; + } + } + + int independent_set_size = ordering->size(); + + // Iterate over the vertices and add all the grey vertices to the + // ordering. At this stage there should only be black or grey + // vertices in the graph. + for (typename vector<Vertex>::const_iterator it = vertex_queue.begin(); + it != vertex_queue.end(); + ++it) { + const Vertex vertex = *it; + DCHECK(vertex_color[vertex] != kWhite); + if (vertex_color[vertex] != kBlack) { + ordering->push_back(vertex); + } + } + + CHECK_EQ(ordering->size(), num_vertices); + return independent_set_size; +} + +// Find the connected component for a vertex implemented using the +// find and update operation for disjoint-set. Recursively traverse +// the disjoint set structure till you reach a vertex whose connected +// component has the same id as the vertex itself. Along the way +// update the connected components of all the vertices. This updating +// is what gives this data structure its efficiency. +template <typename Vertex> +Vertex FindConnectedComponent(const Vertex& vertex, + HashMap<Vertex, Vertex>* union_find) { + typename HashMap<Vertex, Vertex>::iterator it = union_find->find(vertex); + DCHECK(it != union_find->end()); + if (it->second != vertex) { + it->second = FindConnectedComponent(it->second, union_find); + } + + return it->second; +} + +// Compute a degree two constrained Maximum Spanning Tree/forest of +// the input graph. Caller owns the result. +// +// Finding degree 2 spanning tree of a graph is not always +// possible. For example a star graph, i.e. a graph with n-nodes +// where one node is connected to the other n-1 nodes does not have +// a any spanning trees of degree less than n-1.Even if such a tree +// exists, finding such a tree is NP-Hard. + +// We get around both of these problems by using a greedy, degree +// constrained variant of Kruskal's algorithm. We start with a graph +// G_T with the same vertex set V as the input graph G(V,E) but an +// empty edge set. We then iterate over the edges of G in decreasing +// order of weight, adding them to G_T if doing so does not create a +// cycle in G_T} and the degree of all the vertices in G_T remains +// bounded by two. This O(|E|) algorithm results in a degree-2 +// spanning forest, or a collection of linear paths that span the +// graph G. +template <typename Vertex> +Graph<Vertex>* +Degree2MaximumSpanningForest(const Graph<Vertex>& graph) { + // Array of edges sorted in decreasing order of their weights. + vector<pair<double, pair<Vertex, Vertex> > > weighted_edges; + Graph<Vertex>* forest = new Graph<Vertex>(); + + // Disjoint-set to keep track of the connected components in the + // maximum spanning tree. + HashMap<Vertex, Vertex> disjoint_set; + + // Sort of the edges in the graph in decreasing order of their + // weight. Also add the vertices of the graph to the Maximum + // Spanning Tree graph and set each vertex to be its own connected + // component in the disjoint_set structure. + const HashSet<Vertex>& vertices = graph.vertices(); + for (typename HashSet<Vertex>::const_iterator it = vertices.begin(); + it != vertices.end(); + ++it) { + const Vertex vertex1 = *it; + forest->AddVertex(vertex1, graph.VertexWeight(vertex1)); + disjoint_set[vertex1] = vertex1; + + const HashSet<Vertex>& neighbors = graph.Neighbors(vertex1); + for (typename HashSet<Vertex>::const_iterator it2 = neighbors.begin(); + it2 != neighbors.end(); + ++it2) { + const Vertex vertex2 = *it2; + if (vertex1 >= vertex2) { + continue; + } + const double weight = graph.EdgeWeight(vertex1, vertex2); + weighted_edges.push_back(make_pair(weight, make_pair(vertex1, vertex2))); + } + } + + // The elements of this vector, are pairs<edge_weight, + // edge>. Sorting it using the reverse iterators gives us the edges + // in decreasing order of edges. + sort(weighted_edges.rbegin(), weighted_edges.rend()); + + // Greedily add edges to the spanning tree/forest as long as they do + // not violate the degree/cycle constraint. + for (int i =0; i < weighted_edges.size(); ++i) { + const pair<Vertex, Vertex>& edge = weighted_edges[i].second; + const Vertex vertex1 = edge.first; + const Vertex vertex2 = edge.second; + + // Check if either of the vertices are of degree 2 already, in + // which case adding this edge will violate the degree 2 + // constraint. + if ((forest->Neighbors(vertex1).size() == 2) || + (forest->Neighbors(vertex2).size() == 2)) { + continue; + } + + // Find the id of the connected component to which the two + // vertices belong to. If the id is the same, it means that the + // two of them are already connected to each other via some other + // vertex, and adding this edge will create a cycle. + Vertex root1 = FindConnectedComponent(vertex1, &disjoint_set); + Vertex root2 = FindConnectedComponent(vertex2, &disjoint_set); + + if (root1 == root2) { + continue; + } + + // This edge can be added, add an edge in either direction with + // the same weight as the original graph. + const double edge_weight = graph.EdgeWeight(vertex1, vertex2); + forest->AddEdge(vertex1, vertex2, edge_weight); + forest->AddEdge(vertex2, vertex1, edge_weight); + + // Connected the two connected components by updating the + // disjoint_set structure. Always connect the connected component + // with the greater index with the connected component with the + // smaller index. This should ensure shallower trees, for quicker + // lookup. + if (root2 < root1) { + std::swap(root1, root2); + }; + + disjoint_set[root2] = root1; + } + return forest; +} + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_GRAPH_ALGORITHMS_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/implicit_schur_complement.cc b/extern/libmv/third_party/ceres/internal/ceres/implicit_schur_complement.cc new file mode 100644 index 00000000000..bd908846362 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/implicit_schur_complement.cc @@ -0,0 +1,237 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/implicit_schur_complement.h" + +#include <glog/logging.h> +#include "Eigen/Dense" +#include "ceres/block_sparse_matrix.h" +#include "ceres/block_structure.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +ImplicitSchurComplement::ImplicitSchurComplement(int num_eliminate_blocks, + bool constant_sparsity, + bool preconditioner) + : num_eliminate_blocks_(num_eliminate_blocks), + constant_sparsity_(constant_sparsity), + preconditioner_(preconditioner), + A_(NULL), + D_(NULL), + b_(NULL), + block_diagonal_EtE_inverse_(NULL), + block_diagonal_FtF_inverse_(NULL) { +} + +ImplicitSchurComplement::~ImplicitSchurComplement() { +} + +void ImplicitSchurComplement::Init(const BlockSparseMatrixBase& A, + const double* D, + const double* b) { + // Since initialization is reasonably heavy, perhaps we can save on + // constructing a new object everytime. + if ((A_ == NULL) || !constant_sparsity_) { + A_.reset(new PartitionedMatrixView(A, num_eliminate_blocks_)); + } + + D_ = D; + b_ = b; + + // Initialize temporary storage and compute the block diagonals of + // E'E and F'E. + if ((!constant_sparsity_) || (block_diagonal_EtE_inverse_ == NULL)) { + block_diagonal_EtE_inverse_.reset(A_->CreateBlockDiagonalEtE()); + if (preconditioner_) { + block_diagonal_FtF_inverse_.reset(A_->CreateBlockDiagonalFtF()); + } + rhs_.resize(A_->num_cols_f()); + rhs_.setZero(); + tmp_rows_.resize(A_->num_rows()); + tmp_e_cols_.resize(A_->num_cols_e()); + tmp_e_cols_2_.resize(A_->num_cols_e()); + tmp_f_cols_.resize(A_->num_cols_f()); + } else { + A_->UpdateBlockDiagonalEtE(block_diagonal_EtE_inverse_.get()); + if (preconditioner_) { + A_->UpdateBlockDiagonalFtF(block_diagonal_FtF_inverse_.get()); + } + } + + // The block diagonals of the augmented linear system contain + // contributions from the diagonal D if it is non-null. Add that to + // the block diagonals and invert them. + if (D_ != NULL) { + AddDiagonalAndInvert(D_, block_diagonal_EtE_inverse_.get()); + if (preconditioner_) { + AddDiagonalAndInvert(D_ + A_->num_cols_e(), + block_diagonal_FtF_inverse_.get()); + } + } else { + AddDiagonalAndInvert(NULL, block_diagonal_EtE_inverse_.get()); + if (preconditioner_) { + AddDiagonalAndInvert(NULL, block_diagonal_FtF_inverse_.get()); + } + } + + // Compute the RHS of the Schur complement system. + UpdateRhs(); +} + +// Evaluate the product +// +// Sx = [F'F - F'E (E'E)^-1 E'F]x +// +// By breaking it down into individual matrix vector products +// involving the matrices E and F. This is implemented using a +// PartitionedMatrixView of the input matrix A. +void ImplicitSchurComplement::RightMultiply(const double* x, double* y) const { + // y1 = F x + tmp_rows_.setZero(); + A_->RightMultiplyF(x, tmp_rows_.data()); + + // y2 = E' y1 + tmp_e_cols_.setZero(); + A_->LeftMultiplyE(tmp_rows_.data(), tmp_e_cols_.data()); + + // y3 = -(E'E)^-1 y2 + tmp_e_cols_2_.setZero(); + block_diagonal_EtE_inverse_->RightMultiply(tmp_e_cols_.data(), + tmp_e_cols_2_.data()); + tmp_e_cols_2_ *= -1.0; + + // y1 = y1 + E y3 + A_->RightMultiplyE(tmp_e_cols_2_.data(), tmp_rows_.data()); + + // y5 = D * x + if (D_ != NULL) { + ConstVectorRef Dref(D_ + A_->num_cols_e(), num_cols()); + VectorRef(y, num_cols()) = + (Dref.array().square() * + ConstVectorRef(x, num_cols()).array()).matrix(); + } else { + VectorRef(y, num_cols()).setZero(); + } + + // y = y5 + F' y1 + A_->LeftMultiplyF(tmp_rows_.data(), y); +} + +// Given a block diagonal matrix and an optional array of diagonal +// entries D, add them to the diagonal of the matrix and compute the +// inverse of each diagonal block. +void ImplicitSchurComplement::AddDiagonalAndInvert( + const double* D, + BlockSparseMatrix* block_diagonal) { + const CompressedRowBlockStructure* block_diagonal_structure = + block_diagonal->block_structure(); + for (int r = 0; r < block_diagonal_structure->rows.size(); ++r) { + const int row_block_pos = block_diagonal_structure->rows[r].block.position; + const int row_block_size = block_diagonal_structure->rows[r].block.size; + const Cell& cell = block_diagonal_structure->rows[r].cells[0]; + MatrixRef m(block_diagonal->mutable_values() + cell.position, + row_block_size, row_block_size); + + if (D != NULL) { + ConstVectorRef d(D + row_block_pos, row_block_size); + m += d.array().square().matrix().asDiagonal(); + } + + m = m + .selfadjointView<Eigen::Upper>() + .ldlt() + .solve(Matrix::Identity(row_block_size, row_block_size)); + } +} + +// Similar to RightMultiply, use the block structure of the matrix A +// to compute y = (E'E)^-1 (E'b - E'F x). +void ImplicitSchurComplement::BackSubstitute(const double* x, double* y) { + const int num_cols_e = A_->num_cols_e(); + const int num_cols_f = A_->num_cols_f(); + const int num_cols = A_->num_cols(); + const int num_rows = A_->num_rows(); + + // y1 = F x + tmp_rows_.setZero(); + A_->RightMultiplyF(x, tmp_rows_.data()); + + // y2 = b - y1 + tmp_rows_ = ConstVectorRef(b_, num_rows) - tmp_rows_; + + // y3 = E' y2 + tmp_e_cols_.setZero(); + A_->LeftMultiplyE(tmp_rows_.data(), tmp_e_cols_.data()); + + // y = (E'E)^-1 y3 + VectorRef(y, num_cols).setZero(); + block_diagonal_EtE_inverse_->RightMultiply(tmp_e_cols_.data(), y); + + // The full solution vector y has two blocks. The first block of + // variables corresponds to the eliminated variables, which we just + // computed via back substitution. The second block of variables + // corresponds to the Schur complement system, so we just copy those + // values from the solution to the Schur complement. + VectorRef(y + num_cols_e, num_cols_f) = ConstVectorRef(x, num_cols_f); +} + +// Compute the RHS of the Schur complement system. +// +// rhs = F'b - F'E (E'E)^-1 E'b +// +// Like BackSubstitute, we use the block structure of A to implement +// this using a series of matrix vector products. +void ImplicitSchurComplement::UpdateRhs() { + // y1 = E'b + tmp_e_cols_.setZero(); + A_->LeftMultiplyE(b_, tmp_e_cols_.data()); + + // y2 = (E'E)^-1 y1 + Vector y2 = Vector::Zero(A_->num_cols_e()); + block_diagonal_EtE_inverse_->RightMultiply(tmp_e_cols_.data(), y2.data()); + + // y3 = E y2 + tmp_rows_.setZero(); + A_->RightMultiplyE(y2.data(), tmp_rows_.data()); + + // y3 = b - y3 + tmp_rows_ = ConstVectorRef(b_, A_->num_rows()) - tmp_rows_; + + // rhs = F' y3 + rhs_.setZero(); + A_->LeftMultiplyF(tmp_rows_.data(), rhs_.data()); +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/implicit_schur_complement.h b/extern/libmv/third_party/ceres/internal/ceres/implicit_schur_complement.h new file mode 100644 index 00000000000..37a319f9c57 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/implicit_schur_complement.h @@ -0,0 +1,176 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// An iterative solver for solving the Schur complement/reduced camera +// linear system that arise in SfM problems. + +#ifndef CERES_INTERNAL_IMPLICIT_SCHUR_COMPLEMENT_H_ +#define CERES_INTERNAL_IMPLICIT_SCHUR_COMPLEMENT_H_ + +#include "ceres/linear_operator.h" +#include "ceres/partitioned_matrix_view.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +class BlockSparseMatrix; +class BlockSparseMatrixBase; + +// This class implements various linear algebraic operations related +// to the Schur complement without explicitly forming it. +// +// +// Given a reactangular linear system Ax = b, where +// +// A = [E F] +// +// The normal equations are given by +// +// A'Ax = A'b +// +// |E'E E'F||y| = |E'b| +// |F'E F'F||z| |F'b| +// +// and the Schur complement system is given by +// +// [F'F - F'E (E'E)^-1 E'F] z = F'b - F'E (E'E)^-1 E'b +// +// Now if we wish to solve Ax = b in the least squares sense, one way +// is to form this Schur complement system and solve it using +// Preconditioned Conjugate Gradients. +// +// The key operation in a conjugate gradient solver is the evaluation of the +// matrix vector product with the Schur complement +// +// S = F'F - F'E (E'E)^-1 E'F +// +// It is straightforward to see that matrix vector products with S can +// be evaluated without storing S in memory. Instead, given (E'E)^-1 +// (which for our purposes is an easily inverted block diagonal +// matrix), it can be done in terms of matrix vector products with E, +// F and (E'E)^-1. This class implements this functionality and other +// auxilliary bits needed to implement a CG solver on the Schur +// complement using the PartitionedMatrixView object. +// +// THREAD SAFETY: This class is nqot thread safe. In particular, the +// RightMultiply (and the LeftMultiply) methods are not thread safe as +// they depend on mutable arrays used for the temporaries needed to +// compute the product y += Sx; +class ImplicitSchurComplement : public LinearOperator { + public: + // num_eliminate_blocks is the number of E blocks in the matrix + // A. + // + // constant_sparsity indicates if across calls to Init, the sparsity + // structure of the matrix A remains constant or not. This makes for + // significant savings across multiple matrices A, e.g. when used in + // conjunction with an optimization algorithm. + // + // preconditioner indicates whether the inverse of the matrix F'F + // should be computed or not as a preconditioner for the Schur + // Complement. + // + // TODO(sameeragarwal): Get rid of the two bools below and replace + // them with enums. + ImplicitSchurComplement(int num_eliminate_blocks, + bool constant_sparsity, + bool preconditioner); + virtual ~ImplicitSchurComplement(); + + // Initialize the Schur complement for a linear least squares + // problem of the form + // + // |A | x = |b| + // |diag(D)| |0| + // + // If D is null, then it is treated as a zero dimensional matrix. It + // is important that the matrix A have a BlockStructure object + // associated with it and has a block structure that is compatible + // with the SchurComplement solver. + void Init(const BlockSparseMatrixBase& A, const double* D, const double* b); + + // y += Sx, where S is the Schur complement. + virtual void RightMultiply(const double* x, double* y) const; + + // The Schur complement is a symmetric positive definite matrix, + // thus the left and right multiply operators are the same. + virtual void LeftMultiply(const double* x, double* y) const { + RightMultiply(x, y); + } + + // y = (E'E)^-1 (E'b - E'F x). Given an estimate of the solution to + // the Schur complement system, this method computes the value of + // the e_block variables that were eliminated to form the Schur + // complement. + void BackSubstitute(const double* x, double* y); + + virtual int num_rows() const { return A_->num_cols_f(); } + virtual int num_cols() const { return A_->num_cols_f(); } + const Vector& rhs() const { return rhs_; } + + const BlockSparseMatrix* block_diagonal_EtE_inverse() const { + return block_diagonal_EtE_inverse_.get(); + } + + const BlockSparseMatrix* block_diagonal_FtF_inverse() const { + return block_diagonal_FtF_inverse_.get(); + } + + private: + void AddDiagonalAndInvert(const double* D, BlockSparseMatrix* matrix); + void UpdateRhs(); + + int num_eliminate_blocks_; + bool constant_sparsity_; + bool preconditioner_; + + scoped_ptr<PartitionedMatrixView> A_; + const double* D_; + const double* b_; + + scoped_ptr<BlockSparseMatrix> block_diagonal_EtE_inverse_; + scoped_ptr<BlockSparseMatrix> block_diagonal_FtF_inverse_; + + Vector rhs_; + + // Temporary storage vectors used to implement RightMultiply. + mutable Vector tmp_rows_; + mutable Vector tmp_e_cols_; + mutable Vector tmp_e_cols_2_; + mutable Vector tmp_f_cols_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_IMPLICIT_SCHUR_COMPLEMENT_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/integral_types.h b/extern/libmv/third_party/ceres/internal/ceres/integral_types.h new file mode 100644 index 00000000000..01e04937e3e --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/integral_types.h @@ -0,0 +1,92 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 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: keir@google.com (Keir Mierle) +// +// Portable typedefs for various fixed-size integers. Uses template +// metaprogramming instead of fragile compiler defines. + +#ifndef CERES_INTERNAL_INTEGRAL_TYPES_H_ +#define CERES_INTERNAL_INTEGRAL_TYPES_H_ + +namespace ceres { +namespace internal { + +// Compile time ternary on types. +template<bool kCondition, typename kTrueType, typename kFalseType> +struct Ternary { + typedef kTrueType type; +}; +template<typename kTrueType, typename kFalseType> +struct Ternary<false, kTrueType, kFalseType> { + typedef kFalseType type; +}; + +#define CERES_INTSIZE(TYPE) \ + typename Ternary<sizeof(TYPE) * 8 == kBits, TYPE, + +template<int kBits> +struct Integer { + typedef + CERES_INTSIZE(char) + CERES_INTSIZE(short) + CERES_INTSIZE(int) + CERES_INTSIZE(long int) + CERES_INTSIZE(long long) + void>::type >::type >::type >::type >::type + type; +}; + +template<int kBits> +struct UnsignedInteger { + typedef + CERES_INTSIZE(unsigned char) + CERES_INTSIZE(unsigned short) + CERES_INTSIZE(unsigned int) + CERES_INTSIZE(unsigned long int) + CERES_INTSIZE(unsigned long long) + void>::type >::type >::type >::type >::type + type; +}; + +#undef CERES_INTSIZE + +typedef Integer< 8>::type int8; +typedef Integer<16>::type int16; +typedef Integer<32>::type int32; +typedef Integer<64>::type int64; + +typedef UnsignedInteger< 8>::type uint8; +typedef UnsignedInteger<16>::type uint16; +typedef UnsignedInteger<32>::type uint32; +typedef UnsignedInteger<64>::type uint64; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_INTEGRAL_TYPES_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/iterative_schur_complement_solver.cc b/extern/libmv/third_party/ceres/internal/ceres/iterative_schur_complement_solver.cc new file mode 100644 index 00000000000..51303195317 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/iterative_schur_complement_solver.cc @@ -0,0 +1,150 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/iterative_schur_complement_solver.h" + +#include <algorithm> +#include <cstring> +#include <vector> + +#include <glog/logging.h> +#include "Eigen/Dense" +#include "ceres/block_sparse_matrix.h" +#include "ceres/block_structure.h" +#include "ceres/conjugate_gradients_solver.h" +#include "ceres/implicit_schur_complement.h" +#include "ceres/linear_solver.h" +#include "ceres/triplet_sparse_matrix.h" +#include "ceres/visibility_based_preconditioner.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/linear_solver.h" +#include "ceres/triplet_sparse_matrix.h" +#include "ceres/types.h" +#include "ceres/visibility_based_preconditioner.h" + +namespace ceres { +namespace internal { + +IterativeSchurComplementSolver::IterativeSchurComplementSolver( + const LinearSolver::Options& options) + : options_(options) { +} + +IterativeSchurComplementSolver::~IterativeSchurComplementSolver() { +} + +LinearSolver::Summary IterativeSchurComplementSolver::SolveImpl( + BlockSparseMatrixBase* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x) { + CHECK_NOTNULL(A->block_structure()); + + // Initialize a ImplicitSchurComplement object. + if ((schur_complement_ == NULL) || (!options_.constant_sparsity)) { + schur_complement_.reset( + new ImplicitSchurComplement(options_.num_eliminate_blocks, + options_.constant_sparsity, + options_.preconditioner_type == JACOBI)); + } + schur_complement_->Init(*A, per_solve_options.D, b); + + // Initialize the solution to the Schur complement system to zero. + // + // TODO(sameeragarwal): There maybe a better initialization than an + // all zeros solution. Explore other cheap starting points. + reduced_linear_system_solution_.resize(schur_complement_->num_rows()); + reduced_linear_system_solution_.setZero(); + + // Instantiate a conjugate gradient solver that runs on the Schur complement + // matrix with the block diagonal of the matrix F'F as the preconditioner. + LinearSolver::Options cg_options; + cg_options.max_num_iterations = options_.max_num_iterations; + ConjugateGradientsSolver cg_solver(cg_options); + LinearSolver::PerSolveOptions cg_per_solve_options; + + cg_per_solve_options.r_tolerance = per_solve_options.r_tolerance; + cg_per_solve_options.q_tolerance = per_solve_options.q_tolerance; + + bool is_preconditioner_good = false; + switch (options_.preconditioner_type) { + case IDENTITY: + is_preconditioner_good = true; + break; + case JACOBI: + // We need to strip the constness of the block_diagonal_FtF_inverse + // matrix here because the only other way to initialize the struct + // cg_solve_options would be to add a constructor to it. We know + // that the only method ever called on the preconditioner is the + // RightMultiply which is a const method so we don't need to worry + // about the object getting modified. + cg_per_solve_options.preconditioner = + const_cast<BlockSparseMatrix*>( + schur_complement_->block_diagonal_FtF_inverse()); + is_preconditioner_good = true; + break; + case SCHUR_JACOBI: + case CLUSTER_JACOBI: + case CLUSTER_TRIDIAGONAL: + if (visibility_based_preconditioner_.get() == NULL) { + visibility_based_preconditioner_.reset( + new VisibilityBasedPreconditioner(*A->block_structure(), options_)); + } + is_preconditioner_good = + visibility_based_preconditioner_->Compute(*A, per_solve_options.D); + cg_per_solve_options.preconditioner = + visibility_based_preconditioner_.get(); + break; + default: + LOG(FATAL) << "Unknown Preconditioner Type"; + } + + LinearSolver::Summary cg_summary; + cg_summary.num_iterations = 0; + cg_summary.termination_type = FAILURE; + + if (is_preconditioner_good) { + cg_summary = cg_solver.Solve(schur_complement_.get(), + schur_complement_->rhs().data(), + cg_per_solve_options, + reduced_linear_system_solution_.data()); + if (cg_summary.termination_type != FAILURE) { + schur_complement_->BackSubstitute( + reduced_linear_system_solution_.data(), x); + } + } + + VLOG(2) << "CG Iterations : " << cg_summary.num_iterations; + return cg_summary; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/iterative_schur_complement_solver.h b/extern/libmv/third_party/ceres/internal/ceres/iterative_schur_complement_solver.h new file mode 100644 index 00000000000..cfeb65e1eec --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/iterative_schur_complement_solver.h @@ -0,0 +1,94 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_ITERATIVE_SCHUR_COMPLEMENT_SOLVER_H_ +#define CERES_INTERNAL_ITERATIVE_SCHUR_COMPLEMENT_SOLVER_H_ + +#include "ceres/linear_solver.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +class BlockSparseMatrix; +class BlockSparseMatrixBase; +class ImplicitSchurComplement; +class VisibilityBasedPreconditioner; + +// This class implements an iterative solver for the linear least +// squares problems that have a bi-partitte sparsity structure common +// to Structure from Motion problems. +// +// The algorithm used by this solver was developed in a series of +// papers - "Agarwal et al, Bundle Adjustment in the Large, ECCV 2010" +// and "Wu et al, Multicore Bundle Adjustment, submitted to CVPR +// 2011" at the Univeristy of Washington. +// +// The key idea is that one can run Conjugate Gradients on the Schur +// Complement system without explicitly forming the Schur Complement +// in memory. The heavy lifting for this is done by the +// ImplicitSchurComplement class. Not forming the Schur complement in +// memory and factoring it results in substantial savings in time and +// memory. Further, iterative solvers like this open up the +// possibility of solving the Newton equations in a non-linear solver +// only approximately and terminating early, thereby saving even more +// time. +// +// For the curious, running CG on the Schur complement is the same as +// running CG on the Normal Equations with an SSOR preconditioner. For +// a proof of this fact and others related to this solver please see +// the section on Domain Decomposition Methods in Saad's book +// "Iterative Methods for Sparse Linear Systems". +class IterativeSchurComplementSolver : public BlockSparseMatrixBaseSolver { + public: + explicit IterativeSchurComplementSolver( + const LinearSolver::Options& options); + + virtual ~IterativeSchurComplementSolver(); + + private: + virtual LinearSolver::Summary SolveImpl( + BlockSparseMatrixBase* A, + const double* b, + const LinearSolver::PerSolveOptions& options, + double* x); + + LinearSolver::Options options_; + scoped_ptr<internal::ImplicitSchurComplement> schur_complement_; + scoped_ptr<VisibilityBasedPreconditioner> visibility_based_preconditioner_; + Vector reduced_linear_system_solution_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_ITERATIVE_SCHUR_COMPLEMENT_SOLVER_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/levenberg_marquardt.cc b/extern/libmv/third_party/ceres/internal/ceres/levenberg_marquardt.cc new file mode 100644 index 00000000000..b40a5162adc --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/levenberg_marquardt.cc @@ -0,0 +1,574 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Implementation of a simple LM algorithm which uses the step sizing +// rule of "Methods for Nonlinear Least Squares" by K. Madsen, +// H.B. Nielsen and O. Tingleff. Available to download from +// +// http://www2.imm.dtu.dk/pubdb/views/edoc_download.php/3215/pdf/imm3215.pdf +// +// The basic algorithm described in this note is an exact step +// algorithm that depends on the Newton(LM) step being solved exactly +// in each iteration. When a suitable iterative solver is available to +// solve the Newton(LM) step, the algorithm will automatically switch +// to an inexact step solution method. This trades some slowdown in +// convergence for significant savings in solve time and memory +// usage. Our implementation of the Truncated Newton algorithm follows +// the discussion and recommendataions in "Stephen G. Nash, A Survey +// of Truncated Newton Methods, Journal of Computational and Applied +// Mathematics, 124(1-2), 45-59, 2000. + +#include "ceres/levenberg_marquardt.h" + +#include <algorithm> +#include <cstdlib> +#include <cmath> +#include <cstring> +#include <string> +#include <vector> + +#include <glog/logging.h> +#include "Eigen/Core" +#include "ceres/evaluator.h" +#include "ceres/file.h" +#include "ceres/linear_least_squares_problems.h" +#include "ceres/linear_solver.h" +#include "ceres/matrix_proto.h" +#include "ceres/sparse_matrix.h" +#include "ceres/stringprintf.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { +namespace { + +// Numbers for clamping the size of the LM diagonal. The size of these +// numbers is heuristic. We will probably be adjusting them in the +// future based on more numerical experience. With jacobi scaling +// enabled, these numbers should be all but redundant. +const double kMinLevenbergMarquardtDiagonal = 1e-6; +const double kMaxLevenbergMarquardtDiagonal = 1e32; + +// Small constant for various floating point issues. +const double kEpsilon = 1e-12; + +// Number of times the linear solver should be retried in case of +// numerical failure. The retries are done by exponentially scaling up +// mu at each retry. This leads to stronger and stronger +// regularization making the linear least squares problem better +// conditioned at each retry. +const int kMaxLinearSolverRetries = 5; + +// D = 1/sqrt(diag(J^T * J)) +void EstimateScale(const SparseMatrix& jacobian, double* D) { + CHECK_NOTNULL(D); + jacobian.SquaredColumnNorm(D); + for (int i = 0; i < jacobian.num_cols(); ++i) { + D[i] = 1.0 / (kEpsilon + sqrt(D[i])); + } +} + +// D = diag(J^T * J) +void LevenbergMarquardtDiagonal(const SparseMatrix& jacobian, + double* D) { + CHECK_NOTNULL(D); + jacobian.SquaredColumnNorm(D); + for (int i = 0; i < jacobian.num_cols(); ++i) { + D[i] = min(max(D[i], kMinLevenbergMarquardtDiagonal), + kMaxLevenbergMarquardtDiagonal); + } +} + +bool RunCallback(IterationCallback* callback, + const IterationSummary& iteration_summary, + Solver::Summary* summary) { + const CallbackReturnType status = (*callback)(iteration_summary); + switch (status) { + case SOLVER_TERMINATE_SUCCESSFULLY: + summary->termination_type = USER_SUCCESS; + VLOG(1) << "Terminating on USER_SUCCESS."; + return false; + case SOLVER_ABORT: + summary->termination_type = USER_ABORT; + VLOG(1) << "Terminating on USER_ABORT."; + return false; + case SOLVER_CONTINUE: + return true; + default: + LOG(FATAL) << "Unknown status returned by callback: " + << status; + return NULL; + } +} + +} // namespace + +LevenbergMarquardt::~LevenbergMarquardt() {} + +void LevenbergMarquardt::Minimize(const Minimizer::Options& options, + Evaluator* evaluator, + LinearSolver* linear_solver, + const double* initial_parameters, + double* final_parameters, + Solver::Summary* summary) { + time_t start_time = time(NULL); + const int num_parameters = evaluator->NumParameters(); + const int num_effective_parameters = evaluator->NumEffectiveParameters(); + const int num_residuals = evaluator->NumResiduals(); + + summary->termination_type = NO_CONVERGENCE; + summary->num_successful_steps = 0; + summary->num_unsuccessful_steps = 0; + + // Allocate the various vectors needed by the algorithm. + memcpy(final_parameters, initial_parameters, + num_parameters * sizeof(*initial_parameters)); + + VectorRef x(final_parameters, num_parameters); + Vector x_new(num_parameters); + + Vector lm_step(num_effective_parameters); + Vector gradient(num_effective_parameters); + Vector scaled_gradient(num_effective_parameters); + // Jacobi scaling vector + Vector scale(num_effective_parameters); + + Vector f_model(num_residuals); + Vector f(num_residuals); + Vector f_new(num_residuals); + Vector D(num_parameters); + Vector muD(num_parameters); + + // Ask the Evaluator to create the jacobian matrix. The sparsity + // pattern of this matrix is going to remain constant, so we only do + // this once and then re-use this matrix for all subsequent Jacobian + // computations. + scoped_ptr<SparseMatrix> jacobian(evaluator->CreateJacobian()); + + double x_norm = x.norm(); + + double cost = 0.0; + D.setOnes(); + f.setZero(); + + // Do initial cost and Jacobian evaluation. + if (!evaluator->Evaluate(x.data(), &cost, f.data(), jacobian.get())) { + LOG(WARNING) << "Failed to compute residuals and Jacobian. " + << "Terminating."; + summary->termination_type = NUMERICAL_FAILURE; + return; + } + + if (options.jacobi_scaling) { + EstimateScale(*jacobian, scale.data()); + jacobian->ScaleColumns(scale.data()); + } else { + scale.setOnes(); + } + + // This is a poor way to do this computation. Even if fixed_cost is + // zero, because we are subtracting two possibly large numbers, we + // are depending on exact cancellation to give us a zero here. But + // initial_cost and cost have been computed by two different + // evaluators. One which runs on the whole problem (in + // solver_impl.cc) in single threaded mode and another which runs + // here on the reduced problem, so fixed_cost can (and does) contain + // some numerical garbage with a relative magnitude of 1e-14. + // + // The right way to do this, would be to compute the fixed cost on + // just the set of residual blocks which are held constant and were + // removed from the original problem when the reduced problem was + // constructed. + summary->fixed_cost = summary->initial_cost - cost; + + double model_cost = f.squaredNorm() / 2.0; + double total_cost = summary->fixed_cost + cost; + + scaled_gradient.setZero(); + jacobian->LeftMultiply(f.data(), scaled_gradient.data()); + gradient = scaled_gradient.array() / scale.array(); + + double gradient_max_norm = gradient.lpNorm<Eigen::Infinity>(); + // We need the max here to guard againt the gradient being zero. + const double gradient_max_norm_0 = max(gradient_max_norm, kEpsilon); + double gradient_tolerance = options.gradient_tolerance * gradient_max_norm_0; + + double mu = options.tau; + double nu = 2.0; + int iteration = 0; + double actual_cost_change = 0.0; + double step_norm = 0.0; + double relative_decrease = 0.0; + + // Insane steps are steps which are not sane, i.e. there is some + // numerical kookiness going on with them. There are various reasons + // for this kookiness, some easier to diagnose then others. From the + // point of view of the non-linear solver, they are steps which + // cannot be used. We return with NUMERICAL_FAILURE after + // kMaxLinearSolverRetries consecutive insane steps. + bool step_is_sane = false; + int num_consecutive_insane_steps = 0; + + // Whether the step resulted in a sufficient decrease in the + // objective function when compared to the decrease in the value of + // the lineariztion. + bool step_is_successful = false; + + // Parse the iterations for which to dump the linear problem. + vector<int> iterations_to_dump = options.lsqp_iterations_to_dump; + sort(iterations_to_dump.begin(), iterations_to_dump.end()); + + IterationSummary iteration_summary; + iteration_summary.iteration = iteration; + iteration_summary.step_is_successful = false; + iteration_summary.cost = total_cost; + iteration_summary.cost_change = actual_cost_change; + iteration_summary.gradient_max_norm = gradient_max_norm; + iteration_summary.step_norm = step_norm; + iteration_summary.relative_decrease = relative_decrease; + iteration_summary.mu = mu; + iteration_summary.eta = options.eta; + iteration_summary.linear_solver_iterations = 0; + iteration_summary.linear_solver_time_sec = 0.0; + iteration_summary.iteration_time_sec = (time(NULL) - start_time); + if (options.logging_type >= PER_MINIMIZER_ITERATION) { + summary->iterations.push_back(iteration_summary); + } + + // Check if the starting point is an optimum. + VLOG(2) << "Gradient max norm: " << gradient_max_norm + << " tolerance: " << gradient_tolerance + << " ratio: " << gradient_max_norm / gradient_max_norm_0 + << " tolerance: " << options.gradient_tolerance; + if (gradient_max_norm <= gradient_tolerance) { + summary->termination_type = GRADIENT_TOLERANCE; + VLOG(1) << "Terminating on GRADIENT_TOLERANCE. " + << "Relative gradient max norm: " + << gradient_max_norm / gradient_max_norm_0 + << " <= " << options.gradient_tolerance; + return; + } + + // Call the various callbacks. + for (int i = 0; i < options.callbacks.size(); ++i) { + if (!RunCallback(options.callbacks[i], iteration_summary, summary)) { + return; + } + } + + // We only need the LM diagonal if we are actually going to do at + // least one iteration of the optimization. So we wait to do it + // until now. + LevenbergMarquardtDiagonal(*jacobian, D.data()); + + while ((iteration < options.max_num_iterations) && + (time(NULL) - start_time) <= options.max_solver_time_sec) { + time_t iteration_start_time = time(NULL); + step_is_sane = false; + step_is_successful = false; + + IterationSummary iteration_summary; + // The while loop here is just to provide an easily breakable + // control structure. We are guaranteed to always exit this loop + // at the end of one iteration or before. + while (1) { + muD = (mu * D).array().sqrt(); + LinearSolver::PerSolveOptions solve_options; + solve_options.D = muD.data(); + solve_options.q_tolerance = options.eta; + // Disable r_tolerance checking. Since we only care about + // termination via the q_tolerance. As Nash and Sofer show, + // r_tolerance based termination is essentially useless in + // Truncated Newton methods. + solve_options.r_tolerance = -1.0; + + const time_t linear_solver_start_time = time(NULL); + LinearSolver::Summary linear_solver_summary = + linear_solver->Solve(jacobian.get(), + f.data(), + solve_options, + lm_step.data()); + iteration_summary.linear_solver_time_sec = + (time(NULL) - linear_solver_start_time); + iteration_summary.linear_solver_iterations = + linear_solver_summary.num_iterations; + + if (binary_search(iterations_to_dump.begin(), + iterations_to_dump.end(), + iteration)) { + CHECK(DumpLinearLeastSquaresProblem(options.lsqp_dump_directory, + iteration, + options.lsqp_dump_format_type, + jacobian.get(), + muD.data(), + f.data(), + lm_step.data(), + options.num_eliminate_blocks)) + << "Tried writing linear least squares problem: " + << options.lsqp_dump_directory + << " but failed."; + } + + // We ignore the case where the linear solver did not converge, + // since the partial solution computed by it still maybe of use, + // and there is no reason to ignore it, especially since we + // spent so much time computing it. + if ((linear_solver_summary.termination_type != TOLERANCE) && + (linear_solver_summary.termination_type != MAX_ITERATIONS)) { + VLOG(1) << "Linear solver failure: retrying with a higher mu"; + break; + } + + step_norm = (lm_step.array() * scale.array()).matrix().norm(); + + // Check step length based convergence. If the step length is + // too small, then we are done. + const double step_size_tolerance = options.parameter_tolerance * + (x_norm + options.parameter_tolerance); + + VLOG(2) << "Step size: " << step_norm + << " tolerance: " << step_size_tolerance + << " ratio: " << step_norm / step_size_tolerance + << " tolerance: " << options.parameter_tolerance; + if (step_norm <= options.parameter_tolerance * + (x_norm + options.parameter_tolerance)) { + summary->termination_type = PARAMETER_TOLERANCE; + VLOG(1) << "Terminating on PARAMETER_TOLERANCE." + << "Relative step size: " << step_norm / step_size_tolerance + << " <= " << options.parameter_tolerance; + return; + } + + Vector delta = -(lm_step.array() * scale.array()).matrix(); + if (!evaluator->Plus(x.data(), delta.data(), x_new.data())) { + LOG(WARNING) << "Failed to compute Plus(x, delta, x_plus_delta). " + << "Terminating."; + summary->termination_type = NUMERICAL_FAILURE; + return; + } + + double cost_new = 0.0; + if (!evaluator->Evaluate(x_new.data(), &cost_new, NULL, NULL)) { + LOG(WARNING) << "Failed to compute the value of the objective " + << "function. Terminating."; + summary->termination_type = NUMERICAL_FAILURE; + return; + } + + f_model.setZero(); + jacobian->RightMultiply(lm_step.data(), f_model.data()); + const double model_cost_new = + (f.segment(0, num_residuals) - f_model).squaredNorm() / 2; + + actual_cost_change = cost - cost_new; + double model_cost_change = model_cost - model_cost_new; + + VLOG(2) << "[Model cost] current: " << model_cost + << " new : " << model_cost_new + << " change: " << model_cost_change; + + VLOG(2) << "[Nonlinear cost] current: " << cost + << " new : " << cost_new + << " change: " << actual_cost_change + << " relative change: " << fabs(actual_cost_change) / cost + << " tolerance: " << options.function_tolerance; + + // In exact arithmetic model_cost_change should never be + // negative. But due to numerical precision issues, we may end up + // with a small negative number. model_cost_change which are + // negative and large in absolute value are indicative of a + // numerical failure in the solver. + if (model_cost_change < -kEpsilon) { + VLOG(1) << "Model cost change is negative.\n" + << "Current : " << model_cost + << " new : " << model_cost_new + << " change: " << model_cost_change << "\n"; + break; + } + + // If we have reached this far, then we are willing to trust the + // numerical quality of the step. + step_is_sane = true; + num_consecutive_insane_steps = 0; + + // Check function value based convergence. + if (fabs(actual_cost_change) < options.function_tolerance * cost) { + VLOG(1) << "Termination on FUNCTION_TOLERANCE." + << " Relative cost change: " << fabs(actual_cost_change) / cost + << " tolerance: " << options.function_tolerance; + summary->termination_type = FUNCTION_TOLERANCE; + return; + } + + // Clamp model_cost_change at kEpsilon from below. + if (model_cost_change < kEpsilon) { + VLOG(1) << "Clamping model cost change " << model_cost_change + << " to " << kEpsilon; + model_cost_change = kEpsilon; + } + + relative_decrease = actual_cost_change / model_cost_change; + VLOG(2) << "actual_cost_change / model_cost_change = " + << relative_decrease; + + if (relative_decrease < options.min_relative_decrease) { + VLOG(2) << "Unsuccessful step."; + break; + } + + VLOG(2) << "Successful step."; + + ++summary->num_successful_steps; + x = x_new; + x_norm = x.norm(); + + if (!evaluator->Evaluate(x.data(), &cost, f.data(), jacobian.get())) { + LOG(WARNING) << "Failed to compute residuals and jacobian. " + << "Terminating."; + summary->termination_type = NUMERICAL_FAILURE; + return; + } + + if (options.jacobi_scaling) { + jacobian->ScaleColumns(scale.data()); + } + + model_cost = f.squaredNorm() / 2.0; + LevenbergMarquardtDiagonal(*jacobian, D.data()); + scaled_gradient.setZero(); + jacobian->LeftMultiply(f.data(), scaled_gradient.data()); + gradient = scaled_gradient.array() / scale.array(); + gradient_max_norm = gradient.lpNorm<Eigen::Infinity>(); + + // Check gradient based convergence. + VLOG(2) << "Gradient max norm: " << gradient_max_norm + << " tolerance: " << gradient_tolerance + << " ratio: " << gradient_max_norm / gradient_max_norm_0 + << " tolerance: " << options.gradient_tolerance; + if (gradient_max_norm <= gradient_tolerance) { + summary->termination_type = GRADIENT_TOLERANCE; + VLOG(1) << "Terminating on GRADIENT_TOLERANCE. " + << "Relative gradient max norm: " + << gradient_max_norm / gradient_max_norm_0 + << " <= " << options.gradient_tolerance + << " (tolerance)."; + return; + } + + mu = mu * max(1.0 / 3.0, 1 - pow(2 * relative_decrease - 1, 3)); + nu = 2.0; + step_is_successful = true; + break; + } + + if (!step_is_sane) { + ++num_consecutive_insane_steps; + } + + if (num_consecutive_insane_steps == kMaxLinearSolverRetries) { + summary->termination_type = NUMERICAL_FAILURE; + VLOG(1) << "Too many consecutive retries; ending with numerical fail."; + + if (!options.crash_and_dump_lsqp_on_failure) { + return; + } + + // Dump debugging information to disk. + CHECK(options.lsqp_dump_format_type == TEXTFILE || + options.lsqp_dump_format_type == PROTOBUF) + << "Dumping the linear least squares problem on crash " + << "requires Solver::Options::lsqp_dump_format_type to be " + << "PROTOBUF or TEXTFILE."; + + if (DumpLinearLeastSquaresProblem(options.lsqp_dump_directory, + iteration, + options.lsqp_dump_format_type, + jacobian.get(), + muD.data(), + f.data(), + lm_step.data(), + options.num_eliminate_blocks)) { + LOG(FATAL) << "Linear least squares problem saved to: " + << options.lsqp_dump_directory + << ". Please provide this to the Ceres developers for " + << " debugging along with the v=2 log."; + } else { + LOG(FATAL) << "Tried writing linear least squares problem: " + << options.lsqp_dump_directory + << " but failed."; + } + } + + if (!step_is_successful) { + // Either the step did not lead to a decrease in cost or there + // was numerical failure. In either case we will scale mu up and + // retry. If it was a numerical failure, we hope that the + // stronger regularization will make the linear system better + // conditioned. If it was numerically sane, but there was no + // decrease in cost, then increasing mu reduces the size of the + // trust region and we look for a decrease closer to the + // linearization point. + ++summary->num_unsuccessful_steps; + mu = mu * nu; + nu = 2 * nu; + } + + ++iteration; + + total_cost = summary->fixed_cost + cost; + + iteration_summary.iteration = iteration; + iteration_summary.step_is_successful = step_is_successful; + iteration_summary.cost = total_cost; + iteration_summary.cost_change = actual_cost_change; + iteration_summary.gradient_max_norm = gradient_max_norm; + iteration_summary.step_norm = step_norm; + iteration_summary.relative_decrease = relative_decrease; + iteration_summary.mu = mu; + iteration_summary.eta = options.eta; + iteration_summary.iteration_time_sec = (time(NULL) - iteration_start_time); + + if (options.logging_type >= PER_MINIMIZER_ITERATION) { + summary->iterations.push_back(iteration_summary); + } + + // Call the various callbacks. + for (int i = 0; i < options.callbacks.size(); ++i) { + if (!RunCallback(options.callbacks[i], iteration_summary, summary)) { + return; + } + } + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/levenberg_marquardt.h b/extern/libmv/third_party/ceres/internal/ceres/levenberg_marquardt.h new file mode 100644 index 00000000000..d00bb9095be --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/levenberg_marquardt.h @@ -0,0 +1,65 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Implmentation of Levenberg Marquardt algorithm based on "Methods for +// Nonlinear Least Squares" by K. Madsen, H.B. Nielsen and +// O. Tingleff. Available to download from +// +// http://www2.imm.dtu.dk/pubdb/views/edoc_download.php/3215/pdf/imm3215.pdf +// + +#ifndef CERES_INTERNAL_LEVENBERG_MARQUARDT_H_ +#define CERES_INTERNAL_LEVENBERG_MARQUARDT_H_ + +#include "ceres/minimizer.h" +#include "ceres/solver.h" + +namespace ceres { +namespace internal { + +class Evaluator; +class LinearSolver; + +class LevenbergMarquardt : public Minimizer { + public: + virtual ~LevenbergMarquardt(); + + virtual void Minimize(const Minimizer::Options& options, + Evaluator* evaluator, + LinearSolver* linear_solver, + const double* initial_parameters, + double* final_parameters, + Solver::Summary* summary); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_LEVENBERG_MARQUARDT_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/linear_least_squares_problems.cc b/extern/libmv/third_party/ceres/internal/ceres/linear_least_squares_problems.cc new file mode 100644 index 00000000000..cca9f442fe7 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/linear_least_squares_problems.cc @@ -0,0 +1,744 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/linear_least_squares_problems.h" + +#include <cstdio> +#include <string> +#include <vector> +#include <glog/logging.h> +#include "ceres/block_sparse_matrix.h" +#include "ceres/block_structure.h" +#include "ceres/casts.h" +#include "ceres/compressed_row_sparse_matrix.h" +#include "ceres/file.h" +#include "ceres/matrix_proto.h" +#include "ceres/triplet_sparse_matrix.h" +#include "ceres/stringprintf.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +LinearLeastSquaresProblem* CreateLinearLeastSquaresProblemFromId(int id) { + switch (id) { + case 0: + return LinearLeastSquaresProblem0(); + case 1: + return LinearLeastSquaresProblem1(); + case 2: + return LinearLeastSquaresProblem2(); + case 3: + return LinearLeastSquaresProblem3(); + default: + LOG(FATAL) << "Unknown problem id requested " << id; + } + return NULL; +} + +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS +LinearLeastSquaresProblem* CreateLinearLeastSquaresProblemFromFile( + const string& filename) { + LinearLeastSquaresProblemProto problem_proto; + { + string serialized_proto; + ReadFileToStringOrDie(filename, &serialized_proto); + CHECK(problem_proto.ParseFromString(serialized_proto)); + } + + LinearLeastSquaresProblem* problem = new LinearLeastSquaresProblem; + const SparseMatrixProto& A = problem_proto.a(); + + if (A.has_block_matrix()) { + problem->A.reset(new BlockSparseMatrix(A)); + } else if (A.has_triplet_matrix()) { + problem->A.reset(new TripletSparseMatrix(A)); + } else { + problem->A.reset(new CompressedRowSparseMatrix(A)); + } + + if (problem_proto.b_size() > 0) { + problem->b.reset(new double[problem_proto.b_size()]); + for (int i = 0; i < problem_proto.b_size(); ++i) { + problem->b[i] = problem_proto.b(i); + } + } + + if (problem_proto.d_size() > 0) { + problem->D.reset(new double[problem_proto.d_size()]); + for (int i = 0; i < problem_proto.d_size(); ++i) { + problem->D[i] = problem_proto.d(i); + } + } + + if (problem_proto.d_size() > 0) { + if (problem_proto.x_size() > 0) { + problem->x_D.reset(new double[problem_proto.x_size()]); + for (int i = 0; i < problem_proto.x_size(); ++i) { + problem->x_D[i] = problem_proto.x(i); + } + } + } else { + if (problem_proto.x_size() > 0) { + problem->x.reset(new double[problem_proto.x_size()]); + for (int i = 0; i < problem_proto.x_size(); ++i) { + problem->x[i] = problem_proto.x(i); + } + } + } + + problem->num_eliminate_blocks = 0; + if (problem_proto.has_num_eliminate_blocks()) { + problem->num_eliminate_blocks = problem_proto.num_eliminate_blocks(); + } + + return problem; +} +#else +LinearLeastSquaresProblem* CreateLinearLeastSquaresProblemFromFile( + const string& filename) { + LOG(FATAL) + << "Loading a least squares problem from disk requires " + << "Ceres to be built with Protocol Buffers support."; + return NULL; +} +#endif // CERES_DONT_HAVE_PROTOCOL_BUFFERS + +/* +A = [1 2] + [3 4] + [6 -10] + +b = [ 8 + 18 + -18] + +x = [2 + 3] + +D = [1 + 2] + +x_D = [1.78448275; + 2.82327586;] + */ +LinearLeastSquaresProblem* LinearLeastSquaresProblem0() { + LinearLeastSquaresProblem* problem = new LinearLeastSquaresProblem; + + TripletSparseMatrix* A = new TripletSparseMatrix(3, 2, 6); + problem->b.reset(new double[3]); + problem->D.reset(new double[2]); + + problem->x.reset(new double[2]); + problem->x_D.reset(new double[2]); + + int* Ai = A->mutable_rows(); + int* Aj = A->mutable_cols(); + double* Ax = A->mutable_values(); + + int counter = 0; + for (int i = 0; i < 3; ++i) { + for (int j = 0; j< 2; ++j) { + Ai[counter]=i; + Aj[counter]=j; + ++counter; + } + }; + + Ax[0] = 1.; + Ax[1] = 2.; + Ax[2] = 3.; + Ax[3] = 4.; + Ax[4] = 6; + Ax[5] = -10; + A->set_num_nonzeros(6); + problem->A.reset(A); + + problem->b[0] = 8; + problem->b[1] = 18; + problem->b[2] = -18; + + problem->x[0] = 2.0; + problem->x[1] = 3.0; + + problem->D[0] = 1; + problem->D[1] = 2; + + problem->x_D[0] = 1.78448275; + problem->x_D[1] = 2.82327586; + return problem; +} + + +/* + A = [1 0 | 2 0 0 + 3 0 | 0 4 0 + 0 5 | 0 0 6 + 0 7 | 8 0 0 + 0 9 | 1 0 0 + 0 0 | 1 1 1] + + b = [0 + 1 + 2 + 3 + 4 + 5] + + c = A'* b = [ 3 + 67 + 33 + 9 + 17] + + A'A = [10 0 2 12 0 + 0 155 65 0 30 + 2 65 70 1 1 + 12 0 1 17 1 + 0 30 1 1 37] + + S = [ 42.3419 -1.4000 -11.5806 + -1.4000 2.6000 1.0000 + 11.5806 1.0000 31.1935] + + r = [ 4.3032 + 5.4000 + 5.0323] + + S\r = [ 0.2102 + 2.1367 + 0.1388] + + A\b = [-2.3061 + 0.3172 + 0.2102 + 2.1367 + 0.1388] +*/ +// The following two functions create a TripletSparseMatrix and a +// BlockSparseMatrix version of this problem. + +// TripletSparseMatrix version. +LinearLeastSquaresProblem* LinearLeastSquaresProblem1() { + int num_rows = 6; + int num_cols = 5; + + LinearLeastSquaresProblem* problem = new LinearLeastSquaresProblem; + TripletSparseMatrix* A = new TripletSparseMatrix(num_rows, + num_cols, + num_rows * num_cols); + problem->b.reset(new double[num_rows]); + problem->D.reset(new double[num_cols]); + problem->num_eliminate_blocks = 2; + + int* rows = A->mutable_rows(); + int* cols = A->mutable_cols(); + double* values = A->mutable_values(); + + int nnz = 0; + + // Row 1 + { + rows[nnz] = 0; + cols[nnz] = 0; + values[nnz++] = 1; + + rows[nnz] = 0; + cols[nnz] = 2; + values[nnz++] = 2; + } + + // Row 2 + { + rows[nnz] = 1; + cols[nnz] = 0; + values[nnz++] = 3; + + rows[nnz] = 1; + cols[nnz] = 3; + values[nnz++] = 4; + } + + // Row 3 + { + rows[nnz] = 2; + cols[nnz] = 1; + values[nnz++] = 5; + + rows[nnz] = 2; + cols[nnz] = 4; + values[nnz++] = 6; + } + + // Row 4 + { + rows[nnz] = 3; + cols[nnz] = 1; + values[nnz++] = 7; + + rows[nnz] = 3; + cols[nnz] = 2; + values[nnz++] = 8; + } + + // Row 5 + { + rows[nnz] = 4; + cols[nnz] = 1; + values[nnz++] = 9; + + rows[nnz] = 4; + cols[nnz] = 2; + values[nnz++] = 1; + } + + // Row 6 + { + rows[nnz] = 5; + cols[nnz] = 2; + values[nnz++] = 1; + + rows[nnz] = 5; + cols[nnz] = 3; + values[nnz++] = 1; + + rows[nnz] = 5; + cols[nnz] = 4; + values[nnz++] = 1; + } + + A->set_num_nonzeros(nnz); + CHECK(A->IsValid()); + + problem->A.reset(A); + + for (int i = 0; i < num_cols; ++i) { + problem->D.get()[i] = 1; + } + + for (int i = 0; i < num_rows; ++i) { + problem->b.get()[i] = i; + } + + return problem; +} + +// BlockSparseMatrix version +LinearLeastSquaresProblem* LinearLeastSquaresProblem2() { + int num_rows = 6; + int num_cols = 5; + + LinearLeastSquaresProblem* problem = new LinearLeastSquaresProblem; + + problem->b.reset(new double[num_rows]); + problem->D.reset(new double[num_cols]); + problem->num_eliminate_blocks = 2; + + CompressedRowBlockStructure* bs = new CompressedRowBlockStructure; + scoped_array<double> values(new double[num_rows * num_cols]); + + for (int c = 0; c < num_cols; ++c) { + bs->cols.push_back(Block()); + bs->cols.back().size = 1; + bs->cols.back().position = c; + } + + int nnz = 0; + + // Row 1 + { + values[nnz++] = 1; + values[nnz++] = 2; + + bs->rows.push_back(CompressedRow()); + CompressedRow& row = bs->rows.back(); + row.block.size = 1; + row.block.position = 0; + row.cells.push_back(Cell(0, 0)); + row.cells.push_back(Cell(2, 1)); + } + + // Row 2 + { + values[nnz++] = 3; + values[nnz++] = 4; + + bs->rows.push_back(CompressedRow()); + CompressedRow& row = bs->rows.back(); + row.block.size = 1; + row.block.position = 1; + row.cells.push_back(Cell(0, 2)); + row.cells.push_back(Cell(3, 3)); + } + + // Row 3 + { + values[nnz++] = 5; + values[nnz++] = 6; + + bs->rows.push_back(CompressedRow()); + CompressedRow& row = bs->rows.back(); + row.block.size = 1; + row.block.position = 2; + row.cells.push_back(Cell(1, 4)); + row.cells.push_back(Cell(4, 5)); + } + + // Row 4 + { + values[nnz++] = 7; + values[nnz++] = 8; + + bs->rows.push_back(CompressedRow()); + CompressedRow& row = bs->rows.back(); + row.block.size = 1; + row.block.position = 3; + row.cells.push_back(Cell(1, 6)); + row.cells.push_back(Cell(2, 7)); + } + + // Row 5 + { + values[nnz++] = 9; + values[nnz++] = 1; + + bs->rows.push_back(CompressedRow()); + CompressedRow& row = bs->rows.back(); + row.block.size = 1; + row.block.position = 4; + row.cells.push_back(Cell(1, 8)); + row.cells.push_back(Cell(2, 9)); + } + + // Row 6 + { + values[nnz++] = 1; + values[nnz++] = 1; + values[nnz++] = 1; + + bs->rows.push_back(CompressedRow()); + CompressedRow& row = bs->rows.back(); + row.block.size = 1; + row.block.position = 5; + row.cells.push_back(Cell(2, 10)); + row.cells.push_back(Cell(3, 11)); + row.cells.push_back(Cell(4, 12)); + } + + BlockSparseMatrix* A = new BlockSparseMatrix(bs); + memcpy(A->mutable_values(), values.get(), nnz * sizeof(*A->values())); + + for (int i = 0; i < num_cols; ++i) { + problem->D.get()[i] = 1; + } + + for (int i = 0; i < num_rows; ++i) { + problem->b.get()[i] = i; + } + + problem->A.reset(A); + + return problem; +} + + +/* + A = [1 0 + 3 0 + 0 5 + 0 7 + 0 9 + 0 0] + + b = [0 + 1 + 2 + 3 + 4 + 5] +*/ +// BlockSparseMatrix version +LinearLeastSquaresProblem* LinearLeastSquaresProblem3() { + int num_rows = 5; + int num_cols = 2; + + LinearLeastSquaresProblem* problem = new LinearLeastSquaresProblem; + + problem->b.reset(new double[num_rows]); + problem->D.reset(new double[num_cols]); + problem->num_eliminate_blocks = 2; + + CompressedRowBlockStructure* bs = new CompressedRowBlockStructure; + scoped_array<double> values(new double[num_rows * num_cols]); + + for (int c = 0; c < num_cols; ++c) { + bs->cols.push_back(Block()); + bs->cols.back().size = 1; + bs->cols.back().position = c; + } + + int nnz = 0; + + // Row 1 + { + values[nnz++] = 1; + bs->rows.push_back(CompressedRow()); + CompressedRow& row = bs->rows.back(); + row.block.size = 1; + row.block.position = 0; + row.cells.push_back(Cell(0, 0)); + } + + // Row 2 + { + values[nnz++] = 3; + bs->rows.push_back(CompressedRow()); + CompressedRow& row = bs->rows.back(); + row.block.size = 1; + row.block.position = 1; + row.cells.push_back(Cell(0, 1)); + } + + // Row 3 + { + values[nnz++] = 5; + bs->rows.push_back(CompressedRow()); + CompressedRow& row = bs->rows.back(); + row.block.size = 1; + row.block.position = 2; + row.cells.push_back(Cell(1, 2)); + } + + // Row 4 + { + values[nnz++] = 7; + bs->rows.push_back(CompressedRow()); + CompressedRow& row = bs->rows.back(); + row.block.size = 1; + row.block.position = 3; + row.cells.push_back(Cell(1, 3)); + } + + // Row 5 + { + values[nnz++] = 9; + bs->rows.push_back(CompressedRow()); + CompressedRow& row = bs->rows.back(); + row.block.size = 1; + row.block.position = 4; + row.cells.push_back(Cell(1, 4)); + } + + BlockSparseMatrix* A = new BlockSparseMatrix(bs); + memcpy(A->mutable_values(), values.get(), nnz * sizeof(*A->values())); + + for (int i = 0; i < num_cols; ++i) { + problem->D.get()[i] = 1; + } + + for (int i = 0; i < num_rows; ++i) { + problem->b.get()[i] = i; + } + + problem->A.reset(A); + + return problem; +} + +bool DumpLinearLeastSquaresProblemToConsole(const string& directory, + int iteration, + const SparseMatrix* A, + const double* D, + const double* b, + const double* x, + int num_eliminate_blocks) { + CHECK_NOTNULL(A); + Matrix AA; + A->ToDenseMatrix(&AA); + LOG(INFO) << "A^T: \n" << AA.transpose(); + + if (D != NULL) { + LOG(INFO) << "A's appended diagonal:\n" + << ConstVectorRef(D, A->num_cols()); + } + + if (b != NULL) { + LOG(INFO) << "b: \n" << ConstVectorRef(b, A->num_rows()); + } + + if (x != NULL) { + LOG(INFO) << "x: \n" << ConstVectorRef(x, A->num_cols()); + } + return true; +}; + +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS +bool DumpLinearLeastSquaresProblemToProtocolBuffer(const string& directory, + int iteration, + const SparseMatrix* A, + const double* D, + const double* b, + const double* x, + int num_eliminate_blocks) { + CHECK_NOTNULL(A); + LinearLeastSquaresProblemProto lsqp; + A->ToProto(lsqp.mutable_a()); + + if (D != NULL) { + for (int i = 0; i < A->num_cols(); ++i) { + lsqp.add_d(D[i]); + } + } + + if (b != NULL) { + for (int i = 0; i < A->num_rows(); ++i) { + lsqp.add_b(b[i]); + } + } + + if (x != NULL) { + for (int i = 0; i < A->num_cols(); ++i) { + lsqp.add_x(x[i]); + } + } + + lsqp.set_num_eliminate_blocks(num_eliminate_blocks); + string format_string = JoinPath(directory, + "lm_iteration_%03d.lsqp"); + string filename = + StringPrintf(format_string.c_str(), iteration); + LOG(INFO) << "Dumping least squares problem for iteration " << iteration + << " to disk. File: " << filename; + WriteStringToFileOrDie(lsqp.SerializeAsString(), filename); + return true; +} +#else +bool DumpLinearLeastSquaresProblemToProtocolBuffer(const string& directory, + int iteration, + const SparseMatrix* A, + const double* D, + const double* b, + const double* x, + int num_eliminate_blocks) { + LOG(ERROR) << "Dumping least squares problems is only " + << "supported when Ceres is compiled with " + << "protocol buffer support."; + return false; +} +#endif + +void WriteArrayToFileOrDie(const string& filename, + const double* x, + const int size) { + CHECK_NOTNULL(x); + VLOG(2) << "Writing array to: " << filename; + FILE* fptr = fopen(filename.c_str(), "w"); + CHECK_NOTNULL(fptr); + for (int i = 0; i < size; ++i) { + fprintf(fptr, "%17f\n", x[i]); + } + fclose(fptr); +} + +bool DumpLinearLeastSquaresProblemToTextFile(const string& directory, + int iteration, + const SparseMatrix* A, + const double* D, + const double* b, + const double* x, + int num_eliminate_blocks) { + CHECK_NOTNULL(A); + string format_string = JoinPath(directory, + "lm_iteration_%03d"); + string filename_prefix = + StringPrintf(format_string.c_str(), iteration); + + { + string filename = filename_prefix + "_A.txt"; + LOG(INFO) << "writing to: " << filename; + FILE* fptr = fopen(filename.c_str(), "w"); + CHECK_NOTNULL(fptr); + A->ToTextFile(fptr); + fclose(fptr); + } + + if (D != NULL) { + string filename = filename_prefix + "_D.txt"; + WriteArrayToFileOrDie(filename, D, A->num_cols()); + } + + if (b != NULL) { + string filename = filename_prefix + "_b.txt"; + WriteArrayToFileOrDie(filename, b, A->num_rows()); + } + + if (x != NULL) { + string filename = filename_prefix + "_x.txt"; + WriteArrayToFileOrDie(filename, x, A->num_cols()); + } + + return true; +} + +bool DumpLinearLeastSquaresProblem(const string& directory, + int iteration, + DumpFormatType dump_format_type, + const SparseMatrix* A, + const double* D, + const double* b, + const double* x, + int num_eliminate_blocks) { + switch (dump_format_type) { + case (CONSOLE): + return DumpLinearLeastSquaresProblemToConsole(directory, + iteration, + A, D, b, x, + num_eliminate_blocks); + case (PROTOBUF): + return DumpLinearLeastSquaresProblemToProtocolBuffer( + directory, + iteration, + A, D, b, x, + num_eliminate_blocks); + case (TEXTFILE): + return DumpLinearLeastSquaresProblemToTextFile(directory, + iteration, + A, D, b, x, + num_eliminate_blocks); + default: + LOG(FATAL) << "Unknown DumpFormatType " << dump_format_type; + }; + + return true; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/linear_least_squares_problems.h b/extern/libmv/third_party/ceres/internal/ceres/linear_least_squares_problems.h new file mode 100644 index 00000000000..553cc0d3db3 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/linear_least_squares_problems.h @@ -0,0 +1,87 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_LINEAR_LEAST_SQUARES_PROBLEMS_H_ +#define CERES_INTERNAL_LINEAR_LEAST_SQUARES_PROBLEMS_H_ + +#include <string> +#include <vector> +#include "ceres/sparse_matrix.h" +#include "ceres/internal/port.h" +#include "ceres/internal/scoped_ptr.h" + +namespace ceres { +namespace internal { + +// Structure defining a linear least squares problem and if possible +// ground truth solutions. To be used by various LinearSolver tests. +struct LinearLeastSquaresProblem { + LinearLeastSquaresProblem() + : A(NULL), b(NULL), D(NULL), num_eliminate_blocks(0), + x(NULL), x_D(NULL) { + } + + scoped_ptr<SparseMatrix> A; + scoped_array<double> b; + scoped_array<double> D; + // If using the schur eliminator then how many of the variable + // blocks are e_type blocks. + int num_eliminate_blocks; + + // Solution to min_x |Ax - b|^2 + scoped_array<double> x; + // Solution to min_x |Ax - b|^2 + |Dx|^2 + scoped_array<double> x_D; +}; + +// Factories for linear least squares problem. +LinearLeastSquaresProblem* CreateLinearLeastSquaresProblemFromId(int id); +LinearLeastSquaresProblem* CreateLinearLeastSquaresProblemFromFile( + const string& filename); + +LinearLeastSquaresProblem* LinearLeastSquaresProblem0(); +LinearLeastSquaresProblem* LinearLeastSquaresProblem1(); +LinearLeastSquaresProblem* LinearLeastSquaresProblem2(); +LinearLeastSquaresProblem* LinearLeastSquaresProblem3(); + +// Write the linear least squares problem to disk. The exact format +// depends on dump_format_type. +bool DumpLinearLeastSquaresProblem(const string& directory, + int iteration, + DumpFormatType dump_format_type, + const SparseMatrix* A, + const double* D, + const double* b, + const double* x, + int num_eliminate_blocks); +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_LINEAR_LEAST_SQUARES_PROBLEMS_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/linear_operator.cc b/extern/libmv/third_party/ceres/internal/ceres/linear_operator.cc new file mode 100644 index 00000000000..4b59fa13009 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/linear_operator.cc @@ -0,0 +1,40 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/linear_operator.h" + +namespace ceres { +namespace internal { + +LinearOperator::~LinearOperator() { +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/linear_operator.h b/extern/libmv/third_party/ceres/internal/ceres/linear_operator.h new file mode 100644 index 00000000000..d5c15cee6a9 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/linear_operator.h @@ -0,0 +1,59 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Base classes for access to an linear operator. + +#ifndef CERES_INTERNAL_LINEAR_OPERATOR_H_ +#define CERES_INTERNAL_LINEAR_OPERATOR_H_ + +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +// This is an abstract base class for linear operators. It supports +// access to size information and left and right multiply operators. +class LinearOperator { + public: + virtual ~LinearOperator(); + + // y = y + Ax; + virtual void RightMultiply(const double* x, double* y) const = 0; + // y = y + A'x; + virtual void LeftMultiply(const double* x, double* y) const = 0; + + virtual int num_rows() const = 0; + virtual int num_cols() const = 0; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_LINEAR_OPERATOR_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/linear_solver.cc b/extern/libmv/third_party/ceres/internal/ceres/linear_solver.cc new file mode 100644 index 00000000000..b2e3941eea1 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/linear_solver.cc @@ -0,0 +1,87 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/linear_solver.h" + +#include <glog/logging.h> +#include "ceres/cgnr_solver.h" +#include "ceres/dense_qr_solver.h" +#include "ceres/iterative_schur_complement_solver.h" +#include "ceres/schur_complement_solver.h" +#include "ceres/sparse_normal_cholesky_solver.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +LinearSolver::~LinearSolver() { +} + +LinearSolver* LinearSolver::Create(const LinearSolver::Options& options) { + switch (options.type) { + case CGNR: + return new CgnrSolver(options); + + case SPARSE_NORMAL_CHOLESKY: +#ifndef CERES_NO_SUITESPARSE + return new SparseNormalCholeskySolver(options); +#else + LOG(WARNING) << "SPARSE_NORMAL_CHOLESKY is not available. Please " + << "build Ceres with SuiteSparse. Returning NULL."; + return NULL; +#endif // CERES_NO_SUITESPARSE + + case SPARSE_SCHUR: +#ifndef CERES_NO_SUITESPARSE + return new SparseSchurComplementSolver(options); +#else + LOG(WARNING) << "SPARSE_SCHUR is not available. Please " + << "build Ceres with SuiteSparse. Returning NULL."; + return NULL; +#endif // CERES_NO_SUITESPARSE + + case DENSE_SCHUR: + return new DenseSchurComplementSolver(options); + + case ITERATIVE_SCHUR: + return new IterativeSchurComplementSolver(options); + + case DENSE_QR: + return new DenseQRSolver(options); + + default: + LOG(FATAL) << "Unknown linear solver type :" + << options.type; + return NULL; // MSVC doesn't understand that LOG(FATAL) never returns. + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/linear_solver.h b/extern/libmv/third_party/ceres/internal/ceres/linear_solver.h new file mode 100644 index 00000000000..5860ecc8a77 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/linear_solver.h @@ -0,0 +1,291 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Abstract interface for objects solving linear systems of various +// kinds. + +#ifndef CERES_INTERNAL_LINEAR_SOLVER_H_ +#define CERES_INTERNAL_LINEAR_SOLVER_H_ + +#include <cstddef> + +#include <glog/logging.h> +#include "ceres/block_sparse_matrix.h" +#include "ceres/casts.h" +#include "ceres/compressed_row_sparse_matrix.h" +#include "ceres/dense_sparse_matrix.h" +#include "ceres/triplet_sparse_matrix.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +class LinearOperator; + +// Abstract base class for objects that implement algorithms for +// solving linear systems +// +// Ax = b +// +// It is expected that a single instance of a LinearSolver object +// maybe used multiple times for solving different linear +// systems. This allows them to cache and reuse information across +// solves if for example the sparsity of the linear system remains +// constant. +// +// Subclasses of LinearSolver use two structs to configure themselves. +// The Options struct configures the LinearSolver object for its +// lifetime. The PerSolveOptions struct is used to specify options for +// a particular Solve call. +class LinearSolver { + public: + struct Options { + Options() + : type(SPARSE_NORMAL_CHOLESKY), + preconditioner_type(JACOBI), + min_num_iterations(1), + max_num_iterations(1), + num_threads(1), + constant_sparsity(false), + num_eliminate_blocks(0), + residual_reset_period(10), + row_block_size(Dynamic), + e_block_size(Dynamic), + f_block_size(Dynamic) { + } + + LinearSolverType type; + + PreconditionerType preconditioner_type; + + // Number of internal iterations that the solver uses. This + // parameter only makes sense for iterative solvers like CG. + int min_num_iterations; + int max_num_iterations; + + // If possible, how many threads can the solver use. + int num_threads; + + // If possible cache and reuse the symbolic factorization across + // multiple calls. + bool constant_sparsity; + + // Eliminate 0 to num_eliminate_blocks - 1 from the Normal + // equations to form a schur complement. Only used by the Schur + // complement based solver. The most common use for this parameter + // is in the case of structure from motion problems where we have + // camera blocks and point blocks. Then setting the + // num_eliminate_blocks to the number of points allows the solver + // to use the Schur complement trick. For more details see the + // description of this parameter in solver.h. + int num_eliminate_blocks; + + // Iterative solvers, e.g. Preconditioned Conjugate Gradients + // maintain a cheap estimate of the residual which may become + // inaccurate over time. Thus for non-zero values of this + // parameter, the solver can be told to recalculate the value of + // the residual using a |b - Ax| evaluation. + int residual_reset_period; + + // If the block sizes in a BlockSparseMatrix are fixed, then in + // some cases the Schur complement based solvers can detect and + // specialize on them. + // + // It is expected that these parameters are set programmatically + // rather than manually. + // + // Please see explicit_schur_complement_solver_impl.h for more + // details. + int row_block_size; + int e_block_size; + int f_block_size; + }; + + // Options for the Solve method. + struct PerSolveOptions { + PerSolveOptions() + : D(NULL), + preconditioner(NULL), + r_tolerance(0.0), + q_tolerance(0.0) { + } + + // This option only makes sense for unsymmetric linear solvers + // that can solve rectangular linear systems. + // + // Given a matrix A, an optional diagonal matrix D as a vector, + // and a vector b, the linear solver will solve for + // + // | A | x = | b | + // | D | | 0 | + // + // If D is null, then it is treated as zero, and the solver returns + // the solution to + // + // A x = b + // + // In either case, x is the vector that solves the following + // optimization problem. + // + // arg min_x ||Ax - b||^2 + ||Dx||^2 + // + // Here A is a matrix of size m x n, with full column rank. If A + // does not have full column rank, the results returned by the + // solver cannot be relied on. D, if it is not null is an array of + // size n. b is an array of size m and x is an array of size n. + double * D; + + // This option only makes sense for iterative solvers. + // + // In general the performance of an iterative linear solver + // depends on the condition number of the matrix A. For example + // the convergence rate of the conjugate gradients algorithm + // is proportional to the square root of the condition number. + // + // One particularly useful technique for improving the + // conditioning of a linear system is to precondition it. In its + // simplest form a preconditioner is a matrix M such that instead + // of solving Ax = b, we solve the linear system AM^{-1} y = b + // instead, where M is such that the condition number k(AM^{-1}) + // is smaller than the conditioner k(A). Given the solution to + // this system, x = M^{-1} y. The iterative solver takes care of + // the mechanics of solving the preconditioned system and + // returning the corrected solution x. The user only needs to + // supply a linear operator. + // + // A null preconditioner is equivalent to an identity matrix being + // used a preconditioner. + LinearOperator* preconditioner; + + + // The following tolerance related options only makes sense for + // iterative solvers. Direct solvers ignore them. + + // Solver terminates when + // + // |Ax - b| <= r_tolerance * |b|. + // + // This is the most commonly used termination criterion for + // iterative solvers. + double r_tolerance; + + // For PSD matrices A, let + // + // Q(x) = x'Ax - 2b'x + // + // be the cost of the quadratic function defined by A and b. Then, + // the solver terminates at iteration i if + // + // i * (Q(x_i) - Q(x_i-1)) / Q(x_i) < q_tolerance. + // + // This termination criterion is more useful when using CG to + // solve the Newton step. This particular convergence test comes + // from Stephen Nash's work on truncated Newton + // methods. References: + // + // 1. Stephen G. Nash & Ariela Sofer, Assessing A Search + // Direction Within A Truncated Newton Method, Operation + // Research Letters 9(1990) 219-221. + // + // 2. Stephen G. Nash, A Survey of Truncated Newton Methods, + // Journal of Computational and Applied Mathematics, + // 124(1-2), 45-59, 2000. + // + double q_tolerance; + }; + + // Summary of a call to the Solve method. We should move away from + // the true/false method for determining solver success. We should + // let the summary object do the talking. + struct Summary { + Summary() + : residual_norm(0.0), + num_iterations(-1), + termination_type(FAILURE) { + } + + double residual_norm; + int num_iterations; + LinearSolverTerminationType termination_type; + }; + + virtual ~LinearSolver(); + + // Solve Ax = b. + virtual Summary Solve(LinearOperator* A, + const double* b, + const PerSolveOptions& per_solve_options, + double* x) = 0; + + static LinearSolver* Create(const Options& options); +}; + +// This templated subclass of LinearSolver serves as a base class for +// other linear solvers that depend on the particular matrix layout of +// the underlying linear operator. For example some linear solvers +// need low level access to the TripletSparseMatrix implementing the +// LinearOperator interface. This class hides those implementation +// details behind a private virtual method, and has the Solve method +// perform the necessary upcasting. +template <typename MatrixType> +class TypedLinearSolver : public LinearSolver { + public: + virtual ~TypedLinearSolver() {} + virtual LinearSolver::Summary Solve( + LinearOperator* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x) { + CHECK_NOTNULL(A); + CHECK_NOTNULL(b); + CHECK_NOTNULL(x); + return SolveImpl(down_cast<MatrixType*>(A), b, per_solve_options, x); + } + + private: + virtual LinearSolver::Summary SolveImpl( + MatrixType* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x) = 0; +}; + +// Linear solvers that depend on acccess to the low level structure of +// a SparseMatrix. +typedef TypedLinearSolver<BlockSparseMatrix> BlockSparseMatrixSolver; // NOLINT +typedef TypedLinearSolver<BlockSparseMatrixBase> BlockSparseMatrixBaseSolver; // NOLINT +typedef TypedLinearSolver<CompressedRowSparseMatrix> CompressedRowSparseMatrixSolver; // NOLINT +typedef TypedLinearSolver<DenseSparseMatrix> DenseSparseMatrixSolver; // NOLINT +typedef TypedLinearSolver<TripletSparseMatrix> TripletSparseMatrixSolver; // NOLINT + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_LINEAR_SOLVER_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/local_parameterization.cc b/extern/libmv/third_party/ceres/internal/ceres/local_parameterization.cc new file mode 100644 index 00000000000..eeae74e3f95 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/local_parameterization.cc @@ -0,0 +1,140 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include <glog/logging.h> +#include "ceres/internal/eigen.h" +#include "ceres/local_parameterization.h" +#include "ceres/rotation.h" + +namespace ceres { + +IdentityParameterization::IdentityParameterization(const int size) + : size_(size) { + CHECK_GT(size, 0); +} + +bool IdentityParameterization::Plus(const double* x, + const double* delta, + double* x_plus_delta) const { + VectorRef(x_plus_delta, size_) = + ConstVectorRef(x, size_) + ConstVectorRef(delta, size_); + return true; +} + +bool IdentityParameterization::ComputeJacobian(const double* x, + double* jacobian) const { + MatrixRef(jacobian, size_, size_) = Matrix::Identity(size_, size_); + return true; +} + +SubsetParameterization::SubsetParameterization( + int size, + const vector<int>& constant_parameters) + : local_size_(size - constant_parameters.size()), + constancy_mask_(size, 0) { + CHECK_GT(constant_parameters.size(), 0) + << "The set of constant parameters should contain at least " + << "one element. If you do not wish to hold any parameters " + << "constant, then do not use a SubsetParameterization"; + + vector<int> constant = constant_parameters; + sort(constant.begin(), constant.end()); + CHECK(unique(constant.begin(), constant.end()) == constant.end()) + << "The set of constant parameters cannot contain duplicates"; + CHECK_LT(constant_parameters.size(), size) + << "Number of parameters held constant should be less " + << "than the size of the parameter block. If you wish " + << "to hold the entire parameter block constant, then a " + << "efficient way is to directly mark it as constant " + << "instead of using a LocalParameterization to do so."; + CHECK_GE(*min_element(constant.begin(), constant.end()), 0); + CHECK_LT(*max_element(constant.begin(), constant.end()), size); + + for (int i = 0; i < constant_parameters.size(); ++i) { + constancy_mask_[constant_parameters[i]] = 1; + } +} + +bool SubsetParameterization::Plus(const double* x, + const double* delta, + double* x_plus_delta) const { + for (int i = 0, j = 0; i < constancy_mask_.size(); ++i) { + if (constancy_mask_[i]) { + x_plus_delta[i] = x[i]; + } else { + x_plus_delta[i] = x[i] + delta[j++]; + } + } + return true; +} + +bool SubsetParameterization::ComputeJacobian(const double* x, + double* jacobian) const { + MatrixRef m(jacobian, constancy_mask_.size(), local_size_); + m.setZero(); + for (int i = 0, j = 0; i < constancy_mask_.size(); ++i) { + if (!constancy_mask_[i]) { + m(i, j++) = 1.0; + } + } + return true; +} + +bool QuaternionParameterization::Plus(const double* x, + const double* delta, + double* x_plus_delta) const { + const double norm_delta = + sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]); + if (norm_delta > 0.0) { + const double sin_delta_by_delta = (sin(norm_delta) / norm_delta); + double q_delta[4]; + q_delta[0] = cos(norm_delta); + q_delta[1] = sin_delta_by_delta * delta[0]; + q_delta[2] = sin_delta_by_delta * delta[1]; + q_delta[3] = sin_delta_by_delta * delta[2]; + QuaternionProduct(q_delta, x, x_plus_delta); + } else { + for (int i = 0; i < 4; ++i) { + x_plus_delta[i] = x[i]; + } + } + return true; +} + +bool QuaternionParameterization::ComputeJacobian(const double* x, + double* jacobian) const { + jacobian[0] = -x[1]; jacobian[1] = -x[2]; jacobian[2] = -x[3]; // NOLINT + jacobian[3] = x[0]; jacobian[4] = x[3]; jacobian[5] = -x[2]; // NOLINT + jacobian[6] = -x[3]; jacobian[7] = x[0]; jacobian[8] = x[1]; // NOLINT + jacobian[9] = x[2]; jacobian[10] = -x[1]; jacobian[11] = x[0]; // NOLINT + return true; +} + +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/loss_function.cc b/extern/libmv/third_party/ceres/internal/ceres/loss_function.cc new file mode 100644 index 00000000000..00b2b184729 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/loss_function.cc @@ -0,0 +1,93 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Purpose: See .h file. + +#include "ceres/loss_function.h" + +#include <cmath> +#include <cstddef> + +namespace ceres { + +void TrivialLoss::Evaluate(double s, double rho[3]) const { + rho[0] = s; + rho[1] = 1; + rho[2] = 0; +} + +void HuberLoss::Evaluate(double s, double rho[3]) const { + if (s > b_) { + // Outlier region. + // 'r' is always positive. + const double r = sqrt(s); + rho[0] = 2 * a_ * r - b_; + rho[1] = a_ / r; + rho[2] = - rho[1] / (2 * s); + } else { + // Inlier region. + rho[0] = s; + rho[1] = 1; + rho[2] = 0; + } +} + +void SoftLOneLoss::Evaluate(double s, double rho[3]) const { + const double sum = 1 + s * c_; + const double tmp = sqrt(sum); + // 'sum' and 'tmp' are always positive, assuming that 's' is. + rho[0] = 2 * b_ * (tmp - 1); + rho[1] = 1 / tmp; + rho[2] = - (c_ * rho[1]) / (2 * sum); +} + +void CauchyLoss::Evaluate(double s, double rho[3]) const { + const double sum = 1 + s * c_; + const double inv = 1 / sum; + // 'sum' and 'inv' are always positive, assuming that 's' is. + rho[0] = b_ * log(sum); + rho[1] = inv; + rho[2] = - c_ * (inv * inv); +} + +void ScaledLoss::Evaluate(double s, double rho[3]) const { + if (rho_.get() == NULL) { + rho[0] = a_ * s; + rho[1] = a_; + rho[2] = 0.0; + } else { + rho_->Evaluate(s, rho); + rho[0] *= a_; + rho[1] *= a_; + rho[2] *= a_; + } +} + +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/map_util.h b/extern/libmv/third_party/ceres/internal/ceres/map_util.h new file mode 100644 index 00000000000..ddf1252f674 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/map_util.h @@ -0,0 +1,129 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 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: keir@google.com (Keir Mierle) +// +// Originally by Anton Carver + +#ifndef CERES_INTERNAL_MAP_UTIL_H_ +#define CERES_INTERNAL_MAP_UTIL_H_ + +#include <utility> +#include "ceres/internal/port.h" + +namespace ceres { + +// Perform a lookup in a map or hash_map, assuming that the key exists. +// Crash if it does not. +// +// This is intended as a replacement for operator[] as an rvalue (for reading) +// when the key is guaranteed to exist. +// +// operator[] is discouraged for several reasons: +// * It has a side-effect of inserting missing keys +// * It is not thread-safe (even when it is not inserting, it can still +// choose to resize the underlying storage) +// * It invalidates iterators (when it chooses to resize) +// * It default constructs a value object even if it doesn't need to +// +// This version assumes the key is printable, and includes it in the fatal log +// message. +template <class Collection> +const typename Collection::value_type::second_type& +FindOrDie(const Collection& collection, + const typename Collection::value_type::first_type& key) { + typename Collection::const_iterator it = collection.find(key); + CHECK(it != collection.end()) << "Map key not found: " << key; + return it->second; +} + +// Perform a lookup in a map or hash_map. +// If the key is present in the map then the value associated with that +// key is returned, otherwise the value passed as a default is returned. +template <class Collection> +const typename Collection::value_type::second_type& +FindWithDefault(const Collection& collection, + const typename Collection::value_type::first_type& key, + const typename Collection::value_type::second_type& value) { + typename Collection::const_iterator it = collection.find(key); + if (it == collection.end()) { + return value; + } + return it->second; +} + +// Insert a new key and value into a map or hash_map. +// If the key is not present in the map the key and value are +// inserted, otherwise nothing happens. True indicates that an insert +// took place, false indicates the key was already present. +template <class Collection> +bool InsertIfNotPresent( + Collection * const collection, + const typename Collection::value_type::first_type& key, + const typename Collection::value_type::second_type& value) { + pair<typename Collection::iterator, bool> ret = + collection->insert(typename Collection::value_type(key, value)); + return ret.second; +} + +// Perform a lookup in a map or hash_map. +// Same as above but the returned pointer is not const and can be used to change +// the stored value. +template <class Collection> +typename Collection::value_type::second_type* +FindOrNull(Collection& collection, // NOLINT + const typename Collection::value_type::first_type& key) { + typename Collection::iterator it = collection.find(key); + if (it == collection.end()) { + return 0; + } + return &it->second; +} + +// Test to see if a set, map, hash_set or hash_map contains a particular key. +// Returns true if the key is in the collection. +template <class Collection, class Key> +bool ContainsKey(const Collection& collection, const Key& key) { + typename Collection::const_iterator it = collection.find(key); + return it != collection.end(); +} + +// Inserts a new key/value into a map or hash_map. +// Dies if the key is already present. +template<class Collection> +void InsertOrDie(Collection* const collection, + const typename Collection::value_type::first_type& key, + const typename Collection::value_type::second_type& data) { + typedef typename Collection::value_type value_type; + CHECK(collection->insert(value_type(key, data)).second) + << "duplicate key: " << key; +} + +} // namespace ceres + +#endif // CERES_INTERNAL_MAP_UTIL_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/matrix_proto.h b/extern/libmv/third_party/ceres/internal/ceres/matrix_proto.h new file mode 100644 index 00000000000..b8a3a1a6de6 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/matrix_proto.h @@ -0,0 +1,40 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 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: keir@google.com (Keir Mierle) +// +// A portability header to make optional protocol buffer support less intrusive. + +#ifndef CERES_INTERNAL_MATRIX_PROTO_H_ +#define CERES_INTERNAL_MATRIX_PROTO_H_ + +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS +#include "ceres/matrix.pb.h" +#endif + +#endif // CERES_INTERNAL_MATRIX_PROTO_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/minimizer.h b/extern/libmv/third_party/ceres/internal/ceres/minimizer.h new file mode 100644 index 00000000000..77cb00cb6b4 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/minimizer.h @@ -0,0 +1,104 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_MINIMIZER_H_ +#define CERES_INTERNAL_MINIMIZER_H_ + +#include <vector> +#include "ceres/solver.h" +#include "ceres/iteration_callback.h" + +namespace ceres { +namespace internal { + +class Evaluator; +class LinearSolver; + +// Interface for non-linear least squares solvers. +class Minimizer { + public: + // Options struct to control the behaviour of the Minimizer. Please + // see solver.h for detailed information about the meaning and + // default values of each of these parameters. + struct Options { + explicit Options(const Solver::Options& options) { + max_num_iterations = options.max_num_iterations; + max_solver_time_sec = options.max_solver_time_sec; + gradient_tolerance = options.gradient_tolerance; + parameter_tolerance = options.parameter_tolerance; + function_tolerance = options.function_tolerance; + min_relative_decrease = options.min_relative_decrease; + eta = options.eta; + tau = options.tau; + jacobi_scaling = options.jacobi_scaling; + crash_and_dump_lsqp_on_failure = options.crash_and_dump_lsqp_on_failure; + lsqp_dump_directory = options.lsqp_dump_directory; + lsqp_iterations_to_dump = options.lsqp_iterations_to_dump; + lsqp_dump_format_type = options.lsqp_dump_format_type; + num_eliminate_blocks = options.num_eliminate_blocks; + logging_type = options.logging_type; + } + + int max_num_iterations; + int max_solver_time_sec; + double gradient_tolerance; + double parameter_tolerance; + double function_tolerance; + double min_relative_decrease; + double eta; + double tau; + bool jacobi_scaling; + bool crash_and_dump_lsqp_on_failure; + vector<int> lsqp_iterations_to_dump; + DumpFormatType lsqp_dump_format_type; + string lsqp_dump_directory; + int num_eliminate_blocks; + LoggingType logging_type; + + // List of callbacks that are executed by the Minimizer at the end + // of each iteration. + // + // Client owns these pointers. + vector<IterationCallback*> callbacks; + }; + + virtual ~Minimizer() {} + virtual void Minimize(const Options& options, + Evaluator* evaluator, + LinearSolver* linear_solver, + const double* initial_parameters, + double* final_parameters, + Solver::Summary* summary) = 0; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_MINIMIZER_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/mutex.h b/extern/libmv/third_party/ceres/internal/ceres/mutex.h new file mode 100644 index 00000000000..6514b107041 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/mutex.h @@ -0,0 +1,312 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 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: Craig Silverstein. +// +// A simple mutex wrapper, supporting locks and read-write locks. +// You should assume the locks are *not* re-entrant. +// +// This class is meant to be internal-only and should be wrapped by an +// internal namespace. Before you use this module, please give the +// name of your internal namespace for this module. Or, if you want +// to expose it, you'll want to move it to the Google namespace. We +// cannot put this class in global namespace because there can be some +// problems when we have multiple versions of Mutex in each shared object. +// +// NOTE: by default, we have #ifdef'ed out the TryLock() method. +// This is for two reasons: +// 1) TryLock() under Windows is a bit annoying (it requires a +// #define to be defined very early). +// 2) TryLock() is broken for NO_THREADS mode, at least in NDEBUG +// mode. +// If you need TryLock(), and either these two caveats are not a +// problem for you, or you're willing to work around them, then +// feel free to #define GMUTEX_TRYLOCK, or to remove the #ifdefs +// in the code below. +// +// CYGWIN NOTE: Cygwin support for rwlock seems to be buggy: +// http://www.cygwin.com/ml/cygwin/2008-12/msg00017.html +// Because of that, we might as well use windows locks for +// cygwin. They seem to be more reliable than the cygwin pthreads layer. +// +// TRICKY IMPLEMENTATION NOTE: +// This class is designed to be safe to use during +// dynamic-initialization -- that is, by global constructors that are +// run before main() starts. The issue in this case is that +// dynamic-initialization happens in an unpredictable order, and it +// could be that someone else's dynamic initializer could call a +// function that tries to acquire this mutex -- but that all happens +// before this mutex's constructor has run. (This can happen even if +// the mutex and the function that uses the mutex are in the same .cc +// file.) Basically, because Mutex does non-trivial work in its +// constructor, it's not, in the naive implementation, safe to use +// before dynamic initialization has run on it. +// +// The solution used here is to pair the actual mutex primitive with a +// bool that is set to true when the mutex is dynamically initialized. +// (Before that it's false.) Then we modify all mutex routines to +// look at the bool, and not try to lock/unlock until the bool makes +// it to true (which happens after the Mutex constructor has run.) +// +// This works because before main() starts -- particularly, during +// dynamic initialization -- there are no threads, so a) it's ok that +// the mutex operations are a no-op, since we don't need locking then +// anyway; and b) we can be quite confident our bool won't change +// state between a call to Lock() and a call to Unlock() (that would +// require a global constructor in one translation unit to call Lock() +// and another global constructor in another translation unit to call +// Unlock() later, which is pretty perverse). +// +// That said, it's tricky, and can conceivably fail; it's safest to +// avoid trying to acquire a mutex in a global constructor, if you +// can. One way it can fail is that a really smart compiler might +// initialize the bool to true at static-initialization time (too +// early) rather than at dynamic-initialization time. To discourage +// that, we set is_safe_ to true in code (not the constructor +// colon-initializer) and set it to true via a function that always +// evaluates to true, but that the compiler can't know always +// evaluates to true. This should be good enough. + +#ifndef CERES_INTERNAL_MUTEX_H_ +#define CERES_INTERNAL_MUTEX_H_ + +#if defined(NO_THREADS) + typedef int MutexType; // to keep a lock-count +#elif defined(_WIN32) || defined(__CYGWIN32__) || defined(__CYGWIN64__) +# define WIN32_LEAN_AND_MEAN // We only need minimal includes +# ifdef GMUTEX_TRYLOCK + // We need Windows NT or later for TryEnterCriticalSection(). If you + // don't need that functionality, you can remove these _WIN32_WINNT + // lines, and change TryLock() to assert(0) or something. +# ifndef _WIN32_WINNT +# define _WIN32_WINNT 0x0400 +# endif +# endif +// To avoid macro definition of ERROR. +# define NOGDI +// To avoid macro definition of min/max. +# define NOMINMAX +# include <windows.h> + typedef CRITICAL_SECTION MutexType; +#elif defined(CERES_HAVE_PTHREAD) && defined(CERES_HAVE_RWLOCK) + // Needed for pthread_rwlock_*. If it causes problems, you could take it + // out, but then you'd have to unset CERES_HAVE_RWLOCK (at least on linux -- + // it *does* cause problems for FreeBSD, or MacOSX, but isn't needed for + // locking there.) +# if defined(__linux__) && !defined(_XOPEN_SOURCE) +# define _XOPEN_SOURCE 500 // may be needed to get the rwlock calls +# endif +# include <pthread.h> + typedef pthread_rwlock_t MutexType; +#elif defined(CERES_HAVE_PTHREAD) +# include <pthread.h> + typedef pthread_mutex_t MutexType; +#else +# error Need to implement mutex.h for your architecture, or #define NO_THREADS +#endif + +// We need to include these header files after defining _XOPEN_SOURCE +// as they may define the _XOPEN_SOURCE macro. +#include <assert.h> +#include <stdlib.h> // for abort() + +namespace ceres { +namespace internal { + +class Mutex { + public: + // Create a Mutex that is not held by anybody. This constructor is + // typically used for Mutexes allocated on the heap or the stack. + // See below for a recommendation for constructing global Mutex + // objects. + inline Mutex(); + + // Destructor + inline ~Mutex(); + + inline void Lock(); // Block if needed until free then acquire exclusively + inline void Unlock(); // Release a lock acquired via Lock() +#ifdef GMUTEX_TRYLOCK + inline bool TryLock(); // If free, Lock() and return true, else return false +#endif + // Note that on systems that don't support read-write locks, these may + // be implemented as synonyms to Lock() and Unlock(). So you can use + // these for efficiency, but don't use them anyplace where being able + // to do shared reads is necessary to avoid deadlock. + inline void ReaderLock(); // Block until free or shared then acquire a share + inline void ReaderUnlock(); // Release a read share of this Mutex + inline void WriterLock() { Lock(); } // Acquire an exclusive lock + inline void WriterUnlock() { Unlock(); } // Release a lock from WriterLock() + + // TODO(hamaji): Do nothing, implement correctly. + inline void AssertHeld() {} + + private: + MutexType mutex_; + // We want to make sure that the compiler sets is_safe_ to true only + // when we tell it to, and never makes assumptions is_safe_ is + // always true. volatile is the most reliable way to do that. + volatile bool is_safe_; + + inline void SetIsSafe() { is_safe_ = true; } + + // Catch the error of writing Mutex when intending MutexLock. + Mutex(Mutex* /*ignored*/) {} + // Disallow "evil" constructors + Mutex(const Mutex&); + void operator=(const Mutex&); +}; + +// Now the implementation of Mutex for various systems +#if defined(NO_THREADS) + +// When we don't have threads, we can be either reading or writing, +// but not both. We can have lots of readers at once (in no-threads +// mode, that's most likely to happen in recursive function calls), +// but only one writer. We represent this by having mutex_ be -1 when +// writing and a number > 0 when reading (and 0 when no lock is held). +// +// In debug mode, we assert these invariants, while in non-debug mode +// we do nothing, for efficiency. That's why everything is in an +// assert. + +Mutex::Mutex() : mutex_(0) { } +Mutex::~Mutex() { assert(mutex_ == 0); } +void Mutex::Lock() { assert(--mutex_ == -1); } +void Mutex::Unlock() { assert(mutex_++ == -1); } +#ifdef GMUTEX_TRYLOCK +bool Mutex::TryLock() { if (mutex_) return false; Lock(); return true; } +#endif +void Mutex::ReaderLock() { assert(++mutex_ > 0); } +void Mutex::ReaderUnlock() { assert(mutex_-- > 0); } + +#elif defined(_WIN32) || defined(__CYGWIN32__) || defined(__CYGWIN64__) + +Mutex::Mutex() { InitializeCriticalSection(&mutex_); SetIsSafe(); } +Mutex::~Mutex() { DeleteCriticalSection(&mutex_); } +void Mutex::Lock() { if (is_safe_) EnterCriticalSection(&mutex_); } +void Mutex::Unlock() { if (is_safe_) LeaveCriticalSection(&mutex_); } +#ifdef GMUTEX_TRYLOCK +bool Mutex::TryLock() { return is_safe_ ? + TryEnterCriticalSection(&mutex_) != 0 : true; } +#endif +void Mutex::ReaderLock() { Lock(); } // we don't have read-write locks +void Mutex::ReaderUnlock() { Unlock(); } + +#elif defined(CERES_HAVE_PTHREAD) && defined(CERES_HAVE_RWLOCK) + +#define SAFE_PTHREAD(fncall) do { /* run fncall if is_safe_ is true */ \ + if (is_safe_ && fncall(&mutex_) != 0) abort(); \ +} while (0) + +Mutex::Mutex() { + SetIsSafe(); + if (is_safe_ && pthread_rwlock_init(&mutex_, NULL) != 0) abort(); +} +Mutex::~Mutex() { SAFE_PTHREAD(pthread_rwlock_destroy); } +void Mutex::Lock() { SAFE_PTHREAD(pthread_rwlock_wrlock); } +void Mutex::Unlock() { SAFE_PTHREAD(pthread_rwlock_unlock); } +#ifdef GMUTEX_TRYLOCK +bool Mutex::TryLock() { return is_safe_ ? + pthread_rwlock_trywrlock(&mutex_) == 0 : + true; } +#endif +void Mutex::ReaderLock() { SAFE_PTHREAD(pthread_rwlock_rdlock); } +void Mutex::ReaderUnlock() { SAFE_PTHREAD(pthread_rwlock_unlock); } +#undef SAFE_PTHREAD + +#elif defined(CERES_HAVE_PTHREAD) + +#define SAFE_PTHREAD(fncall) do { /* run fncall if is_safe_ is true */ \ + if (is_safe_ && fncall(&mutex_) != 0) abort(); \ +} while (0) + +Mutex::Mutex() { + SetIsSafe(); + if (is_safe_ && pthread_mutex_init(&mutex_, NULL) != 0) abort(); +} +Mutex::~Mutex() { SAFE_PTHREAD(pthread_mutex_destroy); } +void Mutex::Lock() { SAFE_PTHREAD(pthread_mutex_lock); } +void Mutex::Unlock() { SAFE_PTHREAD(pthread_mutex_unlock); } +#ifdef GMUTEX_TRYLOCK +bool Mutex::TryLock() { return is_safe_ ? + pthread_mutex_trylock(&mutex_) == 0 : true; } +#endif +void Mutex::ReaderLock() { Lock(); } +void Mutex::ReaderUnlock() { Unlock(); } +#undef SAFE_PTHREAD + +#endif + +// -------------------------------------------------------------------------- +// Some helper classes + +// MutexLock(mu) acquires mu when constructed and releases it when destroyed. +class MutexLock { + public: + explicit MutexLock(Mutex *mu) : mu_(mu) { mu_->Lock(); } + ~MutexLock() { mu_->Unlock(); } + private: + Mutex * const mu_; + // Disallow "evil" constructors + MutexLock(const MutexLock&); + void operator=(const MutexLock&); +}; + +// ReaderMutexLock and WriterMutexLock do the same, for rwlocks +class ReaderMutexLock { + public: + explicit ReaderMutexLock(Mutex *mu) : mu_(mu) { mu_->ReaderLock(); } + ~ReaderMutexLock() { mu_->ReaderUnlock(); } + private: + Mutex * const mu_; + // Disallow "evil" constructors + ReaderMutexLock(const ReaderMutexLock&); + void operator=(const ReaderMutexLock&); +}; + +class WriterMutexLock { + public: + explicit WriterMutexLock(Mutex *mu) : mu_(mu) { mu_->WriterLock(); } + ~WriterMutexLock() { mu_->WriterUnlock(); } + private: + Mutex * const mu_; + // Disallow "evil" constructors + WriterMutexLock(const WriterMutexLock&); + void operator=(const WriterMutexLock&); +}; + +// Catch bug where variable name is omitted, e.g. MutexLock (&mu); +#define MutexLock(x) COMPILE_ASSERT(0, mutex_lock_decl_missing_var_name) +#define ReaderMutexLock(x) COMPILE_ASSERT(0, rmutex_lock_decl_missing_var_name) +#define WriterMutexLock(x) COMPILE_ASSERT(0, wmutex_lock_decl_missing_var_name) + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_MUTEX_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/normal_prior.cc b/extern/libmv/third_party/ceres/internal/ceres/normal_prior.cc new file mode 100644 index 00000000000..f30bbc8b46b --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/normal_prior.cc @@ -0,0 +1,67 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/normal_prior.h" + +#include <cstddef> +#include <vector> + +#include <glog/logging.h> +#include "ceres/internal/eigen.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { + +NormalPrior::NormalPrior(const Matrix& A, const Vector& b) + : A_(A), b_(b) { + CHECK_GT(b_.rows(), 0); + CHECK_GT(A_.rows(), 0); + CHECK_EQ(b_.rows(), A.cols()); + set_num_residuals(A_.rows()); + mutable_parameter_block_sizes()->push_back(b_.rows()); +} + +bool NormalPrior::Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const { + ConstVectorRef p(parameters[0], parameter_block_sizes()[0]); + VectorRef r(residuals, num_residuals()); + // The following line should read + // r = A_ * (p - b_); + // The extra eval is to get around a bug in the eigen library. + r = A_ * (p - b_).eval(); + if ((jacobians != NULL) && (jacobians[0] != NULL)) { + MatrixRef(jacobians[0], num_residuals(), parameter_block_sizes()[0]) = A_; + } + return true; +} + +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/parameter_block.h b/extern/libmv/third_party/ceres/internal/ceres/parameter_block.h new file mode 100644 index 00000000000..4bac1a85828 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/parameter_block.h @@ -0,0 +1,256 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 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: keir@google.com (Keir Mierle) + +#ifndef CERES_INTERNAL_PARAMETER_BLOCK_H_ +#define CERES_INTERNAL_PARAMETER_BLOCK_H_ + +#include <cstdlib> +#include "ceres/integral_types.h" +#include <glog/logging.h> +#include "ceres/internal/eigen.h" +#include "ceres/internal/port.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/local_parameterization.h" +#include "ceres/residual_block_utils.h" + +namespace ceres { +namespace internal { + +class ProblemImpl; + +// The parameter block encodes the location of the user's original value, and +// also the "current state" of the parameter. The evaluator uses whatever is in +// the current state of the parameter when evaluating. This is inlined since the +// methods are performance sensitive. +// +// The class is not thread-safe, unless only const methods are called. The +// parameter block may also hold a pointer to a local parameterization; the +// parameter block does not take ownership of this pointer, so the user is +// responsible for the proper disposal of the local parameterization. +class ParameterBlock { + public: + ParameterBlock(double* user_state, int size) { + Init(user_state, size, NULL); + } + ParameterBlock(double* user_state, + int size, + LocalParameterization* local_parameterization) { + Init(user_state, size, local_parameterization); + } + + // The size of the parameter block. + int Size() const { return size_; } + + // Manipulate the parameter state. + bool SetState(const double* x) { + CHECK(x != NULL) + << "Tried to set the state of constant parameter " + << "with user location " << user_state_; + CHECK(!is_constant_) + << "Tried to set the state of constant parameter " + << "with user location " << user_state_; + + state_ = x; + return UpdateLocalParameterizationJacobian(); + } + + // Copy the current parameter state out to x. This is "GetState()" rather than + // simply "state()" since it is actively copying the data into the passed + // pointer. + void GetState(double *x) const { + if (x != state_) { + memcpy(x, state_, sizeof(*state_) * size_); + } + } + + // Direct pointers to the current state. + const double* state() const { return state_; } + const double* user_state() const { return user_state_; } + double* mutable_user_state() { return user_state_; } + LocalParameterization* local_parameterization() const { + return local_parameterization_; + } + LocalParameterization* mutable_local_parameterization() { + return local_parameterization_; + } + + // Set this parameter block to vary or not. + void SetConstant() { is_constant_ = true; } + void SetVarying() { is_constant_ = false; } + bool IsConstant() const { return is_constant_; } + + // This parameter block's index in an array. + int index() const { return index_; } + void set_index(int index) { index_ = index; } + + // This parameter offset inside a larger state vector. + int state_offset() const { return state_offset_; } + void set_state_offset(int state_offset) { state_offset_ = state_offset; } + + // This parameter offset inside a larger delta vector. + int delta_offset() const { return delta_offset_; } + void set_delta_offset(int delta_offset) { delta_offset_ = delta_offset; } + + // Methods relating to the parameter block's parameterization. + + // The local to global jacobian. Returns NULL if there is no local + // parameterization for this parameter block. The returned matrix is row-major + // and has Size() rows and LocalSize() columns. + const double* LocalParameterizationJacobian() const { + return local_parameterization_jacobian_.get(); + } + + int LocalSize() const { + return (local_parameterization_ == NULL) + ? size_ + : local_parameterization_->LocalSize(); + } + + // Set the parameterization. The parameterization can be set exactly once; + // multiple calls to set the parameterization to different values will crash. + // It is an error to pass NULL for the parameterization. The parameter block + // does not take ownership of the parameterization. + void SetParameterization(LocalParameterization* new_parameterization) { + CHECK(new_parameterization != NULL) << "NULL parameterization invalid."; + CHECK(new_parameterization->GlobalSize() == size_) + << "Invalid parameterization for parameter block. The parameter block " + << "has size " << size_ << " while the parameterization has a global " + << "size of " << new_parameterization->GlobalSize() << ". Did you " + << "accidentally use the wrong parameter block or parameterization?"; + if (new_parameterization != local_parameterization_) { + CHECK(local_parameterization_ == NULL) + << "Can't re-set the local parameterization; it leads to " + << "ambiguous ownership."; + local_parameterization_ = new_parameterization; + local_parameterization_jacobian_.reset( + new double[local_parameterization_->GlobalSize() * + local_parameterization_->LocalSize()]); + CHECK(UpdateLocalParameterizationJacobian()) + "Local parameterization Jacobian computation failed" + "for x: " << ConstVectorRef(state_, Size()).transpose(); + } else { + // Ignore the case that the parameterizations match. + } + } + + // Generalization of the addition operation. This is the same as + // LocalParameterization::Plus() but uses the parameter's current state + // instead of operating on a passed in pointer. + bool Plus(const double *x, const double* delta, double* x_plus_delta) { + if (local_parameterization_ == NULL) { + VectorRef(x_plus_delta, size_) = ConstVectorRef(x, size_) + + ConstVectorRef(delta, size_); + return true; + } + return local_parameterization_->Plus(x, delta, x_plus_delta); + } + + private: + void Init(double* user_state, + int size, + LocalParameterization* local_parameterization) { + user_state_ = user_state; + size_ = size; + is_constant_ = false; + state_ = user_state_; + + local_parameterization_ = NULL; + if (local_parameterization != NULL) { + SetParameterization(local_parameterization); + } + + index_ = -1; + state_offset_ = -1; + delta_offset_ = -1; + } + + bool UpdateLocalParameterizationJacobian() { + if (local_parameterization_ == NULL) { + return true; + } + + // Update the local to global Jacobian. In some cases this is + // wasted effort; if this is a bottleneck, we will find a solution + // at that time. + + const int jacobian_size = Size() * LocalSize(); + InvalidateArray(jacobian_size, + local_parameterization_jacobian_.get()); + if (!local_parameterization_->ComputeJacobian( + state_, + local_parameterization_jacobian_.get())) { + LOG(WARNING) << "Local parameterization Jacobian computation failed" + "for x: " << ConstVectorRef(state_, Size()).transpose(); + return false; + } + + if (!IsArrayValid(jacobian_size, local_parameterization_jacobian_.get())) { + LOG(WARNING) << "Local parameterization Jacobian computation returned" + << "an invalid matrix for x: " + << ConstVectorRef(state_, Size()).transpose() + << "\n Jacobian matrix : " + << ConstMatrixRef(local_parameterization_jacobian_.get(), + Size(), + LocalSize()); + return false; + } + return true; + } + + double* user_state_; + int size_; + bool is_constant_; + LocalParameterization* local_parameterization_; + + // The "state" of the parameter. These fields are only needed while the + // solver is running. While at first glance using mutable is a bad idea, this + // ends up simplifying the internals of Ceres enough to justify the potential + // pitfalls of using "mutable." + mutable const double* state_; + mutable scoped_array<double> local_parameterization_jacobian_; + + // The index of the parameter. This is used by various other parts of Ceres to + // permit switching from a ParameterBlock* to an index in another array. + int32 index_; + + // The offset of this parameter block inside a larger state vector. + int32 state_offset_; + + // The offset of this parameter block inside a larger delta vector. + int32 delta_offset_; + + // Necessary so ProblemImpl can clean up the parameterizations. + friend class ProblemImpl; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_PARAMETER_BLOCK_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/partitioned_matrix_view.cc b/extern/libmv/third_party/ceres/internal/ceres/partitioned_matrix_view.cc new file mode 100644 index 00000000000..fcf8fd53aed --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/partitioned_matrix_view.cc @@ -0,0 +1,315 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#define EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD 10 + +#include "ceres/partitioned_matrix_view.h" + +#include <algorithm> +#include <cstring> +#include <vector> +#include <glog/logging.h> +#include "ceres/block_sparse_matrix.h" +#include "ceres/block_structure.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +PartitionedMatrixView::PartitionedMatrixView( + const BlockSparseMatrixBase& matrix, + int num_col_blocks_a) + : matrix_(matrix), + num_col_blocks_e_(num_col_blocks_a) { + const CompressedRowBlockStructure* bs = matrix_.block_structure(); + CHECK_NOTNULL(bs); + + num_col_blocks_f_ = bs->cols.size() - num_col_blocks_a; + + // Compute the number of row blocks in E. The number of row blocks + // in E maybe less than the number of row blocks in the input matrix + // as some of the row blocks at the bottom may not have any + // e_blocks. For a definition of what an e_block is, please see + // explicit_schur_complement_solver.h + num_row_blocks_e_ = 0; + for (int r = 0; r < bs->rows.size(); ++r) { + const vector<Cell>& cells = bs->rows[r].cells; + if (cells[0].block_id < num_col_blocks_a) { + ++num_row_blocks_e_; + } + } + + // Compute the number of columns in E and F. + num_cols_e_ = 0; + num_cols_f_ = 0; + + for (int c = 0; c < bs->cols.size(); ++c) { + const Block& block = bs->cols[c]; + if (c < num_col_blocks_a) { + num_cols_e_ += block.size; + } else { + num_cols_f_ += block.size; + } + } + + CHECK_EQ(num_cols_e_ + num_cols_f_, matrix_.num_cols()); +} + +PartitionedMatrixView::~PartitionedMatrixView() { +} + +// The next four methods don't seem to be particularly cache +// friendly. This is an artifact of how the BlockStructure of the +// input matrix is constructed. These methods will benefit from +// multithreading as well as improved data layout. + +void PartitionedMatrixView::RightMultiplyE(const double* x, double* y) const { + const CompressedRowBlockStructure* bs = matrix_.block_structure(); + + // Iterate over the first num_row_blocks_e_ row blocks, and multiply + // by the first cell in each row block. + for (int r = 0; r < num_row_blocks_e_; ++r) { + const double* row_values = matrix_.RowBlockValues(r); + const Cell& cell = bs->rows[r].cells[0]; + const int row_block_pos = bs->rows[r].block.position; + const int row_block_size = bs->rows[r].block.size; + const int col_block_id = cell.block_id; + const int col_block_pos = bs->cols[col_block_id].position; + const int col_block_size = bs->cols[col_block_id].size; + + ConstVectorRef xref(x + col_block_pos, col_block_size); + VectorRef yref(y + row_block_pos, row_block_size); + ConstMatrixRef m(row_values + cell.position, + row_block_size, + col_block_size); + yref += m.lazyProduct(xref); + } +} + +void PartitionedMatrixView::RightMultiplyF(const double* x, double* y) const { + const CompressedRowBlockStructure* bs = matrix_.block_structure(); + + // Iterate over row blocks, and if the row block is in E, then + // multiply by all the cells except the first one which is of type + // E. If the row block is not in E (i.e its in the bottom + // num_row_blocks - num_row_blocks_e row blocks), then all the cells + // are of type F and multiply by them all. + for (int r = 0; r < bs->rows.size(); ++r) { + const int row_block_pos = bs->rows[r].block.position; + const int row_block_size = bs->rows[r].block.size; + VectorRef yref(y + row_block_pos, row_block_size); + const vector<Cell>& cells = bs->rows[r].cells; + for (int c = (r < num_row_blocks_e_) ? 1 : 0; c < cells.size(); ++c) { + const double* row_values = matrix_.RowBlockValues(r); + const int col_block_id = cells[c].block_id; + const int col_block_pos = bs->cols[col_block_id].position; + const int col_block_size = bs->cols[col_block_id].size; + + ConstVectorRef xref(x + col_block_pos - num_cols_e(), + col_block_size); + ConstMatrixRef m(row_values + cells[c].position, + row_block_size, + col_block_size); + yref += m.lazyProduct(xref); + } + } +} + +void PartitionedMatrixView::LeftMultiplyE(const double* x, double* y) const { + const CompressedRowBlockStructure* bs = matrix_.block_structure(); + + // Iterate over the first num_row_blocks_e_ row blocks, and multiply + // by the first cell in each row block. + for (int r = 0; r < num_row_blocks_e_; ++r) { + const Cell& cell = bs->rows[r].cells[0]; + const double* row_values = matrix_.RowBlockValues(r); + const int row_block_pos = bs->rows[r].block.position; + const int row_block_size = bs->rows[r].block.size; + const int col_block_id = cell.block_id; + const int col_block_pos = bs->cols[col_block_id].position; + const int col_block_size = bs->cols[col_block_id].size; + + ConstVectorRef xref(x + row_block_pos, row_block_size); + VectorRef yref(y + col_block_pos, col_block_size); + ConstMatrixRef m(row_values + cell.position, + row_block_size, + col_block_size); + yref += m.transpose().lazyProduct(xref); + } +} + +void PartitionedMatrixView::LeftMultiplyF(const double* x, double* y) const { + const CompressedRowBlockStructure* bs = matrix_.block_structure(); + + // Iterate over row blocks, and if the row block is in E, then + // multiply by all the cells except the first one which is of type + // E. If the row block is not in E (i.e its in the bottom + // num_row_blocks - num_row_blocks_e row blocks), then all the cells + // are of type F and multiply by them all. + for (int r = 0; r < bs->rows.size(); ++r) { + const int row_block_pos = bs->rows[r].block.position; + const int row_block_size = bs->rows[r].block.size; + ConstVectorRef xref(x + row_block_pos, row_block_size); + const vector<Cell>& cells = bs->rows[r].cells; + for (int c = (r < num_row_blocks_e_) ? 1 : 0; c < cells.size(); ++c) { + const double* row_values = matrix_.RowBlockValues(r); + const int col_block_id = cells[c].block_id; + const int col_block_pos = bs->cols[col_block_id].position; + const int col_block_size = bs->cols[col_block_id].size; + + VectorRef yref(y + col_block_pos - num_cols_e(), col_block_size); + ConstMatrixRef m(row_values + cells[c].position, + row_block_size, + col_block_size); + yref += m.transpose().lazyProduct(xref); + } + } +} + +// Given a range of columns blocks of a matrix m, compute the block +// structure of the block diagonal of the matrix m(:, +// start_col_block:end_col_block)'m(:, start_col_block:end_col_block) +// and return a BlockSparseMatrix with the this block structure. The +// caller owns the result. +BlockSparseMatrix* PartitionedMatrixView::CreateBlockDiagonalMatrixLayout( + int start_col_block, int end_col_block) const { + const CompressedRowBlockStructure* bs = matrix_.block_structure(); + CompressedRowBlockStructure* block_diagonal_structure = + new CompressedRowBlockStructure; + + int block_position = 0; + int diagonal_cell_position = 0; + + // Iterate over the column blocks, creating a new diagonal block for + // each column block. + for (int c = start_col_block; c < end_col_block; ++c) { + const Block& block = bs->cols[c]; + block_diagonal_structure->cols.push_back(Block()); + Block& diagonal_block = block_diagonal_structure->cols.back(); + diagonal_block.size = block.size; + diagonal_block.position = block_position; + + block_diagonal_structure->rows.push_back(CompressedRow()); + CompressedRow& row = block_diagonal_structure->rows.back(); + row.block = diagonal_block; + + row.cells.push_back(Cell()); + Cell& cell = row.cells.back(); + cell.block_id = c - start_col_block; + cell.position = diagonal_cell_position; + + block_position += block.size; + diagonal_cell_position += block.size * block.size; + } + + // Build a BlockSparseMatrix with the just computed block + // structure. + return new BlockSparseMatrix(block_diagonal_structure); +} + +BlockSparseMatrix* PartitionedMatrixView::CreateBlockDiagonalEtE() const { + BlockSparseMatrix* block_diagonal = + CreateBlockDiagonalMatrixLayout(0, num_col_blocks_e_); + UpdateBlockDiagonalEtE(block_diagonal); + return block_diagonal; +} + +BlockSparseMatrix* PartitionedMatrixView::CreateBlockDiagonalFtF() const { + BlockSparseMatrix* block_diagonal = + CreateBlockDiagonalMatrixLayout( + num_col_blocks_e_, num_col_blocks_e_ + num_col_blocks_f_); + UpdateBlockDiagonalFtF(block_diagonal); + return block_diagonal; +} + +// Similar to the code in RightMultiplyE, except instead of the matrix +// vector multiply its an outer product. +// +// block_diagonal = block_diagonal(E'E) +void PartitionedMatrixView::UpdateBlockDiagonalEtE( + BlockSparseMatrix* block_diagonal) const { + const CompressedRowBlockStructure* bs = matrix_.block_structure(); + const CompressedRowBlockStructure* block_diagonal_structure = + block_diagonal->block_structure(); + + block_diagonal->SetZero(); + + for (int r = 0; r < num_row_blocks_e_ ; ++r) { + const double* row_values = matrix_.RowBlockValues(r); + const Cell& cell = bs->rows[r].cells[0]; + const int row_block_size = bs->rows[r].block.size; + const int block_id = cell.block_id; + const int col_block_size = bs->cols[block_id].size; + ConstMatrixRef m(row_values + cell.position, + row_block_size, + col_block_size); + + const int cell_position = + block_diagonal_structure->rows[block_id].cells[0].position; + + MatrixRef(block_diagonal->mutable_values() + cell_position, + col_block_size, col_block_size).noalias() += m.transpose() * m; + } +} + +// Similar to the code in RightMultiplyF, except instead of the matrix +// vector multiply its an outer product. +// +// block_diagonal = block_diagonal(F'F) +// +void PartitionedMatrixView::UpdateBlockDiagonalFtF( + BlockSparseMatrix* block_diagonal) const { + const CompressedRowBlockStructure* bs = matrix_.block_structure(); + const CompressedRowBlockStructure* block_diagonal_structure = + block_diagonal->block_structure(); + + block_diagonal->SetZero(); + for (int r = 0; r < bs->rows.size(); ++r) { + const int row_block_size = bs->rows[r].block.size; + const vector<Cell>& cells = bs->rows[r].cells; + const double* row_values = matrix_.RowBlockValues(r); + for (int c = (r < num_row_blocks_e_) ? 1 : 0; c < cells.size(); ++c) { + const int col_block_id = cells[c].block_id; + const int col_block_size = bs->cols[col_block_id].size; + ConstMatrixRef m(row_values + cells[c].position, + row_block_size, + col_block_size); + const int diagonal_block_id = col_block_id - num_col_blocks_e_; + const int cell_position = + block_diagonal_structure->rows[diagonal_block_id].cells[0].position; + + MatrixRef(block_diagonal->mutable_values() + cell_position, + col_block_size, col_block_size).noalias() += m.transpose() * m; + } + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/partitioned_matrix_view.h b/extern/libmv/third_party/ceres/internal/ceres/partitioned_matrix_view.h new file mode 100644 index 00000000000..cfe4de5b436 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/partitioned_matrix_view.h @@ -0,0 +1,121 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// For generalized bi-partite Jacobian matrices that arise in +// Structure from Motion related problems, it is sometimes useful to +// have access to the two parts of the matrix as linear operators +// themselves. This class provides that functionality. + +#ifndef CERES_INTERNAL_PARTITIONED_MATRIX_VIEW_H_ +#define CERES_INTERNAL_PARTITIONED_MATRIX_VIEW_H_ + +#include "ceres/block_sparse_matrix.h" + +namespace ceres { +namespace internal { + +// Given generalized bi-partite matrix A = [E F], with the same block +// structure as required by the Schur complement based solver, found +// in explicit_schur_complement_solver.h, provide access to the +// matrices E and F and their outer products E'E and F'F with +// themselves. +// +// Lack of BlockStructure object will result in a crash and if the +// block structure of the matrix does not satisfy the requirements of +// the Schur complement solver it will result in unpredictable and +// wrong output. +// +// This class lives in the internal name space as its a utility class +// to be used by the IterativeSchurComplementSolver class, found in +// iterative_schur_complement_solver.h, and is not meant for general +// consumption. +class PartitionedMatrixView { + public: + // matrix = [E F], where the matrix E contains the first + // num_col_blocks_a column blocks. + PartitionedMatrixView(const BlockSparseMatrixBase& matrix, + int num_col_blocks_a); + ~PartitionedMatrixView(); + + // y += E'x + void LeftMultiplyE(const double* x, double* y) const; + + // y += F'x + void LeftMultiplyF(const double* x, double* y) const; + + // y += Ex + void RightMultiplyE(const double* x, double* y) const; + + // y += Fx + void RightMultiplyF(const double* x, double* y) const; + + // Create and return the block diagonal of the matrix E'E. + BlockSparseMatrix* CreateBlockDiagonalEtE() const; + + // Create and return the block diagonal of the matrix F'F. + BlockSparseMatrix* CreateBlockDiagonalFtF() const; + + // Compute the block diagonal of the matrix E'E and store it in + // block_diagonal. The matrix block_diagonal is expected to have a + // BlockStructure (preferably created using + // CreateBlockDiagonalMatrixEtE) which is has the same structure as + // the block diagonal of E'E. + void UpdateBlockDiagonalEtE(BlockSparseMatrix* block_diagonal) const; + + // Compute the block diagonal of the matrix F'F and store it in + // block_diagonal. The matrix block_diagonal is expected to have a + // BlockStructure (preferably created using + // CreateBlockDiagonalMatrixFtF) which is has the same structure as + // the block diagonal of F'F. + void UpdateBlockDiagonalFtF(BlockSparseMatrix* block_diagonal) const; + + int num_col_blocks_e() const { return num_col_blocks_e_; } + int num_col_blocks_f() const { return num_col_blocks_f_; } + int num_cols_e() const { return num_cols_e_; } + int num_cols_f() const { return num_cols_f_; } + int num_rows() const { return matrix_.num_rows(); } + int num_cols() const { return matrix_.num_cols(); } + + private: + BlockSparseMatrix* CreateBlockDiagonalMatrixLayout(int start_col_block, + int end_col_block) const; + + const BlockSparseMatrixBase& matrix_; + int num_row_blocks_e_; + int num_col_blocks_e_; + int num_col_blocks_f_; + int num_cols_e_; + int num_cols_f_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_PARTITIONED_MATRIX_VIEW_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/problem.cc b/extern/libmv/third_party/ceres/internal/ceres/problem.cc new file mode 100644 index 00000000000..b8c25d9db84 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/problem.cc @@ -0,0 +1,149 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// keir@google.com (Keir Mierle) + +#include "ceres/problem.h" + +#include <vector> +#include "ceres/problem_impl.h" + +namespace ceres { + +class ResidualBlock; + +Problem::Problem() : problem_impl_(new internal::ProblemImpl) {} +Problem::Problem(const Problem::Options& options) + : problem_impl_(new internal::ProblemImpl(options)) {} +Problem::~Problem() {} + +ResidualBlockId Problem::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + const vector<double*>& parameter_blocks) { + return problem_impl_->AddResidualBlock(cost_function, + loss_function, + parameter_blocks); +} + +ResidualBlockId Problem::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + double* x0) { + return problem_impl_->AddResidualBlock(cost_function, + loss_function, + x0); +} + +ResidualBlockId Problem::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1) { + return problem_impl_->AddResidualBlock(cost_function, + loss_function, + x0, x1); +} + +ResidualBlockId Problem::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2) { + return problem_impl_->AddResidualBlock(cost_function, + loss_function, + x0, x1, x2); +} + +ResidualBlockId Problem::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, double* x3) { + return problem_impl_->AddResidualBlock(cost_function, + loss_function, + x0, x1, x2, x3); +} + +ResidualBlockId Problem::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, double* x3, double* x4) { + return problem_impl_->AddResidualBlock(cost_function, + loss_function, + x0, x1, x2, x3, x4); +} + +ResidualBlockId Problem::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, double* x3, double* x4, double* x5) { + return problem_impl_->AddResidualBlock(cost_function, + loss_function, + x0, x1, x2, x3, x4, x5); +} + +void Problem::AddParameterBlock(double* values, int size) { + problem_impl_->AddParameterBlock(values, size); +} + +void Problem::AddParameterBlock(double* values, + int size, + LocalParameterization* local_parameterization) { + problem_impl_->AddParameterBlock(values, size, local_parameterization); +} + +void Problem::SetParameterBlockConstant(double* values) { + problem_impl_->SetParameterBlockConstant(values); +} + +void Problem::SetParameterBlockVariable(double* values) { + problem_impl_->SetParameterBlockVariable(values); +} + +void Problem::SetParameterization( + double* values, + LocalParameterization* local_parameterization) { + problem_impl_->SetParameterization(values, local_parameterization); +} + +int Problem::NumParameterBlocks() const { + return problem_impl_->NumParameterBlocks(); +} + +int Problem::NumParameters() const { + return problem_impl_->NumParameters(); +} + +int Problem::NumResidualBlocks() const { + return problem_impl_->NumResidualBlocks(); +} + +int Problem::NumResiduals() const { + return problem_impl_->NumResiduals(); +} + +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/problem_impl.cc b/extern/libmv/third_party/ceres/internal/ceres/problem_impl.cc new file mode 100644 index 00000000000..68242477d6f --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/problem_impl.cc @@ -0,0 +1,359 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// keir@google.com (Keir Mierle) + +#include "ceres/problem_impl.h" + +#include <algorithm> +#include <cstddef> +#include <set> +#include <string> +#include <utility> +#include <vector> + +#include <glog/logging.h> +#include "ceres/parameter_block.h" +#include "ceres/program.h" +#include "ceres/residual_block.h" +#include "ceres/stl_util.h" +#include "ceres/map_util.h" +#include "ceres/stringprintf.h" +#include "ceres/cost_function.h" +#include "ceres/loss_function.h" + +namespace ceres { +namespace internal { + +typedef map<double*, internal::ParameterBlock*> ParameterMap; + +// Returns true if two regions of memory, a and b, with sizes size_a and size_b +// respectively, overlap. +static bool RegionsAlias(const double* a, int size_a, + const double* b, int size_b) { + return (a < b) ? b < (a + size_a) + : a < (b + size_b); +} + +static void CheckForNoAliasing(double* existing_block, + int existing_block_size, + double* new_block, + int new_block_size) { + CHECK(!RegionsAlias(existing_block, existing_block_size, + new_block, new_block_size)) + << "Aliasing detected between existing parameter block at memory " + << "location " << existing_block + << " and has size " << existing_block_size << " with new parameter " + << "block that has memory adderss " << new_block << " and would have " + << "size " << new_block_size << "."; +} + +static ParameterBlock* InternalAddParameterBlock( + double* values, + int size, + ParameterMap* parameter_map, + vector<ParameterBlock*>* parameter_blocks) { + CHECK(values) << "Null pointer passed to AddParameterBlock for a parameter " + << "with size " << size; + + // Ignore the request if there is a block for the given pointer already. + ParameterMap::iterator it = parameter_map->find(values); + if (it != parameter_map->end()) { + int existing_size = it->second->Size(); + CHECK(size == existing_size) + << "Tried adding a parameter block with the same double pointer, " + << values << ", twice, but with different block sizes. Original " + << "size was " << existing_size << " but new size is " + << size; + return it->second; + } + // Before adding the parameter block, also check that it doesn't alias any + // other parameter blocks. + if (!parameter_map->empty()) { + ParameterMap::iterator lb = parameter_map->lower_bound(values); + + // If lb is not the first block, check the previous block for aliasing. + if (lb != parameter_map->begin()) { + ParameterMap::iterator previous = lb; + --previous; + CheckForNoAliasing(previous->first, + previous->second->Size(), + values, + size); + } + + // If lb is not off the end, check lb for aliasing. + if (lb != parameter_map->end()) { + CheckForNoAliasing(lb->first, + lb->second->Size(), + values, + size); + } + } + ParameterBlock* new_parameter_block = new ParameterBlock(values, size); + (*parameter_map)[values] = new_parameter_block; + parameter_blocks->push_back(new_parameter_block); + return new_parameter_block; +} + +ProblemImpl::ProblemImpl() : program_(new internal::Program) {} +ProblemImpl::ProblemImpl(const Problem::Options& options) + : options_(options), + program_(new internal::Program) {} + +ProblemImpl::~ProblemImpl() { + // Collect the unique cost/loss functions and delete the residuals. + set<CostFunction*> cost_functions; + set<LossFunction*> loss_functions; + for (int i = 0; i < program_->residual_blocks_.size(); ++i) { + ResidualBlock* residual_block = program_->residual_blocks_[i]; + + // The const casts here are legit, since ResidualBlock holds these + // pointers as const pointers but we have ownership of them and + // have the right to destroy them when the destructor is called. + if (options_.cost_function_ownership == TAKE_OWNERSHIP) { + cost_functions.insert( + const_cast<CostFunction*>(residual_block->cost_function())); + } + if (options_.loss_function_ownership == TAKE_OWNERSHIP) { + loss_functions.insert( + const_cast<LossFunction*>(residual_block->loss_function())); + } + + delete residual_block; + } + + // Collect the unique parameterizations and delete the parameters. + set<LocalParameterization*> local_parameterizations; + for (int i = 0; i < program_->parameter_blocks_.size(); ++i) { + ParameterBlock* parameter_block = program_->parameter_blocks_[i]; + + if (options_.local_parameterization_ownership == TAKE_OWNERSHIP) { + local_parameterizations.insert(parameter_block->local_parameterization_); + } + + delete parameter_block; + } + + // Delete the owned cost/loss functions and parameterizations. + STLDeleteContainerPointers(local_parameterizations.begin(), + local_parameterizations.end()); + STLDeleteContainerPointers(cost_functions.begin(), + cost_functions.end()); + STLDeleteContainerPointers(loss_functions.begin(), + loss_functions.end()); +} + +const ResidualBlock* ProblemImpl::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + const vector<double*>& parameter_blocks) { + CHECK_NOTNULL(cost_function); + CHECK_EQ(parameter_blocks.size(), + cost_function->parameter_block_sizes().size()); + + // Check the sizes match. + const vector<int16>& parameter_block_sizes = + cost_function->parameter_block_sizes(); + CHECK_EQ(parameter_block_sizes.size(), parameter_blocks.size()) + << "Number of blocks input is different than the number of blocks " + << "that the cost function expects."; + + // Check for duplicate parameter blocks. + vector<double*> sorted_parameter_blocks(parameter_blocks); + sort(sorted_parameter_blocks.begin(), sorted_parameter_blocks.end()); + vector<double*>::const_iterator duplicate_items = + unique(sorted_parameter_blocks.begin(), + sorted_parameter_blocks.end()); + if (duplicate_items != sorted_parameter_blocks.end()) { + string blocks; + for (int i = 0; i < parameter_blocks.size(); ++i) { + blocks += internal::StringPrintf(" %p ", parameter_blocks[i]); + } + + LOG(FATAL) << "Duplicate parameter blocks in a residual parameter " + << "are not allowed. Parameter block pointers: [" + << blocks << "]"; + } + + // Add parameter blocks and convert the double*'s to parameter blocks. + vector<ParameterBlock*> parameter_block_ptrs(parameter_blocks.size()); + for (int i = 0; i < parameter_blocks.size(); ++i) { + parameter_block_ptrs[i] = + InternalAddParameterBlock(parameter_blocks[i], + parameter_block_sizes[i], + ¶meter_block_map_, + &program_->parameter_blocks_); + } + + // Check that the block sizes match the block sizes expected by the + // cost_function. + for (int i = 0; i < parameter_block_ptrs.size(); ++i) { + CHECK_EQ(cost_function->parameter_block_sizes()[i], + parameter_block_ptrs[i]->Size()) + << "The cost function expects parameter block " << i + << " of size " << cost_function->parameter_block_sizes()[i] + << " but was given a block of size " + << parameter_block_ptrs[i]->Size(); + } + + ResidualBlock* new_residual_block = + new ResidualBlock(cost_function, + loss_function, + parameter_block_ptrs); + program_->residual_blocks_.push_back(new_residual_block); + return new_residual_block; +} + +// Unfortunately, macros don't help much to reduce this code, and var args don't +// work because of the ambiguous case that there is no loss function. +const ResidualBlock* ProblemImpl::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + double* x0) { + vector<double*> residual_parameters; + residual_parameters.push_back(x0); + return AddResidualBlock(cost_function, loss_function, residual_parameters); +} + +const ResidualBlock* ProblemImpl::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1) { + vector<double*> residual_parameters; + residual_parameters.push_back(x0); + residual_parameters.push_back(x1); + return AddResidualBlock(cost_function, loss_function, residual_parameters); +} + +const ResidualBlock* ProblemImpl::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2) { + vector<double*> residual_parameters; + residual_parameters.push_back(x0); + residual_parameters.push_back(x1); + residual_parameters.push_back(x2); + return AddResidualBlock(cost_function, loss_function, residual_parameters); +} + +const ResidualBlock* ProblemImpl::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, double* x3) { + vector<double*> residual_parameters; + residual_parameters.push_back(x0); + residual_parameters.push_back(x1); + residual_parameters.push_back(x2); + residual_parameters.push_back(x3); + return AddResidualBlock(cost_function, loss_function, residual_parameters); +} + +const ResidualBlock* ProblemImpl::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, double* x3, double* x4) { + vector<double*> residual_parameters; + residual_parameters.push_back(x0); + residual_parameters.push_back(x1); + residual_parameters.push_back(x2); + residual_parameters.push_back(x3); + residual_parameters.push_back(x4); + return AddResidualBlock(cost_function, loss_function, residual_parameters); +} + +const ResidualBlock* ProblemImpl::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, double* x3, double* x4, double* x5) { + vector<double*> residual_parameters; + residual_parameters.push_back(x0); + residual_parameters.push_back(x1); + residual_parameters.push_back(x2); + residual_parameters.push_back(x3); + residual_parameters.push_back(x4); + residual_parameters.push_back(x5); + return AddResidualBlock(cost_function, loss_function, residual_parameters); +} + + +void ProblemImpl::AddParameterBlock(double* values, int size) { + InternalAddParameterBlock(values, + size, + ¶meter_block_map_, + &program_->parameter_blocks_); +} + +void ProblemImpl::AddParameterBlock( + double* values, + int size, + LocalParameterization* local_parameterization) { + ParameterBlock* parameter_block = + InternalAddParameterBlock(values, + size, + ¶meter_block_map_, + &program_->parameter_blocks_); + if (local_parameterization != NULL) { + parameter_block->SetParameterization(local_parameterization); + } +} + +void ProblemImpl::SetParameterBlockConstant(double* values) { + FindOrDie(parameter_block_map_, values)->SetConstant(); +} + +void ProblemImpl::SetParameterBlockVariable(double* values) { + FindOrDie(parameter_block_map_, values)->SetVarying(); +} + +void ProblemImpl::SetParameterization( + double* values, + LocalParameterization* local_parameterization) { + FindOrDie(parameter_block_map_, values) + ->SetParameterization(local_parameterization); +} + +int ProblemImpl::NumParameterBlocks() const { + return program_->NumParameterBlocks(); +} + +int ProblemImpl::NumParameters() const { + return program_->NumParameters(); +} + +int ProblemImpl::NumResidualBlocks() const { + return program_->NumResidualBlocks(); +} + +int ProblemImpl::NumResiduals() const { + return program_->NumResiduals(); +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/problem_impl.h b/extern/libmv/third_party/ceres/internal/ceres/problem_impl.h new file mode 100644 index 00000000000..523860e652a --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/problem_impl.h @@ -0,0 +1,127 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 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: keir@google.com (Keir Mierle) +// +// This is the implementation of the public Problem API. The pointer to +// implementation (PIMPL) idiom makes it possible for Ceres internal code to +// refer to the private data members without needing to exposing it to the +// world. An alternative to PIMPL is to have a factory which returns instances +// of a virtual base class; while that approach would work, it requires clients +// to always put a Problem object into a scoped pointer; this needlessly muddies +// client code for little benefit. Therefore, the PIMPL comprise was chosen. + +#ifndef CERES_PUBLIC_PROBLEM_IMPL_H_ +#define CERES_PUBLIC_PROBLEM_IMPL_H_ + +#include <map> +#include <vector> + +#include "ceres/internal/macros.h" +#include "ceres/internal/port.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/problem.h" +#include "ceres/types.h" + +namespace ceres { + +class CostFunction; +class LossFunction; +class LocalParameterization; + +namespace internal { + +class Program; +class ResidualBlock; + +class ProblemImpl { + public: + typedef map<double*, ParameterBlock*> ParameterMap; + + ProblemImpl(); + explicit ProblemImpl(const Problem::Options& options); + + ~ProblemImpl(); + + // See the public problem.h file for description of these methods. + ResidualBlockId AddResidualBlock(CostFunction* cost_function, + LossFunction* loss_function, + const vector<double*>& parameter_blocks); + ResidualBlockId AddResidualBlock(CostFunction* cost_function, + LossFunction* loss_function, + double* x0); + ResidualBlockId AddResidualBlock(CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1); + ResidualBlockId AddResidualBlock(CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2); + ResidualBlockId AddResidualBlock(CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, + double* x3); + ResidualBlockId AddResidualBlock(CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, + double* x3, double* x4); + ResidualBlockId AddResidualBlock(CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, + double* x3, double* x4, double* x5); + void AddParameterBlock(double* values, int size); + void AddParameterBlock(double* values, + int size, + LocalParameterization* local_parameterization); + void SetParameterBlockConstant(double* values); + void SetParameterBlockVariable(double* values); + void SetParameterization(double* values, + LocalParameterization* local_parameterization); + int NumParameterBlocks() const; + int NumParameters() const; + int NumResidualBlocks() const; + int NumResiduals() const; + + const Program& program() const { return *program_; } + Program* mutable_program() { return program_.get(); } + + const ParameterMap& parameter_map() const { return parameter_block_map_; } + + private: + const Problem::Options options_; + + // The mapping from user pointers to parameter blocks. + map<double*, ParameterBlock*> parameter_block_map_; + + internal::scoped_ptr<internal::Program> program_; + DISALLOW_COPY_AND_ASSIGN(ProblemImpl); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_PUBLIC_PROBLEM_IMPL_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/program.cc b/extern/libmv/third_party/ceres/internal/ceres/program.cc new file mode 100644 index 00000000000..444b1020253 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/program.cc @@ -0,0 +1,233 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 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: keir@google.com (Keir Mierle) + +#include "ceres/program.h" + +#include <map> +#include <vector> +#include "ceres/parameter_block.h" +#include "ceres/residual_block.h" +#include "ceres/stl_util.h" +#include "ceres/map_util.h" +#include "ceres/problem.h" +#include "ceres/cost_function.h" +#include "ceres/loss_function.h" +#include "ceres/local_parameterization.h" + +namespace ceres { +namespace internal { + +Program::Program() {} + +Program::Program(const Program& program) + : parameter_blocks_(program.parameter_blocks_), + residual_blocks_(program.residual_blocks_) { +} + +const vector<ParameterBlock*>& Program::parameter_blocks() const { + return parameter_blocks_; +} + +const vector<ResidualBlock*>& Program::residual_blocks() const { + return residual_blocks_; +} + +vector<ParameterBlock*>* Program::mutable_parameter_blocks() { + return ¶meter_blocks_; +} + +vector<ResidualBlock*>* Program::mutable_residual_blocks() { + return &residual_blocks_; +} + +bool Program::StateVectorToParameterBlocks(const double *state) { + for (int i = 0; i < parameter_blocks_.size(); ++i) { + if (!parameter_blocks_[i]->SetState(state)) { + return false; + } + state += parameter_blocks_[i]->Size(); + } + return true; +} + +void Program::ParameterBlocksToStateVector(double *state) const { + for (int i = 0; i < parameter_blocks_.size(); ++i) { + parameter_blocks_[i]->GetState(state); + state += parameter_blocks_[i]->Size(); + } +} + +void Program::CopyParameterBlockStateToUserState() { + for (int i = 0; i < parameter_blocks_.size(); ++i) { + parameter_blocks_[i]->GetState( + parameter_blocks_[i]->mutable_user_state()); + } +} + +bool Program::Plus(const double* state, + const double* delta, + double* state_plus_delta) const { + for (int i = 0; i < parameter_blocks_.size(); ++i) { + if (!parameter_blocks_[i]->Plus(state, delta, state_plus_delta)) { + return false; + } + state += parameter_blocks_[i]->Size(); + delta += parameter_blocks_[i]->LocalSize(); + state_plus_delta += parameter_blocks_[i]->Size(); + } + return true; +} + +void Program::SetParameterOffsetsAndIndex() { + // Set positions for all parameters appearing as arguments to residuals to one + // past the end of the parameter block array. + for (int i = 0; i < residual_blocks_.size(); ++i) { + ResidualBlock* residual_block = residual_blocks_[i]; + for (int j = 0; j < residual_block->NumParameterBlocks(); ++j) { + residual_block->parameter_blocks()[j]->set_index(-1); + } + } + // For parameters that appear in the program, set their position and offset. + int state_offset = 0; + int delta_offset = 0; + for (int i = 0; i < parameter_blocks_.size(); ++i) { + parameter_blocks_[i]->set_index(i); + parameter_blocks_[i]->set_state_offset(state_offset); + parameter_blocks_[i]->set_delta_offset(delta_offset); + state_offset += parameter_blocks_[i]->Size(); + delta_offset += parameter_blocks_[i]->LocalSize(); + } +} + +int Program::NumResidualBlocks() const { + return residual_blocks_.size(); +} + +int Program::NumParameterBlocks() const { + return parameter_blocks_.size(); +} + +int Program::NumResiduals() const { + int num_residuals = 0; + for (int i = 0; i < residual_blocks_.size(); ++i) { + num_residuals += residual_blocks_[i]->NumResiduals(); + } + return num_residuals; +} + +int Program::NumParameters() const { + int num_parameters = 0; + for (int i = 0; i < parameter_blocks_.size(); ++i) { + num_parameters += parameter_blocks_[i]->Size(); + } + return num_parameters; +} + +int Program::NumEffectiveParameters() const { + int num_parameters = 0; + for (int i = 0; i < parameter_blocks_.size(); ++i) { + num_parameters += parameter_blocks_[i]->LocalSize(); + } + return num_parameters; +} + +int Program::MaxScratchDoublesNeededForEvaluate() const { + // Compute the scratch space needed for evaluate. + int max_scratch_bytes_for_evaluate = 0; + for (int i = 0; i < residual_blocks_.size(); ++i) { + max_scratch_bytes_for_evaluate = + max(max_scratch_bytes_for_evaluate, + residual_blocks_[i]->NumScratchDoublesForEvaluate()); + } + return max_scratch_bytes_for_evaluate; +} + +int Program::MaxDerivativesPerResidualBlock() const { + int max_derivatives = 0; + for (int i = 0; i < residual_blocks_.size(); ++i) { + int derivatives = 0; + ResidualBlock* residual_block = residual_blocks_[i]; + int num_parameters = residual_block->NumParameterBlocks(); + for (int j = 0; j < num_parameters; ++j) { + derivatives += residual_block->NumResiduals() * + residual_block->parameter_blocks()[j]->LocalSize(); + } + max_derivatives = max(max_derivatives, derivatives); + } + return max_derivatives; +} + +int Program::MaxParametersPerResidualBlock() const { + int max_parameters = 0; + for (int i = 0; i < residual_blocks_.size(); ++i) { + max_parameters = max(max_parameters, + residual_blocks_[i]->NumParameterBlocks()); + } + return max_parameters; +} + +bool Program::Evaluate(double* cost, double* residuals) { + *cost = 0.0; + + // Scratch space is only needed if residuals is NULL. + scoped_array<double> scratch; + if (residuals == NULL) { + scratch.reset(new double[MaxScratchDoublesNeededForEvaluate()]); + } else { + // TODO(keir): Is this needed? Check by removing the equivalent statement in + // dense_evaluator.cc and running the tests. + VectorRef(residuals, NumResiduals()).setZero(); + } + + for (int i = 0; i < residual_blocks_.size(); ++i) { + ResidualBlock* residual_block = residual_blocks_[i]; + + // Evaluate the cost function for this residual. + double residual_cost; + if (!residual_block->Evaluate(&residual_cost, + residuals, + NULL, // No jacobian. + scratch.get())) { + return false; + } + + // Accumulate residual cost into the total cost. + *cost += residual_cost; + + // Update the residuals cursor. + if (residuals != NULL) { + residuals += residual_block->NumResiduals(); + } + } + return true; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/program.h b/extern/libmv/third_party/ceres/internal/ceres/program.h new file mode 100644 index 00000000000..113d352d562 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/program.h @@ -0,0 +1,128 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 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: keir@google.com (Keir Mierle) + +#ifndef CERES_INTERNAL_PROGRAM_H_ +#define CERES_INTERNAL_PROGRAM_H_ + +#include <vector> +#include "ceres/internal/port.h" + +namespace ceres { +namespace internal { + +class ParameterBlock; +class ProblemImpl; +class ResidualBlock; + +// A nonlinear least squares optimization problem. This is different from the +// similarly-named "Problem" object, which offers a mutation interface for +// adding and modifying parameters and residuals. The Program contains the core +// part of the Problem, which is the parameters and the residuals, stored in a +// particular ordering. The ordering is critical, since it defines the mapping +// between (residual, parameter) pairs and a position in the jacobian of the +// objective function. Various parts of Ceres transform one Program into +// another; for example, the first stage of solving involves stripping all +// constant parameters and residuals. This is in contrast with Problem, which is +// not built for transformation. +class Program { + public: + Program(); + explicit Program(const Program& program); + + // The ordered parameter and residual blocks for the program. + const vector<ParameterBlock*>& parameter_blocks() const; + const vector<ResidualBlock*>& residual_blocks() const; + vector<ParameterBlock*>* mutable_parameter_blocks(); + vector<ResidualBlock*>* mutable_residual_blocks(); + + // Serialize to/from the program and update states. + // + // NOTE: Setting the state of a parameter block can trigger the + // computation of the Jacobian of its local parameterization. If + // this computation fails for some reason, then this method returns + // false and the state of the parameter blocks cannot be trusted. + bool StateVectorToParameterBlocks(const double *state); + void ParameterBlocksToStateVector(double *state) const; + + // Copy internal state out to the user's parameters. + void CopyParameterBlockStateToUserState(); + + // Update a state vector for the program given a delta. + bool Plus(const double* state, + const double* delta, + double* state_plus_delta) const; + + // Set the parameter indices and offsets. This permits mapping backward + // from a ParameterBlock* to an index in the parameter_blocks() vector. For + // any parameter block p, after calling SetParameterOffsetsAndIndex(), it + // is true that + // + // parameter_blocks()[p->index()] == p + // + // If a parameter appears in a residual but not in the parameter block, then + // it will have an index of -1. + // + // This also updates p->state_offset() and p->delta_offset(), which are the + // position of the parameter in the state and delta vector respectively. + void SetParameterOffsetsAndIndex(); + + // See problem.h for what these do. + int NumParameterBlocks() const; + int NumParameters() const; + int NumEffectiveParameters() const; + int NumResidualBlocks() const; + int NumResiduals() const; + + int MaxScratchDoublesNeededForEvaluate() const; + int MaxDerivativesPerResidualBlock() const; + int MaxParametersPerResidualBlock() const; + + // Evaluate the cost and maybe the residuals for the program. If residuals is + // NULL, then residuals are not calculated. If the jacobian is needed, instead + // use the various evaluators (e.g. dense_evaluator.h). + // + // This is a trivial implementation of evaluate not intended for use in the + // core solving loop. The other evaluators, which support constructing the + // jacobian in addition to the cost and residuals, are considerably + // complicated by the need to construct the jacobian. + bool Evaluate(double* cost, double* residuals); + + private: + // The Program does not own the ParameterBlock or ResidualBlock objects. + vector<ParameterBlock*> parameter_blocks_; + vector<ResidualBlock*> residual_blocks_; + + friend class ProblemImpl; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_PROGRAM_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/program_evaluator.h b/extern/libmv/third_party/ceres/internal/ceres/program_evaluator.h new file mode 100644 index 00000000000..7ec74b1b269 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/program_evaluator.h @@ -0,0 +1,279 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 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: keir@google.com (Keir Mierle) +// +// The ProgramEvaluator runs the cost functions contained in each residual block +// and stores the result into a jacobian. The particular type of jacobian is +// abstracted out using two template parameters: +// +// - An "EvaluatePreparer" that is responsible for creating the array with +// pointers to the jacobian blocks where the cost function evaluates to. +// - A "JacobianWriter" that is responsible for storing the resulting +// jacobian blocks in the passed sparse matrix. +// +// This abstraction affords an efficient evaluator implementation while still +// supporting writing to multiple sparse matrix formats. For example, when the +// ProgramEvaluator is parameterized for writing to block sparse matrices, the +// residual jacobians are written directly into their final position in the +// block sparse matrix by the user's CostFunction; there is no copying. +// +// The evaluation is threaded with OpenMP. +// +// The EvaluatePreparer and JacobianWriter interfaces are as follows: +// +// class EvaluatePreparer { +// // Prepare the jacobians array for use as the destination of a call to +// // a cost function's evaluate method. +// void Prepare(const ResidualBlock* residual_block, +// int residual_block_index, +// SparseMatrix* jacobian, +// double** jacobians); +// } +// +// class JacobianWriter { +// // Create a jacobian that this writer can write. Same as +// // Evaluator::CreateJacobian. +// SparseMatrix* CreateJacobian() const; +// +// // Create num_threads evaluate preparers. Caller owns result which must +// // be freed with delete[]. Resulting preparers are valid while *this is. +// EvaluatePreparer* CreateEvaluatePreparers(int num_threads); +// +// // Write the block jacobians from a residual block evaluation to the +// // larger sparse jacobian. +// void Write(int residual_id, +// int residual_offset, +// double** jacobians, +// SparseMatrix* jacobian); +// } +// +// Note: The ProgramEvaluator is not thread safe, since internally it maintains +// some per-thread scratch space. + +#ifndef CERES_INTERNAL_PROGRAM_EVALUATOR_H_ +#define CERES_INTERNAL_PROGRAM_EVALUATOR_H_ + +#ifdef CERES_USE_OPENMP +#include <omp.h> +#endif + +#include "ceres/parameter_block.h" +#include "ceres/program.h" +#include "ceres/residual_block.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/scoped_ptr.h" + +namespace ceres { +namespace internal { + +template<typename EvaluatePreparer, typename JacobianWriter> +class ProgramEvaluator : public Evaluator { + public: + ProgramEvaluator(const Evaluator::Options &options, Program* program) + : options_(options), + program_(program), + jacobian_writer_(options, program), + evaluate_preparers_( + jacobian_writer_.CreateEvaluatePreparers(options.num_threads)) { +#ifndef CERES_USE_OPENMP + CHECK_EQ(1, options_.num_threads) + << "OpenMP support is not compiled into this binary; " + << "only options.num_threads=1 is supported."; +#endif + + BuildResidualLayout(*program, &residual_layout_); + evaluate_scratch_.reset(CreateEvaluatorScratch(*program, + options.num_threads)); + } + + // Implementation of Evaluator interface. + SparseMatrix* CreateJacobian() const { + return jacobian_writer_.CreateJacobian(); + } + + bool Evaluate(const double* state, + double* cost, + double* residuals, + SparseMatrix* jacobian) { + // The parameters are stateful, so set the state before evaluating. + if (!program_->StateVectorToParameterBlocks(state)) { + return false; + } + + if (jacobian) { + jacobian->SetZero(); + } + + // Each thread gets it's own cost and evaluate scratch space. + for (int i = 0; i < options_.num_threads; ++i) { + evaluate_scratch_[i].cost = 0.0; + } + + // This bool is used to disable the loop if an error is encountered + // without breaking out of it. The remaining loop iterations are still run, + // but with an empty body, and so will finish quickly. + bool abort = false; + int num_residual_blocks = program_->NumResidualBlocks(); +#pragma omp parallel for num_threads(options_.num_threads) + for (int i = 0; i < num_residual_blocks; ++i) { +// Disable the loop instead of breaking, as required by OpenMP. +#pragma omp flush(abort) + if (abort) { + continue; + } + +#ifdef CERES_USE_OPENMP + int thread_id = omp_get_thread_num(); +#else + int thread_id = 0; +#endif + EvaluatePreparer* preparer = &evaluate_preparers_[thread_id]; + EvaluateScratch* scratch = &evaluate_scratch_[thread_id]; + + // Prepare block residuals if requested. + const ResidualBlock* residual_block = program_->residual_blocks()[i]; + double* block_residuals = (residuals != NULL) + ? (residuals + residual_layout_[i]) + : NULL; + + // Prepare block jacobians if requested. + double** block_jacobians = NULL; + if (jacobian != NULL) { + preparer->Prepare(residual_block, + i, + jacobian, + scratch->jacobian_block_ptrs.get()); + block_jacobians = scratch->jacobian_block_ptrs.get(); + } + + // Evaluate the cost, residuals, and jacobians. + double block_cost; + if (!residual_block->Evaluate(&block_cost, + block_residuals, + block_jacobians, + scratch->scratch.get())) { + abort = true; +// This ensures that the OpenMP threads have a consistent view of 'abort'. Do +// the flush inside the failure case so that there is usually only one +// synchronization point per loop iteration instead of two. +#pragma omp flush(abort) + continue; + } + + scratch->cost += block_cost; + + if (jacobian != NULL) { + jacobian_writer_.Write(i, + residual_layout_[i], + block_jacobians, + jacobian); + } + } + + if (!abort) { + // Sum the cost from each thread. + (*cost) = 0.0; + for (int i = 0; i < options_.num_threads; ++i) { + (*cost) += evaluate_scratch_[i].cost; + } + } + return !abort; + } + + bool Plus(const double* state, + const double* delta, + double* state_plus_delta) const { + return program_->Plus(state, delta, state_plus_delta); + } + + int NumParameters() const { + return program_->NumParameters(); + } + int NumEffectiveParameters() const { + return program_->NumEffectiveParameters(); + } + + int NumResiduals() const { + return program_->NumResiduals(); + } + + private: + struct EvaluateScratch { + void Init(int max_parameters_per_residual_block, + int max_scratch_doubles_needed_for_evaluate) { + jacobian_block_ptrs.reset( + new double*[max_parameters_per_residual_block]); + scratch.reset(new double[max_scratch_doubles_needed_for_evaluate]); + } + + double cost; + scoped_array<double> scratch; + scoped_array<double*> jacobian_block_ptrs; + }; + + static void BuildResidualLayout(const Program& program, + vector<int>* residual_layout) { + const vector<ResidualBlock*>& residual_blocks = program.residual_blocks(); + residual_layout->resize(program.NumResidualBlocks()); + int residual_pos = 0; + for (int i = 0; i < residual_blocks.size(); ++i) { + const int num_residuals = residual_blocks[i]->NumResiduals(); + (*residual_layout)[i] = residual_pos; + residual_pos += num_residuals; + } + } + + // Create scratch space for each thread evaluating the program. + static EvaluateScratch* CreateEvaluatorScratch(const Program& program, + int num_threads) { + int max_parameters_per_residual_block = + program.MaxParametersPerResidualBlock(); + int max_scratch_doubles_needed_for_evaluate = + program.MaxScratchDoublesNeededForEvaluate(); + + EvaluateScratch* evaluate_scratch = new EvaluateScratch[num_threads]; + for (int i = 0; i < num_threads; i++) { + evaluate_scratch[i].Init(max_parameters_per_residual_block, + max_scratch_doubles_needed_for_evaluate); + } + return evaluate_scratch; + } + + Evaluator::Options options_; + Program* program_; + JacobianWriter jacobian_writer_; + scoped_array<EvaluatePreparer> evaluate_preparers_; + scoped_array<EvaluateScratch> evaluate_scratch_; + vector<int> residual_layout_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_PROGRAM_EVALUATOR_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/random.h b/extern/libmv/third_party/ceres/internal/ceres/random.h new file mode 100644 index 00000000000..769e0b4dd27 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/random.h @@ -0,0 +1,47 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 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: keir@google.com (Keir Mierle) + +#ifndef CERES_INTERNAL_RANDOM_H_ +#define CERES_INTERNAL_RANDOM_H_ + +namespace ceres { + +inline double RandDouble() { + double r = rand(); + return r / RAND_MAX; +} + +inline int Uniform(int n) { + return rand() % n; +} + +} // namespace ceres + +#endif // CERES_INTERNAL_RANDOM_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/residual_block.cc b/extern/libmv/third_party/ceres/internal/ceres/residual_block.cc new file mode 100644 index 00000000000..03867891dba --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/residual_block.cc @@ -0,0 +1,212 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 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: keir@google.com (Keir Mierle) +// sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/residual_block.h" + +#include <algorithm> +#include <cstddef> +#include <vector> + +#include "ceres/corrector.h" +#include "ceres/parameter_block.h" +#include "ceres/residual_block_utils.h" +#include "ceres/cost_function.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/fixed_array.h" +#include "ceres/local_parameterization.h" +#include "ceres/loss_function.h" + +namespace ceres { +namespace internal { + +ResidualBlock::ResidualBlock(const CostFunction* cost_function, + const LossFunction* loss_function, + const vector<ParameterBlock*>& parameter_blocks) + : cost_function_(cost_function), + loss_function_(loss_function), + parameter_blocks_( + new ParameterBlock* [ + cost_function->parameter_block_sizes().size()]) { + std::copy(parameter_blocks.begin(), + parameter_blocks.end(), + parameter_blocks_.get()); +} + +bool ResidualBlock::Evaluate(double* cost, + double* residuals, + double** jacobians, + double* scratch) const { + const int num_parameter_blocks = NumParameterBlocks(); + const int num_residuals = cost_function_->num_residuals(); + + // Collect the parameters from their blocks. This will rarely allocate, since + // residuals taking more than 8 parameter block arguments are rare. + FixedArray<const double*, 8> parameters(num_parameter_blocks); + for (int i = 0; i < num_parameter_blocks; ++i) { + parameters[i] = parameter_blocks_[i]->state(); + } + + // Put pointers into the scratch space into global_jacobians as appropriate. + FixedArray<double*, 8> global_jacobians(num_parameter_blocks); + if (jacobians != NULL) { + for (int i = 0; i < num_parameter_blocks; ++i) { + const ParameterBlock* parameter_block = parameter_blocks_[i]; + if (jacobians[i] != NULL && + parameter_block->LocalParameterizationJacobian() != NULL) { + global_jacobians[i] = scratch; + scratch += num_residuals * parameter_block->Size(); + } else { + global_jacobians[i] = jacobians[i]; + } + } + } + + // If the caller didn't request residuals, use the scratch space for them. + bool outputting_residuals = (residuals != NULL); + if (!outputting_residuals) { + residuals = scratch; + } + + // Invalidate the evaluation buffers so that we can check them after + // the CostFunction::Evaluate call, to see if all the return values + // that were required were written to and that they are finite. + double** eval_jacobians = (jacobians != NULL) ? global_jacobians.get() : NULL; + + InvalidateEvaluation(*this, cost, residuals, eval_jacobians); + + if (!cost_function_->Evaluate(parameters.get(), residuals, eval_jacobians) || + !IsEvaluationValid(*this, + parameters.get(), + cost, + residuals, + eval_jacobians)) { + string message = + "\n\n" + "Error in evaluating the ResidualBlock.\n\n" + "There are two possible reasons. Either the CostFunction did not evaluate and fill all \n" // NOLINT + "residual and jacobians that were requested or there was a non-finite value (nan/infinite)\n" // NOLINT + "generated during the or jacobian computation. \n\n" + + EvaluationToString(*this, + parameters.get(), + cost, + residuals, + eval_jacobians); + LOG(WARNING) << message; + return false; + } + + double squared_norm = VectorRef(residuals, num_residuals).squaredNorm(); + + // Update the jacobians with the local parameterizations. + if (jacobians != NULL) { + for (int i = 0; i < num_parameter_blocks; ++i) { + if (jacobians[i] != NULL) { + const ParameterBlock* parameter_block = parameter_blocks_[i]; + + // Apply local reparameterization to the jacobians. + if (parameter_block->LocalParameterizationJacobian() != NULL) { + ConstMatrixRef local_to_global( + parameter_block->LocalParameterizationJacobian(), + parameter_block->Size(), + parameter_block->LocalSize()); + MatrixRef global_jacobian(global_jacobians[i], + num_residuals, + parameter_block->Size()); + MatrixRef local_jacobian(jacobians[i], + num_residuals, + parameter_block->LocalSize()); + local_jacobian.noalias() = global_jacobian * local_to_global; + } + } + } + } + + if (loss_function_ == NULL) { + *cost = 0.5 * squared_norm; + return true; + } + + double rho[3]; + loss_function_->Evaluate(squared_norm, rho); + *cost = 0.5 * rho[0]; + + // No jacobians and not outputting residuals? All done. Doing an early exit + // here avoids constructing the "Corrector" object below in a common case. + if (jacobians == NULL && !outputting_residuals) { + return true; + } + + // Correct for the effects of the loss function. The jacobians need to be + // corrected before the residuals, since they use the uncorrected residuals. + Corrector correct(squared_norm, rho); + if (jacobians != NULL) { + for (int i = 0; i < num_parameter_blocks; ++i) { + if (jacobians[i] != NULL) { + const ParameterBlock* parameter_block = parameter_blocks_[i]; + + // Correct the jacobians for the loss function. + correct.CorrectJacobian(num_residuals, + parameter_block->LocalSize(), + residuals, + jacobians[i]); + } + } + } + + // Correct the residuals with the loss function. + if (outputting_residuals) { + correct.CorrectResiduals(num_residuals, residuals); + } + return true; +} + +int ResidualBlock::NumScratchDoublesForEvaluate() const { + // Compute the amount of scratch space needed to store the full-sized + // jacobians. For parameters that have no local parameterization no storage + // is needed and the passed-in jacobian array is used directly. Also include + // space to store the residuals, which is needed for cost-only evaluations. + // This is slightly pessimistic, since both won't be needed all the time, but + // the amount of excess should not cause problems for the caller. + int num_parameters = NumParameterBlocks(); + int scratch_doubles = 1; + for (int i = 0; i < num_parameters; ++i) { + const ParameterBlock* parameter_block = parameter_blocks_[i]; + if (!parameter_block->IsConstant() && + parameter_block->LocalParameterizationJacobian() != NULL) { + scratch_doubles += parameter_block->Size(); + } + } + scratch_doubles *= NumResiduals(); + return scratch_doubles; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/residual_block.h b/extern/libmv/third_party/ceres/internal/ceres/residual_block.h new file mode 100644 index 00000000000..e0a06e78958 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/residual_block.h @@ -0,0 +1,124 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// keir@google.com (Keir Mierle) +// +// Purpose : Class and struct definitions for parameter and residual blocks. + +#ifndef CERES_INTERNAL_RESIDUAL_BLOCK_H_ +#define CERES_INTERNAL_RESIDUAL_BLOCK_H_ + +#include <vector> + +#include "ceres/cost_function.h" +#include "ceres/internal/port.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { + +class LossFunction; + +namespace internal { + +class ParameterBlock; + +// A term in the least squares problem. The mathematical form of each term in +// the overall least-squares cost function is: +// +// 1 +// --- loss_function( || cost_function(block1, block2, ...) ||^2 ), +// 2 +// +// Storing the cost function and the loss function separately permits optimizing +// the problem with standard non-linear least techniques, without requiring a +// more general non-linear solver. +// +// The residual block stores pointers to but does not own the cost functions, +// loss functions, and parameter blocks. +class ResidualBlock { + public: + ResidualBlock(const CostFunction* cost_function, + const LossFunction* loss_function, + const vector<ParameterBlock*>& parameter_blocks); + + // Evaluates the residual term, storing the scalar cost in *cost, the residual + // components in *residuals, and the jacobians between the parameters and + // residuals in jacobians[i], in row-major order. If residuals is NULL, the + // residuals are not computed. If jacobians is NULL, no jacobians are + // computed. If jacobians[i] is NULL, then the jacobian for that parameter is + // not computed. + // + // Evaluate needs scratch space which must be supplied by the caller via + // scratch. The array should have at least NumScratchDoublesForEvaluate() + // space available. + // + // The return value indicates the success or failure. If the function returns + // false, the caller should expect the the output memory locations to have + // been modified. + // + // The returned cost and jacobians have had robustification and local + // parameterizations applied already; for example, the jacobian for a + // 4-dimensional quaternion parameter using the "QuaternionParameterization" + // is num_residuals by 3 instead of num_residuals by 4. + bool Evaluate(double* cost, + double* residuals, + double** jacobians, + double* scratch) const; + + const CostFunction* cost_function() const { return cost_function_; } + const LossFunction* loss_function() const { return loss_function_; } + + // Access the parameter blocks for this residual. The array has size + // NumParameterBlocks(). + ParameterBlock* const* parameter_blocks() const { + return parameter_blocks_.get(); + } + + // Number of variable blocks that this residual term depends on. + int NumParameterBlocks() const { + return cost_function_->parameter_block_sizes().size(); + } + + // The size of the residual vector returned by this residual function. + int NumResiduals() const { return cost_function_->num_residuals(); } + + // The minimum amount of scratch space needed to pass to Evaluate(). + int NumScratchDoublesForEvaluate() const; + + private: + const CostFunction* cost_function_; + const LossFunction* loss_function_; + scoped_array<ParameterBlock*> parameter_blocks_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_RESIDUAL_BLOCK_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/residual_block_utils.cc b/extern/libmv/third_party/ceres/internal/ceres/residual_block_utils.cc new file mode 100644 index 00000000000..28e03130844 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/residual_block_utils.cc @@ -0,0 +1,185 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/residual_block_utils.h" + +#include <cmath> +#include <cstddef> +#include <limits> +#include <glog/logging.h> +#include "ceres/residual_block.h" +#include "ceres/parameter_block.h" +#include "ceres/stringprintf.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/port.h" + +#ifdef _MSC_VER +# define isfinite _finite +#endif + +namespace ceres { +namespace internal { + +// It is a near impossibility that user code generates this exact +// value in normal operation, thus we will use it to fill arrays +// before passing them to user code. If on return an element of the +// array still contains this value, we will assume that the user code +// did not write to that memory location. +static const double kImpossibleValue = 1e302; + +bool IsArrayValid(const int size, const double* x) { + if (x != NULL) { + for (int i = 0; i < size; ++i) { + if (!isfinite(x[i]) || (x[i] == kImpossibleValue)) { + return false; + } + } + } + return true; +} + +void InvalidateArray(const int size, double* x) { + if (x != NULL) { + for (int i = 0; i < size; ++i) { + x[i] = kImpossibleValue; + } + } +} + +void InvalidateEvaluation(const ResidualBlock& block, + double* cost, + double* residuals, + double** jacobians) { + const int num_parameter_blocks = block.NumParameterBlocks(); + const int num_residuals = block.NumResiduals(); + + InvalidateArray(1, cost); + InvalidateArray(num_residuals, residuals); + if (jacobians != NULL) { + for (int i = 0; i < num_parameter_blocks; ++i) { + const int parameter_block_size = block.parameter_blocks()[i]->Size(); + InvalidateArray(num_residuals * parameter_block_size, jacobians[i]); + } + } +} + +// Utility routine to print an array of doubles to a string. If the +// array pointer is NULL, it is treated as an array of zeros. +void AppendArrayToString(const int size, const double* x, string* result) { + for (int i = 0; i < size; ++i) { + if (x == NULL) { + StringAppendF(result, "Not Computed "); + } else { + if (x[i] == kImpossibleValue) { + StringAppendF(result, "Uninitialized "); + } else { + StringAppendF(result, "%12g ", x[i]); + } + } + } +} + +string EvaluationToString(const ResidualBlock& block, + double const* const* parameters, + double* cost, + double* residuals, + double** jacobians) { + CHECK_NOTNULL(cost); + CHECK_NOTNULL(residuals); + + const int num_parameter_blocks = block.NumParameterBlocks(); + const int num_residuals = block.NumResiduals(); + string result = ""; + + StringAppendF(&result, + "Residual Block size: %d parameter blocks x %d residuals\n\n", + num_parameter_blocks, num_residuals); + result += + "For each parameter block, the value of the parameters are printed in the first column \n" // NOLINT + "and the value of the jacobian under the corresponding residual. If a ParameterBlock was \n" // NOLINT + "held constant then the corresponding jacobian is printed as 'Not Computed'. If an entry \n" // NOLINT + "of the Jacobian/residual array was requested but was not written to by user code, it is \n" // NOLINT + "indicated by 'Uninitialized'. This is an error. Residuals or Jacobian values evaluating \n" // NOLINT + "to Inf or NaN is also an error. \n\n"; // NOLINT + + string space = "Residuals: "; + result += space; + AppendArrayToString(num_residuals, residuals, &result); + StringAppendF(&result, "\n\n"); + + for (int i = 0; i < num_parameter_blocks; ++i) { + const int parameter_block_size = block.parameter_blocks()[i]->Size(); + StringAppendF( + &result, "Parameter Block %d, size: %d\n", i, parameter_block_size); + StringAppendF(&result, "\n"); + for (int j = 0; j < parameter_block_size; ++j) { + AppendArrayToString(1, parameters[i] + j, &result); + StringAppendF(&result, "| "); + for (int k = 0; k < num_residuals; ++k) { + AppendArrayToString(1, + (jacobians != NULL && jacobians[i] != NULL) + ? jacobians[i] + k * parameter_block_size + j + : NULL, + &result); + } + StringAppendF(&result, "\n"); + } + StringAppendF(&result, "\n"); + } + StringAppendF(&result, "\n"); + return result; +} + +bool IsEvaluationValid(const ResidualBlock& block, + double const* const* parameters, + double* cost, + double* residuals, + double** jacobians) { + const int num_parameter_blocks = block.NumParameterBlocks(); + const int num_residuals = block.NumResiduals(); + + if (!IsArrayValid(num_residuals, residuals)) { + return false; + } + + if (jacobians != NULL) { + for (int i = 0; i < num_parameter_blocks; ++i) { + const int parameter_block_size = block.parameter_blocks()[i]->Size(); + if (!IsArrayValid(num_residuals * parameter_block_size, jacobians[i])) { + return false; + } + } + } + + return true; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/residual_block_utils.h b/extern/libmv/third_party/ceres/internal/ceres/residual_block_utils.h new file mode 100644 index 00000000000..228867cc60c --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/residual_block_utils.h @@ -0,0 +1,89 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Utility routines for ResidualBlock evaluation. +// +// These are useful for detecting two common class of errors. +// +// 1. Uninitialized memory - where the user for some reason did not +// compute part of a cost/residual/jacobian. +// +// 2. Numerical failure while computing the cost/residual/jacobian, +// e.g. NaN, infinities etc. This is particularly useful since the +// automatic differentiation code does computations that are not +// evident to the user and can silently generate hard to debug errors. + +#ifndef CERES_INTERNAL_RESIDUAL_BLOCK_UTILS_H_ +#define CERES_INTERNAL_RESIDUAL_BLOCK_UTILS_H_ + +#include <string> +#include "ceres/internal/port.h" + +namespace ceres { +namespace internal { + +class ResidualBlock; + +// Fill the array x with an impossible value that the user code is +// never expected to compute. +void InvalidateArray(int size, double* x); + +// Check if all the entries of the array x are valid, i.e. all the +// values in the array should be finite and none of them should be +// equal to the "impossible" value used by InvalidateArray. +bool IsArrayValid(int size, const double* x); + +// Invalidate cost, resdual and jacobian arrays (if not NULL). +void InvalidateEvaluation(const ResidualBlock& block, + double* cost, + double* residuals, + double** jacobians); + +// Check if any of the arrays cost, residuals or jacobians contains an +// NaN, return true if it does. +bool IsEvaluationValid(const ResidualBlock& block, + double const* const* parameters, + double* cost, + double* residuals, + double** jacobians); + +// Create a string representation of the Residual block containing the +// value of the parameters, residuals and jacobians if present. +// Useful for debugging output. +string EvaluationToString(const ResidualBlock& block, + double const* const* parameters, + double* cost, + double* residuals, + double** jacobians); + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_RESIDUAL_BLOCK_UTILS_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/runtime_numeric_diff_cost_function.cc b/extern/libmv/third_party/ceres/internal/ceres/runtime_numeric_diff_cost_function.cc new file mode 100644 index 00000000000..ac6d8aa279a --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/runtime_numeric_diff_cost_function.cc @@ -0,0 +1,218 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 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: keir@google.com (Keir Mierle) +// +// Based on the templated version in public/numeric_diff_cost_function.h. + +#include "ceres/runtime_numeric_diff_cost_function.h" + +#include <algorithm> +#include <numeric> +#include <vector> + +#include <glog/logging.h> +#include "Eigen/Dense" +#include "ceres/cost_function.h" +#include "ceres/internal/scoped_ptr.h" + +namespace ceres { +namespace internal { +namespace { + +bool EvaluateJacobianForParameterBlock(const CostFunction* function, + int parameter_block_size, + int parameter_block, + RuntimeNumericDiffMethod method, + double relative_step_size, + double const* residuals_at_eval_point, + double** parameters, + double** jacobians) { + using Eigen::Map; + using Eigen::Matrix; + using Eigen::Dynamic; + using Eigen::RowMajor; + + typedef Matrix<double, Dynamic, 1> ResidualVector; + typedef Matrix<double, Dynamic, 1> ParameterVector; + typedef Matrix<double, Dynamic, Dynamic, RowMajor> JacobianMatrix; + + int num_residuals = function->num_residuals(); + + Map<JacobianMatrix> parameter_jacobian(jacobians[parameter_block], + num_residuals, + parameter_block_size); + + // Mutate one element at a time and then restore. + Map<ParameterVector> x_plus_delta(parameters[parameter_block], + parameter_block_size); + ParameterVector x(x_plus_delta); + ParameterVector step_size = x.array().abs() * relative_step_size; + + // To handle cases where a paremeter is exactly zero, instead use the mean + // step_size for the other dimensions. + double fallback_step_size = step_size.sum() / step_size.rows(); + if (fallback_step_size == 0.0) { + // If all the parameters are zero, there's no good answer. Use the given + // relative step_size as absolute step_size and hope for the best. + fallback_step_size = relative_step_size; + } + + // For each parameter in the parameter block, use finite differences to + // compute the derivative for that parameter. + for (int j = 0; j < parameter_block_size; ++j) { + if (step_size(j) == 0.0) { + // The parameter is exactly zero, so compromise and use the mean step_size + // from the other parameters. This can break in many cases, but it's hard + // to pick a good number without problem specific knowledge. + step_size(j) = fallback_step_size; + } + x_plus_delta(j) = x(j) + step_size(j); + + ResidualVector residuals(num_residuals); + if (!function->Evaluate(parameters, &residuals[0], NULL)) { + // Something went wrong; bail. + return false; + } + + // Compute this column of the jacobian in 3 steps: + // 1. Store residuals for the forward part. + // 2. Subtract residuals for the backward (or 0) part. + // 3. Divide out the run. + parameter_jacobian.col(j) = residuals; + + double one_over_h = 1 / step_size(j); + if (method == CENTRAL) { + // Compute the function on the other side of x(j). + x_plus_delta(j) = x(j) - step_size(j); + + if (!function->Evaluate(parameters, &residuals[0], NULL)) { + // Something went wrong; bail. + return false; + } + parameter_jacobian.col(j) -= residuals; + one_over_h /= 2; + } else { + // Forward difference only; reuse existing residuals evaluation. + parameter_jacobian.col(j) -= + Map<const ResidualVector>(residuals_at_eval_point, num_residuals); + } + x_plus_delta(j) = x(j); // Restore x_plus_delta. + + // Divide out the run to get slope. + parameter_jacobian.col(j) *= one_over_h; + } + return true; +} + +class RuntimeNumericDiffCostFunction : public CostFunction { + public: + RuntimeNumericDiffCostFunction(const CostFunction* function, + RuntimeNumericDiffMethod method, + double relative_step_size) + : function_(function), + method_(method), + relative_step_size_(relative_step_size) { + *mutable_parameter_block_sizes() = function->parameter_block_sizes(); + set_num_residuals(function->num_residuals()); + } + + virtual ~RuntimeNumericDiffCostFunction() { } + + virtual bool Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const { + // Get the function value (residuals) at the the point to evaluate. + bool success = function_->Evaluate(parameters, residuals, NULL); + if (!success) { + // Something went wrong; ignore the jacobian. + return false; + } + if (!jacobians) { + // Nothing to do; just forward. + return true; + } + + const vector<int16>& block_sizes = function_->parameter_block_sizes(); + CHECK(!block_sizes.empty()); + + // Create local space for a copy of the parameters which will get mutated. + int parameters_size = accumulate(block_sizes.begin(), block_sizes.end(), 0); + vector<double> parameters_copy(parameters_size); + vector<double*> parameters_references_copy(block_sizes.size()); + parameters_references_copy[0] = ¶meters_copy[0]; + for (int block = 1; block < block_sizes.size(); ++block) { + parameters_references_copy[block] = parameters_references_copy[block - 1] + + block_sizes[block - 1]; + } + + // Copy the parameters into the local temp space. + for (int block = 0; block < block_sizes.size(); ++block) { + memcpy(parameters_references_copy[block], + parameters[block], + block_sizes[block] * sizeof(*parameters[block])); + } + + for (int block = 0; block < block_sizes.size(); ++block) { + if (!jacobians[block]) { + // No jacobian requested for this parameter / residual pair. + continue; + } + if (!EvaluateJacobianForParameterBlock(function_, + block_sizes[block], + block, + method_, + relative_step_size_, + residuals, + ¶meters_references_copy[0], + jacobians)) { + return false; + } + } + return true; + } + + private: + const CostFunction* function_; + RuntimeNumericDiffMethod method_; + double relative_step_size_; +}; + +} // namespace + +CostFunction* CreateRuntimeNumericDiffCostFunction( + const CostFunction* cost_function, + RuntimeNumericDiffMethod method, + double relative_step_size) { + return new RuntimeNumericDiffCostFunction(cost_function, + method, + relative_step_size); +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/runtime_numeric_diff_cost_function.h b/extern/libmv/third_party/ceres/internal/ceres/runtime_numeric_diff_cost_function.h new file mode 100644 index 00000000000..01b57f92ef3 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/runtime_numeric_diff_cost_function.h @@ -0,0 +1,87 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 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: keir@google.com (Keir Mierle) +// +// Create CostFunctions as needed by the least squares framework with jacobians +// computed via numeric differentiation. +// +// To get a numerically differentiated cost function, define a subclass of +// CostFunction such that the Evaluate() function ignores the jacobian +// parameter. The numeric differentiation wrapper will fill in the jacobian +// parameter if nececssary by repeatedly calling the Evaluate() function with +// small changes to the appropriate parameters, and computing the slope. This +// implementation is not templated (hence the "Runtime" prefix), which is a bit +// slower than but is more convenient than the templated version in +// numeric_diff_cost_function.h +// +// The numerically differentiated version of a cost function for a cost function +// can be constructed as follows: +// +// CostFunction* cost_function = +// CreateRuntimeNumericDiffCostFunction(new MyCostFunction(...), +// CENTRAL, +// TAKE_OWNERSHIP); +// +// The central difference method is considerably more accurate; consider using +// to start and only after that works, trying forward difference. +// +// TODO(keir): Characterize accuracy; mention pitfalls; provide alternatives. + +#ifndef CERES_INTERNAL_RUNTIME_NUMERIC_DIFF_COST_FUNCTION_H_ +#define CERES_INTERNAL_RUNTIME_NUMERIC_DIFF_COST_FUNCTION_H_ + +#include "ceres/cost_function.h" + +namespace ceres { +namespace internal { + +enum RuntimeNumericDiffMethod { + CENTRAL, + FORWARD, +}; + +// Create a cost function that evaluates the derivative with finite differences. +// The base cost_function's implementation of Evaluate() only needs to fill in +// the "residuals" argument and not the "jacobians". Any data written to the +// jacobians by the base cost_function is overwritten. +// +// Forward difference or central difference is selected with CENTRAL or FORWARD. +// The relative eps, which determines the step size for forward and central +// differencing, is set with relative eps. Caller owns the resulting cost +// function, and the resulting cost function does not own the base cost +// function. +CostFunction *CreateRuntimeNumericDiffCostFunction( + const CostFunction *cost_function, + RuntimeNumericDiffMethod method, + double relative_eps); + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_RUNTIME_NUMERIC_DIFF_COST_FUNCTION_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/schur_complement_solver.cc b/extern/libmv/third_party/ceres/internal/ceres/schur_complement_solver.cc new file mode 100644 index 00000000000..2bc8cdd6bec --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/schur_complement_solver.cc @@ -0,0 +1,285 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include <algorithm> +#include <ctime> +#include <set> +#include <vector> +#include "Eigen/Dense" +#include "ceres/block_random_access_dense_matrix.h" +#include "ceres/block_random_access_matrix.h" +#include "ceres/block_random_access_sparse_matrix.h" +#include "ceres/block_sparse_matrix.h" +#include "ceres/block_structure.h" +#include "ceres/detect_structure.h" +#include "ceres/linear_solver.h" +#include "ceres/schur_complement_solver.h" +#include "ceres/suitesparse.h" +#include "ceres/triplet_sparse_matrix.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/port.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +LinearSolver::Summary SchurComplementSolver::SolveImpl( + BlockSparseMatrixBase* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x) { + const time_t start_time = time(NULL); + if (!options_.constant_sparsity || (eliminator_.get() == NULL)) { + InitStorage(A->block_structure()); + DetectStructure(*A->block_structure(), + options_.num_eliminate_blocks, + &options_.row_block_size, + &options_.e_block_size, + &options_.f_block_size); + eliminator_.reset(CHECK_NOTNULL(SchurEliminatorBase::Create(options_))); + eliminator_->Init(options_.num_eliminate_blocks, A->block_structure()); + }; + const time_t init_time = time(NULL); + fill(x, x + A->num_cols(), 0.0); + + LinearSolver::Summary summary; + summary.num_iterations = 1; + summary.termination_type = FAILURE; + eliminator_->Eliminate(A, b, per_solve_options.D, lhs_.get(), rhs_.get()); + const time_t eliminate_time = time(NULL); + + double* reduced_solution = x + A->num_cols() - lhs_->num_cols(); + const bool status = SolveReducedLinearSystem(reduced_solution); + const time_t solve_time = time(NULL); + + if (!status) { + return summary; + } + + eliminator_->BackSubstitute(A, b, per_solve_options.D, reduced_solution, x); + const time_t backsubstitute_time = time(NULL); + summary.termination_type = TOLERANCE; + + VLOG(2) << "time (sec) total: " << backsubstitute_time - start_time + << " init: " << init_time - start_time + << " eliminate: " << eliminate_time - init_time + << " solve: " << solve_time - eliminate_time + << " backsubstitute: " << backsubstitute_time - solve_time; + return summary; +} + +// Initialize a BlockRandomAccessDenseMatrix to store the Schur +// complement. +void DenseSchurComplementSolver::InitStorage( + const CompressedRowBlockStructure* bs) { + const int num_eliminate_blocks = options().num_eliminate_blocks; + const int num_col_blocks = bs->cols.size(); + + vector<int> blocks(num_col_blocks - num_eliminate_blocks, 0); + for (int i = num_eliminate_blocks, j = 0; + i < num_col_blocks; + ++i, ++j) { + blocks[j] = bs->cols[i].size; + } + + set_lhs(new BlockRandomAccessDenseMatrix(blocks)); + set_rhs(new double[lhs()->num_rows()]); +} + +// Solve the system Sx = r, assuming that the matrix S is stored in a +// BlockRandomAccessDenseMatrix. The linear system is solved using +// Eigen's Cholesky factorization. +bool DenseSchurComplementSolver::SolveReducedLinearSystem(double* solution) { + const BlockRandomAccessDenseMatrix* m = + down_cast<const BlockRandomAccessDenseMatrix*>(lhs()); + const int num_rows = m->num_rows(); + + // The case where there are no f blocks, and the system is block + // diagonal. + if (num_rows == 0) { + return true; + } + + // TODO(sameeragarwal): Add proper error handling; this completely ignores + // the quality of the solution to the solve. + VectorRef(solution, num_rows) = + ConstMatrixRef(m->values(), num_rows, num_rows) + .selfadjointView<Eigen::Upper>() + .ldlt() + .solve(ConstVectorRef(rhs(), num_rows)); + + return true; +} + +#ifndef CERES_NO_SUITESPARSE +SparseSchurComplementSolver::SparseSchurComplementSolver( + const LinearSolver::Options& options) + : SchurComplementSolver(options), + symbolic_factor_(NULL) { +} + +SparseSchurComplementSolver::~SparseSchurComplementSolver() { + if (symbolic_factor_ != NULL) { + ss_.Free(symbolic_factor_); + symbolic_factor_ = NULL; + } +} + +// Determine the non-zero blocks in the Schur Complement matrix, and +// initialize a BlockRandomAccessSparseMatrix object. +void SparseSchurComplementSolver::InitStorage( + const CompressedRowBlockStructure* bs) { + const int num_eliminate_blocks = options().num_eliminate_blocks; + const int num_col_blocks = bs->cols.size(); + const int num_row_blocks = bs->rows.size(); + + vector<int> blocks(num_col_blocks - num_eliminate_blocks, 0); + for (int i = num_eliminate_blocks; i < num_col_blocks; ++i) { + blocks[i - num_eliminate_blocks] = bs->cols[i].size; + } + + set<pair<int, int> > block_pairs; + for (int i = 0; i < blocks.size(); ++i) { + block_pairs.insert(make_pair(i, i)); + } + + int r = 0; + while (r < num_row_blocks) { + int e_block_id = bs->rows[r].cells.front().block_id; + if (e_block_id >= num_eliminate_blocks) { + break; + } + vector<int> f_blocks; + + // Add to the chunk until the first block in the row is + // different than the one in the first row for the chunk. + for (; r < num_row_blocks; ++r) { + const CompressedRow& row = bs->rows[r]; + if (row.cells.front().block_id != e_block_id) { + break; + } + + // Iterate over the blocks in the row, ignoring the first + // block since it is the one to be eliminated. + for (int c = 1; c < row.cells.size(); ++c) { + const Cell& cell = row.cells[c]; + f_blocks.push_back(cell.block_id - num_eliminate_blocks); + } + } + + sort(f_blocks.begin(), f_blocks.end()); + f_blocks.erase(unique(f_blocks.begin(), f_blocks.end()), f_blocks.end()); + for (int i = 0; i < f_blocks.size(); ++i) { + for (int j = i + 1; j < f_blocks.size(); ++j) { + block_pairs.insert(make_pair(f_blocks[i], f_blocks[j])); + } + } + } + + // Remaing rows do not contribute to the chunks and directly go + // into the schur complement via an outer product. + for (; r < num_row_blocks; ++r) { + const CompressedRow& row = bs->rows[r]; + CHECK_GE(row.cells.front().block_id, num_eliminate_blocks); + for (int i = 0; i < row.cells.size(); ++i) { + int r_block1_id = row.cells[i].block_id - num_eliminate_blocks; + for (int j = 0; j < row.cells.size(); ++j) { + int r_block2_id = row.cells[j].block_id - num_eliminate_blocks; + if (r_block1_id <= r_block2_id) { + block_pairs.insert(make_pair(r_block1_id, r_block2_id)); + } + } + } + } + + set_lhs(new BlockRandomAccessSparseMatrix(blocks, block_pairs)); + set_rhs(new double[lhs()->num_rows()]); +} + +// Solve the system Sx = r, assuming that the matrix S is stored in a +// BlockRandomAccessSparseMatrix. The linear system is solved using +// CHOLMOD's sparse cholesky factorization routines. +bool SparseSchurComplementSolver::SolveReducedLinearSystem(double* solution) { + // Extract the TripletSparseMatrix that is used for actually storing S. + TripletSparseMatrix* tsm = + const_cast<TripletSparseMatrix*>( + down_cast<const BlockRandomAccessSparseMatrix*>(lhs())->matrix()); + + const int num_rows = tsm->num_rows(); + + // The case where there are no f blocks, and the system is block + // diagonal. + if (num_rows == 0) { + return true; + } + + cholmod_sparse* cholmod_lhs = ss_.CreateSparseMatrix(tsm); + // The matrix is symmetric, and the upper triangular part of the + // matrix contains the values. + cholmod_lhs->stype = 1; + + cholmod_dense* cholmod_rhs = + ss_.CreateDenseVector(const_cast<double*>(rhs()), num_rows, num_rows); + + // Symbolic factorization is computed if we don't already have one handy. + if (symbolic_factor_ == NULL) { + symbolic_factor_ = ss_.AnalyzeCholesky(cholmod_lhs); + } + + cholmod_dense* cholmod_solution = + ss_.SolveCholesky(cholmod_lhs, symbolic_factor_, cholmod_rhs); + + ss_.Free(cholmod_lhs); + cholmod_lhs = NULL; + ss_.Free(cholmod_rhs); + cholmod_rhs = NULL; + + // If sparsity is not constant across calls, then reset the symbolic + // factorization. + if (!options().constant_sparsity) { + ss_.Free(symbolic_factor_); + symbolic_factor_ = NULL; + } + + if (cholmod_solution == NULL) { + LOG(ERROR) << "CHOLMOD solve failed."; + return false; + } + + VectorRef(solution, num_rows) + = VectorRef(static_cast<double*>(cholmod_solution->x), num_rows); + ss_.Free(cholmod_solution); + return true; +} +#endif // CERES_NO_SUITESPARSE + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/schur_complement_solver.h b/extern/libmv/third_party/ceres/internal/ceres/schur_complement_solver.h new file mode 100644 index 00000000000..039bc09e3ce --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/schur_complement_solver.h @@ -0,0 +1,182 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_SCHUR_COMPLEMENT_SOLVER_H_ +#define CERES_INTERNAL_SCHUR_COMPLEMENT_SOLVER_H_ + +#include "ceres/block_random_access_matrix.h" +#include "ceres/block_sparse_matrix.h" +#include "ceres/block_structure.h" +#include "ceres/linear_solver.h" +#include "ceres/schur_eliminator.h" +#include "ceres/suitesparse.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +class BlockSparseMatrixBase; + +// Base class for Schur complement based linear least squares +// solvers. It assumes that the input linear system Ax = b can be +// partitioned into +// +// E y + F z = b +// +// Where x = [y;z] is a partition of the variables. The paritioning +// of the variables is such that, E'E is a block diagonal +// matrix. Further, the rows of A are ordered so that for every +// variable block in y, all the rows containing that variable block +// occur as a vertically contiguous block. i.e the matrix A looks like +// +// E F +// A = [ y1 0 0 0 | z1 0 0 0 z5] +// [ y1 0 0 0 | z1 z2 0 0 0] +// [ 0 y2 0 0 | 0 0 z3 0 0] +// [ 0 0 y3 0 | z1 z2 z3 z4 z5] +// [ 0 0 y3 0 | z1 0 0 0 z5] +// [ 0 0 0 y4 | 0 0 0 0 z5] +// [ 0 0 0 y4 | 0 z2 0 0 0] +// [ 0 0 0 y4 | 0 0 0 0 0] +// [ 0 0 0 0 | z1 0 0 0 0] +// [ 0 0 0 0 | 0 0 z3 z4 z5] +// +// This structure should be reflected in the corresponding +// CompressedRowBlockStructure object associated with A. The linear +// system Ax = b should either be well posed or the array D below +// should be non-null and the diagonal matrix corresponding to it +// should be non-singular. +// +// SchurComplementSolver has two sub-classes. +// +// DenseSchurComplementSolver: For problems where the Schur complement +// matrix is small and dense, or if CHOLMOD/SuiteSparse is not +// installed. For structure from motion problems, this is solver can +// be used for problems with upto a few hundred cameras. +// +// SparseSchurComplementSolver: For problems where the Schur +// complement matrix is large and sparse. It requires that +// CHOLMOD/SuiteSparse be installed, as it uses CHOLMOD to find a +// sparse Cholesky factorization of the Schur complement. This solver +// can be used for solving structure from motion problems with tens of +// thousands of cameras, though depending on the exact sparsity +// structure, it maybe better to use an iterative solver. +// +// The two solvers can be instantiated by calling +// LinearSolver::CreateLinearSolver with LinearSolver::Options::type +// set to DENSE_SCHUR and SPARSE_SCHUR +// respectively. LinearSolver::Options::num_eliminate_blocks should be +// at least 1. +class SchurComplementSolver : public BlockSparseMatrixBaseSolver { + public: + explicit SchurComplementSolver(const LinearSolver::Options& options) + : options_(options) { + CHECK_GT(options.num_eliminate_blocks, 0); + } + + // LinearSolver methods + virtual ~SchurComplementSolver() {} + virtual LinearSolver::Summary SolveImpl( + BlockSparseMatrixBase* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x); + + protected: + const LinearSolver::Options& options() const { return options_; } + + const BlockRandomAccessMatrix* lhs() const { return lhs_.get(); } + void set_lhs(BlockRandomAccessMatrix* lhs) { lhs_.reset(lhs); } + const double* rhs() const { return rhs_.get(); } + void set_rhs(double* rhs) { rhs_.reset(rhs); } + + private: + virtual void InitStorage(const CompressedRowBlockStructure* bs) = 0; + virtual bool SolveReducedLinearSystem(double* solution) = 0; + + LinearSolver::Options options_; + + scoped_ptr<SchurEliminatorBase> eliminator_; + scoped_ptr<BlockRandomAccessMatrix> lhs_; + scoped_array<double> rhs_; + + DISALLOW_COPY_AND_ASSIGN(SchurComplementSolver); +}; + +// Dense Cholesky factorization based solver. +class DenseSchurComplementSolver : public SchurComplementSolver { + public: + explicit DenseSchurComplementSolver(const LinearSolver::Options& options) + : SchurComplementSolver(options) {} + virtual ~DenseSchurComplementSolver() {} + + private: + virtual void InitStorage(const CompressedRowBlockStructure* bs); + virtual bool SolveReducedLinearSystem(double* solution); + + DISALLOW_COPY_AND_ASSIGN(DenseSchurComplementSolver); +}; + +#ifndef CERES_NO_SUITESPARSE +// Sparse Cholesky factorization based solver. +class SparseSchurComplementSolver : public SchurComplementSolver { + public: + explicit SparseSchurComplementSolver(const LinearSolver::Options& options); + virtual ~SparseSchurComplementSolver(); + + private: + virtual void InitStorage(const CompressedRowBlockStructure* bs); + virtual bool SolveReducedLinearSystem(double* solution); + + + SuiteSparse ss_; + // Symbolic factorization of the reduced linear system. Precomputed + // once and reused if constant_sparsity_ is true. + cholmod_factor* symbolic_factor_; + DISALLOW_COPY_AND_ASSIGN(SparseSchurComplementSolver); +}; +#else // CERES_NO_SUITESPARSE +class SparseSchurComplementSolver : public SchurComplementSolver { + public: + explicit SparseSchurComplementSolver(const LinearSolver::Options& options) + : SchurComplementSolver(options) { + LOG(FATAL) << "SPARSE_SCHUR is not available. Please " + "build Ceres with SuiteSparse."; + } + + virtual ~SparseSchurComplementSolver() {} +}; +#endif // CERES_NO_SUITESPARSE + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_SCHUR_COMPLEMENT_SOLVER_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/schur_eliminator.cc b/extern/libmv/third_party/ceres/internal/ceres/schur_eliminator.cc new file mode 100644 index 00000000000..44f5be3b4e9 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/schur_eliminator.cc @@ -0,0 +1,141 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_template_specializations.py. +// Editing it manually is not recommended. + +#include "ceres/linear_solver.h" +#include "ceres/schur_eliminator.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +SchurEliminatorBase* +SchurEliminatorBase::Create(const LinearSolver::Options& options) { +#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION + if ((options.row_block_size == 2) && + (options.e_block_size == 2) && + (options.f_block_size == 2)) { + return new SchurEliminator<2, 2, 2>(options); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 2) && + (options.f_block_size == 3)) { + return new SchurEliminator<2, 2, 3>(options); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 2) && + (options.f_block_size == 4)) { + return new SchurEliminator<2, 2, 4>(options); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 2) && + (options.f_block_size == Dynamic)) { + return new SchurEliminator<2, 2, Dynamic>(options); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 3) && + (options.f_block_size == 3)) { + return new SchurEliminator<2, 3, 3>(options); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 3) && + (options.f_block_size == 4)) { + return new SchurEliminator<2, 3, 4>(options); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 3) && + (options.f_block_size == 9)) { + return new SchurEliminator<2, 3, 9>(options); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 3) && + (options.f_block_size == Dynamic)) { + return new SchurEliminator<2, 3, Dynamic>(options); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 4) && + (options.f_block_size == 3)) { + return new SchurEliminator<2, 4, 3>(options); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 4) && + (options.f_block_size == 4)) { + return new SchurEliminator<2, 4, 4>(options); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 4) && + (options.f_block_size == Dynamic)) { + return new SchurEliminator<2, 4, Dynamic>(options); + } + if ((options.row_block_size == 4) && + (options.e_block_size == 4) && + (options.f_block_size == 2)) { + return new SchurEliminator<4, 4, 2>(options); + } + if ((options.row_block_size == 4) && + (options.e_block_size == 4) && + (options.f_block_size == 3)) { + return new SchurEliminator<4, 4, 3>(options); + } + if ((options.row_block_size == 4) && + (options.e_block_size == 4) && + (options.f_block_size == 4)) { + return new SchurEliminator<4, 4, 4>(options); + } + if ((options.row_block_size == 4) && + (options.e_block_size == 4) && + (options.f_block_size == Dynamic)) { + return new SchurEliminator<4, 4, Dynamic>(options); + } + if ((options.row_block_size == Dynamic) && + (options.e_block_size == Dynamic) && + (options.f_block_size == Dynamic)) { + return new SchurEliminator<Dynamic, Dynamic, Dynamic>(options); + } + +#endif + VLOG(1) << "Template specializations not found for <" + << options.row_block_size << "," + << options.e_block_size << "," + << options.f_block_size << ">"; + return new SchurEliminator<Dynamic, Dynamic, Dynamic>(options); +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/schur_eliminator.h b/extern/libmv/third_party/ceres/internal/ceres/schur_eliminator.h new file mode 100644 index 00000000000..c24fe435f54 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/schur_eliminator.h @@ -0,0 +1,339 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_SCHUR_ELIMINATOR_H_ +#define CERES_INTERNAL_SCHUR_ELIMINATOR_H_ + +#include <map> +#include <vector> +#include "ceres/mutex.h" +#include "ceres/block_random_access_matrix.h" +#include "ceres/block_sparse_matrix.h" +#include "ceres/block_structure.h" +#include "ceres/linear_solver.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/scoped_ptr.h" + +namespace ceres { +namespace internal { + +// Classes implementing the SchurEliminatorBase interface implement +// variable elimination for linear least squares problems. Assuming +// that the input linear system Ax = b can be partitioned into +// +// E y + F z = b +// +// Where x = [y;z] is a partition of the variables. The paritioning +// of the variables is such that, E'E is a block diagonal matrix. Or +// in other words, the parameter blocks in E form an independent set +// of the of the graph implied by the block matrix A'A. Then, this +// class provides the functionality to compute the Schur complement +// system +// +// S z = r +// +// where +// +// S = F'F - F'E (E'E)^{-1} E'F and r = F'b - F'E(E'E)^(-1) E'b +// +// This is the Eliminate operation, i.e., construct the linear system +// obtained by eliminating the variables in E. +// +// The eliminator also provides the reverse functionality, i.e. given +// values for z it can back substitute for the values of y, by solving the +// linear system +// +// Ey = b - F z +// +// which is done by observing that +// +// y = (E'E)^(-1) [E'b - E'F z] +// +// The eliminator has a number of requirements. +// +// The rows of A are ordered so that for every variable block in y, +// all the rows containing that variable block occur as a vertically +// contiguous block. i.e the matrix A looks like +// +// E F chunk +// A = [ y1 0 0 0 | z1 0 0 0 z5] 1 +// [ y1 0 0 0 | z1 z2 0 0 0] 1 +// [ 0 y2 0 0 | 0 0 z3 0 0] 2 +// [ 0 0 y3 0 | z1 z2 z3 z4 z5] 3 +// [ 0 0 y3 0 | z1 0 0 0 z5] 3 +// [ 0 0 0 y4 | 0 0 0 0 z5] 4 +// [ 0 0 0 y4 | 0 z2 0 0 0] 4 +// [ 0 0 0 y4 | 0 0 0 0 0] 4 +// [ 0 0 0 0 | z1 0 0 0 0] non chunk blocks +// [ 0 0 0 0 | 0 0 z3 z4 z5] non chunk blocks +// +// This structure should be reflected in the corresponding +// CompressedRowBlockStructure object associated with A. The linear +// system Ax = b should either be well posed or the array D below +// should be non-null and the diagonal matrix corresponding to it +// should be non-singular. For simplicity of exposition only the case +// with a null D is described. +// +// The usual way to do the elimination is as follows. Starting with +// +// E y + F z = b +// +// we can form the normal equations, +// +// E'E y + E'F z = E'b +// F'E y + F'F z = F'b +// +// multiplying both sides of the first equation by (E'E)^(-1) and then +// by F'E we get +// +// F'E y + F'E (E'E)^(-1) E'F z = F'E (E'E)^(-1) E'b +// F'E y + F'F z = F'b +// +// now subtracting the two equations we get +// +// [FF' - F'E (E'E)^(-1) E'F] z = F'b - F'E(E'E)^(-1) E'b +// +// Instead of forming the normal equations and operating on them as +// general sparse matrices, the algorithm here deals with one +// parameter block in y at a time. The rows corresponding to a single +// parameter block yi are known as a chunk, and the algorithm operates +// on one chunk at a time. The mathematics remains the same since the +// reduced linear system can be shown to be the sum of the reduced +// linear systems for each chunk. This can be seen by observing two +// things. +// +// 1. E'E is a block diagonal matrix. +// +// 2. When E'F is computed, only the terms within a single chunk +// interact, i.e for y1 column blocks when transposed and multiplied +// with F, the only non-zero contribution comes from the blocks in +// chunk1. +// +// Thus, the reduced linear system +// +// FF' - F'E (E'E)^(-1) E'F +// +// can be re-written as +// +// sum_k F_k F_k' - F_k'E_k (E_k'E_k)^(-1) E_k' F_k +// +// Where the sum is over chunks and E_k'E_k is dense matrix of size y1 +// x y1. +// +// Advanced usage. Uptil now it has been assumed that the user would +// be interested in all of the Schur Complement S. However, it is also +// possible to use this eliminator to obtain an arbitrary submatrix of +// the full Schur complement. When the eliminator is generating the +// blocks of S, it asks the RandomAccessBlockMatrix instance passed to +// it if it has storage for that block. If it does, the eliminator +// computes/updates it, if not it is skipped. This is useful when one +// is interested in constructing a preconditioner based on the Schur +// Complement, e.g., computing the block diagonal of S so that it can +// be used as a preconditioner for an Iterative Substructuring based +// solver [See Agarwal et al, Bundle Adjustment in the Large, ECCV +// 2008 for an example of such use]. +// +// Example usage: Please see schur_complement_solver.cc +class SchurEliminatorBase { + public: + virtual ~SchurEliminatorBase() {} + + // Initialize the eliminator. It is the user's responsibilty to call + // this function before calling Eliminate or BackSubstitute. It is + // also the caller's responsibilty to ensure that the + // CompressedRowBlockStructure object passed to this method is the + // same one (or is equivalent to) the one associated with the + // BlockSparseMatrixBase objects below. + virtual void Init(int num_eliminate_blocks, + const CompressedRowBlockStructure* bs) = 0; + + // Compute the Schur complement system from the augmented linear + // least squares problem [A;D] x = [b;0]. The left hand side and the + // right hand side of the reduced linear system are returned in lhs + // and rhs respectively. + // + // It is the caller's responsibility to construct and initialize + // lhs. Depending upon the structure of the lhs object passed here, + // the full or a submatrix of the Schur complement will be computed. + // + // Since the Schur complement is a symmetric matrix, only the upper + // triangular part of the Schur complement is computed. + virtual void Eliminate(const BlockSparseMatrixBase* A, + const double* b, + const double* D, + BlockRandomAccessMatrix* lhs, + double* rhs) = 0; + + // Given values for the variables z in the F block of A, solve for + // the optimal values of the variables y corresponding to the E + // block in A. + virtual void BackSubstitute(const BlockSparseMatrixBase* A, + const double* b, + const double* D, + const double* z, + double* y) = 0; + // Factory + static SchurEliminatorBase* Create(const LinearSolver::Options& options); +}; + +// Templated implementation of the SchurEliminatorBase interface. The +// templating is on the sizes of the row, e and f blocks sizes in the +// input matrix. In many problems, the sizes of one or more of these +// blocks are constant, in that case, its worth passing these +// parameters as template arguments so that they are visible to the +// compiler and can be used for compile time optimization of the low +// level linear algebra routines. +// +// This implementation is mulithreaded using OpenMP. The level of +// parallelism is controlled by LinearSolver::Options::num_threads. +template <int kRowBlockSize = Dynamic, + int kEBlockSize = Dynamic, + int kFBlockSize = Dynamic > +class SchurEliminator : public SchurEliminatorBase { + public: + explicit SchurEliminator(const LinearSolver::Options& options) + : num_threads_(options.num_threads) { + } + + // SchurEliminatorBase Interface + virtual ~SchurEliminator(); + virtual void Init(int num_eliminate_blocks, + const CompressedRowBlockStructure* bs); + virtual void Eliminate(const BlockSparseMatrixBase* A, + const double* b, + const double* D, + BlockRandomAccessMatrix* lhs, + double* rhs); + virtual void BackSubstitute(const BlockSparseMatrixBase* A, + const double* b, + const double* D, + const double* z, + double* y); + + private: + // Chunk objects store combinatorial information needed to + // efficiently eliminate a whole chunk out of the least squares + // problem. Consider the first chunk in the example matrix above. + // + // [ y1 0 0 0 | z1 0 0 0 z5] + // [ y1 0 0 0 | z1 z2 0 0 0] + // + // One of the intermediate quantities that needs to be calculated is + // for each row the product of the y block transposed with the + // non-zero z block, and the sum of these blocks across rows. A + // temporary array "buffer_" is used for computing and storing them + // and the buffer_layout maps the indices of the z-blocks to + // position in the buffer_ array. The size of the chunk is the + // number of row blocks/residual blocks for the particular y block + // being considered. + // + // For the example chunk shown above, + // + // size = 2 + // + // The entries of buffer_layout will be filled in the following order. + // + // buffer_layout[z1] = 0 + // buffer_layout[z5] = y1 * z1 + // buffer_layout[z2] = y1 * z1 + y1 * z5 + typedef map<int, int> BufferLayoutType; + struct Chunk { + Chunk() : size(0) {} + int size; + int start; + BufferLayoutType buffer_layout; + }; + + void ChunkDiagonalBlockAndGradient( + const Chunk& chunk, + const BlockSparseMatrixBase* A, + const double* b, + int row_block_counter, + typename EigenTypes<kEBlockSize, kEBlockSize>::Matrix* eet, + typename EigenTypes<kEBlockSize>::Vector* g, + double* buffer, + BlockRandomAccessMatrix* lhs); + + void UpdateRhs(const Chunk& chunk, + const BlockSparseMatrixBase* A, + const double* b, + int row_block_counter, + const Vector& inverse_ete_g, + double* rhs); + + void ChunkOuterProduct(const CompressedRowBlockStructure* bs, + const Matrix& inverse_eet, + const double* buffer, + const BufferLayoutType& buffer_layout, + BlockRandomAccessMatrix* lhs); + void EBlockRowOuterProduct(const BlockSparseMatrixBase* A, + int row_block_index, + BlockRandomAccessMatrix* lhs); + + + void NoEBlockRowsUpdate(const BlockSparseMatrixBase* A, + const double* b, + int row_block_counter, + BlockRandomAccessMatrix* lhs, + double* rhs); + + void NoEBlockRowOuterProduct(const BlockSparseMatrixBase* A, + int row_block_index, + BlockRandomAccessMatrix* lhs); + + int num_eliminate_blocks_; + + // Block layout of the columns of the reduced linear system. Since + // the f blocks can be of varying size, this vector stores the + // position of each f block in the row/col of the reduced linear + // system. Thus lhs_row_layout_[i] is the row/col position of the + // i^th f block. + vector<int> lhs_row_layout_; + + // Combinatorial structure of the chunks in A. For more information + // see the documentation of the Chunk object above. + vector<Chunk> chunks_; + + // Buffer to store the products of the y and z blocks generated + // during the elimination phase. + scoped_array<double> buffer_; + int buffer_size_; + int num_threads_; + int uneliminated_row_begins_; + + // Locks for the blocks in the right hand side of the reduced linear + // system. + vector<Mutex*> rhs_locks_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_SCHUR_ELIMINATOR_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/schur_eliminator_impl.h b/extern/libmv/third_party/ceres/internal/ceres/schur_eliminator_impl.h new file mode 100644 index 00000000000..a388d005424 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/schur_eliminator_impl.h @@ -0,0 +1,702 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// TODO(sameeragarwal): row_block_counter can perhaps be replaced by +// Chunk::start ? + +#ifndef CERES_INTERNAL_SCHUR_ELIMINATOR_IMPL_H_ +#define CERES_INTERNAL_SCHUR_ELIMINATOR_IMPL_H_ + +#ifdef CERES_USE_OPENMP +#include <omp.h> +#endif + +// Eigen has an internal threshold switching between different matrix +// multiplication algorithms. In particular for matrices larger than +// EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD it uses a cache friendly +// matrix matrix product algorithm that has a higher setup cost. For +// matrix sizes close to this threshold, especially when the matrices +// are thin and long, the default choice may not be optimal. This is +// the case for us, as the default choice causes a 30% performance +// regression when we moved from Eigen2 to Eigen3. +#define EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD 10 + +#include <algorithm> +#include <map> +#include <glog/logging.h> +#include "Eigen/Dense" +#include "ceres/block_random_access_matrix.h" +#include "ceres/block_sparse_matrix.h" +#include "ceres/block_structure.h" +#include "ceres/map_util.h" +#include "ceres/schur_eliminator.h" +#include "ceres/stl_util.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/scoped_ptr.h" + +namespace ceres { +namespace internal { + +template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> +SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>::~SchurEliminator() { + STLDeleteElements(&rhs_locks_); +} + +template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> +void +SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: +Init(int num_eliminate_blocks, const CompressedRowBlockStructure* bs) { + CHECK_GT(num_eliminate_blocks, 0) + << "SchurComplementSolver cannot be initialized with " + << "num_eliminate_blocks = 0."; + + num_eliminate_blocks_ = num_eliminate_blocks; + + const int num_col_blocks = bs->cols.size(); + const int num_row_blocks = bs->rows.size(); + + buffer_size_ = 1; + chunks_.clear(); + lhs_row_layout_.clear(); + + int lhs_num_rows = 0; + // Add a map object for each block in the reduced linear system + // and build the row/column block structure of the reduced linear + // system. + lhs_row_layout_.resize(num_col_blocks - num_eliminate_blocks_); + for (int i = num_eliminate_blocks_; i < num_col_blocks; ++i) { + lhs_row_layout_[i - num_eliminate_blocks_] = lhs_num_rows; + lhs_num_rows += bs->cols[i].size; + } + + int r = 0; + // Iterate over the row blocks of A, and detect the chunks. The + // matrix should already have been ordered so that all rows + // containing the same y block are vertically contiguous. Along + // the way also compute the amount of space each chunk will need + // to perform the elimination. + while (r < num_row_blocks) { + const int chunk_block_id = bs->rows[r].cells.front().block_id; + if (chunk_block_id >= num_eliminate_blocks_) { + break; + } + + chunks_.push_back(Chunk()); + Chunk& chunk = chunks_.back(); + chunk.size = 0; + chunk.start = r; + int buffer_size = 0; + const int e_block_size = bs->cols[chunk_block_id].size; + + // Add to the chunk until the first block in the row is + // different than the one in the first row for the chunk. + while (r + chunk.size < num_row_blocks) { + const CompressedRow& row = bs->rows[r + chunk.size]; + if (row.cells.front().block_id != chunk_block_id) { + break; + } + + // Iterate over the blocks in the row, ignoring the first + // block since it is the one to be eliminated. + for (int c = 1; c < row.cells.size(); ++c) { + const Cell& cell = row.cells[c]; + if (InsertIfNotPresent( + &(chunk.buffer_layout), cell.block_id, buffer_size)) { + buffer_size += e_block_size * bs->cols[cell.block_id].size; + } + } + + buffer_size_ = max(buffer_size, buffer_size_); + ++chunk.size; + } + + CHECK_GT(chunk.size, 0); + r += chunk.size; + } + const Chunk& chunk = chunks_.back(); + + uneliminated_row_begins_ = chunk.start + chunk.size; + if (num_threads_ > 1) { + random_shuffle(chunks_.begin(), chunks_.end()); + } + + buffer_.reset(new double[buffer_size_ * num_threads_]); + + STLDeleteElements(&rhs_locks_); + rhs_locks_.resize(num_col_blocks - num_eliminate_blocks_); + for (int i = 0; i < num_col_blocks - num_eliminate_blocks_; ++i) { + rhs_locks_[i] = new Mutex; + } + + VLOG(1) << "Eliminator threads: " << num_threads_; +} + +template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> +void +SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: +Eliminate(const BlockSparseMatrixBase* A, + const double* b, + const double* D, + BlockRandomAccessMatrix* lhs, + double* rhs) { + if (lhs->num_rows() > 0) { + lhs->SetZero(); + VectorRef(rhs, lhs->num_rows()).setZero(); + } + + const CompressedRowBlockStructure* bs = A->block_structure(); + const int num_col_blocks = bs->cols.size(); + + // Add the diagonal to the schur complement. + if (D != NULL) { +#pragma omp parallel for num_threads(num_threads_) schedule(dynamic) + for (int i = num_eliminate_blocks_; i < num_col_blocks; ++i) { + const int block_id = i - num_eliminate_blocks_; + int r, c, row_stride, col_stride; + CellInfo* cell_info = lhs->GetCell(block_id, block_id, + &r, &c, + &row_stride, &col_stride); + if (cell_info != NULL) { + const int block_size = bs->cols[i].size; + typename EigenTypes<kFBlockSize>::ConstVectorRef + diag(D + bs->cols[i].position, block_size); + + MutexLock l(&cell_info->m); + MatrixRef m(cell_info->values, row_stride, col_stride); + m.block(r, c, block_size, block_size).diagonal() + += diag.array().square().matrix(); + } + } + } + + // Eliminate y blocks one chunk at a time. For each chunk,x3 + // compute the entries of the normal equations and the gradient + // vector block corresponding to the y block and then apply + // Gaussian elimination to them. The matrix ete stores the normal + // matrix corresponding to the block being eliminated and array + // buffer_ contains the non-zero blocks in the row corresponding + // to this y block in the normal equations. This computation is + // done in ChunkDiagonalBlockAndGradient. UpdateRhs then applies + // gaussian elimination to the rhs of the normal equations, + // updating the rhs of the reduced linear system by modifying rhs + // blocks for all the z blocks that share a row block/residual + // term with the y block. EliminateRowOuterProduct does the + // corresponding operation for the lhs of the reduced linear + // system. +#pragma omp parallel for num_threads(num_threads_) schedule(dynamic) + for (int i = 0; i < chunks_.size(); ++i) { +#ifdef CERES_USE_OPENMP + int thread_id = omp_get_thread_num(); +#else + int thread_id = 0; +#endif + double* buffer = buffer_.get() + thread_id * buffer_size_; + const Chunk& chunk = chunks_[i]; + const int e_block_id = bs->rows[chunk.start].cells.front().block_id; + const int e_block_size = bs->cols[e_block_id].size; + + VectorRef(buffer, buffer_size_).setZero(); + + typename EigenTypes<kEBlockSize, kEBlockSize>::Matrix + ete(e_block_size, e_block_size); + + if (D != NULL) { + const typename EigenTypes<kEBlockSize>::ConstVectorRef + diag(D + bs->cols[e_block_id].position, e_block_size); + ete = diag.array().square().matrix().asDiagonal(); + } else { + ete.setZero(); + } + + typename EigenTypes<kEBlockSize>::Vector g(e_block_size); + g.setZero(); + + // We are going to be computing + // + // S += F'F - F'E(E'E)^{-1}E'F + // + // for each Chunk. The computation is broken down into a number of + // function calls as below. + + // Compute the outer product of the e_blocks with themselves (ete + // = E'E). Compute the product of the e_blocks with the + // corresonding f_blocks (buffer = E'F), the gradient of the terms + // in this chunk (g) and add the outer product of the f_blocks to + // Schur complement (S += F'F). + ChunkDiagonalBlockAndGradient( + chunk, A, b, chunk.start, &ete, &g, buffer, lhs); + + // Normally one wouldn't compute the inverse explicitly, but + // e_block_size will typically be a small number like 3, in + // which case its much faster to compute the inverse once and + // use it to multiply other matrices/vectors instead of doing a + // Solve call over and over again. + typename EigenTypes<kEBlockSize, kEBlockSize>::Matrix inverse_ete = + ete + .template selfadjointView<Eigen::Upper>() + .ldlt() + .solve(Matrix::Identity(e_block_size, e_block_size)); + + // For the current chunk compute and update the rhs of the reduced + // linear system. + // + // rhs = F'b - F'E(E'E)^(-1) E'b + UpdateRhs(chunk, A, b, chunk.start, inverse_ete * g, rhs); + + // S -= F'E(E'E)^{-1}E'F + ChunkOuterProduct(bs, inverse_ete, buffer, chunk.buffer_layout, lhs); + } + + // For rows with no e_blocks, the schur complement update reduces to + // S += F'F. + NoEBlockRowsUpdate(A, b, uneliminated_row_begins_, lhs, rhs); +} + +template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> +void +SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: +BackSubstitute(const BlockSparseMatrixBase* A, + const double* b, + const double* D, + const double* z, + double* y) { + const CompressedRowBlockStructure* bs = A->block_structure(); +#pragma omp parallel for num_threads(num_threads_) schedule(dynamic) + for (int i = 0; i < chunks_.size(); ++i) { + const Chunk& chunk = chunks_[i]; + const int e_block_id = bs->rows[chunk.start].cells.front().block_id; + const int e_block_size = bs->cols[e_block_id].size; + + typename EigenTypes<kEBlockSize>::VectorRef y_block( + y + bs->cols[e_block_id].position, e_block_size); + + typename EigenTypes<kEBlockSize, kEBlockSize>::Matrix + ete(e_block_size, e_block_size); + if (D != NULL) { + const typename EigenTypes<kEBlockSize>::ConstVectorRef + diag(D + bs->cols[e_block_id].position, e_block_size); + ete = diag.array().square().matrix().asDiagonal(); + } else { + ete.setZero(); + } + + for (int j = 0; j < chunk.size; ++j) { + const CompressedRow& row = bs->rows[chunk.start + j]; + const double* row_values = A->RowBlockValues(chunk.start + j); + const Cell& e_cell = row.cells.front(); + DCHECK_EQ(e_block_id, e_cell.block_id); + const typename EigenTypes<kRowBlockSize, kEBlockSize>::ConstMatrixRef + e_block(row_values + e_cell.position, + row.block.size, + e_block_size); + + typename EigenTypes<kRowBlockSize>::Vector + sj = + typename EigenTypes<kRowBlockSize>::ConstVectorRef + (b + bs->rows[chunk.start + j].block.position, + row.block.size); + + for (int c = 1; c < row.cells.size(); ++c) { + const int f_block_id = row.cells[c].block_id; + const int f_block_size = bs->cols[f_block_id].size; + const typename EigenTypes<kRowBlockSize, kFBlockSize>::ConstMatrixRef + f_block(row_values + row.cells[c].position, + row.block.size, f_block_size); + const int r_block = f_block_id - num_eliminate_blocks_; + + sj -= f_block * + typename EigenTypes<kFBlockSize>::ConstVectorRef + (z + lhs_row_layout_[r_block], f_block_size); + } + + y_block += e_block.transpose() * sj; + ete.template selfadjointView<Eigen::Upper>() + .rankUpdate(e_block.transpose(), 1.0); + } + + y_block = + ete + .template selfadjointView<Eigen::Upper>() + .ldlt() + .solve(y_block); + } +} + +// Update the rhs of the reduced linear system. Compute +// +// F'b - F'E(E'E)^(-1) E'b +template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> +void +SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: +UpdateRhs(const Chunk& chunk, + const BlockSparseMatrixBase* A, + const double* b, + int row_block_counter, + const Vector& inverse_ete_g, + double* rhs) { + const CompressedRowBlockStructure* bs = A->block_structure(); + const int e_block_size = inverse_ete_g.rows(); + int b_pos = bs->rows[row_block_counter].block.position; + for (int j = 0; j < chunk.size; ++j) { + const CompressedRow& row = bs->rows[row_block_counter + j]; + const double *row_values = A->RowBlockValues(row_block_counter + j); + const Cell& e_cell = row.cells.front(); + + const typename EigenTypes<kRowBlockSize, kEBlockSize>::ConstMatrixRef + e_block(row_values + e_cell.position, + row.block.size, + e_block_size); + + const typename EigenTypes<kRowBlockSize>::Vector + sj = + typename EigenTypes<kRowBlockSize>::ConstVectorRef + (b + b_pos, row.block.size) - e_block * (inverse_ete_g); + + for (int c = 1; c < row.cells.size(); ++c) { + const int block_id = row.cells[c].block_id; + const int block_size = bs->cols[block_id].size; + const typename EigenTypes<kRowBlockSize, kFBlockSize>::ConstMatrixRef + b(row_values + row.cells[c].position, + row.block.size, block_size); + + const int block = block_id - num_eliminate_blocks_; + MutexLock l(rhs_locks_[block]); + typename EigenTypes<kFBlockSize>::VectorRef + (rhs + lhs_row_layout_[block], block_size).noalias() + += b.transpose() * sj; + } + b_pos += row.block.size; + } +} + +// Given a Chunk - set of rows with the same e_block, e.g. in the +// following Chunk with two rows. +// +// E F +// [ y11 0 0 0 | z11 0 0 0 z51] +// [ y12 0 0 0 | z12 z22 0 0 0] +// +// this function computes twp matrices. The diagonal block matrix +// +// ete = y11 * y11' + y12 * y12' +// +// and the off diagonal blocks in the Guass Newton Hessian. +// +// buffer = [y11'(z11 + z12), y12' * z22, y11' * z51] +// +// which are zero compressed versions of the block sparse matrices E'E +// and E'F. +// +// and the gradient of the e_block, E'b. +template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> +void +SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: +ChunkDiagonalBlockAndGradient( + const Chunk& chunk, + const BlockSparseMatrixBase* A, + const double* b, + int row_block_counter, + typename EigenTypes<kEBlockSize, kEBlockSize>::Matrix* ete, + typename EigenTypes<kEBlockSize>::Vector* g, + double* buffer, + BlockRandomAccessMatrix* lhs) { + const CompressedRowBlockStructure* bs = A->block_structure(); + + int b_pos = bs->rows[row_block_counter].block.position; + const int e_block_size = ete->rows(); + + // Iterate over the rows in this chunk, for each row, compute the + // contribution of its F blocks to the Schur complement, the + // contribution of its E block to the matrix EE' (ete), and the + // corresponding block in the gradient vector. + for (int j = 0; j < chunk.size; ++j) { + const CompressedRow& row = bs->rows[row_block_counter + j]; + const double *row_values = A->RowBlockValues(row_block_counter + j); + + if (row.cells.size() > 1) { + EBlockRowOuterProduct(A, row_block_counter + j, lhs); + } + + // Extract the e_block, ETE += E_i' E_i + const Cell& e_cell = row.cells.front(); + const typename EigenTypes<kRowBlockSize, kEBlockSize>::ConstMatrixRef + e_block(row_values + e_cell.position, + row.block.size, + e_block_size); + + ete->template selfadjointView<Eigen::Upper>() + .rankUpdate(e_block.transpose(), 1.0); + + // g += E_i' b_i + g->noalias() += e_block.transpose() * + typename EigenTypes<kRowBlockSize>::ConstVectorRef + (b + b_pos, row.block.size); + + // buffer = E'F. This computation is done by iterating over the + // f_blocks for each row in the chunk. + for (int c = 1; c < row.cells.size(); ++c) { + const int f_block_id = row.cells[c].block_id; + const int f_block_size = bs->cols[f_block_id].size; + const typename EigenTypes<kRowBlockSize, kFBlockSize>::ConstMatrixRef + f_block(row_values + row.cells[c].position, + row.block.size, f_block_size); + + double* buffer_ptr = + buffer + FindOrDie(chunk.buffer_layout, f_block_id); + + typename EigenTypes<kEBlockSize, kFBlockSize>::MatrixRef + (buffer_ptr, e_block_size, f_block_size).noalias() + += e_block.transpose() * f_block; + } + b_pos += row.block.size; + } +} + +// Compute the outer product F'E(E'E)^{-1}E'F and subtract it from the +// Schur complement matrix, i.e +// +// S -= F'E(E'E)^{-1}E'F. +template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> +void +SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: +ChunkOuterProduct(const CompressedRowBlockStructure* bs, + const Matrix& inverse_ete, + const double* buffer, + const BufferLayoutType& buffer_layout, + BlockRandomAccessMatrix* lhs) { + // This is the most computationally expensive part of this + // code. Profiling experiments reveal that the bottleneck is not the + // computation of the right-hand matrix product, but memory + // references to the left hand side. + const int e_block_size = inverse_ete.rows(); + BufferLayoutType::const_iterator it1 = buffer_layout.begin(); + // S(i,j) -= bi' * ete^{-1} b_j + for (; it1 != buffer_layout.end(); ++it1) { + const int block1 = it1->first - num_eliminate_blocks_; + const int block1_size = bs->cols[it1->first].size; + + const typename EigenTypes<kEBlockSize, kFBlockSize>::ConstMatrixRef + b1(buffer + it1->second, e_block_size, block1_size); + const typename EigenTypes<kFBlockSize, kEBlockSize>::Matrix + b1_transpose_inverse_ete = b1.transpose() * inverse_ete; + + BufferLayoutType::const_iterator it2 = it1; + for (; it2 != buffer_layout.end(); ++it2) { + const int block2 = it2->first - num_eliminate_blocks_; + + int r, c, row_stride, col_stride; + CellInfo* cell_info = lhs->GetCell(block1, block2, + &r, &c, + &row_stride, &col_stride); + if (cell_info == NULL) { + continue; + } + + const int block2_size = bs->cols[it2->first].size; + const typename EigenTypes<kEBlockSize, kFBlockSize>::ConstMatrixRef + b2(buffer + it2->second, e_block_size, block2_size); + + MutexLock l(&cell_info->m); + MatrixRef m(cell_info->values, row_stride, col_stride); + + // We explicitly construct a block object here instead of using + // m.block(), as m.block() variant of the constructor does not + // allow mixing of template sizing and runtime sizing parameters + // like the Matrix class does. + Eigen::Block<MatrixRef, kFBlockSize, kFBlockSize> + block(m, r, c, block1_size, block2_size); + block.noalias() -= b1_transpose_inverse_ete * b2; + } + } +} + +// For rows with no e_blocks, the schur complement update reduces to S +// += F'F. This function iterates over the rows of A with no e_block, +// and calls NoEBlockRowOuterProduct on each row. +template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> +void +SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: +NoEBlockRowsUpdate(const BlockSparseMatrixBase* A, + const double* b, + int row_block_counter, + BlockRandomAccessMatrix* lhs, + double* rhs) { + const CompressedRowBlockStructure* bs = A->block_structure(); + for (; row_block_counter < bs->rows.size(); ++row_block_counter) { + const CompressedRow& row = bs->rows[row_block_counter]; + const double *row_values = A->RowBlockValues(row_block_counter); + for (int c = 0; c < row.cells.size(); ++c) { + const int block_id = row.cells[c].block_id; + const int block_size = bs->cols[block_id].size; + const int block = block_id - num_eliminate_blocks_; + VectorRef(rhs + lhs_row_layout_[block], block_size).noalias() + += (ConstMatrixRef(row_values + row.cells[c].position, + row.block.size, block_size).transpose() * + ConstVectorRef(b + row.block.position, row.block.size)); + } + NoEBlockRowOuterProduct(A, row_block_counter, lhs); + } +} + + +// A row r of A, which has no e_blocks gets added to the Schur +// Complement as S += r r'. This function is responsible for computing +// the contribution of a single row r to the Schur complement. It is +// very similar in structure to EBlockRowOuterProduct except for +// one difference. It does not use any of the template +// parameters. This is because the algorithm used for detecting the +// static structure of the matrix A only pays attention to rows with +// e_blocks. This is becase rows without e_blocks are rare and +// typically arise from regularization terms in the original +// optimization problem, and have a very different structure than the +// rows with e_blocks. Including them in the static structure +// detection will lead to most template parameters being set to +// dynamic. Since the number of rows without e_blocks is small, the +// lack of templating is not an issue. +template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> +void +SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: +NoEBlockRowOuterProduct(const BlockSparseMatrixBase* A, + int row_block_index, + BlockRandomAccessMatrix* lhs) { + const CompressedRowBlockStructure* bs = A->block_structure(); + const CompressedRow& row = bs->rows[row_block_index]; + const double *row_values = A->RowBlockValues(row_block_index); + for (int i = 0; i < row.cells.size(); ++i) { + const int block1 = row.cells[i].block_id - num_eliminate_blocks_; + DCHECK_GE(block1, 0); + + const int block1_size = bs->cols[row.cells[i].block_id].size; + const ConstMatrixRef b1(row_values + row.cells[i].position, + row.block.size, block1_size); + int r, c, row_stride, col_stride; + CellInfo* cell_info = lhs->GetCell(block1, block1, + &r, &c, + &row_stride, &col_stride); + if (cell_info != NULL) { + MutexLock l(&cell_info->m); + MatrixRef m(cell_info->values, row_stride, col_stride); + m.block(r, c, block1_size, block1_size) + .selfadjointView<Eigen::Upper>() + .rankUpdate(b1.transpose(), 1.0); + } + + for (int j = i + 1; j < row.cells.size(); ++j) { + const int block2 = row.cells[j].block_id - num_eliminate_blocks_; + DCHECK_GE(block2, 0); + DCHECK_LT(block1, block2); + int r, c, row_stride, col_stride; + CellInfo* cell_info = lhs->GetCell(block1, block2, + &r, &c, + &row_stride, &col_stride); + if (cell_info == NULL) { + continue; + } + + const int block2_size = bs->cols[row.cells[j].block_id].size; + MutexLock l(&cell_info->m); + MatrixRef m(cell_info->values, row_stride, col_stride); + m.block(r, c, block1_size, block2_size).noalias() += + b1.transpose() * ConstMatrixRef(row_values + row.cells[j].position, + row.block.size, + block2_size); + } + } +} + +// For a row with an e_block, compute the contribition S += F'F. This +// function has the same structure as NoEBlockRowOuterProduct, except +// that this function uses the template parameters. +template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> +void +SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: +EBlockRowOuterProduct(const BlockSparseMatrixBase* A, + int row_block_index, + BlockRandomAccessMatrix* lhs) { + const CompressedRowBlockStructure* bs = A->block_structure(); + const CompressedRow& row = bs->rows[row_block_index]; + const double *row_values = A->RowBlockValues(row_block_index); + for (int i = 1; i < row.cells.size(); ++i) { + const int block1 = row.cells[i].block_id - num_eliminate_blocks_; + DCHECK_GE(block1, 0); + + const int block1_size = bs->cols[row.cells[i].block_id].size; + const typename EigenTypes<kRowBlockSize, kFBlockSize>::ConstMatrixRef + b1(row_values + row.cells[i].position, + row.block.size, block1_size); + { + int r, c, row_stride, col_stride; + CellInfo* cell_info = lhs->GetCell(block1, block1, + &r, &c, + &row_stride, &col_stride); + if (cell_info == NULL) { + continue; + } + + MutexLock l(&cell_info->m); + MatrixRef m(cell_info->values, row_stride, col_stride); + + Eigen::Block<MatrixRef, kFBlockSize, kFBlockSize> + block(m, r, c, block1_size, block1_size); + block.template selfadjointView<Eigen::Upper>() + .rankUpdate(b1.transpose(), 1.0); + } + + for (int j = i + 1; j < row.cells.size(); ++j) { + const int block2 = row.cells[j].block_id - num_eliminate_blocks_; + DCHECK_GE(block2, 0); + DCHECK_LT(block1, block2); + const int block2_size = bs->cols[row.cells[j].block_id].size; + int r, c, row_stride, col_stride; + CellInfo* cell_info = lhs->GetCell(block1, block2, + &r, &c, + &row_stride, &col_stride); + if (cell_info == NULL) { + continue; + } + + const typename EigenTypes<kRowBlockSize, kFBlockSize>::ConstMatrixRef + b2(row_values + row.cells[j].position, + row.block.size, + block2_size); + + MutexLock l(&cell_info->m); + MatrixRef m(cell_info->values, row_stride, col_stride); + Eigen::Block<MatrixRef, kFBlockSize, kFBlockSize> + block(m, r, c, block1_size, block2_size); + block.noalias() += b1.transpose() * b2; + } + } +} + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_SCHUR_ELIMINATOR_IMPL_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/schur_ordering.cc b/extern/libmv/third_party/ceres/internal/ceres/schur_ordering.cc new file mode 100644 index 00000000000..c4fc1da3c2f --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/schur_ordering.cc @@ -0,0 +1,113 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/schur_ordering.h" + +#include <glog/logging.h> +#include "ceres/graph.h" +#include "ceres/graph_algorithms.h" +#include "ceres/map_util.h" +#include "ceres/parameter_block.h" +#include "ceres/program.h" +#include "ceres/residual_block.h" +#include "ceres/internal/scoped_ptr.h" + +CERES_HASH_NAMESPACE_START + +// Allow us to hash pointers as if they were int's +template<> struct hash< ::ceres::internal::ParameterBlock*> { + size_t operator()(::ceres::internal::ParameterBlock* x) const { + return reinterpret_cast<size_t>(x); + } +}; + +CERES_HASH_NAMESPACE_END + +namespace ceres { +namespace internal { + +int ComputeSchurOrdering(const Program& program, + vector<ParameterBlock*>* ordering) { + CHECK_NOTNULL(ordering)->clear(); + + scoped_ptr<Graph< ParameterBlock*> > graph( + CHECK_NOTNULL(CreateHessianGraph(program))); + int independent_set_size = + IndependentSetOrdering<ParameterBlock*>(*graph, ordering); + const vector<ParameterBlock*>& parameter_blocks = program.parameter_blocks(); + + // Add the excluded blocks to back of the ordering vector. + for (int i = 0; i < parameter_blocks.size(); ++i) { + ParameterBlock* parameter_block = parameter_blocks[i]; + if (parameter_block->IsConstant()) { + ordering->push_back(parameter_block); + } + } + + return independent_set_size; +} + +Graph<ParameterBlock*>* +CreateHessianGraph(const Program& program) { + Graph<ParameterBlock*>* graph = new Graph<ParameterBlock*>; + const vector<ParameterBlock*>& parameter_blocks = program.parameter_blocks(); + for (int i = 0; i < parameter_blocks.size(); ++i) { + ParameterBlock* parameter_block = parameter_blocks[i]; + if (!parameter_block->IsConstant()) { + graph->AddVertex(parameter_block); + } + } + + const vector<ResidualBlock*>& residual_blocks = program.residual_blocks(); + for (int i = 0; i < residual_blocks.size(); ++i) { + const ResidualBlock* residual_block = residual_blocks[i]; + const int num_parameter_blocks = residual_block->NumParameterBlocks(); + ParameterBlock* const* parameter_blocks = + residual_block->parameter_blocks(); + for (int j = 0; j < num_parameter_blocks; ++j) { + if (parameter_blocks[j]->IsConstant()) { + continue; + } + + for (int k = j + 1; k < num_parameter_blocks; ++k) { + if (parameter_blocks[k]->IsConstant()) { + continue; + } + + graph->AddEdge(parameter_blocks[j], parameter_blocks[k]); + } + } + } + + return graph; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/schur_ordering.h b/extern/libmv/third_party/ceres/internal/ceres/schur_ordering.h new file mode 100644 index 00000000000..1f9a4ff354f --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/schur_ordering.h @@ -0,0 +1,74 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Compute a parameter block ordering for use with the Schur +// complement based algorithms. + +#ifndef CERES_INTERNAL_SCHUR_ORDERING_H_ +#define CERES_INTERNAL_SCHUR_ORDERING_H_ + +#include <vector> +#include "ceres/graph.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +class Program; +class ParameterBlock; + +// Uses an approximate independent set ordering to order the parameter +// blocks of a problem so that it is suitable for use with Schur +// complement based solvers. The output variable ordering contains an +// ordering of the parameter blocks and the return value is size of +// the independent set or the number of e_blocks (see +// schur_complement_solver.h for an explanation). Constant parameters +// are added to the end. +// +// The ordering vector has the structure +// +// ordering = [independent set, +// complement of the independent set, +// fixed blocks] +int ComputeSchurOrdering(const Program& program, + vector<ParameterBlock* >* ordering); + + +// Builds a graph on the parameter blocks of a Problem, whose +// structure reflects the sparsity structure of the Hessian. Each +// vertex corresponds to a parameter block in the Problem except for +// parameter blocks that are marked constant. An edge connects two +// parameter blocks, if they co-occur in a residual block. +Graph<ParameterBlock*>* CreateHessianGraph(const Program& program); + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_SCHUR_ORDERING_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/scratch_evaluate_preparer.cc b/extern/libmv/third_party/ceres/internal/ceres/scratch_evaluate_preparer.cc new file mode 100644 index 00000000000..6f0ceefd87d --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/scratch_evaluate_preparer.cc @@ -0,0 +1,78 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 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: keir@google.com (Keir Mierle) + +#include "ceres/scratch_evaluate_preparer.h" + +#include "ceres/parameter_block.h" +#include "ceres/program.h" +#include "ceres/residual_block.h" + +namespace ceres { +namespace internal { + +ScratchEvaluatePreparer* ScratchEvaluatePreparer::Create( + const Program &program, + int num_threads) { + ScratchEvaluatePreparer* preparers = new ScratchEvaluatePreparer[num_threads]; + int max_derivatives_per_residual_block = + program.MaxDerivativesPerResidualBlock(); + for (int i = 0; i < num_threads; i++) { + preparers[i].Init(max_derivatives_per_residual_block); + } + return preparers; +} + +void ScratchEvaluatePreparer::Init(int max_derivatives_per_residual_block) { + jacobian_scratch_.reset( + new double[max_derivatives_per_residual_block]); +} + +// Point the jacobian blocks into the scratch area of this evaluate preparer. +void ScratchEvaluatePreparer::Prepare(const ResidualBlock* residual_block, + int /* residual_block_index */, + SparseMatrix* /* jacobian */, + double** jacobians) { + double* jacobian_block_cursor = jacobian_scratch_.get(); + int num_residuals = residual_block->NumResiduals(); + 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()) { + jacobians[j] = NULL; + } else { + jacobians[j] = jacobian_block_cursor; + jacobian_block_cursor += num_residuals * parameter_block->LocalSize(); + } + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/scratch_evaluate_preparer.h b/extern/libmv/third_party/ceres/internal/ceres/scratch_evaluate_preparer.h new file mode 100644 index 00000000000..6b127081976 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/scratch_evaluate_preparer.h @@ -0,0 +1,69 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 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: keir@google.com (Keir Mierle) +// +// A scratch evaluate preparer provides temporary storage for the jacobians that +// are created when running user-provided cost functions. The evaluator takes +// care to avoid evaluating the jacobian for fixed parameters. + +#ifndef CERES_INTERNAL_SCRATCH_EVALUATE_PREPARER_H_ +#define CERES_INTERNAL_SCRATCH_EVALUATE_PREPARER_H_ + +#include "ceres/internal/scoped_ptr.h" + +namespace ceres { +namespace internal { + +class Program; +class ResidualBlock; +class SparseMatrix; + +class ScratchEvaluatePreparer { + public: + // Create num_threads ScratchEvaluatePreparers. + static ScratchEvaluatePreparer* Create(const Program &program, + int num_threads); + + // EvaluatePreparer interface + void Init(int max_derivatives_per_residual_block); + void Prepare(const ResidualBlock* residual_block, + int residual_block_index, + SparseMatrix* jacobian, + double** jacobians); + + private: + // Scratch space for the jacobians; each jacobian is packed one after another. + // There is enough scratch to hold all the jacobians for the largest residual. + scoped_array<double> jacobian_scratch_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_SCRATCH_EVALUATE_PREPARER_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/solver.cc b/extern/libmv/third_party/ceres/internal/ceres/solver.cc new file mode 100644 index 00000000000..77f04d1d918 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/solver.cc @@ -0,0 +1,230 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 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: keir@google.com (Keir Mierle) +// sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/solver.h" + +#include <vector> +#include "ceres/levenberg_marquardt.h" +#include "ceres/program.h" +#include "ceres/solver_impl.h" +#include "ceres/stringprintf.h" +#include "ceres/problem.h" + +namespace ceres { + +Solver::~Solver() {} + +// TODO(sameeragarwal): The timing code here should use a sub-second +// timer. +void Solver::Solve(const Solver::Options& options, + Problem* problem, + Solver::Summary* summary) { + time_t start_time_seconds = time(NULL); + internal::SolverImpl::Solve(options, problem, summary); + summary->total_time_in_seconds = time(NULL) - start_time_seconds; + summary->preprocessor_time_in_seconds = + summary->total_time_in_seconds - summary->minimizer_time_in_seconds; +} + +void Solve(const Solver::Options& options, + Problem* problem, + Solver::Summary* summary) { + time_t start_time_seconds = time(NULL); + internal::SolverImpl::Solve(options, problem, summary); + summary->total_time_in_seconds = time(NULL) - start_time_seconds; + summary->preprocessor_time_in_seconds = + summary->total_time_in_seconds - summary->minimizer_time_in_seconds; +} + +Solver::Summary::Summary() + // Invalid values for most fields, to ensure that we are not + // accidentally reporting default values. + : termination_type(DID_NOT_RUN), + initial_cost(-1.0), + final_cost(-1.0), + fixed_cost(-1.0), + num_successful_steps(-1), + num_unsuccessful_steps(-1), + preprocessor_time_in_seconds(-1.0), + minimizer_time_in_seconds(-1.0), + total_time_in_seconds(-1.0), + num_parameter_blocks(-1), + num_parameters(-1), + num_residual_blocks(-1), + num_residuals(-1), + num_parameter_blocks_reduced(-1), + num_parameters_reduced(-1), + num_residual_blocks_reduced(-1), + num_residuals_reduced(-1), + num_eliminate_blocks_given(-1), + num_eliminate_blocks_used(-1), + num_threads_given(-1), + num_threads_used(-1), + num_linear_solver_threads_given(-1), + num_linear_solver_threads_used(-1), + linear_solver_type_given(SPARSE_NORMAL_CHOLESKY), + linear_solver_type_used(SPARSE_NORMAL_CHOLESKY), + preconditioner_type(IDENTITY), + ordering_type(NATURAL) { +} + +string Solver::Summary::BriefReport() const { + string report = "Ceres Solver Report: "; + if (termination_type == DID_NOT_RUN) { + CHECK(!error.empty()) + << "Solver terminated with DID_NOT_RUN but the solver did not " + << "return a reason. This is a Ceres error. Please report this " + << "to the Ceres team"; + return report + "Termination: DID_NOT_RUN, because " + error; + } + + internal::StringAppendF(&report, "Iterations: %d", + num_successful_steps + num_unsuccessful_steps); + internal::StringAppendF(&report, ", Initial cost: %e", initial_cost); + + // If the solver failed or was aborted, then the final_cost has no + // meaning. + if (termination_type != NUMERICAL_FAILURE && + termination_type != USER_ABORT) { + internal::StringAppendF(&report, ", Final cost: %e", final_cost); + } + + internal::StringAppendF(&report, ", Termination: %s.", + SolverTerminationTypeToString(termination_type)); + return report; +}; + +string Solver::Summary::FullReport() const { + string report = + "\n" + "Ceres Solver Report\n" + "-------------------\n"; + + if (termination_type == DID_NOT_RUN) { + internal::StringAppendF(&report, " Original\n"); + internal::StringAppendF(&report, "Parameter blocks % 10d\n", + num_parameter_blocks); + internal::StringAppendF(&report, "Parameters % 10d\n", + num_parameters); + internal::StringAppendF(&report, "Residual blocks % 10d\n", + num_residual_blocks); + internal::StringAppendF(&report, "Residual % 10d\n\n", + num_residuals); + } else { + internal::StringAppendF(&report, "%45s %21s\n", "Original", "Reduced"); + internal::StringAppendF(&report, "Parameter blocks % 25d% 25d\n", + num_parameter_blocks, num_parameter_blocks_reduced); + internal::StringAppendF(&report, "Parameters % 25d% 25d\n", + num_parameters, num_parameters_reduced); + internal::StringAppendF(&report, "Residual blocks % 25d% 25d\n", + num_residual_blocks, num_residual_blocks_reduced); + internal::StringAppendF(&report, "Residual % 25d% 25d\n\n", + num_residuals, num_residuals_reduced); + } + + internal::StringAppendF(&report, "%45s %21s\n", "Given", "Used"); + internal::StringAppendF(&report, "Linear solver %25s%25s\n", + LinearSolverTypeToString(linear_solver_type_given), + LinearSolverTypeToString(linear_solver_type_used)); + + if (linear_solver_type_given == CGNR || + linear_solver_type_given == ITERATIVE_SCHUR) { + internal::StringAppendF(&report, "Preconditioner %25s%25s\n", + PreconditionerTypeToString(preconditioner_type), + PreconditionerTypeToString(preconditioner_type)); + } else { + internal::StringAppendF(&report, "Preconditioner %25s%25s\n", + "N/A", "N/A"); + } + + internal::StringAppendF(&report, "Ordering %25s%25s\n", + OrderingTypeToString(ordering_type), + OrderingTypeToString(ordering_type)); + + if (IsSchurType(linear_solver_type_given)) { + if (ordering_type == SCHUR) { + internal::StringAppendF(&report, "num_eliminate_blocks%25s% 25d\n", + "N/A", + num_eliminate_blocks_used); + } else { + internal::StringAppendF(&report, "num_eliminate_blocks% 25d% 25d\n", + num_eliminate_blocks_given, + num_eliminate_blocks_used); + } + } + + internal::StringAppendF(&report, "Threads: % 25d% 25d\n", + num_threads_given, num_threads_used); + internal::StringAppendF(&report, "Linear Solver Threads:% 23d% 25d\n", + num_linear_solver_threads_given, + num_linear_solver_threads_used); + + + if (termination_type == DID_NOT_RUN) { + CHECK(!error.empty()) + << "Solver terminated with DID_NOT_RUN but the solver did not " + << "return a reason. This is a Ceres error. Please report this " + << "to the Ceres team"; + internal::StringAppendF(&report, "Termination: %20s\n", + "DID_NOT_RUN"); + internal::StringAppendF(&report, "Reason: %s\n", error.c_str()); + return report; + } + + internal::StringAppendF(&report, "\nCost:\n"); + internal::StringAppendF(&report, "Initial % 30e\n", initial_cost); + if (termination_type != NUMERICAL_FAILURE && termination_type != USER_ABORT) { + internal::StringAppendF(&report, "Final % 30e\n", final_cost); + internal::StringAppendF(&report, "Change % 30e\n", + initial_cost - final_cost); + } + + internal::StringAppendF(&report, "\nNumber of iterations:\n"); + internal::StringAppendF(&report, "Successful % 20d\n", + num_successful_steps); + internal::StringAppendF(&report, "Unsuccessful % 20d\n", + num_unsuccessful_steps); + internal::StringAppendF(&report, "Total % 20d\n", + num_successful_steps + num_unsuccessful_steps); + internal::StringAppendF(&report, "\nTime (in seconds):\n"); + internal::StringAppendF(&report, "Preprocessor % 25e\n", + preprocessor_time_in_seconds); + internal::StringAppendF(&report, "Minimizer % 25e\n", + minimizer_time_in_seconds); + internal::StringAppendF(&report, "Total % 25e\n", + total_time_in_seconds); + + internal::StringAppendF(&report, "Termination: %25s\n", + SolverTerminationTypeToString(termination_type)); + return report; +}; + +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/solver_impl.cc b/extern/libmv/third_party/ceres/internal/ceres/solver_impl.cc new file mode 100644 index 00000000000..ed07d9dc6d7 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/solver_impl.cc @@ -0,0 +1,693 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 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: keir@google.com (Keir Mierle) + +#include "ceres/solver_impl.h" + +#include <iostream> // NOLINT +#include <numeric> +#include "ceres/evaluator.h" +#include "ceres/gradient_checking_cost_function.h" +#include "ceres/levenberg_marquardt.h" +#include "ceres/linear_solver.h" +#include "ceres/map_util.h" +#include "ceres/minimizer.h" +#include "ceres/parameter_block.h" +#include "ceres/problem_impl.h" +#include "ceres/program.h" +#include "ceres/residual_block.h" +#include "ceres/schur_ordering.h" +#include "ceres/stringprintf.h" +#include "ceres/iteration_callback.h" +#include "ceres/problem.h" + +namespace ceres { +namespace internal { +namespace { + +void EvaluateCostAndResiduals(ProblemImpl* problem_impl, + double* cost, + vector<double>* residuals) { + CHECK_NOTNULL(cost); + Program* program = CHECK_NOTNULL(problem_impl)->mutable_program(); + if (residuals != NULL) { + residuals->resize(program->NumResiduals()); + program->Evaluate(cost, &(*residuals)[0]); + } else { + program->Evaluate(cost, NULL); + } +} + +// Callback for updating the user's parameter blocks. Updates are only +// done if the step is successful. +class StateUpdatingCallback : public IterationCallback { + public: + StateUpdatingCallback(Program* program, double* parameters) + : program_(program), parameters_(parameters) {} + + CallbackReturnType operator()(const IterationSummary& summary) { + if (summary.step_is_successful) { + program_->StateVectorToParameterBlocks(parameters_); + program_->CopyParameterBlockStateToUserState(); + } + return SOLVER_CONTINUE; + } + + private: + Program* program_; + double* parameters_; +}; + +// Callback for logging the state of the minimizer to STDERR or STDOUT +// depending on the user's preferences and logging level. +class LoggingCallback : public IterationCallback { + public: + explicit LoggingCallback(bool log_to_stdout) + : log_to_stdout_(log_to_stdout) {} + + ~LoggingCallback() {} + + CallbackReturnType operator()(const IterationSummary& summary) { + const char* kReportRowFormat = + "% 4d: f:% 8e d:% 3.2e g:% 3.2e h:% 3.2e " + "rho:% 3.2e mu:% 3.2e li:% 3d"; + string output = StringPrintf(kReportRowFormat, + summary.iteration, + summary.cost, + summary.cost_change, + summary.gradient_max_norm, + summary.step_norm, + summary.relative_decrease, + summary.mu, + summary.linear_solver_iterations); + if (log_to_stdout_) { + cout << output << endl; + } else { + VLOG(1) << output; + } + return SOLVER_CONTINUE; + } + + private: + const bool log_to_stdout_; +}; + +} // namespace + +void SolverImpl::Minimize(const Solver::Options& options, + Program* program, + Evaluator* evaluator, + LinearSolver* linear_solver, + double* initial_parameters, + double* final_parameters, + Solver::Summary* summary) { + Minimizer::Options minimizer_options(options); + + LoggingCallback logging_callback(options.minimizer_progress_to_stdout); + if (options.logging_type != SILENT) { + minimizer_options.callbacks.push_back(&logging_callback); + } + + StateUpdatingCallback updating_callback(program, initial_parameters); + if (options.update_state_every_iteration) { + minimizer_options.callbacks.push_back(&updating_callback); + } + + LevenbergMarquardt levenberg_marquardt; + + time_t start_minimizer_time_seconds = time(NULL); + levenberg_marquardt.Minimize(minimizer_options, + evaluator, + linear_solver, + initial_parameters, + final_parameters, + summary); + summary->minimizer_time_in_seconds = + time(NULL) - start_minimizer_time_seconds; +} + +void SolverImpl::Solve(const Solver::Options& original_options, + Problem* problem, + Solver::Summary* summary) { + Solver::Options options(original_options); + +#ifndef CERES_USE_OPENMP + if (options.num_threads > 1) { + LOG(WARNING) + << "OpenMP support is not compiled into this binary; " + << "only options.num_threads=1 is supported. Switching" + << "to single threaded mode."; + options.num_threads = 1; + } + if (options.num_linear_solver_threads > 1) { + LOG(WARNING) + << "OpenMP support is not compiled into this binary; " + << "only options.num_linear_solver_threads=1 is supported. Switching" + << "to single threaded mode."; + options.num_linear_solver_threads = 1; + } +#endif + + // Reset the summary object to its default values; + *CHECK_NOTNULL(summary) = Solver::Summary(); + summary->linear_solver_type_given = options.linear_solver_type; + summary->num_eliminate_blocks_given = original_options.num_eliminate_blocks; + summary->num_threads_given = original_options.num_threads; + summary->num_linear_solver_threads_given = + original_options.num_linear_solver_threads; + summary->ordering_type = original_options.ordering_type; + + ProblemImpl* problem_impl = CHECK_NOTNULL(problem)->problem_impl_.get(); + + summary->num_parameter_blocks = problem_impl->NumParameterBlocks(); + summary->num_parameters = problem_impl->NumParameters(); + summary->num_residual_blocks = problem_impl->NumResidualBlocks(); + summary->num_residuals = problem_impl->NumResiduals(); + + summary->num_threads_used = options.num_threads; + + // Evaluate the initial cost and residual vector (if needed). The + // initial cost needs to be computed on the original unpreprocessed + // problem, as it is used to determine the value of the "fixed" part + // of the objective function after the problem has undergone + // reduction. Also the initial residuals are in the order in which + // the user added the ResidualBlocks to the optimization problem. + EvaluateCostAndResiduals(problem_impl, + &summary->initial_cost, + options.return_initial_residuals + ? &summary->initial_residuals + : NULL); + + // If the user requests gradient checking, construct a new + // ProblemImpl by wrapping the CostFunctions of problem_impl inside + // GradientCheckingCostFunction and replacing problem_impl with + // gradient_checking_problem_impl. + scoped_ptr<ProblemImpl> gradient_checking_problem_impl; + if (options.check_gradients) { + VLOG(1) << "Checking Gradients"; + gradient_checking_problem_impl.reset( + CreateGradientCheckingProblemImpl( + problem_impl, + options.numeric_derivative_relative_step_size, + options.gradient_check_relative_precision)); + + // From here on, problem_impl will point to the GradientChecking version. + problem_impl = gradient_checking_problem_impl.get(); + } + + // Create the three objects needed to minimize: the transformed program, the + // evaluator, and the linear solver. + + scoped_ptr<Program> reduced_program( + CreateReducedProgram(&options, problem_impl, &summary->error)); + if (reduced_program == NULL) { + return; + } + + summary->num_parameter_blocks_reduced = reduced_program->NumParameterBlocks(); + summary->num_parameters_reduced = reduced_program->NumParameters(); + summary->num_residual_blocks_reduced = reduced_program->NumResidualBlocks(); + summary->num_residuals_reduced = reduced_program->NumResiduals(); + + scoped_ptr<LinearSolver> + linear_solver(CreateLinearSolver(&options, &summary->error)); + summary->linear_solver_type_used = options.linear_solver_type; + summary->preconditioner_type = options.preconditioner_type; + summary->num_eliminate_blocks_used = options.num_eliminate_blocks; + summary->num_linear_solver_threads_used = options.num_linear_solver_threads; + + if (linear_solver == NULL) { + return; + } + + if (!MaybeReorderResidualBlocks(options, + reduced_program.get(), + &summary->error)) { + return; + } + + scoped_ptr<Evaluator> evaluator( + CreateEvaluator(options, reduced_program.get(), &summary->error)); + if (evaluator == NULL) { + return; + } + + // The optimizer works on contiguous parameter vectors; allocate some. + Vector initial_parameters(reduced_program->NumParameters()); + Vector optimized_parameters(reduced_program->NumParameters()); + + // Collect the discontiguous parameters into a contiguous state vector. + reduced_program->ParameterBlocksToStateVector(&initial_parameters[0]); + + // Run the optimization. + Minimize(options, + reduced_program.get(), + evaluator.get(), + linear_solver.get(), + initial_parameters.data(), + optimized_parameters.data(), + summary); + + // If the user aborted mid-optimization or the optimization + // terminated because of a numerical failure, then return without + // updating user state. + if (summary->termination_type == USER_ABORT || + summary->termination_type == NUMERICAL_FAILURE) { + return; + } + + // Push the contiguous optimized parameters back to the user's parameters. + reduced_program->StateVectorToParameterBlocks(&optimized_parameters[0]); + reduced_program->CopyParameterBlockStateToUserState(); + + // Return the final cost and residuals for the original problem. + EvaluateCostAndResiduals(problem->problem_impl_.get(), + &summary->final_cost, + options.return_final_residuals + ? &summary->final_residuals + : NULL); + + // Stick a fork in it, we're done. + return; +} + +// Strips varying parameters and residuals, maintaining order, and updating +// num_eliminate_blocks. +bool SolverImpl::RemoveFixedBlocksFromProgram(Program* program, + int* num_eliminate_blocks, + string* error) { + int original_num_eliminate_blocks = *num_eliminate_blocks; + vector<ParameterBlock*>* parameter_blocks = + program->mutable_parameter_blocks(); + + // Mark all the parameters as unused. Abuse the index member of the parameter + // blocks for the marking. + for (int i = 0; i < parameter_blocks->size(); ++i) { + (*parameter_blocks)[i]->set_index(-1); + } + + // Filter out residual that have all-constant parameters, and mark all the + // parameter blocks that appear in residuals. + { + vector<ResidualBlock*>* residual_blocks = + program->mutable_residual_blocks(); + int j = 0; + for (int i = 0; i < residual_blocks->size(); ++i) { + ResidualBlock* residual_block = (*residual_blocks)[i]; + int num_parameter_blocks = residual_block->NumParameterBlocks(); + + // Determine if the residual block is fixed, and also mark varying + // parameters that appear in the residual block. + bool all_constant = true; + for (int k = 0; k < num_parameter_blocks; k++) { + ParameterBlock* parameter_block = residual_block->parameter_blocks()[k]; + if (!parameter_block->IsConstant()) { + all_constant = false; + parameter_block->set_index(1); + } + } + + if (!all_constant) { + (*residual_blocks)[j++] = (*residual_blocks)[i]; + } + } + residual_blocks->resize(j); + } + + // Filter out unused or fixed parameter blocks, and update + // num_eliminate_blocks as necessary. + { + vector<ParameterBlock*>* parameter_blocks = + program->mutable_parameter_blocks(); + int j = 0; + for (int i = 0; i < parameter_blocks->size(); ++i) { + ParameterBlock* parameter_block = (*parameter_blocks)[i]; + if (parameter_block->index() == 1) { + (*parameter_blocks)[j++] = parameter_block; + } else if (i < original_num_eliminate_blocks) { + (*num_eliminate_blocks)--; + } + } + parameter_blocks->resize(j); + } + + CHECK(((program->NumResidualBlocks() == 0) && + (program->NumParameterBlocks() == 0)) || + ((program->NumResidualBlocks() != 0) && + (program->NumParameterBlocks() != 0))) + << "Congratulations, you found a bug in Ceres. Please report it."; + return true; +} + +Program* SolverImpl::CreateReducedProgram(Solver::Options* options, + ProblemImpl* problem_impl, + string* error) { + Program* original_program = problem_impl->mutable_program(); + scoped_ptr<Program> transformed_program(new Program(*original_program)); + + if (options->ordering_type == USER && + !ApplyUserOrdering(*problem_impl, + options->ordering, + transformed_program.get(), + error)) { + return NULL; + } + + if (options->ordering_type == SCHUR && options->num_eliminate_blocks != 0) { + *error = "Can't specify SCHUR ordering and num_eliminate_blocks " + "at the same time; SCHUR ordering determines " + "num_eliminate_blocks automatically."; + return NULL; + } + + if (options->ordering_type == SCHUR && options->ordering.size() != 0) { + *error = "Can't specify SCHUR ordering type and the ordering " + "vector at the same time; SCHUR ordering determines " + "a suitable parameter ordering automatically."; + return NULL; + } + + int num_eliminate_blocks = options->num_eliminate_blocks; + + if (!RemoveFixedBlocksFromProgram(transformed_program.get(), + &num_eliminate_blocks, + error)) { + return NULL; + } + + if (transformed_program->NumParameterBlocks() == 0) { + LOG(WARNING) << "No varying parameter blocks to optimize; " + << "bailing early."; + return transformed_program.release(); + } + + if (options->ordering_type == SCHUR) { + vector<ParameterBlock*> schur_ordering; + num_eliminate_blocks = ComputeSchurOrdering(*transformed_program, + &schur_ordering); + CHECK_EQ(schur_ordering.size(), transformed_program->NumParameterBlocks()) + << "Congratulations, you found a Ceres bug! Please report this error " + << "to the developers."; + + // Replace the transformed program's ordering with the schur ordering. + swap(*transformed_program->mutable_parameter_blocks(), schur_ordering); + } + options->num_eliminate_blocks = num_eliminate_blocks; + CHECK_GE(options->num_eliminate_blocks, 0) + << "Congratulations, you found a Ceres bug! Please report this error " + << "to the developers."; + + // Since the transformed program is the "active" program, and it is mutated, + // update the parameter offsets and indices. + transformed_program->SetParameterOffsetsAndIndex(); + return transformed_program.release(); +} + +LinearSolver* SolverImpl::CreateLinearSolver(Solver::Options* options, + string* error) { +#ifdef CERES_NO_SUITESPARSE + if (options->linear_solver_type == SPARSE_NORMAL_CHOLESKY) { + *error = "Can't use SPARSE_NORMAL_CHOLESKY because SuiteSparse was not " + "enabled when Ceres was built."; + return NULL; + } +#endif // CERES_NO_SUITESPARSE + + if (options->linear_solver_max_num_iterations <= 0) { + *error = "Solver::Options::linear_solver_max_num_iterations is 0."; + return NULL; + } + if (options->linear_solver_min_num_iterations <= 0) { + *error = "Solver::Options::linear_solver_min_num_iterations is 0."; + return NULL; + } + if (options->linear_solver_min_num_iterations > + options->linear_solver_max_num_iterations) { + *error = "Solver::Options::linear_solver_min_num_iterations > " + "Solver::Options::linear_solver_max_num_iterations."; + return NULL; + } + + LinearSolver::Options linear_solver_options; + linear_solver_options.constant_sparsity = true; + linear_solver_options.min_num_iterations = + options->linear_solver_min_num_iterations; + linear_solver_options.max_num_iterations = + options->linear_solver_max_num_iterations; + linear_solver_options.type = options->linear_solver_type; + linear_solver_options.preconditioner_type = options->preconditioner_type; + +#ifdef CERES_NO_SUITESPARSE + if (linear_solver_options.preconditioner_type == SCHUR_JACOBI) { + *error = "SCHUR_JACOBI preconditioner not suppored. Please build Ceres " + "with SuiteSparse support"; + return NULL; + } + + if (linear_solver_options.preconditioner_type == CLUSTER_JACOBI) { + *error = "CLUSTER_JACOBI preconditioner not suppored. Please build Ceres " + "with SuiteSparse support"; + return NULL; + } + + if (linear_solver_options.preconditioner_type == CLUSTER_TRIDIAGONAL) { + *error = "CLUSTER_TRIDIAGONAL preconditioner not suppored. Please build " + "Ceres with SuiteSparse support"; + return NULL; + } +#endif + + linear_solver_options.num_threads = options->num_linear_solver_threads; + linear_solver_options.num_eliminate_blocks = + options->num_eliminate_blocks; + + if ((linear_solver_options.num_eliminate_blocks == 0) && + IsSchurType(linear_solver_options.type)) { +#ifndef CERES_NO_SUITESPARSE + LOG(INFO) << "No elimination block remaining " + << "switching to SPARSE_NORMAL_CHOLESKY."; + linear_solver_options.type = SPARSE_NORMAL_CHOLESKY; +#else + LOG(INFO) << "No elimination block remaining switching to DENSE_QR."; + linear_solver_options.type = DENSE_QR; +#endif // CERES_NO_SUITESPARSE + } + +#ifdef CERES_NO_SUITESPARSE + if (linear_solver_options.type == SPARSE_SCHUR) { + *error = "Can't use SPARSE_SCHUR because SuiteSparse was not " + "enabled when Ceres was built."; + return NULL; + } +#endif // CERES_NO_SUITESPARSE + + // The matrix used for storing the dense Schur complement has a + // single lock guarding the whole matrix. Running the + // SchurComplementSolver with multiple threads leads to maximum + // contention and slowdown. If the problem is large enough to + // benefit from a multithreaded schur eliminator, you should be + // using a SPARSE_SCHUR solver anyways. + if ((linear_solver_options.num_threads > 1) && + (linear_solver_options.type == DENSE_SCHUR)) { + LOG(WARNING) << "Warning: Solver::Options::num_linear_solver_threads = " + << options->num_linear_solver_threads + << " with DENSE_SCHUR will result in poor performance; " + << "switching to single-threaded."; + linear_solver_options.num_threads = 1; + } + + options->linear_solver_type = linear_solver_options.type; + options->num_linear_solver_threads = linear_solver_options.num_threads; + + return LinearSolver::Create(linear_solver_options); +} + +bool SolverImpl::ApplyUserOrdering(const ProblemImpl& problem_impl, + vector<double*>& ordering, + Program* program, + string* error) { + if (ordering.size() != program->NumParameterBlocks()) { + *error = StringPrintf("User specified ordering does not have the same " + "number of parameters as the problem. The problem" + "has %d blocks while the ordering has %ld blocks.", + program->NumParameterBlocks(), + ordering.size()); + return false; + } + + // Ensure that there are no duplicates in the user's ordering. + { + vector<double*> ordering_copy(ordering); + sort(ordering_copy.begin(), ordering_copy.end()); + if (unique(ordering_copy.begin(), ordering_copy.end()) + != ordering_copy.end()) { + *error = "User specified ordering contains duplicates."; + return false; + } + } + + vector<ParameterBlock*>* parameter_blocks = + program->mutable_parameter_blocks(); + + fill(parameter_blocks->begin(), + parameter_blocks->end(), + static_cast<ParameterBlock*>(NULL)); + + const ProblemImpl::ParameterMap& parameter_map = problem_impl.parameter_map(); + for (int i = 0; i < ordering.size(); ++i) { + ProblemImpl::ParameterMap::const_iterator it = + parameter_map.find(ordering[i]); + if (it == parameter_map.end()) { + *error = StringPrintf("User specified ordering contains a pointer " + "to a double that is not a parameter block in the " + "problem. The invalid double is at position %d " + " in options.ordering.", i); + return false; + } + (*parameter_blocks)[i] = it->second; + } + return true; +} + +// Find the minimum index of any parameter block to the given residual. +// Parameter blocks that have indices greater than num_eliminate_blocks are +// considered to have an index equal to num_eliminate_blocks. +int MinParameterBlock(const ResidualBlock* residual_block, + int num_eliminate_blocks) { + int min_parameter_block_position = num_eliminate_blocks; + for (int i = 0; i < residual_block->NumParameterBlocks(); ++i) { + ParameterBlock* parameter_block = residual_block->parameter_blocks()[i]; + DCHECK_NE(parameter_block->index(), -1) + << "Did you forget to call Program::SetParameterOffsetsAndIndex()?"; + min_parameter_block_position = std::min(parameter_block->index(), + min_parameter_block_position); + } + return min_parameter_block_position; +} + +// Reorder the residuals for program, if necessary, so that the residuals +// involving each E block occur together. This is a necessary condition for the +// Schur eliminator, which works on these "row blocks" in the jacobian. +bool SolverImpl::MaybeReorderResidualBlocks(const Solver::Options& options, + Program* program, + string* error) { + // Only Schur types require the lexicographic reordering. + if (!IsSchurType(options.linear_solver_type)) { + return true; + } + + CHECK_NE(0, options.num_eliminate_blocks) + << "Congratulations, you found a Ceres bug! Please report this error " + << "to the developers."; + + // Create a histogram of the number of residuals for each E block. There is an + // extra bucket at the end to catch all non-eliminated F blocks. + vector<int> residual_blocks_per_e_block(options.num_eliminate_blocks + 1); + vector<ResidualBlock*>* residual_blocks = program->mutable_residual_blocks(); + vector<int> min_position_per_residual(residual_blocks->size()); + for (int i = 0; i < residual_blocks->size(); ++i) { + ResidualBlock* residual_block = (*residual_blocks)[i]; + int position = MinParameterBlock(residual_block, + options.num_eliminate_blocks); + min_position_per_residual[i] = position; + DCHECK_LE(position, options.num_eliminate_blocks); + residual_blocks_per_e_block[position]++; + } + + // Run a cumulative sum on the histogram, to obtain offsets to the start of + // each histogram bucket (where each bucket is for the residuals for that + // E-block). + vector<int> offsets(options.num_eliminate_blocks + 1); + std::partial_sum(residual_blocks_per_e_block.begin(), + residual_blocks_per_e_block.end(), + offsets.begin()); + CHECK_EQ(offsets.back(), residual_blocks->size()) + << "Congratulations, you found a Ceres bug! Please report this error " + << "to the developers."; + + CHECK(find(residual_blocks_per_e_block.begin(), + residual_blocks_per_e_block.end() - 1, 0) != + residual_blocks_per_e_block.end()) + << "Congratulations, you found a Ceres bug! Please report this error " + << "to the developers."; + + // Fill in each bucket with the residual blocks for its corresponding E block. + // Each bucket is individually filled from the back of the bucket to the front + // of the bucket. The filling order among the buckets is dictated by the + // residual blocks. This loop uses the offsets as counters; subtracting one + // from each offset as a residual block is placed in the bucket. When the + // filling is finished, the offset pointerts should have shifted down one + // entry (this is verified below). + vector<ResidualBlock*> reordered_residual_blocks( + (*residual_blocks).size(), static_cast<ResidualBlock*>(NULL)); + for (int i = 0; i < residual_blocks->size(); ++i) { + int bucket = min_position_per_residual[i]; + + // Decrement the cursor, which should now point at the next empty position. + offsets[bucket]--; + + // Sanity. + CHECK(reordered_residual_blocks[offsets[bucket]] == NULL) + << "Congratulations, you found a Ceres bug! Please report this error " + << "to the developers."; + + reordered_residual_blocks[offsets[bucket]] = (*residual_blocks)[i]; + } + + // Sanity check #1: The difference in bucket offsets should match the + // histogram sizes. + for (int i = 0; i < options.num_eliminate_blocks; ++i) { + CHECK_EQ(residual_blocks_per_e_block[i], offsets[i + 1] - offsets[i]) + << "Congratulations, you found a Ceres bug! Please report this error " + << "to the developers."; + } + // Sanity check #2: No NULL's left behind. + for (int i = 0; i < reordered_residual_blocks.size(); ++i) { + CHECK(reordered_residual_blocks[i] != NULL) + << "Congratulations, you found a Ceres bug! Please report this error " + << "to the developers."; + } + + // Now that the residuals are collected by E block, swap them in place. + swap(*program->mutable_residual_blocks(), reordered_residual_blocks); + return true; +} + +Evaluator* SolverImpl::CreateEvaluator(const Solver::Options& options, + Program* program, + string* error) { + Evaluator::Options evaluator_options; + evaluator_options.linear_solver_type = options.linear_solver_type; + evaluator_options.num_eliminate_blocks = options.num_eliminate_blocks; + evaluator_options.num_threads = options.num_threads; + return Evaluator::Create(evaluator_options, program, error); +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/solver_impl.h b/extern/libmv/third_party/ceres/internal/ceres/solver_impl.h new file mode 100644 index 00000000000..957ebcc65df --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/solver_impl.h @@ -0,0 +1,111 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 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: keir@google.com (Keir Mierle) + +#ifndef CERES_INTERNAL_SOLVER_IMPL_H_ +#define CERES_INTERNAL_SOLVER_IMPL_H_ + +#include "ceres/solver.h" + +namespace ceres { +namespace internal { + +class Evaluator; +class LinearSolver; +class ProblemImpl; +class Program; + +class SolverImpl { + public: + // Mirrors the interface in solver.h, but exposes implementation + // details for testing internally. + static void Solve(const Solver::Options& options, + Problem* problem, + Solver::Summary* summary); + + // Create the transformed Program, which has all the fixed blocks + // and residuals eliminated, and in the case of automatic schur + // ordering, has the E blocks first in the resulting program, with + // options.num_eliminate_blocks set appropriately. + static Program* CreateReducedProgram(Solver::Options* options, + ProblemImpl* problem_impl, + string* error); + + // Create the appropriate linear solver, taking into account any + // config changes decided by CreateTransformedProgram(). The + // selected linear solver, which may be different from what the user + // selected; consider the case that the remaining elimininated + // blocks is zero after removing fixed blocks. + static LinearSolver* CreateLinearSolver(Solver::Options* options, + string* error); + + // Reorder the parameter blocks in program using the vector + // ordering. A return value of true indicates success and false + // indicates an error was encountered whose cause is logged to + // LOG(ERROR). + static bool ApplyUserOrdering(const ProblemImpl& problem_impl, + vector<double*>& ordering, + Program* program, + string* error); + + // Reorder the residuals for program, if necessary, so that the + // residuals involving each E block occur together. This is a + // necessary condition for the Schur eliminator, which works on + // these "row blocks" in the jacobian. + static bool MaybeReorderResidualBlocks(const Solver::Options& options, + Program* program, + string* error); + + // Create the appropriate evaluator for the transformed program. + static Evaluator* CreateEvaluator(const Solver::Options& options, + Program* program, + string* error); + + // Run the minimization for the given evaluator and configuration. + static void Minimize(const Solver::Options &options, + Program* program, + Evaluator* evaluator, + LinearSolver* linear_solver, + double* initial_parameters, + double* final_parameters, + Solver::Summary* summary); + + // Remove the fixed or unused parameter blocks and residuals + // depending only on fixed parameters from the problem. Also updates + // num_eliminate_blocks, since removed parameters changes the point + // at which the eliminated blocks is valid. + static bool RemoveFixedBlocksFromProgram(Program* program, + int* num_eliminate_blocks, + string* error); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_SOLVER_IMPL_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/sparse_matrix.cc b/extern/libmv/third_party/ceres/internal/ceres/sparse_matrix.cc new file mode 100644 index 00000000000..55336fd3130 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/sparse_matrix.cc @@ -0,0 +1,40 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/sparse_matrix.h" + +namespace ceres { +namespace internal { + +SparseMatrix::~SparseMatrix() { +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/sparse_matrix.h b/extern/libmv/third_party/ceres/internal/ceres/sparse_matrix.h new file mode 100644 index 00000000000..562210dfec8 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/sparse_matrix.h @@ -0,0 +1,114 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Interface definition for sparse matrices. + +#ifndef CERES_INTERNAL_SPARSE_MATRIX_H_ +#define CERES_INTERNAL_SPARSE_MATRIX_H_ + +#include <cstdio> +#include "ceres/linear_operator.h" +#include "ceres/internal/eigen.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +class SparseMatrixProto; + +// This class defines the interface for storing and manipulating +// sparse matrices. The key property that differentiates different +// sparse matrices is how they are organized in memory and how the +// information about the sparsity structure of the matrix is +// stored. This has significant implications for linear solvers +// operating on these matrices. +// +// To deal with the different kinds of layouts, we will assume that a +// sparse matrix will have a two part representation. A values array +// that will be used to store the entries of the sparse matrix and +// some sort of a layout object that tells the user the sparsity +// structure and layout of the values array. For example in case of +// the TripletSparseMatrix, this information is carried in the rows +// and cols arrays and for the BlockSparseMatrix, this information is +// carried in the CompressedRowBlockStructure object. +// +// This interface deliberately does not contain any information about +// the structure of the sparse matrix as that seems to be highly +// matrix type dependent and we are at this stage unable to come up +// with an efficient high level interface that spans multiple sparse +// matrix types. +class SparseMatrix : public LinearOperator { + public: + virtual ~SparseMatrix(); + + // y += Ax; + virtual void RightMultiply(const double* x, double* y) const = 0; + // y += A'x; + virtual void LeftMultiply(const double* x, double* y) const = 0; + + // In MATLAB notation sum(A.*A, 1) + virtual void SquaredColumnNorm(double* x) const = 0; + // A = A * diag(scale) + virtual void ScaleColumns(const double* scale) = 0; + + // A = 0. A->num_nonzeros() == 0 is true after this call. The + // sparsity pattern is preserved. + virtual void SetZero() = 0; + + // Resize and populate dense_matrix with a dense version of the + // sparse matrix. + virtual void ToDenseMatrix(Matrix* dense_matrix) const = 0; + +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS + // Dump the sparse matrix to a proto. Destroys the contents of proto. + virtual void ToProto(SparseMatrixProto* proto) const = 0; +#endif + + // Write out the matrix as a sequence of (i,j,s) triplets. This + // format is useful for loading the matrix into MATLAB/octave as a + // sparse matrix. + virtual void ToTextFile(FILE* file) const = 0; + + // Accessors for the values array that stores the entries of the + // sparse matrix. The exact interpreptation of the values of this + // array depends on the particular kind of SparseMatrix being + // accessed. + virtual double* mutable_values() = 0; + virtual const double* values() const = 0; + + virtual int num_rows() const = 0; + virtual int num_cols() const = 0; + virtual int num_nonzeros() const = 0; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_SPARSE_MATRIX_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/sparse_normal_cholesky_solver.cc b/extern/libmv/third_party/ceres/internal/ceres/sparse_normal_cholesky_solver.cc new file mode 100644 index 00000000000..59222dc374d --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/sparse_normal_cholesky_solver.cc @@ -0,0 +1,129 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_NO_SUITESPARSE + +#include "ceres/sparse_normal_cholesky_solver.h" + +#include <algorithm> +#include <cstring> +#include <ctime> +#include "ceres/compressed_row_sparse_matrix.h" +#include "ceres/linear_solver.h" +#include "ceres/suitesparse.h" +#include "ceres/triplet_sparse_matrix.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +SparseNormalCholeskySolver::SparseNormalCholeskySolver( + const LinearSolver::Options& options) + : options_(options), symbolic_factor_(NULL) {} + +SparseNormalCholeskySolver::~SparseNormalCholeskySolver() { + if (symbolic_factor_ != NULL) { + ss_.Free(symbolic_factor_); + symbolic_factor_ = NULL; + } +} + +LinearSolver::Summary SparseNormalCholeskySolver::SolveImpl( + CompressedRowSparseMatrix* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double * x) { + const time_t start_time = time(NULL); + const int num_cols = A->num_cols(); + + LinearSolver::Summary summary; + Vector Atb = Vector::Zero(num_cols); + A->LeftMultiply(b, Atb.data()); + + 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. + CompressedRowSparseMatrix D(per_solve_options.D, num_cols); + A->AppendRows(D); + } + + VectorRef(x, num_cols).setZero(); + + scoped_ptr<cholmod_sparse> lhs(ss_.CreateSparseMatrixTransposeView(A)); + CHECK_NOTNULL(lhs.get()); + + cholmod_dense* rhs = ss_.CreateDenseVector(Atb.data(), num_cols, num_cols); + const time_t init_time = time(NULL); + + if (symbolic_factor_ == NULL) { + symbolic_factor_ = CHECK_NOTNULL(ss_.AnalyzeCholesky(lhs.get())); + } + + const time_t symbolic_time = time(NULL); + + cholmod_dense* sol = ss_.SolveCholesky(lhs.get(), symbolic_factor_, rhs); + const time_t solve_time = time(NULL); + + ss_.Free(rhs); + rhs = NULL; + + if (per_solve_options.D != NULL) { + A->DeleteRows(num_cols); + } + + if (!options_.constant_sparsity) { + ss_.Free(symbolic_factor_); + symbolic_factor_ = NULL; + } + + summary.num_iterations = 1; + if (sol != NULL) { + memcpy(x, sol->x, num_cols * sizeof(*x)); + + ss_.Free(sol); + sol = NULL; + summary.termination_type = TOLERANCE; + } + + const time_t cleanup_time = time(NULL); + VLOG(2) << "time (sec) total: " << cleanup_time - start_time + << " init: " << init_time - start_time + << " symbolic: " << symbolic_time - init_time + << " solve: " << solve_time - symbolic_time + << " cleanup: " << cleanup_time - solve_time; + return summary; +} + +} // namespace internal +} // namespace ceres + +#endif // CERES_NO_SUITESPARSE diff --git a/extern/libmv/third_party/ceres/internal/ceres/sparse_normal_cholesky_solver.h b/extern/libmv/third_party/ceres/internal/ceres/sparse_normal_cholesky_solver.h new file mode 100644 index 00000000000..ce1d6d285be --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/sparse_normal_cholesky_solver.h @@ -0,0 +1,77 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// A solver for sparse linear least squares problem based on solving +// the normal equations via a sparse cholesky factorization. + +#ifndef CERES_INTERNAL_SPARSE_NORMAL_CHOLESKY_SOLVER_H_ +#define CERES_INTERNAL_SPARSE_NORMAL_CHOLESKY_SOLVER_H_ + +#ifndef CERES_NO_SUITESPARSE + +#include "cholmod.h" +#include "cholmod_core.h" +#include "ceres/linear_solver.h" +#include "ceres/suitesparse.h" +#include "ceres/internal/macros.h" + +namespace ceres { +namespace internal { + +class CompressedRowSparseMatrix; + +// Solves the normal equations (A'A + D'D) x = A'b, using the CHOLMOD sparse +// cholesky solver. +class SparseNormalCholeskySolver : public CompressedRowSparseMatrixSolver { + public: + explicit SparseNormalCholeskySolver(const LinearSolver::Options& options); + virtual ~SparseNormalCholeskySolver(); + + private: + virtual LinearSolver::Summary SolveImpl( + CompressedRowSparseMatrix* A, + const double* b, + const LinearSolver::PerSolveOptions& options, + double* x); + + const LinearSolver::Options options_; + SuiteSparse ss_; + + // Cached factorization + cholmod_factor* symbolic_factor_; + DISALLOW_COPY_AND_ASSIGN(SparseNormalCholeskySolver); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_NO_SUITESPARSE + +#endif // CERES_INTERNAL_SPARSE_NORMAL_CHOLESKY_SOLVER_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/split.cc b/extern/libmv/third_party/ceres/internal/ceres/split.cc new file mode 100644 index 00000000000..4fa1bd468b9 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/split.cc @@ -0,0 +1,115 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 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: keir@google.com (Keir Mierle) + +#include <string> +#include <vector> +#include <iterator> +#include "ceres/internal/port.h" + +namespace ceres { + +// If we know how much to allocate for a vector of strings, we can allocate the +// vector<string> only once and directly to the right size. This saves in +// between 33-66 % of memory space needed for the result, and runs faster in the +// microbenchmarks. +// +// The reserve is only implemented for the single character delim. +// +// The implementation for counting is cut-and-pasted from +// SplitStringToIteratorUsing. I could have written my own counting iterator, +// and use the existing template function, but probably this is more clear and +// more sure to get optimized to reasonable code. +static int CalculateReserveForVector(const string& full, const char* delim) { + int count = 0; + if (delim[0] != '\0' && delim[1] == '\0') { + // Optimize the common case where delim is a single character. + char c = delim[0]; + const char* p = full.data(); + const char* end = p + full.size(); + while (p != end) { + if (*p == c) { // This could be optimized with hasless(v,1) trick. + ++p; + } else { + while (++p != end && *p != c) { + // Skip to the next occurence of the delimiter. + } + ++count; + } + } + } + return count; +} + +template <typename StringType, typename ITR> +static inline +void SplitStringToIteratorUsing(const StringType& full, + const char* delim, + ITR& result) { + // Optimize the common case where delim is a single character. + if (delim[0] != '\0' && delim[1] == '\0') { + char c = delim[0]; + const char* p = full.data(); + const char* end = p + full.size(); + while (p != end) { + if (*p == c) { + ++p; + } else { + const char* start = p; + while (++p != end && *p != c) { + // Skip to the next occurence of the delimiter. + } + *result++ = StringType(start, p - start); + } + } + return; + } + + string::size_type begin_index, end_index; + begin_index = full.find_first_not_of(delim); + while (begin_index != string::npos) { + end_index = full.find_first_of(delim, begin_index); + if (end_index == string::npos) { + *result++ = full.substr(begin_index); + return; + } + *result++ = full.substr(begin_index, (end_index - begin_index)); + begin_index = full.find_first_not_of(delim, end_index); + } +} + +void SplitStringUsing(const string& full, + const char* delim, + vector<string>* result) { + result->reserve(result->size() + CalculateReserveForVector(full, delim)); + back_insert_iterator< vector<string> > it(*result); + SplitStringToIteratorUsing(full, delim, it); +} + +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/stl_util.h b/extern/libmv/third_party/ceres/internal/ceres/stl_util.h new file mode 100644 index 00000000000..a1a19e8b3ce --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/stl_util.h @@ -0,0 +1,75 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 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: keir@google.com (Keir Mierle) + +#ifndef CERES_INTERNAL_STL_UTIL_H_ +#define CERES_INTERNAL_STL_UTIL_H_ + +namespace ceres { + +// STLDeleteContainerPointers() +// For a range within a container of pointers, calls delete +// (non-array version) on these pointers. +// NOTE: for these three functions, we could just implement a DeleteObject +// functor and then call for_each() on the range and functor, but this +// requires us to pull in all of algorithm.h, which seems expensive. +// For hash_[multi]set, it is important that this deletes behind the iterator +// because the hash_set may call the hash function on the iterator when it is +// advanced, which could result in the hash function trying to deference a +// stale pointer. +template <class ForwardIterator> +void STLDeleteContainerPointers(ForwardIterator begin, + ForwardIterator end) { + while (begin != end) { + ForwardIterator temp = begin; + ++begin; + delete *temp; + } +} + +// STLDeleteElements() deletes all the elements in an STL container and clears +// the container. This function is suitable for use with a vector, set, +// hash_set, or any other STL container which defines sensible begin(), end(), +// and clear() methods. +// +// If container is NULL, this function is a no-op. +// +// As an alternative to calling STLDeleteElements() directly, consider +// ElementDeleter (defined below), which ensures that your container's elements +// are deleted when the ElementDeleter goes out of scope. +template <class T> +void STLDeleteElements(T *container) { + if (!container) return; + STLDeleteContainerPointers(container->begin(), container->end()); + container->clear(); +} + +} // namespace ceres + +#endif // CERES_INTERNAL_STL_UTIL_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/stringprintf.cc b/extern/libmv/third_party/ceres/internal/ceres/stringprintf.cc new file mode 100644 index 00000000000..c0f35225bc3 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/stringprintf.cc @@ -0,0 +1,126 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 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: Sanjay Ghemawat + +#include <cerrno> +#include <cstdarg> // For va_list and related operations +#include <cstdio> // MSVC requires this for _vsnprintf +#include <string> +#include <vector> + +#include "ceres/internal/port.h" + +namespace ceres { +namespace internal { + +#ifdef _MSC_VER +enum { IS_COMPILER_MSVC = 1 }; +#define va_copy(d,s) ((d) = (s)) +#else +enum { IS_COMPILER_MSVC = 0 }; +#endif + +void StringAppendV(string* dst, const char* format, va_list ap) { + // First try with a small fixed size buffer + char space[1024]; + + // It's possible for methods that use a va_list to invalidate + // the data in it upon use. The fix is to make a copy + // of the structure before using it and use that copy instead. + va_list backup_ap; + va_copy(backup_ap, ap); + int result = vsnprintf(space, sizeof(space), format, backup_ap); + va_end(backup_ap); + + if (result < sizeof(space)) { + if (result >= 0) { + // Normal case -- everything fit. + dst->append(space, result); + return; + } + + if (IS_COMPILER_MSVC) { + // Error or MSVC running out of space. MSVC 8.0 and higher + // can be asked about space needed with the special idiom below: + va_copy(backup_ap, ap); + result = vsnprintf(NULL, 0, format, backup_ap); + va_end(backup_ap); + } + + if (result < 0) { + // Just an error. + return; + } + } + + // Increase the buffer size to the size requested by vsnprintf, + // plus one for the closing \0. + int length = result+1; + char* buf = new char[length]; + + // Restore the va_list before we use it again + va_copy(backup_ap, ap); + result = vsnprintf(buf, length, format, backup_ap); + va_end(backup_ap); + + if (result >= 0 && result < length) { + // It fit + dst->append(buf, result); + } + delete[] buf; +} + + +string StringPrintf(const char* format, ...) { + va_list ap; + va_start(ap, format); + string result; + StringAppendV(&result, format, ap); + va_end(ap); + return result; +} + +const string& SStringPrintf(string* dst, const char* format, ...) { + va_list ap; + va_start(ap, format); + dst->clear(); + StringAppendV(dst, format, ap); + va_end(ap); + return *dst; +} + +void StringAppendF(string* dst, const char* format, ...) { + va_list ap; + va_start(ap, format); + StringAppendV(dst, format, ap); + va_end(ap); +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/stringprintf.h b/extern/libmv/third_party/ceres/internal/ceres/stringprintf.h new file mode 100644 index 00000000000..30b974e7ae5 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/stringprintf.h @@ -0,0 +1,89 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 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: Sanjay Ghemawat +// +// Printf variants that place their output in a C++ string. +// +// Usage: +// string result = StringPrintf("%d %s\n", 10, "hello"); +// SStringPrintf(&result, "%d %s\n", 10, "hello"); +// StringAppendF(&result, "%d %s\n", 20, "there"); + +#ifndef CERES_INTERNAL_STRINGPRINTF_H_ +#define CERES_INTERNAL_STRINGPRINTF_H_ + +#include <cstdarg> +#include <string> + +#include "ceres/internal/port.h" + +namespace ceres { +namespace internal { + +#if (defined(__GNUC__) || defined(__clang__)) +// Tell the compiler to do printf format string checking if the compiler +// supports it; see the 'format' attribute in +// <http://gcc.gnu.org/onlinedocs/gcc-4.3.0/gcc/Function-Attributes.html>. +// +// N.B.: As the GCC manual states, "[s]ince non-static C++ methods +// have an implicit 'this' argument, the arguments of such methods +// should be counted from two, not one." +#define PRINTF_ATTRIBUTE(string_index, first_to_check) \ + __attribute__((__format__ (__printf__, string_index, first_to_check))) +#define SCANF_ATTRIBUTE(string_index, first_to_check) \ + __attribute__((__format__ (__scanf__, string_index, first_to_check))) +#else +#define PRINTF_ATTRIBUTE(string_index, first_to_check) +#endif + +// Return a C++ string. +extern string StringPrintf(const char* format, ...) + // Tell the compiler to do printf format string checking. + PRINTF_ATTRIBUTE(1,2); + +// Store result into a supplied string and return it. +extern const string& SStringPrintf(string* dst, const char* format, ...) + // Tell the compiler to do printf format string checking. + PRINTF_ATTRIBUTE(2,3); + +// Append result to a supplied string. +extern void StringAppendF(string* dst, const char* format, ...) + // Tell the compiler to do printf format string checking. + PRINTF_ATTRIBUTE(2,3); + +// Lower-level routine that takes a va_list and appends to a specified string. +// All other routines are just convenience wrappers around it. +extern void StringAppendV(string* dst, const char* format, va_list ap); + +#undef PRINTF_ATTRIBUTE + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_STRINGPRINTF_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/suitesparse.cc b/extern/libmv/third_party/ceres/internal/ceres/suitesparse.cc new file mode 100644 index 00000000000..1cf6a7496a7 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/suitesparse.cc @@ -0,0 +1,193 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_NO_SUITESPARSE + +#include "ceres/suitesparse.h" + +#include "cholmod.h" +#include "ceres/compressed_row_sparse_matrix.h" +#include "ceres/triplet_sparse_matrix.h" +namespace ceres { +namespace internal { + +cholmod_sparse* SuiteSparse::CreateSparseMatrix(TripletSparseMatrix* A) { + cholmod_triplet triplet; + + triplet.nrow = A->num_rows(); + triplet.ncol = A->num_cols(); + triplet.nzmax = A->max_num_nonzeros(); + triplet.nnz = A->num_nonzeros(); + triplet.i = reinterpret_cast<void*>(A->mutable_rows()); + triplet.j = reinterpret_cast<void*>(A->mutable_cols()); + triplet.x = reinterpret_cast<void*>(A->mutable_values()); + triplet.stype = 0; // Matrix is not symmetric. + triplet.itype = CHOLMOD_INT; + triplet.xtype = CHOLMOD_REAL; + triplet.dtype = CHOLMOD_DOUBLE; + + return cholmod_triplet_to_sparse(&triplet, triplet.nnz, &cc_); +} + + +cholmod_sparse* SuiteSparse::CreateSparseMatrixTranspose( + TripletSparseMatrix* A) { + cholmod_triplet triplet; + + triplet.ncol = A->num_rows(); // swap row and columns + triplet.nrow = A->num_cols(); + triplet.nzmax = A->max_num_nonzeros(); + triplet.nnz = A->num_nonzeros(); + + // swap rows and columns + triplet.j = reinterpret_cast<void*>(A->mutable_rows()); + triplet.i = reinterpret_cast<void*>(A->mutable_cols()); + triplet.x = reinterpret_cast<void*>(A->mutable_values()); + triplet.stype = 0; // Matrix is not symmetric. + triplet.itype = CHOLMOD_INT; + triplet.xtype = CHOLMOD_REAL; + triplet.dtype = CHOLMOD_DOUBLE; + + return cholmod_triplet_to_sparse(&triplet, triplet.nnz, &cc_); +} + +cholmod_sparse* SuiteSparse::CreateSparseMatrixTransposeView( + CompressedRowSparseMatrix* A) { + cholmod_sparse* m = new cholmod_sparse_struct; + m->nrow = A->num_cols(); + m->ncol = A->num_rows(); + m->nzmax = A->num_nonzeros(); + + m->p = reinterpret_cast<void*>(A->mutable_rows()); + m->i = reinterpret_cast<void*>(A->mutable_cols()); + m->x = reinterpret_cast<void*>(A->mutable_values()); + + m->stype = 0; // Matrix is not symmetric. + m->itype = CHOLMOD_INT; + m->xtype = CHOLMOD_REAL; + m->dtype = CHOLMOD_DOUBLE; + m->sorted = 1; + m->packed = 1; + + return m; +} + +cholmod_dense* SuiteSparse::CreateDenseVector(const double* x, + int in_size, + int out_size) { + CHECK_LE(in_size, out_size); + cholmod_dense* v = cholmod_zeros(out_size, 1, CHOLMOD_REAL, &cc_); + if (x != NULL) { + memcpy(v->x, x, in_size*sizeof(*x)); + } + return v; +} + +cholmod_factor* SuiteSparse::AnalyzeCholesky(cholmod_sparse* A) { + cholmod_factor* factor = cholmod_analyze(A, &cc_); + CHECK_EQ(cc_.status, CHOLMOD_OK) + << "Cholmod symbolic analysis failed " << cc_.status; + CHECK_NOTNULL(factor); + return factor; +} + +bool SuiteSparse::Cholesky(cholmod_sparse* A, cholmod_factor* L) { + CHECK_NOTNULL(A); + CHECK_NOTNULL(L); + + cc_.quick_return_if_not_posdef = 1; + int status = cholmod_factorize(A, L, &cc_); + switch (cc_.status) { + case CHOLMOD_NOT_INSTALLED: + LOG(WARNING) << "Cholmod failure: method not installed."; + return false; + case CHOLMOD_OUT_OF_MEMORY: + LOG(WARNING) << "Cholmod failure: out of memory."; + return false; + case CHOLMOD_TOO_LARGE: + LOG(WARNING) << "Cholmod failure: integer overflow occured."; + return false; + case CHOLMOD_INVALID: + LOG(WARNING) << "Cholmod failure: invalid input."; + return false; + case CHOLMOD_NOT_POSDEF: + // TODO(sameeragarwal): These two warnings require more + // sophisticated handling going forward. For now we will be + // strict and treat them as failures. + LOG(WARNING) << "Cholmod warning: matrix not positive definite."; + return false; + case CHOLMOD_DSMALL: + LOG(WARNING) << "Cholmod warning: D for LDL' or diag(L) or " + << "LL' has tiny absolute value."; + return false; + case CHOLMOD_OK: + if (status != 0) { + return true; + } + LOG(WARNING) << "Cholmod failure: cholmod_factorize returned zero " + << "but cholmod_common::status is CHOLMOD_OK." + << "Please report this to ceres-solver@googlegroups.com."; + return false; + default: + LOG(WARNING) << "Unknown cholmod return code. " + << "Please report this to ceres-solver@googlegroups.com."; + return false; + } + return false; +} + +cholmod_dense* SuiteSparse::Solve(cholmod_factor* L, + cholmod_dense* b) { + if (cc_.status != CHOLMOD_OK) { + LOG(WARNING) << "CHOLMOD status NOT OK"; + return NULL; + } + + return cholmod_solve(CHOLMOD_A, L, b, &cc_); +} + +cholmod_dense* SuiteSparse::SolveCholesky(cholmod_sparse* A, + cholmod_factor* L, + cholmod_dense* b) { + CHECK_NOTNULL(A); + CHECK_NOTNULL(L); + CHECK_NOTNULL(b); + + if (Cholesky(A, L)) { + return Solve(L, b); + } + + return NULL; +} + +} // namespace internal +} // namespace ceres + +#endif // CERES_NO_SUITESPARSE diff --git a/extern/libmv/third_party/ceres/internal/ceres/suitesparse.h b/extern/libmv/third_party/ceres/internal/ceres/suitesparse.h new file mode 100644 index 00000000000..091e67a69a9 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/suitesparse.h @@ -0,0 +1,159 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// A simple C++ interface to the SuiteSparse and CHOLMOD libraries. + +#ifndef CERES_INTERNAL_SUITESPARSE_H_ +#define CERES_INTERNAL_SUITESPARSE_H_ + +#ifndef CERES_NO_SUITESPARSE + +#include <cstring> +#include <string> + +#include <glog/logging.h> +#include "cholmod.h" +#include "ceres/internal/port.h" + +namespace ceres { +namespace internal { + +class CompressedRowSparseMatrix; +class TripletSparseMatrix; + +// The raw CHOLMOD and SuiteSparseQR libraries have a slightly +// cumbersome c like calling format. This object abstracts it away and +// provides the user with a simpler interface. The methods here cannot +// be static as a cholmod_common object serves as a global variable +// for all cholmod function calls. +class SuiteSparse { + public: + SuiteSparse() { cholmod_start(&cc_); } + ~SuiteSparse() { cholmod_finish(&cc_); } + + // Functions for building cholmod_sparse objects from sparse + // matrices stored in triplet form. The matrix A is not + // modifed. Called owns the result. + cholmod_sparse* CreateSparseMatrix(TripletSparseMatrix* A); + + // This function works like CreateSparseMatrix, except that the + // return value corresponds to A' rather than A. + cholmod_sparse* CreateSparseMatrixTranspose(TripletSparseMatrix* A); + + // Create a cholmod_sparse wrapper around the contents of A. This is + // a shallow object, which refers to the contents of A and does not + // use the SuiteSparse machinery to allocate memory, this object + // should be disposed off with a delete and not a call to Free as is + // the case for objects returned by CreateSparseMatrixTranspose. + cholmod_sparse* CreateSparseMatrixTransposeView(CompressedRowSparseMatrix* A); + + // Given a vector x, build a cholmod_dense vector of size out_size + // with the first in_size entries copied from x. If x is NULL, then + // an all zeros vector is returned. Caller owns the result. + cholmod_dense* CreateDenseVector(const double* x, int in_size, int out_size); + + // The matrix A is scaled using the matrix whose diagonal is the + // vector scale. mode describes how scaling is applied. Possible + // values are CHOLMOD_ROW for row scaling - diag(scale) * A, + // CHOLMOD_COL for column scaling - A * diag(scale) and CHOLMOD_SYM + // for symmetric scaling which scales both the rows and the columns + // - diag(scale) * A * diag(scale). + void Scale(cholmod_dense* scale, int mode, cholmod_sparse* A) { + cholmod_scale(scale, mode, A, &cc_); + } + + // Create and return a matrix m = A * A'. Caller owns the + // result. The matrix A is not modified. + cholmod_sparse* AATranspose(cholmod_sparse* A) { + cholmod_sparse*m = cholmod_aat(A, NULL, A->nrow, 1, &cc_); + m->stype = 1; // Pay attention to the upper triangular part. + return m; + } + + // y = alpha * A * x + beta * y. Only y is modified. + void SparseDenseMultiply(cholmod_sparse* A, double alpha, double beta, + cholmod_dense* x, cholmod_dense* y) { + double alpha_[2] = {alpha, 0}; + double beta_[2] = {beta, 0}; + cholmod_sdmult(A, 0, alpha_, beta_, x, y, &cc_); + } + + // Analyze the sparsity structure of the matrix A compute the + // symbolic factorization of A. A is not modified, only the pattern + // of non-zeros of A is used, the actual numerical values in A are + // of no consequence. Caller owns the result. + cholmod_factor* AnalyzeCholesky(cholmod_sparse* A); + + // Use the symbolic factorization in L, to find the numerical + // factorization for the matrix A or AA^T. Return true if + // successful, false otherwise. L contains the numeric factorization + // on return. + bool Cholesky(cholmod_sparse* A, cholmod_factor* L); + + // Given a Cholesky factorization of a matrix A = LL^T, solve the + // linear system Ax = b, and return the result. If the Solve fails + // NULL is returned. Caller owns the result. + cholmod_dense* Solve(cholmod_factor* L, cholmod_dense* b); + + // Combine the calls to Cholesky and Solve into a single call. If + // the cholesky factorization or the solve fails, return + // NULL. Caller owns the result. + cholmod_dense* SolveCholesky(cholmod_sparse* A, + cholmod_factor* L, + cholmod_dense* b); + + void Free(cholmod_sparse* m) { cholmod_free_sparse(&m, &cc_); } + void Free(cholmod_dense* m) { cholmod_free_dense(&m, &cc_); } + void Free(cholmod_factor* m) { cholmod_free_factor(&m, &cc_); } + + void Print(cholmod_sparse* m, const string& name) { + cholmod_print_sparse(m, const_cast<char*>(name.c_str()), &cc_); + } + + void Print(cholmod_dense* m, const string& name) { + cholmod_print_dense(m, const_cast<char*>(name.c_str()), &cc_); + } + + void Print(cholmod_triplet* m, const string& name) { + cholmod_print_triplet(m, const_cast<char*>(name.c_str()), &cc_); + } + + cholmod_common* mutable_cc() { return &cc_; } + + private: + cholmod_common cc_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_NO_SUITESPARSE + +#endif // CERES_INTERNAL_SUITESPARSE_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/triplet_sparse_matrix.cc b/extern/libmv/third_party/ceres/internal/ceres/triplet_sparse_matrix.cc new file mode 100644 index 00000000000..247ab2e697b --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/triplet_sparse_matrix.cc @@ -0,0 +1,306 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/triplet_sparse_matrix.h" + +#include <algorithm> +#include <cstddef> +#include <glog/logging.h> +#include "ceres/matrix_proto.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/port.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +TripletSparseMatrix::TripletSparseMatrix() + : num_rows_(0), + num_cols_(0), + max_num_nonzeros_(0), + num_nonzeros_(0), + rows_(NULL), + cols_(NULL), + values_(NULL) {} + +TripletSparseMatrix::~TripletSparseMatrix() {} + +TripletSparseMatrix::TripletSparseMatrix(int num_rows, + int num_cols, + int max_num_nonzeros) + : num_rows_(num_rows), + num_cols_(num_cols), + max_num_nonzeros_(max_num_nonzeros), + num_nonzeros_(0), + rows_(NULL), + cols_(NULL), + values_(NULL) { + // All the sizes should at least be zero + CHECK_GE(num_rows, 0); + CHECK_GE(num_cols, 0); + CHECK_GE(max_num_nonzeros, 0); + AllocateMemory(); +} + +TripletSparseMatrix::TripletSparseMatrix(const TripletSparseMatrix& orig) + : num_rows_(orig.num_rows_), + num_cols_(orig.num_cols_), + max_num_nonzeros_(orig.max_num_nonzeros_), + num_nonzeros_(orig.num_nonzeros_), + rows_(NULL), + cols_(NULL), + values_(NULL) { + AllocateMemory(); + CopyData(orig); +} + +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS +TripletSparseMatrix::TripletSparseMatrix(const SparseMatrixProto& outer_proto) { + CHECK(outer_proto.has_triplet_matrix()); + + const TripletSparseMatrixProto& proto = outer_proto.triplet_matrix(); + CHECK(proto.has_num_rows()); + CHECK(proto.has_num_cols()); + CHECK_EQ(proto.rows_size(), proto.cols_size()); + CHECK_EQ(proto.cols_size(), proto.values_size()); + + // Initialize the matrix with the appropriate size and capacity. + max_num_nonzeros_ = 0; + set_num_nonzeros(0); + Reserve(proto.num_nonzeros()); + Resize(proto.num_rows(), proto.num_cols()); + set_num_nonzeros(proto.num_nonzeros()); + + // Copy the entries in. + for (int i = 0; i < proto.num_nonzeros(); ++i) { + rows_[i] = proto.rows(i); + cols_[i] = proto.cols(i); + values_[i] = proto.values(i); + } +} +#endif + +TripletSparseMatrix& TripletSparseMatrix::operator=( + const TripletSparseMatrix& rhs) { + num_rows_ = rhs.num_rows_; + num_cols_ = rhs.num_cols_; + num_nonzeros_ = rhs.num_nonzeros_; + max_num_nonzeros_ = rhs.max_num_nonzeros_; + AllocateMemory(); + CopyData(rhs); + return *this; +} + +bool TripletSparseMatrix::AllTripletsWithinBounds() const { + for (int i = 0; i < num_nonzeros_; ++i) { + if ((rows_[i] < 0) || (rows_[i] >= num_rows_) || + (cols_[i] < 0) || (cols_[i] >= num_cols_)) + return false; + } + return true; +} + +void TripletSparseMatrix::Reserve(int new_max_num_nonzeros) { + CHECK_LE(num_nonzeros_, new_max_num_nonzeros) + << "Reallocation will cause data loss"; + + // Nothing to do if we have enough space already. + if (new_max_num_nonzeros <= max_num_nonzeros_) + return; + + int* new_rows = new int[new_max_num_nonzeros]; + int* new_cols = new int[new_max_num_nonzeros]; + double* new_values = new double[new_max_num_nonzeros]; + + for (int i = 0; i < num_nonzeros_; ++i) { + new_rows[i] = rows_[i]; + new_cols[i] = cols_[i]; + new_values[i] = values_[i]; + } + + rows_.reset(new_rows); + cols_.reset(new_cols); + values_.reset(new_values); + + max_num_nonzeros_ = new_max_num_nonzeros; +} + +void TripletSparseMatrix::SetZero() { + fill(values_.get(), values_.get() + max_num_nonzeros_, 0.0); + num_nonzeros_ = 0; +} + +void TripletSparseMatrix::set_num_nonzeros(int num_nonzeros) { + CHECK_GE(num_nonzeros, 0); + CHECK_LE(num_nonzeros, max_num_nonzeros_); + num_nonzeros_ = num_nonzeros; +}; + +void TripletSparseMatrix::AllocateMemory() { + rows_.reset(new int[max_num_nonzeros_]); + cols_.reset(new int[max_num_nonzeros_]); + values_.reset(new double[max_num_nonzeros_]); +} + +void TripletSparseMatrix::CopyData(const TripletSparseMatrix& orig) { + for (int i = 0; i < num_nonzeros_; ++i) { + rows_[i] = orig.rows_[i]; + cols_[i] = orig.cols_[i]; + values_[i] = orig.values_[i]; + } +} + +void TripletSparseMatrix::RightMultiply(const double* x, double* y) const { + for (int i = 0; i < num_nonzeros_; ++i) { + y[rows_[i]] += values_[i]*x[cols_[i]]; + } +} + +void TripletSparseMatrix::LeftMultiply(const double* x, double* y) const { + for (int i = 0; i < num_nonzeros_; ++i) { + y[cols_[i]] += values_[i]*x[rows_[i]]; + } +} + +void TripletSparseMatrix::SquaredColumnNorm(double* x) const { + CHECK_NOTNULL(x); + VectorRef(x, num_cols_).setZero(); + for (int i = 0; i < num_nonzeros_; ++i) { + x[cols_[i]] += values_[i] * values_[i]; + } +} + +void TripletSparseMatrix::ScaleColumns(const double* scale) { + CHECK_NOTNULL(scale); + for (int i = 0; i < num_nonzeros_; ++i) { + values_[i] = values_[i] * scale[cols_[i]]; + } +} + +void TripletSparseMatrix::ToDenseMatrix(Matrix* dense_matrix) const { + dense_matrix->resize(num_rows_, num_cols_); + dense_matrix->setZero(); + Matrix& m = *dense_matrix; + for (int i = 0; i < num_nonzeros_; ++i) { + m(rows_[i], cols_[i]) += values_[i]; + } +} + +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS +void TripletSparseMatrix::ToProto(SparseMatrixProto *proto) const { + proto->Clear(); + + TripletSparseMatrixProto* tsm_proto = proto->mutable_triplet_matrix(); + tsm_proto->set_num_rows(num_rows_); + tsm_proto->set_num_cols(num_cols_); + tsm_proto->set_num_nonzeros(num_nonzeros_); + for (int i = 0; i < num_nonzeros_; ++i) { + tsm_proto->add_rows(rows_[i]); + tsm_proto->add_cols(cols_[i]); + tsm_proto->add_values(values_[i]); + } +} +#endif + +void TripletSparseMatrix::AppendRows(const TripletSparseMatrix& B) { + CHECK_EQ(B.num_cols(), num_cols_); + Reserve(num_nonzeros_ + B.num_nonzeros_); + for (int i = 0; i < B.num_nonzeros_; ++i) { + rows_.get()[num_nonzeros_] = B.rows()[i] + num_rows_; + cols_.get()[num_nonzeros_] = B.cols()[i]; + values_.get()[num_nonzeros_++] = B.values()[i]; + } + num_rows_ = num_rows_ + B.num_rows(); +} + +void TripletSparseMatrix::AppendCols(const TripletSparseMatrix& B) { + CHECK_EQ(B.num_rows(), num_rows_); + Reserve(num_nonzeros_ + B.num_nonzeros_); + for (int i = 0; i < B.num_nonzeros_; ++i, ++num_nonzeros_) { + rows_.get()[num_nonzeros_] = B.rows()[i]; + cols_.get()[num_nonzeros_] = B.cols()[i] + num_cols_; + values_.get()[num_nonzeros_] = B.values()[i]; + } + num_cols_ = num_cols_ + B.num_cols(); +} + + +void TripletSparseMatrix::Resize(int new_num_rows, int new_num_cols) { + if ((new_num_rows >= num_rows_) && (new_num_cols >= num_cols_)) { + num_rows_ = new_num_rows; + num_cols_ = new_num_cols; + return; + } + + num_rows_ = new_num_rows; + num_cols_ = new_num_cols; + + int* r_ptr = rows_.get(); + int* c_ptr = cols_.get(); + double* v_ptr = values_.get(); + + int dropped_terms = 0; + for (int i = 0; i < num_nonzeros_; ++i) { + if ((r_ptr[i] < num_rows_) && (c_ptr[i] < num_cols_)) { + if (dropped_terms) { + r_ptr[i-dropped_terms] = r_ptr[i]; + c_ptr[i-dropped_terms] = c_ptr[i]; + v_ptr[i-dropped_terms] = v_ptr[i]; + } + } else { + ++dropped_terms; + } + } + num_nonzeros_ -= dropped_terms; +} + +TripletSparseMatrix* TripletSparseMatrix::CreateSparseDiagonalMatrix( + const double* values, int num_rows) { + TripletSparseMatrix* m = + new TripletSparseMatrix(num_rows, num_rows, num_rows); + for (int i = 0; i < num_rows; ++i) { + m->mutable_rows()[i] = i; + m->mutable_cols()[i] = i; + m->mutable_values()[i] = values[i]; + } + m->set_num_nonzeros(num_rows); + return m; +} + +void TripletSparseMatrix::ToTextFile(FILE* file) const { + CHECK_NOTNULL(file); + for (int i = 0; i < num_nonzeros_; ++i) { + fprintf(file, "% 10d % 10d %17f\n", rows_[i], cols_[i], values_[i]); + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/triplet_sparse_matrix.h b/extern/libmv/third_party/ceres/internal/ceres/triplet_sparse_matrix.h new file mode 100644 index 00000000000..300e74d0bbc --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/triplet_sparse_matrix.h @@ -0,0 +1,137 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_TRIPLET_SPARSE_MATRIX_H_ +#define CERES_INTERNAL_TRIPLET_SPARSE_MATRIX_H_ + +#include "ceres/sparse_matrix.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +class SparseMatrixProto; + +// An implementation of the SparseMatrix interface to store and +// manipulate sparse matrices in triplet (i,j,s) form. This object is +// inspired by the design of the cholmod_triplet struct used in the +// SuiteSparse package and is memory layout compatible with it. +class TripletSparseMatrix : public SparseMatrix { + public: + TripletSparseMatrix(); + TripletSparseMatrix(int num_rows, int num_cols, int max_num_nonzeros); + explicit TripletSparseMatrix(const TripletSparseMatrix& orig); +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS + explicit TripletSparseMatrix(const SparseMatrixProto& proto); +#endif + + TripletSparseMatrix& operator=(const TripletSparseMatrix& rhs); + + ~TripletSparseMatrix(); + + // Implementation of the SparseMatrix interface. + virtual void SetZero(); + virtual void RightMultiply(const double* x, double* y) const; + virtual void LeftMultiply(const double* x, double* y) const; + virtual void SquaredColumnNorm(double* x) const; + virtual void ScaleColumns(const double* scale); + virtual void ToDenseMatrix(Matrix* dense_matrix) const; +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS + virtual void ToProto(SparseMatrixProto *proto) const; +#endif + virtual void ToTextFile(FILE* file) const; + virtual int num_rows() const { return num_rows_; } + virtual int num_cols() const { return num_cols_; } + virtual int num_nonzeros() const { return num_nonzeros_; } + virtual const double* values() const { return values_.get(); } + virtual double* mutable_values() { return values_.get(); } + virtual void set_num_nonzeros(int num_nonzeros); + + // Increase max_num_nonzeros and correspondingly increase the size + // of rows_, cols_ and values_. If new_max_num_nonzeros is smaller + // than max_num_nonzeros_, then num_non_zeros should be less than or + // equal to new_max_num_nonzeros, otherwise data loss is possible + // and the method crashes. + void Reserve(int new_max_num_nonzeros); + + // Append the matrix B at the bottom of this matrix. B should have + // the same number of columns as num_cols_. + void AppendRows(const TripletSparseMatrix& B); + + // Append the matrix B at the right of this matrix. B should have + // the same number of rows as num_rows_; + void AppendCols(const TripletSparseMatrix& B); + + // Resize the matrix. Entries which fall outside the new matrix + // bounds are dropped and the num_non_zeros changed accordingly. + void Resize(int new_num_rows, int new_num_cols); + + int max_num_nonzeros() const { return max_num_nonzeros_; } + const int* rows() const { return rows_.get(); } + const int* cols() const { return cols_.get(); } + int* mutable_rows() { return rows_.get(); } + int* mutable_cols() { return cols_.get(); } + + // Returns true if the entries of the matrix obey the row, column, + // and column size bounds and false otherwise. + bool AllTripletsWithinBounds() const; + + bool IsValid() const { return AllTripletsWithinBounds(); } + + // Build a sparse diagonal matrix of size num_rows x num_rows from + // the array values. Entries of the values array are copied into the + // sparse matrix. + static TripletSparseMatrix* CreateSparseDiagonalMatrix(const double* values, + int num_rows); + + private: + void AllocateMemory(); + void CopyData(const TripletSparseMatrix& orig); + + int num_rows_; + int num_cols_; + int max_num_nonzeros_; + int num_nonzeros_; + + // The data is stored as three arrays. For each i, values_[i] is + // stored at the location (rows_[i], cols_[i]). If the there are + // multiple entries with the same (rows_[i], cols_[i]), the values_ + // entries corresponding to them are summed up. + scoped_array<int> rows_; + scoped_array<int> cols_; + scoped_array<double> values_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_TRIPLET_SPARSE_MATRIX_H__ diff --git a/extern/libmv/third_party/ceres/internal/ceres/types.cc b/extern/libmv/third_party/ceres/internal/ceres/types.cc new file mode 100644 index 00000000000..860f8a43f37 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/types.cc @@ -0,0 +1,98 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include <string> +#include "ceres/types.h" + +namespace ceres { + +#define CASESTR(x) case x: return #x + +const char* LinearSolverTypeToString(LinearSolverType solver_type) { + switch (solver_type) { + CASESTR(SPARSE_NORMAL_CHOLESKY); + CASESTR(DENSE_QR); + CASESTR(DENSE_SCHUR); + CASESTR(SPARSE_SCHUR); + CASESTR(ITERATIVE_SCHUR); + CASESTR(CGNR); + default: + return "UNKNOWN"; + } +} + +const char* PreconditionerTypeToString( + PreconditionerType preconditioner_type) { + switch (preconditioner_type) { + CASESTR(IDENTITY); + CASESTR(JACOBI); + CASESTR(SCHUR_JACOBI); + CASESTR(CLUSTER_JACOBI); + CASESTR(CLUSTER_TRIDIAGONAL); + default: + return "UNKNOWN"; + } +} + +const char* OrderingTypeToString(OrderingType ordering_type) { + switch (ordering_type) { + CASESTR(NATURAL); + CASESTR(USER); + CASESTR(SCHUR); + default: + return "UNKNOWN"; + } +} + +const char* SolverTerminationTypeToString( + SolverTerminationType termination_type) { + switch (termination_type) { + CASESTR(NO_CONVERGENCE); + CASESTR(FUNCTION_TOLERANCE); + CASESTR(GRADIENT_TOLERANCE); + CASESTR(PARAMETER_TOLERANCE); + CASESTR(NUMERICAL_FAILURE); + CASESTR(USER_ABORT); + CASESTR(USER_SUCCESS); + CASESTR(DID_NOT_RUN); + default: + return "UNKNOWN"; + } +} + +#undef CASESTR + +bool IsSchurType(LinearSolverType type) { + return ((type == SPARSE_SCHUR) || + (type == DENSE_SCHUR) || + (type == ITERATIVE_SCHUR)); +} + +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/visibility.cc b/extern/libmv/third_party/ceres/internal/ceres/visibility.cc new file mode 100644 index 00000000000..fd41648a7af --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/visibility.cc @@ -0,0 +1,150 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 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: kushalav@google.com (Avanish Kushal) + +#include <cmath> +#include <ctime> +#include <algorithm> +#include <set> +#include <vector> +#include <utility> + +#include <glog/logging.h> +#include "ceres/block_structure.h" +#include "ceres/collections_port.h" +#include "ceres/graph.h" + +namespace ceres { +namespace internal { + +void ComputeVisibility(const CompressedRowBlockStructure& block_structure, + const int num_eliminate_blocks, + vector< set<int> >* visibility) { + CHECK_NOTNULL(visibility); + + // Clear the visibility vector and resize it to hold a + // vector for each camera. + visibility->resize(0); + visibility->resize(block_structure.cols.size() - num_eliminate_blocks); + + for (int i = 0; i < block_structure.rows.size(); ++i) { + const vector<Cell>& cells = block_structure.rows[i].cells; + int block_id = cells[0].block_id; + // If the first block is not an e_block, then skip this row block. + if (block_id >= num_eliminate_blocks) { + continue; + } + + for (int j = 1; j < cells.size(); ++j) { + int camera_block_id = cells[j].block_id - num_eliminate_blocks; + DCHECK_GE(camera_block_id, 0); + DCHECK_LT(camera_block_id, visibility->size()); + (*visibility)[camera_block_id].insert(block_id); + } + } +} + +Graph<int>* CreateSchurComplementGraph(const vector<set<int> >& visibility) { + const time_t start_time = time(NULL); + // Compute the number of e_blocks/point blocks. Since the visibility + // set for each e_block/camera contains the set of e_blocks/points + // visible to it, we find the maximum across all visibility sets. + int num_points = 0; + for (int i = 0; i < visibility.size(); i++) { + if (visibility[i].size() > 0) { + num_points = max(num_points, (*visibility[i].rbegin()) + 1); + } + } + + // Invert the visibility. The input is a camera->point mapping, + // which tells us which points are visible in which + // cameras. However, to compute the sparsity structure of the Schur + // Complement efficiently, its better to have the point->camera + // mapping. + vector<set<int> > inverse_visibility(num_points); + for (int i = 0; i < visibility.size(); i++) { + const set<int>& visibility_set = visibility[i]; + for (set<int>::const_iterator it = visibility_set.begin(); + it != visibility_set.end(); + ++it) { + inverse_visibility[*it].insert(i); + } + } + + // Map from camera pairs to number of points visible to both cameras + // in the pair. + HashMap<pair<int, int>, int > camera_pairs; + + // Count the number of points visible to each camera/f_block pair. + for (vector<set<int> >::const_iterator it = inverse_visibility.begin(); + it != inverse_visibility.end(); + ++it) { + const set<int>& inverse_visibility_set = *it; + for (set<int>::const_iterator camera1 = inverse_visibility_set.begin(); + camera1 != inverse_visibility_set.end(); + ++camera1) { + set<int>::const_iterator camera2 = camera1; + for (++camera2; camera2 != inverse_visibility_set.end(); ++camera2) { + ++(camera_pairs[make_pair(*camera1, *camera2)]); + } + } + } + + Graph<int>* graph = new Graph<int>(); + + // Add vertices and initialize the pairs for self edges so that self + // edges are guaranteed. This is needed for the Canonical views + // algorithm to work correctly. + static const double kSelfEdgeWeight = 1.0; + for (int i = 0; i < visibility.size(); ++i) { + graph->AddVertex(i); + graph->AddEdge(i, i, kSelfEdgeWeight); + } + + // Add an edge for each camera pair. + for (HashMap<pair<int, int>, int>::const_iterator it = camera_pairs.begin(); + it != camera_pairs.end(); + ++it) { + const int camera1 = it->first.first; + const int camera2 = it->first.second; + CHECK_NE(camera1, camera2); + + const int count = it->second; + // Static cast necessary for Windows. + const double weight = static_cast<double>(count) / + (sqrt(static_cast<double>(visibility[camera1].size() * visibility[camera2].size()))); + graph->AddEdge(camera1, camera2, weight); + } + + VLOG(2) << "Schur complement graph time: " << (time(NULL) - start_time); + return graph; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/visibility.h b/extern/libmv/third_party/ceres/internal/ceres/visibility.h new file mode 100644 index 00000000000..692dd87201e --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/visibility.h @@ -0,0 +1,77 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 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: kushalav@google.com (Avanish Kushal) +// sameeragarwal@google.com (Sameer Agarwal) +// +// Functions to manipulate visibility information from the block +// structure of sparse matrices. + +#ifndef CERES_INTERNAL_VISIBILITY_H_ +#define CERES_INTERNAL_VISIBILITY_H_ + +#include <set> +#include <vector> +#include "ceres/graph.h" + +namespace ceres { +namespace internal { + +class CompressedRowBlockStructure; + +// Given a compressed row block structure, computes the set of +// e_blocks "visible" to each f_block. If an e_block co-occurs with an +// f_block in a residual block, it is visible to the f_block. The +// first num_eliminate_blocks columns blocks are e_blocks and the rest +// f_blocks. +// +// In a structure from motion problem, e_blocks correspond to 3D +// points and f_blocks correspond to cameras. +void ComputeVisibility(const CompressedRowBlockStructure& block_structure, + int num_eliminate_blocks, + vector<set<int> >* visibility); + +// Given f_block visibility as computed by the ComputeVisibility +// function above, construct and return a graph whose vertices are +// f_blocks and an edge connects two vertices if they have atleast one +// e_block in common. The weight of this edge is normalized dot +// product between the visibility vectors of the two +// vertices/f_blocks. +// +// This graph reflects the sparsity structure of reduced camera +// matrix/Schur complement matrix obtained by eliminating the e_blocks +// from the normal equations. +// +// Caller acquires ownership of the returned Graph pointer +// (heap-allocated). +Graph<int>* CreateSchurComplementGraph(const vector<set<int> >& visibility); + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_VISIBILITY_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/visibility_based_preconditioner.cc b/extern/libmv/third_party/ceres/internal/ceres/visibility_based_preconditioner.cc new file mode 100644 index 00000000000..aca77528215 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/visibility_based_preconditioner.cc @@ -0,0 +1,611 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/visibility_based_preconditioner.h" + +#include <algorithm> +#include <functional> +#include <iterator> +#include <numeric> +#include <set> +#include <utility> +#include <vector> +#include <glog/logging.h> +#include "Eigen/Dense" +#include "ceres/block_random_access_sparse_matrix.h" +#include "ceres/block_sparse_matrix.h" +#include "ceres/canonical_views_clustering.h" +#include "ceres/collections_port.h" +#include "ceres/detect_structure.h" +#include "ceres/graph.h" +#include "ceres/graph_algorithms.h" +#include "ceres/linear_solver.h" +#include "ceres/schur_eliminator.h" +#include "ceres/visibility.h" +#include "ceres/internal/scoped_ptr.h" + +namespace ceres { +namespace internal { + +// TODO(sameeragarwal): Currently these are magic weights for the +// preconditioner construction. Move these higher up into the Options +// struct and provide some guidelines for choosing them. +// +// This will require some more work on the clustering algorithm and +// possibly some more refactoring of the code. +static const double kSizePenaltyWeight = 3.0; +static const double kSimilarityPenaltyWeight = 0.0; + +#ifndef CERES_NO_SUITESPARSE +VisibilityBasedPreconditioner::VisibilityBasedPreconditioner( + const CompressedRowBlockStructure& bs, + const LinearSolver::Options& options) + : options_(options), + num_blocks_(0), + num_clusters_(0), + factor_(NULL) { + CHECK_GT(options_.num_eliminate_blocks, 0); + CHECK(options_.preconditioner_type == SCHUR_JACOBI || + options_.preconditioner_type == CLUSTER_JACOBI || + options_.preconditioner_type == CLUSTER_TRIDIAGONAL) + << "Unknown preconditioner type: " << options_.preconditioner_type; + num_blocks_ = bs.cols.size() - options_.num_eliminate_blocks; + CHECK_GT(num_blocks_, 0) + << "Jacobian should have atleast 1 f_block for " + << "visibility based preconditioning."; + + // Vector of camera block sizes + block_size_.resize(num_blocks_); + for (int i = 0; i < num_blocks_; ++i) { + block_size_[i] = bs.cols[i + options_.num_eliminate_blocks].size; + } + + const time_t start_time = time(NULL); + switch (options_.preconditioner_type) { + case SCHUR_JACOBI: + ComputeSchurJacobiSparsity(bs); + break; + case CLUSTER_JACOBI: + ComputeClusterJacobiSparsity(bs); + break; + case CLUSTER_TRIDIAGONAL: + ComputeClusterTridiagonalSparsity(bs); + break; + default: + LOG(FATAL) << "Unknown preconditioner type"; + } + const time_t structure_time = time(NULL); + InitStorage(bs); + const time_t storage_time = time(NULL); + InitEliminator(bs); + const time_t eliminator_time = time(NULL); + + // Allocate temporary storage for a vector used during + // RightMultiply. + tmp_rhs_ = CHECK_NOTNULL(ss_.CreateDenseVector(NULL, + m_->num_rows(), + m_->num_rows())); + const time_t init_time = time(NULL); + VLOG(2) << "init time: " + << init_time - start_time + << " structure time: " << structure_time - start_time + << " storage time:" << storage_time - structure_time + << " eliminator time: " << eliminator_time - storage_time; +} + +VisibilityBasedPreconditioner::~VisibilityBasedPreconditioner() { + if (factor_ != NULL) { + ss_.Free(factor_); + factor_ = NULL; + } + if (tmp_rhs_ != NULL) { + ss_.Free(tmp_rhs_); + tmp_rhs_ = NULL; + } +} + +// Determine the sparsity structure of the SCHUR_JACOBI +// preconditioner. SCHUR_JACOBI is an extreme case of a visibility +// based preconditioner where each camera block corresponds to a +// cluster and there is no interaction between clusters. +void VisibilityBasedPreconditioner::ComputeSchurJacobiSparsity( + const CompressedRowBlockStructure& bs) { + num_clusters_ = num_blocks_; + cluster_membership_.resize(num_blocks_); + cluster_pairs_.clear(); + + // Each camea block is a member of its own cluster and the only + // cluster pairs are the self edges (i,i). + for (int i = 0; i < num_clusters_; ++i) { + cluster_membership_[i] = i; + cluster_pairs_.insert(make_pair(i, i)); + } +} + +// Determine the sparsity structure of the CLUSTER_JACOBI +// preconditioner. It clusters cameras using their scene +// visibility. The clusters form the diagonal blocks of the +// preconditioner matrix. +void VisibilityBasedPreconditioner::ComputeClusterJacobiSparsity( + const CompressedRowBlockStructure& bs) { + vector<set<int> > visibility; + ComputeVisibility(bs, options_.num_eliminate_blocks, &visibility); + CHECK_EQ(num_blocks_, visibility.size()); + ClusterCameras(visibility); + cluster_pairs_.clear(); + for (int i = 0; i < num_clusters_; ++i) { + cluster_pairs_.insert(make_pair(i, i)); + } +} + +// Determine the sparsity structure of the CLUSTER_TRIDIAGONAL +// preconditioner. It clusters cameras using using the scene +// visibility and then finds the strongly interacting pairs of +// clusters by constructing another graph with the clusters as +// vertices and approximating it with a degree-2 maximum spanning +// forest. The set of edges in this forest are the cluster pairs. +void VisibilityBasedPreconditioner::ComputeClusterTridiagonalSparsity( + const CompressedRowBlockStructure& bs) { + vector<set<int> > visibility; + ComputeVisibility(bs, options_.num_eliminate_blocks, &visibility); + CHECK_EQ(num_blocks_, visibility.size()); + ClusterCameras(visibility); + + // Construct a weighted graph on the set of clusters, where the + // edges are the number of 3D points/e_blocks visible in both the + // clusters at the ends of the edge. Return an approximate degree-2 + // maximum spanning forest of this graph. + vector<set<int> > cluster_visibility; + ComputeClusterVisibility(visibility, &cluster_visibility); + scoped_ptr<Graph<int> > cluster_graph( + CHECK_NOTNULL(CreateClusterGraph(cluster_visibility))); + scoped_ptr<Graph<int> > forest( + CHECK_NOTNULL(Degree2MaximumSpanningForest(*cluster_graph))); + ForestToClusterPairs(*forest, &cluster_pairs_); +} + +// Allocate storage for the preconditioner matrix. +void VisibilityBasedPreconditioner::InitStorage( + const CompressedRowBlockStructure& bs) { + ComputeBlockPairsInPreconditioner(bs); + m_.reset(new BlockRandomAccessSparseMatrix(block_size_, block_pairs_)); +} + +// Call the canonical views algorithm and cluster the cameras based on +// their visibility sets. The visibility set of a camera is the set of +// e_blocks/3D points in the scene that are seen by it. +// +// The cluster_membership_ vector is updated to indicate cluster +// memberships for each camera block. +void VisibilityBasedPreconditioner::ClusterCameras( + const vector<set<int> >& visibility) { + scoped_ptr<Graph<int> > schur_complement_graph( + CHECK_NOTNULL(CreateSchurComplementGraph(visibility))); + + CanonicalViewsClusteringOptions options; + options.size_penalty_weight = kSizePenaltyWeight; + options.similarity_penalty_weight = kSimilarityPenaltyWeight; + + vector<int> centers; + HashMap<int, int> membership; + ComputeCanonicalViewsClustering(*schur_complement_graph, + options, + ¢ers, + &membership); + num_clusters_ = centers.size(); + CHECK_GT(num_clusters_, 0); + VLOG(2) << "num_clusters: " << num_clusters_; + FlattenMembershipMap(membership, &cluster_membership_); +} + +// Compute the block sparsity structure of the Schur complement +// matrix. For each pair of cameras contributing a non-zero cell to +// the schur complement, determine if that cell is present in the +// preconditioner or not. +// +// A pair of cameras contribute a cell to the preconditioner if they +// are part of the same cluster or if the the two clusters that they +// belong have an edge connecting them in the degree-2 maximum +// spanning forest. +// +// For example, a camera pair (i,j) where i belonges to cluster1 and +// j belongs to cluster2 (assume that cluster1 < cluster2). +// +// The cell corresponding to (i,j) is present in the preconditioner +// if cluster1 == cluster2 or the pair (cluster1, cluster2) were +// connected by an edge in the degree-2 maximum spanning forest. +// +// Since we have already expanded the forest into a set of camera +// pairs/edges, including self edges, the check can be reduced to +// checking membership of (cluster1, cluster2) in cluster_pairs_. +void VisibilityBasedPreconditioner::ComputeBlockPairsInPreconditioner( + const CompressedRowBlockStructure& bs) { + block_pairs_.clear(); + for (int i = 0; i < num_blocks_; ++i) { + block_pairs_.insert(make_pair(i, i)); + } + + int r = 0; + set<pair<int, int> > skipped_pairs; + const int num_row_blocks = bs.rows.size(); + const int num_eliminate_blocks = options_.num_eliminate_blocks; + + // Iterate over each row of the matrix. The block structure of the + // matrix is assumed to be sorted in order of the e_blocks/point + // blocks. Thus all row blocks containing an e_block/point occur + // contiguously. Further, if present, an e_block is always the first + // parameter block in each row block. These structural assumptions + // are common to all Schur complement based solvers in Ceres. + // + // For each e_block/point block we identify the set of cameras + // seeing it. The cross product of this set with itself is the set + // of non-zero cells contibuted by this e_block. + // + // The time complexity of this is O(nm^2) where, n is the number of + // 3d points and m is the maximum number of cameras seeing any + // point, which for most scenes is a fairly small number. + while (r < num_row_blocks) { + int e_block_id = bs.rows[r].cells.front().block_id; + if (e_block_id >= num_eliminate_blocks) { + // Skip the rows whose first block is an f_block. + break; + } + + set<int> f_blocks; + for (; r < num_row_blocks; ++r) { + const CompressedRow& row = bs.rows[r]; + if (row.cells.front().block_id != e_block_id) { + break; + } + + // Iterate over the blocks in the row, ignoring the first block + // since it is the one to be eliminated and adding the rest to + // the list of f_blocks associated with this e_block. + for (int c = 1; c < row.cells.size(); ++c) { + const Cell& cell = row.cells[c]; + const int f_block_id = cell.block_id - num_eliminate_blocks; + CHECK_GE(f_block_id, 0); + f_blocks.insert(f_block_id); + } + } + + for (set<int>::const_iterator block1 = f_blocks.begin(); + block1 != f_blocks.end(); + ++block1) { + set<int>::const_iterator block2 = block1; + ++block2; + for (; block2 != f_blocks.end(); ++block2) { + if (IsBlockPairInPreconditioner(*block1, *block2)) { + block_pairs_.insert(make_pair(*block1, *block2)); + } else { + skipped_pairs.insert(make_pair(*block1, *block2)); + } + } + } + } + + // The remaining rows which do not contain any e_blocks. + for (; r < num_row_blocks; ++r) { + const CompressedRow& row = bs.rows[r]; + CHECK_GE(row.cells.front().block_id, num_eliminate_blocks); + for (int i = 0; i < row.cells.size(); ++i) { + const int block1 = row.cells[i].block_id - num_eliminate_blocks; + for (int j = 0; j < row.cells.size(); ++j) { + const int block2 = row.cells[j].block_id - num_eliminate_blocks; + if (block1 <= block2) { + if (IsBlockPairInPreconditioner(block1, block2)) { + block_pairs_.insert(make_pair(block1, block2)); + } else { + skipped_pairs.insert(make_pair(block1, block2)); + } + } + } + } + } + + VLOG(1) << "Block pair stats: " + << block_pairs_.size() << " included " + << skipped_pairs.size() << " excluded"; +} + +// Initialize the SchurEliminator. +void VisibilityBasedPreconditioner::InitEliminator( + const CompressedRowBlockStructure& bs) { + LinearSolver::Options eliminator_options; + eliminator_options.num_eliminate_blocks = options_.num_eliminate_blocks; + eliminator_options.num_threads = options_.num_threads; + eliminator_options.constant_sparsity = true; + + DetectStructure(bs, options_.num_eliminate_blocks, + &eliminator_options.row_block_size, + &eliminator_options.e_block_size, + &eliminator_options.f_block_size); + + eliminator_.reset(SchurEliminatorBase::Create(eliminator_options)); + eliminator_->Init(options_.num_eliminate_blocks, &bs); +} + +// Compute the values of the preconditioner matrix and factorize it. +bool VisibilityBasedPreconditioner::Compute(const BlockSparseMatrixBase& A, + const double* D) { + const time_t start_time = time(NULL); + const int num_rows = m_->num_rows(); + CHECK_GT(num_rows, 0); + + // We need a dummy rhs vector and a dummy b vector since the Schur + // eliminator combines the computation of the reduced camera matrix + // with the computation of the right hand side of that linear + // system. + // + // TODO(sameeragarwal): Perhaps its worth refactoring the + // SchurEliminator::Eliminate function to allow NULL for the rhs. As + // of now it does not seem to be worth the effort. + Vector rhs = Vector::Zero(m_->num_rows()); + Vector b = Vector::Zero(A.num_rows()); + + // Compute a subset of the entries of the Schur complement. + eliminator_->Eliminate(&A, b.data(), D, m_.get(), rhs.data()); + + // Try factorizing the matrix. For SCHUR_JACOBI and CLUSTER_JACOBI, + // this should always succeed modulo some numerical/conditioning + // problems. For CLUSTER_TRIDIAGONAL, in general the preconditioner + // matrix as constructed is not positive definite. However, we will + // go ahead and try factorizing it. If it works, great, otherwise we + // scale all the cells in the preconditioner corresponding to the + // edges in the degree-2 forest and that guarantees positive + // definiteness. The proof of this fact can be found in Lemma 1 in + // "Visibility Based Preconditioning for Bundle Adjustment". + // + // Doing the factorization like this saves us matrix mass when + // scaling is not needed, which is quite often in our experience. + bool status = Factorize(); + + // The scaling only affects the tri-diagonal case, since + // ScaleOffDiagonalBlocks only pays attenion to the cells that + // belong to the edges of the degree-2 forest. In the SCHUR_JACOBI + // and the CLUSTER_JACOBI cases, the preconditioner is guaranteed to + // be positive semidefinite. + if (!status && options_.preconditioner_type == CLUSTER_TRIDIAGONAL) { + VLOG(1) << "Unscaled factorization failed. Retrying with off-diagonal " + << "scaling"; + ScaleOffDiagonalCells(); + status = Factorize(); + } + + VLOG(2) << "Compute time: " << time(NULL) - start_time; + return status; +} + +// Consider the preconditioner matrix as meta-block matrix, whose +// blocks correspond to the clusters. Then cluster pairs corresponding +// to edges in the degree-2 forest are off diagonal entries of this +// matrix. Scaling these off-diagonal entries by 1/2 forces this +// matrix to be positive definite. +void VisibilityBasedPreconditioner::ScaleOffDiagonalCells() { + for (set< pair<int, int> >::const_iterator it = block_pairs_.begin(); + it != block_pairs_.end(); + ++it) { + const int block1 = it->first; + const int block2 = it->second; + if (!IsBlockPairOffDiagonal(block1, block2)) { + continue; + } + + int r, c, row_stride, col_stride; + CellInfo* cell_info = m_->GetCell(block1, block2, + &r, &c, + &row_stride, &col_stride); + CHECK(cell_info != NULL) + << "Cell missing for block pair (" << block1 << "," << block2 << ")" + << " cluster pair (" << cluster_membership_[block1] + << " " << cluster_membership_[block2] << ")"; + + // Ah the magic of tri-diagonal matrices and diagonal + // dominance. See Lemma 1 in "Visibility Based Preconditioning + // For Bundle Adjustment". + MatrixRef m(cell_info->values, row_stride, col_stride); + m.block(r, c, block_size_[block1], block_size_[block2]) *= 0.5; + } +} + +// Compute the sparse Cholesky factorization of the preconditioner +// matrix. +bool VisibilityBasedPreconditioner::Factorize() { + // Extract the TripletSparseMatrix that is used for actually storing + // S and convert it into a cholmod_sparse object. + cholmod_sparse* lhs = ss_.CreateSparseMatrix( + down_cast<BlockRandomAccessSparseMatrix*>( + m_.get())->mutable_matrix()); + + // The matrix is symmetric, and the upper triangular part of the + // matrix contains the values. + lhs->stype = 1; + + // Symbolic factorization is computed if we don't already have one + // handy. + if (factor_ == NULL) { + factor_ = ss_.AnalyzeCholesky(lhs); + } + + bool status = ss_.Cholesky(lhs, factor_); + ss_.Free(lhs); + return status; +} + +void VisibilityBasedPreconditioner::RightMultiply(const double* x, + double* y) const { + CHECK_NOTNULL(x); + CHECK_NOTNULL(y); + SuiteSparse* ss = const_cast<SuiteSparse*>(&ss_); + + const int num_rows = m_->num_rows(); + memcpy(CHECK_NOTNULL(tmp_rhs_)->x, x, m_->num_rows() * sizeof(*x)); + cholmod_dense* solution = CHECK_NOTNULL(ss->Solve(factor_, tmp_rhs_)); + memcpy(y, solution->x, sizeof(*y) * num_rows); + ss->Free(solution); +} + +int VisibilityBasedPreconditioner::num_rows() const { + return m_->num_rows(); +} + +// Classify camera/f_block pairs as in and out of the preconditioner, +// based on whether the cluster pair that they belong to is in the +// preconditioner or not. +bool VisibilityBasedPreconditioner::IsBlockPairInPreconditioner( + const int block1, + const int block2) const { + int cluster1 = cluster_membership_[block1]; + int cluster2 = cluster_membership_[block2]; + if (cluster1 > cluster2) { + std::swap(cluster1, cluster2); + } + return (cluster_pairs_.count(make_pair(cluster1, cluster2)) > 0); +} + +bool VisibilityBasedPreconditioner::IsBlockPairOffDiagonal( + const int block1, + const int block2) const { + return (cluster_membership_[block1] != cluster_membership_[block2]); +} + +// Convert a graph into a list of edges that includes self edges for +// each vertex. +void VisibilityBasedPreconditioner::ForestToClusterPairs( + const Graph<int>& forest, + HashSet<pair<int, int> >* cluster_pairs) const { + CHECK_NOTNULL(cluster_pairs)->clear(); + const HashSet<int>& vertices = forest.vertices(); + CHECK_EQ(vertices.size(), num_clusters_); + + // Add all the cluster pairs corresponding to the edges in the + // forest. + for (HashSet<int>::const_iterator it1 = vertices.begin(); + it1 != vertices.end(); + ++it1) { + const int cluster1 = *it1; + cluster_pairs->insert(make_pair(cluster1, cluster1)); + const HashSet<int>& neighbors = forest.Neighbors(cluster1); + for (HashSet<int>::const_iterator it2 = neighbors.begin(); + it2 != neighbors.end(); + ++it2) { + const int cluster2 = *it2; + if (cluster1 < cluster2) { + cluster_pairs->insert(make_pair(cluster1, cluster2)); + } + } + } +} + +// The visibilty set of a cluster is the union of the visibilty sets +// of all its cameras. In other words, the set of points visible to +// any camera in the cluster. +void VisibilityBasedPreconditioner::ComputeClusterVisibility( + const vector<set<int> >& visibility, + vector<set<int> >* cluster_visibility) const { + CHECK_NOTNULL(cluster_visibility)->resize(0); + cluster_visibility->resize(num_clusters_); + for (int i = 0; i < num_blocks_; ++i) { + const int cluster_id = cluster_membership_[i]; + (*cluster_visibility)[cluster_id].insert(visibility[i].begin(), + visibility[i].end()); + } +} + +// Construct a graph whose vertices are the clusters, and the edge +// weights are the number of 3D points visible to cameras in both the +// vertices. +Graph<int>* VisibilityBasedPreconditioner::CreateClusterGraph( + const vector<set<int> >& cluster_visibility) const { + Graph<int>* cluster_graph = new Graph<int>; + + for (int i = 0; i < num_clusters_; ++i) { + cluster_graph->AddVertex(i); + } + + for (int i = 0; i < num_clusters_; ++i) { + const set<int>& cluster_i = cluster_visibility[i]; + for (int j = i+1; j < num_clusters_; ++j) { + vector<int> intersection; + const set<int>& cluster_j = cluster_visibility[j]; + set_intersection(cluster_i.begin(), cluster_i.end(), + cluster_j.begin(), cluster_j.end(), + back_inserter(intersection)); + + if (intersection.size() > 0) { + // Clusters interact strongly when they share a large number + // of 3D points. The degree-2 maximum spanning forest + // alorithm, iterates on the edges in decreasing order of + // their weight, which is the number of points shared by the + // two cameras that it connects. + cluster_graph->AddEdge(i, j, intersection.size()); + } + } + } + return cluster_graph; +} + +// Canonical views clustering returns a HashMap from vertices to +// cluster ids. Convert this into a flat array for quick lookup. It is +// possible that some of the vertices may not be associated with any +// cluster. In that case, randomly assign them to one of the clusters. +void VisibilityBasedPreconditioner::FlattenMembershipMap( + const HashMap<int, int>& membership_map, + vector<int>* membership_vector) const { + CHECK_NOTNULL(membership_vector)->resize(0); + membership_vector->resize(num_blocks_, -1); + // Iterate over the cluster membership map and update the + // cluster_membership_ vector assigning arbitrary cluster ids to + // the few cameras that have not been clustered. + for (HashMap<int, int>::const_iterator it = membership_map.begin(); + it != membership_map.end(); + ++it) { + const int camera_id = it->first; + int cluster_id = it->second; + + // If the view was not clustered, randomly assign it to one of the + // clusters. This preserves the mathematical correctness of the + // preconditioner. If there are too many views which are not + // clustered, it may lead to some quality degradation though. + // + // TODO(sameeragarwal): Check if a large number of views have not + // been clustered and deal with it? + if (cluster_id == -1) { + cluster_id = camera_id % num_clusters_; + } + + membership_vector->at(camera_id) = cluster_id; + } +} + +#endif // CERES_NO_SUITESPARSE + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/visibility_based_preconditioner.h b/extern/libmv/third_party/ceres/internal/ceres/visibility_based_preconditioner.h new file mode 100644 index 00000000000..fa095ca1dd8 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/visibility_based_preconditioner.h @@ -0,0 +1,273 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Preconditioners for linear systems that arise in Structure from +// Motion problems. VisibilityBasedPreconditioner implements three +// preconditioners: +// +// SCHUR_JACOBI +// CLUSTER_JACOBI +// CLUSTER_TRIDIAGONAL +// +// Detailed descriptions of these preconditions beyond what is +// documented here can be found in +// +// Bundle Adjustment in the Large +// S. Agarwal, N. Snavely, S. Seitz & R. Szeliski, ECCV 2010 +// http://www.cs.washington.edu/homes/sagarwal/bal.pdf +// +// Visibility Based Preconditioning for Bundle Adjustment +// A. Kushal & S. Agarwal, submitted to CVPR 2012 +// http://www.cs.washington.edu/homes/sagarwal/vbp.pdf +// +// The three preconditioners share enough code that its most efficient +// to implement them as part of the same code base. + +#ifndef CERES_INTERNAL_VISIBILITY_BASED_PRECONDITIONER_H_ +#define CERES_INTERNAL_VISIBILITY_BASED_PRECONDITIONER_H_ + +#include <set> +#include <vector> +#include <utility> +#include "ceres/collections_port.h" +#include "ceres/graph.h" +#include "ceres/linear_solver.h" +#include "ceres/linear_operator.h" +#include "ceres/suitesparse.h" +#include "ceres/internal/macros.h" +#include "ceres/internal/scoped_ptr.h" + +namespace ceres { +namespace internal { + +class BlockRandomAccessSparseMatrix; +class BlockSparseMatrixBase; +class CompressedRowBlockStructure; +class SchurEliminatorBase; + +// This class implements three preconditioners for Structure from +// Motion/Bundle Adjustment problems. The name +// VisibilityBasedPreconditioner comes from the fact that the sparsity +// structure of the preconditioner matrix is determined by analyzing +// the visibility structure of the scene, i.e. which cameras see which +// points. +// +// Strictly speaking, SCHUR_JACOBI is not a visibility based +// preconditioner but it is an extreme case of CLUSTER_JACOBI, where +// every cluster contains exactly one camera block. Treating it as a +// special case of CLUSTER_JACOBI makes it easy to implement as part +// of the same code base with no significant loss of performance. +// +// In the following, we will only discuss CLUSTER_JACOBI and +// CLUSTER_TRIDIAGONAL. +// +// The key idea of visibility based preconditioning is to identify +// cameras that we expect have strong interactions, and then using the +// entries in the Schur complement matrix corresponding to these +// camera pairs as an approximation to the full Schur complement. +// +// CLUSTER_JACOBI identifies these camera pairs by clustering cameras, +// and considering all non-zero camera pairs within each cluster. The +// clustering in the current implementation is done using the +// Canonical Views algorithm of Simon et al. (see +// canonical_views_clustering.h). For the purposes of clustering, the +// similarity or the degree of interaction between a pair of cameras +// is measured by counting the number of points visible in both the +// cameras. Thus the name VisibilityBasedPreconditioner. Further, if we +// were to permute the parameter blocks such that all the cameras in +// the same cluster occur contiguously, the preconditioner matrix will +// be a block diagonal matrix with blocks corresponding to the +// clusters. Thus in analogy with the Jacobi preconditioner we refer +// to this as the CLUSTER_JACOBI preconditioner. +// +// CLUSTER_TRIDIAGONAL adds more mass to the CLUSTER_JACOBI +// preconditioner by considering the interaction between clusters and +// identifying strong interactions between cluster pairs. This is done +// by constructing a weighted graph on the clusters, with the weight +// on the edges connecting two clusters proportional to the number of +// 3D points visible to cameras in both the clusters. A degree-2 +// maximum spanning forest is identified in this graph and the camera +// pairs contained in the edges of this forest are added to the +// preconditioner. The detailed reasoning for this construction is +// explained in the paper mentioned above. +// +// Degree-2 spanning trees and forests have the property that they +// correspond to tri-diagonal matrices. Thus there exist a permutation +// of the camera blocks under which the CLUSTER_TRIDIAGONAL +// preconditioner matrix is a block tridiagonal matrix, and thus the +// name for the preconditioner. +// +// Thread Safety: This class is NOT thread safe. +// +// Example usage: +// +// LinearSolver::Options options; +// options.preconditioner_type = CLUSTER_JACOBI; +// options.num_eliminate_blocks = num_points; +// VisibilityBasedPreconditioner preconditioner( +// *A.block_structure(), options); +// preconditioner.Compute(A, NULL); +// preconditioner.RightMultiply(x, y); +// + +#ifndef CERES_NO_SUITESPARSE +class VisibilityBasedPreconditioner : public LinearOperator { + public: + // Initialize the symbolic structure of the preconditioner. bs is + // the block structure of the linear system to be solved. It is used + // to determine the sparsity structure of the preconditioner matrix. + // + // It has the same structural requirement as other Schur complement + // based solvers. Please see schur_eliminator.h for more details. + // + // LinearSolver::Options::num_eliminate_blocks should be set to the + // number of e_blocks in the block structure. + // + // TODO(sameeragarwal): The use of LinearSolver::Options should + // ultimately be replaced with Preconditioner::Options and some sort + // of preconditioner factory along the lines of + // LinearSolver::CreateLinearSolver. I will wait to do this till I + // create a general purpose block Jacobi preconditioner for general + // sparse problems along with a CGLS solver. + VisibilityBasedPreconditioner(const CompressedRowBlockStructure& bs, + const LinearSolver::Options& options); + virtual ~VisibilityBasedPreconditioner(); + + // Compute the numerical value of the preconditioner for the linear + // system: + // + // | A | x = |b| + // |diag(D)| |0| + // + // for some vector b. It is important that the matrix A have the + // same block structure as the one used to construct this object. + // + // D can be NULL, in which case its interpreted as a diagonal matrix + // of size zero. + bool Compute(const BlockSparseMatrixBase& A, + const double* D); + + // LinearOperator interface. Since the operator is symmetric, + // LeftMultiply and num_cols are just calls to RightMultiply and + // num_rows respectively. Compute() must be called before + // RightMultiply can be called. + virtual void RightMultiply(const double* x, double* y) const; + virtual void LeftMultiply(const double* x, double* y) const { + RightMultiply(x, y); + } + virtual int num_rows() const; + virtual int num_cols() const { return num_rows(); } + + friend class VisibilityBasedPreconditionerTest; + private: + void ComputeSchurJacobiSparsity(const CompressedRowBlockStructure& bs); + void ComputeClusterJacobiSparsity(const CompressedRowBlockStructure& bs); + void ComputeClusterTridiagonalSparsity(const CompressedRowBlockStructure& bs); + void InitStorage(const CompressedRowBlockStructure& bs); + void InitEliminator(const CompressedRowBlockStructure& bs); + bool Factorize(); + void ScaleOffDiagonalCells(); + + void ClusterCameras(const vector< set<int> >& visibility); + void FlattenMembershipMap(const HashMap<int, int>& membership_map, + vector<int>* membership_vector) const; + void ComputeClusterVisibility(const vector<set<int> >& visibility, + vector<set<int> >* cluster_visibility) const; + Graph<int>* CreateClusterGraph(const vector<set<int> >& visibility) const; + void ForestToClusterPairs(const Graph<int>& forest, + HashSet<pair<int, int> >* cluster_pairs) const; + void ComputeBlockPairsInPreconditioner(const CompressedRowBlockStructure& bs); + bool IsBlockPairInPreconditioner(int block1, int block2) const; + bool IsBlockPairOffDiagonal(int block1, int block2) const; + + LinearSolver::Options options_; + + // Number of parameter blocks in the schur complement. + int num_blocks_; + int num_clusters_; + + // Sizes of the blocks in the schur complement. + vector<int> block_size_; + + // Mapping from cameras to clusters. + vector<int> cluster_membership_; + + // Non-zero camera pairs from the schur complement matrix that are + // present in the preconditioner, sorted by row (first element of + // each pair), then column (second). + set<pair<int, int> > block_pairs_; + + // Set of cluster pairs (including self pairs (i,i)) in the + // preconditioner. + HashSet<pair<int, int> > cluster_pairs_; + scoped_ptr<SchurEliminatorBase> eliminator_; + + // Preconditioner matrix. + scoped_ptr<BlockRandomAccessSparseMatrix> m_; + + // RightMultiply is a const method for LinearOperators. It is + // implemented using CHOLMOD's sparse triangular matrix solve + // function. This however requires non-const access to the + // SuiteSparse context object, even though it does not result in any + // of the state of the preconditioner being modified. + SuiteSparse ss_; + + // Symbolic and numeric factorization of the preconditioner. + cholmod_factor* factor_; + + // Temporary vector used by RightMultiply. + cholmod_dense* tmp_rhs_; + DISALLOW_COPY_AND_ASSIGN(VisibilityBasedPreconditioner); +}; +#else // SuiteSparse +// If SuiteSparse is not compiled in, the preconditioner is not +// available. +class VisibilityBasedPreconditioner : public LinearOperator { + public: + VisibilityBasedPreconditioner(const CompressedRowBlockStructure& bs, + const LinearSolver::Options& options) { + LOG(FATAL) << "Visibility based preconditioning is not available. Please " + "build Ceres with SuiteSparse."; + } + virtual ~VisibilityBasedPreconditioner() {} + virtual void RightMultiply(const double* x, double* y) const {} + virtual void LeftMultiply(const double* x, double* y) const {} + virtual int num_rows() const { return -1; } + virtual int num_cols() const { return -1; } + bool Compute(const BlockSparseMatrixBase& A, const double* D) { + return false; + } +}; +#endif // CERES_NO_SUITESPARSE + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_VISIBILITY_BASED_PRECONDITIONER_H_ |