VS2019創建RealSense2.0 SDK新項目--使用深度數據測量物理世界中兩點間的距離

項目：

硬件平臺：

 win10 x64   

Visual Studio 2019     

Intel Realsense SDK 2.0

這是在官網上下載的例程，默認安裝在C盤,include是頭文件，samples是例子
先說明：Hello第一個例子添加的依賴項只需要realsense2.lib就行，其他的例程則需要添加更多的依賴項，否則會報錯

問題描述：

在VS新建工程這裏坑比較多，以下是手把手教建工程，如果已經配置成功的可略過不看哦~

1.環境變量配置（設置完之後，建議電腦重啓一下，不過問題不大，看實際情況）
電腦——屬性——高級系統設置——環境變量——系統變量——Path——如下圖：

2.打開vs，新建一個工程，我把這個工程放在E盤,然後在解決方案資源管理器，右鍵打開屬性
引入頭文件和庫目錄+添加依賴項，完成一切後，連上相機就可以啦~

原因分析：

大量的頭文件和依賴項，沒有的話，靠一個main函數跑不了，路徑什麼的一定要設置好纔行，不過每次新建都得引入一次是真的煩人，可以試下在其他的環境下跑吧。

關於用深度測兩點間的距離：

本教程介紹了使用深度數據測量現實距離的簡單方法。

注意：測量真實對象的尺寸是深度相機的明顯應用之一。
該示例並不是要成爲合適的度量工具，而是要展示關鍵概念。
使用更好的算法，可以大大提高測量結果。

在本教程中，您將學習如何：

在空間上使顏色流與深度對齊（與rs-align中的深度與顏色對齊相反）
利用後處理來處理丟失或嘈雜的深度數據
在2D像素和3D空間中的點之間轉換
利用多核來並行化數據流
使用OpenGL在深度上方疊加顏色

#include<librealsense2/rs.hpp>
#include<librealsense2/rsutil.h>
#include "example.hpp"

#include<math.h>
//#include<corecrt_math_defines.h>  //該頭文件包含M_PI的定義
#include<queue>
#include<unordered_set>
#include<map>
#include<thread>
#include<atomic>
#include<mutex>

#define _USE_MATH_DEFINES
#define M_PI       3.14159265358979323846   // pi
using pixel = std::pair<int, int>;

//Distance 3D 用於計算兩個像素之間的真實3D距離
float dist_3d(const rs2::depth_frame& frame, pixel u, pixel v);

//Toggle helper類將用於渲染兩個按鈕
//控制尺子的邊緣
struct toggle
{
   
   
    toggle() : x(0.f), y(0.f) {
   
   }
    toggle(float xl, float yl)
        : x(std::min(std::max(xl, 0.f), 1.f)),
        y(std::min(std::max(yl, 0.f), 1.f))
    {
   
   }

    //從[0,1]空間移動到特定幀的像素空間
    pixel get_pixel(rs2::depth_frame frm) const
    {
   
   
        int px = x * frm.get_width();
        int py = y * frm.get_height();
        return{
   
    px, py };
    }

    void render(const window& app)
    {
   
   
        glColor4f(0.f, 0.0f, 0.0f, 0.2f);
        render_circle(app, 10);
        render_circle(app, 8);
        glColor4f(1.f, 0.9f, 1.0f, 1.f);
        render_circle(app, 6);
    }

    void render_circle(const window& app, float r)
    {
   
   
        const float segments = 16;
        glBegin(GL_TRIANGLE_STRIP);
        for (auto i = 0; i <= segments; i++)
        {
   
   
            auto t = 2 * M_PI * float(i) / segments;

            glVertex2f(x * app.width() + cos(t) * r,
                y * app.height() + sin(t) * r);

            glVertex2f(x * app.width(),
                y * app.height());
        }
        glEnd();
    }

    //這個輔助函數用於查找最接近鼠標光標的按鈕
    //因爲只比較這個距離，可安全的跳過sqrt
    float dist_2d(const toggle& other) const
    {
   
   
        return pow(x - other.x, 2) + pow(y - other.y, 2);
    }

    float x;
    float y;
    bool selected = false;
};

//在主線程和GLFW事件之間共享應用狀態
struct state
{
   
   
    bool mouse_down = false;
    toggle ruler_start;
    toggle ruler_end;
};
//註冊UI事件的helper函數
void register_glfw_callbacks(window& app, state& app_state);


// Simple distance是3D點之間的經典畢達哥拉斯距離
//這個距離忽略了對象的拓撲結構，並且可以通過兩者
//通過空氣和固體
void render_simple_distance(const rs2::depth_frame& depth,
    const state& s,
    const window& app);

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

    //顏色和深度幀的OpenGL紋理t
    texture depth_image, color_image;
/
//深度處理管道

    //着色器用於可視化深度數據
    rs2::colorizer color_map;

    //使用黑到白的彩色地圖
    color_map.set_option(RS2_OPTION_COLOR_SCHEME, 2.f);

    //Decimation filter減少數據量（同時保留最好的樣本）
    rs2::decimation_filter dec;

    //如果演示太慢，確保你在release中運行（-DCMAK_BUILD_TYPE=Release）
    //但你也可增加下面的參數來減少深度（降低質量）
    dec.set_option(RS2_OPTION_FILTER_MAGNITUDE, 2);

    //定義視差域和視差域的轉換
    rs2::disparity_transform depth2disparity;
    rs2::disparity_transform disparity2depth(false);

    //定義空間濾波器（邊緣保持）
    rs2::spatial_filter spat;

    //啓用hole-filling
    //填洞是一種侵略性的啓發式方法，它多次得到錯誤的深度
    //但是，這個演示程序不是用來處理漏洞的
    spat.set_option(RS2_OPTION_HOLES_FILL, 5);//5是所有的0管道

    //定義時間過濾器
    rs2::temporal_filter temp;

    //空間對齊所有流到深度視口
    //(1)通常depth有更寬的FOV，只需要depth來做這個演示
    //(2)不想引入新的漏洞
    rs2::align align_to(RS2_STREAM_DEPTH);


//接下來，我們爲深度+顏色流配置相機管道：


    //聲明Realsende管道，封裝實際的設備和傳感器
    
    rs2::pipeline pipe;

    rs2::config cfg;
   
    //啓動缺省深度
    cfg.enable_stream(RS2_STREAM_DEPTH);

    //對於顏色流，設置格式爲RGBA
    //允許在深度框架上混合顏色框架
    cfg.enable_stream(RS2_STREAM_COLOR, RS2_FORMAT_RGBA8);
    auto profile = pipe.start(cfg);


//我們的目標是生成沒有任何孔的深度，因爲這些孔將對我們的算法造成直接的問題。
//減少像素丟失的最好方法是讓硬件來完成。
//D400攝像機具有我們可以利用的高密度預設。

    auto sensor = profile.get_device().first<rs2::depth_sensor>();

    //將設備調至D400立體相機的高精度預設置值
    if (sensor && sensor.is<rs2::depth_stereo_sensor>())
    {
   
   
        sensor.set_option(RS2_OPTION_VISUAL_PRESET, RS2_RS400_VISUAL_PRESET_HIGH_ACCURACY);
    }

    auto stream = profile.get_stream(RS2_STREAM_DEPTH).as<rs2::video_stream_profile>(); 

    //創建一個簡單的OpenGL窗口用於渲染
    window app(stream.width(), stream.height(), "RealSense Measure Example");

    //定義應用程序狀態和定位標尺按鈕
    state app_state;
    app_state.ruler_start = {
   
    0.45f, 0.5f };
    app_state.ruler_end = {
   
    0.55f, 0.5f };
    register_glfw_callbacks(app, app_state);

    //初始化後處理後，幀將流入這個隊列
    rs2::frame_queue postprocessed_frames;

    //bool值將通知工作線程結束
    std::atomic_bool alive{
   
    true };

    //視頻處理線程將從相機中獲取幀，應用後處理並將結果發送給主線程進行渲染
    //它接收同步的(但不是空間對齊的)對，並輸出同步的和對齊的對

    std::thread video_processing_thread([&]() {
   
   
        while (alive)
        {
   
   
            //從管道中獲取幀並將它們發送給處理
            rs2::frameset data;
            if (pipe.poll_for_frames(&data))
            {
   
   
                //首先使幀空間對齊
                data = data.apply_filter(align_to);

                //抽取將降低深度圖像的結果，封閉小孔，加速算法
                data = data.apply_filter(dec);

                //確保遠處的對象按比例過濾
                data = data.apply_filter(depth2disparity);

                //切換到視差域
                data = data.apply_filter(spat);

                //應用時間濾波
                data = data.apply_filter(temp);

                //如果在視差域，切換回深度
                data = data.apply_filter(disparity2depth);

                //應用彩色地圖來顯示深度
                data = data.apply_filter(color_map);

                //在主線程中發送結果幀
                postprocessed_frames.enqueue(data);


            }

        }

        });
    rs2::frameset current_frameset;
    while (app)//應用程序仍然活着?
    {
   
   
        //獲取最新可用的後處理框架集
        postprocessed_frames.poll_for_frame(&current_frameset);

        if (current_frameset)
        {
   
   
            auto depth = current_frameset.get_depth_frame();
            auto color = current_frameset.get_color_frame();
            auto colorized_depth = current_frameset.first(RS2_STREAM_DEPTH, RS2_FORMAT_RGB8);

            glEnable(GL_BLEND);

            //使用Alpha通道進行混合
            glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

            //首先渲染彩色深度圖像
            depth_image.render(colorized_depth, {
   
    0, 0, app.width(), app.height() });

            //渲染顏色幀(因爲我們選擇了RGBA格式，FOV中的像素會顯示爲透明)
            color_image.render(color, {
   
    0, 0, app.width(), app.height() });

            //渲染簡單的畢達哥拉斯距離
            render_simple_distance(depth, app_state, app);

            // Render the ruler
            app_state.ruler_start.render(app);
            app_state.ruler_end.render(app);

            glColor3f(1.f, 1.f, 1.f);
            glDisable(GL_BLEND);
        }
    }
    //通知線程完成並等待它們完成
    alive = false;
    video_processing_thread.join();
    return EXIT_SUCCESS;
}

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

/
//使用類get_distance功能可以獲取以米爲單位的距離depth_frame。

//get_distance過度調用可能會導致性能下降，因爲編譯器無法跨模塊邊界進行優化，因此DEPTH_UNITS直接從中讀取選項depth_sensor並將其用於將原始深度像素轉換爲米可能是有益的。

//將所有內容放在一起會產生相當冗長的dist_3d功能：

float dist_3d(const rs2::depth_frame& frame, pixel u, pixel v)
{
   
   
    float upixel[2]; // From pixel(像素)
    float upoint[3]; // From point (in 3D)（點，3D）

    float vpixel[2]; // To pixel
    float vpoint[3]; // To point (in 3D)

    //將像素複製到數組中(以匹配rsutil簽名)
    upixel[0] = u.first;
    upixel[1] = u.second;
    vpixel[0] = v.first;
    vpixel[1] = v.second;

    //查詢幀的距離
    //注意:這個可以被優化
    //不建議對每個像素都調用API(因爲編譯器不能內聯這些)
    //然而，在這個例子中，它不是瓶頸之一
    auto udist = frame.get_distance(upixel[0], upixel[1]);
    auto vdist = frame.get_distance(vpixel[0], vpixel[1]);

    //在3D中從像素到點的反投影
    rs2_intrinsics intr = frame.get_profile().as<rs2::video_stream_profile>().get_intrinsics(); // Calibration data
    rs2_deproject_pixel_to_point(upoint, &intr, upixel, udist);
    rs2_deproject_pixel_to_point(vpoint, &intr, vpixel, vdist);

    //計算兩點之間的歐氏距離
    return sqrt(pow(upoint[0] - vpoint[0], 2) +
        pow(upoint[1] - vpoint[1], 2) +
        pow(upoint[2] - vpoint[2], 2));
}

void draw_line(float x0, float y0, float x1, float y1, int width)
{
   
   
    glPushAttrib(GL_ENABLE_BIT);
    glLineStipple(1, 0x00ff);
    glEnable(GL_LINE_STIPPLE);
    glLineWidth(width);
    glBegin(GL_LINE_STRIP);
    glVertex2f(x0, y0);
    glVertex2f(x1, y1);
    glEnd();
    glPopAttrib();
}

void render_simple_distance(const rs2::depth_frame& depth,
    const state& s,
    const window& app)
{
   
   
    pixel center;

    glColor4f(0.f, 0.0f, 0.0f, 0.2f);
    draw_line(s.ruler_start.x * app.width(),
        s.ruler_start.y * app.height(),
        s.ruler_end.x * app.width(),
        s.ruler_end.y * app.height(), 9);

    glColor4f(0.f, 0.0f, 0.0f, 0.3f);
    draw_line(s.ruler_start.x * app.width(),
        s.ruler_start.y * app.height(),
        s.ruler_end.x * app.width(),
        s.ruler_end.y * app.height(), 7);

    glColor4f(1.f, 1.0f, 1.0f, 1.f);
    draw_line(s.ruler_start.x * app.width(),
        s.ruler_start.y * app.height(),
        s.ruler_end.x * app.width(),
        s.ruler_end.y * app.height(), 3);

    auto from_pixel = s.ruler_start.get_pixel(depth);
    auto to_pixel = s.ruler_end.get_pixel(depth);
    float air_dist = dist_3d(depth, from_pixel, to_pixel);

    center.first = (from_pixel.first + to_pixel.first) / 2;
    center.second = (from_pixel.second + to_pixel.second) / 2;

    std::stringstream ss;
    ss << int(air_dist * 100) << " cm";
    auto str = ss.str();
    auto x = (float(center.first) / depth.get_width()) * app.width() + 15;
    auto y = (float(center.second) / depth.get_height()) * app.height() + 15;

    auto w = stb_easy_font_width((char*)str.c_str());

    //爲文本標籤繪製黑色背景
    glColor4f(0.f, 0.f, 0.f, 0.4f);
    glBegin(GL_TRIANGLES);
    glVertex2f(x - 3, y - 10);
    glVertex2f(x + w + 2, y - 10);
    glVertex2f(x + w + 2, y + 2);
    glVertex2f(x + w + 2, y + 2);
    glVertex2f(x - 3, y + 2);
    glVertex2f(x - 3, y - 10);
    glEnd();

    //繪製白色文本標籤
    glColor4f(1.f, 1.f, 1.f, 1.f);
    draw_text(x, y, str.c_str());
}

//實現按鈕的拖放行爲:
void register_glfw_callbacks(window& app, state& app_state)
{
   
   
    app.on_left_mouse = [&](bool pressed)
    {
   
   
        app_state.mouse_down = pressed;
    };

    app.on_mouse_move = [&](double x, double y)
    {
   
   
        toggle cursor{
   
    float(x) / app.width(), float(y) / app.height() };
        std::vector<toggle*> toggles{
   
   
            &app_state.ruler_start,
            &app_state.ruler_end };

        if (app_state.mouse_down)
        {
   
   
            toggle* best = toggles.front();
            for (auto&& t : toggles)
            {
   
   
                if (t->dist_2d(cursor) < best->dist_2d(cursor))
                {
   
   
                    best = t;
                }
            }
            best->selected = true;
        }
        else
        {
   
   
            for (auto&& t : toggles) t->selected = false;
        }

        for (auto&& t : toggles)
        {
   
   
            if (t->selected) *t = cursor;
        }
    };
}

我們使用glBlendFunc顏色Alpha通道在深度上疊加對齊顏色（流必須具有一定格式RGBA才能正常工作）。

此示例說明了一個簡短而複雜的處理流程。每個線程的速率略有不同，它們都需要同步但彼此之間不必阻塞。
這是通過使用線程安全frame_queue的作爲同步原語和rs2::frame引用計數來跨線程進行對象生存期管理來實現的。

註釋可能表達的不太準確，可去官網下載源碼，自己翻譯~
第一次寫，主要是想讓大家遇見這麼沒有什麼技術性可言的坑時能順利跳過~