test

3_PointCloud/main.cpp

@@ -76,7 +76,19 @@ static bool writeFile = false;
 static bool captureFrame = false;
 int lastX, lastY;
 
-bool enableCustomCloudFilter = false;
+
+float degreeToRadioan(float degree) {
+	return degree*M_PI / 180.0;
+}
+
+//Filter Settings
+	//Custom
+//float angleDegrees = 20.0f;
+float maxNormalAngle = degreeToRadioan(25.0f);
+bool enableDepthFilter = false;
+bool enableNormalFilter = true;//true;
+	//Bilateral / FastDepthNoise
+float repairThreshold = 100.0f;
 
 
 void DrawCoordinateSystem() {
@@ -158,57 +170,6 @@ bool initKinect() {
 	}
 }
 
-//bilateral Filter
- //https://github.com/anlcnydn/bilateral/blob/master/bilateral_filter.cpp
-float distance(int x, int y, int i, int j) {
-	return float(sqrt(pow(x - i, 2) + pow(y - j, 2)));
-}
-
-double gaussian(float x, double sigma) {
-	return exp(-(pow(x, 2)) / (2 * pow(sigma, 2))) / (2 * M_PI * pow(sigma, 2));
-
-}
-
-void applyBilateralFilter(unsigned short* source, unsigned short* filteredImage, int x, int y, int diameter, double sigmaI, double sigmaS) {
-	double iFiltered = 0;
-	double wP = 0;
-	int neighbor_x = 0;
-	int neighbor_y = 0;
-	int half = diameter / 2;
-	//why
-	const int width = 512;
-	const int height = 424;
-
-	for (int i = 0; i < diameter; i++) {
-		for (int j = 0; j < diameter; j++) {
-			neighbor_x = x - (half - i);
-			neighbor_y = y - (half - j);
-			double gi = gaussian(source[neighbor_x + width* neighbor_y] - source[x + width* y], sigmaI);
-			double gs = gaussian(distance(x, y, neighbor_x, neighbor_y), sigmaS);
-			double w = gi * gs;
-			iFiltered = iFiltered + source[neighbor_x + width* neighbor_y] * w;
-			wP = wP + w;
-		}
-	}
-	iFiltered = iFiltered / wP;
-	filteredImage[x + width* y] = iFiltered;
-
-
-}
-
-void bilateralFilterOwn(unsigned short* source, unsigned short* filterd, int diameter, double sigmaI, double sigmaS) {
-	const int width = 512;
-	const int height = 424;
-
-	for (int i = 4; i < width - 12; i++) {
-		for (int j = 4; j < height - 12; j++) {
-			applyBilateralFilter(source, filterd, i, j, diameter, sigmaI, sigmaS);
-		}
-	}
-}
-//bilateral Filter end
-
-
 void getDepthData(IMultiSourceFrame* frame) {
 	IDepthFrame* depthframe;
 	IDepthFrameReference* frameref = NULL;
@@ -241,7 +202,7 @@ void getDepthData(IMultiSourceFrame* frame) {
 	cv::Mat FilerdDepth;
 	FilerdDepth.create(OriginalDepthFrame.size(), OriginalDepthFrame.type());
 	
-	FastDepthNoiseRemoval filter(3,0.0f,3, 100.0f);
+	FastDepthNoiseRemoval filter(3,0.0f,3, repairThreshold);
 	filter.setKillBorder(true);
 	filter.setInput(OriginalDepthFrame);
 	filter.filter(OriginalDepthFrame);
@@ -261,13 +222,17 @@ void getDepthData(IMultiSourceFrame* frame) {
 		{
 			unsigned int i = x + y* width;
 			framePositions[i] = glm::vec3(depth2xyz[i].X, depth2xyz[i].Y, depth2xyz[i].Z);
-			if (enableCustomCloudFilter && x > 0 && y > 0 && x < width - 1 && y < height - 1) {
+
+			if ((enableNormalFilter || enableDepthFilter) && x > 0 && y > 0 && x < width - 1 && y < height - 1) {
+				if (enableNormalFilter) {
 					//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;
+					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);
+				}
+				if (enableDepthFilter) {
 					// dz tiefenwert
 					float z = depth2xyz[i].Z;
 					if (true) {
@@ -279,9 +244,9 @@ void getDepthData(IMultiSourceFrame* frame) {
 					else {
 						frameDepthDifference[i] = 0.0f;
 					}
-				//if (x == width / 2 && y == height / 2) std::cout << "Frame Difference " << frameDepthDifference[i]<< std::endl;
 				}
-			else {
+				//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;
 			}
@@ -336,6 +301,7 @@ void getRgbData(IMultiSourceFrame* frame) {
 		}
 		else {
 			
+
 			if (depth2xyz[i].Z > maxTrackingKinect || depth2xyz[i].Z < minTrackingKinect) {
 				*fdest++ = 0;
 				*fdest++ = 255;
@@ -345,8 +311,7 @@ void getRgbData(IMultiSourceFrame* frame) {
 				continue;
 			}
 
-			if (enableCustomCloudFilter) {
-				if ( frameDepthDifference[i] > 100.0) {
+			if (enableDepthFilter &&  frameDepthDifference[i] > 100.0) {
 					*fdest++ = 255;
 					*fdest++ = 140;
 					*fdest++ = 0;
@@ -355,8 +320,10 @@ void getRgbData(IMultiSourceFrame* frame) {
 					continue;
 				}
 
+
+			if (enableNormalFilter) {
 				float angle = dot(frameNormals[i], glm::vec3(0.0, 0.0, 1.0));
-				if ( angle < 0.05) {
+				if ( angle < maxNormalAngle) {
 					*fdest++ = 0;
 					*fdest++ = 0;
 					*fdest++ = 255;
@@ -364,8 +331,8 @@ void getRgbData(IMultiSourceFrame* frame) {
 					*fvalid++ = false;
 					continue;
 				}
-
 			}
+
 			int idx = (int)p.X + colorwidth*(int)p.Y;
 			*fdest++ = rgbimage[4 * idx + 0];
 			*fdest++ = rgbimage[4 * idx + 1];
@@ -413,10 +380,14 @@ void processCurrentFrameForExport() {
 
 	for (unsigned int i = 0; i < width*height; i++) {
 		if (frameValidPoints[i]) {
-			glm::vec3 positionLocal = framePositions[i];
-			glm::vec3 positionGlobal = glm::vec3(*currentControlerPos * controlerToKinect * glm::vec4(positionLocal, 1.0f));
+			glm::vec3 position = framePositions[i];
+			glm::vec3 positionGlobal = glm::vec3(*currentControlerPos * controlerToKinect * glm::vec4(position, 1.0f));
+
+			if (isinf(position.x) || isinf(position.y) || isinf(position.z))
+				continue;
+
+			
 			
-			glm::vec3 position = positionLocal;
 			if (position.x < minx) minx = position.x;
 			if (position.x > maxx) maxx = position.x;
 			if (position.y < miny) miny = position.y;
@@ -684,9 +655,7 @@ void captureNextFrame(vr::VREvent_t event)
 	}
 }
 
-float degreeToRadioan(float degree) {
-	return degree*M_PI / 180.0;
-}
+
 
 
 void printVector(glm::vec3 vector)

3_PointCloud/tileset.json

@@ -16,6 +16,12 @@
     "refine": "add",
     "children": [
       {
+	    "transform": [
+			1.0, 0.0, 0.0, 0.0,
+			0.0, 1.0, 0.0, 0.0,
+			0.0, 0.0, 1.0, 0.0,
+			0.0, 0.0, 0.0, 1.0
+		  ],
         "boundingVolume": {
           "sphere": [
             0,