Обратная функция к cvProjectPoints2

[Dasdranagon 1]

Привет Всем!

Уже несколько дней топчусь на месте. Может кто уже встречался с подобной проблемой?

OpenCV функция cvProjectPoints2 проецирует заданный 3D point на снимок с камеры, при известных rotation_vector, translation_vector, intrinsic_matrix и

distortion_coeffs. Это работает безукоризненно.

Необходимо же решить обратную задачу:

Есть точка на снимке.

Небходимо найти луч (вектор) в 3D пространстве, направленный из камеры в эту точку при тех же rotation_vector, translation_vector, intrinsic_matrix и

distortion_coeffs. Система координат та же, что использовалась для cvProjectPoints2.

Заранее благодарю за помощь!

[Dasdranagon 1]

Как говориться: "Спасение утопающих..."

Ниже привожу решение проблемы.

Функция выдаёт 3D точки, соответствующие 2D пикселям.

Чтобы получить вектора, надо вызвать эту функцию дважды, с различными значениями z.

При z=0 функция вернёт координаты оптического центра камеры.

int ProjectImagePixelsTo3D(vector <CvPoint2D64f> m,  // Undistorted Image pixels (input)

                          vector <CvPoint3D64f> &M, // Object 3D points (output)

                          double R[],               // Rotation matrix [9] (3х3). See cvRodrigues2.

                          double t[],               // Translation vector [3]

                          double I[],               // Intrinsic matrix   [9]

                          double z)                 // Scaling Factor (for camera origin z=0)

{

  int i,j,l, count;

  double x,y,fx,fy,cx,cy;


  CvMat * Kx = cvCreateMat(3,3,CV_64FC1);

  CvMat * Ky = cvCreateMat(3,1,CV_64FC1);

  CvMat * KC = cvCreateMat(3,1,CV_64FC1);


  fx = I[0];

  fy = I[4];

  cx = I[2];

  cy = I[5];


  l = 0;

  for(i = 0;i < 3;i++)

    for(j = 0;j < 3;j++)

      cvSetReal2D(Kx,i,j,R[l++]);


  count =(int) m.size();


  for( i = 0; i < count; i++ )

  {

    x = (m[i].x - cx) / fx * z;

    y = (m[i].y - cy) / fy * z;


    cvSetReal2D( Ky, 0, 0, x - t[0]);

    cvSetReal2D( Ky, 1, 0, y - t[1]);

    cvSetReal2D( Ky, 2, 0, z - t[2]);


    cvSolve(Kx,Ky,KC,CV_LU);


    M[i].x = CV_MAT_ELEM( *KC, double, 0,0);

    M[i].y = CV_MAT_ELEM( *KC, double, 1,0);

    M[i].z = CV_MAT_ELEM( *KC, double, 2,0);

  }


  cvReleaseMat(&Kx);

  cvReleaseMat(&Ky);

  cvReleaseMat(&KC);


  return 0;

}

[/codebox]

[quosego 5]

А по какому принципу нужно выбирать масштабный множитель Z?

[Dasdranagon 1]

А по какому принципу нужно выбирать масштабный множитель Z?

Любой положительный, например 1 и 10.

Но, на практике логично выбрать 0 и 1, (для 0 создаём vector <CvPoint2D64f> m0(1) и vector <CvPoint3D64f> M0(1), - состоящие из одного элемента).

Тогда получается вектор, направленный из оптического центра камеры M0[0] в направлении M.