← Back to branch summary

~ubuntu-branches/ubuntu/raring/ceres-solver/raring

« back to all changes in this revision

Viewing changes to internal/ceres/linear_least_squares_problems.cc

  • Committer: Package Import Robot
  • Author(s): Koichi Akabe
  • Date: 2012-06-04 07:15:43 UTC
  • Revision ID: package-import@ubuntu.com-20120604071543-zx6uthupvmtqn3k2
Tags: upstream-1.1.1
Import upstream version 1.1.1

Show diffs side-by-side

added added

removed removed

Lines of Context:
internal/ceres/linear_least_squares_problems.cc
 
1
// Ceres Solver - A fast non-linear least squares minimizer
 
2
// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
 
3
// http://code.google.com/p/ceres-solver/
 
4
//
 
5
// Redistribution and use in source and binary forms, with or without
 
6
// modification, are permitted provided that the following conditions are met:
 
7
//
 
8
// * Redistributions of source code must retain the above copyright notice,
 
9
//   this list of conditions and the following disclaimer.
 
10
// * Redistributions in binary form must reproduce the above copyright notice,
 
11
//   this list of conditions and the following disclaimer in the documentation
 
12
//   and/or other materials provided with the distribution.
 
13
// * Neither the name of Google Inc. nor the names of its contributors may be
 
14
//   used to endorse or promote products derived from this software without
 
15
//   specific prior written permission.
 
16
//
 
17
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 
18
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 
19
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 
20
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 
21
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 
22
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 
23
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 
24
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 
25
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 
26
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 
27
// POSSIBILITY OF SUCH DAMAGE.
 
28
//
 
29
// Author: sameeragarwal@google.com (Sameer Agarwal)
 
30
 
 
31
#include "ceres/linear_least_squares_problems.h"
 
32
 
 
33
#include <cstdio>
 
34
#include <string>
 
35
#include <vector>
 
36
#include <glog/logging.h>
 
37
#include "ceres/block_sparse_matrix.h"
 
38
#include "ceres/block_structure.h"
 
39
#include "ceres/casts.h"
 
40
#include "ceres/compressed_row_sparse_matrix.h"
 
41
#include "ceres/file.h"
 
42
#include "ceres/matrix_proto.h"
 
43
#include "ceres/triplet_sparse_matrix.h"
 
44
#include "ceres/stringprintf.h"
 
45
#include "ceres/internal/scoped_ptr.h"
 
46
#include "ceres/types.h"
 
47
 
 
48
namespace ceres {
 
49
namespace internal {
 
50
 
 
51
LinearLeastSquaresProblem* CreateLinearLeastSquaresProblemFromId(int id) {
 
52
  switch (id) {
 
53
    case 0:
 
54
      return LinearLeastSquaresProblem0();
 
55
    case 1:
 
56
      return LinearLeastSquaresProblem1();
 
57
    case 2:
 
58
      return LinearLeastSquaresProblem2();
 
59
    case 3:
 
60
      return LinearLeastSquaresProblem3();
 
61
    default:
 
62
      LOG(FATAL) << "Unknown problem id requested " << id;
 
63
  }
 
64
}
 
65
 
 
66
#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS
 
67
LinearLeastSquaresProblem* CreateLinearLeastSquaresProblemFromFile(
 
68
    const string& filename) {
 
69
  LinearLeastSquaresProblemProto problem_proto;
 
70
  {
 
71
    string serialized_proto;
 
72
    ReadFileToStringOrDie(filename, &serialized_proto);
 
73
    CHECK(problem_proto.ParseFromString(serialized_proto));
 
74
  }
 
75
 
 
76
  LinearLeastSquaresProblem* problem = new LinearLeastSquaresProblem;
 
77
  const SparseMatrixProto& A = problem_proto.a();
 
78
 
 
79
  if (A.has_block_matrix()) {
 
80
    problem->A.reset(new BlockSparseMatrix(A));
 
81
  } else if (A.has_triplet_matrix()) {
 
82
    problem->A.reset(new TripletSparseMatrix(A));
 
83
  } else {
 
84
    problem->A.reset(new CompressedRowSparseMatrix(A));
 
85
  }
 
86
 
 
87
  if (problem_proto.b_size() > 0) {
 
88
    problem->b.reset(new double[problem_proto.b_size()]);
 
89
    for (int i = 0; i < problem_proto.b_size(); ++i) {
 
90
      problem->b[i] = problem_proto.b(i);
 
91
    }
 
92
  }
 
93
 
 
94
  if (problem_proto.d_size() > 0) {
 
95
    problem->D.reset(new double[problem_proto.d_size()]);
 
96
    for (int i = 0; i < problem_proto.d_size(); ++i) {
 
97
      problem->D[i] = problem_proto.d(i);
 
98
    }
 
99
  }
 
100
 
 
101
  if (problem_proto.d_size() > 0) {
 
102
    if (problem_proto.x_size() > 0) {
 
103
      problem->x_D.reset(new double[problem_proto.x_size()]);
 
104
      for (int i = 0; i < problem_proto.x_size(); ++i) {
 
105
        problem->x_D[i] = problem_proto.x(i);
 
106
      }
 
107
    }
 
108
  } else {
 
109
    if (problem_proto.x_size() > 0) {
 
110
      problem->x.reset(new double[problem_proto.x_size()]);
 
111
      for (int i = 0; i < problem_proto.x_size(); ++i) {
 
112
        problem->x[i] = problem_proto.x(i);
 
113
      }
 
114
    }
 
115
  }
 
116
 
 
117
  problem->num_eliminate_blocks = 0;
 
118
  if (problem_proto.has_num_eliminate_blocks()) {
 
119
    problem->num_eliminate_blocks = problem_proto.num_eliminate_blocks();
 
120
  }
 
121
 
 
122
  return problem;
 
123
}
 
124
#else
 
125
LinearLeastSquaresProblem* CreateLinearLeastSquaresProblemFromFile(
 
126
    const string& filename) {
 
127
  LOG(FATAL)
 
128
      << "Loading a least squares problem from disk requires "
 
129
      << "Ceres to be built with Protocol Buffers support.";
 
130
  return NULL;
 
131
}
 
132
#endif  // CERES_DONT_HAVE_PROTOCOL_BUFFERS
 
133
 
 
134
/*
 
135
A = [1   2]
 
136
    [3   4]
 
137
    [6 -10]
 
138
 
 
139
b = [  8
 
140
      18
 
141
     -18]
 
142
 
 
143
x = [2
 
144
     3]
 
145
 
 
146
D = [1
 
147
     2]
 
148
 
 
149
x_D = [1.78448275;
 
150
       2.82327586;]
 
151
 */
 
152
LinearLeastSquaresProblem* LinearLeastSquaresProblem0() {
 
153
  LinearLeastSquaresProblem* problem = new LinearLeastSquaresProblem;
 
154
 
 
155
  TripletSparseMatrix* A = new TripletSparseMatrix(3, 2, 6);
 
156
  problem->b.reset(new double[3]);
 
157
  problem->D.reset(new double[2]);
 
158
 
 
159
  problem->x.reset(new double[2]);
 
160
  problem->x_D.reset(new double[2]);
 
161
 
 
162
  int* Ai = A->mutable_rows();
 
163
  int* Aj = A->mutable_cols();
 
164
  double* Ax = A->mutable_values();
 
165
 
 
166
  int counter = 0;
 
167
  for (int i = 0; i < 3; ++i) {
 
168
    for (int j = 0; j< 2; ++j) {
 
169
      Ai[counter]=i;
 
170
      Aj[counter]=j;
 
171
      ++counter;
 
172
    }
 
173
  };
 
174
 
 
175
  Ax[0] = 1.;
 
176
  Ax[1] = 2.;
 
177
  Ax[2] = 3.;
 
178
  Ax[3] = 4.;
 
179
  Ax[4] = 6;
 
180
  Ax[5] = -10;
 
181
  A->set_num_nonzeros(6);
 
182
  problem->A.reset(A);
 
183
 
 
184
  problem->b[0] = 8;
 
185
  problem->b[1] = 18;
 
186
  problem->b[2] = -18;
 
187
 
 
188
  problem->x[0] = 2.0;
 
189
  problem->x[1] = 3.0;
 
190
 
 
191
  problem->D[0] = 1;
 
192
  problem->D[1] = 2;
 
193
 
 
194
  problem->x_D[0] = 1.78448275;
 
195
  problem->x_D[1] = 2.82327586;
 
196
  return problem;
 
197
}
 
198
 
 
199
 
 
200
/*
 
201
      A = [1 0  | 2 0 0
 
202
           3 0  | 0 4 0
 
203
           0 5  | 0 0 6
 
204
           0 7  | 8 0 0
 
205
           0 9  | 1 0 0
 
206
           0 0  | 1 1 1]
 
207
 
 
208
      b = [0
 
209
           1
 
210
           2
 
211
           3
 
212
           4
 
213
           5]
 
214
 
 
215
      c = A'* b = [ 3
 
216
                   67
 
217
                   33
 
218
                    9
 
219
                   17]
 
220
 
 
221
      A'A = [10    0    2   12   0
 
222
              0  155   65    0  30
 
223
              2   65   70    1   1
 
224
             12    0    1   17   1
 
225
              0   30    1    1  37]
 
226
 
 
227
      S = [ 42.3419  -1.4000  -11.5806
 
228
            -1.4000   2.6000    1.0000
 
229
            11.5806   1.0000   31.1935]
 
230
 
 
231
      r = [ 4.3032
 
232
            5.4000
 
233
            5.0323]
 
234
 
 
235
      S\r = [ 0.2102
 
236
              2.1367
 
237
              0.1388]
 
238
 
 
239
      A\b = [-2.3061
 
240
              0.3172
 
241
              0.2102
 
242
              2.1367
 
243
              0.1388]
 
244
*/
 
245
// The following two functions create a TripletSparseMatrix and a
 
246
// BlockSparseMatrix version of this problem.
 
247
 
 
248
// TripletSparseMatrix version.
 
249
LinearLeastSquaresProblem* LinearLeastSquaresProblem1() {
 
250
  int num_rows = 6;
 
251
  int num_cols = 5;
 
252
 
 
253
  LinearLeastSquaresProblem* problem = new LinearLeastSquaresProblem;
 
254
  TripletSparseMatrix* A = new TripletSparseMatrix(num_rows,
 
255
                                                   num_cols,
 
256
                                                   num_rows * num_cols);
 
257
  problem->b.reset(new double[num_rows]);
 
258
  problem->D.reset(new double[num_cols]);
 
259
  problem->num_eliminate_blocks = 2;
 
260
 
 
261
  int* rows = A->mutable_rows();
 
262
  int* cols = A->mutable_cols();
 
263
  double* values = A->mutable_values();
 
264
 
 
265
  int nnz = 0;
 
266
 
 
267
  // Row 1
 
268
  {
 
269
    rows[nnz] = 0;
 
270
    cols[nnz] = 0;
 
271
    values[nnz++] = 1;
 
272
 
 
273
    rows[nnz] = 0;
 
274
    cols[nnz] = 2;
 
275
    values[nnz++] = 2;
 
276
  }
 
277
 
 
278
  // Row 2
 
279
  {
 
280
    rows[nnz] = 1;
 
281
    cols[nnz] = 0;
 
282
    values[nnz++] = 3;
 
283
 
 
284
    rows[nnz] = 1;
 
285
    cols[nnz] = 3;
 
286
    values[nnz++] = 4;
 
287
  }
 
288
 
 
289
  // Row 3
 
290
  {
 
291
    rows[nnz] = 2;
 
292
    cols[nnz] = 1;
 
293
    values[nnz++] = 5;
 
294
 
 
295
    rows[nnz] = 2;
 
296
    cols[nnz] = 4;
 
297
    values[nnz++] = 6;
 
298
  }
 
299
 
 
300
  // Row 4
 
301
  {
 
302
    rows[nnz] = 3;
 
303
    cols[nnz] = 1;
 
304
    values[nnz++] = 7;
 
305
 
 
306
    rows[nnz] = 3;
 
307
    cols[nnz] = 2;
 
308
    values[nnz++] = 8;
 
309
  }
 
310
 
 
311
  // Row 5
 
312
  {
 
313
    rows[nnz] = 4;
 
314
    cols[nnz] = 1;
 
315
    values[nnz++] = 9;
 
316
 
 
317
    rows[nnz] = 4;
 
318
    cols[nnz] = 2;
 
319
    values[nnz++] = 1;
 
320
  }
 
321
 
 
322
  // Row 6
 
323
  {
 
324
    rows[nnz] = 5;
 
325
    cols[nnz] = 2;
 
326
    values[nnz++] = 1;
 
327
 
 
328
    rows[nnz] = 5;
 
329
    cols[nnz] = 3;
 
330
    values[nnz++] = 1;
 
331
 
 
332
    rows[nnz] = 5;
 
333
    cols[nnz] = 4;
 
334
    values[nnz++] = 1;
 
335
  }
 
336
 
 
337
  A->set_num_nonzeros(nnz);
 
338
  CHECK(A->IsValid());
 
339
 
 
340
  problem->A.reset(A);
 
341
 
 
342
  for (int i = 0; i < num_cols; ++i) {
 
343
    problem->D.get()[i] = 1;
 
344
  }
 
345
 
 
346
  for (int i = 0; i < num_rows; ++i) {
 
347
    problem->b.get()[i] = i;
 
348
  }
 
349
 
 
350
  return problem;
 
351
}
 
352
 
 
353
// BlockSparseMatrix version
 
354
LinearLeastSquaresProblem* LinearLeastSquaresProblem2() {
 
355
  int num_rows = 6;
 
356
  int num_cols = 5;
 
357
 
 
358
  LinearLeastSquaresProblem* problem = new LinearLeastSquaresProblem;
 
359
 
 
360
  problem->b.reset(new double[num_rows]);
 
361
  problem->D.reset(new double[num_cols]);
 
362
  problem->num_eliminate_blocks = 2;
 
363
 
 
364
  CompressedRowBlockStructure* bs = new CompressedRowBlockStructure;
 
365
  scoped_array<double> values(new double[num_rows * num_cols]);
 
366
 
 
367
  for (int c = 0; c < num_cols; ++c) {
 
368
    bs->cols.push_back(Block());
 
369
    bs->cols.back().size = 1;
 
370
    bs->cols.back().position = c;
 
371
  }
 
372
 
 
373
  int nnz = 0;
 
374
 
 
375
  // Row 1
 
376
  {
 
377
    values[nnz++] = 1;
 
378
    values[nnz++] = 2;
 
379
 
 
380
    bs->rows.push_back(CompressedRow());
 
381
    CompressedRow& row = bs->rows.back();
 
382
    row.block.size = 1;
 
383
    row.block.position = 0;
 
384
    row.cells.push_back(Cell(0, 0));
 
385
    row.cells.push_back(Cell(2, 1));
 
386
  }
 
387
 
 
388
  // Row 2
 
389
  {
 
390
    values[nnz++] = 3;
 
391
    values[nnz++] = 4;
 
392
 
 
393
    bs->rows.push_back(CompressedRow());
 
394
    CompressedRow& row = bs->rows.back();
 
395
    row.block.size = 1;
 
396
    row.block.position = 1;
 
397
    row.cells.push_back(Cell(0, 2));
 
398
    row.cells.push_back(Cell(3, 3));
 
399
  }
 
400
 
 
401
  // Row 3
 
402
  {
 
403
    values[nnz++] = 5;
 
404
    values[nnz++] = 6;
 
405
 
 
406
    bs->rows.push_back(CompressedRow());
 
407
    CompressedRow& row = bs->rows.back();
 
408
    row.block.size = 1;
 
409
    row.block.position = 2;
 
410
    row.cells.push_back(Cell(1, 4));
 
411
    row.cells.push_back(Cell(4, 5));
 
412
  }
 
413
 
 
414
  // Row 4
 
415
  {
 
416
    values[nnz++] = 7;
 
417
    values[nnz++] = 8;
 
418
 
 
419
    bs->rows.push_back(CompressedRow());
 
420
    CompressedRow& row = bs->rows.back();
 
421
    row.block.size = 1;
 
422
    row.block.position = 3;
 
423
    row.cells.push_back(Cell(1, 6));
 
424
    row.cells.push_back(Cell(2, 7));
 
425
  }
 
426
 
 
427
  // Row 5
 
428
  {
 
429
    values[nnz++] = 9;
 
430
    values[nnz++] = 1;
 
431
 
 
432
    bs->rows.push_back(CompressedRow());
 
433
    CompressedRow& row = bs->rows.back();
 
434
    row.block.size = 1;
 
435
    row.block.position = 4;
 
436
    row.cells.push_back(Cell(1, 8));
 
437
    row.cells.push_back(Cell(2, 9));
 
438
  }
 
439
 
 
440
  // Row 6
 
441
  {
 
442
    values[nnz++] = 1;
 
443
    values[nnz++] = 1;
 
444
    values[nnz++] = 1;
 
445
 
 
446
    bs->rows.push_back(CompressedRow());
 
447
    CompressedRow& row = bs->rows.back();
 
448
    row.block.size = 1;
 
449
    row.block.position = 5;
 
450
    row.cells.push_back(Cell(2, 10));
 
451
    row.cells.push_back(Cell(3, 11));
 
452
    row.cells.push_back(Cell(4, 12));
 
453
  }
 
454
 
 
455
  BlockSparseMatrix* A = new BlockSparseMatrix(bs);
 
456
  memcpy(A->mutable_values(), values.get(), nnz * sizeof(*A->values()));
 
457
 
 
458
  for (int i = 0; i < num_cols; ++i) {
 
459
    problem->D.get()[i] = 1;
 
460
  }
 
461
 
 
462
  for (int i = 0; i < num_rows; ++i) {
 
463
    problem->b.get()[i] = i;
 
464
  }
 
465
 
 
466
  problem->A.reset(A);
 
467
 
 
468
  return problem;
 
469
}
 
470
 
 
471
 
 
472
/*
 
473
      A = [1 0
 
474
           3 0
 
475
           0 5
 
476
           0 7
 
477
           0 9
 
478
           0 0]
 
479
 
 
480
      b = [0
 
481
           1
 
482
           2
 
483
           3
 
484
           4
 
485
           5]
 
486
*/
 
487
// BlockSparseMatrix version
 
488
LinearLeastSquaresProblem* LinearLeastSquaresProblem3() {
 
489
  int num_rows = 5;
 
490
  int num_cols = 2;
 
491
 
 
492
  LinearLeastSquaresProblem* problem = new LinearLeastSquaresProblem;
 
493
 
 
494
  problem->b.reset(new double[num_rows]);
 
495
  problem->D.reset(new double[num_cols]);
 
496
  problem->num_eliminate_blocks = 2;
 
497
 
 
498
  CompressedRowBlockStructure* bs = new CompressedRowBlockStructure;
 
499
  scoped_array<double> values(new double[num_rows * num_cols]);
 
500
 
 
501
  for (int c = 0; c < num_cols; ++c) {
 
502
    bs->cols.push_back(Block());
 
503
    bs->cols.back().size = 1;
 
504
    bs->cols.back().position = c;
 
505
  }
 
506
 
 
507
  int nnz = 0;
 
508
 
 
509
  // Row 1
 
510
  {
 
511
    values[nnz++] = 1;
 
512
    bs->rows.push_back(CompressedRow());
 
513
    CompressedRow& row = bs->rows.back();
 
514
    row.block.size = 1;
 
515
    row.block.position = 0;
 
516
    row.cells.push_back(Cell(0, 0));
 
517
  }
 
518
 
 
519
  // Row 2
 
520
  {
 
521
    values[nnz++] = 3;
 
522
    bs->rows.push_back(CompressedRow());
 
523
    CompressedRow& row = bs->rows.back();
 
524
    row.block.size = 1;
 
525
    row.block.position = 1;
 
526
    row.cells.push_back(Cell(0, 1));
 
527
  }
 
528
 
 
529
  // Row 3
 
530
  {
 
531
    values[nnz++] = 5;
 
532
    bs->rows.push_back(CompressedRow());
 
533
    CompressedRow& row = bs->rows.back();
 
534
    row.block.size = 1;
 
535
    row.block.position = 2;
 
536
    row.cells.push_back(Cell(1, 2));
 
537
  }
 
538
 
 
539
  // Row 4
 
540
  {
 
541
    values[nnz++] = 7;
 
542
    bs->rows.push_back(CompressedRow());
 
543
    CompressedRow& row = bs->rows.back();
 
544
    row.block.size = 1;
 
545
    row.block.position = 3;
 
546
    row.cells.push_back(Cell(1, 3));
 
547
  }
 
548
 
 
549
  // Row 5
 
550
  {
 
551
    values[nnz++] = 9;
 
552
    bs->rows.push_back(CompressedRow());
 
553
    CompressedRow& row = bs->rows.back();
 
554
    row.block.size = 1;
 
555
    row.block.position = 4;
 
556
    row.cells.push_back(Cell(1, 4));
 
557
  }
 
558
 
 
559
  BlockSparseMatrix* A = new BlockSparseMatrix(bs);
 
560
  memcpy(A->mutable_values(), values.get(), nnz * sizeof(*A->values()));
 
561
 
 
562
  for (int i = 0; i < num_cols; ++i) {
 
563
    problem->D.get()[i] = 1;
 
564
  }
 
565
 
 
566
  for (int i = 0; i < num_rows; ++i) {
 
567
    problem->b.get()[i] = i;
 
568
  }
 
569
 
 
570
  problem->A.reset(A);
 
571
 
 
572
  return problem;
 
573
}
 
574
 
 
575
bool DumpLinearLeastSquaresProblemToConsole(const string& directory,
 
576
                                            int iteration,
 
577
                                            const SparseMatrix* A,
 
578
                                            const double* D,
 
579
                                            const double* b,
 
580
                                            const double* x,
 
581
                                            int num_eliminate_blocks) {
 
582
  CHECK_NOTNULL(A);
 
583
  Matrix AA;
 
584
  A->ToDenseMatrix(&AA);
 
585
  LOG(INFO) << "A^T: \n" << AA.transpose();
 
586
 
 
587
  if (D != NULL) {
 
588
    LOG(INFO) << "A's appended diagonal:\n"
 
589
              << ConstVectorRef(D, A->num_cols());
 
590
  }
 
591
 
 
592
  if (b != NULL) {
 
593
    LOG(INFO) << "b: \n" << ConstVectorRef(b, A->num_rows());
 
594
  }
 
595
 
 
596
  if (x != NULL) {
 
597
    LOG(INFO) << "x: \n" << ConstVectorRef(x, A->num_cols());
 
598
  }
 
599
  return true;
 
600
};
 
601
 
 
602
#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS
 
603
bool DumpLinearLeastSquaresProblemToProtocolBuffer(const string& directory,
 
604
                                                   int iteration,
 
605
                                                   const SparseMatrix* A,
 
606
                                                   const double* D,
 
607
                                                   const double* b,
 
608
                                                   const double* x,
 
609
                                                   int num_eliminate_blocks) {
 
610
  CHECK_NOTNULL(A);
 
611
  LinearLeastSquaresProblemProto lsqp;
 
612
  A->ToProto(lsqp.mutable_a());
 
613
 
 
614
  if (D != NULL) {
 
615
    for (int i = 0; i < A->num_cols(); ++i) {
 
616
      lsqp.add_d(D[i]);
 
617
    }
 
618
  }
 
619
 
 
620
  if (b != NULL) {
 
621
    for (int i = 0; i < A->num_rows(); ++i) {
 
622
      lsqp.add_b(b[i]);
 
623
    }
 
624
  }
 
625
 
 
626
  if (x != NULL) {
 
627
    for (int i = 0; i < A->num_cols(); ++i) {
 
628
      lsqp.add_x(x[i]);
 
629
    }
 
630
  }
 
631
 
 
632
  lsqp.set_num_eliminate_blocks(num_eliminate_blocks);
 
633
  string format_string = JoinPath(directory,
 
634
                                  "lm_iteration_%03d.lsqp");
 
635
  string filename =
 
636
      StringPrintf(format_string.c_str(),  iteration);
 
637
  LOG(INFO) << "Dumping least squares problem for iteration " << iteration
 
638
            << " to disk. File: " << filename;
 
639
  WriteStringToFileOrDie(lsqp.SerializeAsString(), filename);
 
640
  return true;
 
641
}
 
642
#else
 
643
bool DumpLinearLeastSquaresProblemToProtocolBuffer(const string& directory,
 
644
                                                   int iteration,
 
645
                                                   const SparseMatrix* A,
 
646
                                                   const double* D,
 
647
                                                   const double* b,
 
648
                                                   const double* x,
 
649
                                                   int num_eliminate_blocks) {
 
650
  LOG(ERROR) << "Dumping least squares problems is only "
 
651
             << "supported when Ceres is compiled with "
 
652
             << "protocol buffer support.";
 
653
  return false;
 
654
}
 
655
#endif
 
656
 
 
657
void WriteArrayToFileOrDie(const string& filename,
 
658
                           const double* x,
 
659
                           const int size) {
 
660
  CHECK_NOTNULL(x);
 
661
  VLOG(2) << "Writing array to: " << filename;
 
662
  FILE* fptr = fopen(filename.c_str(), "w");
 
663
  CHECK_NOTNULL(fptr);
 
664
  for (int i = 0; i < size; ++i) {
 
665
    fprintf(fptr, "%17f\n", x[i]);
 
666
  }
 
667
  fclose(fptr);
 
668
}
 
669
 
 
670
bool DumpLinearLeastSquaresProblemToTextFile(const string& directory,
 
671
                                             int iteration,
 
672
                                             const SparseMatrix* A,
 
673
                                             const double* D,
 
674
                                             const double* b,
 
675
                                             const double* x,
 
676
                                             int num_eliminate_blocks) {
 
677
  CHECK_NOTNULL(A);
 
678
  string format_string = JoinPath(directory,
 
679
                                  "lm_iteration_%03d");
 
680
  string filename_prefix =
 
681
      StringPrintf(format_string.c_str(), iteration);
 
682
 
 
683
  LOG(INFO) << "writing to: " << filename_prefix << "*";
 
684
 
 
685
  string matlab_script;
 
686
  StringAppendF(&matlab_script,
 
687
                "function lsqp = lm_iteration_%03d()\n",
 
688
                iteration,
 
689
                iteration);
 
690
 
 
691
  StringAppendF(&matlab_script,
 
692
                "lsqp.num_rows = %d;\n", A->num_rows());
 
693
  StringAppendF(&matlab_script,
 
694
                "lsqp.num_cols = %d;\n", A->num_cols());
 
695
 
 
696
  {
 
697
    string filename = filename_prefix + "_A.txt";
 
698
    FILE* fptr = fopen(filename.c_str(), "w");
 
699
    CHECK_NOTNULL(fptr);
 
700
    A->ToTextFile(fptr);
 
701
    fclose(fptr);
 
702
    StringAppendF(&matlab_script,
 
703
                  "tmp = load('%s', '-ascii');\n", filename.c_str());
 
704
    StringAppendF(
 
705
        &matlab_script,
 
706
        "lsqp.A = sparse(tmp(:, 1) + 1, tmp(:, 2) + 1, tmp(:, 3), %d, %d);\n",
 
707
        A->num_rows(),
 
708
        A->num_cols());
 
709
  }
 
710
 
 
711
 
 
712
  if (D != NULL) {
 
713
    string filename = filename_prefix + "_D.txt";
 
714
    WriteArrayToFileOrDie(filename, D, A->num_cols());
 
715
    StringAppendF(&matlab_script,
 
716
                  "lsqp.D = load('%s', '-ascii');\n", filename.c_str());
 
717
  }
 
718
 
 
719
  if (b != NULL) {
 
720
    string filename = filename_prefix + "_b.txt";
 
721
    WriteArrayToFileOrDie(filename, b, A->num_rows());
 
722
    StringAppendF(&matlab_script,
 
723
                  "lsqp.b = load('%s', '-ascii');\n", filename.c_str());
 
724
  }
 
725
 
 
726
  if (x != NULL) {
 
727
    string filename = filename_prefix + "_x.txt";
 
728
    WriteArrayToFileOrDie(filename, x, A->num_cols());
 
729
    StringAppendF(&matlab_script,
 
730
                  "lsqp.x = load('%s', '-ascii');\n", filename.c_str());
 
731
  }
 
732
 
 
733
  string matlab_filename = filename_prefix + ".m";
 
734
  WriteStringToFileOrDie(matlab_script, matlab_filename);
 
735
  return true;
 
736
}
 
737
 
 
738
bool DumpLinearLeastSquaresProblem(const string& directory,
 
739
                                   int iteration,
 
740
                                   DumpFormatType dump_format_type,
 
741
                                   const SparseMatrix* A,
 
742
                                   const double* D,
 
743
                                   const double* b,
 
744
                                   const double* x,
 
745
                                   int num_eliminate_blocks) {
 
746
  switch (dump_format_type) {
 
747
    case (CONSOLE):
 
748
      return DumpLinearLeastSquaresProblemToConsole(directory,
 
749
                                                    iteration,
 
750
                                                    A, D, b, x,
 
751
                                                    num_eliminate_blocks);
 
752
    case (PROTOBUF):
 
753
      return DumpLinearLeastSquaresProblemToProtocolBuffer(
 
754
          directory,
 
755
          iteration,
 
756
          A, D, b, x,
 
757
          num_eliminate_blocks);
 
758
    case (TEXTFILE):
 
759
      return DumpLinearLeastSquaresProblemToTextFile(directory,
 
760
                                                     iteration,
 
761
                                                     A, D, b, x,
 
762
                                                     num_eliminate_blocks);
 
763
    default:
 
764
      LOG(FATAL) << "Unknown DumpFormatType " << dump_format_type;
 
765
  };
 
766
 
 
767
  return true;
 
768
}
 
769
 
 
770
}  // namespace internal
 
771
}  // namespace ceres