Point2d Camera::worldToPixel(const Point3d& world) const
{
// all code based on OpenCV cvProjectPoints2 function
double X, Y, Z, x, y, z;
double r2, r4, r6, a1, a2, a3, cdist;
double xd, yd, fx, fy, cx , cy;
// world frame
X = world.x;
Y = world.y;
Z = 0;
// camera frame
x = _R(0, 0)*X + _R(0, 1)*Y + _R(0, 2)*Z + _t(0, 0);
y = _R(1, 0)*X + _R(1, 1)*Y + _R(1, 2)*Z + _t(1, 0);
z = _R(2, 0)*X + _R(2, 1)*Y + _R(2, 2)*Z + _t(2, 0);
// image frame
z = z ? 1./z : 1;
x *= z;
y *= z;
fx = _A(0, 0);
fy = _A(1, 1);
cx = _A(0, 2);
cy = _A(1, 2);
r2 = x*x + y*y;
r4 = r2*r2;
r6 = r4*r2;
a1 = 2*x*y;
a2 = r2 + 2*x*x;
a3 = r2 + 2*y*y;
cdist = 1 + _k(0, 0)*r2 + _k(1, 0)*r4 + _k(4, 0)*r6;
xd = x*cdist + _k(2, 0)*a1 + _k(3, 0)*a2;
yd = y*cdist + _k(2, 0)*a3 + _k(3, 0)*a1;
return Point2d(xd*fx + cx, yd*fy + cy);
}
