Mugichoko's blog

Mugichoko’s blog

プログラミングを中心としたメモ書き.

RGB-D SLAMを実装する #2

大域的目標

KellerらのRGB-D SLAM[1]が実装したい!と思い立ったので実装していく,というモチベーションの日誌.ちょっとずつ実装していく.今回が2回目.

前回(以下,参照)は論文読解とアルゴリズムの整理を行った.今回は,いよいよ実装に入る. mugichoko.hatenablog.com

方針転換

GLSLによる制約?

早速だが,実装の都合上,方針を変えようと思う.今回,比較的慣れているGLSLを使って実装しようとしているが,以下の通り,並列処理&&動的な配列を作れないことによる制約が大きいように思える.CUDAを使えばその辺り,扱いやすいのか??

  • グローバルなマップ(配列Gと呼ぶ)に新たな点Pを追加することが難しい(追加点Pの数(例えばkコ)が分かっていれば,大きな配列Gの最後の要素の番号lからl+k番目まで,同時/順に書き込めばよいことになるが…)
    1. まず,Pの数を数えるのが面倒.例えば,GPUのメモリ上にあるマスク画像(ステンシルバッファ)をメインメモリに転送して,有効画素数を数える?
    2. また,数が分かったとして,新たな点の内,どの点が何番目かが分からないので,配列Gのどの要素に書き込めばいいかIDを振って,そのIDのルックアップテーブルをGPUに転送し,一気に書き込む?
  • グローバルなマップ(配列G)にある点を削除するのが難しい
    1. 削除する点の入った要素以降の要素をシフトさせる?それってGLSLを使って処理できる??
    2. 信頼度を0にして,その点を一生使わないようにする?

解決案

そこで,キーフレームベースの手法を実装することにした.つまり,グローバルなマップの代わりに,キーフレームからの相対位置姿勢を求めるようにする.言い換えれば,キーフレームをグローバルなマップとしてトラッキングし,キーフレームから一定距離離れたら,そこに新たなキーフレームを生成し,同じことを繰り返す.これによって… 1. メモリの管理はキーフレーム単位になり,管理しやすい 2. 後に,ループクロージャを実装しやすくなる

と考えた.欠点として,複数のキーフレームに同一点が写っている場合,1つに統合されないことが考えられるが,利点の方が大きいので,とりあえずは良しとする.

今回の目標

  1. キーフレームクラスを作る(1画素に相当する要素は,グローバルなマップの1点と同じ)
    • カラー (uchar×3)
      • 使い道がないかもしれない
    • 奥行 (float)
      • 頂点で代替できるだろうけれど
    • 頂点 (float×3)
    • 法線 (float×3)
    • 信頼度 (float)
    • 半径 (float)
    • タイムスタンプ (int)
  2. キーフレームの頂点を別フレーム(別のカメラ位置姿勢)に投影して,処理が合っているか定性的に確認
    • 定性的に確認:投影した点が,それらしく別フレームに重なって見えるかどうか
    • これにはRGB-Dフレームの位置姿勢のグランドゥールスが提供されているTUMのRGB-D SLAM Dataset and Benchmarkを用いる

つまり,前回にまとめたアルゴリズムの内の1~3に相当する処理を実装する.尚,頂点マップ及び法線マップはこちらの記事を参照のこと.

実装

処理フローとメモリ管理

  1. 初期化
    • GLSLの初期化
    • キーフレーム作成(GPUメモリに必要な容量を確保)
  2. フレームkとk+nのRGB-D画像の読み込み
  3. フレームkの頂点マップの作成
    • 奥行画像,内部パラメータを入力として頂点マップを作成
  4. フレームkからフレームk+nへの変換行列を計算
  5. GLSLを使ってフレームkからフレームk+nに頂点を投影
    • 上手くいっていれば,RGB画像に重なるように頂点画像が描画されるはず

サンプルプログラム

ベースとなるOpenGL (GLSL) のクラス群に関しては,過去のGLSL関係の記事を参照のこと.

mugichoko.hatenablog.com

tumRGBDUtil

TUM RGB-D SLAM Datasetのデータを読み込むクラス.

tumRGBDUtil.h

#pragma once

#include <iostream>
using namespace std;
#include <fstream>
#include <sstream>
#include <glm/glm.hpp>
#include <glm/ext.hpp>
#include <glm/detail/setup.hpp>
#include<glm/gtc/quaternion.hpp>

class tumRGBDUtil
{
private:
    ifstream ifs;

public:
    tumRGBDUtil();
    tumRGBDUtil(
        const string &gtPath   // File name of a ground truth text file
    );
    ~tumRGBDUtil();

    void open(
        const string &gtPath   // File name of a ground truth text file
    );
    bool readFrameData(
        string &timestamp,
        float &tx,
        float &ty,
        float &tz,
        float &qx,
        float &qy,
        float &qz,
        float &qw
    );

    bool readFrameData(
        string &timestamp,
        glm::mat4 &T
    );
};

tumRGBDUtil.cpp

#include "tumRGBDUtil.h"

tumRGBDUtil::tumRGBDUtil() {}
tumRGBDUtil::tumRGBDUtil(
    const string &gtPath   // File name of a ground truth text file
)
{
    open(gtPath);
}
tumRGBDUtil::~tumRGBDUtil() { }

void tumRGBDUtil::open(
    const string &gtPath   // File name of a ground truth text file
)
{
    // Read initial pose
    ifs.open(gtPath);

    // Ignore first three lines
    string line;
    getline(ifs, line); getline(ifs, line); getline(ifs, line);
}

bool tumRGBDUtil::readFrameData(
    string &timestamp,
    float &tx,
    float &ty,
    float &tz,
    float &qx,
    float &qy,
    float &qz,
    float &qw
)
{
    string line;
    if (getline(ifs, line))
    {
        istringstream iss(line);
        iss >> timestamp >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
        return true;
    }
    else
    {
        return false;
    }
}

bool tumRGBDUtil::readFrameData(
    string &timestamp,
    glm::mat4 &T
)
{
    float tx, ty, tz, qx, qy, qz, qw;
    if (readFrameData(timestamp, tx, ty, tz, qx, qy, qz, qw))
    {
        // Calculate transformation matrix
        glm::quat qt(qw, qx, qy, qz);
        glm::mat3 r = glm::mat3_cast(qt);
        glm::vec3 t(tx, ty, tz);
        T = glm::lookAt(t, t + r[2], -r[1]);

        return true;
    }
    else
    {
        return false;
    }
}

rgbdFrame

RGB-D画像をGPUメモリ上に確保するためのクラス.

rgbdFrame.h

#pragma once

#include <iostream>
using namespace std;
#include <GL/glew.h>
#include <GLFW/glfw3.h>

namespace rgbd
{
    class rgbdFrame
    {
    private:
        const int width;
        const int height;
        GLuint texColorID;
        GLuint texDepthID;

    public:
        rgbdFrame(
            int w,
            int h
        );
        ~rgbdFrame();

        void updateRGBDTex(
            const void *colorData,
            const void *depthData
        );

        int getWidth();
        int getHeight();
        GLuint getColorTexID();
        GLuint getDepthTexID();
    };
}

rgbdFrame.cpp

#include "rgbdTex.h"

namespace rgbd
{
    rgbdFrame::rgbdFrame(
        int w,
        int h
    ) : width(w), height(h)
    {
        // ----- Create textures
        // Buffer for color image
        glGenTextures(1, &texColorID);
        glBindTexture(GL_TEXTURE_2D, texColorID);
        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
        glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB8, w, h, 0, GL_BGR, GL_UNSIGNED_BYTE, 0);
        glBindTexture(GL_TEXTURE_2D, 0);
        // Buffer for depth image
        glGenTextures(1, &texDepthID);
        glBindTexture(GL_TEXTURE_2D, texDepthID);
        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
        glTexImage2D(GL_TEXTURE_2D, 0, GL_R32F, w, h, 0, GL_RED, GL_FLOAT, 0);
        glBindTexture(GL_TEXTURE_2D, 0);
    }
    rgbdFrame::~rgbdFrame()
    {
        glDeleteTextures(1, &texColorID);
        glDeleteTextures(1, &texDepthID);
    }

    void rgbdFrame::updateRGBDTex(
        const void *colorData,
        const void *depthData
    )
    {
        glBindTexture(GL_TEXTURE_2D, texColorID);
        glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, width, height, GL_BGR, GL_UNSIGNED_BYTE, colorData);
        glBindTexture(GL_TEXTURE_2D, texDepthID);
        glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, width, height, GL_RED, GL_FLOAT, depthData);
        glBindTexture(GL_TEXTURE_2D, 0);
    }

    int rgbdFrame::getWidth()
    {
        return width;
    }
    int rgbdFrame::getHeight()
    {
        return height;
    }
    GLuint rgbdFrame::getColorTexID()
    {
        return texColorID;
    }
    GLuint rgbdFrame::getDepthTexID()
    {
        return texDepthID;
    }
}

keyframe

キーフレームのデータをGPUメモリ上に確保するためのクラス.

keyframe.h

#pragma once

#include <iostream>
using namespace std;
#include <GL/glew.h>
#include <GLFW/glfw3.h>

namespace rgbd
{
    class keyframe
    {
    private:
        const int width;
        const int height;
        GLuint colorMapTexID;
        GLuint vertMapTexID;
        GLuint confMapTexID;
        GLuint radiMapTexID;
        GLuint timeStampMapTexID;

    public:
        keyframe(
            int w,
            int h
        );
        ~keyframe();

        void initialize(
            const GLuint colorMapID,
            const GLuint vertMapID
        );

        int getWidth();
        int getHeight();
        GLuint getColorMapTexID();
        GLuint getVertMapTexID();
        GLuint getConfMapTexID();
        GLuint getRadiMapTexID();
        GLuint getTimeStampMapTexID();
    };
}

keyframe.cpp

#include "keyframe.h"

namespace rgbd
{
    keyframe::keyframe(
        int w,
        int h
    ) : width(w), height(h)
    {
        // ----- Create textures
        // Color map (float x 3)
        glGenTextures(1, &colorMapTexID);
        glBindTexture(GL_TEXTURE_2D, colorMapTexID);
        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_BORDER);
        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_BORDER);
        glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB8, w, h, 0, GL_BGR, GL_UNSIGNED_BYTE, 0);
        glBindTexture(GL_TEXTURE_2D, 0);
        // Vertex map (float x 3)
        glGenTextures(1, &vertMapTexID);
        glBindTexture(GL_TEXTURE_2D, vertMapTexID);
        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_BORDER);
        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_BORDER);
        glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA32F, w, h, 0, GL_RGB, GL_FLOAT, 0);
        glBindTexture(GL_TEXTURE_2D, 0);
        // Confidence map (float x 1)
        glGenTextures(1, &confMapTexID);
        glBindTexture(GL_TEXTURE_2D, confMapTexID);
        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_BORDER);
        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_BORDER);
        glTexImage2D(GL_TEXTURE_2D, 0, GL_R32F, w, h, 0, GL_RED, GL_FLOAT, 0);
        glBindTexture(GL_TEXTURE_2D, 0);
        // Radius map (float x 1)
        glGenTextures(1, &radiMapTexID);
        glBindTexture(GL_TEXTURE_2D, radiMapTexID);
        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_BORDER);
        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_BORDER);
        glTexImage2D(GL_TEXTURE_2D, 0, GL_R32F, w, h, 0, GL_RED, GL_FLOAT, 0);
        glBindTexture(GL_TEXTURE_2D, 0);
        // Time stamp map (int x 1)
        glGenTextures(1, &timeStampMapTexID);
        glBindTexture(GL_TEXTURE_2D, timeStampMapTexID);
        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_BORDER);
        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_BORDER);
        glTexImage2D(GL_TEXTURE_2D, 0, GL_R32I, w, h, 0, GL_RED, GL_INT, 0);
        glBindTexture(GL_TEXTURE_2D, 0);
    }
    keyframe::~keyframe()
    {
        glDeleteTextures(1, &colorMapTexID);
        glDeleteTextures(1, &vertMapTexID);
        glDeleteTextures(1, &confMapTexID);
        glDeleteTextures(1, &radiMapTexID);
        glDeleteTextures(1, &timeStampMapTexID);
    }

    void keyframe::initialize(
        const GLuint colorMapID,
        const GLuint vertMapID
    )
    {
        // Ref: https://devtalk.nvidia.com/default/topic/739535/opengl/glcopyimagesubdata/
        // Copy color maps
        glCopyImageSubData(colorMapID, GL_TEXTURE_2D, 0, 0, 0, 0, colorMapTexID, GL_TEXTURE_2D, 0, 0, 0, 0, width, height, 1);

        // Copy vertex maps
        glCopyImageSubData(vertMapID, GL_TEXTURE_2D, 0, 0, 0, 0, vertMapTexID, GL_TEXTURE_2D, 0, 0, 0, 0, width, height, 1);
    }

    int keyframe::getWidth()
    {
        return width;
    }
    int keyframe::getHeight()
    {
        return height;
    }

    GLuint keyframe::getColorMapTexID()
    {
        return colorMapTexID;
    }

    GLuint keyframe::getVertMapTexID()
    {
        return vertMapTexID;
    }
    GLuint keyframe::getConfMapTexID()
    {
        return confMapTexID;
    }
    GLuint keyframe::getRadiMapTexID()
    {
        return radiMapTexID;
    }
    GLuint keyframe::getTimeStampMapTexID()
    {
        return timeStampMapTexID;
    }
}

rgbdProc

RGB-D画像に色々と処理を行うクラス.例えば,頂点マップや法線マップを生成する.

rgbdProc.h

#pragma once
#include <glm/glm.hpp>
#include <glm/ext.hpp>
#include "OpenGLWrapper/shaderUtil.h"

namespace rgbd
{
    // ----- Bilateral filter
    // Ref: https://www.shadertoy.com/view/4dfGDH
    class csBilateralFilter : public gl::compShader
    {
    public:
        csBilateralFilter(
            int w,
            int h,
            const string &compShaderName
        );
        void csBilateralFilter::execute(
            GLuint inTexID,
            float sigma = 10.0f,
            float bSigma = 0.1f
        );
    };

    // ----- Vertex map calculation
    class csCalcVertMap : public gl::compShader
    {
    private:
        const glm::mat3 K; // Intrinsic parameters

    public:
        csCalcVertMap(
            int w,
            int h,
            const glm::mat3 &K,
            const string &compShaderName
        );

        void csCalcVertMap::execute(
            GLuint inTexID
        );
    };

    // ----- Normal map calculation
    class csCalcNormMap : public gl::compShader
    {
    public:
        csCalcNormMap(
            int w,
            int h,
            const string &compShaderName
        );

        void csCalcNormMap::execute(
            GLuint inTexID
        );
    };
}

rgbdProc.cpp

#include "rgbdProc.h"

namespace rgbd
{
    csBilateralFilter::csBilateralFilter(
        int w,
        int h,
        const string &compShaderName
    ) : gl::compShader(w, h, 1, GL_R32F, GL_RED, GL_FLOAT, compShaderName)
    {
    }

    void csBilateralFilter::csBilateralFilter::execute(
        GLuint inTexID,
        float sigma,
        float bSigma
    )
    {
        glUseProgram(prog);
        {
            GLuint locSigma = glGetUniformLocation(prog, "sigma");
            glUniform1f(locSigma, sigma);
            GLuint locBSigma = glGetUniformLocation(prog, "bSigma");
            glUniform1f(locBSigma, bSigma);

            // Input texture
            glBindImageTexture(0, inTexID, 0, GL_FALSE, 0, GL_READ_ONLY, GL_R32F);
            // Output texture
            glBindImageTexture(1, texID, 0, GL_FALSE, 0, GL_WRITE_ONLY, GL_R32F);
            glDispatchCompute(width / 32, height / 32, depth);
        }
        glUseProgram(0);
    }


    csCalcVertMap::csCalcVertMap(
        int w,
        int h,
        const glm::mat3 &K,
        const string &compShaderName
    ) : gl::compShader(w, h, 1, GL_RGBA32F, GL_RGB, GL_FLOAT, compShaderName), K(K)
    {
        glUseProgram(prog);
        {
            // Update the uniform
            GLuint locInvK = glGetUniformLocation(prog, "invK");
            glUniformMatrix3fv(locInvK, 1, GL_FALSE, glm::value_ptr(glm::inverse(K)));
        }
        glUseProgram(0);
    }

    void csCalcVertMap::execute(
        GLuint inTexID
    )
    {
        glUseProgram(prog);
        {
            // Input texture
            glBindImageTexture(0, inTexID, 0, GL_FALSE, 0, GL_READ_ONLY, GL_R32F);
            // Output texture
            glBindImageTexture(1, texID, 0, GL_FALSE, 0, GL_WRITE_ONLY, GL_RGBA32F);
            glDispatchCompute(width / 32, height / 32, depth);
        }
        glUseProgram(0);
    }

    csCalcNormMap::csCalcNormMap(
        int w,
        int h,
        const string &compShaderName
    ) : gl::compShader(w, h, 1, GL_RGBA32F, GL_RGB, GL_FLOAT, compShaderName)
    {
    }

    void csCalcNormMap::execute(
        GLuint inTexID
    )
    {
        glUseProgram(prog);
        {
            // Input texture
            glBindImageTexture(0, inTexID, 0, GL_FALSE, 0, GL_READ_ONLY, GL_RGBA32F);
            // Output texture
            glBindImageTexture(1, texID, 0, GL_FALSE, 0, GL_WRITE_ONLY, GL_RGBA32F);
            glDispatchCompute(width / 32, height / 32, depth);
        }
        glUseProgram(0);
    }
}

main.cpp

メインの記述.

#include <glm/glm.hpp>
#include <glm/ext.hpp>
#include <opencv2/opencv.hpp>
#include "OpenGLWrapper/window.h"
#include "OpenGLWrapper/shaderUtil.h"
#include "OpenGLWrapper/modelTex.h"
#include "OpenGLWrapper/projMatFromCalib.h"
#include "safeDelete.h"
#include "rgbdProc.h"
#include "rgbdFrame.h"
#include "keyframe.h"
#include "iniRead.h"
#include "tumRGBDUtil.h"

#include <Windows.h>

class MyWindow : gl::window
{
private:
    rgbd::csBilateralFilter *myCSBilateralFilter;
    rgbd::csCalcVertMap *myCSCalcVertMap;
    rgbd::csCalcNormMap *myCSCalcNormMap;
    rgbd::rgbdFrame *myRGBDFrame;
    gl::modelTex *myModelTex;

    rgbd::keyframe *kf;
    const glm::mat3 K;
    const string gtPath;

    tumRGBDUtil tum;

public:
    MyWindow(
        int w,
        int h,
        const string &wndName,
        const glm::mat3 &K,    // Intrinsic parameters
        const float factor,   // See: http://vision.in.tum.de/data/datasets/rgbd-dataset/file_formats#intrinsic_camera_calibration_of_the_kinect
        const string &groundTruthPath,
        const string &colorImgName1,
        const string &depthImgName1,
        const string &compBilateralFilterShaderName,
        const string &compVertMapShaderName,
        const string &compNormMapShaderName
    ) : window(w, h, wndName), K(K), gtPath(groundTruthPath)
    {
        // Read images
        cv::Mat imgC1 = cv::imread(colorImgName1);      // Color image
        cv::Mat imgD1 = cv::imread(depthImgName1, -1); // Depth image
        cv::flip(imgC1, imgC1, 0);
        cv::flip(imgD1, imgD1, 0);
        imgD1.convertTo(imgD1, CV_32F);
        imgD1 /= factor;

        // Upload the RGB-D image to your GPU
        myRGBDFrame = new rgbd::rgbdFrame(imgC1.cols, imgC1.rows);
        myRGBDFrame->updateRGBDTex(imgC1.data, imgD1.data);
        
        // Create computer shaders
        myCSBilateralFilter = new rgbd::csBilateralFilter(imgC1.cols, imgC1.rows, compBilateralFilterShaderName);
        myCSCalcVertMap = new rgbd::csCalcVertMap(imgC1.cols, imgC1.rows, K, compVertMapShaderName);
        myCSCalcNormMap = new rgbd::csCalcNormMap(imgC1.cols, imgC1.rows, compNormMapShaderName);

        // Keyframe
        kf = new rgbd::keyframe(width, height);

        // Create a texture polygon to display the results
        myModelTex = new gl::modelTex(imgC1.cols, imgC1.rows, "../shader/modelTex.vert", "../shader/modelTex.frag");

        // TUM RGB-D dataset
        tum.open(groundTruthPath);
    }
    ~MyWindow()
    {
        safeDelete(myRGBDFrame);
        safeDelete(myCSBilateralFilter);
        safeDelete(myCSCalcVertMap);
        safeDelete(myCSCalcNormMap);
        safeDelete(myModelTex);
        safeDelete(kf);
    }

    void render()
    {
        // ----------
        // ----- TEST
        // --------------------
        // ----- Create a program
        GLuint prog;
        gl::initializeProgram(prog, "../shader/projVert.vert", "../shader/projVert.frag");

        // ----- Calculate projection matrix
        glm::mat4 P = gl::makeCalibratedProjectionMatrixYdown(
            0.1f, 10.0f, width, height,
            K[2][0], K[2][1], K[0][0], K[1][1], 0.0f
        );

        // ----- Read initial pose
        string timestamp;
        glm::mat4 T1;
        tum.readFrameData(timestamp, T1);

        // Rendering loop
        glm::mat4 T2;
        while (!glfwWindowShouldClose(wnd) && tum.readFrameData(timestamp, T2))
        {
            // Clear color and depth buffers
            glClearColor(0.1f, 0.7f, 0.3f, 0.0f);
            glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

            // Bilateral filter
            myCSBilateralFilter->execute(myRGBDFrame->getDepthTexID(), 2.0f, 0.05f);
            // Calculate a vertex map from a depth map
            myCSCalcVertMap->execute(myCSBilateralFilter->getTexID());
            // Calculate a normal map from a depth map
            myCSCalcNormMap->execute(myCSCalcVertMap->getTexID());

            // Create a keyframe
            kf->initialize(myRGBDFrame->getColorTexID(), myCSCalcVertMap->getTexID());

            // Project the keyframe vertex map to the other frame
            glUseProgram(prog);
            {
                // Calculate translation matrix
                glm::mat4 T = T2 * glm::inverse(T1);
                glUniformMatrix4fv(glGetUniformLocation(prog, "T"), 1, GL_FALSE, glm::value_ptr(T));
                glUniformMatrix4fv(glGetUniformLocation(prog, "P"), 1, GL_FALSE, glm::value_ptr(P));
                glBindTexture(GL_TEXTURE_2D, kf->getVertMapTexID());
                glDrawArrays(GL_POINTS, 0, width * height);
                glBindTexture(GL_TEXTURE_2D, 0);
            }
            glUseProgram(0);

            // Swap front and back buffers
            glfwSwapBuffers(wnd);
            // Poll for and process events
            glfwPollEvents();

            Sleep(5);
        }
    }
};

int main()
{
    // Read input parameters
    iniRead reader("../data/input_param.txt");
    float fx, fy, cx, cy;
    int w, h;
    string wndName, gtPath, colorFramePath, depthFramePath;
    reader.readData(fx, fy, cx, cy, w, h, wndName, gtPath, colorFramePath, depthFramePath);
    
    // Intrinsic parameters
    glm::mat3 K(fx, 0.0f, 0.0f, 0.0f, fy, 0.0f, cx, cy, 1.0f);

    MyWindow wnd(
        w, h, wndName, 
        K, 5000.0f,
        gtPath,
        colorFramePath, // 1st color frame
        depthFramePath, // 1st depth frame
        "../shader/bilateralFilter.comp",
        "../shader/calcVertMap.comp",
        "../shader/calcNormMap.comp"
    );
    wnd.render();

    return 0;
}

シェーダ

Bilateral Filterを実行するシェーダ

mugichoko.hatenablog.com

頂点・法線マップを生成するシェーダ

mugichoko.hatenablog.com

頂点マップを別フレームに投影するシェーダ

projVert.vert
#version 330

uniform sampler2D vertMap;

uniform mat4 T; // Translation matrix from the keyframe to the current view
uniform mat4 P; // Camera matrix
const mat4x3 A = mat4x3(
    1.0f, 0.0f, 0.0f,
    0.0f, 1.0f, 0.0f,
    0.0f, 0.0f, 1.0f,
    0.0f, 0.0f, 0.0f
    );

out vec3 color;

void main()
{
    ivec2 resolution = textureSize(vertMap, 0);
    vec2 texcoord = vec2(
        (gl_VertexID % resolution.x) / float(resolution.x),
        (gl_VertexID / resolution.x) / float(resolution.y)
    );  // NOTE: Texture coordinates: Left bottom origin

    // Vertex position
    vec4 vert = texture(vertMap, texcoord);
    color = vert.xyz;

    gl_Position = P * T * vert;
}
projVert.frag
#version 330

in vec3 color;

void main()
{
    //gl_FragColor = vec4(1.0, 1.0, 1.0, 1.0);
    gl_FragColor = vec4(color, 1.0);
}

結果

以下のGIF動画にあるように,初期フレームの奥行マップを別フレームの奥行マップに投影できた.「①初期フレームのカラー画像,②初期フレームの奥行マップ,③別フレームのカラー画像,④別フレームの奥行マップ(投影結果)」のループ再生を行っている.目標達成!

f:id:Mugichoko:20170731155535g:plain

尚,上記のGIF動画は,紹介しているプログラムの直接的な出力結果ではありません.紹介しているプログラムでは,単に,投影された頂点マップが画面に映し出されます.この内の1枚を取り出して,対応するカラー画像と合わせて見せているのが,上記のGIF動画です.

参考文献

  1. M. Keller, D. Lefloch, M. Lambers, S. Izadi, T. Weyrich, and A. Kolb, “Real-Time 3D Reconstruction in Dynamic Scenes Using Point-Based Fusion,” Proc. Int. Conf. on 3D Vision, pp. 1 - 8, 2013.