kinect 深度图与彩色图对齐程序
//#include "duiqi.hpp"
#include "kinect.h"
#include <iostream>
#include "opencv2/opencv.hpp"
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <fstream>
using namespace cv;
using namespace std;
Mat depthFilter(UINT16 *depthData);
template<class Interface>
inline void SafeRelease(Interface *& pInterfaceToRelease)
{
if (pInterfaceToRelease != NULL)
{
pInterfaceToRelease->Release();
pInterfaceToRelease = NULL;
}
}
UINT16 uDepthMin = 0, uDepthMax = 0;
int main()
{
IKinectSensor* m_pKinectSensor;
HRESULT hr;
ICoordinateMapper* m_pCoordinateMapper = NULL;
CameraIntrinsics* m_pCameraIntrinsics = new CameraIntrinsics();
hr = GetDefaultKinectSensor(&m_pKinectSensor);
if (FAILED(hr))
{
return hr;
}
IMultiSourceFrameReader* m_pMultiFrameReader = NULL;
if (m_pKinectSensor)
{
hr = m_pKinectSensor->Open();
if (SUCCEEDED(hr))
{
hr = m_pKinectSensor->OpenMultiSourceFrameReader(
FrameSourceTypes::FrameSourceTypes_Color |
FrameSourceTypes::FrameSourceTypes_Infrared |
FrameSourceTypes::FrameSourceTypes_Depth,
&m_pMultiFrameReader);
}
}
if (SUCCEEDED(hr))
{
hr = m_pKinectSensor->get_CoordinateMapper(&m_pCoordinateMapper);
}
if (!m_pKinectSensor || FAILED(hr))
{
return E_FAIL;
}
IMultiSourceFrame* m_pMultiFrame = nullptr;
IDepthFrameReference* m_pDepthFrameReference = NULL;
IColorFrameReference* m_pColorFrameReference = NULL;
IInfraredFrameReference* m_pInfraredFrameReference = NULL;
IInfraredFrame* m_pInfraredFrame = NULL;
IDepthFrame* m_pDepthFrame = NULL;
IColorFrame* m_pColorFrame = NULL;
Mat rgb(1080, 1920, CV_8UC4);
Mat rgb_resize(540, 960, CV_8UC4);
DepthSpacePoint* m_pDepthCoordinates = NULL;
ColorSpacePoint* m_pColorCoordinates = NULL;
CameraSpacePoint* m_pCameraCoordinates = NULL;
m_pColorCoordinates = new ColorSpacePoint[512 * 424];
m_pCameraCoordinates = new CameraSpacePoint[512 * 424];
UINT16 *depthData = new UINT16[424 * 512];
Mat depth(424, 512, CV_16UC1);
Mat depth8U(424, 512, CV_8U);
vector<DepthSpacePoint> depthSpacePoints(1920 * 1080);
Mat CoordinateMapperMat(1080, 1520, CV_8U);
Mat CoordinateMapperMat_resize(540, 960, CV_8U);
int savecount = 0;
while (true)
{
hr = m_pMultiFrameReader->AcquireLatestFrame(&m_pMultiFrame);
if (FAILED(hr) || !m_pMultiFrame)
{
continue;
}
if (SUCCEEDED(hr))
hr = m_pMultiFrame->get_ColorFrameReference(&m_pColorFrameReference);
if (SUCCEEDED(hr))
hr = m_pColorFrameReference->AcquireFrame(&m_pColorFrame);
if (SUCCEEDED(hr))
hr = m_pMultiFrame->get_DepthFrameReference(&m_pDepthFrameReference);
if (SUCCEEDED(hr))
hr = m_pDepthFrameReference->AcquireFrame(&m_pDepthFrame);
/*m_pDepthFrame->get_DepthMinReliableDistance(&uDepthMin);
m_pDepthFrame->get_DepthMaxReliableDistance(&uDepthMax);
cout << "Reliable Distance: " << uDepthMin << " - " << uDepthMax << endl;*/
savecount++;
cout << savecount << endl;
ostringstream savecountstr;
savecountstr << savecount;
UINT nColorBufferSize = 1920 * 1080 * 4;
if (SUCCEEDED(hr))
{
hr = m_pColorFrame->CopyConvertedFrameDataToArray(nColorBufferSize, reinterpret_cast<BYTE*>(rgb.data), ColorImageFormat::ColorImageFormat_Bgra);
Rect rect(200, 0, 1520, 1080);
Mat rgb_roi = rgb(rect);
resize(rgb_roi, rgb_resize, Size(), 0.4, 0.4);
imshow("color_resize", rgb_resize);
imwrite("D:/file/hust/ARcodes/ARKinect∂‘∆Î181107/ARKinect/save6/color/" + savecountstr.str() + ".png", rgb_resize);
}
UINT nDepthBufferSize = 424 * 512;
if (SUCCEEDED(hr))
{
hr = m_pDepthFrame->CopyFrameDataToArray(nDepthBufferSize, reinterpret_cast<UINT16*>(depthData));
m_pDepthFrame->CopyFrameDataToArray(nDepthBufferSize, reinterpret_cast<UINT16*>(depth.data));
//16位转成了8位显示
depth.convertTo(depth8U, CV_8U, 255.0f / 4500.0f);
imshow("depth", depth8U);
}
//16位数据
Mat filterDepth = depthFilter(depthData);
Mat filterDepth8U;
filterDepth.convertTo(filterDepth8U, CV_8U, 255.0f / 4500.0f);
if (SUCCEEDED(hr))
{
hr = m_pCoordinateMapper->MapColorFrameToDepthSpace(424 * 512, reinterpret_cast<UINT16*>(filterDepth.data), 1920 * 1080, &depthSpacePoints[0]);
}
if (SUCCEEDED(hr))
{
CoordinateMapperMat = Scalar(0, 0, 0, 0); // ∂®“ÂŒ™Mat(colorHeight, colorWidth, CV_8UC4)
for (int y = 0; y < 1080; y++)
{
for (int x = 200; x < 1720; x++)
//for (int x = 0; x < 1920; x++)
{
unsigned int index = y * 1920 + x;
DepthSpacePoint p = depthSpacePoints[index];
if (p.X != -std::numeric_limits<float>::infinity() && p.Y != -std::numeric_limits<float>::infinity())
{
int depthX = static_cast<int>(p.X + 0.2f);
int depthY = static_cast<int>(p.Y + 0.2f);
if ((depthX >= 0) && (depthX < 512) && (depthY >= 0) && (depthY < 424))
{
CoordinateMapperMat.at<uchar>(y, x - 200) = filterDepth8U.at<uchar>(depthY, depthX);
}
}
}
}
resize(CoordinateMapperMat, CoordinateMapperMat_resize, Size(), 0.4, 0.4);
imshow("CoordinateMapper", CoordinateMapperMat_resize);
imwrite("D:/file/hust/ARcodes/ARKinect∂‘∆Î181107/ARKinect/save6/result/" + savecountstr.str() + ".png", CoordinateMapperMat_resize);
}
int c = waitKey(1);
if ((char)c == VK_ESCAPE)
break;
SafeRelease(m_pColorFrame);
SafeRelease(m_pDepthFrame);
SafeRelease(m_pColorFrameReference);
SafeRelease(m_pDepthFrameReference);
SafeRelease(m_pMultiFrame);
}
cv::destroyAllWindows();
SafeRelease(m_pCoordinateMapper);
m_pKinectSensor->Close();
std::system("pause");
return 0;
}
Mat depthFilter(UINT16 *depthData) {
Mat i_before(424, 512, CV_8UC4);
Mat i_after(424, 512, CV_8UC4);
Mat i_result(424, 512, CV_16UC1);
cv::Mat i_result8U;
unsigned short maxDepth = 0;
unsigned short iZeroCountBefore = 0;
unsigned short iZeroCountAfter = 0;
unsigned short* depthArray = (unsigned short*)depthData;
for (int i = 0; i < 512 * 424; i++)
{
int row = i / 512;
int col = i % 512;
unsigned short depthValue = depthArray[row * 512 + col];
if (depthValue == 0)
{
i_before.data[i * 4] = 255;
i_before.data[i * 4 + 1] = 0;
i_before.data[i * 4 + 2] = 0;
i_before.data[i * 4 + 3] = depthValue / 256;
iZeroCountBefore++;
}
else
{
i_before.data[i * 4] = depthValue / 4500.0f * 256;
i_before.data[i * 4 + 1] = depthValue / 4500.0f * 256;
i_before.data[i * 4 + 2] = depthValue / 4500.0f * 256;
i_before.data[i * 4 + 3] = depthValue / 4500.0f * 256;
}
maxDepth = depthValue > maxDepth ? depthValue : maxDepth;
}
//cout << "max depth value: " << maxDepth << endl;
unsigned short* smoothDepthArray = (unsigned short*)i_result.data;
int widthBound = 512 - 1;
int heightBound = 424 - 1;
int innerBandThreshold = 1;
int outerBandThreshold = 3;
for (int depthArrayRowIndex = 0; depthArrayRowIndex<424; depthArrayRowIndex++)
{
for (int depthArrayColumnIndex = 0; depthArrayColumnIndex < 512; depthArrayColumnIndex++)
{
int depthIndex = depthArrayColumnIndex + (depthArrayRowIndex * 512);
if (depthArray[depthIndex] == 0)
{
int x = depthIndex % 512;
int y = (depthIndex - x) / 512;
unsigned short filterCollection[24][2] = { 0 };
int innerBandCount = 0;
int outerBandCount = 0;
for (int yi = -2; yi < 3; yi++)
{
for (int xi = -2; xi < 3; xi++)
{
if (xi != 0 || yi != 0)
{
int xSearch = x + xi;
int ySearch = y + yi;
if (xSearch >= 0 && xSearch <= widthBound &&
ySearch >= 0 && ySearch <= heightBound)
{
int index = xSearch + (ySearch * 512);
if (depthArray[index] != 0)
{
for (int i = 0; i < 24; i++)
{
if (filterCollection[i][0] == depthArray[index])
{
filterCollection[i][1]++;
break;
}
else if (filterCollection[i][0] == 0)
{
filterCollection[i][0] = depthArray[index];
filterCollection[i][1]++;
break;
}
}
if (yi != 2 && yi != -2 && xi != 2 && xi != -2)
innerBandCount++;
else
outerBandCount++;
}
}
}
}
}
if (innerBandCount >= innerBandThreshold || outerBandCount >= outerBandThreshold)
{
short frequency = 0;
short depth = 0;
for (int i = 0; i < 24; i++)
{
if (filterCollection[i][0] == 0)
break;
if (filterCollection[i][1] > frequency)
{
depth = filterCollection[i][0];
frequency = filterCollection[i][1];
}
}
smoothDepthArray[depthIndex] = depth;
}
else
{
smoothDepthArray[depthIndex] = 0;
}
}
else
{
smoothDepthArray[depthIndex] = depthArray[depthIndex];
}
}
}
for (int i = 0; i < 512 * 424; i++)
{
int row = i / 512;
int col = i % 512;
unsigned short depthValue = smoothDepthArray[row * 512 + col];
if (depthValue == 0)
{
i_after.data[i * 4] = 255;
i_after.data[i * 4 + 1] = 0;
i_after.data[i * 4 + 2] = 0;
i_after.data[i * 4 + 3] = depthValue / 256;
iZeroCountAfter++;
}
else
{
i_after.data[i * 4] = depthValue / 4500.0f * 256;
i_after.data[i * 4 + 1] = depthValue / 4500.0f * 256;
i_after.data[i * 4 + 2] = depthValue / 4500.0f * 256;
i_after.data[i * 4 + 3] = depthValue / 4500.0f * 256;
}
}
i_result.convertTo(i_result8U, CV_8U, 255.0f / 4500.0f); // uDepthMax
return i_result;
}
