added getSegmentationDepth

bbiwarg/Graphics/Output2D.cs

@@ -90,11 +90,17 @@ namespace bbiwarg.Graphics
                     maxValue = histogram[i];
             }
 
+            Int16[] segmentationDepth = videoHandle.getSegementationDepth();
+
             GL.Begin(PrimitiveType.LineStrip);
             GL.Color3(0, 0, 1.0);
             GL.LineWidth(5.0f);
             for (int i = 0; i < histogram.Length; ++i)
             {
+                if (i > segmentationDepth[0] - minDepth) GL.Color3(Color.Yellow);
+                if (i > segmentationDepth[1] - minDepth) GL.Color3(Color.Red);
+                if (i > segmentationDepth[2] - minDepth) GL.Color3(Color.Silver);
+                
                 GL.Vertex3(-0.25 + i * (0.5 / histogram.Length), -0.25 + histogram[i] * (0.5 / (maxValue - 5)), -0.5);
             }
             GL.End();

bbiwarg/Images/DepthImage.cs

@@ -102,6 +102,36 @@ namespace bbiwarg.Images
             }
         }
 
+        public Int16[] getSegmentationDepth()
+        {
+            //get first peak
+            Int16 firstMaxIndex = 0;
+
+            for (Int16 i = 0; i < histogram.Length; i++) {
+                if (histogram[i] > histogram[firstMaxIndex]) {
+                    firstMaxIndex = i;
+                }
+            }
+
+            // get second peak
+            Int16 minDistance = 20; // mm
+            Int16 secondMaxIndex = 0;
+            for (Int16 i = 0; i < histogram.Length; i++) {
+                if (Math.Abs(firstMaxIndex - i) > minDistance && histogram[i] > histogram[secondMaxIndex]) {
+                    secondMaxIndex = i;
+                }
+            }
+
+            Int16 minDepth = getMinDepth();
+
+            Int16[] result = new Int16[3];
+            result[0] = (Int16) (Math.Min(firstMaxIndex, secondMaxIndex) + minDepth);
+            result[1] = (Int16) ((firstMaxIndex + secondMaxIndex) / 2 + minDepth);
+            result[2] = (Int16) (Math.Max(firstMaxIndex, secondMaxIndex) + minDepth);
+
+            return result;
+        }
+
         public Int16 getPeakDepth()
         {
             Int16 minDepth = getMinDepth();

bbiwarg/VideoHandle/IVideoHandle.cs

@@ -33,7 +33,7 @@ namespace bbiwarg.VideoHandles
         int getWidth();
         int getHeight();
 
-        Int16 getPeakDepth();
         int[] getSmoothedHistogram();
+        Int16[] getSegementationDepth();
     }
 }

bbiwarg/VideoHandle/VideoHandle.cs

@@ -25,7 +25,7 @@ namespace bbiwarg.VideoHandles
         private ColorImage colorImage;
 
         private DepthImage handImage;
-        private Int16 peakDepth;
+        private Int16[] segmentationDepth;
         private int[] histogram;
 
         private float maxU;
@@ -85,12 +85,12 @@ namespace bbiwarg.VideoHandles
             handImage.thresholdDepth(minDepth, minDepth + 200);
 
             histogram = handImage.getSmoothedHistogram();
-            peakDepth = handImage.getPeakDepth();
+            segmentationDepth = handImage.getSegmentationDepth();
         }
 
-        public Int16 getPeakDepth()
+        public Int16[] getSegementationDepth()
         {
-            return peakDepth;
+            return segmentationDepth;
         }
 
         public int[] getSmoothedHistogram()