kinect2深度圖對齊以及三維座標的計算
阿新 • • 發佈：2019-02-04
1. kinect2 深度圖與彩色圖對齊
IDepthFrame*frame1;
hResult = depthreader->AcquireLatestFrame(&frame1);
Mat depth_16bit(424, 512, CV_16UC1);//深度資料，需要賦值，這裡假設已賦值
Mat color(height, width, CV_8UC4);//彩色圖，需要賦值，這裡假設已賦值
Mat coordinateMapperMat(height_d, width_d, CV_8UC4);//對齊後彩色圖的儲存
if (SUCCEEDED(hResult))
{
ICoordinateMapper* pCoordinateMapper;
kinect->get_CoordinateMapper(&pCoordinateMapper);
hResult = pCoordinateMapper->MapDepthFrameToColorSpace(width_d * height_d, reinterpret_cast 2. 根據深度圖計算框選區域三維座標
vector<double> getxyz(int u, int v, Mat depth_16bit, vector<double> params)//獲取指定畫素的3維座標，可以經過兩個迴圈計算出點雲
{
int camera_factor = params[0];
double cx = params[1];//以下內參需要標定
double cy = params[2];
double fx = params[3];
double fy = params[4];
double z = depth_16bit.at<short>(u, v)/camera_factor;
double x = (u - cx) * z / fx;
double y = (v - cy) * z / fy;
vector<double> xyz;
xyz.push_back(x);
xyz.push_back(y);
xyz.push_back(z);
return xyz;
}3. 互動功能如下
| 操作 | 功能 |
|---|---|
| “s” | 儲存當前幀彩色圖 |
| 滑鼠 | 框選區域（左上角按滑鼠左鍵，拖到右下角鬆開） |
4. 完整程式碼
#include "stdafx.h"
#include <iostream>
#include <sstream>
#include "opencv2/core.hpp"
#include "opencv2/imgproc.hpp"
#include "opencv2/highgui.hpp"
#include "opencv2/videoio.hpp"
#include <Kinect.h>
#pragma comment ( lib, "kinect20.lib" )
using namespace cv;
using namespace std;
//全域性變數
Rect select1 = Rect(0,0,0,0);
bool tracking = false;
vector<double> params(5);//存放縮放係數和kinect2的內參
Mat depth_16bit(424, 512, CV_16UC1);
Mat depth(424, 512, CV_8UC1);
vector<double> getxyz(int u, int v, Mat depth_16bit, vector<double> params)
{
int camera_factor = params[0];
double cx = params[1];
double cy = params[2];
double fx = params[3];
double fy = params[4];
double z = depth_16bit.at<short>(u, v)/camera_factor;
double x = (u - cx) * z / fx;
double y = (v - cy) * z / fy;
vector<double> xyz;
xyz.push_back(x);
xyz.push_back(y);
xyz.push_back(z);
return xyz;
}
void onMouse(int event,int x,int y,int,void*)//滑鼠從左上角到右下角進行框選
{
if(event==CV_EVENT_LBUTTONDOWN)
{
select1.x=x;
select1.y=y;
tracking=false;
}
else if(event==CV_EVENT_LBUTTONUP)
{
select1.width=x-select1.x;
select1.height=y-select1.y;
tracking=true;//左鍵完後，開始跟蹤
int u = select1.x + 0.5 * select1.width;
int v = select1.y + 0.5 * select1.height;
vector<double> xyz = getxyz(v, u, depth_16bit, params);
cout << "3d position of the selected pixel: [" << xyz[0] << " " << xyz[1] << " " << xyz[2] << "]" << endl;
}
}
int main()
{
//kinect2的縮放係數和內參
params[0] = 1000;
params[1] = 0.9677*1e3;
params[2] = 0.5423*1e3;
params[3] = 1.0634*1e3;
params[4] = 1.0634*1e3;
HRESULT hResult = S_OK; //用於檢測操作是否成功
IKinectSensor *kinect; //建立一個感應器
GetDefaultKinectSensor(&kinect);
kinect->Open(); //開啟感應器
IColorFrameSource *colorsource;
IColorFrameReader *colorreader;
IFrameDescription *colorde;
kinect->get_ColorFrameSource(&colorsource);
colorsource->OpenReader(&colorreader);
colorsource->get_FrameDescription(&colorde);
IDepthFrameSource *depthrsource;
IDepthFrameReader *depthreader;
IFrameDescription *depthde;
kinect->get_DepthFrameSource(&depthrsource);
depthrsource->OpenReader(&depthreader);
depthrsource->get_FrameDescription(&depthde);
int width = 0; //長和寬
int height = 0;
colorde->get_Height(&height);
colorde->get_Width(&width);
Mat color(height, width, CV_8UC4);//注意：這裡必須為4通道的圖，Kinect的資料只能以Bgra格式傳出
namedWindow("color");
int width_d = 0;
int height_d = 0;
depthde->get_Height(&height_d);
depthde->get_Width(&width_d);
//namedWindow("depth");
UINT16 *data = new UINT16[width_d*height_d];
cout << "color picture size:" << width << "x" << height << endl;
cout << "depth picture size:" << width_d << "x" << height_d << endl;
//深度圖與彩色圖對齊
vector<ColorSpacePoint> MappingMatrix(width_d * height_d);
Mat coordinateMapperMat(height_d, width_d, CV_8UC4);
namedWindow("Map color");
setMouseCallback("Map color", onMouse, 0);
int num = 1;
while (1)
{
//彩色圖顯示
IColorFrame*frame;
hResult = colorreader->AcquireLatestFrame(&frame);
if (SUCCEEDED(hResult))
{
frame->CopyConvertedFrameDataToArray(height*width * 4,
reinterpret_cast<BYTE*>(color.data), ColorImageFormat::ColorImageFormat_Bgra); //傳出資料
}
if (frame != NULL) //釋放
{
frame->Release();
frame = NULL;
}
if (waitKey(30) == VK_ESCAPE)
break;
cv::imshow("color", color);
stringstream ss;
ss << num << ".jpg";
if (waitKey(100)==115)//s的ASCII碼為115
{
cv::imwrite(ss.str(), color);
cout<<ss.str()<<" saved."<<endl;
num++;
}
//深度圖顯示
IDepthFrame*frame1;
hResult = depthreader->AcquireLatestFrame(&frame1);
if (SUCCEEDED(hResult))
{
frame1->CopyFrameDataToArray(height_d*width_d, data);
for (int i = 0; i < width_d*height_d; i++)
{
byte intensity = (byte)(data[i] >> 5); //型別的轉換,data內的值即為距離，單位mm
reinterpret_cast<BYTE*>(depth.data)[i] = intensity;
depth_16bit.at<short>( i / width_d, i%width_d) = data[i];
}
}
if (frame1 != NULL)
{
frame1->Release();
frame1 = NULL;
}
if (waitKey(30) == VK_ESCAPE)
break;
cv::imshow("depth", depth);
if (SUCCEEDED(hResult))
{
ICoordinateMapper* pCoordinateMapper;
kinect->get_CoordinateMapper(&pCoordinateMapper);
hResult = pCoordinateMapper->MapDepthFrameToColorSpace(width_d * height_d, reinterpret_cast<UINT16*>(depth_16bit.data), width_d * height_d, &MappingMatrix[0]);
}
if (SUCCEEDED(hResult))
{
coordinateMapperMat = cv::Scalar(0, 0, 0, 0);
for (int y = 0; y < height_d; y++)
{
for (int x = 0; x < width_d; x++)
{
unsigned int index = y * width_d + x;
ColorSpacePoint point = MappingMatrix[index];
int colorX = static_cast<int>(std::floor(point.X + 0.5));
int colorY = static_cast<int>(std::floor(point.Y + 0.5));
if ((colorX >= 0) && (colorX < width) && (colorY >= 0) && (colorY < height))
{
coordinateMapperMat.at<cv::Vec4b>(y, x) = color.at<cv::Vec4b>(colorY, colorX);
}
}
}
}
if (waitKey(30) == VK_ESCAPE)
break;
rectangle(coordinateMapperMat, select1, Scalar(255,0,0), 3, 8, 0);
cv::imshow("Map color", coordinateMapperMat);
waitKey(30);
}
//colorful
if (colorsource != NULL) //全部釋放掉
{
colorsource->Release();
colorsource = NULL;
}
if (colorreader != NULL)
{
colorreader->Release();
colorreader = NULL;
}
if (colorde != NULL)
{
colorde->Release();
colorde = NULL;
}
//depth
if (depthrsource != NULL)
{
depthrsource->Release();
depthrsource = NULL;
}
if (depthreader != NULL)
{
depthreader->Release();
depthreader = NULL;
}
if (depthde != NULL)
{
depthde->Release();
depthde = NULL;
}
//kinect2
if (kinect)
{
kinect->Close();
}
if (kinect != NULL)
{
kinect->Release();
kinect = NULL;
}
destroyAllWindows();
}