| // 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 <string> |
| #include <vector> |
| #include "ceres/block_sparse_matrix.h" |
| #include "ceres/block_structure.h" |
| #include "ceres/casts.h" |
| #include "ceres/file.h" |
| #include "ceres/internal/scoped_ptr.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; |
| 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; |
| } |
| |
| /* |
| 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; |
| scoped_array<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_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; |
| } |
| |
| 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& filename_base, |
| const SparseMatrix* A, |
| const double* D, |
| const double* b, |
| const double* x, |
| int num_eliminate_blocks) { |
| CHECK_NOTNULL(A); |
| 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_NOTNULL(fptr); |
| 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 |
