kinectV2平面检测

最近做一个关于kinect的东西，主要是在RGB图上提取指定的平面。对于kinect也是刚刚接触不是很熟悉，捣鼓了两天做了很粗糙的东西，但是也学到了一些东西，所以记录一下。

思路大概就是：

在RGB中选择一个感兴趣的平面，标定三个点来指定这个平面，由kinect深度图获取指定点的深度，也就是这个点在kinect坐标下的z值，再由kinect参数算出xy值，由此就可以算出这个平面在空间中的位置了，然后对RGB上的每一个点都使用同样的方法算出空间位置计算和指定平面的距离，去掉不再平面上的点即可。

这个思路最基本的就是如何把RGB上的点转换到空间中去。

本来最开始打算看看kinect有没有官方提供这样的功能，后来想了一下，不是很难算，就自己算了。

算法就是一个视椎：

知道了A点平面内的坐标和深度，计算B点位置。

可以直接使用投影矩阵。但是介于简便快速，还是直接上公式：

depth = 深度图取值
point.z = depth
point.y = 2*depth*tan(PI/6)*(RGB像素高度/2-pix.y)/RGB像素高度
point.x = (pix.x-RGB像素宽度/2)*point.y/(RGB像素高度/2-pix.y)

pix.x,pix.y就是RGB图上指定点的像素坐标。由于图像的原点在左上角所以做了个左边变换。

PI/6是kinect的垂直FOV的一般，也就是30度。
KinectV2的垂直fov是60度，水平是70.kinectV2输出的深度图分辨率是512*424，我根据这个视角和高度算过宽度，算出来结果基本就在424左右，所以这个深度图的确可以看成是视椎的一个截面。

另外，由于精度要求不是很高，所以红外摄像头和RGB摄像头之间的没有计算在内。

有了这些东西，就可以计算出RGB上指定坐标点在空间中的位置了。

使用下列公式可以计算出三个点指定的平面一般式ax+by+cz+d=0的参数adcd，用函数的形式给出：

void get_panel(Vec3f p1,Vec3f p2,Vec3f p3,int &a,int &b,int &c,int &d)  
{  

    a = ( (p2.y-p1.y)*(p3.z-p1.z)-(p2.z-p1.z)*(p3.y-p1.y) );  

    b = ( (p2.z-p1.z)*(p3.x-p1.x)-(p2.x-p1.x)*(p3.z-p1.z) );  

    c = ( (p2.x-p1.x)*(p3.y-p1.y)-(p2.y-p1.y)*(p3.x-p1.x) );  

    d = -(a*p1.x+b*p1.y+c*p1.z);  

} 

有了一般式，可以直接算出点到平面的距离：

float dis_pt2panel(Vec3f pt)
{  
    return abs(a*pt.x+b*pt.y+c*pt.z+d)/sqrt(a*a+b*b+c*c);  
}   

现在万事具备了，只要做一些简单的数学计算就可以算出结果。

途中遇到一些问题：

1、kinect返回的原始深度图是16位，前三位是player index，需要右移三位才能得到深度值

2、opengl纹理格式要使用GL_RGBA32F,把抽出的深度值换成小数放进去才正常。

下面是基于openframeworks写的一些散乱的代码：

//.h
#pragma once

#include "ofMain.h"
#include "....addonsofxGuisrcofxGui.h"
#include "ofxKinectCommonBridge.h"

class testApp : public ofBaseApp{
    public:
        void setup();
        void update();
        void draw();
        void keyPressed(int key);
        void keyReleased(int key);
        void mouseMoved(int x, int y);
        void mouseDragged(int x, int y, int button);
        void mousePressed(int x, int y, int button);
        void mouseReleased(int x, int y, int button);
        void windowResized(int w, int h);
        void dragEvent(ofDragInfo dragInfo);
        void gotMessage(ofMessage msg);

             ofxKinectCommonBridge kinect;


        ofPixels px;
        ofPixels pxg;
        int w;
        int h;
        ofShortImage Image_Depth;
        ofShortImage im;
        ofFbo fbo;
        
        ofImage out;
        ofImage ext;

        ofShader s;

        ofxPanel gui;
        ofxFloatSlider addjustgui;
};
    

//.cpp
#include "testApp.h"

/*
Readme
鼠标左中右按钮指定一个平面，不推荐超过RGB图的边界
console会打印选定点的空间坐标值
滑动滑块来调整平面距离精度，这个值就是每个点距离平面的阈值
这个值调大不会出现闪烁
测量计算单位精度为1cm


全局变量:a,b,c,d是指定平面参数
kinectV2参数：
水平FOV：70度
垂直FOV：60度
计算公式：
depth = 深度图取值
point.z = depth
point.y = 2*depth*tan(PI/6)*(RGB像素高度/2-pix.y)/RGB像素高度
point.x = (pix.x-RGB像素宽度/2)*point.y/(RGB像素高度/2-pix.y)


注意：kinect未检测到的点没有做处理
*/
ofVec2f po;
ofVec3f _3po;
ofVec2f po1;
ofVec3f _3po1;
ofVec2f po2;
ofVec3f _3po2;
float a,b,c,d;

void get_panel(ofVec3f p1,ofVec3f p2,ofVec3f p3)  
{  

    a = ( (p2.y-p1.y)*(p3.z-p1.z)-(p2.z-p1.z)*(p3.y-p1.y) );  

    b = ( (p2.z-p1.z)*(p3.x-p1.x)-(p2.x-p1.x)*(p3.z-p1.z) );  

    c = ( (p2.x-p1.x)*(p3.y-p1.y)-(p2.y-p1.y)*(p3.x-p1.x) );  

    d = -(a*p1.x+b*p1.y+c*p1.z);  

} 

float dis_pt2panel(ofVec3f pt)
{  

    return abs(a*pt.x+b*pt.y+c*pt.z+d)/sqrt(a*a+b*b+c*c);  

}   
//--------------------------------------------------------------
void testApp::setup(){

    ofSetWindowShape(1640,500);

    ofDisableArbTex();

    kinect.initSensor(1);
    kinect.initColorStream(true);
    kinect.initDepthStream(true);
    kinect.initBodyIndexStream();
    kinect.initSkeletonStream(true);
    //kinect.initIRStream(640,480);

    //simple start
    kinect.start();
    ofDisableAlphaBlending(); //Kinect alpha channel is default 0;

    w = kinect.getDepthPixelsRef().getWidth();
    h = kinect.getDepthPixelsRef().getHeight();
    px.allocate(w,h,ofImageType::OF_IMAGE_COLOR);

    pxg.allocate(w,h,ofImageType::OF_IMAGE_GRAYSCALE);

    Image_Depth.allocate(513,425,ofImageType::OF_IMAGE_GRAYSCALE);
    fbo.allocate(513,425,GL_RGBA32F);
    out.allocate(512,424,ofImageType::OF_IMAGE_COLOR_ALPHA);
    ext.allocate(512,424,ofImageType::OF_IMAGE_COLOR_ALPHA);
    s.load("vertex.c","fragment.c");

    gui.setup();
    gui.add(addjustgui.setup("adjust",0,0,20,200,20));

}

//--------------------------------------------------------------
void testApp::update(){
    kinect.update();
}

//--------------------------------------------------------------
void testApp::draw()
{
    ofBackground(0, 255);


    if(!kinect.isFrameNewDepth())
        return;

    kinect.mapDepthToColor(kinect.getColorPixelsRef(),px);

    kinect.drawDepth(640,0,640,480);

    ofShortPixels& P = kinect.getRawDepthPixelsRef();
    Image_Depth.setFromPixels(P);
    Image_Depth.update();
    int wd = P.getWidth();
    int ht = P.getHeight();   
    //计算指定点的空间坐标 
    ofShortColor C1 = P.getColor(po.x,po.y);
    _3po.z = C1.r>>3;
    _3po.y = _3po.z*0.57735f/212.f*(212-po.y);
    _3po.x = (po.x-256.f)*_3po.y/(-po.y+212.f);

    _3po1.z = P.getColor(po1.x,po1.y).r>>3;
    _3po1.y = _3po1.z*0.57735f/212.f*(212-po1.y);
    _3po1.x = (po1.x-256.f)*_3po1.y/(-po1.y+212.f);

    _3po2.z = P.getColor(po2.x,po2.y).r>>3;
    _3po2.y = _3po2.z*0.57735f/212.f*(212-po2.y);
    _3po2.x = (po2.x-256.f)*_3po2.y/(-po2.y+212.f);

    printf("(%d,%d,%d),(%d,%d,%d),(%d,%d,%d)
",(int)_3po.x,(int)_3po.y,(int)_3po.z,(int)_3po1.x,(int)_3po1.y,(int)_3po1.z,(int)_3po2.x,(int)_3po2.y,(int)_3po2.z);

    for(int i=0;i<wd;i++)
    {
        for(int j=0;j<ht;j++)
        {
　　　　　　　//将深度数据处理好放入fbo
            ofFloatColor CCC;
            ofShortColor C = P.getColor(i,j);
            CCC.r = (C.r>>3)/1000.f;
            CCC.g = (C.g>>3)/1000.f;
            CCC.b = (C.b>>3)/1000.f;
            CCC.a = 255;
            ext.setColor(i,j,CCC);
            Image_Depth.setColor(i,j,C);
　　　　　　　
            ofShortColor CC = P.getColor(i,j);
            float depth = CC.r>>3;
            float ry = depth*0.57735/212.f*(212-j);
            float rx = (i-256)*ry/(212-j);
            float rz = depth;

            ofVec3f v(rx,ry,rz);

            get_panel(_3po,_3po1,_3po2);

            float dis = dis_pt2panel(v);

            if(dis<=addjustgui)
            {
                out.setColor(i,j,px.getColor(i,j));
            }
            else
            {
                out.setColor(i,j,ofColor::red);
            }
        }
    }

    Image_Depth.update();
    ext.update();
    out.update();

    ofImage i(px);
    fbo.begin();
    ext.draw(0,0);
    fbo.end();
    s.begin();
    s.setUniform3f("iResolution",513,425,0);
    s.setUniform3f("pt1",_3po.x,_3po.y,_3po.z);
    s.setUniform3f("pt2",_3po1.x,_3po1.y,_3po1.z);
    s.setUniform3f("pt3",_3po2.x,_3po2.y,_3po2.z);
    s.setUniformTexture("texture0",fbo.getTextureReference(),1);
    s.setUniformTexture("colortex",i.getTextureReference(),2);
    s.setUniform1f("adjust",addjustgui);


    ofRect(-1,-1,0.5,1);

    s.end();

    i.draw(0,0);

    kinect.drawDepth(512,0,512,424);

    ofPushStyle();
    ofSetColor(255,0,0);
    ofRect(po.x,po.y,0,5,5);
    ofRect(po.x+512,po.y,0,5,5);
    ofSetColor(0,0,255);
    ofRect(po1.x,po1.y,0,5,5);
    ofRect(po1.x+512,po1.y,0,5,5);
    ofSetColor(0,255,0);
    ofRect(po2.x,po2.y,0,5,5);
    ofRect(po2.x+512,po2.y,0,5,5);
    ofPopStyle();

    out.draw(512,424,512,424);
    Image_Depth.draw(1024,0,512,424);

    gui.draw();
}
//--------------------------------------------------------------
void testApp::mousePressed(int x, int y, int button)
{
    printf("%d,%d

",x,y);
    if(button==0)
    {
        po.x = x;
        po.y = y;
    }
    else
        if(button==1)
        {
            po1.x = x;
            po1.y = y;
        }
        else if(button==2)
        {
            po2.x = x;
            po2.y = y;
        }

}

上面的代码实现，使用了for迭代和shader两种计算方案。由于是逐像素处理，fragment shader GPU计算起来会比CPU快得多。

shader代如下：

#version 120 
#extension GL_ARB_texture_rectangle : enable
uniform vec3 iResolution;
uniform vec4 cgd;
uniform float iGlobalTime;
uniform float ps;
uniform float factor;
uniform sampler2D texture0;
uniform sampler2D colortex;
uniform vec3 pt1;
uniform vec3 pt2;
uniform vec3 pt3;
uniform float adjust;

float a,b,c,d;

void get_panel(vec3 p1,vec3 p2,vec3 p3)  
{  
  
    a = ( (p2.y-p1.y)*(p3.z-p1.z)-(p2.z-p1.z)*(p3.y-p1.y) );  
  
    b = ( (p2.z-p1.z)*(p3.x-p1.x)-(p2.x-p1.x)*(p3.z-p1.z) );  
  
    c = ( (p2.x-p1.x)*(p3.y-p1.y)-(p2.y-p1.y)*(p3.x-p1.x) );  
  
    d = ( 0-(a*p1.x+b*p1.y+c*p1.z) );  
  
} 

float dis_pt2panel(vec3 pt)
{  
  
 return abs(a*pt.x+b*pt.y+c*pt.z+d)/sqrt(a*a+b*b+c*c);  
  
}   


void main()
{

    vec2 pos = gl_FragCoord.xy/iResolution.xy;
    vec4 c0 = texture2D(texture0, pos);
    vec4 ccc = texture2D(colortex,pos);
    vec4 blackc = vec4(1,0,0,0);

    
    float depth = c0.r*1000;
    
    float ry = depth*0.57735/212.f*(212-pos.y*424);
    float rx = (pos.x*512-256)*ry/(212-pos.y*424);
    float rz = depth;
    
    
    vec3 pt = {rx,ry,rz};

    
    get_panel(pt1,pt2,pt3);
    
    float dis = dis_pt2panel(pt);
    
    if(dis<=adjust)
    {
        gl_FragColor = ccc;
    }
    else
    {
        gl_FragColor = blackc;
    }

}

方法都是一样的，由于GLSL类C，所以代码都差不多。

运行结果：

可以看出来，基本功能还是实现了，但是在准确性上依然有很多问题需要解决。