kinect环境获取mkv视频中的点云数据,放到pcl点云数组中,实现的类

文章链接:https://codemouse.online/archives/2020-05-16225206

kinectRecord.h

#pragma once

#include <k4a/k4a.h>
#include <k4arecord/playback.h>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/features/integral_image_normal.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>
#include <pcl/filters/filter.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/parameter/parameters.hpp>
#include <stdio.h>
#include <stdlib.h>
#include <k4a/k4a.h>
#include <k4arecord/playback.h>


#include <stdio.h>
#include <malloc.h>
#include <iostream>
#include <vector>
#include<algorithm>
#include<cmath>
#include <time.h>
#include <string>


using namespace std;

class kinectRecord
{
public:

	kinectRecord();

	int getPointsCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr &point_cloud_ptr); // 获取点云
	int setRecordTime(long long offset); // 设置视频帧位置
	int initRecord(string filename); // 初始化视频
private:
	int addPointsCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr &point_cloud_ptr, k4a_image_t point_cloud_image, k4a_image_t color_image);
	k4a_image_t getPointsCloudImage(k4a_image_t color_image, k4a_image_t depth_image);

private:
	string filename; // 文件名
	k4a_playback_t handle; // 视频句柄
	k4a_record_configuration_t record_config; // 视频配置
	k4a_capture_t capture; // 获取到的视频帧
	k4a_transformation_t transformation_handle; // 点云转换的东西
};

kinectRecord.cpp

#include "kinectRecord.h"

kinectRecord::kinectRecord() {
	transformation_handle = NULL;
	handle = NULL;
	capture = NULL;
	
}

int kinectRecord::initRecord(string filename) {
	if (filename.size() == 0)
	{
		printf("请不要输入一个空的文件名,重新初始化\n");
		return -1;
	}
	this->filename = filename;
	k4a_result_t result = K4A_RESULT_SUCCEEDED;
	// 打开视频
	result = k4a_playback_open(filename.c_str(), &handle);
	if (result != K4A_RESULT_SUCCEEDED)
	{
		printf("Failed to open file: %s\n", filename.c_str());
		return -2;
	}
	// 获取视频的配置信息
	result = k4a_playback_get_record_configuration(handle, &record_config);
	if (result != K4A_RESULT_SUCCEEDED)
	{
		printf("Failed to get record configuration for file: %s\n", filename.c_str());
		return -3;
	}
	printf("startOffset:%d  suborDelay:%d \n", record_config.start_timestamp_offset_usec, record_config.subordinate_delay_off_master_usec);

	k4a_calibration_t calibration;
	k4a_playback_get_calibration(handle, &calibration);
	// 用来转换3d点云的玩意,一定要弄
	transformation_handle = k4a_transformation_create(&calibration);
	return 1;
}


int kinectRecord::addPointsCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr &point_cloud_ptr, k4a_image_t point_cloud_image, k4a_image_t color_image)
{
	if (point_cloud_image == NULL || color_image == NULL)
	{
		printf("point_cloud_image or color image not have\n");
		return -6;
	}
	int width = k4a_image_get_width_pixels(color_image);
	int height = k4a_image_get_height_pixels(color_image);


	int16_t *point_cloud_image_data = (int16_t *)(void *)k4a_image_get_buffer(point_cloud_image);
	uint8_t *color_image_data = k4a_image_get_buffer(color_image);


	for (int i = 0; i < width * height; i++)
	{

		pcl::PointXYZRGB point;
		uint32_t rgb = 0;

		point.x = -point_cloud_image_data[3 * i + 0];
		point.y = -point_cloud_image_data[3 * i + 1];
		point.z = -point_cloud_image_data[3 * i + 2];
		// 这里设置背景深度
		if (point.z == 0 || point.z < -3000)
		{
			continue;
		}
		// 获取到颜色信息,颜色格式:bgra 
		//rgb |= ((color_image_data[4 * i + 0]) << 0);
		//rgb |= ((color_image_data[4 * i + 1]) << 8);
		//rgb |= ((color_image_data[4 * i + 2]) << 16);
		//rgb |= ((color_image_data[4 * i + 3]) << 24);
		// 获取到的彩色图片不知道怎么回事,这里设置颜色就行了
		rgb = 0;


		//cout << static_cast<uint32_t>(color_image_data[4 * i + 3]) << endl;
		//uint8_t alpha = color_image_data[4 * i + 3];
		//&& point.x <= -1000 && point.x >= 1000 && point.y <= -1000 && point.y >= 1000)
		//if (point.x == 0 && point.y == 0 && point.y == 0 && alpha == 0)
		//{
		//	continue;
		//}
		if (point.x == 0 && point.y == 0 && point.y == 0)
		{
			continue;
		}
		point.rgb = rgb;
		point_cloud_ptr->points.push_back(point);
	}
	return 1;

}



k4a_image_t kinectRecord::getPointsCloudImage(k4a_image_t color_image, k4a_image_t depth_image)
{

	int color_image_width_pixels = k4a_image_get_width_pixels(color_image);
	int color_image_height_pixels = k4a_image_get_height_pixels(color_image);
		

	k4a_image_t transformed_depth_image = NULL;
	// 创建点云深度图
	if (K4A_RESULT_SUCCEEDED != k4a_image_create(K4A_IMAGE_FORMAT_DEPTH16,
		color_image_width_pixels,
		color_image_height_pixels,
		color_image_width_pixels  * (int)sizeof(uint16_t),
		&transformed_depth_image))
	{
		cout << "Failed to create transformed depth image" << endl;
		return NULL;
	}


	k4a_image_t point_cloud_image = NULL;
	// 创建点云图片
	if (K4A_RESULT_SUCCEEDED != k4a_image_create(K4A_IMAGE_FORMAT_CUSTOM,
		color_image_width_pixels,
		color_image_height_pixels,
		color_image_width_pixels * 3 * (int)sizeof(int16_t),
		&point_cloud_image))
	{
		cout << "Failed to create point cloud image" << endl;
		return NULL;
	}
	

	// 将深度图转换到彩色图中
	if (K4A_RESULT_SUCCEEDED !=
		k4a_transformation_depth_image_to_color_camera(transformation_handle, depth_image, transformed_depth_image))
	{
		cout << "Failed to compute transformed depth image" << endl;
		return NULL;
	}


	// 初始化点云图片
	if (K4A_RESULT_SUCCEEDED != k4a_transformation_depth_image_to_point_cloud(transformation_handle,
		transformed_depth_image,
		K4A_CALIBRATION_TYPE_COLOR,
		point_cloud_image))
	{
		cout << "Failed to compute point cloud" << endl;
		return NULL;
	}
	k4a_image_release(transformed_depth_image);
	return point_cloud_image;
}


int kinectRecord::getPointsCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr &point_cloud_ptr)
{
	if (handle == NULL)
		return -7;
	k4a_image_t depth_image = NULL;
	k4a_image_t color_image = NULL;
	k4a_result_t result = K4A_RESULT_SUCCEEDED;
	// 获取capture
	k4a_stream_result_t stream_result = k4a_playback_get_next_capture(handle, &capture);
	if (stream_result == K4A_STREAM_RESULT_EOF)
	{
		printf("ERROR: Recording file is not next capture: %s\n", filename.c_str());
		return -1;
	}

	// 获取深度图
	depth_image = k4a_capture_get_depth_image(capture);
	if (depth_image == NULL)
	{
		cout << "Failed to get depth image from capture" << endl;;
		return -3;
	}

	// 获取彩图
	color_image = k4a_capture_get_color_image(capture);
	if (color_image == NULL)
	{
		k4a_image_release(depth_image);
		cout << "Failed to get color image from capture" << endl;
		return -4;
	}
	
	// 得到点云图片
	k4a_image_t point_cloud_image = NULL;
	point_cloud_image = getPointsCloudImage(color_image, depth_image);

	if (point_cloud_image == NULL)
	{
		k4a_image_release(depth_image);
		k4a_image_release(color_image);
		cout << "Failed to get point_cloud_image from capture" << endl;
		return -4;
	}

	//uint64_t timestamp = k4a_image_get_device_timestamp_usec(color_image);
	//printf("image:  %7ju usec\n", timestamp);
	// 将点云图的点云信息加入到点云结构体中
	addPointsCloud(point_cloud_ptr, point_cloud_image, color_image);

	// 清理图片缓存

	k4a_image_release(point_cloud_image);
	k4a_image_release(depth_image);
	k4a_image_release(color_image);
	// 释放capure
	k4a_capture_release(capture);
	return 1;
}

// 设置视频的时间位置,相当于进度条,1000000是一秒
int kinectRecord::setRecordTime(long long offset) {
	if (k4a_playback_seek_timestamp(handle,
			record_config.start_timestamp_offset_usec,
			K4A_PLAYBACK_SEEK_BEGIN) != K4A_RESULT_SUCCEEDED) {
		return -1; 
	}
	return 1;

}

demo

#include "kinectRecord.h"

//pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_ptr;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_ptr(new pcl::PointCloud<pcl::PointXYZRGB>);

int main(int argc, char **argv)
{
	if (argc < 3)
	{
		printf("Usage: playback_external_sync.exe <master.mkv> <sub1.mkv>...\n");
		return 1;
	}

	// 输入视频名字
	kinectRecord record1,record2,record3;


	if (record1.initRecord(argv[1]) < 0)
	{
		printf("初始化失败!请检查是否有该文件\n");
		return 1;
	}
	else
	// 设置视频的帧位置
	//record1.setRecordTime(1500000);
	// 获取点云
	for (int i = 0; i < 100; i++)
	{
		if (record1.getPointsCloud(point_cloud_ptr) < 0) {
			printf("没有得到任何的点\n");
			// 重新设置到视频头
			record1.setRecordTime(0);
		}

		printf("size() : %d \n", point_cloud_ptr->points.size());
		point_cloud_ptr->clear();
	}
	


}