diff options
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_ |