关于KINECT V2.0 C++ SDK 基础教程的笔记 EP3

最近忙着搞老师的任务，没来得及更新点云系列。
目前在做Kinect，在这里接着做个笔记。
原文地址： Kinect Tutorials
这仅仅是做一个笔记以及自己的实际操作记录

---

---

跟着网上的教程，学习Kinect c++ SDK。
⬇⬇⬇⬇⬇⬇⬇⬇⬇⬇⬇⬇⬇⬇⬇⬇⬇⬇⬇⬇⬇⬇⬇⬇⬇⬇⬇⬇⬇⬇⬇⬇⬇⬇⬇⬇⬇⬇⬇⬇⬇
➡➡➡➡➡<<< Kinect Tutorials >>>⬅⬅⬅⬅⬅⬅
⬆⬆⬆⬆⬆⬆⬆⬆⬆⬆⬆⬆⬆⬆⬆⬆⬆⬆⬆⬆⬆⬆⬆⬆⬆⬆⬆⬆⬆⬆⬆⬆⬆⬆⬆⬆⬆⬆⬆⬆⬆
跟着网上的教程，学习Kinect c++ SDK。

---

1、概述

通过Kinect来实现点云的生成与展示，重点在于RGB与Depth的对齐，因为两个图像的位姿不一样，并且他们的分辨率也不一样。
样例地址
⬇下载

1.1 坐标系讲解

1.2 对齐

对齐简单的思路——是将RGB与depth的影响重叠，对每个对应点赋予XYZ-RGB的信息。但是会导致生成的分辨率较低，因为Depth的分辨率只有512x424，应且由于两个相机位姿不用，导致镜头重叠区域外的位置无法对齐。（这点很好理解，比如你凑近观察物体，左眼和右眼能够看到的东西是不完全相同的。）

所以需要对两个相机进行对齐（Registration或者叫做Aligning）

但是在Kinect SDK的存在函数可以直接实现，我们现在粗浅使用只需要将函数调用，产生点云即可。

当然根据这个内置函数出现的结果可能并不能满足某些精确测量的需求，这个时候可以通过手动测定两个镜头的内参外参，之后再进行Align就可以了。

---

1.3 代码实现

首先添加新的接口：
之前两章都只是用了单一数据源。这时候我们需要数据同步采集，那么就要使用MultiSourceFrameReader这个，能提供同步多个数据源输入。
两个相机的空间坐标系是不同的，这时候就需要用ICoordinateMapper来进行同步坐标，完成转换。
官网CoordinateMapper类说明

1.3.1 初始化

#include <Windows.h>
#include <Ole2.h>

#include <gl/GL.h>
#include <gl/GLU.h>
#include <gl/glut.h>

#include <Kinect.h>


// Kinect variables 变量
IKinectSensor* sensor;     //Kinect传感器
IMultiSourceFrameReader* reader;   //获取多数据源
ICoordinateMapper* mapper; //初始化ICoordinateMapper

bool initKinect() {
	if (FAILED(GetDefaultKinectSensor(&sensor))) {
		return false;
	}
	if (sensor) {
		sensor->get_CoordinateMapper(&mapper);

		sensor->Open();
		sensor->OpenMultiSourceFrameReader(
			FrameSourceTypes::FrameSourceTypes_Depth | FrameSourceTypes::FrameSourceTypes_Color,
			& reader);
		//通过MultiSourceFrameReader读取多种数据，实现同步读取
		return reader;
	}
	else {
		return false;
	}
}

1.3.2 获取数据框架

首先书从帧中获取数据的代码框架

void getKinectData() {
	IMultiSourceFrame* frame = NULL;
	reader->AcquireLatestFrame(&frame);
	//...待填入
	getDepthData(frame, dest);
	//...待填入
	getColorData(frame, dest);
}

获得深度数据的代码框架

void getDepthData(IMultiSourceFrame* frame, GLubyte* dest) {
	IDepthFrame* depthframe;
	IDepthFrameReference* frameref = NULL;
	frame->get_DepthFrameReference(&frameref);
	frameref->AcquireFrame(&depthframe);
	if (frameref) frameref->Release();
	if (!depthframe) return;

	// 处理深度数据code

	if (depthframe) depthframe->Release();
}

获得彩色数据的代码框架

void getColorData(IMultiSourceFrame* frame, GLubyte* dest) {
	IColorFrame* colorframe;
	IColorFrameReference* frameref = NULL;
	frame->get_ColorFrameReference(&frameref);
	frameref->AcquireFrame(&colorframe);
	if (frameref) frameref->Release();
	if (!colorframe) return;

	//处理彩色数据code

	if (colorframe) colorframe->Release();
}

代码中的内容后续填入

1.3.3 使用座标映射接口

首先确定存在三个坐标空间：
3D的点云空间（XYZ）；
19201080的二维彩色空间
512424的二维深度空间

目标：为3D空间中的每一个点都赋予XYZ和RGB值
方法：使用CoordinateMapper获取depth与三维空间的查找表映射，获取深度像素坐标与彩色图像中相应坐标的映射。
调用ICoordinateMapper函数，大多数映射的函数都需要深度帧作为输入数组

在此对不同空间数据进行设置

//常量与全局变量
#define width 512
#define height 424
/*
作为参考
彩色空间点{float X,Y}
相机空间点{float X,Y,Z}
*/
ColorSpacePoint depth2rgb[width * height];//深度像素映射到彩色像素
CameraSpacePoint depth2xyz[width * height];//深度像素映射到3D坐标

然后填补深度数据框架中的内容：

void getDepthData(IMultiSourceFrame* frame, GLubyte* dest) {
	IDepthFrame* depthframe;
	IDepthFrameReference* frameref = NULL;//从 MultiSourceFrame 填充深度帧
	frame->get_DepthFrameReference(&frameref);
	frameref->AcquireFrame(&depthframe);
	if (frameref) frameref->Release();
	if (!depthframe) return;//从深度帧获取数据

	unsigned int sz;
	unsigned short* buf;

	depthframe->AccessUnderlyingBuffer(&sz, &buf);//通过该函数将数据复制到数组
	//第一个是个记录大小的变量，第二个是个接受数据的数组指针，传入后会分别返回数组的大小以及数据。
	//它传入数组的数据代表的是那一个像素点上的物体距离传感器的位置

	mapper->MapDepthFrameToCameraSpace(
		width * height, buf, // 深度帧数据和深度帧大小
		width * height, depth2xyz//输出空间点数组和大小
	);
	mapper->MapDepthFrameToColorSpace(
		width*height, buf,// 深度帧数据和深度帧大小
		width*height,depth2rgb//输出彩色空间点数组和大小
	);

	if (depthframe) depthframe->Release();
}
}

1.3.4 从Kienct获取深度数据

GetDepthData（）函数中我们使用点的坐标而不是深度值来填充缓冲区。
需要对缓冲区填充到widthheight3*sizeof(float)的大小

void getDepthData(IMultiSourceFrame* frame, GLubyte* dest) {
	//以上部分是 填充到深度转xyz
	float* fdest = (float*)dest;
	for (int i = 0; i < width * height; i++) {
		*fdest++ = depth2xyz[i].X;
		*fdest++ = depth2xyz[i].Y;
		*fdest++ = depth2xyz[i].Z;
	}
	//同样我们也可以使用fdest 指针，这样既等效又简便
	//for (int i = 0; i < width*height; i++) {
	//	fdest[3*i+0] = depth2xyz[i].X;
	//	fdest[3*i+1] = depth2xyz[i].Y;
	//	fdest[3*i+2] = depth2xyz[i].Z;
	//}
	if (depthframe) depthframe->Release();
}

1.3.5 从Kienct获取彩色数据

我们彩色输出应与深度点关联，那么我们将使用类似getdepthdata（）函数的方法，来设置大小为widthheight3*sizeof(float)的缓冲区（buffer）存储点云RGB值

void getColorData(IMultiSourceFrame* frame, GLubyte* dest) {
	IColorFrame* colorframe;
	IColorFrameReference* frameref = NULL;
	frame->get_ColorFrameReference(&frameref);
	frameref->AcquireFrame(&colorframe);
	if (frameref) frameref->Release();
	if (!colorframe) return;

	//处理彩色数据code
	colorframe->CopyConvertedFrameDataToArray(colorwidth * colorheight * 4, rgbimage, ColorImageFormat_Rgba);
	// 为顶点写入颜色数组
	float* fdest = (float*)dest;
	for (int i = 0; i < width*height; i++) {
		ColorSpacePoint p = depth2rgb[i];
		// 判断颜色像素坐标是否在边界内;
		if (p.X < 0 || p.Y < 0 || p.X > colorwidth || p.Y > colorheight) {
			*fdest++ = 0;
			*fdest++ = 0;
			*fdest++ = 0;
		}
		else
		{
			int idx = (int)p.X + colorwidth * (int)p.Y;
			*fdest++ = rgbimage[4 * idx + 0] / 255.;
			*fdest++ = rgbimage[4 * idx + 1] / 255.;
			*fdest++ = rgbimage[4 * idx + 2] / 255.;
		}
	}
	if (colorframe) colorframe->Release();  
}

在这个块中，我们遍历深度图像的像素，使用从CoordinateMapper获取的depth2xyz映射查找彩色图像中的对应坐标。我们检查每个深度像素是否确实映射到了 RGB 图像中的某个像素上面，else，那么我们就手动给这个点 分配为黑色。

解读一下：
首先else中语句主要目的是为查找到的对应点上色：
因为GBRA格式的数据，每个通道一字节，逐行排列，所以 （x，y） 的线性指数是x + width*y。由于X,Y是浮点数，所以在作为数组索引前取整为int型。
我们想要的四字节块位于linearindex*4。
最后我们想要把按字节取值（0~255）的BGRA格式转为浮点数取值（0.0 ~ 1.0)的RGB格式。
所以取前三通道，除以255。
rgbimage[4*linearindex + channel]/255.f

---

1.4 OpenGL显示

要用数组缓存（array buffers）显示点云。
数组缓存存在了GPU中，显示效率更高。

1.4.1 安装GLEW

地址
跟之前glut的使用方法一样，glew32.lib加入附加依赖项中。

1.4.2 OpenGL代码

将OpenGL函数放入我么的主函数中
我们要注意相机的设置，利用gluPerspective()和gluLookAt()函数来为我们解决这个问题。

int main(int argc, char* argv[]) {
	if (!init(argc, argv)) return 1;
	if (!initKinect()) return 1;

	// 设置OpenGL 
	glClearColor(0, 0, 0, 0);
	glClearDepth(1.0f);

	//设置数组缓冲区
	const int dataSize = width * height * 3 * 4;
	glGenBuffers(1, &vboId);
	glBindBuffer(GL_ARRAY_BUFFER, vboId);
	glBufferData(GL_ARRAY_BUFFER, dataSize, 0, GL_DYNAMIC_DRAW);
	glGenBuffers(1, &cboId);
	glBindBuffer(GL_ARRAY_BUFFER, cboId);
	glBufferData(GL_ARRAY_BUFFER, dataSize, 0, GL_DYNAMIC_DRAW);

	// 设置相机
	glViewport(0, 0, width, height);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(45, width / (GLdouble)height, 0.1, 1000);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	gluLookAt(0, 0, 0, 0, 0, 1, 0, 1, 0);

	// 主循环
	execute();
	return 0;
}

---

1.5 代码块融合

写好了getDepthData()和getRgbData()，将适当的数据从MultiSourceFrame中复制到 GPU 上

void getKinectData() {
	IMultiSourceFrame* frame = NULL;
	if (SUCCEEDED(reader->AcquireLatestFrame(&frame))) {
		GLubyte* ptr;
		glBindBuffer(GL_ARRAY_BUFFER, vboId);
		ptr = (GLubyte*)glMapBuffer(GL_ARRAY_BUFFER, GL_WRITE_ONLY);
		if (ptr) {
			getDepthData(frame, ptr);
		}
		glUnmapBuffer(GL_ARRAY_BUFFER);
		glBindBuffer(GL_ARRAY_BUFFER, cboId);
		ptr = (GLubyte*)glMapBuffer(GL_ARRAY_BUFFER, GL_WRITE_ONLY);
		if (ptr) {
			getColorData(frame, ptr);
		}
		glUnmapBuffer(GL_ARRAY_BUFFER);
	}
	if (frame) frame->Release();
}

在使用**glDrawArrays()**函数来绘制点云

void drawKinectData() {
    getKinectData();
    rotateCamera();

    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    glEnableClientState(GL_VERTEX_ARRAY);
    glEnableClientState(GL_COLOR_ARRAY);

    glBindBuffer(GL_ARRAY_BUFFER, vboId);
    glVertexPointer(3, GL_FLOAT, 0, NULL);

    glBindBuffer(GL_ARRAY_BUFFER, cboId);
    glColorPointer(3, GL_FLOAT, 0, NULL);

    glPointSize(1.f);
    glDrawArrays(GL_POINTS, 0, width*height);

    glDisableClientState(GL_VERTEX_ARRAY);
    glDisableClientState(GL_COLOR_ARRAY);
}

---

1.6 完整代码

#include <Windows.h>
#include <Ole2.h>

#include <cmath>
#include <cstdio>

#include <gl/glew.h>
#include <gl/GL.h>
#include <gl/GLU.h>
#include <gl/glut.h>

#include <Kinect.h>



//常量与全局变量
#define width 512
#define height 424
#define colorwidth 1920
#define colorheight 1080
/*
作为参考
ColorSpacePoint彩色空间点{float X,Y}
CameraSpacePoint相机空间点{float X,Y,Z}
*/
unsigned char rgbimage[colorwidth * colorheight * 4];    // 储存彩色图片
ColorSpacePoint depth2rgb[width * height];//深度像素映射到彩色像素
CameraSpacePoint depth2xyz[width * height];//深度像素映射到3D坐标


//OpenGL变量
GLuint textureID; //包含 Kinect RGB 数据的纹理 ID
GLubyte data[width * height * 4];//包含纹理数据BGRA 的数组
GLuint vboId; // 顶点缓冲区 ID
GLuint cboId; // 色彩缓冲区ID

// Kinect variables 变量
IKinectSensor* sensor;             //Kinect传感器
IMultiSourceFrameReader* reader;   //获取多数据源
ICoordinateMapper* mapper;         //初始化ICoordinateMapper



//Kinect初始化
//initKinect()函数的作用是初始化一个 Kinect 设备。
//它包含了两个部分：首先，我们需要找到一个已连接到电脑的 Kinect 传感器，然后将其初始化并准备从中读取数据。
bool initKinect() {
	if (FAILED(GetDefaultKinectSensor(&sensor))) {
		return false;
	}
	if (sensor) {
		sensor->get_CoordinateMapper(&mapper);

		sensor->Open();
		sensor->OpenMultiSourceFrameReader(
			FrameSourceTypes::FrameSourceTypes_Depth | FrameSourceTypes::FrameSourceTypes_Color,
			& reader);
		//通过MultiSourceFrameReader读取多种数据，实现同步读取
		return reader;
	}
	else {
		return false;
	}
}

void getDepthData(IMultiSourceFrame* frame, GLubyte* dest) {
	IDepthFrame* depthframe;
	IDepthFrameReference* frameref = NULL;//从 MultiSourceFrame 填充深度帧
	frame->get_DepthFrameReference(&frameref);
	frameref->AcquireFrame(&depthframe);
	if (frameref) frameref->Release();
	if (!depthframe) return;
	//从深度帧获取数据
	unsigned int sz;
	unsigned short* buf;

	depthframe->AccessUnderlyingBuffer(&sz, &buf);//通过该函数将数据复制到数组
	//第一个是个记录大小的变量，第二个是个接受数据的数组指针，传入后会分别返回数组的大小以及数据。
	//它传入数组的数据代表的是那一个像素点上的物体距离传感器的位置

	mapper->MapDepthFrameToCameraSpace(
		width * height, buf, // 深度帧数据和深度帧大小
		width * height, depth2xyz//输出空间点数组和大小
	);

	float* fdest = (float*)dest;
	for (int i = 0; i < width * height; i++) {
		fdest[3 * i + 0] = depth2xyz[i].X;
		fdest[3 * i + 1] = depth2xyz[i].Y;
		fdest[3 * i + 2] = depth2xyz[i].Z;
	}
	mapper->MapDepthFrameToColorSpace(
		width*height, buf,// 深度帧数据和深度帧大小
		width*height,depth2rgb//输出彩色空间点数组和大小
	);
	//以上部分是 填充到深度转xyz

	if (depthframe) depthframe->Release();
}

void getColorData(IMultiSourceFrame* frame, GLubyte* dest) {
	IColorFrame* colorframe;
	IColorFrameReference* frameref = NULL;
	frame->get_ColorFrameReference(&frameref);
	frameref->AcquireFrame(&colorframe);
	if (frameref) frameref->Release();
	if (!colorframe) return;

	//处理彩色数据code
	colorframe->CopyConvertedFrameDataToArray(colorwidth * colorheight * 4, rgbimage, ColorImageFormat_Rgba);
	// 为顶点写入颜色数组
	float* fdest = (float*)dest;
	for (int i = 0; i < width*height; i++) {
		ColorSpacePoint p = depth2rgb[i];
		// 判断颜色像素坐标是否在边界内;
		if (p.X < 0 || p.Y < 0 || p.X > colorwidth || p.Y > colorheight) {
			*fdest++ = 0;
			*fdest++ = 0;
			*fdest++ = 0;
		}
		else
		{
			int idx = (int)p.X + colorwidth * (int)p.Y;
			*fdest++ = rgbimage[4 * idx + 0] / 255.;
			*fdest++ = rgbimage[4 * idx + 1] / 255.;
			*fdest++ = rgbimage[4 * idx + 2] / 255.;
		}
	}
	if (colorframe) colorframe->Release();  
}

//将数据从Multisourceframe复制到GPU上
void getKinectData() {
	IMultiSourceFrame* frame = NULL;
	if (SUCCEEDED(reader->AcquireLatestFrame(&frame))) {
		GLubyte* ptr;
		glBindBuffer(GL_ARRAY_BUFFER, vboId);
		ptr = (GLubyte*)glMapBuffer(GL_ARRAY_BUFFER, GL_WRITE_ONLY);
		if (ptr) {
			getDepthData(frame, ptr);
		}
		glUnmapBuffer(GL_ARRAY_BUFFER);
		glBindBuffer(GL_ARRAY_BUFFER, cboId);
		ptr = (GLubyte*)glMapBuffer(GL_ARRAY_BUFFER, GL_WRITE_ONLY);
		if (ptr) {
			getColorData(frame, ptr);
		}
		glUnmapBuffer(GL_ARRAY_BUFFER);
	}
	if (frame) frame->Release();
}

void rotateCamera() {
	static double angle = 0.;
	static double radius = 3.;
	double x = radius * sin(angle);
	double z = radius * (1 - cos(angle)) - radius / 2;
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	gluLookAt(x, 0, z, 0, 0, radius / 2, 0, 1, 0);
	angle += 0.002;
}


//用glDrawArrays()绘制点云
void drawKinectData() {
	getKinectData();
	rotateCamera();

	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
	glEnableClientState(GL_VERTEX_ARRAY);
	glEnableClientState(GL_COLOR_ARRAY);

	glBindBuffer(GL_ARRAY_BUFFER, vboId);
	glVertexPointer(3, GL_FLOAT, 0, NULL);

	glBindBuffer(GL_ARRAY_BUFFER, cboId);
	glColorPointer(3, GL_FLOAT, 0, NULL);

	glPointSize(1.f);
	glDrawArrays(GL_POINTS, 0, width * height);

	glDisableClientState(GL_VERTEX_ARRAY);
	glDisableClientState(GL_COLOR_ARRAY);
}

//前几章设置的作图程序
void draw() {
	drawKinectData();
	glutSwapBuffers();
}

void execute() {
	glutMainLoop();
}

bool init(int argc, char* argv[]) {
	glutInit(&argc, argv);
	glutInitDisplayMode(GLUT_DEPTH | GLUT_DOUBLE | GLUT_RGBA);
	glutInitWindowSize(width, height);
	glutCreateWindow("Kinect SDK Tutorial");
	glutDisplayFunc(draw);
	glutIdleFunc(draw);
	glewInit();
	return true;
}


int main(int argc, char* argv[]) {
	if (!init(argc, argv)) return 1;
	if (!initKinect()) return 1;

	// 设置OpenGL 
	glClearColor(0, 0, 0, 0);
	glClearDepth(1.0f);

	//设置数组缓冲区
	const int dataSize = width * height * 3 * 4;
	glGenBuffers(1, &vboId);
	glBindBuffer(GL_ARRAY_BUFFER, vboId);
	glBufferData(GL_ARRAY_BUFFER, dataSize, 0, GL_DYNAMIC_DRAW);
	glGenBuffers(1, &cboId);
	glBindBuffer(GL_ARRAY_BUFFER, cboId);
	glBufferData(GL_ARRAY_BUFFER, dataSize, 0, GL_DYNAMIC_DRAW);

	// 设置相机
	glViewport(0, 0, width, height);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(45, width / (GLdouble)height, 0.1, 1000);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	gluLookAt(0, 0, 0, 0, 0, 1, 0, 1, 0);

	// 主循环
	execute();
	return 0;
}

1.7 效果图

此图会旋转，但是不可使用手动拖动，需要的话应该可以借助open3d或者pcl来实现。
本期的Kinect SDK探究到此结束，我这边并不涉及骨骼追踪，就不多加学习了。
有兴趣或者疑问的小伙伴欢迎探讨。