Calculated the grid mid after the grid node (hole center) rejection is done...

Calculated the grid mid after the grid node (hole center) rejection is done for a more stable grid mid when the grid nodes are very noisy.

VRKinectReg/Plugins/VRKinectRegistration/Source/VRKinectRegistration/Private/GridDetector.cpp

@@ -1051,17 +1051,8 @@ GridDetectionResult GridDetector::detectGrid(PointCloudImage pointCloud) {
 		if (planeN.Z > 0)
 			planeN = planeN * -1;
 
-		std::vector<FVector> gridMidPoints = 
-			calculateGridMidPoints(pointCloud.data,pointCloud.sourceImageWidth, std::pair<FVector, FVector>(planeN, planeP),scanlinesOfHoles);
-
-		// Calculate weighted:
-		FVector gridMidWeighted = mean(gridMidPoints);
-
-		// Only use grid points in specific radius for more precise mid prediction:
-		std::vector<FVector> midPointsInRadius = filterByDistance(gridMidWeighted, gridMidPoints, MID_POINT_PREDICTION_MAXIMAL_RADIUS);
-
-		// Calculate again:
-		gridMidWeighted = mean(midPointsInRadius);
+		std::vector<FVector> gridMidPoints =
+			calculateGridMidPoints(pointCloud.data, pointCloud.sourceImageWidth, std::pair<FVector, FVector>(planeN, planeP), scanlinesOfHoles);
 
 		// Generate rotation-clusters of near-by vectors:
 		std::vector<std::vector<std::tuple<float, int, int>>> clusters = clusterGridMidPointsByRotation(gridMidPoints);
@@ -1081,6 +1072,18 @@ GridDetectionResult GridDetector::detectGrid(PointCloudImage pointCloud) {
 		FVector axis2 = (gridMidPoints[std::get<2>(cluster1Mean)] - gridMidPoints[std::get<1>(cluster1Mean)]);
 		axis2.Normalize();
 
+		// Filter all points which are not connected to another point over minimal 1 good angle:
+		gridMidPoints = filterByTwoMajorClusters(clusters, gridMidPoints, 1);
+
+		// Calculate weighted:
+		FVector gridMidWeighted = mean(gridMidPoints);
+
+		// Only use grid points in specific radius for more precise mid prediction:
+		std::vector<FVector> midPointsInRadius = filterByDistance(gridMidWeighted, gridMidPoints, MID_POINT_PREDICTION_MAXIMAL_RADIUS);
+
+		// Calculate again:
+		gridMidWeighted = mean(midPointsInRadius);
+
 		// Find nearest point to the weighted grid mid and use it at estimated grid mid (to remove imprecision if some hole mid points
 		// are not recognized):
 		int nearestPointTo = findNearestPointTo(gridMidWeighted, midPointsInRadius);
@@ -1089,9 +1092,6 @@ GridDetectionResult GridDetector::detectGrid(PointCloudImage pointCloud) {
 
 		FVector probableGridMid = midPointsInRadius[nearestPointTo];
 
-		// Filter all points which are not connected to another point over minimal 1 good angle:
-		gridMidPoints = filterByTwoMajorClusters(clusters, gridMidPoints, 1);
-
 		gridMidPoints = filterByAxisDistance(probableGridMid, gridMidPoints, axis1, axis2, CLUSTER_MID_POINTS_MAXIMAL_AXIS_DISTANCE_TO_MID);
 		
 		if (gridMidPoints.size() == 0)