Initial commit of Ceres Solver.
diff --git a/internal/ceres/linear_least_squares_problems.cc b/internal/ceres/linear_least_squares_problems.cc
new file mode 100644
index 0000000..9e3d8bd
--- /dev/null
+++ b/internal/ceres/linear_least_squares_problems.cc
@@ -0,0 +1,573 @@
+// 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 <string>
+#include <vector>
+#include <glog/logging.h>
+#include "ceres/block_sparse_matrix.h"
+#include "ceres/block_structure.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/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;
+ }
+}
+
+#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;
+}
+
+} // namespace internal
+} // namespace ceres
