failed attempt to add projection3DTo3D (crashes while calibrating)

bbiwarg/Input/InputHandling/InputHandler.cs

@@ -24,7 +24,6 @@ namespace bbiwarg.Input.InputHandling
     public class InputHandler
     {
         private InputProvider inputProvider;
-        private CoordinateConverter coordinateConverter;
         private bool resetFlag;
 
         private FingerDetector fingerDetector;
@@ -37,7 +36,8 @@ namespace bbiwarg.Input.InputHandling
         private PalmTracker palmTracker;
         private TouchTracker touchTracker;
 
-        public ImageSize ImageSize;
+        public ImageSize ImageSize { get; private set; }
+        public CoordinateConverter CoordinateConverter { get; private set; }
         public FrameData FrameData { get; private set; }
 
         public event NewProcessedFrameEventHandler NewProcessedFrameEvent;
@@ -56,10 +56,10 @@ namespace bbiwarg.Input.InputHandling
         private void initialize()
         {
             ImageSize = new ImageSize(inputProvider.ImageWidth, inputProvider.ImageHeight);
-            coordinateConverter = new CoordinateConverter(ImageSize, inputProvider.HFOV, inputProvider.VFOV);
+            CoordinateConverter = new CoordinateConverter(ImageSize, inputProvider.HFOV, inputProvider.VFOV);
             resetFlag = false;
 
-            fingerDetector = new FingerDetector(coordinateConverter);
+            fingerDetector = new FingerDetector(CoordinateConverter);
             handDetector = new HandDetector();
             palmDetector = new PalmDetector();
             touchDetector = new TouchDetector();

bbiwarg/Output/GlassesOutput/GlassesWindow.cs

@@ -26,7 +26,8 @@ namespace bbiwarg.Output.GlassesOutput
         private ImageSize inputSize;
         private ImageSize outputSize;
         private OutputImage image;
-        private Projection2DTo2D projection;
+        private Projection2DTo2D projection1;
+        private Projection3DTo3D projection2;
         private Vector2D currentCalibrationPoint;
         private bool calibrationImageUpToDate;
         private Random rand;
@@ -44,7 +45,8 @@ namespace bbiwarg.Output.GlassesOutput
 
             rand = new Random();
             currentCalibrationPoint = getRandomOutputPoint();
-            projection = new Projection2DTo2D(inputSize, outputSize, Parameters.GlassesWindowNumCalibrationPoints);
+            projection1 = new Projection2DTo2D(inputSize, outputSize, Parameters.GlassesWindowNumCalibrationPoints);
+            projection2 = new Projection3DTo3D(inputSize, outputSize, Parameters.GlassesWindowNumCalibrationPoints);
 
             Name = name;
             Text = name;
@@ -75,7 +77,7 @@ namespace bbiwarg.Output.GlassesOutput
             if (!inputProvider.IsActive)
                 Close();
 
-            if (projection.IsCalibrated)
+            if (projection1.IsCalibrated)
             {
                 FrameData frameData = inputHandler.FrameData;
                 if (frameData != null)
@@ -119,17 +121,17 @@ namespace bbiwarg.Output.GlassesOutput
             foreach (Palm palm in frameData.TrackedPalms)
             {
                 Quadrangle quadInput = palm.Quad;
-                Vector2D a = projection.projectPoint(quadInput.TopLeft);
-                Vector2D b = projection.projectPoint(quadInput.TopRight);
-                Vector2D c = projection.projectPoint(quadInput.BottomRight);
-                Vector2D d = projection.projectPoint(quadInput.BottomLeft);
+                Vector2D a = projection1.projectPoint(quadInput.TopLeft);
+                Vector2D b = projection1.projectPoint(quadInput.TopRight);
+                Vector2D c = projection1.projectPoint(quadInput.BottomRight);
+                Vector2D d = projection1.projectPoint(quadInput.BottomLeft);
                 Quadrangle quadOutput = new Quadrangle(a, b, c, d);
                 image.drawQuadrangleGrid(quadOutput, Color.Yellow, Color.Orange, 3, 4);
             }
 
             foreach (Finger finger in frameData.TrackedFingers)
             {
-                Vector2D tipProjected = projection.projectPoint(finger.TipPoint);
+                Vector2D tipProjected = projection1.projectPoint(finger.TipPoint);
                 image.fillCircle(tipProjected, 10, Color.Yellow);
             }
 
@@ -167,8 +169,10 @@ namespace bbiwarg.Output.GlassesOutput
                         {
                             Vector2D pointOutput = currentCalibrationPoint;
                             Vector2D pointInput = frameData.TrackedFingers[0].TipPoint;
+                            Vector3D point3D = inputHandler.CoordinateConverter.convertCoordinate2Dto3D(pointInput, frameData.DepthImage.getDepthAt(pointInput));
 
-                            projection.addCalibrationPoints(pointInput, pointOutput);
+                            projection1.addCalibrationPoints(pointInput, pointOutput);
+                            projection2.addCalibrationPoints(point3D, pointInput, pointOutput);
 
                             currentCalibrationPoint = getRandomOutputPoint();
                         }
@@ -177,7 +181,7 @@ namespace bbiwarg.Output.GlassesOutput
             }
             else if (e.KeyCode == Keys.R)
             {
-                projection.reset();
+                projection1.reset();
                 currentCalibrationPoint = getRandomOutputPoint();
             }
         }

bbiwarg/Parameters.cs

@@ -23,11 +23,11 @@ namespace bbiwarg
         public static readonly int ConsoleHeight = 30;
 
         // input
-        public static readonly InputType InputSource = InputType.Movie;
+        public static readonly InputType InputSource = InputType.Camera;
         public static readonly String InputMoviePath = "..\\..\\videos\\touch\\4.skv";
 
         // Logger
-        public static readonly bool LoggerTimerOutputEnabled = true;
+        public static readonly bool LoggerTimerOutputEnabled = false;
         public static readonly LogSubject LoggerEnabledSubjects = LogSubject.None;
 
         // Debug window
@@ -36,7 +36,7 @@ namespace bbiwarg
         public static readonly String DebugWindowTitle = "BBIWARG - DebugOutput";
 
         // glasses window
-        public static readonly bool GlassesWindowEnabled = false;
+        public static readonly bool GlassesWindowEnabled = true;
         public static readonly int GlassesWindowUpdateInterval = 1000 / 30; // 30fps
         public static readonly String GlassesWindowTitle = "BBIWARG - GlassesOuty   put";
         public static readonly int GlassesWindowNumCalibrationPoints = 20;

bbiwarg/Utility/CoordinateConverter.cs

@@ -6,7 +6,7 @@ using System.Threading.Tasks;
 
 namespace bbiwarg.Utility
 {
-    class CoordinateConverter
+    public class CoordinateConverter
     {
         private ImageSize imageSize;
         private float hfov;

bbiwarg/Utility/ImageSize.cs

@@ -1,6 +1,7 @@
 using System;
 using System.Collections.Generic;
 using System.Linq;
+using System.Drawing;
 using System.Text;
 using System.Threading.Tasks;
 
@@ -23,5 +24,9 @@ namespace bbiwarg.Utility
         public Vector2D getAbsolutePoint(Vector2D relativePoint) {
             return relativePoint.scale(MaxPixel);
         }
+
+        public Vector2D getRelativePoint(Vector2D absolutePoint) {
+            return new Vector2D(absolutePoint.X / Width, absolutePoint.Y / Height);
+        }
     }
 }

bbiwarg/Utility/Projection2DTo2D.cs

@@ -50,9 +50,18 @@ namespace bbiwarg.Utility
         }
 
         private void calibrate() {
-            //homography = CameraCalibration.GetPerspectiveTransform(calibrationPointsA.ToArray(), calibrationPointsB.ToArray());
             homography = CameraCalibration.FindHomography(calibrationPointsA.ToArray(), calibrationPointsB.ToArray(), Emgu.CV.CvEnum.HOMOGRAPHY_METHOD.DEFAULT, 0.995);
+            calibrationPointsA.Clear();
+            calibrationPointsB.Clear();
             IsCalibrated = true;
+
+            Console.WriteLine("HOMOGRAPHY:");
+            for (int r = 0; r < homography.Size.Height; r++) {
+                for (int c = 0; c < homography.Size.Width; c++) {
+                    Console.Write(homography.Data[r, c] + "\t");
+                }
+                Console.WriteLine();
+            }
         }
     }
 }

bbiwarg/Utility/Projection3DTo3D.cs

@@ -0,0 +1,82 @@
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+using System.Drawing;
+using System.Threading.Tasks;
+using Emgu.CV;
+using Emgu.CV.Structure;
+
+namespace bbiwarg.Utility
+{
+    class Projection3DTo3D
+    {
+        private ImageSize sizeA;
+        private ImageSize sizeB;
+        private List<Vector3D> calibrationPoints3D;
+        private List<Vector2D> calibrationPointsA;
+        private List<Vector2D> calibrationPointsB;
+        private int numPointsForCalibration;
+
+        private IntrinsicCameraParameters intrinsicParam1;
+        private IntrinsicCameraParameters intrinsicParam2;
+        private ExtrinsicCameraParameters[] extrinsicParams;
+        private Matrix<double> foundamentalMatrix;
+        private Matrix<double> essentialMatrix;
+
+        public bool IsCalibrated { get; private set; }
+
+        public Projection3DTo3D(ImageSize sizeA, ImageSize sizeB, int numPointsForCalibration = 8)
+        {
+            this.sizeA = sizeA;
+            this.sizeB = sizeB;
+            this.numPointsForCalibration = numPointsForCalibration;
+
+            reset();
+        }
+
+        public void reset()
+        {
+            IsCalibrated = false;
+            calibrationPoints3D = new List<Vector3D>();
+            calibrationPointsA = new List<Vector2D>();
+            calibrationPointsB = new List<Vector2D>();
+
+            intrinsicParam1 = new IntrinsicCameraParameters();
+            intrinsicParam2 = new IntrinsicCameraParameters();
+        }
+
+        public void addCalibrationPoints(Vector3D p3D, Vector2D pA, Vector2D pB)
+        {
+            calibrationPoints3D.Add(p3D);
+            calibrationPointsA.Add(pA);
+            calibrationPointsB.Add(sizeA.getAbsolutePoint(sizeB.getRelativePoint(pB)));
+
+            if (calibrationPointsA.Count == numPointsForCalibration)
+                calibrate();
+        }
+
+        private void calibrate()
+        {
+            MCvPoint3D32f[][] points3D = new MCvPoint3D32f[numPointsForCalibration][];
+            PointF[][] pointsA = new PointF[numPointsForCalibration][];
+            PointF[][] pointsB = new PointF[numPointsForCalibration][];
+
+            for (int i = 0; i < numPointsForCalibration; i++)
+            {
+                points3D[i] = new MCvPoint3D32f[1] { new MCvPoint3D32f(calibrationPoints3D[i].X, calibrationPoints3D[i].Y, calibrationPoints3D[i].Z) };
+                pointsA[i] = new PointF[1] { calibrationPointsA[i] };
+                pointsB[i] = new PointF[1] { calibrationPointsB[i] };
+            }
+            /*
+            CameraCalibration.StereoCalibrate(points3D, pointsA, pointsB, intrinsicParam1, intrinsicParam2, new Size(sizeA.Width, sizeB.Width), Emgu.CV.CvEnum.CALIB_TYPE.DEFAULT, new MCvTermCriteria(0.1e5), out extrinsicParams, out foundamentalMatrix, out essentialMatrix);
+
+            Console.WriteLine("EXTRINSICS:");
+            Console.WriteLine(extrinsicParams.RotationVector.ToString());
+            Console.WriteLine(extrinsicParams.TranslationVector.ToString());*/
+
+            
+            //CameraCalibration.CalibrateCamera(points3D, pointsB, new Size(sizeA.Width, sizeB.Width), intrinsicParam2, Emgu.CV.CvEnum.CALIB_TYPE.DEFAULT, new MCvTermCriteria(0.1e5), out extrinsicParams);
+        }
+    }
+}

bbiwarg/Utility/Vector3D.cs

@@ -6,7 +6,7 @@ using System.Threading.Tasks;
 
 namespace bbiwarg.Utility
 {
-    class Vector3D
+    public class Vector3D
     {
         public float X { get; private set; }
         public float Y { get; private set; }

bbiwarg/bbiwarg.csproj

@@ -151,6 +151,7 @@
     <Compile Include="Utility\Line2D.cs" />
     <Compile Include="Utility\LineSegment2D.cs" />
     <Compile Include="Utility\Logger.cs" />
+    <Compile Include="Utility\Projection3DTo3D.cs" />
     <Compile Include="Utility\Quadrangle.cs" />
     <Compile Include="Utility\Timer.cs" />
     <Compile Include="Utility\Vector2D.cs" />