Function ComputeDiagonalBlockP

lib/PBA/SparseBundleCPU.cpp:1789–1846

template <class Float>
void ComputeDiagonalBlockP(size_t npt, float lambda1, float lambda2,
                           const Float* jp, const int* pmap, Float* di,
                           Float* bi, int mt) {
  if (mt > 1) {
    MYTHREAD threads[THREAD_NUM_MAX];
    const size_t thread_num = std::min(mt, THREAD_NUM_MAX);
    for (size_t i = 0; i < thread_num; ++i) {
      size_t first = npt * i / thread_num;
      size_t last_ = npt * (i + 1) / thread_num;
      size_t last = std::min(last_, npt);
      RUN_THREAD(ComputeDiagonalBlockP, threads[i], (last - first), lambda1,
                 lambda2, jp, pmap + first, di + POINT_ALIGN * first,
                 bi + 6 * first);
    }
    WAIT_THREAD(threads, thread_num);
  } else {
    for (size_t i = 0; i < npt; ++i, ++pmap, di += POINT_ALIGN, bi += 6) {
      int idx1 = pmap[0], idx2 = pmap[1];

      Float M00 = 0, M01 = 0, M02 = 0, M11 = 0, M12 = 0, M22 = 0;
      const Float *jxp = jp + idx1 * (POINT_ALIGN2), *jyp = jxp + POINT_ALIGN;
      for (int j = idx1; j < idx2;
           ++j, jxp += POINT_ALIGN2, jyp += POINT_ALIGN2) {
        M00 += (jxp[0] * jxp[0] + jyp[0] * jyp[0]);
        M01 += (jxp[0] * jxp[1] + jyp[0] * jyp[1]);
        M02 += (jxp[0] * jxp[2] + jyp[0] * jyp[2]);
        M11 += (jxp[1] * jxp[1] + jyp[1] * jyp[1]);
        M12 += (jxp[1] * jxp[2] + jyp[1] * jyp[2]);
        M22 += (jxp[2] * jxp[2] + jyp[2] * jyp[2]);
      }

      /////////////////////////////////
      di[0] = M00;
      di[1] = M11;
      di[2] = M22;

      /////////////////////////////
      M00 = M00 * lambda2 + lambda1;
      M11 = M11 * lambda2 + lambda1;
      M22 = M22 * lambda2 + lambda1;

      ///////////////////////////////
      Float det = (M00 * M11 - M01 * M01) * M22 + Float(2.0) * M01 * M12 * M02 -
                  M02 * M02 * M11 - M12 * M12 * M00;
      if (det >= FLT_MAX || det <= FLT_MIN * 2.0f) {
        // SetVectorZero(bi, bi + 6);
        for (int j = 0; j < 6; ++j) bi[j] = 0;
      } else {
        bi[0] = (M11 * M22 - M12 * M12) / det;
        bi[1] = -(M01 * M22 - M12 * M02) / det;
        bi[2] = (M01 * M12 - M02 * M11) / det;
        bi[3] = (M00 * M22 - M02 * M02) / det;
        bi[4] = -(M00 * M12 - M01 * M02) / det;
        bi[5] = (M00 * M11 - M01 * M01) / det;
      }
    }
  }
}