Filter

3_PointCloud/main.cpp

@@ -227,20 +227,30 @@ void getDepthData(IMultiSourceFrame* frame) {
 	//bilateralFilterOwn(buf, filterd, 5, 12.0, 16.0);
 	//const unsigned short* curr = (const unsigned short*)filterd;
 
-	cv::Mat OriginalDepthFrame = cv::Mat(width, height, CV_16UC1, &buf);
+	cv::Mat OriginalDepthFrame(height, width, CV_16UC1);// , &buf);
+	const unsigned short* curr = (const unsigned short*)buf;
+
+	for (unsigned int y = 0; y < height; y++)
+	{
+		for (unsigned int x = 0; x < width; x++)
+		{
+			OriginalDepthFrame.ptr<unsigned short>(y)[x] = curr[x + y* width];
+		}
+	}
+
 	cv::Mat FilerdDepth;
 	FilerdDepth.create(OriginalDepthFrame.size(), OriginalDepthFrame.type());
 	
-	FastDepthNoiseRemoval filter(3,0.0f,3, 1.0f);
+	FastDepthNoiseRemoval filter(3,0.0f,3, 100.0f);
 	filter.setKillBorder(true);
 	filter.setInput(OriginalDepthFrame);
-	//filter.filter(OriginalDepthFrame);
-	
+	filter.filter(OriginalDepthFrame);
+//	cv::imshow("Input", OriginalDepthFrame);
 	//cv::bilateralFilter(OriginalDepthFrame, FilerdDepth, 15, 80, 80);
 
 
 	//const unsigned short* curr = (const unsigned short*)buf;
-	const unsigned short* curr = (const unsigned short*)OriginalDepthFrame.data;
+	curr = (const unsigned short*)OriginalDepthFrame.data;
 
 	// Write vertex coordinates
 	mapper->MapDepthFrameToCameraSpace(width*height, curr, width*height, depth2xyz);
@@ -325,7 +335,7 @@ void getRgbData(IMultiSourceFrame* frame) {
 			*fvalid++ = false;
 		}
 		else {
-			
+
 			if (depth2xyz[i].Z > maxTrackingKinect || depth2xyz[i].Z < minTrackingKinect) {
 				*fdest++ = 0;
 				*fdest++ = 255;
@@ -335,25 +345,27 @@ void getRgbData(IMultiSourceFrame* frame) {
 				continue;
 			}
 
-			if (enableCustomCloudFilter && frameDepthDifference[i] > 100.0) {
-				*fdest++ = 255;
-				*fdest++ = 140;
-				*fdest++ = 0;
+			if (enableCustomCloudFilter) {
+				if ( frameDepthDifference[i] > 100.0) {
+					*fdest++ = 255;
+					*fdest++ = 140;
+					*fdest++ = 0;
 
-				*fvalid++ = false;
-				continue;
-			}
+					*fvalid++ = false;
+					continue;
+				}
 
-			float angle = dot(frameNormals[i], glm::vec3(0.0, 0.0, 1.0));
-			if (enableCustomCloudFilter && angle < 0.05) {
-				*fdest++ = 0;
-				*fdest++ = 0;
-				*fdest++ = 255;
+				float angle = dot(frameNormals[i], glm::vec3(0.0, 0.0, 1.0));
+				if ( angle < 0.05) {
+					*fdest++ = 0;
+					*fdest++ = 0;
+					*fdest++ = 255;
 
-				*fvalid++ = false;
-				continue;
-			}
-			
+					*fvalid++ = false;
+					continue;
+				}
+
+		}
 			int idx = (int)p.X + colorwidth*(int)p.Y;
 			*fdest++ = rgbimage[4 * idx + 0];
 			*fdest++ = rgbimage[4 * idx + 1];