1 // Ceres Solver - A fast non-linear least squares minimizer
2 // Copyright 2013 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 // Simple blas functions for use in the Schur Eliminator. These are
32 // fairly basic implementations which already yield a significant
33 // speedup in the eliminator performance.
34
35 #ifndef CERES_INTERNAL_SMALL_BLAS_H_
36 #define CERES_INTERNAL_SMALL_BLAS_H_
37
38 #include "ceres/internal/port.h"
39 #include "ceres/internal/eigen.h"
40 #include "glog/logging.h"
41
42 namespace ceres {
43 namespace internal {
44
45 // The following three macros are used to share code and reduce
46 // template junk across the various GEMM variants.
47 #define CERES_GEMM_BEGIN(name) \
48 template<int kRowA, int kColA, int kRowB, int kColB, int kOperation> \
49 inline void name(const double* A, \
50 const int num_row_a, \
51 const int num_col_a, \
52 const double* B, \
53 const int num_row_b, \
54 const int num_col_b, \
55 double* C, \
56 const int start_row_c, \
57 const int start_col_c, \
58 const int row_stride_c, \
59 const int col_stride_c)
60
61 #define CERES_GEMM_NAIVE_HEADER \
62 DCHECK_GT(num_row_a, 0); \
63 DCHECK_GT(num_col_a, 0); \
64 DCHECK_GT(num_row_b, 0); \
65 DCHECK_GT(num_col_b, 0); \
66 DCHECK_GE(start_row_c, 0); \
67 DCHECK_GE(start_col_c, 0); \
68 DCHECK_GT(row_stride_c, 0); \
69 DCHECK_GT(col_stride_c, 0); \
70 DCHECK((kRowA == Eigen::Dynamic) || (kRowA == num_row_a)); \
71 DCHECK((kColA == Eigen::Dynamic) || (kColA == num_col_a)); \
72 DCHECK((kRowB == Eigen::Dynamic) || (kRowB == num_row_b)); \
73 DCHECK((kColB == Eigen::Dynamic) || (kColB == num_col_b)); \
74 const int NUM_ROW_A = (kRowA != Eigen::Dynamic ? kRowA : num_row_a); \
75 const int NUM_COL_A = (kColA != Eigen::Dynamic ? kColA : num_col_a); \
76 const int NUM_ROW_B = (kColB != Eigen::Dynamic ? kRowB : num_row_b); \
77 const int NUM_COL_B = (kColB != Eigen::Dynamic ? kColB : num_col_b);
78
79 #define CERES_GEMM_EIGEN_HEADER \
80 const typename EigenTypes<kRowA, kColA>::ConstMatrixRef \
81 Aref(A, num_row_a, num_col_a); \
82 const typename EigenTypes<kRowB, kColB>::ConstMatrixRef \
83 Bref(B, num_row_b, num_col_b); \
84 MatrixRef Cref(C, row_stride_c, col_stride_c); \
85
86 #define CERES_CALL_GEMM(name) \
87 name<kRowA, kColA, kRowB, kColB, kOperation>( \
88 A, num_row_a, num_col_a, \
89 B, num_row_b, num_col_b, \
90 C, start_row_c, start_col_c, row_stride_c, col_stride_c);
91
92
93 // For the matrix-matrix functions below, there are three variants for
94 // each functionality. Foo, FooNaive and FooEigen. Foo is the one to
95 // be called by the user. FooNaive is a basic loop based
96 // implementation and FooEigen uses Eigen's implementation. Foo
97 // chooses between FooNaive and FooEigen depending on how many of the
98 // template arguments are fixed at compile time. Currently, FooEigen
99 // is called if all matrix dimensions are compile time
100 // constants. FooNaive is called otherwise. This leads to the best
101 // performance currently.
102 //
103 // The MatrixMatrixMultiply variants compute:
104 //
105 // C op A * B;
106 //
107 // The MatrixTransposeMatrixMultiply variants compute:
108 //
109 // C op A' * B
110 //
111 // where op can be +=, -=, or =.
112 //
113 // The template parameters (kRowA, kColA, kRowB, kColB) allow
114 // specialization of the loop at compile time. If this information is
115 // not available, then Eigen::Dynamic should be used as the template
116 // argument.
117 //
118 // kOperation = 1 -> C += A * B
119 // kOperation = -1 -> C -= A * B
120 // kOperation = 0 -> C = A * B
121 //
122 // The functions can write into matrices C which are larger than the
123 // matrix A * B. This is done by specifying the true size of C via
124 // row_stride_c and col_stride_c, and then indicating where A * B
125 // should be written into by start_row_c and start_col_c.
126 //
127 // Graphically if row_stride_c = 10, col_stride_c = 12, start_row_c =
128 // 4 and start_col_c = 5, then if A = 3x2 and B = 2x4, we get
129 //
130 // ------------
131 // ------------
132 // ------------
133 // ------------
134 // -----xxxx---
135 // -----xxxx---
136 // -----xxxx---
137 // ------------
138 // ------------
139 // ------------
140 //
CERES_GEMM_BEGIN(MatrixMatrixMultiplyEigen)141 CERES_GEMM_BEGIN(MatrixMatrixMultiplyEigen) {
142 CERES_GEMM_EIGEN_HEADER
143 Eigen::Block<MatrixRef, kRowA, kColB>
144 block(Cref, start_row_c, start_col_c, num_row_a, num_col_b);
145
146 if (kOperation > 0) {
147 block.noalias() += Aref * Bref;
148 } else if (kOperation < 0) {
149 block.noalias() -= Aref * Bref;
150 } else {
151 block.noalias() = Aref * Bref;
152 }
153 }
154
CERES_GEMM_BEGIN(MatrixMatrixMultiplyNaive)155 CERES_GEMM_BEGIN(MatrixMatrixMultiplyNaive) {
156 CERES_GEMM_NAIVE_HEADER
157 DCHECK_EQ(NUM_COL_A, NUM_ROW_B);
158
159 const int NUM_ROW_C = NUM_ROW_A;
160 const int NUM_COL_C = NUM_COL_B;
161 DCHECK_LE(start_row_c + NUM_ROW_C, row_stride_c);
162 DCHECK_LE(start_col_c + NUM_COL_C, col_stride_c);
163
164 for (int row = 0; row < NUM_ROW_C; ++row) {
165 for (int col = 0; col < NUM_COL_C; ++col) {
166 double tmp = 0.0;
167 for (int k = 0; k < NUM_COL_A; ++k) {
168 tmp += A[row * NUM_COL_A + k] * B[k * NUM_COL_B + col];
169 }
170
171 const int index = (row + start_row_c) * col_stride_c + start_col_c + col;
172 if (kOperation > 0) {
173 C[index] += tmp;
174 } else if (kOperation < 0) {
175 C[index] -= tmp;
176 } else {
177 C[index] = tmp;
178 }
179 }
180 }
181 }
182
CERES_GEMM_BEGIN(MatrixMatrixMultiply)183 CERES_GEMM_BEGIN(MatrixMatrixMultiply) {
184 #ifdef CERES_NO_CUSTOM_BLAS
185
186 CERES_CALL_GEMM(MatrixMatrixMultiplyEigen)
187 return;
188
189 #else
190
191 if (kRowA != Eigen::Dynamic && kColA != Eigen::Dynamic &&
192 kRowB != Eigen::Dynamic && kColB != Eigen::Dynamic) {
193 CERES_CALL_GEMM(MatrixMatrixMultiplyEigen)
194 } else {
195 CERES_CALL_GEMM(MatrixMatrixMultiplyNaive)
196 }
197
198 #endif
199 }
200
CERES_GEMM_BEGIN(MatrixTransposeMatrixMultiplyEigen)201 CERES_GEMM_BEGIN(MatrixTransposeMatrixMultiplyEigen) {
202 CERES_GEMM_EIGEN_HEADER
203 Eigen::Block<MatrixRef, kColA, kColB> block(Cref,
204 start_row_c, start_col_c,
205 num_col_a, num_col_b);
206 if (kOperation > 0) {
207 block.noalias() += Aref.transpose() * Bref;
208 } else if (kOperation < 0) {
209 block.noalias() -= Aref.transpose() * Bref;
210 } else {
211 block.noalias() = Aref.transpose() * Bref;
212 }
213 }
214
CERES_GEMM_BEGIN(MatrixTransposeMatrixMultiplyNaive)215 CERES_GEMM_BEGIN(MatrixTransposeMatrixMultiplyNaive) {
216 CERES_GEMM_NAIVE_HEADER
217 DCHECK_EQ(NUM_ROW_A, NUM_ROW_B);
218
219 const int NUM_ROW_C = NUM_COL_A;
220 const int NUM_COL_C = NUM_COL_B;
221 DCHECK_LE(start_row_c + NUM_ROW_C, row_stride_c);
222 DCHECK_LE(start_col_c + NUM_COL_C, col_stride_c);
223
224 for (int row = 0; row < NUM_ROW_C; ++row) {
225 for (int col = 0; col < NUM_COL_C; ++col) {
226 double tmp = 0.0;
227 for (int k = 0; k < NUM_ROW_A; ++k) {
228 tmp += A[k * NUM_COL_A + row] * B[k * NUM_COL_B + col];
229 }
230
231 const int index = (row + start_row_c) * col_stride_c + start_col_c + col;
232 if (kOperation > 0) {
233 C[index]+= tmp;
234 } else if (kOperation < 0) {
235 C[index]-= tmp;
236 } else {
237 C[index]= tmp;
238 }
239 }
240 }
241 }
242
CERES_GEMM_BEGIN(MatrixTransposeMatrixMultiply)243 CERES_GEMM_BEGIN(MatrixTransposeMatrixMultiply) {
244 #ifdef CERES_NO_CUSTOM_BLAS
245
246 CERES_CALL_GEMM(MatrixTransposeMatrixMultiplyEigen)
247 return;
248
249 #else
250
251 if (kRowA != Eigen::Dynamic && kColA != Eigen::Dynamic &&
252 kRowB != Eigen::Dynamic && kColB != Eigen::Dynamic) {
253 CERES_CALL_GEMM(MatrixTransposeMatrixMultiplyEigen)
254 } else {
255 CERES_CALL_GEMM(MatrixTransposeMatrixMultiplyNaive)
256 }
257
258 #endif
259 }
260
261 // Matrix-Vector multiplication
262 //
263 // c op A * b;
264 //
265 // where op can be +=, -=, or =.
266 //
267 // The template parameters (kRowA, kColA) allow specialization of the
268 // loop at compile time. If this information is not available, then
269 // Eigen::Dynamic should be used as the template argument.
270 //
271 // kOperation = 1 -> c += A' * b
272 // kOperation = -1 -> c -= A' * b
273 // kOperation = 0 -> c = A' * b
274 template<int kRowA, int kColA, int kOperation>
MatrixVectorMultiply(const double * A,const int num_row_a,const int num_col_a,const double * b,double * c)275 inline void MatrixVectorMultiply(const double* A,
276 const int num_row_a,
277 const int num_col_a,
278 const double* b,
279 double* c) {
280 #ifdef CERES_NO_CUSTOM_BLAS
281 const typename EigenTypes<kRowA, kColA>::ConstMatrixRef
282 Aref(A, num_row_a, num_col_a);
283 const typename EigenTypes<kColA>::ConstVectorRef bref(b, num_col_a);
284 typename EigenTypes<kRowA>::VectorRef cref(c, num_row_a);
285
286 // lazyProduct works better than .noalias() for matrix-vector
287 // products.
288 if (kOperation > 0) {
289 cref += Aref.lazyProduct(bref);
290 } else if (kOperation < 0) {
291 cref -= Aref.lazyProduct(bref);
292 } else {
293 cref = Aref.lazyProduct(bref);
294 }
295 #else
296
297 DCHECK_GT(num_row_a, 0);
298 DCHECK_GT(num_col_a, 0);
299 DCHECK((kRowA == Eigen::Dynamic) || (kRowA == num_row_a));
300 DCHECK((kColA == Eigen::Dynamic) || (kColA == num_col_a));
301
302 const int NUM_ROW_A = (kRowA != Eigen::Dynamic ? kRowA : num_row_a);
303 const int NUM_COL_A = (kColA != Eigen::Dynamic ? kColA : num_col_a);
304
305 for (int row = 0; row < NUM_ROW_A; ++row) {
306 double tmp = 0.0;
307 for (int col = 0; col < NUM_COL_A; ++col) {
308 tmp += A[row * NUM_COL_A + col] * b[col];
309 }
310
311 if (kOperation > 0) {
312 c[row] += tmp;
313 } else if (kOperation < 0) {
314 c[row] -= tmp;
315 } else {
316 c[row] = tmp;
317 }
318 }
319 #endif // CERES_NO_CUSTOM_BLAS
320 }
321
322 // Similar to MatrixVectorMultiply, except that A is transposed, i.e.,
323 //
324 // c op A' * b;
325 template<int kRowA, int kColA, int kOperation>
MatrixTransposeVectorMultiply(const double * A,const int num_row_a,const int num_col_a,const double * b,double * c)326 inline void MatrixTransposeVectorMultiply(const double* A,
327 const int num_row_a,
328 const int num_col_a,
329 const double* b,
330 double* c) {
331 #ifdef CERES_NO_CUSTOM_BLAS
332 const typename EigenTypes<kRowA, kColA>::ConstMatrixRef
333 Aref(A, num_row_a, num_col_a);
334 const typename EigenTypes<kRowA>::ConstVectorRef bref(b, num_row_a);
335 typename EigenTypes<kColA>::VectorRef cref(c, num_col_a);
336
337 // lazyProduct works better than .noalias() for matrix-vector
338 // products.
339 if (kOperation > 0) {
340 cref += Aref.transpose().lazyProduct(bref);
341 } else if (kOperation < 0) {
342 cref -= Aref.transpose().lazyProduct(bref);
343 } else {
344 cref = Aref.transpose().lazyProduct(bref);
345 }
346 #else
347
348 DCHECK_GT(num_row_a, 0);
349 DCHECK_GT(num_col_a, 0);
350 DCHECK((kRowA == Eigen::Dynamic) || (kRowA == num_row_a));
351 DCHECK((kColA == Eigen::Dynamic) || (kColA == num_col_a));
352
353 const int NUM_ROW_A = (kRowA != Eigen::Dynamic ? kRowA : num_row_a);
354 const int NUM_COL_A = (kColA != Eigen::Dynamic ? kColA : num_col_a);
355
356 for (int row = 0; row < NUM_COL_A; ++row) {
357 double tmp = 0.0;
358 for (int col = 0; col < NUM_ROW_A; ++col) {
359 tmp += A[col * NUM_COL_A + row] * b[col];
360 }
361
362 if (kOperation > 0) {
363 c[row] += tmp;
364 } else if (kOperation < 0) {
365 c[row] -= tmp;
366 } else {
367 c[row] = tmp;
368 }
369 }
370 #endif // CERES_NO_CUSTOM_BLAS
371 }
372
373 #undef CERES_GEMM_BEGIN
374 #undef CERES_GEMM_EIGEN_HEADER
375 #undef CERES_GEMM_NAIVE_HEADER
376 #undef CERES_CALL_GEMM
377
378 } // namespace internal
379 } // namespace ceres
380
381 #endif // CERES_INTERNAL_SMALL_BLAS_H_
382