【OpenCV 4.0 C++】 Kinect Fusion 使用

【承接图像分类、检测、分割、生成相关项目，私信。】
完整的工程应用：3DFR Star Please

编译源码：
参考这篇博客OpenCV+OpenCV-contrib 编译步骤 ，特别的要勾选OPENCV_ENABLE_NONFREE 在 CMake 中 ，这样才能使用kinect fusion算法。

效果图（左：重建后效果图，右：depth图）：

Depth 数据(RGBD-dataset）： 代码使用到的数据集。

当使用自己的数据集时，应设置自己的cv::kinfu::params ,这涉及到frame_size等重要参数。默认参数如下。
KinFu Params(src code)：

Ptr<Params> Params::defaultParams()
{
    Params p;

    p.frameSize = Size(640, 480);

    float fx, fy, cx, cy;
    fx = fy = 525.f;
    cx = p.frameSize.width/2 - 0.5f;
    cy = p.frameSize.height/2 - 0.5f;
    p.intr = Matx33f(fx,  0, cx,
                      0, fy, cy,
                      0,  0,  1);

    // 5000 for the 16-bit PNG files
    // 1 for the 32-bit float images in the ROS bag files
    p.depthFactor = 5000;

    // sigma_depth is scaled by depthFactor when calling bilateral filter
    p.bilateral_sigma_depth = 0.04f;  //meter
    p.bilateral_sigma_spatial = 4.5; //pixels
    p.bilateral_kernel_size = 7;     //pixels

    p.icpAngleThresh = (float)(30. * CV_PI / 180.); // radians
    p.icpDistThresh = 0.1f; // meters

    p.icpIterations = {10, 5, 4};
    p.pyramidLevels = (int)p.icpIterations.size();

    p.tsdf_min_camera_movement = 0.f; //meters, disabled

    p.volumeDims = Vec3i::all(512); //number of voxels

    float volSize = 3.f;
    p.voxelSize = volSize/512.f; //meters

    // default pose of volume cube
    p.volumePose = Affine3f().translate(Vec3f(-volSize/2.f, -volSize/2.f, 0.5f));
    p.tsdf_trunc_dist = 0.04f; //meters;
    p.tsdf_max_weight = 64;   //frames

    p.raycast_step_factor = 0.25f;  //in voxel sizes
    // gradient delta factor is fixed at 1.0f and is not used
    //p.gradient_delta_factor = 0.5f; //in voxel sizes

    //p.lightPose = p.volume_pose.translation()/4; //meters
    p.lightPose = Vec3f::all(0.f); //meters

    // depth truncation is not used by default
    //p.icp_truncate_depth_dist = 0.f;        //meters, disabled

    return makePtr<Params>(p);
}

下面的代码是精简过的，去掉了摄像头的相关部分，毕竟一般也用不到。

Kinect Fusion 使用样例代码(demo code) ：

#include 
#include 
#include 
#include 
#include 
#include 

using namespace cv;
using namespace cv::kinfu;
using namespace std;

static vector<string> readDepth(std::string fileList);
static vector<string> readDepth(std::string fileList)
{
    vector<string> v;
    fstream file(fileList);
    if(!file.is_open())
        throw std::runtime_error("Failed to read depth list");
    std::string dir;
    size_t slashIdx = fileList.rfind('/');
    slashIdx = slashIdx != std::string::npos ? slashIdx : fileList.rfind('\\');
    dir = fileList.substr(0, slashIdx);

    while(!file.eof())
    {
        std::string s, imgPath;
        std::getline(file, s);
        if(s.empty() || s[0] == '#') continue;
        std::stringstream ss;
        ss << s;
        double thumb;
        ss >> thumb >> imgPath;
        v.push_back(dir+'/'+imgPath);
    }

    return v;
}

struct DepthWriter
{
    DepthWriter(string fileList) :
        file(fileList, ios::out), count(0), dir()
    {
        size_t slashIdx = fileList.rfind('/');
        slashIdx = slashIdx != std::string::npos ? slashIdx : fileList.rfind('\\');
        dir = fileList.substr(0, slashIdx);

        if(!file.is_open())
            throw std::runtime_error("Failed to write depth list");

        file << "# depth maps saved from device" << endl;
        file << "# useless_number filename" << endl;
    }

    void append(InputArray _depth)
    {
        Mat depth = _depth.getMat();
        string depthFname = cv::format("%04d.png", count);
        string fullDepthFname = dir + '/' + depthFname;
        if(!imwrite(fullDepthFname, depth))
            throw std::runtime_error("Failed to write depth to file " + fullDepthFname);
        file << count++ << " " << depthFname << endl;
    }

    fstream file;
    int count;
    string dir;
};

namespace Kinect2Params
{
    static const Size frameSize = Size(512, 424);
    // approximate values, no guarantee to be correct
    static const float focal = 366.1f;
    static const float cx = 258.2f;
    static const float cy = 204.f;
    static const float k1 =  0.12f;
    static const float k2 = -0.34f;
    static const float k3 =  0.12f;
};

struct DepthSource
{
public:
    DepthSource(int cam) :
        DepthSource("", cam)
    { }

    DepthSource(String fileListName) :
        DepthSource(fileListName, -1)
    { }

    DepthSource(String fileListName, int cam) :
        depthFileList(fileListName.empty() ? vector<string>() : readDepth(fileListName)),
        frameIdx(0),
        vc( cam >= 0 ? VideoCapture(VideoCaptureAPIs::CAP_OPENNI2 + cam) : VideoCapture()),
        undistortMap1(),
        undistortMap2(),
        useKinect2Workarounds(true)
    { }

    UMat getDepth()
    {
        UMat out;
        if (!vc.isOpened())
        {
            if (frameIdx < depthFileList.size())
            {
                Mat f = cv::imread(depthFileList[frameIdx++], IMREAD_ANYDEPTH);
                f.copyTo(out);
            }
            else
            {
                return UMat();
            }
        }
        else
        {
            vc.grab();
            vc.retrieve(out, CAP_OPENNI_DEPTH_MAP);

            // workaround for Kinect 2
            if(useKinect2Workarounds)
            {
                out = out(Rect(Point(), Kinect2Params::frameSize));

                UMat outCopy;
                // linear remap adds gradient between valid and invalid pixels
                // which causes garbage, use nearest instead
                remap(out, outCopy, undistortMap1, undistortMap2, cv::INTER_NEAREST);

                cv::flip(outCopy, out, 1);
            }
        }
        if (out.empty())
            throw std::runtime_error("Matrix is empty");
        return out;
    }

    bool empty()
    {
        return depthFileList.empty() && !(vc.isOpened());
    }

    void updateParams(Params& params)
    {
        if (vc.isOpened())
        {
            // this should be set in according to user's depth sensor
            int w = (int)vc.get(VideoCaptureProperties::CAP_PROP_FRAME_WIDTH);
            int h = (int)vc.get(VideoCaptureProperties::CAP_PROP_FRAME_HEIGHT);

            float focal = (float)vc.get(CAP_OPENNI_DEPTH_GENERATOR | CAP_PROP_OPENNI_FOCAL_LENGTH);

            // it's recommended to calibrate sensor to obtain its intrinsics
            float fx, fy, cx, cy;
            Size frameSize;
            if(useKinect2Workarounds)
            {
                fx = fy = Kinect2Params::focal;
                cx = Kinect2Params::cx;
                cy = Kinect2Params::cy;

                frameSize = Kinect2Params::frameSize;
            }
            else
            {
                fx = fy = focal;
                cx = w/2 - 0.5f;
                cy = h/2 - 0.5f;

                frameSize = Size(w, h);
            }

            Matx33f camMatrix = Matx33f(fx,  0, cx,
                                        0,  fy, cy,
                                        0,   0,  1);

            params.frameSize = frameSize;
            params.intr = camMatrix;
            params.depthFactor = 1000.f;

            Matx<float, 1, 5> distCoeffs;
            distCoeffs(0) = Kinect2Params::k1;
            distCoeffs(1) = Kinect2Params::k2;
            distCoeffs(4) = Kinect2Params::k3;
            if(useKinect2Workarounds)
                initUndistortRectifyMap(camMatrix, distCoeffs, cv::noArray(),
                                        camMatrix, frameSize, CV_16SC2,
                                        undistortMap1, undistortMap2);
        }
    }

    vector<string> depthFileList;
    size_t frameIdx;
    VideoCapture vc;
    UMat undistortMap1, undistortMap2;
    bool useKinect2Workarounds;
};


static const char* keys =
{
    "{help h usage ? | | print this message   }"
    "{depth  | | Path to depth.txt file listing a set of depth images }"
    "{camera |0| Index of depth camera to be used as a depth source }"
    "{coarse | | Run on coarse settings (fast but ugly) or on default (slow but looks better),"
        " in coarse mode points and normals are displayed }"
    "{idle   | | Do not run KinFu, just display depth frames }"
    "{record | | Write depth frames to specified file list"
        " (the same format as for the 'depth' key) }"
};

static const std::string message =
 "\nThis demo uses live depth input or RGB-D dataset taken from"
 "\nhttps://vision.in.tum.de/data/datasets/rgbd-dataset"
 "\nto demonstrate KinectFusion implementation \n";


int main(int argc, char **argv)
{
    bool coarse = false;
    bool idle = false;
    string recordPath;

    CommandLineParser parser(argc, argv, keys);
    parser.about(message);

    if(!parser.check())
    {
        parser.printMessage();
        parser.printErrors();
        return -1;
    }

    if(parser.has("help"))
    {
        parser.printMessage();
        return 0;
    }
    if(parser.has("coarse"))
    {
        coarse = true;
    }
    if(parser.has("record"))
    {
        recordPath = parser.get<String>("record");
    }
    if(parser.has("idle"))
    {
        idle = true;
    }

    Ptr<DepthSource> ds;
    if (parser.has("depth"))
        ds = makePtr<DepthSource>(parser.get<String>("depth"));
    else
        ds = makePtr<DepthSource>(parser.get<int>("camera"));

    if (ds->empty())
    {
        std::cerr << "Failed to open depth source" << std::endl;
        parser.printMessage();
        return -1;
    }

    Ptr<DepthWriter> depthWriter;
    if(!recordPath.empty())
        depthWriter = makePtr<DepthWriter>(recordPath);

    Ptr<Params> params;
    Ptr<KinFu> kf;

    if(coarse)
        params = Params::coarseParams();
    else
        params = Params::defaultParams();
    // These params can be different for each depth sensor
    ds->updateParams(*params);
    // Enables OpenCL explicitly (by default can be switched-off)
    cv::setUseOptimized(true);
    if(!idle)
        kf = KinFu::create(params);
    UMat rendered;
    UMat points;
    UMat normals;
    int64 prevTime = getTickCount();
    for(UMat frame = ds->getDepth(); !frame.empty(); frame = ds->getDepth())
    {
        if(depthWriter)
            depthWriter->append(frame);
        {
            UMat cvt8;
            float depthFactor = params->depthFactor;
            convertScaleAbs(frame, cvt8, 0.25*256. / depthFactor);
            if(!idle)
            {
                imshow("depth", cvt8);

                if(!kf->update(frame))
                {
                    kf->reset();
                    std::cout << "reset" << std::endl;
                }
                kf->render(rendered);
            }
            else
            {
                rendered = cvt8;
            }
        }

        int64 newTime = getTickCount();
        putText(rendered, cv::format("FPS: %2d press R to reset, P to pause, Q to quit",
                                     (int)(getTickFrequency()/(newTime - prevTime))),
                Point(0, rendered.rows-1), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0, 255, 255));
        prevTime = newTime;

        imshow("render", rendered);

        int c = waitKey(1);
        switch (c)
        {
        case 'r':
            if(!idle)
                kf->reset();
            break;
        case 'q':
            return 0;
        default:
            break;
        }
    }
    return 0;
}