Point3d Camera::pixelToWorld(const Point2d& pixel) const { double im[2] = {pixel.x, pixel.y}; double wl[3]; double z; // setup intrinsic and extrinsic parameters CvMat img_frm = cvMat(1, 1, CV_64FC2, im); Mat world_frm = Mat(T_ROWS, T_COLS, TYPE, wl); // convert from distorted pixels to normalized camera frame // cv:: version does not allow doubles for some odd reason, // so use the C version CvMat A = _A; CvMat k = _k; cvUndistortPoints(&img_frm, &img_frm, &A, &k); // convert from camera frame to world frame /*z = _t(2, 0) ? _t(2, 0) : 1; // Wrong z != t3. z = r31 X + r32 Y + t3 wl[0] = z*im[0] - _t(0, 0); wl[1] = z*im[1] - _t(1, 0); wl[2] = 0; world_frm = _R.t() * world_frm; */ wl[0] = (_R(1,1) * _t(0,0)-_R(0,1) * _t(1,0)+_R(2,1) * _t(1,0) * im[0]- _R(1,1)* _t(2,0) * im[0]-_R(2,1)* _t(0,0)* im[1]+_R(0,1) * _t(2,0) * im[1])/(_R(0,1) * _R(1,0)-_R(0,0)* _R(1,1)+_R(1,1)* _R(2,0) *im[0]-_R(1,0) *_R(2,1) *im[0]-_R(0,1) *_R(2,0) * im[1]+_R(0,0) *_R(2,1) *im[1]); wl[1] = (-_R(0,0) * _t(1,0)+_R(2,0)* _t(1,0)*im[0]+_R(1,0) *(_t(0,0)-_t(2,0)*im[0])-_R(2,0) * _t(0,0)*im[1]+_R(0,0) * _t(2,0) * im[1])/(-_R(0,1) * _R(1,0)+_R(0,0) * _R(1,1)-_R(1,1)*_R(2,0) *im[0]+_R(1,0) * _R(2,1) * im[0]+_R(0,1) * _R(2,0) * im[1]-_R(0,0) * _R(2,1) *im[1]); wl[2] = 0; return Point3d(wl[0], wl[1], wl[2]); }