【openFrameworks】ポイントクラウドデータの分割テクスチャ保存
ポイントクラウドのデータをテクスチャとして保存する。という手順を最近、周りの方から教えていただきました。
今まできちんとプログラムをデータとして見てこなかったせいで、途中の処理過程や、発生したミスをスムーズに解決できませんでした。
やってみたことを備忘録としてまとめます。
今回やること
・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;
}
この記事が気に入ったらサポートをしてみませんか？