| // 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::internal { |
| |
| std::unique_ptr<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(); |
| case 5: |
| return LinearLeastSquaresProblem5(); |
| case 6: |
| return LinearLeastSquaresProblem6(); |
| default: |
| LOG(FATAL) << "Unknown problem id requested " << id; |
| } |
| return nullptr; |
| } |
| |
| /* |
| A = [1 2] |
| [3 4] |
| [6 -10] |
| |
| b = [ 8 |
| 18 |
| -18] |
| |
| x = [2 |
| 3] |
| |
| D = [1 |
| 2] |
| |
| x_D = [1.78448275; |
| 2.82327586;] |
| */ |
| std::unique_ptr<LinearLeastSquaresProblem> LinearLeastSquaresProblem0() { |
| auto problem = std::make_unique<LinearLeastSquaresProblem>(); |
| |
| auto A = std::make_unique<TripletSparseMatrix>(3, 2, 6); |
| problem->b = std::make_unique<double[]>(3); |
| problem->D = std::make_unique<double[]>(2); |
| |
| problem->x = std::make_unique<double[]>(2); |
| problem->x_D = std::make_unique<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 = std::move(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] |
| |
| cond(A'A) = 200.36 |
| |
| S = [ 42.3419 -1.4000 -11.5806 |
| -1.4000 2.6000 1.0000 |
| -11.5806 1.0000 31.1935] |
| |
| r = [ 4.3032 |
| 5.4000 |
| 4.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. |
| std::unique_ptr<LinearLeastSquaresProblem> LinearLeastSquaresProblem1() { |
| int num_rows = 6; |
| int num_cols = 5; |
| |
| auto problem = std::make_unique<LinearLeastSquaresProblem>(); |
| |
| auto A = std::make_unique<TripletSparseMatrix>( |
| num_rows, num_cols, num_rows * num_cols); |
| problem->b = std::make_unique<double[]>(num_rows); |
| problem->D = std::make_unique<double[]>(num_cols); |
| problem->num_eliminate_blocks = 2; |
| |
| problem->x = std::make_unique<double[]>(num_cols); |
| problem->x[0] = -2.3061; |
| problem->x[1] = 0.3172; |
| problem->x[2] = 0.2102; |
| problem->x[3] = 2.1367; |
| problem->x[4] = 0.1388; |
| |
| 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 = std::move(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 |
| std::unique_ptr<LinearLeastSquaresProblem> LinearLeastSquaresProblem2() { |
| int num_rows = 6; |
| int num_cols = 5; |
| |
| auto problem = std::make_unique<LinearLeastSquaresProblem>(); |
| |
| problem->b = std::make_unique<double[]>(num_rows); |
| problem->D = std::make_unique<double[]>(num_cols); |
| problem->num_eliminate_blocks = 2; |
| |
| problem->x = std::make_unique<double[]>(num_cols); |
| problem->x[0] = -2.3061; |
| problem->x[1] = 0.3172; |
| problem->x[2] = 0.2102; |
| problem->x[3] = 2.1367; |
| problem->x[4] = 0.1388; |
| |
| auto* bs = new CompressedRowBlockStructure; |
| auto values = std::make_unique<double[]>(num_rows * num_cols); |
| |
| for (int c = 0; c < num_cols; ++c) { |
| bs->cols.emplace_back(); |
| bs->cols.back().size = 1; |
| bs->cols.back().position = c; |
| } |
| |
| int nnz = 0; |
| |
| // Row 1 |
| { |
| values[nnz++] = 1; |
| values[nnz++] = 2; |
| |
| bs->rows.emplace_back(); |
| CompressedRow& row = bs->rows.back(); |
| row.block.size = 1; |
| row.block.position = 0; |
| row.cells.emplace_back(0, 0); |
| row.cells.emplace_back(2, 1); |
| } |
| |
| // Row 2 |
| { |
| values[nnz++] = 3; |
| values[nnz++] = 4; |
| |
| bs->rows.emplace_back(); |
| CompressedRow& row = bs->rows.back(); |
| row.block.size = 1; |
| row.block.position = 1; |
| row.cells.emplace_back(0, 2); |
| row.cells.emplace_back(3, 3); |
| } |
| |
| // Row 3 |
| { |
| values[nnz++] = 5; |
| values[nnz++] = 6; |
| |
| bs->rows.emplace_back(); |
| CompressedRow& row = bs->rows.back(); |
| row.block.size = 1; |
| row.block.position = 2; |
| row.cells.emplace_back(1, 4); |
| row.cells.emplace_back(4, 5); |
| } |
| |
| // Row 4 |
| { |
| values[nnz++] = 7; |
| values[nnz++] = 8; |
| |
| bs->rows.emplace_back(); |
| CompressedRow& row = bs->rows.back(); |
| row.block.size = 1; |
| row.block.position = 3; |
| row.cells.emplace_back(1, 6); |
| row.cells.emplace_back(2, 7); |
| } |
| |
| // Row 5 |
| { |
| values[nnz++] = 9; |
| values[nnz++] = 1; |
| |
| bs->rows.emplace_back(); |
| CompressedRow& row = bs->rows.back(); |
| row.block.size = 1; |
| row.block.position = 4; |
| row.cells.emplace_back(1, 8); |
| row.cells.emplace_back(2, 9); |
| } |
| |
| // Row 6 |
| { |
| values[nnz++] = 1; |
| values[nnz++] = 1; |
| values[nnz++] = 1; |
| |
| bs->rows.emplace_back(); |
| CompressedRow& row = bs->rows.back(); |
| row.block.size = 1; |
| row.block.position = 5; |
| row.cells.emplace_back(2, 10); |
| row.cells.emplace_back(3, 11); |
| row.cells.emplace_back(4, 12); |
| } |
| |
| auto A = std::make_unique<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 = std::move(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 |
| std::unique_ptr<LinearLeastSquaresProblem> LinearLeastSquaresProblem3() { |
| int num_rows = 5; |
| int num_cols = 2; |
| |
| auto problem = std::make_unique<LinearLeastSquaresProblem>(); |
| |
| problem->b = std::make_unique<double[]>(num_rows); |
| problem->D = std::make_unique<double[]>(num_cols); |
| problem->num_eliminate_blocks = 2; |
| |
| auto* bs = new CompressedRowBlockStructure; |
| auto values = std::make_unique<double[]>(num_rows * num_cols); |
| |
| for (int c = 0; c < num_cols; ++c) { |
| bs->cols.emplace_back(); |
| bs->cols.back().size = 1; |
| bs->cols.back().position = c; |
| } |
| |
| int nnz = 0; |
| |
| // Row 1 |
| { |
| values[nnz++] = 1; |
| bs->rows.emplace_back(); |
| CompressedRow& row = bs->rows.back(); |
| row.block.size = 1; |
| row.block.position = 0; |
| row.cells.emplace_back(0, 0); |
| } |
| |
| // Row 2 |
| { |
| values[nnz++] = 3; |
| bs->rows.emplace_back(); |
| CompressedRow& row = bs->rows.back(); |
| row.block.size = 1; |
| row.block.position = 1; |
| row.cells.emplace_back(0, 1); |
| } |
| |
| // Row 3 |
| { |
| values[nnz++] = 5; |
| bs->rows.emplace_back(); |
| CompressedRow& row = bs->rows.back(); |
| row.block.size = 1; |
| row.block.position = 2; |
| row.cells.emplace_back(1, 2); |
| } |
| |
| // Row 4 |
| { |
| values[nnz++] = 7; |
| bs->rows.emplace_back(); |
| CompressedRow& row = bs->rows.back(); |
| row.block.size = 1; |
| row.block.position = 3; |
| row.cells.emplace_back(1, 3); |
| } |
| |
| // Row 5 |
| { |
| values[nnz++] = 9; |
| bs->rows.emplace_back(); |
| CompressedRow& row = bs->rows.back(); |
| row.block.size = 1; |
| row.block.position = 4; |
| row.cells.emplace_back(1, 4); |
| } |
| |
| auto A = std::make_unique<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 = std::move(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. |
| std::unique_ptr<LinearLeastSquaresProblem> LinearLeastSquaresProblem4() { |
| int num_rows = 3; |
| int num_cols = 7; |
| |
| auto problem = std::make_unique<LinearLeastSquaresProblem>(); |
| |
| problem->b = std::make_unique<double[]>(num_rows); |
| problem->D = std::make_unique<double[]>(num_cols); |
| problem->num_eliminate_blocks = 1; |
| |
| auto* bs = new CompressedRowBlockStructure; |
| auto values = std::make_unique<double[]>(num_rows * num_cols); |
| |
| // Column block structure |
| bs->cols.emplace_back(); |
| bs->cols.back().size = 2; |
| bs->cols.back().position = 0; |
| |
| bs->cols.emplace_back(); |
| bs->cols.back().size = 3; |
| bs->cols.back().position = 2; |
| |
| bs->cols.emplace_back(); |
| bs->cols.back().size = 2; |
| bs->cols.back().position = 5; |
| |
| int nnz = 0; |
| |
| // Row 1 & 2 |
| { |
| bs->rows.emplace_back(); |
| CompressedRow& row = bs->rows.back(); |
| row.block.size = 2; |
| row.block.position = 0; |
| |
| row.cells.emplace_back(0, nnz); |
| values[nnz++] = 1; |
| values[nnz++] = 2; |
| values[nnz++] = 1; |
| values[nnz++] = 4; |
| |
| row.cells.emplace_back(2, nnz); |
| values[nnz++] = 1; |
| values[nnz++] = 1; |
| values[nnz++] = 5; |
| values[nnz++] = 6; |
| } |
| |
| // Row 3 |
| { |
| bs->rows.emplace_back(); |
| CompressedRow& row = bs->rows.back(); |
| row.block.size = 1; |
| row.block.position = 2; |
| |
| row.cells.emplace_back(1, nnz); |
| values[nnz++] = 9; |
| values[nnz++] = 0; |
| values[nnz++] = 0; |
| |
| row.cells.emplace_back(2, nnz); |
| values[nnz++] = 3; |
| values[nnz++] = 1; |
| } |
| |
| auto A = std::make_unique<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 = std::move(A); |
| return problem; |
| } |
| |
| /* |
| A problem with block-diagonal F'F. |
| |
| A = [1 0 | 0 0 2 |
| 3 0 | 0 0 4 |
| 0 -1 | 0 1 0 |
| 0 -3 | 0 1 0 |
| 0 -1 | 3 0 0 |
| 0 -2 | 1 0 0] |
| |
| b = [0 |
| 1 |
| 2 |
| 3 |
| 4 |
| 5] |
| |
| c = A'* b = [ 22 |
| -25 |
| 17 |
| 7 |
| 4] |
| |
| A'A = [10 0 0 0 10 |
| 0 15 -5 -4 0 |
| 0 -5 10 0 0 |
| 0 -4 0 2 0 |
| 10 0 0 0 20] |
| |
| cond(A'A) = 41.402 |
| |
| S = [ 8.3333 -1.3333 0 |
| -1.3333 0.9333 0 |
| 0 0 10.0000] |
| |
| r = [ 8.6667 |
| -1.6667 |
| 1.0000] |
| |
| S\r = [ 0.9778 |
| -0.3889 |
| 0.1000] |
| |
| A\b = [ 0.2 |
| -1.4444 |
| 0.9777 |
| -0.3888 |
| 0.1] |
| */ |
| |
| std::unique_ptr<LinearLeastSquaresProblem> LinearLeastSquaresProblem5() { |
| int num_rows = 6; |
| int num_cols = 5; |
| |
| auto problem = std::make_unique<LinearLeastSquaresProblem>(); |
| problem->b = std::make_unique<double[]>(num_rows); |
| problem->D = std::make_unique<double[]>(num_cols); |
| problem->num_eliminate_blocks = 2; |
| |
| // TODO: add x |
| problem->x = std::make_unique<double[]>(num_cols); |
| problem->x[0] = 0.2; |
| problem->x[1] = -1.4444; |
| problem->x[2] = 0.9777; |
| problem->x[3] = -0.3888; |
| problem->x[4] = 0.1; |
| |
| auto* bs = new CompressedRowBlockStructure; |
| auto values = std::make_unique<double[]>(num_rows * num_cols); |
| |
| for (int c = 0; c < num_cols; ++c) { |
| bs->cols.emplace_back(); |
| bs->cols.back().size = 1; |
| bs->cols.back().position = c; |
| } |
| |
| int nnz = 0; |
| |
| // Row 1 |
| { |
| values[nnz++] = -1; |
| values[nnz++] = 2; |
| |
| bs->rows.emplace_back(); |
| CompressedRow& row = bs->rows.back(); |
| row.block.size = 1; |
| row.block.position = 0; |
| row.cells.emplace_back(0, 0); |
| row.cells.emplace_back(4, 1); |
| } |
| |
| // Row 2 |
| { |
| values[nnz++] = 3; |
| values[nnz++] = 4; |
| |
| bs->rows.emplace_back(); |
| CompressedRow& row = bs->rows.back(); |
| row.block.size = 1; |
| row.block.position = 1; |
| row.cells.emplace_back(0, 2); |
| row.cells.emplace_back(4, 3); |
| } |
| |
| // Row 3 |
| { |
| values[nnz++] = -1; |
| values[nnz++] = 1; |
| |
| bs->rows.emplace_back(); |
| CompressedRow& row = bs->rows.back(); |
| row.block.size = 1; |
| row.block.position = 2; |
| row.cells.emplace_back(1, 4); |
| row.cells.emplace_back(3, 5); |
| } |
| |
| // Row 4 |
| { |
| values[nnz++] = -3; |
| values[nnz++] = 1; |
| |
| bs->rows.emplace_back(); |
| CompressedRow& row = bs->rows.back(); |
| row.block.size = 1; |
| row.block.position = 3; |
| row.cells.emplace_back(1, 6); |
| row.cells.emplace_back(3, 7); |
| } |
| |
| // Row 5 |
| { |
| values[nnz++] = -1; |
| values[nnz++] = 3; |
| |
| bs->rows.emplace_back(); |
| CompressedRow& row = bs->rows.back(); |
| row.block.size = 1; |
| row.block.position = 4; |
| row.cells.emplace_back(1, 8); |
| row.cells.emplace_back(2, 9); |
| } |
| |
| // Row 6 |
| { |
| // values[nnz++] = 2; |
| values[nnz++] = -2; |
| values[nnz++] = 1; |
| |
| bs->rows.emplace_back(); |
| CompressedRow& row = bs->rows.back(); |
| row.block.size = 1; |
| row.block.position = 5; |
| // row.cells.emplace_back(0, 10); |
| row.cells.emplace_back(1, 10); |
| row.cells.emplace_back(2, 11); |
| } |
| |
| auto A = std::make_unique<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 = std::move(A); |
| |
| return problem; |
| } |
| |
| /* |
| A = [1 2 0 0 0 1 1 |
| 1 4 0 0 0 5 6 |
| 3 4 0 0 0 7 8 |
| 5 6 0 0 0 9 0 |
| 0 0 9 0 0 3 1] |
| |
| b = [0 |
| 1 |
| 2 |
| 3 |
| 4] |
| */ |
| // 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. |
| // |
| // Additionally, this problem has the first row of the last row block of E being |
| // larger than number of row blocks in E |
| // |
| // NOTE: This problem is too small and rank deficient to be solved without |
| // the diagonal regularization. |
| std::unique_ptr<LinearLeastSquaresProblem> LinearLeastSquaresProblem6() { |
| int num_rows = 5; |
| int num_cols = 7; |
| |
| auto problem = std::make_unique<LinearLeastSquaresProblem>(); |
| |
| problem->b = std::make_unique<double[]>(num_rows); |
| problem->D = std::make_unique<double[]>(num_cols); |
| problem->num_eliminate_blocks = 1; |
| |
| auto* bs = new CompressedRowBlockStructure; |
| auto values = std::make_unique<double[]>(num_rows * num_cols); |
| |
| // Column block structure |
| bs->cols.emplace_back(); |
| bs->cols.back().size = 2; |
| bs->cols.back().position = 0; |
| |
| bs->cols.emplace_back(); |
| bs->cols.back().size = 3; |
| bs->cols.back().position = 2; |
| |
| bs->cols.emplace_back(); |
| bs->cols.back().size = 2; |
| bs->cols.back().position = 5; |
| |
| int nnz = 0; |
| |
| // Row 1 & 2 |
| { |
| bs->rows.emplace_back(); |
| CompressedRow& row = bs->rows.back(); |
| row.block.size = 2; |
| row.block.position = 0; |
| |
| row.cells.emplace_back(0, nnz); |
| values[nnz++] = 1; |
| values[nnz++] = 2; |
| values[nnz++] = 1; |
| values[nnz++] = 4; |
| |
| row.cells.emplace_back(2, nnz); |
| values[nnz++] = 1; |
| values[nnz++] = 1; |
| values[nnz++] = 5; |
| values[nnz++] = 6; |
| } |
| |
| // Row 3 & 4 |
| { |
| bs->rows.emplace_back(); |
| CompressedRow& row = bs->rows.back(); |
| row.block.size = 2; |
| row.block.position = 2; |
| |
| row.cells.emplace_back(0, nnz); |
| values[nnz++] = 3; |
| values[nnz++] = 4; |
| values[nnz++] = 5; |
| values[nnz++] = 6; |
| |
| row.cells.emplace_back(2, nnz); |
| values[nnz++] = 7; |
| values[nnz++] = 8; |
| values[nnz++] = 9; |
| values[nnz++] = 0; |
| } |
| |
| // Row 5 |
| { |
| bs->rows.emplace_back(); |
| CompressedRow& row = bs->rows.back(); |
| row.block.size = 1; |
| row.block.position = 4; |
| |
| row.cells.emplace_back(1, nnz); |
| values[nnz++] = 9; |
| values[nnz++] = 0; |
| values[nnz++] = 0; |
| |
| row.cells.emplace_back(2, nnz); |
| values[nnz++] = 3; |
| values[nnz++] = 1; |
| } |
| |
| auto A = std::make_unique<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 = std::move(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 != nullptr) { |
| LOG(INFO) << "A's appended diagonal:\n" << ConstVectorRef(D, A->num_cols()); |
| } |
| |
| if (b != nullptr) { |
| LOG(INFO) << "b: \n" << ConstVectorRef(b, A->num_rows()); |
| } |
| |
| if (x != nullptr) { |
| LOG(INFO) << "x: \n" << ConstVectorRef(x, A->num_cols()); |
| } |
| return true; |
| } |
| |
| void WriteArrayToFileOrDie(const std::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 std::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 << "*"; |
| |
| std::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()); |
| |
| { |
| std::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 != nullptr) { |
| std::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 != nullptr) { |
| std::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 != nullptr) { |
| std::string filename = filename_base + "_x.txt"; |
| WriteArrayToFileOrDie(filename, x, A->num_cols()); |
| StringAppendF( |
| &matlab_script, "lsqp.x = load('%s', '-ascii');\n", filename.c_str()); |
| } |
| |
| std::string matlab_filename = filename_base + ".m"; |
| WriteStringToFileOrDie(matlab_script, matlab_filename); |
| return true; |
| } |
| } // namespace |
| |
| bool DumpLinearLeastSquaresProblem(const std::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 ceres::internal |