kinect-terrain-tracker/KinectProgram.cpp at main · T-Lind/kinect-terrain-tracker · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
#include "KinectProgram.h"

const void KinectProgram::Init()
{
	CalibrateFloorDetection();

	++frame_num;
}

const void KinectProgram::UpdateDepth(int index, USHORT depth, RGBQUAD* pRGBX)
{
	using std::cos;
	using std::sin;

	float x_angle = PixelXAngle(GetXValue1D(index));
	float y_angle = PixelYAngle(GetYValue1D(index)) + ToRadians(CAMERA_ANGLE);

	depths[GetYValue1D(index)][GetXValue1D(index)] = depth;
	pixels[GetYValue1D(index)][GetXValue1D(index)] = pRGBX;

	positions[index].x = -sin(x_angle) * cos(y_angle) * depth;
	positions[index].y = cos(x_angle) * cos(y_angle) * depth;
	positions[index].z = depth * sin(y_angle);
	positions[index].pixel = pRGBX;
}

const void KinectProgram::AnalyzeFrame()
{
	if (frame_num  == 0)
	{
		Init();
		return;
	}

	// Mark the center of the frame
	pixels[DEPTH_FRAME_HEIGHT/2][DEPTH_FRAME_WIDTH/2]->rgbBlue = 255;

	FloorDetection();

	// Draw a box where the camera is, 5x5
	for (int y = 0; y < 5; y++) {
		for (int x = 0; x < 5; x++) {
			pixels[DEPTH_FRAME_HEIGHT - 1 - y][DEPTH_FRAME_WIDTH / 2 - 2 + x]->rgbRed = 255;
		}
	}

	++frame_num;
}

const void KinectProgram::BasicFloorDetection(Coordinate& coordinate)
{

	if (coordinate.z < -camera_height)
	{
		USHORT x_index = TopViewPlotDepthX(coordinate.x);
		USHORT y_index = TopViewPlotDepthY(coordinate.y);

		if (x_index > 0 && x_index < DEPTH_FRAME_WIDTH && y_index > 0 && y_index < DEPTH_FRAME_HEIGHT) {
			pixels[y_index][x_index]->rgbGreen = (200000 / (coordinate.y) - 15);
		}
	}
}

const void KinectProgram::CalibrateFloorDetection()
{
	int sum = 0;
	int cnt = 0;
	for (int i = 0; i < sizeof(positions) / sizeof(Coordinate);i++)
	{
		const Coordinate coordinate = positions[i];
		if (coordinate.z < -camera_height)
		{
			sum += abs(coordinate.z);
			++cnt;

			// Draw floor
			USHORT x_index = TopViewPlotDepthX(coordinate.x);
			USHORT y_index = TopViewPlotDepthY(coordinate.y);

			if (x_index > 0 && x_index < DEPTH_FRAME_WIDTH && y_index > 0 && y_index < DEPTH_FRAME_HEIGHT)
			{
				pixels[y_index][x_index]->rgbGreen = (200000 / (coordinate.y) - 15);
			}

		}
	}

	floor_average = abs((float)sum / cnt);
}

const void KinectProgram::FloorDetection()
{
	if (frame_num % 100 == 0)
	{
		CalibrateFloorDetection();
	}

	for (int i = 0; i < sizeof(positions) / sizeof(Coordinate); i++)
	{
		const Coordinate coordinate = positions[i];
		if (floor_average - MARGIN_OF_ERROR < abs(coordinate.z) && abs(coordinate.z) < floor_average + MARGIN_OF_ERROR)
		{
			// Draw floor
			USHORT x_index = TopViewPlotDepthX(coordinate.x);
			USHORT y_index = TopViewPlotDepthY(coordinate.y);

			if (x_index > 0 && x_index < DEPTH_FRAME_WIDTH && y_index > 0 && y_index < DEPTH_FRAME_HEIGHT)
			{
				pixels[y_index][x_index]->rgbGreen = (200000 / (coordinate.y) - 15);
			}

		}
	}

}

const int KinectProgram::TopViewPlotDepthX(float x_distance)
{
	float distance_percent = x_distance / (SIZE_OF_TOP_VIEW_X);

	if (distance_percent > 1)
	{
		distance_percent = 1;
	}

	return (int)(distance_percent * (DEPTH_FRAME_WIDTH / 2) + (DEPTH_FRAME_WIDTH / 2));
}

const int KinectProgram::TopViewPlotDepthY(float y_distance)
{
	float distance_percent = y_distance / (SIZE_OF_TOP_VIEW_Y);

	if (distance_percent > 1)
	{
		distance_percent = 1;
	}

	return (int)(DEPTH_FRAME_HEIGHT - distance_percent * DEPTH_FRAME_HEIGHT);
}