← Back to branch summary

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

« back to all changes in this revision

Viewing changes to internal/ceres/block_random_access_sparse_matrix.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/block_random_access_sparse_matrix.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/block_random_access_sparse_matrix.h"
 
32
 
 
33
#include <algorithm>
 
34
#include <set>
 
35
#include <utility>
 
36
#include <vector>
 
37
#include <glog/logging.h>
 
38
#include "ceres/mutex.h"
 
39
#include "ceres/triplet_sparse_matrix.h"
 
40
#include "ceres/internal/port.h"
 
41
#include "ceres/internal/scoped_ptr.h"
 
42
#include "ceres/types.h"
 
43
 
 
44
namespace ceres {
 
45
namespace internal {
 
46
 
 
47
BlockRandomAccessSparseMatrix::BlockRandomAccessSparseMatrix(
 
48
    const vector<int>& blocks,
 
49
    const set<pair<int, int> >& block_pairs)
 
50
    : kMaxRowBlocks(10 * 1000 * 1000),
 
51
      blocks_(blocks) {
 
52
  CHECK_LT(blocks.size(), kMaxRowBlocks);
 
53
 
 
54
  // Build the row/column layout vector and count the number of scalar
 
55
  // rows/columns.
 
56
  int num_cols = 0;
 
57
  vector<int> col_layout;
 
58
  for (int i = 0; i < blocks_.size(); ++i) {
 
59
    col_layout.push_back(num_cols);
 
60
    num_cols += blocks_[i];
 
61
  }
 
62
 
 
63
  // Count the number of scalar non-zero entries and build the layout
 
64
  // object for looking into the values array of the
 
65
  // TripletSparseMatrix.
 
66
  int num_nonzeros = 0;
 
67
  for (set<pair<int, int> >::const_iterator it = block_pairs.begin();
 
68
       it != block_pairs.end();
 
69
       ++it) {
 
70
    const int row_block_size = blocks_[it->first];
 
71
    const int col_block_size = blocks_[it->second];
 
72
    num_nonzeros += row_block_size * col_block_size;
 
73
  }
 
74
 
 
75
  VLOG(1) << "Matrix Size [" << num_cols
 
76
          << "," << num_cols
 
77
          << "] " << num_nonzeros;
 
78
 
 
79
  tsm_.reset(new TripletSparseMatrix(num_cols, num_cols, num_nonzeros));
 
80
  tsm_->set_num_nonzeros(num_nonzeros);
 
81
  int* rows = tsm_->mutable_rows();
 
82
  int* cols = tsm_->mutable_cols();
 
83
  double* values = tsm_->mutable_values();
 
84
 
 
85
  int pos = 0;
 
86
  for (set<pair<int, int> >::const_iterator it = block_pairs.begin();
 
87
       it != block_pairs.end();
 
88
       ++it) {
 
89
    const int row_block_size = blocks_[it->first];
 
90
    const int col_block_size = blocks_[it->second];
 
91
    layout_[IntPairToLong(it->first, it->second)] =
 
92
        new CellInfo(values + pos);
 
93
    pos += row_block_size * col_block_size;
 
94
  }
 
95
 
 
96
  // Fill the sparsity pattern of the underlying matrix.
 
97
  for (set<pair<int, int> >::const_iterator it = block_pairs.begin();
 
98
       it != block_pairs.end();
 
99
       ++it) {
 
100
    const int row_block_id = it->first;
 
101
    const int col_block_id = it->second;
 
102
    const int row_block_size = blocks_[row_block_id];
 
103
    const int col_block_size = blocks_[col_block_id];
 
104
    int pos =
 
105
        layout_[IntPairToLong(row_block_id, col_block_id)]->values - values;
 
106
    for (int r = 0; r < row_block_size; ++r) {
 
107
      for (int c = 0; c < col_block_size; ++c, ++pos) {
 
108
          rows[pos] = col_layout[row_block_id] + r;
 
109
          cols[pos] = col_layout[col_block_id] + c;
 
110
          values[pos] = 1.0;
 
111
          DCHECK_LT(rows[pos], tsm_->num_rows());
 
112
          DCHECK_LT(cols[pos], tsm_->num_rows());
 
113
      }
 
114
    }
 
115
  }
 
116
}
 
117
 
 
118
// Assume that the user does not hold any locks on any cell blocks
 
119
// when they are calling SetZero.
 
120
BlockRandomAccessSparseMatrix::~BlockRandomAccessSparseMatrix() {
 
121
  for (LayoutType::iterator it = layout_.begin();
 
122
       it != layout_.end();
 
123
       ++it) {
 
124
    delete it->second;
 
125
  }
 
126
}
 
127
 
 
128
CellInfo* BlockRandomAccessSparseMatrix::GetCell(int row_block_id,
 
129
                                                 int col_block_id,
 
130
                                                 int* row,
 
131
                                                 int* col,
 
132
                                                 int* row_stride,
 
133
                                                 int* col_stride) {
 
134
  const LayoutType::iterator it  =
 
135
      layout_.find(IntPairToLong(row_block_id, col_block_id));
 
136
  if (it == layout_.end()) {
 
137
    return NULL;
 
138
  }
 
139
 
 
140
  // Each cell is stored contiguously as its own little dense matrix.
 
141
  *row = 0;
 
142
  *col = 0;
 
143
  *row_stride = blocks_[row_block_id];
 
144
  *col_stride = blocks_[col_block_id];
 
145
  return it->second;
 
146
}
 
147
 
 
148
// Assume that the user does not hold any locks on any cell blocks
 
149
// when they are calling SetZero.
 
150
void BlockRandomAccessSparseMatrix::SetZero() {
 
151
  if (tsm_->num_nonzeros()) {
 
152
    VectorRef(tsm_->mutable_values(),
 
153
              tsm_->num_nonzeros()).setZero();
 
154
  }
 
155
}
 
156
 
 
157
}  // namespace internal
 
158
}  // namespace ceres