RealSenseSDK2で距離マップと画像の座標を一致した状態で取得する

D415やD435でrealsenseSDK2を使い距離と画像を同時取得しようと別々に処理かけると座標と見える領域にズレが生じます。
距離情報と画像情報を使って処理させるにはこのズレを一致させないといけません。

環境

ubuntu16.04 kernel 4.4.0-130-generic
opencv3.4

画像と距離情報の取得

例えば以下のページでenableして画像と距離情報を取得しているのは、ズレが生じた状態です。
librealsense/getting_started_with_openCV.md at master · IntelRealSense/librealsense · GitHub


座標系を一致した状態で取得

サンプルにあるDNNから編集しました。

#include <librealsense2/rs.hpp>
#include "cv-helpers.hpp"
#include <opencv2/opencv.hpp>   // Include OpenCV API
#include <iostream>
#include <stdio.h>
#include <stdlib.h>

using namespace cv;
using namespace std;
using namespace rs2;
const size_t inWidth      = 640;
const size_t inHeight     = 480;
const float WHRatio       = inWidth / (float)inHeight;

int main(int argc, char** argv) try
{

    // Start streaming from Intel RealSense Camera
    pipeline pipe;
    auto config = pipe.start();
    auto profile = config.get_stream(RS2_STREAM_COLOR)
                         .as<video_stream_profile>();
    rs2::align align_to(RS2_STREAM_COLOR);

    Size cropSize;
    if (profile.width() / (float)profile.height() > WHRatio)
    {
        cropSize = Size(static_cast<int>(profile.height() * WHRatio),
                        profile.height());
    }
    else
    {
        cropSize = Size(profile.width(),
                        static_cast<int>(profile.width() / WHRatio));
    }

    Rect crop(Point((profile.width() - cropSize.width) / 2,
                    (profile.height() - cropSize.height) / 2),
              cropSize);

    const auto window_name = "Display Image";
    namedWindow(window_name, WINDOW_AUTOSIZE);

    while (cvGetWindowHandle(window_name))
    {
        // Wait for the next set of frames
        auto data = pipe.wait_for_frames();
        // Make sure the frames are spatially aligned
        data = align_to.process(data);

        auto color_frame = data.get_color_frame();
        auto depth_frame = data.get_depth_frame();

        // If we only received new depth frame, 
        // but the color did not update, continue
        static int last_frame_number = 0;
        if (color_frame.get_frame_number() == last_frame_number) continue;
        last_frame_number = color_frame.get_frame_number();

        // Convert RealSense frame to OpenCV matrix:
        auto color_mat = frame_to_mat(color_frame);
        auto depth_mat = depth_frame_to_meters(pipe, depth_frame);
        
        imshow(window_name, color_mat);
        imshow("img", depth_mat);
        char key = waitKey( 5 );   

    }
}catch (const rs2::error & e)
{
    std::cerr << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args() << "):\n    " << e.what() << std::endl;
    return EXIT_FAILURE;
}
catch (const std::exception& e)
{
    std::cerr << e.what() << std::endl;
    return EXIT_FAILURE;
}