diff options
Diffstat (limited to 'internal/ceres/blas.h')
| -rw-r--r-- | internal/ceres/blas.h | 406 |
1 files changed, 406 insertions, 0 deletions
diff --git a/internal/ceres/blas.h b/internal/ceres/blas.h new file mode 100644 index 0000000..9629b3d --- /dev/null +++ b/internal/ceres/blas.h @@ -0,0 +1,406 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2013 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Simple blas functions for use in the Schur Eliminator. These are +// fairly basic implementations which already yield a significant +// speedup in the eliminator performance. + +#ifndef CERES_INTERNAL_BLAS_H_ +#define CERES_INTERNAL_BLAS_H_ + +#include "ceres/internal/eigen.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +// Remove the ".noalias()" annotation from the matrix matrix +// mutliplies to produce a correct build with the Android NDK, +// including versions 6, 7, 8, and 8b, when built with STLPort and the +// non-standalone toolchain (i.e. ndk-build). This appears to be a +// compiler bug; if the workaround is not in place, the line +// +// block.noalias() -= A * B; +// +// gets compiled to +// +// block.noalias() += A * B; +// +// which breaks schur elimination. Introducing a temporary by removing the +// .noalias() annotation causes the issue to disappear. Tracking this +// issue down was tricky, since the test suite doesn't run when built with +// the non-standalone toolchain. +// +// TODO(keir): Make a reproduction case for this and send it upstream. +#ifdef CERES_WORK_AROUND_ANDROID_NDK_COMPILER_BUG +#define CERES_MAYBE_NOALIAS +#else +#define CERES_MAYBE_NOALIAS .noalias() +#endif + +// The following three macros are used to share code and reduce +// template junk across the various GEMM variants. +#define CERES_GEMM_BEGIN(name) \ + template<int kRowA, int kColA, int kRowB, int kColB, int kOperation> \ + inline void name(const double* A, \ + const int num_row_a, \ + const int num_col_a, \ + const double* B, \ + const int num_row_b, \ + const int num_col_b, \ + double* C, \ + const int start_row_c, \ + const int start_col_c, \ + const int row_stride_c, \ + const int col_stride_c) + +#define CERES_GEMM_NAIVE_HEADER \ + DCHECK_GT(num_row_a, 0); \ + DCHECK_GT(num_col_a, 0); \ + DCHECK_GT(num_row_b, 0); \ + DCHECK_GT(num_col_b, 0); \ + DCHECK_GE(start_row_c, 0); \ + DCHECK_GE(start_col_c, 0); \ + DCHECK_GT(row_stride_c, 0); \ + DCHECK_GT(col_stride_c, 0); \ + DCHECK((kRowA == Eigen::Dynamic) || (kRowA == num_row_a)); \ + DCHECK((kColA == Eigen::Dynamic) || (kColA == num_col_a)); \ + DCHECK((kRowB == Eigen::Dynamic) || (kRowB == num_row_b)); \ + DCHECK((kColB == Eigen::Dynamic) || (kColB == num_col_b)); \ + const int NUM_ROW_A = (kRowA != Eigen::Dynamic ? kRowA : num_row_a); \ + const int NUM_COL_A = (kColA != Eigen::Dynamic ? kColA : num_col_a); \ + const int NUM_ROW_B = (kColB != Eigen::Dynamic ? kRowB : num_row_b); \ + const int NUM_COL_B = (kColB != Eigen::Dynamic ? kColB : num_col_b); + +#define CERES_GEMM_EIGEN_HEADER \ + const typename EigenTypes<kRowA, kColA>::ConstMatrixRef \ + Aref(A, num_row_a, num_col_a); \ + const typename EigenTypes<kRowB, kColB>::ConstMatrixRef \ + Bref(B, num_row_b, num_col_b); \ + MatrixRef Cref(C, row_stride_c, col_stride_c); \ + +#define CERES_CALL_GEMM(name) \ + name<kRowA, kColA, kRowB, kColB, kOperation>( \ + A, num_row_a, num_col_a, \ + B, num_row_b, num_col_b, \ + C, start_row_c, start_col_c, row_stride_c, col_stride_c); + + +// For the matrix-matrix functions below, there are three variants for +// each functionality. Foo, FooNaive and FooEigen. Foo is the one to +// be called by the user. FooNaive is a basic loop based +// implementation and FooEigen uses Eigen's implementation. Foo +// chooses between FooNaive and FooEigen depending on how many of the +// template arguments are fixed at compile time. Currently, FooEigen +// is called if all matrix dimensions are compile time +// constants. FooNaive is called otherwise. This leads to the best +// performance currently. +// +// The MatrixMatrixMultiply variants compute: +// +// C op A * B; +// +// The MatrixTransposeMatrixMultiply variants compute: +// +// C op A' * B +// +// where op can be +=, -=, or =. +// +// The template parameters (kRowA, kColA, kRowB, kColB) allow +// specialization of the loop at compile time. If this information is +// not available, then Eigen::Dynamic should be used as the template +// argument. +// +// kOperation = 1 -> C += A * B +// kOperation = -1 -> C -= A * B +// kOperation = 0 -> C = A * B +// +// The functions can write into matrices C which are larger than the +// matrix A * B. This is done by specifying the true size of C via +// row_stride_c and col_stride_c, and then indicating where A * B +// should be written into by start_row_c and start_col_c. +// +// Graphically if row_stride_c = 10, col_stride_c = 12, start_row_c = +// 4 and start_col_c = 5, then if A = 3x2 and B = 2x4, we get +// +// ------------ +// ------------ +// ------------ +// ------------ +// -----xxxx--- +// -----xxxx--- +// -----xxxx--- +// ------------ +// ------------ +// ------------ +// +CERES_GEMM_BEGIN(MatrixMatrixMultiplyEigen) { + CERES_GEMM_EIGEN_HEADER + Eigen::Block<MatrixRef, kRowA, kColB> + block(Cref, start_row_c, start_col_c, num_row_a, num_col_b); + + if (kOperation > 0) { + block CERES_MAYBE_NOALIAS += Aref * Bref; + } else if (kOperation < 0) { + block CERES_MAYBE_NOALIAS -= Aref * Bref; + } else { + block CERES_MAYBE_NOALIAS = Aref * Bref; + } +} + +CERES_GEMM_BEGIN(MatrixMatrixMultiplyNaive) { + CERES_GEMM_NAIVE_HEADER + DCHECK_EQ(NUM_COL_A, NUM_ROW_B); + + const int NUM_ROW_C = NUM_ROW_A; + const int NUM_COL_C = NUM_COL_B; + DCHECK_LE(start_row_c + NUM_ROW_C, row_stride_c); + DCHECK_LE(start_col_c + NUM_COL_C, col_stride_c); + + for (int row = 0; row < NUM_ROW_C; ++row) { + for (int col = 0; col < NUM_COL_C; ++col) { + double tmp = 0.0; + for (int k = 0; k < NUM_COL_A; ++k) { + tmp += A[row * NUM_COL_A + k] * B[k * NUM_COL_B + col]; + } + + const int index = (row + start_row_c) * col_stride_c + start_col_c + col; + if (kOperation > 0) { + C[index] += tmp; + } else if (kOperation < 0) { + C[index] -= tmp; + } else { + C[index] = tmp; + } + } + } +} + +CERES_GEMM_BEGIN(MatrixMatrixMultiply) { +#ifdef CERES_NO_CUSTOM_BLAS + + CERES_CALL_GEMM(MatrixMatrixMultiplyEigen) + return; + +#else + + if (kRowA != Eigen::Dynamic && kColA != Eigen::Dynamic && + kRowB != Eigen::Dynamic && kColB != Eigen::Dynamic) { + CERES_CALL_GEMM(MatrixMatrixMultiplyEigen) + } else { + CERES_CALL_GEMM(MatrixMatrixMultiplyNaive) + } + +#endif +} + +CERES_GEMM_BEGIN(MatrixTransposeMatrixMultiplyEigen) { + CERES_GEMM_EIGEN_HEADER + Eigen::Block<MatrixRef, kColA, kColB> block(Cref, + start_row_c, start_col_c, + num_col_a, num_col_b); + if (kOperation > 0) { + block CERES_MAYBE_NOALIAS += Aref.transpose() * Bref; + } else if (kOperation < 0) { + block CERES_MAYBE_NOALIAS -= Aref.transpose() * Bref; + } else { + block CERES_MAYBE_NOALIAS = Aref.transpose() * Bref; + } +} + +CERES_GEMM_BEGIN(MatrixTransposeMatrixMultiplyNaive) { + CERES_GEMM_NAIVE_HEADER + DCHECK_EQ(NUM_ROW_A, NUM_ROW_B); + + const int NUM_ROW_C = NUM_COL_A; + const int NUM_COL_C = NUM_COL_B; + DCHECK_LE(start_row_c + NUM_ROW_C, row_stride_c); + DCHECK_LE(start_col_c + NUM_COL_C, col_stride_c); + + for (int row = 0; row < NUM_ROW_C; ++row) { + for (int col = 0; col < NUM_COL_C; ++col) { + double tmp = 0.0; + for (int k = 0; k < NUM_ROW_A; ++k) { + tmp += A[k * NUM_COL_A + row] * B[k * NUM_COL_B + col]; + } + + const int index = (row + start_row_c) * col_stride_c + start_col_c + col; + if (kOperation > 0) { + C[index]+= tmp; + } else if (kOperation < 0) { + C[index]-= tmp; + } else { + C[index]= tmp; + } + } + } +} + +CERES_GEMM_BEGIN(MatrixTransposeMatrixMultiply) { +#ifdef CERES_NO_CUSTOM_BLAS + + CERES_CALL_GEMM(MatrixTransposeMatrixMultiplyEigen) + return; + +#else + + if (kRowA != Eigen::Dynamic && kColA != Eigen::Dynamic && + kRowB != Eigen::Dynamic && kColB != Eigen::Dynamic) { + CERES_CALL_GEMM(MatrixTransposeMatrixMultiplyEigen) + } else { + CERES_CALL_GEMM(MatrixTransposeMatrixMultiplyNaive) + } + +#endif +} + +// Matrix-Vector multiplication +// +// c op A * b; +// +// where op can be +=, -=, or =. +// +// The template parameters (kRowA, kColA) allow specialization of the +// loop at compile time. If this information is not available, then +// Eigen::Dynamic should be used as the template argument. +// +// kOperation = 1 -> c += A' * b +// kOperation = -1 -> c -= A' * b +// kOperation = 0 -> c = A' * b +template<int kRowA, int kColA, int kOperation> +inline void MatrixVectorMultiply(const double* A, + const int num_row_a, + const int num_col_a, + const double* b, + double* c) { +#ifdef CERES_NO_CUSTOM_BLAS + const typename EigenTypes<kRowA, kColA>::ConstMatrixRef + Aref(A, num_row_a, num_col_a); + const typename EigenTypes<kColA>::ConstVectorRef bref(b, num_col_a); + typename EigenTypes<kRowA>::VectorRef cref(c, num_row_a); + + // lazyProduct works better than .noalias() for matrix-vector + // products. + if (kOperation > 0) { + cref += Aref.lazyProduct(bref); + } else if (kOperation < 0) { + cref -= Aref.lazyProduct(bref); + } else { + cref = Aref.lazyProduct(bref); + } +#else + + DCHECK_GT(num_row_a, 0); + DCHECK_GT(num_col_a, 0); + DCHECK((kRowA == Eigen::Dynamic) || (kRowA == num_row_a)); + DCHECK((kColA == Eigen::Dynamic) || (kColA == num_col_a)); + + const int NUM_ROW_A = (kRowA != Eigen::Dynamic ? kRowA : num_row_a); + const int NUM_COL_A = (kColA != Eigen::Dynamic ? kColA : num_col_a); + + for (int row = 0; row < NUM_ROW_A; ++row) { + double tmp = 0.0; + for (int col = 0; col < NUM_COL_A; ++col) { + tmp += A[row * NUM_COL_A + col] * b[col]; + } + + if (kOperation > 0) { + c[row] += tmp; + } else if (kOperation < 0) { + c[row] -= tmp; + } else { + c[row] = tmp; + } + } +#endif // CERES_NO_CUSTOM_BLAS +} + +// Similar to MatrixVectorMultiply, except that A is transposed, i.e., +// +// c op A' * b; +template<int kRowA, int kColA, int kOperation> +inline void MatrixTransposeVectorMultiply(const double* A, + const int num_row_a, + const int num_col_a, + const double* b, + double* c) { +#ifdef CERES_NO_CUSTOM_BLAS + const typename EigenTypes<kRowA, kColA>::ConstMatrixRef + Aref(A, num_row_a, num_col_a); + const typename EigenTypes<kRowA>::ConstVectorRef bref(b, num_row_a); + typename EigenTypes<kColA>::VectorRef cref(c, num_col_a); + + // lazyProduct works better than .noalias() for matrix-vector + // products. + if (kOperation > 0) { + cref += Aref.transpose().lazyProduct(bref); + } else if (kOperation < 0) { + cref -= Aref.transpose().lazyProduct(bref); + } else { + cref = Aref.transpose().lazyProduct(bref); + } +#else + + DCHECK_GT(num_row_a, 0); + DCHECK_GT(num_col_a, 0); + DCHECK((kRowA == Eigen::Dynamic) || (kRowA == num_row_a)); + DCHECK((kColA == Eigen::Dynamic) || (kColA == num_col_a)); + + const int NUM_ROW_A = (kRowA != Eigen::Dynamic ? kRowA : num_row_a); + const int NUM_COL_A = (kColA != Eigen::Dynamic ? kColA : num_col_a); + + for (int row = 0; row < NUM_COL_A; ++row) { + double tmp = 0.0; + for (int col = 0; col < NUM_ROW_A; ++col) { + tmp += A[col * NUM_COL_A + row] * b[col]; + } + + if (kOperation > 0) { + c[row] += tmp; + } else if (kOperation < 0) { + c[row] -= tmp; + } else { + c[row] = tmp; + } + } +#endif // CERES_NO_CUSTOM_BLAS +} + + +#undef CERES_MAYBE_NOALIAS +#undef CERES_GEMM_BEGIN +#undef CERES_GEMM_EIGEN_HEADER +#undef CERES_GEMM_NAIVE_HEADER +#undef CERES_CALL_GEMM + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_BLAS_H_ |