Merge pull request #83 from mariuszhermansdorfer/feature/Calibration

Feature/calibration

SandWorm/Components/SandWormComponent.cs

@@ -100,18 +100,21 @@ protected override void SolveInstance(IGH_DataAccess DA)
             DA.GetData(0, ref reset);
             if (reset || _reset)
             {
+                if (_calibrate.Active)
+                    _sensorElevation.Value = Calibrate((KinectTypes)_sensorType.Value);
+
                 if ((KinectTypes)_sensorType.Value == KinectTypes.KinectForWindows)
                     KinectForWindows.RemoveRef();
                 else 
                     KinectAzureController.RemoveRef();
 
-                Reset();
+                ResetDataArrays();
                 unitsMultiplier = GeneralHelpers.ConvertDrawingUnits(RhinoDoc.ActiveDoc.ModelUnitSystem);
             }
 
             if (_resize)
             {
-                Reset();
+                ResetDataArrays();
             }
 
             // Flip left and right columns for Kinect for Windows
@@ -299,14 +302,14 @@ protected void ScheduleDelegate(GH_Document doc)
             ExpireSolution(false);
         }
 
-        private void Reset()
+        private void ResetDataArrays()
         {
             _quadMesh = null;
             trimmedXYLookupTable = null;
             runningSum = null;
             renderBuffer.Clear();
 
-            _calibrate.Active = false;
+            _calibrate.Active = false; // Untick the UI checkbox
             _resize = false;
             _reset = false;
         }

SandWorm/ComponentsUI/SandWormComponentUI.cs

@@ -62,7 +62,7 @@ public static void MainComponentUI(GH_ExtendableComponentAttributes attr)
             _refreshRate.AddItem("0.2 FPS", "0.2 FPS");
 
             MenuStaticText sensorElevationHeader = new MenuStaticText("Sensor elevation", "Distance between the sensor and the table. \nInput should be in millimeters.\nTo automatically estimate this value, check the 'Calibrate' checkbox and reset.");
-            _sensorElevation = new MenuSlider(sensorElevationHeader, 1, 250, 2000, 1000, 0);
+            _sensorElevation = new MenuSlider(sensorElevationHeader, 1, 0, 1500, 1000, 0);
             _calibrate = new MenuCheckBox(10001, "Calibrate", "Calibrate");
 
             MenuStaticText leftColumnsHeader = new MenuStaticText("Left columns", "Number of pixels to trim from the left.");
@@ -130,7 +130,7 @@ public static void MainComponentUI(GH_ExtendableComponentAttributes attr)
             _colorPalette.Value = (int)Structs.ColorPalettes.Europe;
 
             MenuStaticText colorGradientHeader = new MenuStaticText("Color gradient range", "Define maximum elevation for color gradient. \nInput should be in millimeters.");
-            _colorGradientRange = new MenuSlider(colorGradientHeader, 24, 4, 100, 40, 0);
+            _colorGradientRange = new MenuSlider(colorGradientHeader, 24, 15, 100, 40, 0);
 
             MenuStaticText contourIntervalHeader = new MenuStaticText("Contour interval", "Define spacing between contours. \nInput should be in millimeters.");
             _contourIntervalRange = new MenuSlider(contourIntervalHeader, 25, 0, 30, 0, 0);

SandWorm/Utilities/Core.cs

@@ -386,5 +386,21 @@ public static void TrimColorArray(BGRA[] source, ref Color[] destination, Struct
                     break;
             }
         }
+
+        public static double Calibrate(Structs.KinectTypes kinectType)
+        {
+            switch(kinectType)
+            {
+                case Structs.KinectTypes.KinectAzureNear:
+                case Structs.KinectTypes.KinectAzureWide:
+                    return KinectAzureController.Calibrate(kinectType);
+
+                case Structs.KinectTypes.KinectForWindows:
+                    return KinectForWindows.Calibrate();
+
+                default:
+                    return 0;
+            }
+        }
     }
 }

SandWorm/Utilities/KinectAzureController.cs

@@ -55,6 +55,41 @@ public static DepthMode GetDepthMode(Structs.KinectTypes type)
             }
         }
 
+        public static double Calibrate(Structs.KinectTypes fieldOfViewMode)
+        {
+            CreateCameraConfig(fieldOfViewMode);
+
+            int minX = 3 * (depthWidth / 2) - (3 * 10); // Multiply by 3 for each of the X,Y,Z coordinates
+            int maxX = 3 * (depthWidth / 2) + (3 * 10); // Multiply by 3 for each of the X,Y,Z coordinates
+            int minY = (depthHeight / 2) - 10;
+            int maxY = (depthHeight / 2) + 10;
+
+            double averagedSensorElevation = 0.0;
+            int counter = 0;
+
+            using (Capture capture = sensor.GetCapture())
+            {
+                if (capture.Depth == null)
+                    return 0; // No depth data obtained 
+
+                calibration = sensor.GetCalibration(deviceConfig.DepthMode, deviceConfig.ColorResolution);
+                transformation = calibration.CreateTransformation();
+
+                var pointCloud = transformation.DepthImageToPointCloud(capture.Depth);
+                var pointCloudBuffer = System.Runtime.InteropServices.MemoryMarshal.Cast<byte, short>(pointCloud.Memory.Span); // Cast byte values to short
+
+                for (int y = minY; y < maxY; y++)       // Iterate over y dimension
+                {
+                    for (int x = minX + 2; x < maxX; x += 3, counter++)       // Iterate over x dimension. Increment by 3 to skip over the X,Y coordinates
+                    {
+                        int i = y * depthWidth * 3 + x;
+                        averagedSensorElevation += pointCloudBuffer[i];
+                    }
+                }
+            }
+            return Math.Round(averagedSensorElevation /= counter);
+        }
+
         public static void RemoveRef()
         {
             sensor.Dispose();
@@ -69,7 +104,7 @@ private static void CreateCameraConfig(Structs.KinectTypes fieldOfViewMode)
                 DepthMode = GetDepthMode(fieldOfViewMode),
                 ColorFormat = ImageFormat.ColorBGRA32,
                 ColorResolution = ColorResolution.R1536p,
-                SynchronizedImagesOnly = true // Color and depth images can be out of sync
+                SynchronizedImagesOnly = true 
             };
 
             if (fieldOfViewMode == Structs.KinectTypes.KinectAzureNear) // We can have 30 FPS in the narrow field of view

SandWorm/Utilities/KinectForWindows.cs

@@ -52,7 +52,7 @@ public static void SetupSensor()
         public static void RemoveRef()
         {
             refc--;
-            if ((sensor != null) && (refc == 0))
+            if ((sensor != null) && (refc <= 0))
             {
                 multiFrameReader.MultiSourceFrameArrived -= Reader_FrameArrived;
                 sensor.Close();
@@ -185,5 +185,41 @@ public static KinectSensor Sensor
                 sensor = value;
             }
         }
+
+        public static double Calibrate()
+        {
+            int minX = (depthWidth / 2) - 10;
+            int maxX = (depthWidth / 2) + 10;
+            int minY = (depthHeight / 2) - 10;
+            int maxY = (depthHeight / 2) + 10;
+
+            double averagedSensorElevation = 0.0;
+            int counter = 0;
+
+            using (DepthFrame depthFrame = multiFrameReader.AcquireLatestFrame().DepthFrameReference.AcquireFrame())
+            {
+                if (depthFrame == null)
+                    return 0;
+
+                using (KinectBuffer buffer = depthFrame.LockImageBuffer())
+                {
+                    depthFrameDescription = depthFrame.FrameDescription;
+                    depthWidth = depthFrameDescription.Width;
+                    depthHeight = depthFrameDescription.Height;
+                    depthFrameData = new ushort[depthWidth * depthHeight];
+                    depthFrame.CopyFrameDataToArray(depthFrameData);
+                }
+
+                for (int y = minY; y < maxY; y++)       // Iterate over y dimension
+                {
+                    for (int x = minX; x < maxX; x++, counter++)       // Iterate over x dimension. 
+                    {
+                        int i = y * depthWidth + x;
+                        averagedSensorElevation += depthFrameData[i];
+                    }
+                }
+            }
+            return Math.Round(averagedSensorElevation /= counter);
+        }
     }
 }