Function ComputeDiagonal

lib/PBA/SparseBundleCPU.cpp:1596–1644

///////////////////////////////////////
template <class Float>
void ComputeDiagonal(const avec<Float>& jcv, const vector<int>& cmapv,
                     const avec<Float>& jpv, const vector<int>& pmapv,
                     const vector<int>& cmlistv, const Float* qw0,
                     avec<Float>& jtjdi, bool jc_transpose, int radial) {
  // first camera part
  if (jcv.size() == 0 || jpv.size() == 0) return;  // not gonna happen

  size_t ncam = cmapv.size() - 1, npts = pmapv.size() - 1;
  const int vn = radial ? 8 : 7;
  SetVectorZero(jtjdi);

  const int* cmap = &cmapv[0];
  const int* pmap = &pmapv[0];
  const int* cmlist = &cmlistv[0];
  const Float* jc = &jcv[0];
  const Float* jp = &jpv[0];
  const Float* qw = qw0;
  Float* jji = &jtjdi[0];

  ///////compute jc part
  for (size_t i = 0; i < ncam; ++i, jji += 8, ++cmap, qw += 2) {
    int idx1 = cmap[0], idx2 = cmap[1];
    //////////////////////////////////////
    for (int j = idx1; j < idx2; ++j) {
      int idx = jc_transpose ? j : cmlist[j];
      const Float* pj = jc + idx * 16;
      ///////////////////////////////////////////
      for (int k = 0; k < vn; ++k)
        jji[k] += (pj[k] * pj[k] + pj[k + 8] * pj[k + 8]);
    }
    if (qw0 && qw[0] > 0) {
      jji[0] += (qw[0] * qw[0] * 2.0f);
      jji[7] += (qw[1] * qw[1] * 2.0f);
    }
  }

  for (size_t i = 0; i < npts; ++i, jji += POINT_ALIGN, ++pmap) {
    int idx1 = pmap[0], idx2 = pmap[1];
    const Float* pj = jp + idx1 * POINT_ALIGN2;
    for (int j = idx1; j < idx2; ++j, pj += POINT_ALIGN2) {
      for (int k = 0; k < 3; ++k)
        jji[k] += (pj[k] * pj[k] + pj[k + POINT_ALIGN] * pj[k + POINT_ALIGN]);
    }
  }
  Float* it = jtjdi.begin();
  for (; it < jtjdi.end(); ++it) {
    *it = (*it == 0) ? 0 : Float(1.0 / (*it));
  }
}

template <class T, int n, int m>
void InvertSymmetricMatrix(T a[n][m], T ai[n][m]) {