利用Kinect（RGB-D）进行简易的三维重建

图为一个人吃着苹果一只手手掌向上，在光源成像下的三维重建

这个是北京化工大学的一个课设，由胡伟老师带领，任务是用微软的Kinect设备做东西。

简介

Kinect是一个由Microsoft公司推出的一个具有深度摄像头和彩色摄像头的设备，并已经停产。

目的

既然已经停产，为什么我还要写这一片分享？当然其一，这是我在本学年的第三学期中的课程设计任务，学校既然曾经买了这么一批设备，就肯定要物尽其用。其二，因为Kinect具有深度摄像头，所以做一些对于纯RGB摄像机来说有困难的事用RGBD摄像机就会简单很多；而我最近在深入学习有关计算机图形学的知识。于是便决定用Kinect来进行简易的三维重建。

环境

• Windows 10
• Microsoft Visual Studio 2017
• Kinect SDK v1.8
• OpenGL v3.3 core

基本过程

• 将OpenGL与Kinect SDK对接，将传感器的数据读入并通过API传递给OpenGL渲染部分的变量中
• 对采集并接收到的点云数据进行简易的三角化，使其成为一个平面。并进行法向量的计算、光照计算等
• 对采集到的RGB信息（坐标、颜色值）与深度摄像头采集到的深度信息进行坐标匹配
• 运用SDK自带方法做基本的骨骼识别
• 做一个简单的AR（由于deadline，没有完全实现）

详细

由于Kinect官方文档已经被删除，虽然可以找到，但是微软的文档我是看得一头雾水，因此这个demo的所有关于Kinect SDK函数的调用都是仿照Kinect ToolKit中自带的demo。因此并不具有权威性与标准性。

OpenGL与Kinect SDK对接

直接去Kinect ToolKit找最简单的demo——Basic Depth D2D版本的，因为这个demo最简单，并且里面有处理深度的代码，虽然最后用不上，但是对Kinect的认知有不小帮助。由于它是用D2D（Direct 2D）写的，OpenGL显然不是，所以需要把它的BasicDepth.cpp中的人口函数删除掉，并且将深度数据（现成的，里面有）作为这个类（微软已经帮你写好了）的API，以便自己在main函数中调用。

以下为连接Kinect的方法：

/// 

/// Create the first connected Kinect found 
/// 
/// indicates success or failure
HRESULT KinectSensor::CreateFirstConnected()
{
    INuiSensor * pNuiSensor;
    HRESULT hr;

    int iSensorCount = 0;
    hr = NuiGetSensorCount(&iSensorCount);
    if (FAILED(hr))
    {
        return hr;
    }

    // Look at each Kinect sensor
    for (int i = 0; i < iSensorCount; ++i)
    {
        // Create the sensor so we can check status, if we can't create it, move on to the next
        hr = NuiCreateSensorByIndex(i, &pNuiSensor);
        if (FAILED(hr))
        {
            continue;
        }

        // Get the status of the sensor, and if connected, then we can initialize it
        hr = pNuiSensor->NuiStatus();
        if (S_OK == hr)
        {
            m_pNuiSensor = pNuiSensor;
            break;
        }

        // This sensor wasn't OK, so release it since we're not using it
        pNuiSensor->Release();
    }

    if (NULL != m_pNuiSensor)
    {
        // Initialize the Kinect and specify that we'll be using depth
        hr = m_pNuiSensor->NuiInitialize(NUI_INITIALIZE_FLAG_USES_COLOR | NUI_INITIALIZE_FLAG_USES_DEPTH | NUI_INITIALIZE_FLAG_USES_SKELETON);
        if (SUCCEEDED(hr))
        {
            // Create an event that will be signaled when depth data is available
            m_hNextDepthFrameEvent = CreateEvent(NULL, TRUE, FALSE, NULL);

            // Open a depth image stream to receive depth frames
            hr = m_pNuiSensor->NuiImageStreamOpen(
                NUI_IMAGE_TYPE_DEPTH,
                NUI_IMAGE_RESOLUTION_640x480,
                0,
                2,
                m_hNextDepthFrameEvent,
                &m_pDepthStreamHandle);

            // Create an event that will be signaled when color data is available
            m_hNextColorFrameEvent = CreateEvent(NULL, TRUE, FALSE, NULL);

            // Open a color image stream to receive color frames
            hr = m_pNuiSensor->NuiImageStreamOpen(
                NUI_IMAGE_TYPE_COLOR,
                NUI_IMAGE_RESOLUTION_640x480,
                0,
                2,
                m_hNextColorFrameEvent,
                &m_pColorStreamHandle);

            // Create an event that will be signaled when skeleton data is available
            m_hNextSkeletonEvent = CreateEventW(NULL, TRUE, FALSE, NULL);

            // Open a skeleton stream to receive skeleton data
            hr = m_pNuiSensor->NuiSkeletonTrackingEnable(m_hNextSkeletonEvent, 0);
        }
    }

    if (NULL == m_pNuiSensor || FAILED(hr))
    {
        SetStatusMessage(L"No ready Kinect found!");
        return E_FAIL;
    }

    return hr;
}

void KinectSensor::DisConnected() {
    if (m_pNuiSensor)
        m_pNuiSensor->NuiShutdown();
}

记得再写一个当关闭程序时断开与Kinect连接的方法。这些写法都能从各种官方demo中找到。

三角化点云

首先，你需要点云。最简单的方法就是自己构建，因为你已经有深度数据了，这个数组是一个一维的数组，必定是按照某种逻辑性排列的，试一试后发现他是从左向右一列一列的点的深度信息，其原点在左上角，和OpenGL的原点在y轴上颠倒。不妨将每个点都按等距排列（事实上就是这样，因为传感器肯定是均匀采集深度的），同样作一个一维数组存储每个点的x、y、z值（因为传入的数据最终都是数据流，详细可以学习一下OpenGL）。
获得点云之后，便可以将其三角化，我也试过PCL发现效果并不如意，最后采取的是最简单暴力的方法——类似于地形的构建。具体思路如图，很简单，如果有了解过一些图形学，对这个算法应该熟悉。简单的说就是用一个专门的数组indices存储要画的点在vertices数组（就是你自己构建的那个数组）中的索引值，用triangleStrip的方法画出来。

高清版，8.4M

低清版，3.4M

法向量计算也很简单，就是简单暴力的用上下左右4个点做一个求平均。

对其RGB与深度信息

2018/9/13日更新-------------------------------------------------------------------------

首先获得深度信息

模仿“DepthBasics-D2D”中的ProcessDepth方法

/// 

/// Handle new depth data
/// 
void CDepthBasics::ProcessDepth()
{
    HRESULT hr;
    NUI_IMAGE_FRAME imageFrame;

    // Attempt to get the depth frame
    hr = m_pNuiSensor->NuiImageStreamGetNextFrame(m_pDepthStreamHandle, 0, &imageFrame);
    if (FAILED(hr))
    {
        return;
    }

    BOOL nearMode;
    INuiFrameTexture* pTexture;

    // Get the depth image pixel texture
    hr = m_pNuiSensor->NuiImageFrameGetDepthImagePixelFrameTexture(
        m_pDepthStreamHandle, &imageFrame, &nearMode, &pTexture);
    if (FAILED(hr))
    {
        goto ReleaseFrame;
    }

    NUI_LOCKED_RECT LockedRect;

    // Lock the frame data so the Kinect knows not to modify it while we're reading it
    pTexture->LockRect(0, &LockedRect, NULL, 0);

    // Make sure we've received valid data
    if (LockedRect.Pitch != 0)
    {
        // Get the min and max reliable depth for the current frame
        int minDepth = (nearMode ? NUI_IMAGE_DEPTH_MINIMUM_NEAR_MODE : NUI_IMAGE_DEPTH_MINIMUM) >> NUI_IMAGE_PLAYER_INDEX_SHIFT;
        int maxDepth = (nearMode ? NUI_IMAGE_DEPTH_MAXIMUM_NEAR_MODE : NUI_IMAGE_DEPTH_MAXIMUM) >> NUI_IMAGE_PLAYER_INDEX_SHIFT;

        BYTE * rgbrun = m_depthRGBX;
        const NUI_DEPTH_IMAGE_PIXEL * pBufferRun = reinterpret_cast(LockedRect.pBits);

        // end pixel is start + width*height - 1
        const NUI_DEPTH_IMAGE_PIXEL * pBufferEnd = pBufferRun + (cDepthWidth * cDepthHeight);

        while ( pBufferRun < pBufferEnd )
        {
            // discard the portion of the depth that contains only the player index
            USHORT depth = pBufferRun->depth;

            // To convert to a byte, we're discarding the most-significant
            // rather than least-significant bits.
            // We're preserving detail, although the intensity will "wrap."
            // Values outside the reliable depth range are mapped to 0 (black).

            // Note: Using conditionals in this loop could degrade performance.
            // Consider using a lookup table instead when writing production code.
            BYTE intensity = static_cast(depth >= minDepth && depth <= maxDepth ? depth / 16 : 0);

            // Write out blue byte
            *(rgbrun++) = intensity;

            // Write out green byte
            *(rgbrun++) = intensity;

            // Write out red byte
            *(rgbrun++) = intensity;

            // We're outputting BGR, the last byte in the 32 bits is unused so skip it
            // If we were outputting BGRA, we would write alpha here.
            ++rgbrun;

            // Increment our index into the Kinect's depth buffer
            ++pBufferRun;
        }

        // Draw the data with Direct2D
        m_pDrawDepth->Draw(m_depthRGBX, cDepthWidth * cDepthHeight * cBytesPerPixel);
    }

    // We're done with the texture so unlock it
    pTexture->UnlockRect(0);

    pTexture->Release();

ReleaseFrame:
    // Release the frame
    m_pNuiSensor->NuiImageStreamReleaseFrame(m_pDepthStreamHandle, &imageFrame);
}

需注意他在这个方法里拿了深度的值作为可视化的参数，即令RGB都等于深度值。我们不需要这些操作，仅仅把深度信息保存到一个数组中，这个深度信息是一个一维数组，并且是一个像素接着一个像素的，所以我们直接保存到时在OpenGL中从x=0 y=0这个点开始一直到x=width y=height令每个点的z值分别等于数组中对应的值即可。

经过改写后

/// 

/// Handle new depth data
/// 
void KinectSensor::ProcessDepth()
{
    HRESULT hr;
    NUI_IMAGE_FRAME imageFrame;
    // Attempt to get the depth frame
    hr = m_pNuiSensor->NuiImageStreamGetNextFrame(m_pDepthStreamHandle, 0, &imageFrame);
    if (FAILED(hr)) 
    {
        return;
    }

    {
        NUI_LOCKED_RECT LockedRect;
        hr = imageFrame.pFrameTexture->LockRect(0, &LockedRect, NULL, 0);
        if (FAILED(hr)) { goto ReleaseFrame; }
        memcpy(m_depthD16, LockedRect.pBits, LockedRect.size);
        hr = imageFrame.pFrameTexture->UnlockRect(0);
        if (FAILED(hr)) { goto ReleaseFrame; };
    }

    BOOL nearMode;
    INuiFrameTexture* pTexture;

    // Get the depth image pixel texture
    hr = m_pNuiSensor->NuiImageFrameGetDepthImagePixelFrameTexture(m_pDepthStreamHandle, &imageFrame, &nearMode, &pTexture);
    if (FAILED(hr))
    {
        goto ReleaseFrame;
    }

    NUI_LOCKED_RECT LockedRect;

    // Lock the frame data so the Kinect knows not to modify it while we're reading it
    pTexture->LockRect(0, &LockedRect, NULL, 0);

    // Make sure we've received valid data
    if (LockedRect.Pitch != 0)
    {

        // Get the min and max reliable depth for the current frame
        // 限制depth的范围，因为Kinect的侦测也是有一个范围的
        int minDepth = (nearMode ? NUI_IMAGE_DEPTH_MINIMUM_NEAR_MODE : NUI_IMAGE_DEPTH_MINIMUM) >> NUI_IMAGE_PLAYER_INDEX_SHIFT;
        int maxDepth = (nearMode ? NUI_IMAGE_DEPTH_MAXIMUM_NEAR_MODE : NUI_IMAGE_DEPTH_MAXIMUM) >> NUI_IMAGE_PLAYER_INDEX_SHIFT;

        USHORT * depthValue = depthValues;

        const NUI_DEPTH_IMAGE_PIXEL * pBufferRun = reinterpret_cast(LockedRect.pBits);

        // end pixel is start + width*height - 1
        const NUI_DEPTH_IMAGE_PIXEL * pBufferEnd = pBufferRun + (cDepthWidth * cDepthHeight);

        while ( pBufferRun < pBufferEnd )
        {
            USHORT depth = pBufferRun->depth;
            *depthValue = (depth >= minDepth && depth <= maxDepth ? depth - minDepth : 0);
            depthValue++;

            // Increment our index into the Kinect's depth buffer
            ++pBufferRun;
        }
    }

    // We're done with the texture so unlock it
    pTexture->UnlockRect(0);
    
    pTexture->Release();

ReleaseFrame:
    // Release the frame
    m_pNuiSensor->NuiImageStreamReleaseFrame(m_pDepthStreamHandle, &imageFrame);


}

颜色信息基本类似，可以到我的GitHub中看

接下来就是匹配深度和颜色信息

从官方demo“DepthWithColor-D3D”中找到一个名为“MapColorToDepth”的方法
如下：

HRESULT CDepthWithColorD3D::MapColorToDepth()
{
    HRESULT hr;

    // Get of x, y coordinates for color in depth space
    // This will allow us to later compensate for the differences in location, angle, etc between the depth and color cameras
    m_pNuiSensor->NuiImageGetColorPixelCoordinateFrameFromDepthPixelFrameAtResolution(
        cColorResolution,
        cDepthResolution,
        m_depthWidth*m_depthHeight,
        m_depthD16,
        m_depthWidth*m_depthHeight*2,
        m_colorCoordinates
        );

    // copy to our d3d 11 color texture
    D3D11_MAPPED_SUBRESOURCE msT;
    hr = m_pImmediateContext->Map(m_pColorTexture2D, NULL, D3D11_MAP_WRITE_DISCARD, NULL, &msT);
    if ( FAILED(hr) ) { return hr; }
    
    // loop over each row and column of the color
    for (LONG y = 0; y < m_colorHeight; ++y)
    {
        LONG* pDest = (LONG*)((BYTE*)msT.pData + msT.RowPitch * y);
        for (LONG x = 0; x < m_colorWidth; ++x)
        {
            // calculate index into depth array
            int depthIndex = x/m_colorToDepthDivisor + y/m_colorToDepthDivisor * m_depthWidth;

            // retrieve the depth to color mapping for the current depth pixel
            LONG colorInDepthX = m_colorCoordinates[depthIndex * 2];
            LONG colorInDepthY = m_colorCoordinates[depthIndex * 2 + 1];

            // make sure the depth pixel maps to a valid point in color space
            if ( colorInDepthX >= 0 && colorInDepthX < m_colorWidth && colorInDepthY >= 0 && colorInDepthY < m_colorHeight )
            {
                // calculate index into color array
                LONG colorIndex = colorInDepthX + colorInDepthY * m_colorWidth;

                // set source for copy to the color pixel
                LONG* pSrc = (LONG *)m_colorRGBX + colorIndex;
                *pDest = *pSrc;
            }
            else
            {
                *pDest = 0;
            }

            pDest++;
        }
    }

    m_pImmediateContext->Unmap(m_pColorTexture2D, NULL);

    return hr;
}

其中重点是这个方法，虽然官方文档已经缺失，但可以大致猜到每个参数的作用。

    m_pNuiSensor->NuiImageGetColorPixelCoordinateFrameFromDepthPixelFrameAtResolution(
        cColorResolution,
        cDepthResolution,
        m_depthWidth*m_depthHeight,
        m_depthD16,
        m_depthWidth*m_depthHeight*2,
        m_colorCoordinates
        );

Kinect SDK自带方法将匹配好的颜色坐标输出到了m_colorCoordinates，从下面的代码可以了解到这是个一维数组并且排列时x坐标y坐标x坐标y坐标……

然后我们把它进行一下改造，让它适合于Opengl，即达到

void KinectSensor::ProcessColor() {

    HRESULT hr;
    NUI_IMAGE_FRAME imageFrame;

    // map color to depth

    // Get of x, y coordinates for color in depth space
    // This will allow us to later compensate for the differences in location, angle, etc between the depth and color cameras
    m_pNuiSensor->NuiImageGetColorPixelCoordinateFrameFromDepthPixelFrameAtResolution(
        NUI_IMAGE_RESOLUTION_640x480,
        NUI_IMAGE_RESOLUTION_640x480,
        kinectWidth*kinectHeight,
        m_depthD16,
        kinectWidth*kinectHeight * 2,
        m_colorCoordinates
    );

    // Attempt to get the color frame
    hr = m_pNuiSensor->NuiImageStreamGetNextFrame(m_pColorStreamHandle, 0, &imageFrame);
    if (FAILED(hr))
    {
        return;
    }

    INuiFrameTexture * pTexture = imageFrame.pFrameTexture;
    NUI_LOCKED_RECT LockedRect;

    // Lock the frame data so the Kinect knows not to modify it while we're reading it
    pTexture->LockRect(0, &LockedRect, NULL, 0);

    // Make sure we've received valid data
    if (LockedRect.Pitch != 0)
    {
        for (int j = 0; j < cDepthHeight; j++)
        {
            for (int i = 0; i < cDepthWidth; i++)
            {
                int depthIndex = i / m_colorToDepthDivisor + j / m_colorToDepthDivisor * kinectWidth;
                // retrieve the depth to color mapping for the current depth pixel
                LONG colorInDepthX = m_colorCoordinates[depthIndex * 2];
                LONG colorInDepthY = m_colorCoordinates[depthIndex * 2 + 1];
                // make sure the depth pixel maps to a valid point in color space
                if (colorInDepthX >= 0 && colorInDepthX < kinectWidth && colorInDepthY >= 0 && colorInDepthY < kinectHeight)
                {
                    //内部数据是4个字节，0-1-2是BGR，第4个现在未使用
                    colorsRGBValues[3 * (cDepthWidth * j + i) + 0] = (unsigned short)LockedRect.pBits[4 * (cDepthWidth * colorInDepthY + colorInDepthX) + 2]; // R
                    colorsRGBValues[3 * (cDepthWidth * j + i) + 1] = (unsigned short)LockedRect.pBits[4 * (cDepthWidth * colorInDepthY + colorInDepthX) + 1]; // G
                    colorsRGBValues[3 * (cDepthWidth * j + i) + 2] = (unsigned short)LockedRect.pBits[4 * (cDepthWidth * colorInDepthY + colorInDepthX) + 0]; // B
                }
            }
        }
    }
    // We're done with the texture so unlock it
    pTexture->UnlockRect(0);

    // Release the frame
    m_pNuiSensor->NuiImageStreamReleaseFrame(m_pColorStreamHandle, &imageFrame);
}

是不是非常相似的模仿。。这样我们就得到了与深度信息相对应的颜色坐标colorsRGBValues,这里有个坑，pBits里带的颜色是按BGR排列的，当时我还卡了很久

到此，我们就拥有了深度信息，颜色信息，并且这两个是匹配好的

利用OpenGL构建三维场景

基本的操作想必都知道，不知道可以去学习下怎么使用OpenGL，可以看下https://learnopengl-cn.github.io/

其实很简单，无非就是将我们的这些信息显示出来罢了，基本操作

#include "stdafx.h"
#include 
#include "KinectSensor.h"
#include "resource.h"

#include
#include

#include
#include
#include

#include"shader.h"
#include "camera.h"

#include "Triangulator.h"
#include "header.h"
#include "Mesh.h"

#include

void framebuffer_size_callback(GLFWwindow* window, int width, int height);
void mouse_callback(GLFWwindow* window, double xpos, double ypos);
void processInput(GLFWwindow *window);

// settings
const unsigned int SCR_WIDTH = 800;
const unsigned int SCR_HEIGHT = 600;

// camera
Camera camera(glm::vec3(0.0f, 0.0f, 3.0f));
float lastX = SCR_WIDTH / 2.0f;
float lastY = SCR_HEIGHT / 2.0f;
bool firstMouse = true;

// timing
float deltaTime = 0.0f; // time between current frame and last frame
float lastFrame = 0.0f;

// vertices
const int vertexStride = 18; //xyz normal(u d l r) rgb
const int rowNum = kinectHeight;
const int colNum = kinectWidth;
const int vertexCount = rowNum * colNum;
const int verticesSize = rowNum * colNum * vertexStride;

//增强可读性
const int xOffset = 0;
const int yOffset = 1;
const int zOffset = 2;
const int uxOffset = 3;
const int uyOffset = 4;
const int uzOffset = 5;
const int lxOffset = 6;
const int lyOffset = 7;
const int lzOffset = 8;
const int dxOffset = 9;
const int dyOffset = 10;
const int dzOffset = 11;
const int rxOffset = 12;
const int ryOffset = 13;
const int rzOffset = 14;
const int rOffset = 15;
const int gOffset = 16;
const int bOffset = 17;


float gapThreshold = 1;//构建面的时候判断两个点是否需要连接的阈值




DisplayMode displayMode;

int main()
{
    KinectSensor application; //初始化Kinect传感器类

    // Look for a connected Kinect, and create it if found
    application.CreateFirstConnected();

    Triangulator triangulator;//这个是无用代码，可删除


    //初始化OpenGL，省略 


    // build and compile our shader program
    Shader ourShader("shader.vs", "shader.fs"); // you can name your shader files however you like
    Shader addiMeshShader("additionalMesh.vs", "additionalMesh.fs");

    //preconstruct point
    float *vertices = new float[verticesSize];
    long indexCount = (colNum - 1) * (rowNum * 2 + 2);
    unsigned int *indices = new unsigned int[colNum * rowNum * 3];//colNum * rowNum * 3是指最坏情况，即每个点都被点了3次。
    
    Mesh cube = Mesh(Mesh::MeshType::cube);

    for (int i = 0; i < rowNum; i++) {
        for (int j = 0; j < colNum; j++)
        {
            vertices[vertexStride * (colNum * i + j) + xOffset] = ((float)j - (float)colNum / 2.0) * 0.01;   // x  range[-3.2, 3.2]
            vertices[vertexStride * (colNum * i + j) + yOffset] = -((float)i - (float)rowNum / 2.0) * 0.01;  // y
            vertices[vertexStride * (colNum * i + j) + zOffset] = 0;                                         //depth
            
            for (int k = uxOffset; k < rOffset; k++) vertices[vertexStride * (colNum * i + j) + k] = 0;      //all vertices that to be calculate
            vertices[vertexStride * (colNum * i + j) + rOffset] = 0;                                         //R
            vertices[vertexStride * (colNum * i + j) + gOffset] = 0;                                         //G
            vertices[vertexStride * (colNum * i + j) + bOffset] = 0;                                         //B
            // initialize indices values;   
            indices[(640 * i + j)] = 0;
            indices[2 * (640 * i + j)] = 0;
            indices[3 * (640 * i + j)] = 0;
        }
    }

    ///for another shader.
    unsigned int meshVBO , meshVAO;
    glGenVertexArrays(1, &meshVAO);
    glGenBuffers(1, &meshVBO);
    glBindVertexArray(meshVAO);
    glBindBuffer(GL_ARRAY_BUFFER, meshVBO);
    glBufferData(GL_ARRAY_BUFFER, cube.vertexCount * sizeof(float), cube.vertices, GL_STATIC_DRAW);
    // note that we update the lamp's position attribute's stride to reflect the updated buffer data
    glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 6 * sizeof(float), (void*)0);
    glEnableVertexAttribArray(0);
    glVertexAttribPointer(1, 3, GL_FLOAT, GL_FALSE, 6 * sizeof(float), (void*)(3 * sizeof(float)));
    glEnableVertexAttribArray(1);



    unsigned int VAO, VBO, EBO;
    glGenVertexArrays(1, &VAO);
    glGenBuffers(1, &VBO);
    glGenBuffers(1, &EBO);  
    // bind the Vertex Array Object first, then bind and set vertex buffer(s), and then configure vertex attributes(s).
    glEnable(GL_DEPTH_TEST);

    // render loop
    // -----------
    while (!glfwWindowShouldClose(window))
    {
        // preprocess
        long additionIndexCount = 0;
        int meshIndexCount = 0;
        application.Update();


        for (long i = 0; i < vertexCount; i++) {
            vertices[vertexStride * i + zOffset] = application.depthValues[i] > 0 ? -(float)application.depthValues[i] * 0.005 : -20;
            vertices[vertexStride * i + rOffset] = (float)application.colorsRGBValues[3 * i + 0] / 255.0;//R
            vertices[vertexStride * i + gOffset] = (float)application.colorsRGBValues[3 * i + 1] / 255.0;//G
            vertices[vertexStride * i + bOffset] = (float)application.colorsRGBValues[3 * i + 2] / 255.0;//B
        }

        // calculate normal vector.计算每个点的法向量，我们这里为了给cpu减负，把这段操作放到shader中让gpu执行运算，于是就需要把这些信息放到缓冲中
        for (long i = 0; i < vertexCount; i++)
        {
            int col = i % colNum;
            int row = i / colNum;

            if (col > 0 && col < colNum - 1 && row > 0 && row < rowNum - 1) {
                //center point
                float cx = vertices[vertexStride * i];
                float cy = vertices[vertexStride * i + 1];
                float cz = vertices[vertexStride * i + 2];
                //glm::vec3 center = glm::vec3(cx, cy, cz);

                //upper
                float ux = vertices[vertexStride * (i + colNum)];
                float uy = vertices[vertexStride * (i + colNum) + 1];
                float uz = vertices[vertexStride * (i + colNum) + 2];
                //glm::vec3 upper = glm::vec3(ux, uy, uz) - center;
                vertices[vertexStride * i + uxOffset] = ux - cx;
                vertices[vertexStride * i + uyOffset] = uy - cy;
                vertices[vertexStride * i + uzOffset] = uz - cz;

                //down
                float dx = vertices[vertexStride * (i - colNum)];
                float dy = vertices[vertexStride * (i - colNum) + 1];
                float dz = vertices[vertexStride * (i - colNum) + 2];
                //glm::vec3 down = glm::vec3(dx, dy, dz) - center;
                vertices[vertexStride * i + dxOffset] = dx - cx;
                vertices[vertexStride * i + dyOffset] = dy - cy;
                vertices[vertexStride * i + dzOffset] = dz - cz;


                //left
                float lx = vertices[vertexStride * (i - 1)];
                float ly = vertices[vertexStride * (i - 1) + 1];
                float lz = vertices[vertexStride * (i - 1) + 2];
                //glm::vec3 left = glm::vec3(lx, ly, lz) - center;
                vertices[vertexStride * i + lxOffset] = lx - cx;
                vertices[vertexStride * i + lyOffset] = ly - cy;
                vertices[vertexStride * i + lzOffset] = lz - cz;

                //right
                float rx = vertices[vertexStride * (i + 1)];
                float ry = vertices[vertexStride * (i + 1) + 1];
                float rz = vertices[vertexStride * (i + 1) + 2];
                //glm::vec3 right = glm::vec3(rx, ry, rz) - center;
                vertices[vertexStride * i + rxOffset] = rx - cx;
                vertices[vertexStride * i + ryOffset] = ry - cy;
                vertices[vertexStride * i + rzOffset] = rz - cz;

            }
        }



        long index = 0;
        for (int i = 0; i < rowNum - 1; i++) {
            for (int j = 0; j < colNum; j++) {
                long currentIndex = colNum * i + j;
                long nextLineOfCurrentIndex = colNum * (i + 1) + j;
                long rightOfCurrentIndex = colNum * i + j + 1;
                if (j == 0) indices[index++] = currentIndex;

                indices[index++] = currentIndex;
                if ((i != rowNum - 1) && (abs((vertices[vertexStride * currentIndex + zOffset] - vertices[vertexStride * (currentIndex + colNum) + zOffset])) >= gapThreshold))
                {
                    indices[index++] = currentIndex;
                    indices[index++] = nextLineOfCurrentIndex;
                    additionIndexCount += 2;  //indicate that two index information have been added in indeces.
                }
                indices[index++] = nextLineOfCurrentIndex;

                if ((j != colNum - 1) && (abs((vertices[vertexStride * nextLineOfCurrentIndex + zOffset] - vertices[vertexStride * rightOfCurrentIndex + zOffset])) >= gapThreshold))
                {
                    indices[index++] = nextLineOfCurrentIndex;
                    indices[index++] = rightOfCurrentIndex;
                    additionIndexCount += 2;

                }

                if (j == colNum - 1) indices[index++] = nextLineOfCurrentIndex;
            }
        }

        glBindVertexArray(VAO);

        glBindBuffer(GL_ARRAY_BUFFER, VBO);
        glBufferData(GL_ARRAY_BUFFER, verticesSize * sizeof(float), vertices, GL_DYNAMIC_DRAW);


        glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, EBO);
        glBufferData(GL_ELEMENT_ARRAY_BUFFER, (indexCount + additionIndexCount) * sizeof(unsigned int), indices, GL_DYNAMIC_DRAW);

        glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, vertexStride * sizeof(float), (void*)0);
        glEnableVertexAttribArray(0);

        glVertexAttribPointer(1, 3, GL_FLOAT, GL_FALSE, vertexStride * sizeof(float), (void*)(uxOffset * sizeof(float)));
        glEnableVertexAttribArray(1);

        glVertexAttribPointer(2, 3, GL_FLOAT, GL_FALSE, vertexStride * sizeof(float), (void*)(lxOffset * sizeof(float)));
        glEnableVertexAttribArray(2);

        glVertexAttribPointer(3, 3, GL_FLOAT, GL_FALSE, vertexStride * sizeof(float), (void*)(dxOffset * sizeof(float)));
        glEnableVertexAttribArray(3);

        glVertexAttribPointer(4, 3, GL_FLOAT, GL_FALSE, vertexStride * sizeof(float), (void*)(rxOffset * sizeof(float)));
        glEnableVertexAttribArray(4);

        glVertexAttribPointer(5, 3, GL_FLOAT, GL_FALSE, vertexStride * sizeof(float), (void*)(rOffset * sizeof(float)));
        glEnableVertexAttribArray(5);

        // note that this is allowed, the call to glVertexAttribPointer registered VBO as the vertex attribute's bound vertex buffer object so afterwards we can safely unbind
        glBindBuffer(GL_ARRAY_BUFFER, 0);

        // You can unbind the VAO afterwards so other VAO calls won't accidentally modify this VAO, but this rarely happens. Modifying other
        // VAOs requires a call to glBindVertexArray anyways so we generally don't unbind VAOs (nor VBOs) when it's not directly necessary.
        glBindVertexArray(0);

        float currentFrame = glfwGetTime();
        deltaTime = currentFrame - lastFrame;
        lastFrame = currentFrame;

        // input
        // -----
        processInput(window);


        displayMode == darkMode ? glClearColor(0.0f, 0.0f, 0.0f, 1.0f) : glClearColor(0.2f, 0.3f, 0.3f, 1.0f);
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

        // draw our first triangle
        ourShader.use();

        // create transformations
        glm::mat4 model = glm::mat4(1.0f);
        glm::mat4 view = camera.GetViewMatrix();
        glm::mat4 projection = glm::mat4(1.0f);
        projection = glm::perspective(glm::radians(45.0f), (float)SCR_WIDTH / (float)SCR_HEIGHT, 0.1f, 200.0f);
        // retrieve the matrix uniform locations
        unsigned int modelLoc = glGetUniformLocation(ourShader.ID, "model");
        unsigned int viewLoc = glGetUniformLocation(ourShader.ID, "view");
        // pass them to the shaders (3 different ways)
        glUniformMatrix4fv(modelLoc, 1, GL_FALSE, glm::value_ptr(model));
        glUniformMatrix4fv(viewLoc, 1, GL_FALSE, &view[0][0]);
        ourShader.setVec3("viewPos", camera.Position);
        // note: currently we set the projection matrix each frame, but since the projection matrix rarely changes it's often best practice to set it outside the main loop only once.
        ourShader.setMat4("projection", projection);
        ourShader.setInt("displayMode", displayMode);

        //{ //hand mark. 这里是骨骼的匹配，并不是很精准
            glm::vec3 lh1, lh2, rh1, rh2;
            lh1.x = (application.leftHandPos[0] - (float)colNum / 4.0) * 0.02; //the magic number is the temp value. Just to be simpler for implementation,
            lh1.y = (application.leftHandPos[1] - (float)rowNum / 4.0) * -0.02;
            lh1.z = 200; // when there is no value, then we draw it far from we could see.
            lh2.x = (application.leftHandPos[2] - (float)rowNum / 4.0) * -0.02;
            lh2.y = (application.leftHandPos[3] - (float)rowNum / 4.0) * -0.02;
            rh1.x = (application.rightHandPos[0] - (float)colNum / 4.0) * 0.02;
            rh1.y = (application.rightHandPos[1] - (float)rowNum / 4.0) * -0.02;
            rh1.z = 200; // when there is no value, then we draw it far from we could see.
            rh2.x = (application.rightHandPos[2] - (float)colNum / 4.0) * 0.02;
            rh2.y = (application.rightHandPos[3] - (float)rowNum / 4.0) * -0.02;
            ourShader.setVec2("leftHand1", lh1);
            ourShader.setVec2("leftHand2", lh2);
            ourShader.setVec2("rightHand1", rh1);
            ourShader.setVec2("rightHand2", rh2);
            
        {
            int j = std::round(application.leftHandPos[0] * 2);
            int i = std::round(application.leftHandPos[1] * 2.0);
            if (vertexStride * (colNum * i + j) + zOffset < vertexCount * vertexStride && vertexStride * (colNum * i + j) + zOffset > 0) {
                //for (int kx = -3; kx < 3; kx++)
                //{
                //  for (int ky = -3; ky < 3; ky++)
                //  {
                //      lh1.z = lh1.z >= vertices[vertexStride * (colNum * (i + ky) + j + kx) + zOffset] ? lh1.z : vertices[vertexStride * (colNum * (i+ky) + j + kx) + zOffset];
                //  }
                //}
                lh1.z = vertices[vertexStride * (colNum * i + j) + zOffset];
            }
        }
        {
            int j = std::round(application.rightHandPos[0] * 2.0);
            int i = std::round(application.rightHandPos[1] * 2.0);
            if (vertexStride * (colNum * i + j) + zOffset < vertexCount * vertexStride && vertexStride * (colNum * i + j) + zOffset > 0) {
                rh1.z = vertices[vertexStride * (colNum * i + j) + zOffset];
            }
        }
            
                
        //}

        // render box
        glBindVertexArray(VAO);
        //glDrawArrays(GL_POINTS, 0, 640 * 480);
        glDrawElements(GL_TRIANGLE_STRIP, indexCount, GL_UNSIGNED_INT, 0);


        addiMeshShader.use();
        addiMeshShader.setMat4("projection", projection);
        addiMeshShader.setMat4("view", view);

        {
            //left hand 1
            model = glm::mat4(1.0f);
            model = glm::translate(model, glm::vec3(0, 0.4, 0));
            model = glm::translate(model, lh1);
            model = glm::rotate(model, (float)glfwGetTime() * glm::radians(50.0f), glm::vec3(0.5f, 1.0f, 0.0f));
            model = glm::scale(model, glm::vec3(0.2f)); // a smaller cube
            addiMeshShader.setMat4("model", model);

            glBindVertexArray(meshVAO);
            glDrawArrays(GL_TRIANGLES, 0, 36);

            //right hand 1
            model = glm::mat4(1.0f);
            model = glm::translate(model, glm::vec3(0, 0.4, 0));
            model = glm::translate(model, rh1);
            model = glm::rotate(model, (float)glfwGetTime() * glm::radians(50.0f), glm::vec3(0.5f, 1.0f, 0.0f));
            model = glm::scale(model, glm::vec3(0.2f)); // a smaller cube
            addiMeshShader.setMat4("model", model);

            glBindVertexArray(meshVAO);
            glDrawArrays(GL_TRIANGLES, 0, 36);

        }



        // glfw: swap buffers and poll IO events (keys pressed/released, mouse moved etc.)
        // -------------------------------------------------------------------------------
        glfwSwapBuffers(window);
        glfwPollEvents();
    }
    // optional: de-allocate all resources once they've outlived their purpose:
    // ------------------------------------------------------------------------
    glDeleteVertexArrays(1, &VAO);
    glDeleteBuffers(1, &VBO);
    glDeleteBuffers(1, &EBO);
    glDeleteVertexArrays(1, &meshVAO);
    glDeleteBuffers(1, &meshVBO);

    // glfw: terminate, clearing all previously allocated GLFW resources.
    // ------------------------------------------------------------------
    glfwTerminate();
        
    application.DisConnected();

    return 0;
}

// process all input: query GLFW whether relevant keys are pressed/released this frame and react accordingly
// ---------------------------------------------------------------------------------------------------------
void processInput(GLFWwindow *window)
{
    if (glfwGetKey(window, GLFW_KEY_ESCAPE) == GLFW_PRESS)
        glfwSetWindowShouldClose(window, true);

    if (glfwGetKey(window, GLFW_KEY_W) == GLFW_PRESS)
        camera.ProcessKeyboard(FORWARD, deltaTime);
    if (glfwGetKey(window, GLFW_KEY_S) == GLFW_PRESS)
        camera.ProcessKeyboard(BACKWARD, deltaTime);
    if (glfwGetKey(window, GLFW_KEY_A) == GLFW_PRESS)
        camera.ProcessKeyboard(LEFT, deltaTime);
    if (glfwGetKey(window, GLFW_KEY_D) == GLFW_PRESS)
        camera.ProcessKeyboard(RIGHT, deltaTime);
    if (glfwGetKey(window, GLFW_KEY_C) == GLFW_PRESS)
        camera.ProcessKeyboard(DOWN, deltaTime);
    if (glfwGetKey(window, GLFW_KEY_SPACE) == GLFW_PRESS)
        camera.ProcessKeyboard(UP, deltaTime);
    if (glfwGetKey(window, GLFW_KEY_1) == GLFW_PRESS)
        displayMode = distantGray;
    if (glfwGetKey(window, GLFW_KEY_2) == GLFW_PRESS)
        displayMode = normalColor;
    if (glfwGetKey(window, GLFW_KEY_3) == GLFW_PRESS)
        displayMode = lightGray;
    if (glfwGetKey(window, GLFW_KEY_4) == GLFW_PRESS)
        displayMode = sampleColor;
    if (glfwGetKey(window, GLFW_KEY_5) == GLFW_PRESS)
        displayMode = darkMode;
    if (glfwGetKey(window, GLFW_KEY_EQUAL) == GLFW_PRESS && gapThreshold < 19.9)
        gapThreshold += 0.1;
    if (glfwGetKey(window, GLFW_KEY_MINUS) == GLFW_PRESS && gapThreshold > 0.1)
        gapThreshold -= 0.1;

}

// glfw: whenever the mouse moves, this callback is called
// -------------------------------------------------------
void mouse_callback(GLFWwindow* window, double xpos, double ypos)
{
    if (firstMouse)
    {
        lastX = xpos;
        lastY = ypos;
        firstMouse = false;
    }

    float xoffset = xpos - lastX;
    float yoffset = lastY - ypos; // reversed since y-coordinates go from bottom to top

    lastX = xpos;
    lastY = ypos;

    camera.ProcessMouseMovement(xoffset, yoffset);
}

// glfw: whenever the window size changed (by OS or user resize) this callback function executes
// ---------------------------------------------------------------------------------------------
void framebuffer_size_callback(GLFWwindow* window, int width, int height)
{
    // make sure the viewport matches the new window dimensions; note that width and 
    // height will be significantly larger than specified on retina displays.
    glViewport(0, 0, width, height);
}

GitHub地址

https://github.com/Dove2/3D-Reconstruction-With-RGBD