视觉SLAM-深蓝学院-第五讲-双目生成点云

视觉SLAM-深蓝学院-第五讲-双目生成点云

前言

双目生成点云这道题，很多人，包括我做的时候深度都有问题。如下图。这是别人博客上的一张结果，我做的时候也是这样，仔细观察可以发现，在这里深度图的宽是原图的一半，深度图读了两次，图中间可以看到一个电线杆的形状，右边又可以看到一个电线杆。但是原深度图的长宽是没有问题的。
这涉及到png的交错存储问题，可以理解成隔行扫描，故问题就很明显了。解决方法有很多种：

1. 转换png格式，改为无交错
2. 修改程序，如下（有其他改法）

    int main(int argc, char **argv) {
        // 内参
        double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;
        // 间距
        double b = 0.573;
        // 读取图像
        cv::Mat left = cv::imread(left_file, 0);
        cv::Mat right = cv::imread(right_file, 0);
        cv::Mat disparity =
            cv::imread(disparity_file,0);  // disparty 为CV_8U,单位为像素
        // 生成点云
        vector> pointcloud;
        // TODO 根据双目模型计算点云
        // 如果你的机器慢，请把后面的v++和u++改成v+=2, u+=2
        for (int v = 0; v < left.rows; v++){
            for (int u = 0; u < left.cols; u++) {
                Vector4d point(
                    0, 0, 0,
                    left.at(v, u) / 255.0);  // 前三维为xyz,第四维为颜色
                // start your code here (~6 lines)
                // 根据双目模型计算 point 的位置
                unsigned short d = disparity.at(v, u/2);
                point[2] = 200 * log(65536.0 / double(d) + 1);
                point[0] = (u - cx) * point[2] / fx;
                point[1] = (v - cy) * point[2] / fy;
                pointcloud.push_back(point);
                // end your code here
            }
        }
        // 画出点云
        showPointCloud(pointcloud);
        return 0;
    }

结果