一，从零开始复现实时三维重建KInectFusion

实时三维重建算法

静态：
知乎：三维重建 3D reconstruction 有哪些实用算法？

【本文代码】

准备环境

我们对Markdown编辑器进行了一些功能拓展与语法支持，除了标准的Markdown编辑器功能，我们增加了如下几点新功能，帮助你用它写博客：

1. OpenCV： 加上viz编译 ，方便可视化 其他类似的可视化工具 pangolin opengl pcl qglviewer vtk open3d等；
2. CUDA 8.0 以上 KinectFusion 的复现工作大多基于cuda8.0 在ubuntu20.04 上难以工作，本次复现采用较新的api;

准备数据集

1. TUM数据集,这里以 f3_long_office为例子
2. 将深度图和RGB图时间同步（后期更新此工具）

检查数据集

#include "read.hpp"
using namespace std;
int main()
{
    DataSet<float> dt("/home/lei/dataset/paper/f3_long_office");
    for (int i = 0; i < dt.pose.frames; i++)
    {
        cv::Mat rgb = cv::imread(dt.color_path[i]);
        cv::Mat depth = cv::imread(dt.depth_path[i]);
        cv::imshow("rgb", rgb);
        cv::imshow("depth", depth);
        cv::waitKey(1);
    }
}

能正确读取rgb图和深度图即可

计算相机坐标系下的点云

定义一个内参结构体

struct Intr
{
    union
    {
        float4 cam;
        struct
        {
            float fx, fy, cx, cy;
        };
    };
    Intr(float4 intr)
    {
        cam = intr;
    }
    __device__ inline float3 cam2world(int u, int v, float z)
    {
        // TODO 这里可预先计算fx，fy倒数，然后计算乘法，应该会快一些
        float x = __fdividef(z * (u - cx), fx);
        float y = __fdividef(z * (v - cy), fy);
        return make_float3(x, y, z);
    }
};

深度图转点云的cuda核函数

__global__ void depth2cam(uint16_t *hd_depth_ptr, float3 *output, Intr intr)
{
    int tx = threadIdx.x; // 640
    int ty = blockIdx.x;  // 480
    int rawidx = tx + ty * 640;
    float pz = hd_depth_ptr[rawidx] * 0.0002f; //深度转换为米
    output[rawidx] = intr.cam2world(tx, ty, pz);
}

main中

int main()
{
    DataSet<float> dt("/home/lei/dataset/paper/f3_long_office");
    ck(cudaGetLastError());
    Intr intr(make_float4(550, 550, 320, 240));
    cv::viz::Viz3d window("map");
    window.showWidget("Coordinate", cv::viz::WCoordinateSystem());

    for (int i = 0; i < dt.pose.frames; i++)
    {
        cv::Mat rgb = cv::imread(dt.color_path[i]);
        cv::Mat depth = cv::imread(dt.depth_path[i], cv::IMREAD_ANYDEPTH);

        uint16_t *hd_depth_ptr;
        cudaMallocManaged((void **)&hd_depth_ptr, depth.rows * depth.cols * sizeof(uint16_t));
        cudaMemcpy(hd_depth_ptr, depth.ptr<uint16_t>(), depth.rows * depth.cols * sizeof(uint16_t), cudaMemcpyHostToDevice);
        float3 *cloud;
        cudaMallocManaged(&cloud, depth.rows * depth.cols * sizeof(float3));
        depth2cam<<<depth.rows, depth.cols>>>(hd_depth_ptr, cloud, intr);
        ck(cudaDeviceSynchronize());
        cv::Mat cpu_cloud(depth.rows * depth.cols, 1, CV_32FC3);
        cudaMemcpy(cpu_cloud.ptr<float *>(), cloud, depth.rows * depth.cols * sizeof(float3), cudaMemcpyDeviceToHost);
        cv::Mat dst2 = rgb.reshape(3,640 * 480);
        window.showWidget("depthmode", cv::viz::WCloud(cpu_cloud, dst2));
        window.spinOnce(true);
        window.spin();
        cv::imshow("rgb", rgb);
        cv::imshow("depth", depth);
        cv::waitKey(1);
        cudaFree(hd_depth_ptr);
        cudaFree(cloud);
    }
}

运行结果如下

创建一个Patch管理GPU的2D内存

template <class T>
struct Patch
{
    T *devPtr;
    size_t pitch = 0;
    size_t rows, cols;
    Patch()
    {
    }
    Patch(int rows, int cols) : rows(rows), cols(cols)
    {
        ck(cudaMallocPitch((void **)&devPtr, &pitch, cols * sizeof(T), rows));
        ck(cudaMemset2D(devPtr, pitch, 0, sizeof(T) * cols, rows));
        // std::cout << pitch << " " << sizeof(T) * cols << std::endl;
    }
    void creat(const int rows, const int cols)
    {
        this->rows = rows;
        this->cols = cols;
        ck(cudaMallocPitch((void **)&devPtr, &pitch, cols * sizeof(T), rows));
        ck(cudaMemset2D(devPtr, pitch, 0, sizeof(T) * cols, rows));
    }
    //拷贝构造函数
    __host__ __device__ Patch(const Patch &lth)
    {
        this->devPtr = lth.devPtr;
        this->rows = lth.rows;
        this->pitch = lth.pitch;
        this->cols = lth.cols;
    }

    ~Patch()
    {
        // cudaFree(devPtr);
    }
    void release()
    {
        cudaFree(devPtr);
    }
    __host__ void upload(const T *host_ptr_arg, size_t host_step_arg)
    {
        ck(cudaMemcpy2D(devPtr, pitch, host_ptr_arg, host_step_arg, cols * sizeof(T), rows, cudaMemcpyHostToDevice));
    }
    __host__ void download(const T *host_ptr_arg, size_t host_step_arg)
    {
        // if (host_step_arg == 0 || devPtr == nullptr || pitch == 0)
        {
            printf("%x,%d %x,%ld\n", host_ptr_arg, host_step_arg, devPtr, pitch);
        }
        ck(cudaMemcpy2D((void *)host_ptr_arg, host_step_arg, devPtr, pitch, sizeof(T) * cols, rows, cudaMemcpyDeviceToHost));
    }
    __device__ inline T &operator()(size_t rows, size_t cols)
    {
        // if (rows < pitch)
        //     return mat[x];
        return *((devPtr + rows * pitch / sizeof(T) + cols));
    }
    __host__ void print()
    {
        printf("%ld %ld %ld\n", pitch, rows, cols);
    }
};

TSDF类

tsdf class

修改后的主函数

int main()
{
    DataSet<float> dt("/home/u20/dataset/paper/f3_long_office");
    ck(cudaGetLastError());

    cv::viz::Viz3d viz1("viz1"), viz2("viz2");
    viz1.showWidget("Coordinate", cv::viz::WCoordinateSystem());

    TSDF tsdf(make_float3(512, 512, 512), make_int2(640, 480));
    tsdf.pintr = new Intr(make_float4(550, 550, 320, 240));
    for (int i = 0; i < dt.pose.frames; i++)
    {
        cv::Mat rgb = cv::imread(dt.color_path[i]);
        cv::Mat depth = cv::imread(dt.depth_path[i], cv::IMREAD_ANYDEPTH);

        tsdf.addScan(depth, rgb);
        std::shared_ptr<Mat> cpu_cloud2;
        Mat cpu_color;
        tsdf.exportCloud(cpu_cloud2, cpu_color);
        viz1.showWidget("depthmode", cv::viz::WCloud(*cpu_cloud2, cpu_color));

        Mat depth_color, cpu_cloud;
        tsdf.depth2cam(depth, rgb, depth_color, cpu_color, cv::Affine3f::Identity());
        cv::Affine3f viewpose = cv::Affine3f::Identity();
        viz1.showWidget("depth", cv::viz::WCloud(depth_color, cpu_color), viewpose.translate(cv::Vec3f(4, 0, 0)));

        // cv::imshow("rgb", rgb);
        // cv::imshow("depth", depth);
        // cv::waitKey(100);
        viz1.spin();
    }
}

最后效果如下