Блог

Главная - Вопросы по программированию - Как я могу захватить один кадр, используя библиотеку облака точек?

Как я могу захватить один кадр, используя библиотеку облака точек?

#c #kinect #point-cloud-library

#c #kinect #point-cloud-library

Вопрос:

         // Sample_PCL.cpp : Defines the entry point for the console application.
    //

    #include "stdafx.h"
    #define NOMINMAX
    #include <Windows.h>
    #include <Kinect.h>                                 // Kinectを使用するためのヘッダファイル
    #include <pcl/visualization/cloud_viewer.h>         // PCLを使用して表示するためのヘッダファイル
    #include <pcl/io/pcd_io.h>                          // 点群データを保存するためのヘッダファイル(.pcd, .ply)
    //#include <pcl/io/ply_io.h>
    #include <pcl/point_types.h>
    #include <iostream>

    template<class Interface>
    inline void SafeRelease(Interface *amp; pInterfaceToRelease)
    {
        if (pInterfaceToRelease != NULL) {
            pInterfaceToRelease->Release();
        }
    }

    int main()
    {
        // Create Sensor Instance
        IKinectSensor* pSensor;
        HRESULT hResult = S_OK;
        hResult = GetDefaultKinectSensor(amp;pSensor);
        if (FAILED(hResult)) {
            std::cerr << "Error : GetDefaultKinectSensor" << std::endl;
            return -1;
        }
        printf("GetDfaultKinectSensor is OKn");

        // Open Sensor
        hResult = pSensor->Open();
        if (FAILED(hResult)) {
            std::cerr << "Error : IKinectSensor::Open()" << std::endl;
            return -1;
        }
        printf("IKinectSensor::Open is OKn");

        // Retrieved Coordinate Mapper
        ICoordinateMapper* pCoordinateMapper;
        hResult = pSensor->get_CoordinateMapper(amp;pCoordinateMapper);
        if (FAILED(hResult)) {
            std::cerr << "Error : IKinectSensor::get_CoordinateMapper()" << std::endl;
            return -1;
        }
        printf("IKinectSensor::get_CoordinateMapper is OKn");


        // Retrieved Color Frame Source
        IColorFrameSource* pColorSource;
        hResult = pSensor->get_ColorFrameSource(amp;pColorSource);
        if (FAILED(hResult)) {
            std::cerr << "Error : IKinectSensor::get_ColorFrameSource()" << std::endl;
            return -1;
        }
        printf("IKinectSensor::get_ColorFrameSource is OKn");

        // Open Color Frame Reader
        IColorFrameReader* pColorReader;
        hResult = pColorSource->OpenReader(amp;pColorReader);
        if (FAILED(hResult)) {
            std::cerr << "Error : IColorFrameSource::OpenReader()" << std::endl;
            return -1;
        }
        printf("IColorFrameSource::OpenReader is OKn");

        // Retrieved Depth Frame Source
        IDepthFrameSource* pDepthSource;
        hResult = pSensor->get_DepthFrameSource(amp;pDepthSource);
        if (FAILED(hResult)) {
            std::cerr << "Error : IKinectSensor::get_DepthFrameSource()" << std::endl;
            return -1;
        }
        printf("IKinectSensor::get_DepthFrameSource is OKn");

        // Open Depth Frame Reader
        IDepthFrameReader* pDepthReader;
        hResult = pDepthSource->OpenReader(amp;pDepthReader);
        if (FAILED(hResult)) {
            std::cerr << "Error : IDepthFrameSource::OpenReader()" << std::endl;
            return -1;
        }
        printf("IDepthFrameSource::OpenReader is OKn");

        // Retrieved Color Frame Size
        IFrameDescription* pColorDescription;
        hResult = pColorSource->get_FrameDescription(amp;pColorDescription);
        if (FAILED(hResult)) {
            std::cerr << "Error : IColorFrameSource::get_FrameDescription()" << std::endl;
            return -1;
        }
        printf("IColorFrameSource::get_FrameDescription is OKn");

        int colorWidth = 0;
        int colorHeight = 0;
        pColorDescription->get_Width(amp;colorWidth); // 1920
        pColorDescription->get_Height(amp;colorHeight); // 1080

                                                     // To Reserve Color Frame Buffer
        std::vector<RGBQUAD> colorBuffer(colorWidth * colorHeight);

        // Retrieved Depth Frame Size
        IFrameDescription* pDepthDescription;
        hResult = pDepthSource->get_FrameDescription(amp;pDepthDescription);
        if (FAILED(hResult)) {
            std::cerr << "Error : IDepthFrameSource::get_FrameDescription()" << std::endl;
            return -1;
        }
        printf("IDepthFrameSource::get_FrameDescription is OKn");

        int depthWidth = 0;
        int depthHeight = 0;
        pDepthDescription->get_Width(amp;depthWidth); // 512
        pDepthDescription->get_Height(amp;depthHeight); // 424

                                                     // To Reserve Depth Frame Buffer
        std::vector<UINT16> depthBuffer(depthWidth * depthHeight);

        printf("Display Point Cloudn");

        // Create Cloud Viewer
        pcl::visualization::CloudViewer viewer("Point Cloud Viewer");       //  点群のウィンドウ表示

        while (!viewer.wasStopped()) {
            // Acquire Latest Color Frame
            IColorFrame* pColorFrame = nullptr;
            hResult = pColorReader->AcquireLatestFrame(amp;pColorFrame);
            if (SUCCEEDED(hResult)) {
                // Retrieved Color Data
                hResult = pColorFrame->CopyConvertedFrameDataToArray(colorBuffer.size() * sizeof(RGBQUAD), reinterpret_cast<BYTE*>(amp;colorBuffer[0]), ColorImageFormat::ColorImageFormat_Bgra);
                if (FAILED(hResult)) {
                    std::cerr << "Error : IColorFrame::CopyConvertedFrameDataToArray()" << std::endl;
                }
            }
            SafeRelease(pColorFrame);

            // Acquire Latest Depth Frame
            IDepthFrame* pDepthFrame = nullptr;
            hResult = pDepthReader->AcquireLatestFrame(amp;pDepthFrame);
            if (SUCCEEDED(hResult)) {
                // Retrieved Depth Data
                hResult = pDepthFrame->CopyFrameDataToArray(depthBuffer.size(), amp;depthBuffer[0]);
                if (FAILED(hResult)) {
                    std::cerr << "Error : IDepthFrame::CopyFrameDataToArray()" << std::endl;
                }
            }
            SafeRelease(pDepthFrame);

            // Point Cloud Libraryの設定
            // Create Point Cloud
            pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointcloud(new pcl::PointCloud<pcl::PointXYZRGB>());     // PCLの構造体
            pointcloud->width = static_cast<uint32_t>(depthWidth);      // 点群の数
            pointcloud->height = static_cast<uint32_t>(depthHeight);
            pointcloud->is_dense = false;

            for (int y = 0; y < depthHeight; y  ) {
                for (int x = 0; x < depthWidth; x  ) {
                    pcl::PointXYZRGB point;     // PCLで使用する点群情報

                    DepthSpacePoint depthSpacePoint = { static_cast<float>(x), static_cast<float>(y) };
                    UINT16 depth = depthBuffer[y * depthWidth   x];

                    // Coordinate Mapping Depth to Color Space, and Setting PointCloud RGB
                    ColorSpacePoint colorSpacePoint = { 0.0f, 0.0f };
                    pCoordinateMapper->MapDepthPointToColorSpace(depthSpacePoint, depth, amp;colorSpacePoint);     //  色の座標系
                    int colorX = static_cast<int>(std::floor(colorSpacePoint.X   0.5f));
                    int colorY = static_cast<int>(std::floor(colorSpacePoint.Y   0.5f));
                    if ((0 <= colorX) amp;amp; (colorX < colorWidth) amp;amp; (0 <= colorY) amp;amp; (colorY < colorHeight)) {
                        RGBQUAD color = colorBuffer[colorY * colorWidth   colorX];
                        // 色の情報を格納する
                        point.b = color.rgbBlue;
                        point.g = color.rgbGreen;
                        point.r = color.rgbRed;
                    }

                    // Coordinate Mapping Depth to Camera Space, and Setting PointCloud XYZ
                    CameraSpacePoint cameraSpacePoint = { 0.0f, 0.0f, 0.0f };           // カメラ空間
                    pCoordinateMapper->MapDepthPointToCameraSpace(depthSpacePoint, depth, amp;cameraSpacePoint);
                    if ((0 <= colorX) amp;amp; (colorX < colorWidth) amp;amp; (0 <= colorY) amp;amp; (colorY < colorHeight)) {
                        // 直交座標系の情報を格納する
                        point.x = cameraSpacePoint.X;
                        point.y = cameraSpacePoint.Y;
                        point.z = cameraSpacePoint.Z;
                    }

                    pointcloud->push_back(point);
                }
            }

            // Show Point Cloud on Cloud Viewer
            viewer.showCloud(pointcloud);

            // Input Key ( Exit ESC key )
            if (GetKeyState(VK_ESCAPE) < 0) {
                pcl::io::savePCDFile("PointCloud.pcd", *pointcloud);
                //pcl::io::savePLYFile("test_pcd2.ply", *pointcloud);           // 最後に取得した点群を保存
                printf("Save Point Cloud Datan");

                //break;
            }
        }

        // End Processing
        SafeRelease(pColorSource);
        SafeRelease(pDepthSource);
        SafeRelease(pColorReader);
        SafeRelease(pDepthReader);
        SafeRelease(pColorDescription);
        SafeRelease(pDepthDescription);
        SafeRelease(pCoordinateMapper);
        if (pSensor) {
            pSensor->Close();
        }
        SafeRelease(pSensor);

        printf("Disconnect Kinect Sensorn");



        return 0;
    }

Предыдущий код — это код, взятый из руководства с веб-сайта библиотеки облаков точек, использующий Kinect для отображения облака точек того, что Kinect видит в режиме реального времени. Поэтому облако точек постоянно меняется. Вот почему я хотел бы получить только кадр, другими словами, я хотел бы, чтобы облако точек зависало вместо постоянного захвата новых кадров.

и вот моя модификация.

         // Sample_PCL.cpp : Defines the entry point for the console application.
    //

    #include "stdafx.h"
    #define NOMINMAX
    #include <Windows.h>
    #include <Kinect.h>                                 // Kinectを使用するためのヘッダファイル
    #include <pcl/visualization/cloud_viewer.h>         // PCLを使用して表示するためのヘッダファイル
    #include <pcl/io/pcd_io.h>                          // 点群データを保存するためのヘッダファイル(.pcd, .ply)
    //#include <pcl/io/ply_io.h>
    #include <pcl/point_types.h>
    #include <iostream>

    template<class Interface>
    inline void SafeRelease(Interface *amp; pInterfaceToRelease)
    {
        if (pInterfaceToRelease != NULL) {
            pInterfaceToRelease->Release();
        }
    }

    int main()
    {
        // Create Sensor Instance
        IKinectSensor* pSensor;
        HRESULT hResult = S_OK;
        hResult = GetDefaultKinectSensor(amp;pSensor);
        if (FAILED(hResult)) {
            std::cerr << "Error : GetDefaultKinectSensor" << std::endl;
            return -1;
        }
        printf("GetDfaultKinectSensor is OKn");

        // Open Sensor
        hResult = pSensor->Open();
        if (FAILED(hResult)) {
            std::cerr << "Error : IKinectSensor::Open()" << std::endl;
            return -1;
        }
        printf("IKinectSensor::Open is OKn");

        // Retrieved Coordinate Mapper
        ICoordinateMapper* pCoordinateMapper;
        hResult = pSensor->get_CoordinateMapper(amp;pCoordinateMapper);
        if (FAILED(hResult)) {
            std::cerr << "Error : IKinectSensor::get_CoordinateMapper()" << std::endl;
            return -1;
        }
        printf("IKinectSensor::get_CoordinateMapper is OKn");


        // Retrieved Color Frame Source
        IColorFrameSource* pColorSource;
        hResult = pSensor->get_ColorFrameSource(amp;pColorSource);
        if (FAILED(hResult)) {
            std::cerr << "Error : IKinectSensor::get_ColorFrameSource()" << std::endl;
            return -1;
        }
        printf("IKinectSensor::get_ColorFrameSource is OKn");

        // Open Color Frame Reader
        IColorFrameReader* pColorReader;
        hResult = pColorSource->OpenReader(amp;pColorReader);
        if (FAILED(hResult)) {
            std::cerr << "Error : IColorFrameSource::OpenReader()" << std::endl;
            return -1;
        }
        printf("IColorFrameSource::OpenReader is OKn");

        // Retrieved Depth Frame Source
        IDepthFrameSource* pDepthSource;
        hResult = pSensor->get_DepthFrameSource(amp;pDepthSource);
        if (FAILED(hResult)) {
            std::cerr << "Error : IKinectSensor::get_DepthFrameSource()" << std::endl;
            return -1;
        }
        printf("IKinectSensor::get_DepthFrameSource is OKn");

        // Open Depth Frame Reader
        IDepthFrameReader* pDepthReader;
        hResult = pDepthSource->OpenReader(amp;pDepthReader);
        if (FAILED(hResult)) {
            std::cerr << "Error : IDepthFrameSource::OpenReader()" << std::endl;
            return -1;
        }
        printf("IDepthFrameSource::OpenReader is OKn");

        // Retrieved Color Frame Size
        IFrameDescription* pColorDescription;
        hResult = pColorSource->get_FrameDescription(amp;pColorDescription);
        if (FAILED(hResult)) {
            std::cerr << "Error : IColorFrameSource::get_FrameDescription()" << std::endl;
            return -1;
        }
        printf("IColorFrameSource::get_FrameDescription is OKn");

        int colorWidth = 0;
        int colorHeight = 0;
        pColorDescription->get_Width(amp;colorWidth); // 1920
        pColorDescription->get_Height(amp;colorHeight); // 1080

                                                     // To Reserve Color Frame Buffer
        std::vector<RGBQUAD> colorBuffer(colorWidth * colorHeight);

        // Retrieved Depth Frame Size
        IFrameDescription* pDepthDescription;
        hResult = pDepthSource->get_FrameDescription(amp;pDepthDescription);
        if (FAILED(hResult)) {
            std::cerr << "Error : IDepthFrameSource::get_FrameDescription()" << std::endl;
            return -1;
        }
        printf("IDepthFrameSource::get_FrameDescription is OKn");

        int depthWidth = 0;
        int depthHeight = 0;
        pDepthDescription->get_Width(amp;depthWidth); // 512
        pDepthDescription->get_Height(amp;depthHeight); // 424

                                                     // To Reserve Depth Frame Buffer
        std::vector<UINT16> depthBuffer(depthWidth * depthHeight);

        printf("Display Point Cloudn");

        // Create Cloud Viewer
        pcl::visualization::CloudViewer viewer("Point Cloud Viewer");       //  点群のウィンドウ表示

        //while (!viewer.wasStopped()) {
            // Acquire Latest Color Frame
            IColorFrame* pColorFrame = nullptr;
            hResult = pColorReader->AcquireLatestFrame(amp;pColorFrame);
            if (SUCCEEDED(hResult)) {
                // Retrieved Color Data
                hResult = pColorFrame->CopyConvertedFrameDataToArray(colorBuffer.size() * sizeof(RGBQUAD), reinterpret_cast<BYTE*>(amp;colorBuffer[0]), ColorImageFormat::ColorImageFormat_Bgra);
                if (FAILED(hResult)) {
                    std::cerr << "Error : IColorFrame::CopyConvertedFrameDataToArray()" << std::endl;
                }
            }
            SafeRelease(pColorFrame);

            // Acquire Latest Depth Frame
            IDepthFrame* pDepthFrame = nullptr;
            hResult = pDepthReader->AcquireLatestFrame(amp;pDepthFrame);
            if (SUCCEEDED(hResult)) {
                // Retrieved Depth Data
                hResult = pDepthFrame->CopyFrameDataToArray(depthBuffer.size(), amp;depthBuffer[0]);
                if (FAILED(hResult)) {
                    std::cerr << "Error : IDepthFrame::CopyFrameDataToArray()" << std::endl;
                }
            }
            SafeRelease(pDepthFrame);



            // Point Cloud Libraryの設定
            // Create Point Cloud
            pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointcloud(new pcl::PointCloud<pcl::PointXYZRGB>());     // PCLの構造体
            pointcloud->width = static_cast<uint32_t>(depthWidth);      // 点群の数
            pointcloud->height = static_cast<uint32_t>(depthHeight);
            pointcloud->is_dense = false;

            for (int y = 0; y < depthHeight; y  ) {

                for (int x = 0; x < depthWidth; x  ) {
                    //printf("scannn");
                    pcl::PointXYZRGB point;     // PCLで使用する点群情報

                    DepthSpacePoint depthSpacePoint = { static_cast<float>(x), static_cast<float>(y) };
                    UINT16 depth = depthBuffer[y * depthWidth   x];

                    // Coordinate Mapping Depth to Color Space, and Setting PointCloud RGB
                    ColorSpacePoint colorSpacePoint = { 0.0f, 0.0f };
                    pCoordinateMapper->MapDepthPointToColorSpace(depthSpacePoint, depth, amp;colorSpacePoint);     //  色の座標系
                    int colorX = static_cast<int>(std::floor(colorSpacePoint.X   0.5f));
                    int colorY = static_cast<int>(std::floor(colorSpacePoint.Y   0.5f));
                    if ((0 <= colorX) amp;amp; (colorX < colorWidth) amp;amp; (0 <= colorY) amp;amp; (colorY < colorHeight)) {
                        RGBQUAD color = colorBuffer[colorY * colorWidth   colorX];
                        // 色の情報を格納する
                        point.b = color.rgbBlue;
                        point.g = color.rgbGreen;
                        point.r = color.rgbRed;
                    }

                    // Coordinate Mapping Depth to Camera Space, and Setting PointCloud XYZ
                    CameraSpacePoint cameraSpacePoint = { 0.0f, 0.0f, 0.0f };           // カメラ空間
                    pCoordinateMapper->MapDepthPointToCameraSpace(depthSpacePoint, depth, amp;cameraSpacePoint);
                    if ((0 <= colorX) amp;amp; (colorX < colorWidth) amp;amp; (0 <= colorY) amp;amp; (colorY < colorHeight)) {
                        // 直交座標系の情報を格納する
                        point.x = cameraSpacePoint.X;
                        point.y = cameraSpacePoint.Y;
                        point.z = cameraSpacePoint.Z;
                    }

                    pointcloud->push_back(point);
                }
            //}

            viewer.showCloud(pointcloud);
            while (!viewer.wasStopped())
            {
            }
            /*pcl::io::savePCDFile("PointCloud.pcd", *pointcloud);
            printf("Saved Point Cloud Datan");

            // Show Point Cloud on Cloud Viewer
            printf("Open viewern");
            viewer.showCloud(pointcloud);
            while (!viewer.wasStopped()) {

            }*/

            // Input Key ( Exit ESC key )
            if (GetKeyState(VK_ESCAPE) < 0) {
                pcl::io::savePCDFile("PointCloud.pcd", *pointcloud);
                //pcl::io::savePLYFile("test_pcd2.ply", *pointcloud);           // 最後に取得した点群を保存
                printf("Save Point Cloud Datan");

                //break;
            }
        }

        // End Processing
        SafeRelease(pColorSource);
        SafeRelease(pDepthSource);
        SafeRelease(pColorReader);
        SafeRelease(pDepthReader);
        SafeRelease(pColorDescription);
        SafeRelease(pDepthDescription);
        SafeRelease(pCoordinateMapper);
        if (pSensor) {
            pSensor->Close();
        }
        SafeRelease(pSensor);

        printf("Disconnect Kinect Sensorn");



        return 0;
    }

Модификация в основном состоит в удалении цикла, который постоянно обновляет облако точек, а именно: вы можете увидеть, как это прокомментировано во втором коде. 

 while (!viewer.wasStopped())

Проблема в том, что средство просмотра облака точек не отображает никаких данных, полученных Kinect, и я хотел бы знать причину, по которой его невозможно отобразить.

Комментарии:

1. Два списка идентичны.

2. Спасибо @Dexter за указание на это, я только что обновил код, не могли бы вы взглянуть на него еще раз?

Ответ №1:

Кажется, что ваш код показывает только самый первый кадр, который он получает от Kinect, который может быть пустым или недействительным. Вы проверили, являются ли точки в облаке, которые вы берете, нормальными?

Однако вы можете подойти к проблеме по-другому:

  1. Оставьте цикл визуализации как есть.
  2. Зарегистрируйте обработчик ключей с помощью registerKeyboardCallback (doc).
  3. Когда нажата определенная клавиша, установите логическое значение true .
  4. В цикле визуализации, если эта логическая переменная имеет значение true, пропустите захват кадра из Kinect. Он должен сохранить ранее установленное облако.