camera 2 and normals

3_PointCloud/main.cpp

@@ -28,6 +28,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];
 unsigned char frameColors[width*height * 3];
 bool frameValidPoints[width*height];
 
@@ -63,7 +64,7 @@ std::vector<glm::vec3> controllerPositions;
 
 
 
-
+bool toggleCam = false;
 Camera mainCam;
 
 static bool trackControllerPos = true;
@@ -165,12 +166,34 @@ void getDepthData(IMultiSourceFrame* frame) {
 	unsigned int sz;
 	unsigned short* buf;
 	depthframe->AccessUnderlyingBuffer(&sz, &buf);
+	const unsigned short* curr = (const unsigned short*)buf;
 
 	// Write vertex coordinates
 	mapper->MapDepthFrameToCameraSpace(width*height, buf, width*height, depth2xyz);
 
+	for (unsigned int y = 0; y < height; y++)
+	{
+		for (unsigned int x = 0; x < width; x++)
+		{
+			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;
+
+				glm::vec3 d(-dzdx, -dzdy, 1.0f);
+				frameNormals[i] = normalize(d);
+			}
+			else {
+				frameNormals[i] = glm::vec3(0.0, 0.0, 1.0);
+			}
+			
+		}
+	}
+
 	for (unsigned int i = 0; i < sz; i++) {
-		framePositions[i] = glm::vec3(depth2xyz[i].X, depth2xyz[i].Y, depth2xyz[i].Z);
+		
 	}
 
 	// Fill in depth2rgb map
@@ -206,19 +229,29 @@ void getRgbData(IMultiSourceFrame* frame) {
 	for (int i = 0; i < width*height; i++) {
 		ColorSpacePoint p = depth2rgb[i];
 		// Check if color pixel coordinates are in bounds
-		if (p.X < 0 || p.Y < 0 || p.X > colorwidth || p.Y > colorheight) {
+		if (p.X < 1 || p.Y < 1 || p.X > colorwidth-1 || p.Y > colorheight-1) {
 			*fdest++ = 255;
 			*fdest++ = 0;
 			*fdest++ = 0;
 			*fvalid++ = false;
 		}
 		else {
+			
 			if (depth2xyz[i].Z > maxTrackingKinect || depth2xyz[i].Z < minTrackingKinect) {
 				*fdest++ = 0;
 				*fdest++ = 255;
 				*fdest++ = 0;
 
 				*fvalid++ = false;
+				continue;
+			}
+			float angle = dot(frameNormals[i], glm::vec3(0.0, 0.0, 1.0));
+			if (angle < 0.1) {
+				*fdest++ = 0;
+				*fdest++ = 0;
+				*fdest++ = 255;
+
+				*fvalid++ = false;
 			}
 			else {
 				int idx = (int)p.X + colorwidth*(int)p.Y;
@@ -406,7 +439,15 @@ void mainRenderLoop() {
 	glMatrixMode(GL_MODELVIEW);
 	glLoadIdentity();
 	//rotateCamera(); //todo
-	mainCam.applyMatrix();
+	if (toggleCam && trackControllerPos) {
+		glMultMatrixf(glm::value_ptr(*currentControlerPos));
+		glMultMatrixf(glm::value_ptr(controlerToKinect));
+		glRotatef(180.0, 0.0, 0.0, 1.0);
+	}
+	else {
+		mainCam.applyMatrix();
+	}
+
 	//glTranslatef(0.0, 5, 0.0);
 
 	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
@@ -493,6 +534,9 @@ void keyPressed(unsigned char key, int x, int y)
 		colors.clear();
 		exportData.size = 0;
 		break;
+	case 'q':
+		toggleCam = !toggleCam;
+		break;
 	case 27: // Escape key
 		exit(0);
 		break;