QT下根据深度信息计算物体尺寸并UI显示的简单方案

本文展示一个根据输入深度点云信息计算物体长宽尺寸的最简单的方案。根据物体深度信息与背景深度信息的差得到当前物体与摄像头的距离，然后通过距离值得到物体x y方向的边缘像素位置，然后结合像素位置与摄像头的FOV信息，计算物体的实际尺寸信息。

将深度信息的单位米转为毫米：

static void transfor_frame(float*pc, Mat& frame, int wid, int hgh)
{
    float tmp_depth_max_double = 7.5f;

    for (int j = 0; j < hgh; ++j) {
        for (int i = 0; i < wid; ++i) {
            float depth = pc[j*wid + i];
            if (depth < 0.010 || depth >= tmp_depth_max_double) {
                frame.at<unsigned short>(j, i) = 0;
            }
            else {
                frame.at< unsigned short >(j, i) = (int)(depth*1000.0f);
                if (j == 90 && i == 120) {
                    printf("center depth mm : %d\n", (int)(depth*1000.0f));
                }
            }
        }
    }
}

中值滤波去噪，重要：

static void img_blur(Mat& src, Mat & dst, int ksize)
{
    medianBlur(src, dst, ksize);
}

计算物体深度信息与背景地面深度信息的差值：

static void diff(Mat& gnd, Mat& blu, Mat& diff, int wid, int hgh)
{

    for (int j = 0; j < hgh; ++j) {
        for (int i = 0; i < wid; ++i) {
            if ((gnd.at< unsigned short >(j, i) - blu.at< unsigned short >(j, i)) >= 0)
                diff.at< unsigned short >(j, i) = gnd.at< unsigned short >(j, i) - blu.at< unsigned short >(j, i);
            else
                diff.at< unsigned short >(j, i) = 0;
            if (j == 90 && i == 120) {
                    printf("center diff mm : %d\n", diff.at< unsigned short >(j, i));
            }
        }
    }

}

获取物体中心点与摄像头的距离信息：

static unsigned int get_dist(Mat& src, int wid, int hgh)
{
    unsigned int dist = 0;

    for (int j = (hgh/2-1); j < (hgh/2+1); ++j) {
        for (int i = (wid/2-1); i < (wid/2+1); ++i) {
            if (src.at< unsigned short >(j, i) > dist )
                if((dist !=0 && src.at< unsigned short >(j, i) < (dist + 50)) || dist ==0)
                    dist = src.at< unsigned short >(j, i);
        }
    }
    return dist;
}

均一化，只要物体与摄像头距离信息大于某个门限，即看作有效值，统一赋值：

static void oz_trans(Mat& src, Mat& dst, int thd, int maxv, int wid, int hgh)
{

    for (int j = 0; j < hgh; ++j) {
        for (int i = 0; i < wid; ++i) {
            if (src.at< unsigned short >(j, i)  >= thd)
                dst.at< unsigned short >(j, i) = maxv;
            else
                dst.at< unsigned short >(j, i) = 0;
            if (j == 90 && i == 120) {
                printf("center oz mm : %d\n", dst.at< unsigned short >(j, i));
            }
        }
    }
}

将摄像头的FOV 转为用距离信息的表示：

static void get_areaOV(unsigned int distance, float* widOV, float* hghOV, float tanH, float tanV, float senW, float senH)
{
    *widOV = (float)distance * tanH + senW;
    *widOV = *widOV * (float)2.0;//(float)3.5;
    *hghOV = (float)distance * tanV + senH;
    *hghOV = *hghOV * (float)2.0;//(float)3.5;
}

求解在物体方形区域里与摄像头最近的点的深度信息：

static void calc_shape(Mat& src, int dist, int x, int y, int wid, int hgh, int* outl, int* outw, unsigned short *outh)
{

    for (int j = y; j < (hgh+y); ++j) {
        for (int i = x; i < (wid+x); ++i) {
            if (src.at< unsigned short >(j, i) < *outh && src.at< unsigned short >(j, i) < (dist-10))
                *outh = src.at< unsigned short >(j, i);
        }
    }

}

MAT转QIMAGE：

static
QImage MatToQImage(const cv::Mat& mat)

{

    // 8-bits unsigned, NO. OF CHANNELS = 1

    if(mat.type() == CV_8UC1)

    {

        QImage image(mat.cols, mat.rows, QImage::Format_Indexed8);

        // Set the color table (used to translate colour indexes to qRgb values)

        image.setColorCount(256);

        for(int i = 0; i < 256; i++)

        {

            image.setColor(i, qRgb(i, i, i));

        }

        // Copy input Mat

        uchar *pSrc = mat.data;

        for(int row = 0; row < mat.rows; row ++)

        {

            uchar *pDest = image.scanLine(row);

            memcpy(pDest, pSrc, mat.cols);

            pSrc += mat.step;

        }

        return image;

    }

    // 8-bits unsigned, NO. OF CHANNELS = 3

    else if(mat.type() == CV_8UC3)

    {

        // Copy input Mat

        const uchar *pSrc = (const uchar*)mat.data;

        // Create QImage with same dimensions as input Mat

        QImage image(pSrc, mat.cols, mat.rows, mat.step, QImage::Format_RGB888);

        return image.rgbSwapped();

    }

    else if(mat.type() == CV_8UC4)

    {

        qDebug() << "CV_8UC4";

        // Copy input Mat

        const uchar *pSrc = (const uchar*)mat.data;

        // Create QImage with same dimensions as input Mat

        QImage image(pSrc, mat.cols, mat.rows, mat.step, QImage::Format_ARGB32);

        return image.copy();

    }

    else

    {

        qDebug() << "ERROR: Mat could not be converted to QImage.";

        return QImage();

    }

}

计算长宽信息：注意这里的ratio，依据物体深度信息算出的长宽尺寸只是相对于背景地面高度上算出来的，但是实际应相对于物体上表面对应的FOV范围，所以需要用物体上表面深度与背景深度算一个ratio来算出最终准确的物体尺寸：

static void calc_xy(float* xcloud, float* ycloud, Point2f* rect, int* len, int* wid, int hght, int distance)
{
    int tmp = 0;
    float x0 = 0.0;
    float y0 = 0.0;
    float x1 = 0.0;
    float y1 = 0.0;
    float ratio = 0.0;
    ratio = (float) (((float)distance -(float) hght) / (float)distance);
    // number 240 below means horizontal pixel number of the sensor.
    x0 = xcloud[(int)rect[1].y*240 + (int)rect[1].x]*(float)1000.0f;
    y0 = ycloud[(int)rect[1].y*240 + (int)rect[1].x]*(float)1000.0f;
    x1 = xcloud[(int)rect[2].y*240 + (int)rect[2].x]*(float)1000.0f;
    y1 = ycloud[(int)rect[2].y*240 + (int)rect[2].x]*(float)1000.0f;
    *wid = (int)(sqrt((x1 - x0)*(x1 - x0) + (y1 - y0)*(y1 - y0))* ratio);
    x0 = xcloud[(int)rect[3].y*240 + (int)rect[3].x]*(float)1000.0f;
    y0 = ycloud[(int)rect[3].y*240 + (int)rect[3].x]*(float)1000.0f;
    *len = (int)(sqrt((x1 - x0)*(x1 - x0) + (y1 - y0)*(y1 - y0))* ratio);
    if(*wid > *len)
    {
        tmp = *wid;
        *wid = *len;
        *len = tmp;
    }
}

整个flow：

void run(float *pcloud)
{
    int index = 320;
    int width = 240;//width height是摄像头的长宽分辨率
    bool filled = false;//背景信息是否填充标志

    while (1){
        TOF_GET_DEPTH_POINTCLOUD(pcloud, width, height);
        transfor_frame(pcloud, colorFrame, width, height);


        img_blur(colorFrame, blurFrame, 5);
        //blurFrame = colorFrame.clone();

#if 1

        if (!filled && /*cnt++ == SKIP_NUM*/start_shape)//make sure ever captured the ground frame first.if not we can not get diff frame from new tof image.
        {
            groundFrame = blurFrame.clone();//store the ground frame.
            distance = get_dist(groundFrame, width, height);
            get_areaOV(distance, &widFOV, &hghFOV, tanHFOV, tanVFOV, sensorH, sensorV);
            filled = true;
            continue;
        }
        if (filled && start_shape)//if we have ground frame already, then we can do the calculate of shape.
        {
            diff(groundFrame, blurFrame, diffFrame, width, height);

            //diffFrame.convertTo(diff8Frame, CV_8UC1, 0.00390625);
            //threshold(diff8Frame, ozFrame, 10 * 0.00390625, 255, THRESH_BINARY/*| THRESH_OTSU*/);
            oz_trans(diffFrame, ozFrame, 20 , 65535, width, height);
            ozFrame.convertTo(diff8Frame, CV_8UC1, 0.00390625);
            findContours(diff8Frame, contours, hierarcy, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);//find the boxes.
            std::cout << "num=" << contours.size() << std::endl;//number of boxes.
            vector<Rect> boundRect(contours.size());  //定义外接矩形集合
            vector<RotatedRect> box(contours.size()); //定义最小外接矩形集合
            Point2f rect[4];
            shapeImg.release();
            shapeImg = Mat::zeros(height, width, CV_8UC3);
            shapes.clear();//make sure shapeImg is clean before new calculate.
            if(isCapture && capshowing)//if mouse ever left clicked and the clicked object's box showed, should not update boxes and their sizes.
            {
                continue;
            }
            for (unsigned int i = 0; i < contours.size(); i++)//calculate all boxes' size and show them.
            {
                box[i] = minAreaRect(Mat(contours[i]));  //计算每个轮廓最小外接矩形
                boundRect[i] = boundingRect(Mat(contours[i]));
                cout << box[i].angle << endl;
                cout << box[i].center << endl;
                cout << box[i].size.width << endl;
                cout << box[i].size.height << endl;
                if(boundRect[i].width < WIDTH_THRESHHOLD || boundRect[i].height < HEIGHT_THRESHHOLD)
                    continue;
                index++;
                if((isCapture && (mx < ((int)box[i].center.x + (int)box[i].size.width/2)) && (mx > ((int)box[i].center.x - (int)box[i].size.width/2))
                        && (my < ((int)box[i].center.y + (int)box[i].size.height/2)) && (my > ((int)box[i].center.y - (int)box[i].size.height/2))) ||
                        !isCapture)
                {
                    if(isCapture && !capshowing)//mouse left clicked, but box is still not show, should show the box and set box to showed status.
                    {
                        capshowing = true;
                    }
                    else if(!isCapture)//is mouse left clicked again, back to continously showing mode.
                    {
                        capshowing = false;
                    }
                    circle(shapeImg, Point(box[i].center.x, box[i].center.y), 5, Scalar(0, 255, 0), -1, 8);  //绘制最小外接矩形的中心点

                    box[i].points(rect);  //把最小外接矩形四个端点复制给rect数组
                    printf("to retangle\n");
                    rectangle(shapeImg, Point(boundRect[i].x, boundRect[i].y), Point(boundRect[i].x + boundRect[i].width, boundRect[i].y + boundRect[i].height), Scalar(0, 255, 0), 2, 8);
                    for (int j = 0; j < 4; j++)
                    {
                        line(shapeImg, rect[j], rect[(j + 1) % 4], Scalar(0, 0, 255), 2, 8);  //绘制最小外接矩形每条边
                    }
                    int len = 0;
                    int wid = 0;
                    unsigned short  hght = 0;
                    printf("to calc_shape, distance:%d, x:%d, y:%d, w:%d, h:%d\n", distance, boundRect[i].x, boundRect[i].y, boundRect[i].width, boundRect[i].height);
                    calc_shape(diffFrame, distance, boundRect[i].x, boundRect[i].y, boundRect[i].width, boundRect[i].height, &len, &wid, &hght);
                    calc_xy(xcloud, ycloud, rect,&len, &wid, hght, distance);

                    shpinfo.index = index;
                    shpinfo.len = len;//(int)(((float)box[i].size.width*(widFOV - (float) hght* tanHFOV*(float)2.0))/(float)width);
                    shpinfo.width =wid;//(int)(((float)box[i].size.height*(hghFOV - (float) hght* tanVFOV*(float)2.0))/(float)height);
                    shpinfo.height = hght;
                    if(shpinfo.len < shpinfo.width)
                    {
                        wid = shpinfo.len;
                        shpinfo.len = shpinfo.width;
                        shpinfo.width = wid;
                    }
                    shapes.push_back(shpinfo);
                    //if (box[i].size.width > WIDTH_THRESHHOLD && box[i].size.height > HEIGHT_THRESHHOLD)
                    {
                        String name = "box-" + to_string( index) + ", " + to_string((int)box[i].center.x) +", " + to_string((int)box[i].center.y) ;
                        //String sizee = to_string(width) +", "+to_string(height) ;
                        //String wh = to_string((int)box[i].size.width) + ", "+to_string((int)box[i].size.height);

                        putText(shapeImg, name, Point(box[i].center.x, box[i].center.y + 15), CV_FONT_HERSHEY_PLAIN, 0.65, Scalar(0, 255, 0), 1, 8);
                        //putText(shapeImg, sizee, Point(box[i].center.x, box[i].center.y + 35), CV_FONT_HERSHEY_PLAIN, 0.65, Scalar(0, 255, 0), 1, 8);
                        //putText(shapeImg, wh, Point(box[i].center.x, box[i].center.y + 55), CV_FONT_HERSHEY_PLAIN, 0.65, Scalar(0, 255, 0), 1, 8);
                        //putText(shapeImg, objwid, Point(box[i].center.x, box[i].center.y + 30), CV_FONT_HERSHEY_COMPLEX_SMALL, 0.65, Scalar(0, 255, 0), 2, 8);
                        //putText(shapeImg, objhght, Point(box[i].center.x, box[i].center.y + 45), CV_FONT_HERSHEY_COMPLEX_SMALL, 0.65, Scalar(0, 255, 0), 2, 8);
                    }
                }
            }
            index = 0;
            emit sendShape();
            emit sendShapeImage(MatToQImage(shapeImg));
        }
}
Sleep(frameRateTime);}