Major bug fixes.

- Fixed FTransform (GridDetector.cpp: l.1393 - I'm wondering how the first demo try worked so well with this...)
- Initialized FVector with zeros (GridDetector.cpp: l. 1264, in rare cases this seemed to be not 0).
- Fixed Illegal Memory Access in rare cases (GridDector.cpp: l. 963, 1246).

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

@@ -960,6 +960,9 @@ std::vector<FVector> filterByAxisDistance(FVector reference, std::vector<FVector
 */
 
 std::vector<FVector> findConvexHull2D(std::vector<FVector> vectors) {
+	if (vectors.size() == 0)
+		return std::vector<FVector>();
+
 	// Calculate lowest y:
 	FVector lowestY = vectors[0];
 	unsigned int lowestI = 0;
@@ -1240,6 +1243,9 @@ GridDetectionResult GridDetector::detectGrid(PointCloudImage pointCloud) {
 
 		gridMidPoints = filterByAxisDistance(probableGridMid, gridMidPoints, axis1, axis2, CLUSTER_MID_POINTS_MAXIMAL_AXIS_DISTANCE_TO_MID);
 		
+		if (gridMidPoints.size() == 0)
+			continue;
+
 		// Find convex hull and minimal oriented bounding box:
 		std::vector<FVector> convexHull = findConvexHull2D(gridMidPoints);
 		MinimalOBB minimalOBB = findMinimalOrientedBoundingBox(convexHull);
@@ -1255,7 +1261,7 @@ GridDetectionResult GridDetector::detectGrid(PointCloudImage pointCloud) {
 			// Filter points AROUND the grid for orientation estimation (these points are normally hands and arms which hold the grid):
 			std::vector<int> pointsAroundGridIndices = filterPointsAroundOBB(pointCloud.data, pointCloud.sourceImageWidth * pointCloud.sourceImageHeight, minimalOBB, 0.25f, 0.6f, 0.10f);
 
-			FVector midPointOfPointsAroundGrid;
+			FVector midPointOfPointsAroundGrid(0);
 
 			for (int i : pointsAroundGridIndices) {
 				float* pc = &pointCloud.data[i * 3];
@@ -1390,9 +1396,10 @@ MotionControllerResult GridDetector::findMotionControllerRelativeToGrid(GridDete
 	FVector leftOffset = assumptions.swapControllers ? assumptions.rightOffsetFromGridMid : assumptions.leftOffsetFromGridMid;
 	FVector rightOffset = assumptions.swapControllers ? assumptions.leftOffsetFromGridMid : assumptions.rightOffsetFromGridMid;
 
-	FTransform transform(gdr.axis1, gdr.axis1, gdr.normal, gdr.gridCenter);
+	FTransform transform(gdr.axis1, gdr.axis2, gdr.normal, gdr.gridCenter);
 	result.leftControllerPosition = transform.TransformPosition(leftOffset);
 	result.rightControllerPosition = transform.TransformPosition(rightOffset);
+
 	return result;
 }
 

VRKinectReg/Source/VRKinectReg/Registration/AzureKinect.cpp

@@ -16,11 +16,25 @@ AAzureKinect::AAzureKinect()
 
 	gridMeshComponent = CreateDefaultSubobject<UStaticMeshComponent>("GridMeshComponent");
 	gridMeshComponent->SetupAttachment(RootComponent);
-	
+
+	estimatedLeftController = CreateDefaultSubobject<UStaticMeshComponent>("EstimatedLeftDebugComponent");
+	estimatedLeftController->SetupAttachment(RootComponent);
+
+	estimatedRightController = CreateDefaultSubobject<UStaticMeshComponent>("EstimatedRightDebugComponent");
+	estimatedRightController->SetupAttachment(RootComponent);
+
 	static ConstructorHelpers::FObjectFinder<UStaticMesh> GridMeshFinder(TEXT("StaticMesh'/Game/Geometry/Meshes/Grid.Grid'"));
 	if (GridMeshFinder.Succeeded()) {
 		gridMeshComponent->SetStaticMesh(GridMeshFinder.Object);
 	}
+
+	static ConstructorHelpers::FObjectFinder<UStaticMesh> SphereMeshFinder(TEXT("StaticMesh'/Engine/BasicShapes/Sphere.Sphere'"));
+	if (GridMeshFinder.Succeeded()) {
+		estimatedLeftController->SetStaticMesh(SphereMeshFinder.Object);
+		estimatedLeftController->SetRelativeScale3D(FVector(0.1f));
+		estimatedRightController->SetStaticMesh(SphereMeshFinder.Object);
+		estimatedRightController->SetRelativeScale3D(FVector(0.1f));
+	}
 }
 
 AAzureKinect::~AAzureKinect() {

VRKinectReg/Source/VRKinectReg/Registration/AzureKinect.h

@@ -34,6 +34,8 @@ public:
 		UPointCloudRenderer* pcRendererComponent;
 
 	UStaticMeshComponent* gridMeshComponent;
+	UStaticMeshComponent* estimatedLeftController;
+	UStaticMeshComponent* estimatedRightController;
 
 	bool ReadImage();
 

VRKinectReg/Source/VRKinectReg/Registration/RegistrationManager.cpp

@@ -144,6 +144,17 @@ void ARegistrationManager::RegisterKinectsIntoHMD() {
 
 		MotionControllerResult mcr = gridDetector.findMotionControllerRelativeToGrid(gdr, assumptions);
 
+		if (!mcr.found)
+			return;
+
+		//debugDrawLeftControllerCS = (gdr.gridCenter + gdr.vectorToHandsMidpoint) * 100.f;
+		//UE_LOG(LogTemp, Warning, TEXT("Pos: %f %f %f"), debugDrawLeftControllerCS.X, debugDrawLeftControllerCS.Y, debugDrawLeftControllerCS.Z)
+
+		//debugDrawRightControllerCS = FVector(10000.f, 0, 0);
+
+		debugDrawLeftControllerCS = mcr.leftControllerPosition * 100.f;
+		debugDrawRightControllerCS = mcr.rightControllerPosition * 100.f;
+
 		if (pcWithHMDRegistrator.getPointCloudCount() != 2) {
 			pcWithHMDRegistrator = MultiplePointCloudsRegistrator(2);
 		}
@@ -188,6 +199,8 @@ void ARegistrationManager::Tick(float DeltaTime)
 	if (kinects.Num() >= 1 && !FTransform::Identity.Equals(kinect0IntoWorldRegistration)) {
 		kinects[0]->SetActorTransform(kinect0IntoWorldRegistration);
 		kinects[0]->gridMeshComponent->SetRelativeTransform(debugDrawGridTransform);
+		kinects[0]->estimatedLeftController->SetRelativeLocation(debugDrawLeftControllerCS);
+		kinects[0]->estimatedRightController->SetRelativeLocation(debugDrawRightControllerCS);
 	}
 
 	if (currentRegistration.Num() >= kinects.Num()) {

VRKinectReg/Source/VRKinectReg/Registration/RegistrationManager.h

@@ -75,6 +75,8 @@ private:
 	bool controllerPositionsAvailable = false;
 
 	FTransform debugDrawGridTransform;
+	FVector debugDrawLeftControllerCS;
+	FVector debugDrawRightControllerCS;
 
 	int tickCounter = 0;
 };
\ No newline at end of file