QT集成IntelRealSense双目摄像头3，3D显示

前两篇文章，介绍了如何继承intel realsense相机和opengl。

这里介绍如何给深度数据和色彩数据一块显示到opengl里面。

首先，需要了解深度数据和彩色数据是如何存储的。先说彩色数据。彩色图像一般都是RGB，也就是每个像素有三个字节，分别是RGB，如果一个图像宽度是2，高度也是2。就是一共有4个像素。如下图。像素顺序是从左上到右下，依次排列。

假设第一个像素是红色，就是（255,0,0），第二个字节是（0,255,0）,第三个是（0,0,255）,最后一个是（0,0,0）。这个图像数据全部的内存就是0,255,0,0,0,255,0,0,255,0,0,0。一共2*2*3=12个字节。实际就是按左右到右下的顺序，给每个点的rgb值放到一块。

依次类推，宽width，高height的图像数据，实际上就是按左上到右下，width*height个RGB数据放到了一起。就是width*height*3个字节。

然后是深度数据，深度数据是16位，就是两个字节的无符号数，实际上就是按左上到右下的顺序，给4个unsigned short值放到了一块例如 800，1000，200，30。每个unsigned short两个字节，共2*2*2=8个字节。

依次类推，宽是width，高是height的深度数据，就是按左上到右下，width*height个unsigned short放到一起，共width*height*2个字节。

有了上面的前题，在Camera类里面增加两个指针，分别存储彩色图像数据，和深度数据，增加两个整数，用于存储图像宽度和高度，以及增加两个QImage，分别是彩色和深度图像。

第一步修改Camera类，每次抓取到图像，给深度数据拷贝到自己准备的缓存区。同时创建两个用于显示的深度图和彩色图。

修改后的Camera类：

cpp 复制代码

#ifndef CAMERA_H
#define CAMERA_H

#include <QObject>
#include <QThread>
#include <librealsense2/rsutil.h>
#include <librealsense2/rs.hpp>
#include <QImage>
class Camera :public QThread
{
    Q_OBJECT
public:
    Camera();
    static Camera* getInstance();//单例模式
    void startCapture();        //开始采集
    void stopCapture();         //结束采集
    void run() override;        //线程函数
    unsigned short * depthData;//深度数据缓存区，需要初始化才能使用
    unsigned char* colorData;;//存储颜色数据缓存区，需要初始化才能使用
    int width;              //宽度
    int height;             //高度
    QImage colorImage;          //色彩图片
    QImage depthImage;         //深度图片
private slots:
    void doCam(int code,QString msg);   //用于接收线程信息，并弹出消息框
signals:
    void onCameraEvent(int code,QString msg);   //线程内部发送消息
    void onFrame();                             //线程内部发送图像采集成功信号
private:
    bool isCapture;             //是否采集中
    rs2::pipeline pipe;         //采集采集对象
};

#endif // CAMERA_H

cpp 复制代码

#include "camera.h"
#include <QDebug>
#include <QException>
#include <common.h>
#include <GL/glu.h>
Camera::Camera()
{
    width=height=0;//没有获取到数据之前，是0
    colorData=NULL;//没有数据之前是空
    depthData=NULL;//没有数据之前是空
    connect(this,&Camera::onCameraEvent,this,&Camera::doCam);//链接线程内部的信号
}
/**
 * @brief Camera::getInstance 单例模式
 * @return
 */
Camera* Camera::getInstance(){
    static Camera* ins=NULL;//static代码，唯一对象
    if(ins==NULL)ins=new Camera();//创建一个实力
    return ins;
}

void Camera::startCapture(){
    loading("打开摄像头",0);//显示打开摄像头弹窗,一直显示，直到成功或是失败
     this->start();//开始子线程
}
/**
 * @brief Camera::stopCapture 停止采集
 */
void Camera::stopCapture(){
    isCapture=false;//退出子线程
    pipe.stop();//停止相机
}
/**
 * @brief Camera::doCam 接收线程的消息，并弹窗显示
 * @param code 消息编码，0表示成功
 * @param msg 消息内容
 */
void Camera::doCam(int code,QString msg){
    if(code==0)success(msg);
    else error(msg);
}
/**
 * @brief Camera::run 线程函数，用于打开相机和采集图像
 */
void Camera::run(){

    try {
        pipe.start();       //尝试打开相机
    }
    catch (const std::exception& e)
    {
        emit onCameraEvent(1, e.what() ); //如果没有成功，发送给主线程一个消息
        return;//返回，不继续了
    }
    emit onCameraEvent(0,"相机打开成功"); //如果打开成功，也发送一个
    rs2::colorizer color_map;//用于将深度数据，转换为图像数据色彩数据的map，
    isCapture=true;//循环控制变量
    while(isCapture){
        try {
            rs2::frameset data= pipe.wait_for_frames(); //获取一帧数据          
            rs2::frame colo=data.get_color_frame();    //获取彩色图像
            rs2::frame dept=data.get_depth_frame();  //获取深度数据,并转换为可见图
            if(width==0 || height==0){              //如果没有初始化
                width = dept.as<rs2::video_frame>().get_width();     //获取图像宽度
                height= dept.as<rs2::video_frame>().get_height();    //获取图像高度
                depthData=(unsigned short*)malloc(2*width*height);  //内存初始化，深度数据是16位，就是两个字节。总字节数量就是宽度*高度*2
                colorData=(unsigned char*)malloc(3*width*height);   //彩色内存初始化，彩色数据RGB，就是3个字节，总字节就是宽度*高度*3
            }
            if(depthData!=NULL){
                memcpy(depthData,dept.get_data(),width*height*2);   //拷贝深度图像数据
                depthImage= QImage((uchar*)dept.apply_filter(color_map).get_data(),width,height,QImage::Format_RGB888); //深度数据创建一个图片
            }
            if(colorData!=NULL){
                memcpy(colorData,colo.get_data(),width*height*3);   //拷贝彩色图像数据
                colorImage=QImage((uchar*)colo.get_data(),width,height,QImage::Format_BGR888);//彩色数据创建一个图片
            }           

            emit onFrame(); //发送消息，数据准备好了

        }
        catch (const std::exception& e) {
            emit onCameraEvent(2,e.what());
        }
    }
}

第二步：在GLwidget类里面绘制三维数据。

具体的原理是，监听Camera类的onFrame事件，每次监听到，就重新绘制。绘制的时候，循环像素的行和列，根据行x列y计算当前像素的下下标i=x+y*width。根据下标，去深度数据里找到对应的深度。去色彩数据寻找对应的RGB值。然后在gl绘制一个点就行了。核心代码：

cpp 复制代码

 Camera* cam=Camera::getInstance();//获取相机示例
    int disx=cam->width/2;//x方向偏移，到中心点
    int disy=cam->height/2;//y方向偏移，到中心点
    int disz=1000;//这个是随便写的
    glBegin(GL_POINTS);//开始绘制点
    for(int y=0;y<cam->height;y++){//循环行
        for(int x=0;x<cam->width;x++){//循环列
            int i=x+y*cam->width;//计算当前像素的下标，
            int z=cam->depthData[i];//深度
            if(z!=0){
              glColor3f(cam->colorData[i*3]/255.0,cam->colorData[i*3+1]/255.0,cam->colorData[i*3+2]/255.0); //rgb是0-255，opengl用0-1表示
              glVertex3f(x-disx,disy-y,disz-z);//绘制一个点
            }

        }
    }
    glEnd();

完整GLWidget类代码：

cpp 复制代码

#ifndef GLWIGET_H
#define GLWIGET_H

#include <QObject>
#include <QGLWidget>
#include <GL/gl.h>
#include <GL/glu.h>
#include <QMouseEvent>
#include <QWheelEvent>
class GLWidget : public QGLWidget
{
    Q_OBJECT
public:
    GLWidget();
    static GLWidget* getInstance();          //单例模式
protected:
    void resizeGL(int w, int h) override;   //窗口大小改变的时候，gl重新初始化
    void initializeGL() override;           //初始化gl
    void paintGL() override;                //核心绘制
    void mousePressEvent(QMouseEvent *event) override;
    void mouseMoveEvent(QMouseEvent *event) override;
    void mouseReleaseEvent(QMouseEvent *event) override;
    void wheelEvent(QWheelEvent *event) override;
private slots:
    void onFrame();//响应数据采集到事件
private:
    double r;//相机距离中心的的半径
    double degZ;//在水平面的角度
    double degY;//在垂直面的角度
    double camx,camy,camz;//相机位置
    void refresh();//重新绘制
    QPoint old;//鼠标原始位置
    bool isPressed;//是否按下
    double rate;//用于换算弧度的比例
};

#endif // GLWIGET_H

cpp 复制代码

#include "glwidget.h"
#include <qmath.h>
#include <QDebug>
#include <camera.h>
GLWidget::GLWidget()
{
    r=1000;//默认距离
    degY=degZ=0;
    camz=r*qCos(degZ);
    camx=r*qSin(degZ);
    camy=r*qSin(degY);
    connect(Camera::getInstance(),&Camera::onFrame,this,&GLWidget::onFrame);//链接相机采集到数据事件
}

/**
 * @brief GLWidget::getInstance 单例模式
 * @return
 */
GLWidget* GLWidget::getInstance(){
    GLWidget* ins=NULL;
    if(ins==NULL)ins=new GLWidget();
    return ins;
}
/**
 * @brief GLWidget::initializeGL 初始化opengl。可以步写
 */
void GLWidget::initializeGL(){

}
/**
 * @brief GLWidget::resizeGL 窗口大小改变
 * @param w
 * @param h
 */
void GLWidget::resizeGL(int w, int h){
    glViewport(0,0,w,h);            //重新适应窗口大小
    glMatrixMode (GL_PROJECTION);
    glLoadIdentity ();
    gluPerspective(60.0, (GLfloat) w/(GLfloat) h, 1, 2000.0);           //设置相机投影参数
    glMatrixMode(GL_MODELVIEW);
    glLoadIdentity();

}
/**
 * @brief GLWidget::paintGL 绘制核心方法
 */
void GLWidget::paintGL(){
    glClearColor(0,0,0,0);                       //背景色
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT | GL_STENCIL_BUFFER_BIT);//清除上一次的缓存
    glLoadIdentity();  //加载单位矩阵
    gluLookAt(camx,camy,camz, 0,0,0,   0.0,1.0,0);                             //设置相机
    Camera* cam=Camera::getInstance();//获取相机示例
    int disx=cam->width/2;//x方向偏移，到中心点
    int disy=cam->height/2;//y方向偏移，到中心点
    int disz=1000;//这个是随便写的
    glBegin(GL_POINTS);//开始绘制点
    for(int y=0;y<cam->height;y++){//循环行
        for(int x=0;x<cam->width;x++){//循环列
            int i=x+y*cam->width;//计算当前像素的下标，
            int z=cam->depthData[i];//深度
            if(z!=0){
              glColor3f(cam->colorData[i*3]/255.0,cam->colorData[i*3+1]/255.0,cam->colorData[i*3+2]/255.0); //rgb是0-255，opengl用0-1表示
              glVertex3f(x-disx,disy-y,disz-z);//绘制一个点
            }
        }
    }
    glEnd();

}
/**
 * @brief GLWidget::onFrame 如果有数据，就重新绘制一下
 */
void GLWidget::onFrame(){
    this->refresh();
}
/**
 * @brief GLWidget::refresh 重新计算相机位置
 */
void GLWidget::refresh(){
    camz=r*qCos(degZ);
    camx=r*qSin(degZ);
    camy=r*qSin(degY);

    this->update();
}


void GLWidget::wheelEvent(QWheelEvent *event){
    qDebug()<<r<<event->delta();
    r+=event->delta()/20;
    if(r<20)r=20;
    if(r>1800)r=1800;
    refresh();
}
void GLWidget::mouseMoveEvent(QMouseEvent *event){
    if(isPressed){
        QPoint p=event->pos();
        degZ+=(p.x()-old.x())/5.0;
        degY+=(p.y()-old.y())/5.0;
        old=p;
        refresh();
    }
}

void GLWidget::mousePressEvent(QMouseEvent *event){

    if(event->button()==Qt::LeftButton){
        isPressed=true;
        old=event->pos();
    }
}
void GLWidget::mouseReleaseEvent(QMouseEvent *event){
    if(event->button()==Qt::LeftButton){
        isPressed=false;
    }
}

运行效果：

PS：common文件里面有一个用于弹出提示的工具，仿的andoroid系统toast效果，实现tip，success，error，loading（等待）等效果。

完整代码见文章附件。