added depth scanning

3_PointCloud/KinectTutorial3.sln

 
-Microsoft Visual Studio Solution File, Format Version 11.00
-# Visual Studio 2010
+Microsoft Visual Studio Solution File, Format Version 12.00
+# Visual Studio 14
+VisualStudioVersion = 14.0.25420.1
+MinimumVisualStudioVersion = 10.0.40219.1
 Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "KinectTutorial3", "KinectTutorial3.vcxproj", "{AFFAABC2-707F-4CE0-87D1-CCA550249103}"
 EndProject
 Global

3_PointCloud/main.cpp

@@ -29,6 +29,7 @@ CameraSpacePoint depth2xyz[width*height];			 // Maps depth pixels to 3d coordina
 //buffers of the current frame
 glm::vec3 framePositions[width*height];
 glm::vec3 frameNormals[width*height];
+float frameDepthDifference[width*height];
 unsigned char frameColors[width*height * 3];
 bool frameValidPoints[width*height];
 
@@ -64,7 +65,7 @@ std::vector<glm::vec3> controllerPositions;
 
 
 
-bool toggleCam = false;
+bool toggleCam = true;
 Camera mainCam;
 
 static bool trackControllerPos = true;
@@ -178,17 +179,32 @@ void getDepthData(IMultiSourceFrame* frame) {
 			unsigned int i = x + y* width;
 			framePositions[i] = glm::vec3(depth2xyz[i].X, depth2xyz[i].Y, depth2xyz[i].Z);
 			if (x > 0 && y > 0 && x < width - 1 && y < height - 1) {
-
-				float dzdx = ( curr[x + 1 + width* y] - curr[x - 1 + width* y]) / 2.0;
-				float dzdy = ( curr[x + width* (y+1)] - curr[x + width* (y - 1)]) / 2.0;
+				//normalen
+				float dzdx = ( curr[x + 1 + width * y] - curr[x - 1 + width * y]) / 2.0;
+				float dzdy = ( curr[x + width * (y+1)] - curr[x + width * (y - 1)]) / 2.0;
 
 				glm::vec3 d(-dzdx, -dzdy, 1.0f);
 				frameNormals[i] = normalize(d);
+				// dz tiefenwert
+				float z = depth2xyz[i].Z;
+				if (true) {
+					frameDepthDifference[i] = (depth2xyz[x + 1 + width * (y)].Z - z)*(depth2xyz[x + 1 + width * (y)].Z - z)
+						+ (depth2xyz[x - 1 + width * (y)].Z - z)*(depth2xyz[x - 1 + width * (y)].Z - z)
+						+ (depth2xyz[x + width * (y + 1)].Z - z)*(depth2xyz[x + width * (y + 1)].Z - z)
+						+ (depth2xyz[x + width * (y - 1)].Z - z)*(depth2xyz[x + width * (y - 1)].Z - z);
+				}
+				else {
+					frameDepthDifference[i];
+				}
+				//if (x == width / 2 && y == height / 2) std::cout << "Frame Difference " << frameDepthDifference[i]<< std::endl;
 			}
 			else {
 				frameNormals[i] = glm::vec3(0.0, 0.0, 1.0);
+				frameDepthDifference[i] = 0.0f;
 			}
 			
+				
+			
 		}
 	}
 
@@ -245,13 +261,24 @@ void getRgbData(IMultiSourceFrame* frame) {
 				*fvalid++ = false;
 				continue;
 			}
+
+			if (frameDepthDifference[i] > 100.0) {
+				*fdest++ = 255;
+				*fdest++ = 140;
+				*fdest++ = 0;
+
+				*fvalid++ = false;
+				continue;
+			}
+
 			float angle = dot(frameNormals[i], glm::vec3(0.0, 0.0, 1.0));
-			if (angle < 0.1) {
+			if (angle < 0.05) {
 				*fdest++ = 0;
 				*fdest++ = 0;
 				*fdest++ = 255;
 
 				*fvalid++ = false;
+				continue;
 			}
 			else {
 				int idx = (int)p.X + colorwidth*(int)p.Y;
@@ -440,9 +467,13 @@ void mainRenderLoop() {
 	glLoadIdentity();
 	//rotateCamera(); //todo
 	if (toggleCam && trackControllerPos) {
-		glMultMatrixf(glm::value_ptr(*currentControlerPos));
-		glMultMatrixf(glm::value_ptr(controlerToKinect));
-		glRotatef(180.0, 0.0, 0.0, 1.0);
+
+		glTranslatef(0.0, -0.3, -1.0);
+		glRotatef(10.0, 1.0, 0.0, 0.0);
+		glRotatef(180.0, 0.0, 1.0, 0.0);
+		glMultMatrixf(glm::value_ptr(glm::inverse(controlerToKinect)));
+		glMultMatrixf(glm::value_ptr(glm::inverse(*currentControlerPos)));
+		//
 	}
 	else {
 		mainCam.applyMatrix();
@@ -636,9 +667,9 @@ int main(int argc, char* argv[]) {
 	controlerToKinect = glm::rotate(controlerToKinect, degreeToRadioan(180.0f), glm::vec3(0.0f, 1.0f, 0.0f));
 	//controlerToKinect = glm::rotate(controlerToKinect, degreeToRadioan(180.0f), glm::vec3(0.0f, 0.0f, 1.0f));
 
-	float dx = 0.0;// KinectOriginOffsetX - KinectSizeX / 2;
-	float dy = KinectSizeY / 2.0f + 0.004 /*HAlterung Platte*/ + 0.011 /* Conroller*/;
-	float dz = 0.0; //-0.023 + 0.02; // 0.045f /*= versatz auf der Halterung; Distanz zur Halterung:*/ + (KinectSizeZ - (0.066 /*halterung z*/ - 0.012 /*halterung berstand*/));
+	float dx = 0.00;// KinectOriginOffsetX - KinectSizeX / 2;
+	float dy = KinectSizeY / 2.0f + 0.004 /*HAlterung Platte*/ + 0.011 /* Conroller*/;// -0.01;
+	float dz = 0.00; //-0.023 + 0.02; // 0.045f /*= versatz auf der Halterung; Distanz zur Halterung:*/ + (KinectSizeZ - (0.066 /*halterung z*/ - 0.012 /*halterung berstand*/));
 
 	controlerToKinect = glm::translate(controlerToKinect, (glm::vec3(dx, -dy, dz)));
 
@@ -745,18 +776,19 @@ void setupGrid()
 	{ //init Grid
 		std::vector<glm::vec3> grid;
 		std::vector<unsigned char> gridcolors;
-
 		for (int i = -20; i < 21; i++)
 		{
 			grid.push_back(glm::vec3(20, 0, i));
 			grid.push_back(glm::vec3(-20, 0, i));
-			gridcolors.push_back(255);
+			
 			gridcolors.push_back(i == 0 ? 0 : 255);
 			gridcolors.push_back(i == 0 ? 0 : 255);
-
 			gridcolors.push_back(255);
+
+			
 			gridcolors.push_back(i == 0 ? 0 : 255);
 			gridcolors.push_back(i == 0 ? 0 : 255);
+			gridcolors.push_back(255);
 
 		}
 		for (int i = -20; i < 21; i++)
@@ -773,12 +805,13 @@ void setupGrid()
 		}
 		grid.push_back(glm::vec3(0, -20, 0));
 		grid.push_back(glm::vec3(0, 20, 0));
-		gridcolors.push_back(255);
+		
 		gridcolors.push_back(0);
+		gridcolors.push_back(255);
 		gridcolors.push_back(0);
 
-		gridcolors.push_back(255);
 		gridcolors.push_back(0);
+		gridcolors.push_back(255);
 		gridcolors.push_back(0);
 
 		glGenBuffers(1, &gridData.vboID);