Squashed 'third_party/ceres/' content from commit e51e9b4
Change-Id: I763587619d57e594d3fa158dc3a7fe0b89a1743b
git-subtree-dir: third_party/ceres
git-subtree-split: e51e9b46f6ca88ab8b2266d0e362771db6d98067
diff --git a/internal/ceres/linear_least_squares_problems.cc b/internal/ceres/linear_least_squares_problems.cc
new file mode 100644
index 0000000..7c523d3
--- /dev/null
+++ b/internal/ceres/linear_least_squares_problems.cc
@@ -0,0 +1,733 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2015 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+
+#include "ceres/linear_least_squares_problems.h"
+
+#include <cstdio>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "ceres/block_sparse_matrix.h"
+#include "ceres/block_structure.h"
+#include "ceres/casts.h"
+#include "ceres/file.h"
+#include "ceres/stringprintf.h"
+#include "ceres/triplet_sparse_matrix.h"
+#include "ceres/types.h"
+#include "glog/logging.h"
+
+namespace ceres {
+namespace internal {
+
+using std::string;
+
+LinearLeastSquaresProblem* CreateLinearLeastSquaresProblemFromId(int id) {
+ switch (id) {
+ case 0:
+ return LinearLeastSquaresProblem0();
+ case 1:
+ return LinearLeastSquaresProblem1();
+ case 2:
+ return LinearLeastSquaresProblem2();
+ case 3:
+ return LinearLeastSquaresProblem3();
+ case 4:
+ return LinearLeastSquaresProblem4();
+ default:
+ LOG(FATAL) << "Unknown problem id requested " << id;
+ }
+ return NULL;
+}
+
+/*
+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;
+ std::unique_ptr<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;
+ std::unique_ptr<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;
+}
+
+/*
+ A = [1 2 0 0 0 1 1
+ 1 4 0 0 0 5 6
+ 0 0 9 0 0 3 1]
+
+ b = [0
+ 1
+ 2]
+*/
+// BlockSparseMatrix version
+//
+// This problem has the unique property that it has two different
+// sized f-blocks, but only one of them occurs in the rows involving
+// the one e-block. So performing Schur elimination on this problem
+// tests the Schur Eliminator's ability to handle non-e-block rows
+// correctly when their structure does not conform to the static
+// structure determined by DetectStructure.
+//
+// NOTE: This problem is too small and rank deficient to be solved without
+// the diagonal regularization.
+LinearLeastSquaresProblem* LinearLeastSquaresProblem4() {
+ int num_rows = 3;
+ int num_cols = 7;
+
+ LinearLeastSquaresProblem* problem = new LinearLeastSquaresProblem;
+
+ problem->b.reset(new double[num_rows]);
+ problem->D.reset(new double[num_cols]);
+ problem->num_eliminate_blocks = 1;
+
+ CompressedRowBlockStructure* bs = new CompressedRowBlockStructure;
+ std::unique_ptr<double[]> values(new double[num_rows * num_cols]);
+
+ // Column block structure
+ bs->cols.push_back(Block());
+ bs->cols.back().size = 2;
+ bs->cols.back().position = 0;
+
+ bs->cols.push_back(Block());
+ bs->cols.back().size = 3;
+ bs->cols.back().position = 2;
+
+ bs->cols.push_back(Block());
+ bs->cols.back().size = 2;
+ bs->cols.back().position = 5;
+
+ int nnz = 0;
+
+ // Row 1 & 2
+ {
+ bs->rows.push_back(CompressedRow());
+ CompressedRow& row = bs->rows.back();
+ row.block.size = 2;
+ row.block.position = 0;
+
+ row.cells.push_back(Cell(0, nnz));
+ values[nnz++] = 1;
+ values[nnz++] = 2;
+ values[nnz++] = 1;
+ values[nnz++] = 4;
+
+ row.cells.push_back(Cell(2, nnz));
+ values[nnz++] = 1;
+ values[nnz++] = 1;
+ values[nnz++] = 5;
+ values[nnz++] = 6;
+ }
+
+ // Row 3
+ {
+ bs->rows.push_back(CompressedRow());
+ CompressedRow& row = bs->rows.back();
+ row.block.size = 1;
+ row.block.position = 2;
+
+ row.cells.push_back(Cell(1, nnz));
+ values[nnz++] = 9;
+ values[nnz++] = 0;
+ values[nnz++] = 0;
+
+ row.cells.push_back(Cell(2, nnz));
+ values[nnz++] = 3;
+ values[nnz++] = 1;
+ }
+
+ 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] = (i + 1) * 100;
+ }
+
+ for (int i = 0; i < num_rows; ++i) {
+ problem->b.get()[i] = i;
+ }
+
+ problem->A.reset(A);
+ return problem;
+}
+
+namespace {
+bool DumpLinearLeastSquaresProblemToConsole(const SparseMatrix* A,
+ const double* D,
+ const double* b,
+ const double* x,
+ int num_eliminate_blocks) {
+ CHECK(A != nullptr);
+ 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;
+}
+
+void WriteArrayToFileOrDie(const string& filename,
+ const double* x,
+ const int size) {
+ CHECK(x != nullptr);
+ VLOG(2) << "Writing array to: " << filename;
+ FILE* fptr = fopen(filename.c_str(), "w");
+ CHECK(fptr != nullptr);
+ for (int i = 0; i < size; ++i) {
+ fprintf(fptr, "%17f\n", x[i]);
+ }
+ fclose(fptr);
+}
+
+bool DumpLinearLeastSquaresProblemToTextFile(const string& filename_base,
+ const SparseMatrix* A,
+ const double* D,
+ const double* b,
+ const double* x,
+ int num_eliminate_blocks) {
+ CHECK(A != nullptr);
+ LOG(INFO) << "writing to: " << filename_base << "*";
+
+ string matlab_script;
+ StringAppendF(&matlab_script,
+ "function lsqp = load_trust_region_problem()\n");
+ StringAppendF(&matlab_script,
+ "lsqp.num_rows = %d;\n", A->num_rows());
+ StringAppendF(&matlab_script,
+ "lsqp.num_cols = %d;\n", A->num_cols());
+
+ {
+ string filename = filename_base + "_A.txt";
+ FILE* fptr = fopen(filename.c_str(), "w");
+ CHECK(fptr != nullptr);
+ A->ToTextFile(fptr);
+ fclose(fptr);
+ StringAppendF(&matlab_script,
+ "tmp = load('%s', '-ascii');\n", filename.c_str());
+ StringAppendF(
+ &matlab_script,
+ "lsqp.A = sparse(tmp(:, 1) + 1, tmp(:, 2) + 1, tmp(:, 3), %d, %d);\n",
+ A->num_rows(),
+ A->num_cols());
+ }
+
+
+ if (D != NULL) {
+ string filename = filename_base + "_D.txt";
+ WriteArrayToFileOrDie(filename, D, A->num_cols());
+ StringAppendF(&matlab_script,
+ "lsqp.D = load('%s', '-ascii');\n", filename.c_str());
+ }
+
+ if (b != NULL) {
+ string filename = filename_base + "_b.txt";
+ WriteArrayToFileOrDie(filename, b, A->num_rows());
+ StringAppendF(&matlab_script,
+ "lsqp.b = load('%s', '-ascii');\n", filename.c_str());
+ }
+
+ if (x != NULL) {
+ string filename = filename_base + "_x.txt";
+ WriteArrayToFileOrDie(filename, x, A->num_cols());
+ StringAppendF(&matlab_script,
+ "lsqp.x = load('%s', '-ascii');\n", filename.c_str());
+ }
+
+ string matlab_filename = filename_base + ".m";
+ WriteStringToFileOrDie(matlab_script, matlab_filename);
+ return true;
+}
+} // namespace
+
+bool DumpLinearLeastSquaresProblem(const string& filename_base,
+ 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(A, D, b, x,
+ num_eliminate_blocks);
+ case TEXTFILE:
+ return DumpLinearLeastSquaresProblemToTextFile(filename_base,
+ A, D, b, x,
+ num_eliminate_blocks);
+ default:
+ LOG(FATAL) << "Unknown DumpFormatType " << dump_format_type;
+ }
+
+ return true;
+}
+
+} // namespace internal
+} // namespace ceres