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 (at) 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 // 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 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 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 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 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 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> 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> 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
