#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, который может быть пустым или недействительным. Вы проверили, являются ли точки в облаке, которые вы берете, нормальными?
Однако вы можете подойти к проблеме по-другому:
- Оставьте цикл визуализации как есть.
- Зарегистрируйте обработчик ключей с помощью
registerKeyboardCallback(doc). - Когда нажата определенная клавиша, установите логическое значение true .
- В цикле визуализации, если эта логическая переменная имеет значение true, пропустите захват кадра из Kinect. Он должен сохранить ранее установленное облако.