单摄像头三维建模：像素坐标，深度图坐标与世界坐标之间的转换

深度摄像机深度图包含x,y(像素坐标)和z(深度值)，已知相机内参下可与以相机光心为原点建立的世界坐标(相机坐标系)相互转换 ：

原理见 https://blog.csdn.net/lyl771857509/article/details/79633412代码如下：

//相机内参
float fx_;
float fy_;
float cx_;
float cy_;

float inv_fx_ = 1.0f / fx_;
float inv_fy_ = 1.0f / fy_;
//深度图（u,v为像素坐标）转世界坐标
Point3f Camera::Depth2World(int u, int v, float depth) {
    return Pixel2ImagePlane(u, v) * depth;
}
//像素坐标转成像平面坐标
Point3f Camera::Pixel2ImagePlane(int u, int v) {
    float x = 1.0f * (u - cx_) * inv_fx_;
    float y = 1.0f * (v - cy_) * inv_fy_;
    return Point3f(x, y, 1.0f);
}
//世界坐标转像素坐标
Point2f Camera::World2Image(const Point3f &wrld) const {
    float x = wrld[0] / wrld[2];
    float y = wrld[1] / wrld[2];
    x = x * fx_ + cx_;
    y = y * fy_ + cy_;
    return Point2f(x, y);