张正友标定法，现已知相机的内参数fx，fy，cx，cy，怎么得到像素坐标和世界坐标的关系

小弟我最近研究张正友标定法，但是各种函数关系搞的有点迷糊，有没有大佬能给出相应的算法，matlab程序最好