图像坐标求解三维坐标标准模板

void Cseed::unproject(const int index0, const int index1,

                      const Cpoint& p0, const Cpoint& p1,

                      Vec4f& coord) const{

  Mat4 A;

  A[0][0] =

    m_fm.m_pss.m_photos[index0].m_projection[m_fm.m_level][0][0] -

    p0.m_icoord[0] * m_fm.m_pss.m_photos[index0].m_projection[m_fm.m_level][2][0];

  A[0][1] =

    m_fm.m_pss.m_photos[index0].m_projection[m_fm.m_level][0][1] -

    p0.m_icoord[0] * m_fm.m_pss.m_photos[index0].m_projection[m_fm.m_level][2][1];

  A[0][2] =

    m_fm.m_pss.m_photos[index0].m_projection[m_fm.m_level][0][2] -

    p0.m_icoord[0] * m_fm.m_pss.m_photos[index0].m_projection[m_fm.m_level][2][2];

  A[1][0] =

    m_fm.m_pss.m_photos[index0].m_projection[m_fm.m_level][1][0] -

    p0.m_icoord[1] * m_fm.m_pss.m_photos[index0].m_projection[m_fm.m_level][2][0];

  A[1][1] =

    m_fm.m_pss.m_photos[index0].m_projection[m_fm.m_level][1][1] -

    p0.m_icoord[1] * m_fm.m_pss.m_photos[index0].m_projection[m_fm.m_level][2][1];

  A[1][2] =

    m_fm.m_pss.m_photos[index0].m_projection[m_fm.m_level][1][2] -

    p0.m_icoord[1] * m_fm.m_pss.m_photos[index0].m_projection[m_fm.m_level][2][2];

  A[2][0] =

    m_fm.m_pss.m_photos[index1].m_projection[m_fm.m_level][0][0] -

    p1.m_icoord[0] * m_fm.m_pss.m_photos[index1].m_projection[m_fm.m_level][2][0];

  A[2][1] =

    m_fm.m_pss.m_photos[index1].m_projection[m_fm.m_level][0][1] -

    p1.m_icoord[0] * m_fm.m_pss.m_photos[index1].m_projection[m_fm.m_level][2][1];

  A[2][2] =

    m_fm.m_pss.m_photos[index1].m_projection[m_fm.m_level][0][2] -

    p1.m_icoord[0] * m_fm.m_pss.m_photos[index1].m_projection[m_fm.m_level][2][2];

  A[3][0] =

    m_fm.m_pss.m_photos[index1].m_projection[m_fm.m_level][1][0] -

    p1.m_icoord[1] * m_fm.m_pss.m_photos[index1].m_projection[m_fm.m_level][2][0];

  A[3][1] =

    m_fm.m_pss.m_photos[index1].m_projection[m_fm.m_level][1][1] -

    p1.m_icoord[1] * m_fm.m_pss.m_photos[index1].m_projection[m_fm.m_level][2][1];

  A[3][2] =

    m_fm.m_pss.m_photos[index1].m_projection[m_fm.m_level][1][2] -

    p1.m_icoord[1] * m_fm.m_pss.m_photos[index1].m_projection[m_fm.m_level][2][2];

 

  Vec4 b;

  b[0] =

    p0.m_icoord[0] * m_fm.m_pss.m_photos[index0].m_projection[m_fm.m_level][2][3] -

    m_fm.m_pss.m_photos[index0].m_projection[m_fm.m_level][0][3];

  b[1] =

    p0.m_icoord[1] * m_fm.m_pss.m_photos[index0].m_projection[m_fm.m_level][2][3] -

    m_fm.m_pss.m_photos[index0].m_projection[m_fm.m_level][1][3];

  b[2] =

    p1.m_icoord[0] * m_fm.m_pss.m_photos[index1].m_projection[m_fm.m_level][2][3] -

    m_fm.m_pss.m_photos[index1].m_projection[m_fm.m_level][0][3];

  b[3] =

    p1.m_icoord[1] * m_fm.m_pss.m_photos[index1].m_projection[m_fm.m_level][2][3] -

    m_fm.m_pss.m_photos[index1].m_projection[m_fm.m_level][1][3];

 

  Mat4 AT = transpose(A);

  Mat4 ATA = AT * A;

  Vec4 ATb = AT * b;

 

  Mat3 ATA3;

  for (int y = 0; y < 3; ++y)

    for (int x = 0; x < 3; ++x)

      ATA3[y][x] = ATA[y][x];

  Vec3 ATb3;

  for (int y = 0; y < 3; ++y)

    ATb3[y] = ATb[y];

 

  Mat3 iATA3;

  invert(iATA3, ATA3);

  Vec3 ans = iATA3 * ATb3;

  for (int y = 0; y < 3; ++y)

    coord[y] = ans[y];

  coord[3] = 1.0f;

}