Function ComputeProjectionX

lib/PBA/SparseBundleCPU.cpp:1257–1295

template <class Float>
void ComputeProjectionX(size_t nproj, const Float* camera, const Float* point,
                        const Float* ms, const int* jmap, Float* pj, int radial,
                        int mt) {
  if (mt > 1 && nproj >= mt) {
    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 = nproj * i / thread_num;
      size_t last_ = nproj * (i + 1) / thread_num;
      size_t last = std::min(last_, nproj);
      RUN_THREAD(ComputeProjectionX, threads[i], last - first, camera, point,
                 ms + 2 * first, jmap + 2 * first, pj + 2 * first, radial);
    }
    WAIT_THREAD(threads, thread_num);
  } else {
    for (size_t i = 0; i < nproj; ++i, jmap += 2, ms += 2, pj += 2) {
      const Float* c = camera + jmap[0] * 16;
      const Float* m = point + jmap[1] * POINT_ALIGN;
      /////////////////////////////////////////////////////
      Float p0 = c[4] * m[0] + c[5] * m[1] + c[6] * m[2] + c[1];
      Float p1 = c[7] * m[0] + c[8] * m[1] + c[9] * m[2] + c[2];
      Float p2 = c[10] * m[0] + c[11] * m[1] + c[12] * m[2] + c[3];
      if (radial == 1) {
        Float rr = Float(1.0) + c[13] * (p0 * p0 + p1 * p1) / (p2 * p2);
        Float f_p2 = c[0] / p2;
        pj[0] = ms[0] / rr - p0 * f_p2;
        pj[1] = ms[1] / rr - p1 * f_p2;
      } else if (radial == -1) {
        Float rd = Float(1.0) + c[13] * (ms[0] * ms[0] + ms[1] * ms[1]);
        Float f_p2 = c[0] / p2 / rd;
        pj[0] = ms[0] - p0 * f_p2;
        pj[1] = ms[1] - p1 * f_p2;
      } else {
        pj[0] = ms[0] - p0 * c[0] / p2;
        pj[1] = ms[1] - p1 * c[0] / p2;
      }
    }
  }
}

template <class Float>
void ComputeProjectionQ(size_t nq, const Float* camera, const int* qmap,