Vehicle-cpp/zjw/ceres-solver-master/internal/ceres/small_blas.h

// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// 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_SMALL_BLAS_H_
#define CERES_INTERNAL_SMALL_BLAS_H_

#include "ceres/internal/eigen.h"
#include "ceres/internal/export.h"
#include "glog/logging.h"
#include "small_blas_generic.h"

namespace ceres::internal {

// 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 = (kRowB != 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);

// clang-format off
#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);
// clang-format on

#define CERES_GEMM_STORE_SINGLE(p, index, value) \
  if (kOperation > 0) {                          \
    p[index] += value;                           \
  } else if (kOperation < 0) {                   \
    p[index] -= value;                           \
  } else {                                       \
    p[index] = value;                            \
  }

#define CERES_GEMM_STORE_PAIR(p, index, v1, v2) \
  if (kOperation > 0) {                         \
    p[index] += v1;                             \
    p[index + 1] += v2;                         \
  } else if (kOperation < 0) {                  \
    p[index] -= v1;                             \
    p[index + 1] -= v2;                         \
  } else {                                      \
    p[index] = v1;                              \
    p[index + 1] = v2;                          \
  }

// 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.noalias() += Aref * Bref;
  } else if (kOperation < 0) {
    block.noalias() -= Aref * Bref;
  } else {
    block.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);
  const int span = 4;

  // Calculate the remainder part first.

  // Process the last odd column if present.
  if (NUM_COL_C & 1) {
    int col = NUM_COL_C - 1;
    const double* pa = &A[0];
    for (int row = 0; row < NUM_ROW_C; ++row, pa += NUM_COL_A) {
      const double* pb = &B[col];
      double tmp = 0.0;
      for (int k = 0; k < NUM_COL_A; ++k, pb += NUM_COL_B) {
        tmp += pa[k] * pb[0];
      }

      const int index = (row + start_row_c) * col_stride_c + start_col_c + col;
      CERES_GEMM_STORE_SINGLE(C, index, tmp);
    }

    // Return directly for efficiency of extremely small matrix multiply.
    if (NUM_COL_C == 1) {
      return;
    }
  }

  // Process the couple columns in remainder if present.
  if (NUM_COL_C & 2) {
    int col = NUM_COL_C & (~(span - 1));
    const double* pa = &A[0];
    for (int row = 0; row < NUM_ROW_C; ++row, pa += NUM_COL_A) {
      const double* pb = &B[col];
      double tmp1 = 0.0, tmp2 = 0.0;
      for (int k = 0; k < NUM_COL_A; ++k, pb += NUM_COL_B) {
        double av = pa[k];
        tmp1 += av * pb[0];
        tmp2 += av * pb[1];
      }

      const int index = (row + start_row_c) * col_stride_c + start_col_c + col;
      CERES_GEMM_STORE_PAIR(C, index, tmp1, tmp2);
    }

    // Return directly for efficiency of extremely small matrix multiply.
    if (NUM_COL_C < span) {
      return;
    }
  }

  // Calculate the main part with multiples of 4.
  int col_m = NUM_COL_C & (~(span - 1));
  for (int col = 0; col < col_m; col += span) {
    for (int row = 0; row < NUM_ROW_C; ++row) {
      const int index = (row + start_row_c) * col_stride_c + start_col_c + col;
      // clang-format off
      MMM_mat1x4(NUM_COL_A, &A[row * NUM_COL_A],
                 &B[col], NUM_COL_B, &C[index], kOperation);
      // clang-format on
    }
  }
}

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
  // clang-format off
  Eigen::Block<MatrixRef, kColA, kColB> block(Cref,
                                              start_row_c, start_col_c,
                                              num_col_a, num_col_b);
  // clang-format on
  if (kOperation > 0) {
    block.noalias() += Aref.transpose() * Bref;
  } else if (kOperation < 0) {
    block.noalias() -= Aref.transpose() * Bref;
  } else {
    block.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);
  const int span = 4;

  // Process the remainder part first.

  // Process the last odd column if present.
  if (NUM_COL_C & 1) {
    int col = NUM_COL_C - 1;
    for (int row = 0; row < NUM_ROW_C; ++row) {
      const double* pa = &A[row];
      const double* pb = &B[col];
      double tmp = 0.0;
      for (int k = 0; k < NUM_ROW_A; ++k) {
        tmp += pa[0] * pb[0];
        pa += NUM_COL_A;
        pb += NUM_COL_B;
      }

      const int index = (row + start_row_c) * col_stride_c + start_col_c + col;
      CERES_GEMM_STORE_SINGLE(C, index, tmp);
    }

    // Return directly for efficiency of extremely small matrix multiply.
    if (NUM_COL_C == 1) {
      return;
    }
  }

  // Process the couple columns in remainder if present.
  if (NUM_COL_C & 2) {
    int col = NUM_COL_C & (~(span - 1));
    for (int row = 0; row < NUM_ROW_C; ++row) {
      const double* pa = &A[row];
      const double* pb = &B[col];
      double tmp1 = 0.0, tmp2 = 0.0;
      for (int k = 0; k < NUM_ROW_A; ++k) {
        double av = *pa;
        tmp1 += av * pb[0];
        tmp2 += av * pb[1];
        pa += NUM_COL_A;
        pb += NUM_COL_B;
      }

      const int index = (row + start_row_c) * col_stride_c + start_col_c + col;
      CERES_GEMM_STORE_PAIR(C, index, tmp1, tmp2);
    }

    // Return directly for efficiency of extremely small matrix multiply.
    if (NUM_COL_C < span) {
      return;
    }
  }

  // Process the main part with multiples of 4.
  int col_m = NUM_COL_C & (~(span - 1));
  for (int col = 0; col < col_m; col += span) {
    for (int row = 0; row < NUM_ROW_C; ++row) {
      const int index = (row + start_row_c) * col_stride_c + start_col_c + col;
      // clang-format off
      MTM_mat1x4(NUM_ROW_A, &A[row], NUM_COL_A,
                 &B[col], NUM_COL_B, &C[index], kOperation);
      // clang-format on
    }
  }
}

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);
  const int span = 4;

  // Calculate the remainder part first.

  // Process the last odd row if present.
  if (NUM_ROW_A & 1) {
    int row = NUM_ROW_A - 1;
    const double* pa = &A[row * NUM_COL_A];
    const double* pb = &b[0];
    double tmp = 0.0;
    for (int col = 0; col < NUM_COL_A; ++col) {
      tmp += (*pa++) * (*pb++);
    }
    CERES_GEMM_STORE_SINGLE(c, row, tmp);

    // Return directly for efficiency of extremely small matrix multiply.
    if (NUM_ROW_A == 1) {
      return;
    }
  }

  // Process the couple rows in remainder if present.
  if (NUM_ROW_A & 2) {
    int row = NUM_ROW_A & (~(span - 1));
    const double* pa1 = &A[row * NUM_COL_A];
    const double* pa2 = pa1 + NUM_COL_A;
    const double* pb = &b[0];
    double tmp1 = 0.0, tmp2 = 0.0;
    for (int col = 0; col < NUM_COL_A; ++col) {
      double bv = *pb++;
      tmp1 += *(pa1++) * bv;
      tmp2 += *(pa2++) * bv;
    }
    CERES_GEMM_STORE_PAIR(c, row, tmp1, tmp2);

    // Return directly for efficiency of extremely small matrix multiply.
    if (NUM_ROW_A < span) {
      return;
    }
  }

  // Calculate the main part with multiples of 4.
  int row_m = NUM_ROW_A & (~(span - 1));
  for (int row = 0; row < row_m; row += span) {
    // clang-format off
    MVM_mat4x1(NUM_COL_A, &A[row * NUM_COL_A], NUM_COL_A,
               &b[0], &c[row], kOperation);
    // clang-format on
  }

#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);
  const int span = 4;

  // Calculate the remainder part first.

  // Process the last odd column if present.
  if (NUM_COL_A & 1) {
    int row = NUM_COL_A - 1;
    const double* pa = &A[row];
    const double* pb = &b[0];
    double tmp = 0.0;
    for (int col = 0; col < NUM_ROW_A; ++col) {
      tmp += *pa * (*pb++);
      pa += NUM_COL_A;
    }
    CERES_GEMM_STORE_SINGLE(c, row, tmp);

    // Return directly for efficiency of extremely small matrix multiply.
    if (NUM_COL_A == 1) {
      return;
    }
  }

  // Process the couple columns in remainder if present.
  if (NUM_COL_A & 2) {
    int row = NUM_COL_A & (~(span - 1));
    const double* pa = &A[row];
    const double* pb = &b[0];
    double tmp1 = 0.0, tmp2 = 0.0;
    for (int col = 0; col < NUM_ROW_A; ++col) {
      // clang-format off
      double bv = *pb++;
      tmp1 += *(pa    ) * bv;
      tmp2 += *(pa + 1) * bv;
      pa += NUM_COL_A;
      // clang-format on
    }
    CERES_GEMM_STORE_PAIR(c, row, tmp1, tmp2);

    // Return directly for efficiency of extremely small matrix multiply.
    if (NUM_COL_A < span) {
      return;
    }
  }

  // Calculate the main part with multiples of 4.
  int row_m = NUM_COL_A & (~(span - 1));
  for (int row = 0; row < row_m; row += span) {
    // clang-format off
    MTV_mat4x1(NUM_ROW_A, &A[row], NUM_COL_A,
               &b[0], &c[row], kOperation);
    // clang-format on
  }

#endif  // CERES_NO_CUSTOM_BLAS
}

#undef CERES_GEMM_BEGIN
#undef CERES_GEMM_EIGEN_HEADER
#undef CERES_GEMM_NAIVE_HEADER
#undef CERES_CALL_GEMM
#undef CERES_GEMM_STORE_SINGLE
#undef CERES_GEMM_STORE_PAIR

}  // namespace ceres::internal

#endif  // CERES_INTERNAL_SMALL_BLAS_H_