void DepthStream::getColorSpacePoints(ColorSpacePoint *colorSpacePointsFromDepth)
{
if (lock()) {
// Calculate the body's position on the screen
const UINT depthPointCount = DEPTH_WIDTH * DEPTH_HEIGHT;
ICoordinateMapper *coordinateMapper = nullptr;
HRESULT hr = m_Device->get().kinect2->get_CoordinateMapper(&coordinateMapper);
if (SUCCEEDED(hr)) {
coordinateMapper->MapDepthFrameToColorSpace(depthPointCount, (UINT16 *)m_Frame.data, depthPointCount, colorSpacePointsFromDepth);
}
safeRelease(coordinateMapper);
unlock();
}
}
int main()
{
// name and position windows
cvNamedWindow("Color Probabilistic Tracking - Samples", 1);
cvMoveWindow("Color Probabilistic Tracking - Samples", 0, 0);
cvNamedWindow("Color Probabilistic Tracking - Result", 1);
cvMoveWindow("Color Probabilistic Tracking - Result", 1000, 0);
//control mouse
setMouseCallback("Color Probabilistic Tracking - Samples", onMouse, 0);
cv::setUseOptimized(true);
// Sensor
IKinectSensor* pSensor;
HRESULT hResult = S_OK;
hResult = GetDefaultKinectSensor(&pSensor);
if (FAILED(hResult)) {
std::cerr << "Error : GetDefaultKinectSensor" << std::endl;
return -1;
}
hResult = pSensor->Open();
if (FAILED(hResult)) {
std::cerr << "Error : IKinectSensor::Open()" << std::endl;
return -1;
}
// Source
IColorFrameSource* pColorSource;
hResult = pSensor->get_ColorFrameSource(&pColorSource);
if (FAILED(hResult)) {
std::cerr << "Error : IKinectSensor::get_ColorFrameSource()" << std::endl;
return -1;
}
IDepthFrameSource* pDepthSource;
hResult = pSensor->get_DepthFrameSource(&pDepthSource);
if (FAILED(hResult)) {
std::cerr << "Error : IKinectSensor::get_DepthFrameSource()" << std::endl;
return -1;
}
/*IBodyIndexFrameSource* pBodyIndexSource;
hResult = pSensor->get_BodyIndexFrameSource(&pBodyIndexSource);*/
// Reader
IColorFrameReader* pColorReader;
hResult = pColorSource->OpenReader(&pColorReader);
if (FAILED(hResult)) {
std::cerr << "Error : IColorFrameSource::OpenReader()" << std::endl;
return -1;
}
IDepthFrameReader* pDepthReader;
hResult = pDepthSource->OpenReader(&pDepthReader);
if (FAILED(hResult)) {
std::cerr << "Error : IDepthFrameSource::OpenReader()" << std::endl;
return -1;
}
//IBodyIndexFrameReader* pBodyIndexReader;//saferealease
//hResult = pBodyIndexSource->OpenReader(&pBodyIndexReader);
// Description
IFrameDescription* pColorDescription;
hResult = pColorSource->get_FrameDescription(&pColorDescription);
if (FAILED(hResult)) {
std::cerr << "Error : IColorFrameSource::get_FrameDescription()" << std::endl;
return -1;
}
int colorWidth = 0;
int colorHeight = 0;
pColorDescription->get_Width(&colorWidth); // 1920
pColorDescription->get_Height(&colorHeight); // 1080
unsigned int colorBufferSize = colorWidth * colorHeight * 4 * sizeof(unsigned char);
cv::Mat colorBufferMat(colorHeight, colorWidth, CV_8UC4);
cv::Mat colorMat(colorHeight / 2, colorWidth / 2, CV_8UC4);
cv::namedWindow("Color");
RGBQUAD* m_pDepthRGBX;
m_pDepthRGBX = new RGBQUAD[512 * 424];// create heap storage for color pixel data in RGBX format
IFrameDescription* pDepthDescription;
hResult = pDepthSource->get_FrameDescription(&pDepthDescription);
if (FAILED(hResult)) {
std::cerr << "Error : IDepthFrameSource::get_FrameDescription()" << std::endl;
return -1;
}
int depthWidth = 0;
int depthHeight = 0;
pDepthDescription->get_Width(&depthWidth); // 512
pDepthDescription->get_Height(&depthHeight); // 424
unsigned int depthBufferSize = depthWidth * depthHeight * sizeof(unsigned short);
cv::Mat depthBufferMat(depthHeight, depthWidth, CV_16UC1);
UINT16* pDepthBuffer = nullptr;
cv::Mat depthMat(depthHeight, depthWidth, CV_8UC1);
cv::namedWindow("Depth");
//UINT32 nBodyIndexSize = 0;
//BYTE* pIndexBuffer = nullptr;//This hasn't been safe realease yet
// Coordinate Mapper
ICoordinateMapper* pCoordinateMapper;
hResult = pSensor->get_CoordinateMapper(&pCoordinateMapper);
if (FAILED(hResult)) {
std::cerr << "Error : IKinectSensor::get_CoordinateMapper()" << std::endl;
return -1;
}
cv::Mat coordinateMapperMat(depthHeight, depthWidth, CV_8UC4);
cv::namedWindow("CoordinateMapper");
unsigned short minDepth, maxDepth;
pDepthSource->get_DepthMinReliableDistance(&minDepth);
pDepthSource->get_DepthMaxReliableDistance(&maxDepth);
while (1) {
double t = (double)getTickCount();
// Color Frame
IColorFrame* pColorFrame = nullptr;
hResult = pColorReader->AcquireLatestFrame(&pColorFrame);
if (SUCCEEDED(hResult)) {
hResult = pColorFrame->CopyConvertedFrameDataToArray(colorBufferSize, reinterpret_cast<BYTE*>(colorBufferMat.data), ColorImageFormat::ColorImageFormat_Bgra);
if (SUCCEEDED(hResult)) {
cv::resize(colorBufferMat, colorMat, cv::Size(), 0.5, 0.5);
}
}
//SafeRelease( pColorFrame );
// Depth Frame
IDepthFrame* pDepthFrame = nullptr;
hResult = pDepthReader->AcquireLatestFrame(&pDepthFrame);
if (SUCCEEDED(hResult)) {
hResult = pDepthFrame->AccessUnderlyingBuffer(&depthBufferSize, reinterpret_cast<UINT16**>(&depthBufferMat.data));
}
if (SUCCEEDED(hResult)) {
hResult = pDepthFrame->AccessUnderlyingBuffer(&depthBufferSize, &pDepthBuffer);
if (SUCCEEDED(hResult))
{
RGBQUAD* pRGBX = m_pDepthRGBX;
// end pixel is start + width*height - 1
const UINT16* pBufferEnd = pDepthBuffer + (512 * 424);
int index = 0;
while (pDepthBuffer < pBufferEnd)
{
USHORT depth = *pDepthBuffer;
BYTE intensity = static_cast<BYTE>((depth >= 50) && (depth <= 5000) ? (depth % 256) : 0);
pRGBX->rgbRed = intensity;
pRGBX->rgbGreen = intensity;
pRGBX->rgbBlue = intensity;
depthData[index] = depth;
++index;
++pRGBX;
++pDepthBuffer;
}
}
}
Mat DepthImage(424, 512, CV_8UC4, m_pDepthRGBX);
//SafeRelease( pDepthFrame );
// Mapping (Depth to Color)
if (SUCCEEDED(hResult)) {
std::vector<ColorSpacePoint> colorSpacePoints(depthWidth * depthHeight);
hResult = pCoordinateMapper->MapDepthFrameToColorSpace(depthWidth * depthHeight, reinterpret_cast<UINT16*>(depthBufferMat.data), depthWidth * depthHeight, &colorSpacePoints[0]);
if (SUCCEEDED(hResult)) {
coordinateMapperMat = cv::Scalar(0, 0, 0, 0);
for (int y = 0; y < depthHeight; y++) {
for (int x = 0; x < depthWidth; x++) {
unsigned int index = y * depthWidth + x;
ColorSpacePoint point = colorSpacePoints[index];
int colorX = static_cast<int>(std::floor(point.X + 0.5));
int colorY = static_cast<int>(std::floor(point.Y + 0.5));
unsigned short depth = depthBufferMat.at<unsigned short>(y, x);
if ((colorX >= 0) && (colorX < colorWidth) && (colorY >= 0) && (colorY < colorHeight)/* && ( depth >= minDepth ) && ( depth <= maxDepth )*/) {
coordinateMapperMat.at<cv::Vec4b>(y, x) = colorBufferMat.at<cv::Vec4b>(colorY, colorX);
}
}
}
}
}
if (SUCCEEDED(hResult))
{
//Particle Filter Start from here
frame = &(IplImage)coordinateMapperMat;//transorm mat into IplImage
if (image == 0)
{
// initialize variables and allocate buffers
image = cvCreateImage(cvGetSize(frame), 8, 4);//every pixel has 4 channels
image->origin = frame->origin;
result = cvCreateImage(cvGetSize(frame), 8, 4);
result->origin = frame->origin;
hsv = cvCreateImage(cvGetSize(frame), 8, 3);
hue = cvCreateImage(cvGetSize(frame), 8, 1);
sat = cvCreateImage(cvGetSize(frame), 8, 1);
histimg_ref = cvCreateImage(cvGetSize(frame), 8, 3);
histimg_ref->origin = frame->origin;
cvZero(histimg_ref);
histimg = cvCreateImage(cvGetSize(frame), 8, 3);
histimg->origin = frame->origin;
cvZero(histimg);
bin_w = histimg_ref->width / BIN;
bin_h = histimg_ref->height / BIN;
data1.sample_t = reinterpret_cast<Region *> (malloc(sizeof(Region)* SAMPLE));
data1.sample_t_1 = reinterpret_cast<Region *> (malloc(sizeof(Region)* SAMPLE));
data1.sample_weight = reinterpret_cast<double *> (malloc(sizeof(double)* SAMPLE));
data1.accum_weight = reinterpret_cast<double *> (malloc(sizeof(double)* SAMPLE));
}
cvCopy(frame, image);
cvCopy(frame, result);
cvCvtColor(image, hsv, CV_BGR2HSV);//image ~ hsv
if (tracking)
{
//v_max = 0.0;
cvSplit(hsv, hue, 0, 0, 0);//hsv->hue
cvSplit(hsv, 0, 0, sat, 0);//hsv-saturation
if (selecting)
{
// get the selected target area
//ref_v_max = 0.0;
area.width = abs(P_org.x - P_end.x);
area.height = abs(P_org.y - P_end.y);
area.x = MIN(P_org.x, P_end.x);
area.y = MIN(P_org.y, P_end.y);
cvZero(histimg_ref);
// build reference histogram
cvSetImageROI(hue, area);
cvSetImageROI(sat, area);
// zero reference histogram
for (i = 0; i < BIN; i++)
for (j = 0; j < BIN; j++)
hist_ref[i][j] = 0.0;
// calculate reference histogram
for (i = 0; i < area.height; i++)
{
for (j = 0; j < area.width; j++)
{
im_hue = cvGet2D(hue, i, j);
im_sat = cvGet2D(sat, i, j);
k = int(im_hue.val[0] / STEP_HUE);
h = int(im_sat.val[0] / STEP_SAT);
hist_ref[k][h] = hist_ref[k][h] + 1.0;
}
}
// rescale the value of each bin in the reference histogram
// and show it as an image
for (i = 0; i < BIN; i++)
{
for (j = 0; j < BIN; j++)
{
hist_ref[i][j] = hist_ref[i][j] / (area.height*area.width);
}
}
cvResetImageROI(hue);
cvResetImageROI(sat);
// initialize tracking and samples
track_win = area;
Initdata(track_win);
track_win_last = track_win;
// set up flag of tracking
selecting = 0;
}
// sample propagation and weighting
track_win = ImProcess(hue, sat, hist_ref, track_win_last);
FrameNumber++;
track_win_last = track_win;
cvZero(histimg);
// draw the one RED bounding box
cvRectangle(image, cvPoint(track_win.x, track_win.y), cvPoint(track_win.x + track_win.width, track_win.y + track_win.height), CV_RGB(255, 0, 0), 2);
printf("width = %d, height = %d\n", track_win.width, track_win.height);
//save certian images
if (FrameNumber % 10 == 0)
{
if (FrameNumber / 10 == 1) cvSaveImage("./imageout1.jpg", image);
if (FrameNumber / 10 == 2) cvSaveImage("./imageout2.jpg", image);
if (FrameNumber / 10 == 3) cvSaveImage("./imageout3.jpg", image);
if (FrameNumber / 10 == 4) cvSaveImage("./imageout4.jpg", image);
if (FrameNumber / 10 == 5) cvSaveImage("./imageout5.jpg", image);
if (FrameNumber / 10 == 6) cvSaveImage("./imageout6.jpg", image);
if (FrameNumber / 10 == 7) cvSaveImage("./imageout7.jpg", image);
if (FrameNumber / 10 == 8) cvSaveImage("./imageout8.jpg", image);
}
//save certian images
if (FrameNumber % 10 == 0)
{
if (FrameNumber / 10 == 1) cvSaveImage("./resultout1.jpg", result);
if (FrameNumber / 10 == 2) cvSaveImage("./resultout2.jpg", result);
if (FrameNumber / 10 == 3) cvSaveImage("./resultout3.jpg", result);
if (FrameNumber / 10 == 4) cvSaveImage("./resultout4.jpg", result);
if (FrameNumber / 10 == 5) cvSaveImage("./resultout5.jpg", result);
if (FrameNumber / 10 == 6) cvSaveImage("./resultout6.jpg", result);
if (FrameNumber / 10 == 7) cvSaveImage("./resultout7.jpg", result);
if (FrameNumber / 10 == 8) cvSaveImage("./resultout8.jpg", result);
}
//draw a same bounding box in DepthImage
rectangle(DepthImage, track_win, CV_RGB(255, 0, 0), 2);
//******************************************************Geodesic Distance***************************************************************************************
//Point propagation and weight
if (PointTrack == 1)
{
if (PointSelect == 1)//only visit once
{
// initialize tracking and samples
for (int i = 0; i < SAMPLE; i++)
{
point[i].x_1 = P_track.x;
point[i].y_1 = P_track.y;
point[i].z_1 = depthData[P_track.x + P_track.y * 512];
point[i].x_1_prime = 0.0;
point[i].y_1_prime = 0.0;
}
refeFlag = 1;
p_win = P_track;
//p_transtart is the start point of the surface mesh
P_transtart.x = track_win.x;
P_transtart.y = track_win.y;
PointSelect = 0;
}
//construct the graph(mesh)
ConstructMesh(depthData, adjlist, P_transtart,track_win.width,track_win.height);
//calculate shortest path
vector<int> vertexDist;
vertexDist.resize(track_win.width*track_win.height);
ShortestPath(P_extre, adjlist, vertexDist);
cvCircle(image, P_extre, 3, CV_RGB(0, 255, 0),1);
//generate the refernce distance for comparing
if (refeFlag > 0)
{
cvCircle(image, p_win, 3, CV_RGB(0, 0, 255), 1);
int track = abs(P_transtart.x - P_track.x) + track_win.width * abs(P_transtart.y - P_track.y);
referDistance = vertexDist[track];
refeFlag = 0;
}
//samples propagation
PredictPoint(p_win);
//get geodesic distance for each sample.
//find the sample which have most similar distance to the refernce distance
float Dist, AbsDist, WinDist, minAbsDist = 10000;
int number,sum=0,count=0;
for (int i = 0; i < SAMPLE; i++)
{
int t = abs(P_transtart.x - point[i].x) + track_win.width * abs(P_transtart.y - point[i].y);
if (adjlist[t].v == false) { count++; continue; }
int refer = abs(point[i].x - P_transtart.x) + track_win.width * abs(point[i].y - P_transtart.y);
Dist = vertexDist[refer];
AbsDist = fabs(referDistance - Dist);
//point[i].SampleWeight = AbsDist;
//point[i].AccumWeight = sum;
//sum = sum + AbsDist;
if (AbsDist < minAbsDist) { AbsDist = Dist; number = i; WinDist = Dist; }
}
//for (int i = 0; i < SAMPLE; i++)
//{
// point[i].SampleWeight = point[i].SampleWeight / sum;
// point[i].AccumWeight = point[i].AccumWeight / sum;
//}
printf("referDist = %f, winDist = %f, discardPoints = %d\n", referDistance, WinDist,count);
p_win_last = p_win;
p_win.x = point[number].x;
p_win.y = point[number].y;
//samples re-location
float deltaX = p_win.x - p_win_last.x;
float deltaY = p_win.y - p_win_last.y;
UpdatePoint(number, deltaX, deltaY);
cvCircle(image, p_win, 5, CV_RGB(0, 0, 0));
}
// //****************************************************************************************************************************************
}
// if still selecting a target, show the RED selected area
else cvRectangle(image, P_org, P_end, CV_RGB(255, 0, 0), 1);
}
imshow("Depth", DepthImage);
cvShowImage("Color Probabilistic Tracking - Samples", image);
cvShowImage("Color Probabilistic Tracking - Result", result);
SafeRelease(pColorFrame);
SafeRelease(pDepthFrame);
//SafeRelease(pBodyIndexFrame);
cv::imshow("Color", colorMat);
cv::imshow("Depth", DepthImage);
cv::imshow("CoordinateMapper", coordinateMapperMat);
//END OF THE TIME POINT
t = ((double)getTickCount() - t) / getTickFrequency();
t = 1 / t;
//cout << "FPS:" << t << "FrameNumber\n" << FrameNumebr<< endl;
printf("FPS:%f Frame:%d \n\n", t, FrameNumber);
if (cv::waitKey(30) == VK_ESCAPE) {
break;
}
}
SafeRelease(pColorSource);
SafeRelease(pDepthSource);
//SafeRelease(pBodyIndexSource);
SafeRelease(pColorReader);
SafeRelease(pDepthReader);
//SafeRelease(pBodyIndexReader);
SafeRelease(pColorDescription);
SafeRelease(pDepthDescription);
SafeRelease(pCoordinateMapper);
if (pSensor) {
pSensor->Close();
}
SafeRelease(pSensor);
cv::destroyAllWindows();
cvReleaseImage(&image);
cvReleaseImage(&result);
cvReleaseImage(&histimg_ref);
cvReleaseImage(&histimg);
cvReleaseImage(&hsv);
cvReleaseImage(&hue);
cvReleaseImage(&sat);
cvDestroyWindow("Color Probabilistic Tracking - Samples");
cvDestroyWindow("Color Probabilistic Tracking - Result");
return 0;
}