MYNT-EYE-S-SDK/3rdparty/ceres-solver-1.11.0/internal/ceres/linear_least_squares_problems.cc
2019-01-03 16:25:18 +08:00

733 lines
18 KiB
C++

// 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