10ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Ceres Solver - A fast non-linear least squares minimizer 20ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. 30ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// http://code.google.com/p/ceres-solver/ 40ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 50ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Redistribution and use in source and binary forms, with or without 60ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// modification, are permitted provided that the following conditions are met: 70ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 80ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Redistributions of source code must retain the above copyright notice, 90ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// this list of conditions and the following disclaimer. 100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Redistributions in binary form must reproduce the above copyright notice, 110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// this list of conditions and the following disclaimer in the documentation 120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// and/or other materials provided with the distribution. 130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Neither the name of Google Inc. nor the names of its contributors may be 140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// used to endorse or promote products derived from this software without 150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// specific prior written permission. 160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// POSSIBILITY OF SUCH DAMAGE. 280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Author: sameeragarwal@google.com (Sameer Agarwal) 300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/linear_least_squares_problems.h" 320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <cstdio> 340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <string> 350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <vector> 360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/block_sparse_matrix.h" 370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/block_structure.h" 380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/casts.h" 390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/file.h" 400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/internal/scoped_ptr.h" 410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/stringprintf.h" 420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/triplet_sparse_matrix.h" 430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/types.h" 440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "glog/logging.h" 450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace ceres { 470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace internal { 480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongLinearLeastSquaresProblem* CreateLinearLeastSquaresProblemFromId(int id) { 500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong switch (id) { 510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong case 0: 520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return LinearLeastSquaresProblem0(); 530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong case 1: 540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return LinearLeastSquaresProblem1(); 550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong case 2: 560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return LinearLeastSquaresProblem2(); 570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong case 3: 580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return LinearLeastSquaresProblem3(); 590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong default: 600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong LOG(FATAL) << "Unknown problem id requested " << id; 610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return NULL; 630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong/* 660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongA = [1 2] 670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong [3 4] 680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong [6 -10] 690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongb = [ 8 710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 18 720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong -18] 730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongx = [2 750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3] 760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongD = [1 780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2] 790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongx_D = [1.78448275; 810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2.82327586;] 820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong */ 830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongLinearLeastSquaresProblem* LinearLeastSquaresProblem0() { 840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong LinearLeastSquaresProblem* problem = new LinearLeastSquaresProblem; 850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong TripletSparseMatrix* A = new TripletSparseMatrix(3, 2, 6); 870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem->b.reset(new double[3]); 880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem->D.reset(new double[2]); 890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem->x.reset(new double[2]); 910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem->x_D.reset(new double[2]); 920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int* Ai = A->mutable_rows(); 940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int* Aj = A->mutable_cols(); 950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double* Ax = A->mutable_values(); 960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int counter = 0; 980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < 3; ++i) { 990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int j = 0; j< 2; ++j) { 1000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Ai[counter]=i; 1010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Aj[counter]=j; 1020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ++counter; 1030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong }; 1050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Ax[0] = 1.; 1070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Ax[1] = 2.; 1080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Ax[2] = 3.; 1090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Ax[3] = 4.; 1100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Ax[4] = 6; 1110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Ax[5] = -10; 1120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong A->set_num_nonzeros(6); 1130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem->A.reset(A); 1140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem->b[0] = 8; 1160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem->b[1] = 18; 1170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem->b[2] = -18; 1180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem->x[0] = 2.0; 1200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem->x[1] = 3.0; 1210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem->D[0] = 1; 1230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem->D[1] = 2; 1240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem->x_D[0] = 1.78448275; 1260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem->x_D[1] = 2.82327586; 1270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return problem; 1280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong/* 1320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong A = [1 0 | 2 0 0 1330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3 0 | 0 4 0 1340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 0 5 | 0 0 6 1350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 0 7 | 8 0 0 1360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 0 9 | 1 0 0 1370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 0 0 | 1 1 1] 1380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong b = [0 1400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1 1410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2 1420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3 1430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4 1440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 5] 1450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong c = A'* b = [ 3 1470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 67 1480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 33 1490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 9 1500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 17] 1510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong A'A = [10 0 2 12 0 1530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 0 155 65 0 30 1540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2 65 70 1 1 1550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 12 0 1 17 1 1560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 0 30 1 1 37] 1570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong S = [ 42.3419 -1.4000 -11.5806 1590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong -1.4000 2.6000 1.0000 1600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 11.5806 1.0000 31.1935] 1610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong r = [ 4.3032 1630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 5.4000 1640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 5.0323] 1650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong S\r = [ 0.2102 1670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2.1367 1680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 0.1388] 1690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong A\b = [-2.3061 1710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 0.3172 1720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 0.2102 1730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2.1367 1740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 0.1388] 1750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong*/ 1760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// The following two functions create a TripletSparseMatrix and a 1770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// BlockSparseMatrix version of this problem. 1780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// TripletSparseMatrix version. 1800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongLinearLeastSquaresProblem* LinearLeastSquaresProblem1() { 1810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int num_rows = 6; 1820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int num_cols = 5; 1830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong LinearLeastSquaresProblem* problem = new LinearLeastSquaresProblem; 1850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong TripletSparseMatrix* A = new TripletSparseMatrix(num_rows, 1860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_cols, 1870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_rows * num_cols); 1880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem->b.reset(new double[num_rows]); 1890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem->D.reset(new double[num_cols]); 1900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem->num_eliminate_blocks = 2; 1910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int* rows = A->mutable_rows(); 1930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int* cols = A->mutable_cols(); 1940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double* values = A->mutable_values(); 1950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int nnz = 0; 1970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Row 1 1990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong { 2000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong rows[nnz] = 0; 2010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cols[nnz] = 0; 2020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong values[nnz++] = 1; 2030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong rows[nnz] = 0; 2050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cols[nnz] = 2; 2060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong values[nnz++] = 2; 2070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Row 2 2100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong { 2110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong rows[nnz] = 1; 2120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cols[nnz] = 0; 2130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong values[nnz++] = 3; 2140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong rows[nnz] = 1; 2160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cols[nnz] = 3; 2170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong values[nnz++] = 4; 2180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Row 3 2210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong { 2220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong rows[nnz] = 2; 2230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cols[nnz] = 1; 2240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong values[nnz++] = 5; 2250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong rows[nnz] = 2; 2270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cols[nnz] = 4; 2280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong values[nnz++] = 6; 2290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Row 4 2320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong { 2330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong rows[nnz] = 3; 2340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cols[nnz] = 1; 2350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong values[nnz++] = 7; 2360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong rows[nnz] = 3; 2380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cols[nnz] = 2; 2390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong values[nnz++] = 8; 2400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Row 5 2430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong { 2440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong rows[nnz] = 4; 2450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cols[nnz] = 1; 2460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong values[nnz++] = 9; 2470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong rows[nnz] = 4; 2490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cols[nnz] = 2; 2500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong values[nnz++] = 1; 2510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Row 6 2540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong { 2550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong rows[nnz] = 5; 2560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cols[nnz] = 2; 2570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong values[nnz++] = 1; 2580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong rows[nnz] = 5; 2600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cols[nnz] = 3; 2610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong values[nnz++] = 1; 2620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong rows[nnz] = 5; 2640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cols[nnz] = 4; 2650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong values[nnz++] = 1; 2660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong A->set_num_nonzeros(nnz); 2690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK(A->IsValid()); 2700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem->A.reset(A); 2720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_cols; ++i) { 2740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem->D.get()[i] = 1; 2750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_rows; ++i) { 2780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem->b.get()[i] = i; 2790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return problem; 2820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// BlockSparseMatrix version 2850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongLinearLeastSquaresProblem* LinearLeastSquaresProblem2() { 2860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int num_rows = 6; 2870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int num_cols = 5; 2880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong LinearLeastSquaresProblem* problem = new LinearLeastSquaresProblem; 2900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem->b.reset(new double[num_rows]); 2920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem->D.reset(new double[num_cols]); 2930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem->num_eliminate_blocks = 2; 2940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CompressedRowBlockStructure* bs = new CompressedRowBlockStructure; 2960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong scoped_array<double> values(new double[num_rows * num_cols]); 2970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int c = 0; c < num_cols; ++c) { 2990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong bs->cols.push_back(Block()); 3000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong bs->cols.back().size = 1; 3010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong bs->cols.back().position = c; 3020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 3030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int nnz = 0; 3050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Row 1 3070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong { 3080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong values[nnz++] = 1; 3090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong values[nnz++] = 2; 3100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong bs->rows.push_back(CompressedRow()); 3120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CompressedRow& row = bs->rows.back(); 3130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong row.block.size = 1; 3140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong row.block.position = 0; 3150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong row.cells.push_back(Cell(0, 0)); 3160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong row.cells.push_back(Cell(2, 1)); 3170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 3180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Row 2 3200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong { 3210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong values[nnz++] = 3; 3220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong values[nnz++] = 4; 3230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong bs->rows.push_back(CompressedRow()); 3250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CompressedRow& row = bs->rows.back(); 3260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong row.block.size = 1; 3270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong row.block.position = 1; 3280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong row.cells.push_back(Cell(0, 2)); 3290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong row.cells.push_back(Cell(3, 3)); 3300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 3310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Row 3 3330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong { 3340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong values[nnz++] = 5; 3350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong values[nnz++] = 6; 3360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong bs->rows.push_back(CompressedRow()); 3380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CompressedRow& row = bs->rows.back(); 3390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong row.block.size = 1; 3400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong row.block.position = 2; 3410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong row.cells.push_back(Cell(1, 4)); 3420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong row.cells.push_back(Cell(4, 5)); 3430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 3440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Row 4 3460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong { 3470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong values[nnz++] = 7; 3480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong values[nnz++] = 8; 3490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong bs->rows.push_back(CompressedRow()); 3510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CompressedRow& row = bs->rows.back(); 3520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong row.block.size = 1; 3530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong row.block.position = 3; 3540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong row.cells.push_back(Cell(1, 6)); 3550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong row.cells.push_back(Cell(2, 7)); 3560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 3570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Row 5 3590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong { 3600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong values[nnz++] = 9; 3610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong values[nnz++] = 1; 3620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong bs->rows.push_back(CompressedRow()); 3640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CompressedRow& row = bs->rows.back(); 3650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong row.block.size = 1; 3660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong row.block.position = 4; 3670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong row.cells.push_back(Cell(1, 8)); 3680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong row.cells.push_back(Cell(2, 9)); 3690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 3700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Row 6 3720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong { 3730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong values[nnz++] = 1; 3740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong values[nnz++] = 1; 3750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong values[nnz++] = 1; 3760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong bs->rows.push_back(CompressedRow()); 3780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CompressedRow& row = bs->rows.back(); 3790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong row.block.size = 1; 3800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong row.block.position = 5; 3810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong row.cells.push_back(Cell(2, 10)); 3820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong row.cells.push_back(Cell(3, 11)); 3830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong row.cells.push_back(Cell(4, 12)); 3840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 3850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong BlockSparseMatrix* A = new BlockSparseMatrix(bs); 3870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong memcpy(A->mutable_values(), values.get(), nnz * sizeof(*A->values())); 3880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_cols; ++i) { 3900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem->D.get()[i] = 1; 3910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 3920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_rows; ++i) { 3940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem->b.get()[i] = i; 3950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 3960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem->A.reset(A); 3980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return problem; 4000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 4010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong/* 4040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong A = [1 0 4050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3 0 4060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 0 5 4070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 0 7 4080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 0 9 4090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 0 0] 4100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong b = [0 4120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1 4130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2 4140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3 4150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4 4160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 5] 4170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong*/ 4180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// BlockSparseMatrix version 4190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongLinearLeastSquaresProblem* LinearLeastSquaresProblem3() { 4200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int num_rows = 5; 4210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int num_cols = 2; 4220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong LinearLeastSquaresProblem* problem = new LinearLeastSquaresProblem; 4240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem->b.reset(new double[num_rows]); 4260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem->D.reset(new double[num_cols]); 4270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem->num_eliminate_blocks = 2; 4280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CompressedRowBlockStructure* bs = new CompressedRowBlockStructure; 4300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong scoped_array<double> values(new double[num_rows * num_cols]); 4310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int c = 0; c < num_cols; ++c) { 4330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong bs->cols.push_back(Block()); 4340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong bs->cols.back().size = 1; 4350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong bs->cols.back().position = c; 4360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 4370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int nnz = 0; 4390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Row 1 4410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong { 4420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong values[nnz++] = 1; 4430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong bs->rows.push_back(CompressedRow()); 4440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CompressedRow& row = bs->rows.back(); 4450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong row.block.size = 1; 4460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong row.block.position = 0; 4470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong row.cells.push_back(Cell(0, 0)); 4480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 4490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Row 2 4510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong { 4520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong values[nnz++] = 3; 4530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong bs->rows.push_back(CompressedRow()); 4540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CompressedRow& row = bs->rows.back(); 4550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong row.block.size = 1; 4560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong row.block.position = 1; 4570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong row.cells.push_back(Cell(0, 1)); 4580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 4590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Row 3 4610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong { 4620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong values[nnz++] = 5; 4630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong bs->rows.push_back(CompressedRow()); 4640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CompressedRow& row = bs->rows.back(); 4650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong row.block.size = 1; 4660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong row.block.position = 2; 4670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong row.cells.push_back(Cell(1, 2)); 4680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 4690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Row 4 4710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong { 4720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong values[nnz++] = 7; 4730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong bs->rows.push_back(CompressedRow()); 4740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CompressedRow& row = bs->rows.back(); 4750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong row.block.size = 1; 4760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong row.block.position = 3; 4770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong row.cells.push_back(Cell(1, 3)); 4780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 4790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Row 5 4810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong { 4820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong values[nnz++] = 9; 4830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong bs->rows.push_back(CompressedRow()); 4840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CompressedRow& row = bs->rows.back(); 4850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong row.block.size = 1; 4860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong row.block.position = 4; 4870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong row.cells.push_back(Cell(1, 4)); 4880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 4890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong BlockSparseMatrix* A = new BlockSparseMatrix(bs); 4910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong memcpy(A->mutable_values(), values.get(), nnz * sizeof(*A->values())); 4920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_cols; ++i) { 4940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem->D.get()[i] = 1; 4950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 4960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_rows; ++i) { 4980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem->b.get()[i] = i; 4990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 5000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 5010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem->A.reset(A); 5020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 5030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return problem; 5040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 5050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 5061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingnamespace { 5071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingbool DumpLinearLeastSquaresProblemToConsole(const SparseMatrix* A, 5080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const double* D, 5090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const double* b, 5100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const double* x, 5110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int num_eliminate_blocks) { 5120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_NOTNULL(A); 5130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Matrix AA; 5140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong A->ToDenseMatrix(&AA); 5150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong LOG(INFO) << "A^T: \n" << AA.transpose(); 5160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 5170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (D != NULL) { 5180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong LOG(INFO) << "A's appended diagonal:\n" 5190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << ConstVectorRef(D, A->num_cols()); 5200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 5210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 5220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (b != NULL) { 5230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong LOG(INFO) << "b: \n" << ConstVectorRef(b, A->num_rows()); 5240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 5250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 5260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (x != NULL) { 5270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong LOG(INFO) << "x: \n" << ConstVectorRef(x, A->num_cols()); 5280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 5290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return true; 5300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong}; 5310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 5320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid WriteArrayToFileOrDie(const string& filename, 5330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const double* x, 5340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int size) { 5350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_NOTNULL(x); 5360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong VLOG(2) << "Writing array to: " << filename; 5370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong FILE* fptr = fopen(filename.c_str(), "w"); 5380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_NOTNULL(fptr); 5390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < size; ++i) { 5400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong fprintf(fptr, "%17f\n", x[i]); 5410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 5420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong fclose(fptr); 5430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 5440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 5451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingbool DumpLinearLeastSquaresProblemToTextFile(const string& filename_base, 5460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const SparseMatrix* A, 5470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const double* D, 5480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const double* b, 5490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const double* x, 5500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int num_eliminate_blocks) { 5510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_NOTNULL(A); 5521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling LOG(INFO) << "writing to: " << filename_base << "*"; 5530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 5540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong string matlab_script; 5550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong StringAppendF(&matlab_script, 5561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling "function lsqp = load_trust_region_problem()\n"); 5570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong StringAppendF(&matlab_script, 5580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong "lsqp.num_rows = %d;\n", A->num_rows()); 5590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong StringAppendF(&matlab_script, 5600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong "lsqp.num_cols = %d;\n", A->num_cols()); 5610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 5620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong { 5631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling string filename = filename_base + "_A.txt"; 5640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong FILE* fptr = fopen(filename.c_str(), "w"); 5650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_NOTNULL(fptr); 5660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong A->ToTextFile(fptr); 5670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong fclose(fptr); 5680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong StringAppendF(&matlab_script, 5690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong "tmp = load('%s', '-ascii');\n", filename.c_str()); 5700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong StringAppendF( 5710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong &matlab_script, 5720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong "lsqp.A = sparse(tmp(:, 1) + 1, tmp(:, 2) + 1, tmp(:, 3), %d, %d);\n", 5730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong A->num_rows(), 5740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong A->num_cols()); 5750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 5760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 5770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 5780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (D != NULL) { 5791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling string filename = filename_base + "_D.txt"; 5800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong WriteArrayToFileOrDie(filename, D, A->num_cols()); 5810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong StringAppendF(&matlab_script, 5820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong "lsqp.D = load('%s', '-ascii');\n", filename.c_str()); 5830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 5840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 5850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (b != NULL) { 5861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling string filename = filename_base + "_b.txt"; 5870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong WriteArrayToFileOrDie(filename, b, A->num_rows()); 5880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong StringAppendF(&matlab_script, 5890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong "lsqp.b = load('%s', '-ascii');\n", filename.c_str()); 5900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 5910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 5920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (x != NULL) { 5931d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling string filename = filename_base + "_x.txt"; 5940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong WriteArrayToFileOrDie(filename, x, A->num_cols()); 5950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong StringAppendF(&matlab_script, 5960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong "lsqp.x = load('%s', '-ascii');\n", filename.c_str()); 5970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 5980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 5991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling string matlab_filename = filename_base + ".m"; 6000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong WriteStringToFileOrDie(matlab_script, matlab_filename); 6010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return true; 6020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 6031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} // namespace 6040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 6051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingbool DumpLinearLeastSquaresProblem(const string& filename_base, 6060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong DumpFormatType dump_format_type, 6070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const SparseMatrix* A, 6080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const double* D, 6090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const double* b, 6100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const double* x, 6110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int num_eliminate_blocks) { 6120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong switch (dump_format_type) { 6131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling case CONSOLE: 6141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return DumpLinearLeastSquaresProblemToConsole(A, D, b, x, 6150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_eliminate_blocks); 6161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling case TEXTFILE: 6171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return DumpLinearLeastSquaresProblemToTextFile(filename_base, 6180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong A, D, b, x, 6190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_eliminate_blocks); 6200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong default: 6210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong LOG(FATAL) << "Unknown DumpFormatType " << dump_format_type; 6220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong }; 6230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 6240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return true; 6250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 6260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 6270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace internal 6280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace ceres 629