kinfu_demo.cpp 15.5 KB
Newer Older
1 2 3 4 5 6 7 8 9
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html

// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this module's directory

#include <iostream>
#include <fstream>
#include <opencv2/imgproc.hpp>
Alexander Alekhin's avatar
Alexander Alekhin committed
10
#include <opencv2/calib3d.hpp>
11 12 13 14 15 16 17
#include <opencv2/highgui.hpp>
#include <opencv2/rgbd/kinfu.hpp>

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

18 19 20 21
#ifdef HAVE_OPENCV_VIZ
#include <opencv2/viz.hpp>
#endif

22 23 24 25 26 27 28 29
static vector<string> readDepth(std::string fileList);

static vector<string> readDepth(std::string fileList)
{
    vector<string> v;

    fstream file(fileList);
    if(!file.is_open())
30
        throw std::runtime_error("Failed to read depth list");
31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51

    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;
}

52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81
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;
};
82

83 84 85 86 87 88 89 90 91 92 93
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;
};
94 95 96 97

struct DepthSource
{
public:
98 99 100 101 102 103 104 105
    enum Type
    {
        DEPTH_LIST,
        DEPTH_KINECT2_LIST,
        DEPTH_KINECT2,
        DEPTH_REALSENSE
    };

106
    DepthSource(int cam) :
107
        DepthSource("", cam)
108 109 110
    { }

    DepthSource(String fileListName) :
111 112 113 114 115
        DepthSource(fileListName, -1)
    { }

    DepthSource(String fileListName, int cam) :
        depthFileList(fileListName.empty() ? vector<string>() : readDepth(fileListName)),
116
        frameIdx(0),
117
        undistortMap1(),
118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141
        undistortMap2()
    {
        if(cam >= 0)
        {
            vc = VideoCapture(VideoCaptureAPIs::CAP_OPENNI2 + cam);
            if(vc.isOpened())
            {
                sourceType = Type::DEPTH_KINECT2;
            }
            else
            {
                vc = VideoCapture(VideoCaptureAPIs::CAP_REALSENSE + cam);
                if(vc.isOpened())
                {
                    sourceType = Type::DEPTH_REALSENSE;
                }
            }
        }
        else
        {
            vc = VideoCapture();
            sourceType = Type::DEPTH_KINECT2_LIST;
        }
    }
142

143
    UMat getDepth()
144
    {
145
        UMat out;
146 147 148
        if (!vc.isOpened())
        {
            if (frameIdx < depthFileList.size())
149 150 151 152
            {
                Mat f = cv::imread(depthFileList[frameIdx++], IMREAD_ANYDEPTH);
                f.copyTo(out);
            }
153 154
            else
            {
155
                return UMat();
156 157 158 159 160
            }
        }
        else
        {
            vc.grab();
161 162 163 164 165 166 167 168 169 170 171 172
            switch (sourceType)
            {
            case Type::DEPTH_KINECT2:
                vc.retrieve(out, CAP_OPENNI_DEPTH_MAP);
                break;
            case Type::DEPTH_REALSENSE:
                vc.retrieve(out, CAP_INTELPERC_DEPTH_MAP);
                break;
            default:
                // unknown depth source
                vc.retrieve(out);
            }
173 174

            // workaround for Kinect 2
175
            if(sourceType == Type::DEPTH_KINECT2)
176
            {
177 178 179 180 181 182 183 184
                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);
185 186 187 188 189 190 191 192 193 194 195 196
            }
        }
        if (out.empty())
            throw std::runtime_error("Matrix is empty");
        return out;
    }

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

197
    void updateParams(Params& params)
198 199 200 201 202 203 204 205 206
    {
        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);

            // it's recommended to calibrate sensor to obtain its intrinsics
            float fx, fy, cx, cy;
207
            float depthFactor = 1000.f;
208
            Size frameSize;
209
            if(sourceType == Type::DEPTH_KINECT2)
210 211 212 213 214 215 216 217 218
            {
                fx = fy = Kinect2Params::focal;
                cx = Kinect2Params::cx;
                cy = Kinect2Params::cy;

                frameSize = Kinect2Params::frameSize;
            }
            else
            {
219 220 221 222 223 224 225 226 227 228 229
                if(sourceType == Type::DEPTH_REALSENSE)
                {
                    fx = (float)vc.get(CAP_PROP_INTELPERC_DEPTH_FOCAL_LENGTH_HORZ);
                    fy = (float)vc.get(CAP_PROP_INTELPERC_DEPTH_FOCAL_LENGTH_VERT);
                    depthFactor = 1.f/(float)vc.get(CAP_PROP_INTELPERC_DEPTH_SATURATION_VALUE);
                }
                else
                {
                    fx = fy = (float)vc.get(CAP_OPENNI_DEPTH_GENERATOR | CAP_PROP_OPENNI_FOCAL_LENGTH);
                }

230 231 232 233 234 235 236 237 238 239 240 241
                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;
242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264
            params.depthFactor = depthFactor;

            // RealSense has shorter depth range, some params should be tuned
            if(sourceType == Type::DEPTH_REALSENSE)
            {
                // all sizes in meters
                float cubeSize = 1.f;
                params.voxelSize = cubeSize/params.volumeDims[0];
                params.tsdf_trunc_dist = 0.01f;
                params.icpDistThresh = 0.01f;
                params.volumePose = Affine3f().translate(Vec3f(-cubeSize/2.f,
                                                               -cubeSize/2.f,
                                                               0.05f));
                params.truncateThreshold = 2.5f;
                params.bilateral_sigma_depth = 0.01f;
            }

            if(sourceType == Type::DEPTH_KINECT2)
            {
                Matx<float, 1, 5> distCoeffs;
                distCoeffs(0) = Kinect2Params::k1;
                distCoeffs(1) = Kinect2Params::k2;
                distCoeffs(4) = Kinect2Params::k3;
265 266 267 268

                initUndistortRectifyMap(camMatrix, distCoeffs, cv::noArray(),
                                        camMatrix, frameSize, CV_16SC2,
                                        undistortMap1, undistortMap2);
269
            }
270 271 272 273 274 275
        }
    }

    vector<string> depthFileList;
    size_t frameIdx;
    VideoCapture vc;
276
    UMat undistortMap1, undistortMap2;
277
    Type sourceType;
278 279
};

280
#ifdef HAVE_OPENCV_VIZ
281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299
const std::string vizWindowName = "cloud";

struct PauseCallbackArgs
{
    PauseCallbackArgs(KinFu& _kf) : kf(_kf)
    { }

    KinFu& kf;
};

void pauseCallback(const viz::MouseEvent& me, void* args);
void pauseCallback(const viz::MouseEvent& me, void* args)
{
    if(me.type == viz::MouseEvent::Type::MouseMove       ||
       me.type == viz::MouseEvent::Type::MouseScrollDown ||
       me.type == viz::MouseEvent::Type::MouseScrollUp)
    {
        PauseCallbackArgs pca = *((PauseCallbackArgs*)(args));
        viz::Viz3d window(vizWindowName);
300
        UMat rendered;
301
        pca.kf.render(rendered, window.getViewerPose().matrix);
302 303 304 305
        imshow("render", rendered);
        waitKey(1);
    }
}
306
#endif
307 308 309 310

static const char* keys =
{
    "{help h usage ? | | print this message   }"
311
    "{depth  | | Path to depth.txt file listing a set of depth images }"
312
    "{camera |0| Index of depth camera to be used as a depth source }"
313 314
    "{coarse | | Run on coarse settings (fast but ugly) or on default (slow but looks better),"
        " in coarse mode points and normals are displayed }"
315 316 317
    "{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) }"
318 319 320
};

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


int main(int argc, char **argv)
{
    bool coarse = false;
329 330
    bool idle = false;
    string recordPath;
331 332 333

    CommandLineParser parser(argc, argv, keys);
    parser.about(message);
334 335 336 337 338 339 340 341

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

342 343 344 345 346 347 348 349 350
    if(parser.has("help"))
    {
        parser.printMessage();
        return 0;
    }
    if(parser.has("coarse"))
    {
        coarse = true;
    }
351
    if(parser.has("record"))
352
    {
353 354 355 356 357
        recordPath = parser.get<String>("record");
    }
    if(parser.has("idle"))
    {
        idle = true;
358 359
    }

360
    Ptr<DepthSource> ds;
361
    if (parser.has("depth"))
362 363 364
        ds = makePtr<DepthSource>(parser.get<String>("depth"));
    else
        ds = makePtr<DepthSource>(parser.get<int>("camera"));
365

366
    if (ds->empty())
367 368 369 370 371
    {
        std::cerr << "Failed to open depth source" << std::endl;
        parser.printMessage();
        return -1;
    }
372

373 374 375 376
    Ptr<DepthWriter> depthWriter;
    if(!recordPath.empty())
        depthWriter = makePtr<DepthWriter>(recordPath);

377
    Ptr<Params> params;
378 379
    Ptr<KinFu> kf;

380
    if(coarse)
381
        params = Params::coarseParams();
382
    else
383
        params = Params::defaultParams();
384

385
    // These params can be different for each depth sensor
386
    ds->updateParams(*params);
387

388 389 390
    // Enables OpenCL explicitly (by default can be switched-off)
    cv::setUseOptimized(true);

391
    // Scene-specific params should be tuned for each scene individually
392 393 394 395 396
    //float cubeSize = 1.f;
    //params->voxelSize = cubeSize/params->volumeDims[0]; //meters
    //params->tsdf_trunc_dist = 0.01f; //meters
    //params->icpDistThresh = 0.01f; //meters
    //params->volumePose = Affine3f().translate(Vec3f(-cubeSize/2.f, -cubeSize/2.f, 0.25f)); //meters
397
    //params->tsdf_max_weight = 16;
398

399 400
    if(!idle)
        kf = KinFu::create(params);
401

402
#ifdef HAVE_OPENCV_VIZ
403 404
    cv::viz::Viz3d window(vizWindowName);
    window.setViewerPose(Affine3f::Identity());
405 406
    bool pause = false;
#endif
407

408 409 410
    UMat rendered;
    UMat points;
    UMat normals;
411

412
    int64 prevTime = getTickCount();
413

414
    for(UMat frame = ds->getDepth(); !frame.empty(); frame = ds->getDepth())
415
    {
416 417 418
        if(depthWriter)
            depthWriter->append(frame);

419
#ifdef HAVE_OPENCV_VIZ
420 421
        if(pause)
        {
422
            // doesn't happen in idle mode
423
            kf->getCloud(points, normals);
424 425 426 427 428 429 430
            if(!points.empty() && !normals.empty())
            {
                viz::WCloud cloudWidget(points, viz::Color::white());
                viz::WCloudNormals cloudNormals(points, normals, /*level*/1, /*scale*/0.05, viz::Color::gray());
                window.showWidget("cloud", cloudWidget);
                window.showWidget("normals", cloudNormals);

431
                Vec3d volSize = kf->getParams().voxelSize*Vec3d(kf->getParams().volumeDims);
432
                window.showWidget("cube", viz::WCube(Vec3d::all(0),
433
                                                     volSize),
434 435
                                  kf->getParams().volumePose);
                PauseCallbackArgs pca(*kf);
436 437 438 439 440
                window.registerMouseCallback(pauseCallback, (void*)&pca);
                window.showWidget("text", viz::WText(cv::String("Move camera in this window. "
                                                                "Close the window or press Q to resume"), Point()));
                window.spin();
                window.removeWidget("text");
441 442
                window.removeWidget("cloud");
                window.removeWidget("normals");
443 444
                window.registerMouseCallback(0);
            }
445 446 447 448

            pause = false;
        }
        else
449
#endif
450
        {
451 452
            UMat cvt8;
            float depthFactor = params->depthFactor;
453
            convertScaleAbs(frame, cvt8, 0.25*256. / depthFactor);
454
            if(!idle)
455
            {
456 457 458 459 460 461 462
                imshow("depth", cvt8);

                if(!kf->update(frame))
                {
                    kf->reset();
                    std::cout << "reset" << std::endl;
                }
463
#ifdef HAVE_OPENCV_VIZ
464
                else
465
                {
466
                    if(coarse)
467
                    {
468 469 470 471 472 473 474 475
                        kf->getCloud(points, normals);
                        if(!points.empty() && !normals.empty())
                        {
                            viz::WCloud cloudWidget(points, viz::Color::white());
                            viz::WCloudNormals cloudNormals(points, normals, /*level*/1, /*scale*/0.05, viz::Color::gray());
                            window.showWidget("cloud", cloudWidget);
                            window.showWidget("normals", cloudNormals);
                        }
476
                    }
477

478 479 480 481 482 483 484 485
                    //window.showWidget("worldAxes", viz::WCoordinateSystem());
                    Vec3d volSize = kf->getParams().voxelSize*kf->getParams().volumeDims;
                    window.showWidget("cube", viz::WCube(Vec3d::all(0),
                                                         volSize),
                                      kf->getParams().volumePose);
                    window.setViewerPose(kf->getPose());
                    window.spinOnce(1, true);
                }
486
#endif
487

488 489 490 491 492 493
                kf->render(rendered);
            }
            else
            {
                rendered = cvt8;
            }
494 495
        }

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

        imshow("render", rendered);

504
        int c = waitKey(1);
505 506 507
        switch (c)
        {
        case 'r':
508 509
            if(!idle)
                kf->reset();
510 511 512
            break;
        case 'q':
            return 0;
513
#ifdef HAVE_OPENCV_VIZ
514
        case 'p':
515 516
            if(!idle)
                pause = true;
517
#endif
518 519 520 521 522 523 524
        default:
            break;
        }
    }

    return 0;
}