LeapMotion の画像歪みを OpenCV で除去する

モチベーション：LeapMotion の画像歪みを除去したい．

制約条件：GPU に書かせるのとかめんどくさいのでお手軽にいきたい．

Before	After

↑我が家のハムスターケージ

Code

#include "stdafx.h"
#include <Leap.h>
#include <opencv2\opencv.hpp>

cv::Mat warp(Leap::Image image) {
    cv::Mat_<cv::Vec3b> out(cv::Size(640, 480));
    const cv::Mat_<unsigned char> cvImage = cv::Mat(image.height(), image.width(), CV_8UC1, image.dataPointer());

    out.forEach([&](cv::Vec3b& pixel, const int pos[])-> void {
        float y = (float)pos[0] / out.rows;
        float x = (float)(pos[1] - 120) / out.rows;

        const Leap::Vector v(
            (x - image.rayOffsetX()) / image.rayScaleX() / 4 * 2,
            // 公式サイトでは tanθ = 4 までが範囲って言ってたけど，そんなに無いと思う．
            (y - image.rayOffsetY()) / image.rayScaleY() / 4 * 2,
            0);

        const auto warped = image.warp(v);

        if (warped.x > 0 && warped.x < (cvImage.cols - 1)
            && warped.y > 0 && warped.y < (cvImage.rows - 1)) {
            // std::cout << warped << std::endl;
            pixel = cv::Vec3b::all(cvImage(warped.y, warped.x));
        }
        else {
            pixel = cv::Vec3b(0, 255, 0);
        }
    });

    return out;
}

int main()
{
    Leap::Controller controller;
    controller.setPolicy(Leap::Controller::POLICY_IMAGES);
    controller.setPolicy(Leap::Controller::POLICY_BACKGROUND_FRAMES);

    while (cv::waitKey(1) != 'q') {
        const auto frame = controller.frame();
        if (!frame.isValid()) {
            std::cout << "Frame is Invalid" << std::endl;
            continue;
        }

        const auto imageList = frame.images();

        if (imageList.isEmpty()) {
            std::cout << "imageList.isEmpty()" << std::endl;
            continue;
        }

        const auto imageL = warp(imageList[0]);
        cv::imshow("imageL", imageL);
    }
    return 0;
}

Depth Map が欲しいなら Intel RealSence のほうがいいですね……