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 "..\..\addons\ofxGui\src\ofxGui.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)\n",(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\n\n",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，所以代码都差不多。

运行结果：

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

时间： 2024-10-28 02:18:36

kinectV2平面检测的相关文章

目标检测---搬砖一个ALPR自动车牌识别的环境

参考License Plate Detection and Recognition in Unconstrained Scenarios@https://www.cnblogs.com/greentomlee/p/10863363.html@https://github.com/sergiomsilva/alpr-unconstrained 环境The current version was tested in an Ubuntu 16.04 machine, with Keras 2.2.4,

ARKit 初体验

ARKIT是苹果公司在今年发布的一个AR开发包,用于现有的IOS设备,是的,就是用在手机或者平板上,类似于pokemon go的效果.看了下演示视屏,嗯,看起来很厉害. 对于一个资深软粉,居然被要求研究它,尽管不情愿,还是得去看看. 先花半天时间熟悉下swift,再去苹果开发者中心看看arkit的文档.接着MacBook和ipad pro也到了(请注意! ARKIT 理论上支持现有的所有IOS设备,但是,要进行平面解析以及位置追踪的话,处理器必须是A9及更快).将它们的系统都升级到最新(MacO

基于ORB-SLAM2的图片识别

基于ORB-SLAM2的图片识别,其功能是首先运行ORB-SLAM2,在运行过程中调起另一个线程进行图像识别,识别成功后在图片上渲染AR中的立方体模型. 识别过程主要基于ORB-SLAM2中的BoW算法,同样使用DBoW2库和ORB特征以及词汇树视觉词典.其主要流程可以概括为:运行ORB-SLAM2,加载ORB词典,然后读取训练图像进行BoW训练,完成之后就是SLAM的过程了,使用Pangolin做的UI界面,开启图像识别之后,线程会取当前帧和训练图像做BoW向量的相似性打分,我的策略是当当前帧

爆款AR游戏如何打造？网易杨鹏以《悠梦》为例详解前沿技术

7月31日,2018云创大会游戏论坛在杭州国际博览中心103B圆满举行.本场游戏论坛聚焦探讨了可能对游戏行业发展有重大推动的新技术.新实践,如AR.区块链.安全.大数据等.网易AR游戏生态合作负责人杨鹏表示,传统游戏模式趋同,AR游戏将是下一个重要风口网易AR游戏生态合作负责人杨鹏做了<从网易悠梦看AR前沿技术>的主题演讲,分享了网易基于AI技术和硬件基础所打造的爆款游戏<悠梦>,并详细了该游戏的AR游戏引擎.AR SDK和洞见内容浏览器等技术方案,助力<悠梦>成为年度

AR+ 实时音视频通话，×××无缝结合

今年中旬 Google 在万众期待下推出了 ARCore,能将现实与数码完美无缝地融合在一起,丰富我们的现实世界.通过它开发者可以更加快速方便地在 Android 平台开发 AR 应用,凭借 AR 技术大量产品能找到新颖的应用场景,甚至开辟出新的一条产品线. 目前市场上已经有不少基于 AR 技术的产品,例如宜家家居的 IKEA Place 应用提供了新型的在线选购家俬方式,用户只需要将手机摄像头摆向想要放置家具的角落,接着选取你想要的家具,通过简单的拖拉以及旋转即可完成布局,查看这件家具是否符合

基于RANSAC的点云面分割算法

该算法在RANSAC和空间检索树的基础上实现的. 算法思路: 1.点云抽希.法线估计 2.出局点索引存储声明 3.平面检测 for (size_t i = 0; i < cloudTemp->points.size(); i++) { 判断为出局点: if (检索一定量的临近点) { 判断搜索点集是否符合要求: 存储搜索的点集 : } RANSAC平面拟合(ransac计算平面模型参数): 判断平面拟合的正确性: /* * 利用拟合平面 计算点云到该面距离, 设置容差  判断点云是否在平面内

线段与非无限平面相交检测

参考自<游戏编程精粹1>,用该方法除了知道是否相交以外还可以得到相交点,从而用于其他判断. 原文用到了平面方程Ax+By+Cz+D=0,通过三角形求出平面,然后用方程求出线段交点 这里直接使用平面,以下为Unity中的实现代码: using System.Collections; using System.Collections.Generic; using UnityEngine; public class Practice : MonoBehaviour { public Transfor

基于人体部件检测子的行人检测

基于人体部件检测子的行人检测 edgelet feature body parts human detection Jointly likelihood function 读"B.Wu, R. Nevatia. Detection of Multiple,Partially Occluded Humans in a Single Image by Bayesian Combination of Edgelet Part Detectors[C], ICCV,2005." 笔记 论文主要

常见电子元器件检测方法。——Arvin

电子设备中使用着大量各种类型的电子元器件,设备发生故障大多是由于电子元器件失效或损坏引起的.因此怎么正确检测电子元器件就显得尤其重要,这也是电子维修人员必须掌握的技能.我在电器维修中积累了部分常见电子元器件检测经验和技巧,供大家参考. 1．测整流电桥各脚的极性 万用表置R×1k挡,黑表笔接桥堆的任意引脚,红表笔先后测其余三只脚,如果读数均为无穷大,则黑表笔所接为桥堆的输出正极,如果读数为4-10kΩ,则黑表笔所接引脚为桥堆的输出负极,其余的两引脚为桥堆的交流输入端. 使用数字万用表时只需将档位打