SLAM初探（四）

Mat image= video->getVideo();//此操作直接获取摄像头的图像矩阵
IplImage img = IplImage(image);
GdkPixbuf  pixbuf=img.data;
GtkWidget *gtk_image_new_from_pixbuf(GdkPixbuf *pixbuf );

#include <pxcsensemanager.h>
#include <pxcsession.h>
#include "util_render.h"
#include <iostream>
#include <string>
#include <stdio.h>
#include <opencv2\opencv.hpp>
using namespace cv;
using namespace std;
int main()
{
    //初始化图像流
    UtilRender *renderColor = new UtilRender(L"COLOR_STREAM");
    //初始化深度数据流
    UtilRender *renderDepth = new UtilRender(L"DEPTH_STREAM");
    //打开摄像头
    PXCSenseManager *psm = PXCSenseManager::CreateInstance();
    //如果摄像头打开失败，则报错
    if (!psm)
    {
        wprintf_s(L"Unabel to create the PXCSenseManager\n");
        return 1;
    }

    //初始化接收图像数据流和深度数据流
    psm->EnableStream(PXCCapture::STREAM_TYPE_COLOR);
    psm->EnableStream(PXCCapture::STREAM_TYPE_DEPTH);
    //确认初始化成功
    if (psm->Init() != PXC_STATUS_NO_ERROR)
    {
        wprintf_s(L"Unable to Init the PXCSenseManager\n");
        return 2;
    }
    //初始化接收的数据
    PXCImage *colorIm, *depthIm;
    PXCImage::ImageData depth_data;
    PXCImage::ImageData color_data;
    PXCImage::ImageInfo depth_information;
    PXCImage::ImageInfo color_information;

    Mat img = Mat(480, 640, CV_8UC1);

    while (waitKey(1))
    {
        if (psm->AcquireFrame(true) < PXC_STATUS_NO_ERROR) break;

        PXCCapture::Sample *sample = psm->QuerySample();

        colorIm = sample->color;
        depthIm = sample->depth;

        colorIm->AcquireAccess(PXCImage::ACCESS_READ,PXCImage :: PIXEL_FORMAT_RGB24, &color_data);
        depthIm->AcquireAccess(PXCImage::ACCESS_READ, &depth_data);
        depth_information = sample->depth->QueryInfo();
        color_information = sample->color->QueryInfo();

        ushort *dpixels = (ushort*)depth_data.planes[0];
        for (int y = 0; y < depth_information.height; y++)
            for (int x = 0; x < depth_information.width; x++)
            {
                uchar d = dpixels[y * depth_information.width + x];
                img.at<uchar>(y, x) = min((int)((double)d / 600 * 255), 255);
            }
        Mat color_img = Mat(color_information.height, color_information.width, CV_8UC3);
        uchar * dp = (uchar*)color_data.planes[0];
        int cnt = 0;
        for (int y = 0; y < color_information.height; y++)
            for (int x = 0; x < color_information.width; x++)
            {
                color_img.at<Vec3b>(y, x)[0] = dp[cnt++];
                color_img.at<Vec3b>(y, x)[1] = dp[cnt++];
                color_img.at<Vec3b>(y, x)[2] = dp[cnt++];
            }
        namedWindow("a", 0);
        imshow("a", color_img);

        //imwrite("100.jpg", img);
        //depthIm->ReleaseAccess(&depth_data);
        //colorIm->ReleaseAccess(&color_data);

        //if (!renderColor->RenderFrame(sample->color)) break;
        //if (!renderDepth->RenderFrame(sample->depth)) break;

        psm->ReleaseFrame();
        cv::imshow("ok", img);
        /*Mat cur;
        medianBlur(img, cur, 5);
        cv::imshow("ok1", cur);*/
    }
    psm->Release();
}