相机标定

有多个坐标系：世界坐标系，相机坐标系，成像平面坐标系，图像像素坐标系。

比如：

void worldCoord2PixelCoord(double xw,double yw,double zw,int* xp,int* yp) //把世界坐标转换为像素坐标/*{{{*/
{
    double xc,yc,zc;
    CvMat * WorldCoord=cvCreateMat(3,1,CV_64FC1);
    cvmSet(WorldCoord,0,0,xw-l);
    cvmSet(WorldCoord,1,0,yw-d);
    cvmSet(WorldCoord,2,0,zw-h);
    CvMat * CameraCoord=cvCreateMat(3,1,CV_64FC1);
    cvMatMul(R,WorldCoord,CameraCoord);
    xc=cvmGet(CameraCoord,0,0);
    yc=cvmGet(CameraCoord,1,0);
    zc=cvmGet(CameraCoord,2,0);
    *xp=(int)(fi*yc/xc+ci);
    *yp=(int)(-fj*zc/xc+cj);
    cvReleaseMat(&WorldCoord);
    cvReleaseMat(&CameraCoord);
}/*}}}*/