【openFrameworks】ポイントクラウドデータの分割テクスチャ保存

2020年5月10日 00:03

ポイントクラウドのデータをテクスチャとして保存する。という手順を最近、周りの方から教えていただきました。
今まできちんとプログラムをデータとして見てこなかったせいで、途中の処理過程や、発生したミスをスムーズに解決できませんでした。
やってみたことを備忘録としてまとめます。

今回やること

・openFrameworksでkinectV2の深度カメラとカラーカメラから色付きのポイントクラウドを取得する。
・取得したポイントクラウドの情報をテクスチャデータとして保存。
・保存したテクスチャデータをSyphonで送信。

最終的には以下のようにポイントクラウドから3つのテクスチャを取り出します。

開発環境

・Mac Book Pro (Catalina)
・openFrameworks v0.11.0
・Xcode 10.3

・openFrameworksでkinectV2の深度カメラとカラーカメラから色付きのポイントクラウドを取得する。

今回はofxKinectV2を使用します。
このアドオンのexampleでは深度データの取得と、カラーカメラを深度カメラの画角に合わせるキャリブレーションをかけたカラーデータの取得がされています。
これをそのまま使用してカラー付きのポイントクラウドを取得します。

表示の際にPCLをつかってダウンサンプリングを行いましたが、ここについては別の記事でまとめています。

ofApp.hファイル

#include "ofMain.h"
#include "ofxKinectV2.h"

class ofApp : public ofBaseApp{
public:
		void setup();
		void update();
		void draw();
    
    std::vector<std::shared_ptr<ofxKinectV2>> kinects;
    std::vector<ofTexture> texRGB;
    std::vector<ofTexture> texRGBRegistered;
    std::vector<ofTexture> texIR;
    std::vector<ofTexture> texDepth;
    
    std::size_t currentKinect = 0;
    
    ofEasyCam ecam;
    ofVboMesh mesh;
};

ofApp.cppファイル

#include "ofApp.h"

//--------------------------------------------------------------
void ofApp::setup(){
    ofSetVerticalSync(true);
    ofSetFrameRate(30);
    ofBackground(0);
    
    //see how many devices we have.
    ofxKinectV2 tmp;
    std::vector <ofxKinectV2::KinectDeviceInfo> deviceList = tmp.getDeviceList();
    
    //allocate for this many devices
    kinects.resize(deviceList.size());
    texDepth.resize(kinects.size());
    texRGB.resize(kinects.size());
    texRGBRegistered.resize(kinects.size());
    texIR.resize(kinects.size());
    
    ofxKinectV2::Settings ksettings;
    ksettings.enableRGB = true;
    ksettings.enableIR = true;
    ksettings.enableDepth = true;
    ksettings.enableRGBRegistration = true;
    ksettings.config.MinDepth = 0.5;
    ksettings.config.MaxDepth = 8.0;
    
    for(int d = 0; d < kinects.size(); d++) {
         kinects[d] = std::make_shared<ofxKinectV2>();
         kinects[d]->open(deviceList[d].serial, ksettings);
    }
    
    //drawMesh
    mesh.setUsage(GL_DYNAMIC_DRAW);
    mesh.setMode(OF_PRIMITIVE_POINTS);
    
    //Camera
    ecam.setAutoDistance(false);
    ecam.setPosition(0, 0, 100);
    ecam.lookAt(ofVec3f(0,0,0));
    
    

}

//--------------------------------------------------------------
void ofApp::update(){
    
    //Kinect
    for (int d = 0; d < kinects.size(); d++)
    {
         kinects[d]->update();
         
         if (kinects[d]->isFrameNew())
         {
              if( kinects[d]->isRGBEnabled()) texRGB[d].loadData(kinects[d]->getPixels());
              if(kinects[d]->getRegisteredPixels().getWidth() > 10) texRGBRegistered[d].loadData(kinects[d]->getRegisteredPixels());
              if(kinects[d]->isDepthEnabled() ) texDepth[d].loadData(kinects[d]->getDepthPixels());
              
              //depth
              
              mesh.clear();
              int count = 0;
              if (showPointCloud)
              {
                   for(int y = 0; y < h; y += step) {
                        for(int x = 0; x < w; x += step) {
                             float dist = kinects[d]->getDistanceAt(x, y)*1000;
                             ofPoint pt = kinects[d]->getWorldCoordinateAt(x, y)*1;
                             if(dist > kinects[d]->minDistance && dist < kinects[d]->maxDistance) {
                                  
                                  ofColor c;
                                  c=(kinects[d]->getRegisteredPixels().getColor(x, y));
                                       mesh.addColor(c);
                                       mesh.addVertex(pt);
                                  }
                             }
                        }
                   }
              }
         }
    }

}

//--------------------------------------------------------------
void ofApp::draw(){
    fboS.begin();
    ofClear(0);
    if (isAssist)
    {
         texRGB[0].draw(0, texRGB[0].getHeight()*0.8, texRGB[0].getWidth()*0.2, texRGB[0].getHeight()*0.2);
    }
    ofPushStyle();
    glPointSize(pointSize);
    ecam.begin();
    glEnable(GL_DEPTH_TEST);
    if(isAssist){
         ofDrawAxis(100);
         ofDrawGrid(50,10);
    }
    ofPushMatrix();
    ofTranslate(0, 0, 100);
    ofScale(100, -100, -100);
    mesh.draw();
    ofPopMatrix();
    ecam.end();
    ofPopStyle();
    
    fboS.end();
    fboS.draw(0, 0, ofGetWidth(), ofGetHeight());
    
    glDisable(GL_DEPTH_TEST);
    
    if( kinects.size() < 1 ) {
         ofDrawBitmapStringHighlight( "No Kinects Detected", 40, 40 );
    }else{
         ofDrawBitmapStringHighlight(ofToString(ofGetFrameRate()), 10, 20);
         ofDrawBitmapStringHighlight("Device Count : " + ofToString(kinects.size()), 10, 40);
    }
    
    output.publishTexture(&fboS.getTexture());
}

・取得したポイントクラウドの情報をテクスチャデータとして保存。

今回の記事の中心部分になります。
そもそもなぜポイントクラウドのデータをテクスチャにして送るのかというところですが、点ごとのデータにすることで、他のTouchDesignerなどの他のアプリケーションでポイントクラウドのデータを点ごとに扱えるようになるなどの利点があります。
また、ポイントクラウドの取得と描画のPCを分けることで処理ができたりなどの利点もあります。
Shaderを使えば受け取ったそのまま点群をGPU処理で扱えます。
そもそも3Dデータを点群で扱う方法はShaderを学んだことがある人にとって当たり前のことのようで。。
自分は今回をきっかけに初めて知りました。
実際にShaderで点群を扱った部分に関してはこちらの記事にまとめています。

今回はポイントクラウドのデータの位置情報を上位ビット、下位ビットにわけます。
これにカラー情報を加えてポイントクラウドのデータを全部で3つのテクスチャにします。
テクスチャへの位置情報を割り当ては以下の様に行われます。
まずポイントクラウドの一つ一つのポイントを何からの順番で（例えば手前から奥方向になど）で数えていきます。
この作業をソートというらしいです。
そして数えたポイントの(x,y,z)の座標情報を、テクスチャの(r,g,b)の情報に当てはめていきます。
テクスチャの(0,0)には最初に数えたポイント情報が入るという感じです。
ポイントのカラー情報を保存するテクスチャに関してはポイントの(R,G,B)情報がテクスチャの(r,g,b)の情報に当てはまります。

位置情報を上位ビット、下位ビットに分ける必要があるのかを説明します。
ofTextureに保存するデータの情報量は16bitです。
ポイントはカラー情報は値が0〜255の8bitの範囲におさまるので問題ありません。
しかし、位置情報は小数点まで含めると例えば
(-0.202315,　0.350982,　0.987822)
など、8bitのデータ量が保存できる256通り以上のデータ量を含んでいます。
なので、データを16bitのshort型を用意して内包した後に8bitのchar型を二つ用意し、上位ビットと下位ビットに分けて保存します。　

実際にc++ではnion型(共用体)を教えてもらいこれを使って記述しました。

union DataConverter {
    char data[4];
    short sValue[2];
    float fValue;
    int iValue;
};

union型では同じメモリ領域を複数のメンバが共用する構造を取ります。
以下の関数で上位ビットと下位ビットの分割を行います。

void ofApp::splitToBytes(ofPoint p_in, ofColor& low, ofColor& up)
{
    PointType p;
    p.x = floor(p_in.x * 10000);
    p.y = floor(p_in.z * 10000);
    p.z = floor(p_in.y * 10000);
    
    DataConverter converterX;
    converterX.iValue = 0;//初期化
    converterX.sValue[0] = p.x;
    
    DataConverter converterY;
    converterY.iValue = 0;//初期化
    converterY.sValue[0] = p.y;
    
    DataConverter converterZ;
    converterZ.iValue = 0;//初期化
    converterZ.sValue[0] = p.z;
    
    // lower
    char x_lower = converterX.data[0];
    char y_lower = converterY.data[0];
    char z_lower = converterZ.data[0];
    ofColor c_lower(x_lower, y_lower, z_lower, 255);
    
    // upper
    char x_upper = converterX.data[1];
    char y_upper = converterY.data[1];
    char z_upper = converterZ.data[1];
    ofColor c_upper(x_upper, y_upper, z_upper, 255);
    
    low = c_lower;
    up = c_upper;
    
}

最後に今回使用したプログラムを載せておきます。

ofApp.h

#include "ofMain.h"
#include "ofxKinectV2.h"
#include "ofxSyphon.h"
#include "ofxGui.h"

class ofApp : public ofBaseApp{

	public:
		void setup();
		void update();
		void draw();
    
    ofxPanel panel;
    
    std::vector<std::shared_ptr<ofxKinectV2>> kinects;
    std::vector<ofTexture> texRGB;
    std::vector<ofTexture> texRGBRegistered;
    std::vector<ofTexture> texIR;
    std::vector<ofTexture> texDepth;
    
    std::size_t currentKinect = 0;
    
    int numRows = 2;
    int numColumns = 2;
    
    ofEasyCam ecam;
    ofVboMesh mesh;
    bool showPointCloud = true;
    
    ofParameter<bool> isAssist;
    ofParameter<int> step;
    ofParameter<bool> isVGF;
    ofParameter<float> pointSize;
    
    //Syphon
    ofxSyphonServer output;
    ofFbo fboS;
    ofxSyphonServer outputUpByte;
    ofxSyphonServer outputLowByte;
    ofxSyphonServer outputColorByte;
    
    // Pixels
    ofPixels pixLowByte;
    ofPixels pixUpByte;
    ofPixels pixColorByte;
    // Texture
    int texWidth = 256;
    int texHeight = 256;
    ofTexture texLowByte;
    ofTexture texUpByte;
    ofTexture texColorByte;
    
    // Convert to Bytes
    void splitToBytes(ofPoint p_in, ofColor& low, ofColor& up);
};

union DataConverter {
    char data[4];
    short sValue[2];
    float fValue;
    int iValue;
};

ofApp.cpp

#include "ofApp.h"

//--------------------------------------------------------------
void ofApp::setup(){
    ofSetVerticalSync(true);
    ofSetFrameRate(30);
    ofBackground(0);
    
    //see how many devices we have.
    ofxKinectV2 tmp;
    std::vector <ofxKinectV2::KinectDeviceInfo> deviceList = tmp.getDeviceList();
    
    //allocate for this many devices
    kinects.resize(deviceList.size());
    texDepth.resize(kinects.size());
    texRGB.resize(kinects.size());
    texRGBRegistered.resize(kinects.size());
    texIR.resize(kinects.size());
    
    panel.setup("", "settings.xml", 10, 100);
    
    ofxKinectV2::Settings ksettings;
    ksettings.enableRGB = true;
    ksettings.enableIR = true;
    ksettings.enableDepth = true;
    ksettings.enableRGBRegistration = true;
    ksettings.config.MinDepth = 0.5;
    ksettings.config.MaxDepth = 8.0;
    
    for(int d = 0; d < kinects.size(); d++) {
         kinects[d] = std::make_shared<ofxKinectV2>();
         kinects[d]->open(deviceList[d].serial, ksettings);
         panel.add(kinects[d]->params);
         panel.add(isAssist.set("Grid", false));
         panel.add(step.set("SamplrRate", 1, 1, 5));
         panel.add(isVGF.set("VGF", true));
         panel.add(pointSize.set("PointSize", 2, 1, 20));
    }
    panel.loadFromFile("settings.xml");
    
    //drawMesh
    mesh.setUsage(GL_DYNAMIC_DRAW);
    mesh.setMode(OF_PRIMITIVE_POINTS);
    
    //Camera
    ecam.setAutoDistance(false);
    ecam.setPosition(0, 0, 100);
    ecam.lookAt(ofVec3f(0,0,0));
    
    //Syphon
    output.setName("OUTPUT");
    fboS.allocate(1920, 1080, GL_RGBA);
    outputUpByte.setName("UpByte");
    outputLowByte.setName("LowByte");
    outputColorByte.setName("ColorByte");
    
    // Pixels
    pixLowByte.allocate(texWidth, texHeight, OF_PIXELS_RGB);
    pixUpByte.allocate(texWidth, texHeight, OF_PIXELS_RGB);
    pixColorByte.allocate(texWidth, texHeight, OF_PIXELS_RGB);
    
    // Texture
    texLowByte.allocate(pixLowByte);
    texLowByte.setTextureMinMagFilter(GL_NEAREST, GL_NEAREST);
    
    texUpByte.allocate(pixUpByte);
    texUpByte.setTextureMinMagFilter(GL_NEAREST, GL_NEAREST);
    
    texColorByte.allocate(pixColorByte);
    texColorByte.setTextureMinMagFilter(GL_NEAREST, GL_NEAREST);

}

//--------------------------------------------------------------
void ofApp::update(){
    
    for (int y = 0; y < texHeight; y++)
    {
         for (int x = 0; x < texWidth; x++)
         {
              pixLowByte.setColor(x, y, ofColor(0, 0, 0, 0));
              pixUpByte.setColor(x, y, ofColor(0, 0, 0, 0));
              pixColorByte.setColor(x, y, ofColor(0, 0, 0, 0));
         }
    }
    
    
    //Kinect
    for (int d = 0; d < kinects.size(); d++)
    {
         kinects[d]->update();
         
         if (kinects[d]->isFrameNew())
         {
              if( kinects[d]->isRGBEnabled()) texRGB[d].loadData(kinects[d]->getPixels());
              if(kinects[d]->getRegisteredPixels().getWidth() > 10) texRGBRegistered[d].loadData(kinects[d]->getRegisteredPixels());
              if(kinects[d]->isDepthEnabled() ) texDepth[d].loadData(kinects[d]->getDepthPixels());
              
              //depth
              int h = kinects[0]->getDepthPixels().getHeight();
              int w = kinects[0]->getDepthPixels().getWidth();
              
              mesh.clear();
              int count = 0;
              ofPoint p_in;
              if (showPointCloud)
              {
                   for(int y = 0; y < h; y += step) {
                        for(int x = 0; x < w; x += step) {
                             float dist = kinects[d]->getDistanceAt(x, y)*1000;
                             float xy = x + (y*w);
                             ofPoint pt = kinects[d]->getWorldCoordinateAt(x, y)*1;
                             
                             if(dist > kinects[d]->minDistance && dist < kinects[d]->maxDistance) {
                                  
                                  ofColor c;
                                  float h = ofMap(dist, 50, 200, 180, 255, true);
                                  c=(kinects[d]->getRegisteredPixels().getColor(x, y));
                                  mesh.addColor(c);
                                  mesh.addVertex(pt);
                                  count++;
                                  p_in.x = pt.x;
                                  p_in.y = pt.y;
                                  p_in.z = pt.z;
                             }
                        }
                   }

                   for(int i = 0; i <count; i++) {
                        ofColor c_lower;
                        ofColor c_upper;
                        splitToBytes(ofPoint(p_in.x, p_in.y, p_in.z), c_lower, c_upper);
                        
                        int x = i % texWidth;
                        int y = i / texWidth;
                        pixLowByte.setColor(x, y, c_lower);
                        pixUpByte.setColor(x, y, c_upper);
                        pixColorByte.setColor(x, y, ofColor(p_in.r, p_in.g, p_in.b, 255));
                   }
              }
         }
    }

}

//--------------------------------------------------------------
void ofApp::draw(){
    fboS.begin();
    ofClear(0);
    if (isAssist)
    {
         texRGB[0].draw(0, texRGB[0].getHeight()*0.8, texRGB[0].getWidth()*0.2, texRGB[0].getHeight()*0.2);
    }
    ofPushStyle();
    glPointSize(pointSize);
    ecam.begin();
    glEnable(GL_DEPTH_TEST);
    if(isAssist){
         ofDrawAxis(100);
         ofDrawGrid(50,10);
    }
    ofPushMatrix();
    ofTranslate(0, 0, 100);
    ofScale(100, -100, -100);
    mesh.draw();
    ofPopMatrix();
    ecam.end();
    ofPopStyle();
    
    fboS.end();
    fboS.draw(0, 0, ofGetWidth(), ofGetHeight());
    
    glDisable(GL_DEPTH_TEST);
    
    if( kinects.size() < 1 ) {
         ofDrawBitmapStringHighlight( "No Kinects Detected", 40, 40 );
    }else{
         ofDrawBitmapStringHighlight(ofToString(ofGetFrameRate()), 10, 20);
         ofDrawBitmapStringHighlight("Device Count : " + ofToString(kinects.size()), 10, 40);
    }
    panel.draw();
    
    output.publishTexture(&fboS.getTexture());
    
    texLowByte.loadData(pixLowByte);
    texUpByte.loadData(pixUpByte);
    texColorByte.loadData(pixColorByte);
    
    outputUpByte.publishTexture(&texUpByte);
    outputLowByte.publishTexture(&texLowByte);
    outputColorByte.publishTexture(&texColorByte);

}

//-----------
void ofApp::splitToBytes(ofPoint p_in, ofColor& low, ofColor& up)
{
    PointType p;
    p.x = floor(p_in.x * 10000);
    p.y = floor(p_in.z * 10000);
    p.z = floor(p_in.y * 10000);

    DataConverter converterX;
    converterX.iValue = 0;
    converterX.sValue[0] = p.x;
    
    DataConverter converterY;
    converterY.iValue = 0;
    converterY.sValue[0] = p.y;
    
    DataConverter converterZ;
    converterZ.iValue = 0;
    converterZ.sValue[0] = p.z;
    
    // lower
    char x_lower = converterX.data[0];
    char y_lower = converterY.data[0];
    char z_lower = converterZ.data[0];
    ofColor c_lower(x_lower, y_lower, z_lower, 255);
    
    
    // upper
    char x_upper = converterX.data[1];
    char y_upper = converterY.data[1];
    char z_upper = converterZ.data[1];
    ofColor c_upper(x_upper, y_upper, z_upper, 255);
    
    low = c_lower;
    up = c_upper;
    
}

【openFrameworks】ポイントクラウドデータの分割テクスチャ保存

今回やること

開発環境

いいなと思ったら応援しよう！