diff --git a/Engine/Emgu.CV.External/Extensions/EmguExtensions.cs b/Engine/Emgu.CV.External/Extensions/EmguExtensions.cs index f1fdfde..c1de444 100644 --- a/Engine/Emgu.CV.External/Extensions/EmguExtensions.cs +++ b/Engine/Emgu.CV.External/Extensions/EmguExtensions.cs @@ -131,7 +131,7 @@ public static BitmapSource ToGradientBitmapSource(this Image image, { var index = (int)((Gradient.Length - 1) * (255.0 - depth) / 255.0); if (index < 0) index = 0; - if (index > Gradient.Length) index = Gradient.Length - 1; + if (index >= Gradient.Length) index = Gradient.Length - 1; color = Gradient[index]; } gradientImage[y, x] = color; diff --git a/Engine/Huddle.Engine/App.config b/Engine/Huddle.Engine/App.config index 5740b3b..12762f8 100644 --- a/Engine/Huddle.Engine/App.config +++ b/Engine/Huddle.Engine/App.config @@ -1,17 +1,29 @@ - + - +
- + + + + + + + + + + + + + - + diff --git a/Engine/Huddle.Engine/Data/RotationMatrixData.cs b/Engine/Huddle.Engine/Data/RotationMatrixData.cs new file mode 100644 index 0000000..309c1d2 --- /dev/null +++ b/Engine/Huddle.Engine/Data/RotationMatrixData.cs @@ -0,0 +1,72 @@ +using Emgu.CV; +using Huddle.Engine.Processor; +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; + +namespace Huddle.Engine.Data +{ + public class RotationMatrixData : BaseData + { + #region properties + + #region RotationMatrix + + /// + /// The property's name. + /// + public const string RotationMatrixPropertyName = "RotationMatrix"; + + private Matrix _rotationMatrix; + + /// + /// Sets and gets the RotationMatrix property. + /// Changes to that property's value raise the PropertyChanged event. + /// + public Matrix RotationMatrix + { + get + { + return _rotationMatrix; + } + + set + { + if (_rotationMatrix == value) + { + return; + } + + RaisePropertyChanging(RotationMatrixPropertyName); + _rotationMatrix = value; + RaisePropertyChanged(RotationMatrixPropertyName); + } + } + + #endregion + + #endregion + + #region ctor + + public RotationMatrixData(IProcessor source, string key, Matrix rotationMatrix) + : base(source, key) + { + RotationMatrix = rotationMatrix; + } + + #endregion + + public override IData Copy() + { + return new RotationMatrixData(Source, Key, RotationMatrix); + } + + public override void Dispose() + { + + } + } +} diff --git a/Engine/Huddle.Engine/Huddle.Engine.csproj b/Engine/Huddle.Engine/Huddle.Engine.csproj index 1e37667..6c7b1ed 100644 --- a/Engine/Huddle.Engine/Huddle.Engine.csproj +++ b/Engine/Huddle.Engine/Huddle.Engine.csproj @@ -162,17 +162,26 @@ true - + False - ..\..\lib\Aforge.NET\AForge.dll + ..\..\packages\Accord.2.14.0\lib\net40\Accord.dll - + False - ..\..\lib\Aforge.NET\AForge.Imaging.dll + ..\..\packages\Accord.Math.2.14.0\lib\net40\Accord.Math.dll - + False - ..\..\lib\Aforge.NET\AForge.Math.dll + ..\..\packages\Accord.Statistics.2.14.0\lib\net40\Accord.Statistics.dll + + + ..\..\packages\AForge.2.2.5\lib\AForge.dll + + + ..\..\packages\AForge.Imaging.2.2.5\lib\AForge.Imaging.dll + + + ..\..\packages\AForge.Math.2.2.5\lib\AForge.Math.dll False @@ -309,6 +318,7 @@ ExceptionMessageBox.xaml + @@ -321,7 +331,10 @@ + + + diff --git a/Engine/Huddle.Engine/Processor/Complex/CameraRotation.cs b/Engine/Huddle.Engine/Processor/Complex/CameraRotation.cs new file mode 100644 index 0000000..5e352d9 --- /dev/null +++ b/Engine/Huddle.Engine/Processor/Complex/CameraRotation.cs @@ -0,0 +1,158 @@ +using System; +using System.Collections.Generic; +using System.Collections.ObjectModel; +using System.Diagnostics; +using System.Linq; +using System.Runtime.Serialization; +using System.Text; +using System.Threading; +using System.Threading.Tasks; +using GalaSoft.MvvmLight; +using GalaSoft.MvvmLight.Threading; +using Huddle.Engine.Data; +using Huddle.Engine.Util; +using Emgu.CV; +using Emgu.CV.CvEnum; +using Emgu.CV.Structure; +using Accord.Math; + +namespace Huddle.Engine.Processor.Complex +{ + /// + /// calculate the table plane and the camera rotation + /// established on 27/4/2015 by Yunlong + /// + [ViewTemplate("Camera Rotation", "CameraRotation")] + public class CameraRotation : BaseProcessor + { + Image depthImage1 = null; + Image depthImage2 = null; + Image depthImage3 = null; + Image depthImage4 = null; + Image depthImage0 = null; + double[] eula0 = { 0.0, 0.0, 0.0 }; + bool dataReady = false; + int shakeNum = 0; + bool init = false; + int cnt = 0; + Matrix cameraRotation = new Matrix(3, 3); + Matrix rotationMatrix; + + public override IDataContainer PreProcess(IDataContainer dataContainer) + { + // EmguCv depth image + foreach (GrayFloatImage imageData in dataContainer.OfType()) + { + //initialize Matrix homoGraph + if (!init) + { + Init(); + init = true; + } + cnt++; + if (cnt == 5) + { + Stopwatch sw = Stopwatch.StartNew(); + rotationMatrix = GetCameraRotation(imageData.Image); + Console.WriteLine("time Rotation: " + sw.ElapsedMilliseconds); + cnt = 0; + } + Stage(new RotationMatrixData(this, "cameraRotation", rotationMatrix)); + } + + Push(); + return null; + } + + public override IData Process(IData data) + { + return null; + } + + private void Init(){ + cameraRotation.SetZero(); + cameraRotation.Data[0, 0] = 1; cameraRotation.Data[1, 1] = 1; cameraRotation.Data[2, 2] = 1; + } + + private Matrix GetCameraRotation(Image depthImage) + { + //time filter + if (!dataReady) + { + depthImage1 = depthImage2 = depthImage3 = depthImage4 = depthImage; + dataReady = true; + } + depthImage0 = depthImage; + depthImage = (depthImage0 + depthImage1 + depthImage2 + depthImage3 + depthImage4) / 5.0; + depthImage4 = depthImage3; depthImage3 = depthImage2; depthImage2 = depthImage1; depthImage1 = depthImage0; + + //median filter + depthImage = depthImage.SmoothMedian(3); + + //subImage !! depthImage.Data [240,320,1] !! + int padding = 8; + int widthSubImage = (int)Math.Ceiling(depthImage.Width * (1.0 - (2.0 / padding))); + int heightSubImage = (int)Math.Ceiling(depthImage.Height * (1.0 - (2.0 / padding))); + int originalX = (int)Math.Ceiling(depthImage.Width * (1.0 / padding)); + int originalY = (int)Math.Ceiling(depthImage.Height * (1.0 / padding)); + Image depthImageSub = new Image(widthSubImage, heightSubImage); + for (int j = 0; j < heightSubImage; j++) + { + for (int i = 0; i < widthSubImage; i++) + { + depthImageSub.Data[j, i, 0] = depthImage.Data[originalY + j, originalX + i, 0]; + } + } + + //sampling + //convent to world coordinates + //move coordinate original to center + int samplingSpace = 3; + int widthSampling = widthSubImage / samplingSpace; + int heightSampling = heightSubImage / samplingSpace; + double focalLengthDepth = depthImage.Width / (2 * Math.Tan((double)(74 * 3.14 / 180) / 2)); + float[, ,] depthData = depthImageSub.Data; + Point3[] depthDataSamples = new Point3[widthSampling * heightSampling]; + for (int j = 0; j < heightSampling; j++) + { + for (int i = 0; i < widthSampling; i++) + { + float z = Math.Abs(depthData[samplingSpace * j, samplingSpace * i, 0]); + float x = (float)((originalX + i * samplingSpace - depthImage.Width / 2.0) * z / focalLengthDepth); + float y = (float)((originalY + j * samplingSpace - depthImage.Height / 2.0) * z / focalLengthDepth); + depthDataSamples[j * widthSampling + i] = new Point3(x, y, z); + } + } + //Ransac + Stopwatch sw = Stopwatch.StartNew(); + RansacFitPlane ransacFitPlane = new RansacFitPlane(depthDataSamples, 5, 20); + Plane plane = ransacFitPlane.FitPlane(); + if (plane != null) + { + Console.WriteLine(plane.A / plane.Offset + " " + plane.B / plane.Offset + " " + plane.C / plane.Offset + " " + plane.Offset); + + //calc rotation matrix + double[] eula = { Math.Atan((plane.B) / (plane.C)), -Math.Atan((plane.A) / (plane.C)), 0 }; + Console.WriteLine("Xaxis: " + Math.Atan((plane.B) / (plane.C)) * 180 / Math.PI + " Yaxis: " + Math.Atan((plane.A) / (plane.C)) * 180 / Math.PI); + //avoid shaking + if (Math.Abs(eula0[0] - eula[0]) > 2 * Math.PI / 180 || Math.Abs(eula0[1] - eula[1]) > 2 * Math.PI / 180) + { + shakeNum++; + if (shakeNum == 5) + { + shakeNum = 0; + eula0[0] = eula[0]; eula0[1] = eula[1]; + } + } + else + { + shakeNum = 0; + } + cameraRotation = RansacFitPlane.EulaArray2Matrix(eula0); + Console.WriteLine("time: " + sw.ElapsedMilliseconds); + + } + return cameraRotation; + } + } +} diff --git a/Engine/Huddle.Engine/Processor/Complex/ImageWarpPerspective.cs b/Engine/Huddle.Engine/Processor/Complex/ImageWarpPerspective.cs new file mode 100644 index 0000000..5a5ec23 --- /dev/null +++ b/Engine/Huddle.Engine/Processor/Complex/ImageWarpPerspective.cs @@ -0,0 +1,185 @@ +using System; +using System.Collections.Generic; +using System.Collections.ObjectModel; +using System.Diagnostics; +using System.Linq; +using System.Runtime.Serialization; +using System.Text; +using System.Threading; +using System.Threading.Tasks; +using GalaSoft.MvvmLight; +using GalaSoft.MvvmLight.Threading; +using Huddle.Engine.Data; +using Huddle.Engine.Util; +using Emgu.CV; +using Emgu.CV.CvEnum; +using Emgu.CV.Structure; +using Accord.Math; + +namespace Huddle.Engine.Processor.Complex +{ + /// + /// image warpPerspective + /// this should be called after depth image is cropped + /// established on 27/4/2015 by Yunlong + /// + [ViewTemplate("Image WarpPerspective", "ImageWarpPerspective")] + public class ImageWarpPerspective : BaseProcessor + { + #region public properties + + #region IsWarpImage + /// + /// The property's name. + /// + public const string IsWarpImagePropertyName = "IsWarpImage"; + // IsInitialized is used to set ROI if filter is used the first time. + private bool _isWarpImage = true; + public bool IsWarpImage + { + get + { + return _isWarpImage; + } + + set + { + if (_isWarpImage == value) + { + return; + } + + RaisePropertyChanging(IsWarpImagePropertyName); + _isWarpImage = value; + RaisePropertyChanged(IsWarpImagePropertyName); + } + } + + #endregion + #endregion + + Matrix cameraRotation = new Matrix(3, 3); + Matrix homoGraphy = new Matrix(3, 3); + + public override IDataContainer PreProcess(IDataContainer dataContainer) + { + if (IsWarpImage) + { + foreach (RotationMatrixData cameraMatrix in dataContainer.OfType()) + { + cameraRotation = cameraMatrix.RotationMatrix; + } + + if (cameraRotation == null) + { + return dataContainer; + } + foreach (GrayFloatImage imageData in dataContainer.OfType()) + { + FindHomoGraphy(cameraRotation, imageData.Image.Width, imageData.Image.Height); + if (imageData.Key.Equals("depth")) + { + Image warpDepthImage = imageData.Image.WarpPerspective(homoGraphy, Emgu.CV.CvEnum.INTER.CV_INTER_CUBIC, Emgu.CV.CvEnum.WARP.CV_WARP_DEFAULT, new Gray()); + Stage(new GrayFloatImage(this, "depth", warpDepthImage)); + } + + } + foreach (RgbImageData imageData in dataContainer.OfType()) + { + FindHomoGraphy(cameraRotation, imageData.Image.Width, imageData.Image.Height); + if (imageData.Key.Equals("color")) + { + Image warpColorImage = imageData.Image.WarpPerspective(homoGraphy, Emgu.CV.CvEnum.INTER.CV_INTER_CUBIC, Emgu.CV.CvEnum.WARP.CV_WARP_DEFAULT, new Rgb(0, 0, 0)); + Stage(new RgbImageData(this, "wrappedColor", warpColorImage)); + } + else if (imageData.Key.Equals("confidence")) + { + Image warpConfidence = imageData.Image.WarpPerspective(homoGraphy, Emgu.CV.CvEnum.INTER.CV_INTER_CUBIC, Emgu.CV.CvEnum.WARP.CV_WARP_DEFAULT, new Rgb(0, 0, 0)); + Stage(new RgbImageData(this, "wrappedConfidence", warpConfidence)); + } + } + } + else + { + foreach (IData imageData in dataContainer) + { + Stage(imageData); + } + } + Push(); + + return null; + } + + public override IData Process(IData data) + { + return data; + } + + private void FindHomoGraphy(Matrix rotateMatrix, int width, int height) + { + //Camera inner parameter + double fov = 74 * 3.14 / 180; + double focalLength = width / (2 * Math.Tan(fov / 2)); + //KK = [focalLength 0 width/2; + // 0 focalLength height/2; + // 0 0 1 ] + Matrix KK = new Matrix(3, 3); + KK.SetValue(0); + KK.Data[0, 0] = (double)focalLength; KK.Data[0, 2] = (double)width / 2; + KK.Data[1, 1] = (double)focalLength; KK.Data[1, 2] = (double)height / 2; + KK.Data[2, 2] = 1; + Matrix KKInv = new Matrix(3, 3); + //KKInv.SetValue(0); + //KKInv.Data[0, 0] = 0.0024; KKInv.Data[0, 2] = -0.7536; + //KKInv.Data[1, 1] = 0.0024; KKInv.Data[1, 2] = -0.5652; + //KKInv.Data[2, 2] = 1; + CvInvoke.cvInvert(KK.Ptr, KKInv.Ptr, SOLVE_METHOD.CV_LU); + + homoGraphy = KK * rotateMatrix; + homoGraphy = homoGraphy * KKInv; + homoGraphy = homoGraphy / homoGraphy.Data[2, 2]; + + /* + //By this way, the size of the warped image can be thounds, so keep all the pixel is not propriat + //calc the size and translation of warped iamge + // (0,0) ________ (w, 0) + // | | + // |________| + // (0,h) (w,h) + Matrix box = new Matrix(3, 4); + box.SetValue(1); + box.Data[0, 0] = 0; box.Data[0, 1] = colorImage.Width; box.Data[0, 2] = colorImage.Width; box.Data[0, 3] = 0; + box.Data[1, 0] = 0; box.Data[1, 1] = 0; box.Data[1, 2] = colorImage.Height; box.Data[1, 3] = colorImage.Height; + Matrix boxWarp = homoGraphy * box; + boxWarp.Data[0, 0] = boxWarp.Data[0, 0] / boxWarp.Data[2, 0]; boxWarp.Data[0, 1] = boxWarp.Data[0, 1] / boxWarp.Data[2, 1]; boxWarp.Data[0, 2] = boxWarp.Data[0, 2] / boxWarp.Data[2, 2]; boxWarp.Data[0, 3] = boxWarp.Data[0, 3] / boxWarp.Data[2, 3]; + boxWarp.Data[1, 0] = boxWarp.Data[1, 0] / boxWarp.Data[2, 0]; boxWarp.Data[1, 1] = boxWarp.Data[1, 1] / boxWarp.Data[2, 1]; boxWarp.Data[1, 2] = boxWarp.Data[1, 2] / boxWarp.Data[2, 2]; boxWarp.Data[1, 3] = boxWarp.Data[1, 3] / boxWarp.Data[2, 3]; + double maxX = Math.Max(Math.Max(boxWarp.Data[0, 0], boxWarp.Data[0, 1]),Math.Max(boxWarp.Data[0, 2], boxWarp.Data[0, 3])); + double minX = Math.Min(Math.Min(boxWarp.Data[0, 0], boxWarp.Data[0, 1]), Math.Min(boxWarp.Data[0, 2], boxWarp.Data[0, 3])); + double maxY = Math.Max(Math.Max(boxWarp.Data[1, 0], boxWarp.Data[1, 1]), Math.Max(boxWarp.Data[1, 2], boxWarp.Data[1, 3])); + double minY = Math.Min(Math.Min(boxWarp.Data[1, 0], boxWarp.Data[1, 1]), Math.Min(boxWarp.Data[1, 2], boxWarp.Data[1, 3])); + widthWarp = maxX - minX; + heightWarp = maxY - minY; + */ + + //Using the center of the image as shift standard + Matrix center = new Matrix(3, 1); + center.Data[0, 0] = width / 2; + center.Data[1, 0] = height / 2; + center.Data[2, 0] = 1; + Matrix centerWarp = homoGraphy * center; + centerWarp.Data[0, 0] = centerWarp.Data[0, 0] / centerWarp.Data[2, 0]; + centerWarp.Data[1, 0] = centerWarp.Data[1, 0] / centerWarp.Data[2, 0]; + double minX = centerWarp.Data[0, 0] - center.Data[0, 0]; + double minY = centerWarp.Data[1, 0] - center.Data[1, 0]; + Matrix preWarp = new Matrix(3, 3); + preWarp.SetValue(0); + preWarp.Data[0, 0] = 1; preWarp.Data[0, 2] = -minX; + preWarp.Data[1, 1] = 1; preWarp.Data[1, 2] = -minY; + preWarp.Data[2, 2] = 1; + homoGraphy = preWarp * homoGraphy; + + } + + } +} diff --git a/Engine/Huddle.Engine/Processor/Complex/RansacFitPlane.cs b/Engine/Huddle.Engine/Processor/Complex/RansacFitPlane.cs new file mode 100644 index 0000000..55ba556 --- /dev/null +++ b/Engine/Huddle.Engine/Processor/Complex/RansacFitPlane.cs @@ -0,0 +1,199 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; +using Accord.Math; +using Accord.Math.Decompositions; +using Accord.Statistics; +using Accord; +using Emgu.CV; + +namespace Huddle.Engine.Processor.Complex +{ + public class RansacFitPlane + { + public Point3[] points { get; set; } + public double threshold { get; set; } + public int maxEvaluations { get; set; } + + private int[] inliers; + private double[] d2; + private int maxSamplings = 5; + private Plane bestPlane; + + public RansacFitPlane(Point3[] points, double threshold, int maxEvaluations) + { + this.points = points; + this.threshold = threshold; + this.maxEvaluations = maxEvaluations; + this.d2 = new double[points.Length]; + } + + public Plane FitPlane() + { + // Initial argument checks + if (points.Length < 3) + throw new ArgumentException("At least three points are required to fit a plane"); + + computeInliers(); + if (inliers.Length == 0) + return null; + + // Compute the final plane + Plane plane = fitting(points.Submatrix(inliers)); + return plane; + + } + + private void computeInliers() + { + int[] bestInliers = null; + int maxInliers = 0; + int size = this.points.Length; + Plane plane = null; + + int count = 0; // Total number of trials performed + double N = maxEvaluations; // Estimative of number of trials needed. + + // While the number of trials is less than our estimative, + // and we have not surpassed the maximum number of trials + while (count < N) + { + + int[] sample = null; + int samplings = 0; + + // While the number of samples attempted is less + // than the maximum limit of attempts + while (samplings < maxSamplings) + { + // Select at random s data points to form a trial model. + sample = Accord.Statistics.Tools.RandomSample(size, 3); + + if (!Point3.Collinear(points[sample[0]], points[sample[1]], points[sample[2]])) + { + Point3[] randPoints = { points[sample[0]], points[sample[1]], points[sample[2]] }; + // Fit model using the random selection of points + plane = fitting(randPoints); + break; + } + + samplings++; // Increase the samplings counter + } + + if (plane == null) + throw new ConvergenceException("A model could not be inferred from the data points"); + + // Now, evaluate the distances between total points and the model returning the + // indices of the points that are inliers (according to a distance threshold t). + inliers = distance(plane, this.threshold); + + // Check if the model was the model which highest number of inliers: + if (bestInliers == null || inliers.Length > maxInliers) + { + // Yes, this model has the highest number of inliers. + + maxInliers = inliers.Length; // Set the new maximum, + bestPlane = plane; // This is the best model found so far, + bestInliers = inliers; // Store the indices of the current inliers. + + // Update estimate of N, the number of trials to ensure we pick, + // with probability p, a data set with no outliers. + double pInlier = (double)inliers.Length / (double)size; + double pNoOutliers = 1.0 - System.Math.Pow(pInlier, 3); + + N = System.Math.Ceiling(System.Math.Log(1.0 - 0.99) / System.Math.Log(pNoOutliers)); + } + Console.WriteLine("trail " + count + " out of " + N); + count++; // Increase the trial counter. + if (count > maxEvaluations) + { + int[] temp = { }; + inliers = temp; + return; + } + } + + inliers = bestInliers; + + } + + private Plane fitting(Point3[] points) + { + // Set up constraint equations of the form AB = 0, + // where B is a column vector of the plane coefficients + // in the form b(1)*X + b(2)*Y +b(3)*Z + b(4) = 0. + // + // A = [XYZ' ones(npts,1)]; % Build constraint matrix + if (points.Length < 3) + return null; + + if (points.Length == 3) + return Plane.FromPoints(points[0], points[1], points[2]); + + float[,] A = new float[points.Length, 4]; + for (int i = 0; i < points.Length; i++) + { + A[i, 0] = points[i].X; + A[i, 1] = points[i].Y; + A[i, 2] = points[i].Z; + A[i, 3] = -1; + } + + SingularValueDecompositionF svd = new SingularValueDecompositionF(A, + computeLeftSingularVectors: false, computeRightSingularVectors: true, + autoTranspose: true, inPlace: true); + + float[,] v = svd.RightSingularVectors; + + float a = v[0, 3]; + float b = v[1, 3]; + float c = v[2, 3]; + float d = v[3, 3]; + + float norm = (float)Math.Sqrt(a * a + b * b + c * c); + + a /= norm; + b /= norm; + c /= norm; + d /= norm; + + return new Plane(a, b, c, -d); + } + + + private int[] distance(Plane p, double t) + { + for (int i = 0; i < points.Length; i++) + d2[i] = p.DistanceToPoint(points[i]); + + return Matrix.Find(d2, z => z < t); + } + + public static Matrix EulaArray2Matrix(double[] eula) + { + Matrix t = new Matrix(1, 3); + t.Data[0, 0] = eula[0]; t.Data[0, 1] = eula[1]; t.Data[0, 2] = eula[2]; + double theta = t.Norm; + t = t / theta; + double x = t.Data[0, 0]; double y = t.Data[0, 1]; double z = t.Data[0, 2]; + + double c = Math.Cos(theta); double s = Math.Sin(theta); double C = 1 - c; + double xs = x * s; double ys = y * s; double zs = z * s; + double xC = x * C; double yC = y * C; double zC = z * C; + double xyC = x * yC; double yzC = y * zC; double zxC = z * xC; + + Matrix T = new Matrix(3, 3); + + // T = [ x*xC+c xyC-zs zxC+ys 0 + // xyC+zs y*yC+c yzC-xs 0 + // zxC-ys yzC+xs z*zC+c 0 + // 0 0 0 1]; + T.Data[0, 0] = x * xC + c; T.Data[0, 1] = xyC - zs; T.Data[0, 2] = zxC + ys; + T.Data[1, 0] = xyC + zs; T.Data[1, 1] = y * yC + c; T.Data[1, 2] = yzC - xs; + T.Data[2, 0] = zxC - ys; T.Data[2, 1] = yzC + xs; T.Data[2, 2] = z * zC + c; + return T; + } + } +} diff --git a/Engine/Huddle.Engine/Processor/Sensors/Senz3DIntel.cs b/Engine/Huddle.Engine/Processor/Sensors/Senz3DIntel.cs index d5db12b..7a8a50d 100644 --- a/Engine/Huddle.Engine/Processor/Sensors/Senz3DIntel.cs +++ b/Engine/Huddle.Engine/Processor/Sensors/Senz3DIntel.cs @@ -69,7 +69,7 @@ public class Senz3DIntel : BaseProcessor /// public const string IsAdaptiveSensingPropertyName = "IsAdaptiveSensing"; - private bool _isAdaptiveSensing = true; + private bool _isAdaptiveSensing = false; /// /// Sets and gets the IsAdaptiveSensing property. @@ -709,6 +709,10 @@ private void GrabFrames() var depth = _pp.QueryImage(PXCMImage.ImageType.IMAGE_TYPE_DEPTH); var depthImageAndConfidence = Senz3DUtils.GetHighPrecisionDepthImage(depth, MinDepthValue, MaxDepthValue); + /* Get original depth image */ + var originalDepth = Senz3DUtils.GetDepthImage(depth, 100, 1000); + var originalDepthImage = (Image)originalDepth; + // do adaptive sensing (alternating low/high confidence threshold) on depth image if enabled if (IsAdaptiveSensing) depthImageAndConfidence = PerformAdaptiveSensing(depthImageAndConfidence); @@ -730,7 +734,7 @@ private void GrabFrames() RenderImages(colorImage, depthImage, confidenceMapImage); // publish images finally - PublishImages(colorImage, depthImage, confidenceMapImage); + PublishImages(colorImage, depthImage, confidenceMapImage, originalDepthImage); } _pp.Close(); @@ -746,13 +750,15 @@ private void GrabFrames() private void PublishImages( Image colorImage, Image depthImage, - Image confidenceMapImage) + Image confidenceMapImage, + Image OriginalDepthImage) { var dc = new DataContainer(++_frameId, DateTime.Now) { new RgbImageData(this, "color", colorImage), new GrayFloatImage(this, "depth", depthImage), new RgbImageData(this, "confidence", confidenceMapImage), + new GrayFloatImage(this, "originalDepth", OriginalDepthImage), }; Publish(dc); diff --git a/Engine/Huddle.Engine/Processor/Sensors/Utils/Senz3DUtils.cs b/Engine/Huddle.Engine/Processor/Sensors/Utils/Senz3DUtils.cs index eda9192..e8d212a 100644 --- a/Engine/Huddle.Engine/Processor/Sensors/Utils/Senz3DUtils.cs +++ b/Engine/Huddle.Engine/Processor/Sensors/Utils/Senz3DUtils.cs @@ -94,6 +94,48 @@ public static IImage[] GetHighPrecisionDepthImage(PXCMImage depthImage, float mi return returnImages; } + public static IImage GetDepthImage(PXCMImage depthImage, float minValue, float maxValue) + { + var inputWidth = Align16(depthImage.info.width); /* aligned width */ + var inputHeight = (int)depthImage.info.height; + + var returnImages = new Image(inputWidth, inputHeight); + + PXCMImage.ImageData cdata; + if (depthImage.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.ColorFormat.COLOR_FORMAT_DEPTH, out cdata) < + pxcmStatus.PXCM_STATUS_NO_ERROR) return returnImages; + + var depthValues = cdata.ToShortArray(0, inputWidth * inputHeight); + depthImage.ReleaseAccess(ref cdata); + + var depthReturnImage = ((Image)returnImages); + var depthReturnImageData = depthReturnImage.Data; + + Parallel.For(0, inputHeight, y => + { + for (int x = 0; x < inputWidth; x++) + { + float depth = depthValues[y * inputWidth + x]; + if (depth != EmguExtensions.LowConfidence && depth != EmguExtensions.Saturation) + { + + if (depth > maxValue) + depth = maxValue; + else if (depth < minValue) + depth = minValue; + + depthReturnImageData[y, x, 0] = depth; + } + else + { + depthReturnImageData[y, x, 0] = depth; + } + } + }); + + return returnImages; + } + /// /// Get UV Map of a PXCMImage. /// diff --git a/Engine/Huddle.Engine/Templates/ComplexTemplates.xaml b/Engine/Huddle.Engine/Templates/ComplexTemplates.xaml index 95e6509..ca56d96 100644 --- a/Engine/Huddle.Engine/Templates/ComplexTemplates.xaml +++ b/Engine/Huddle.Engine/Templates/ComplexTemplates.xaml @@ -114,4 +114,27 @@ + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Engine/Huddle.Engine/packages.config b/Engine/Huddle.Engine/packages.config index 93869a4..a19d027 100644 --- a/Engine/Huddle.Engine/packages.config +++ b/Engine/Huddle.Engine/packages.config @@ -1,5 +1,11 @@  + + + + + + diff --git a/Pipelines/HuddleLamp.hep b/Pipelines/HuddleLamp.hep index 2b1e7ee..423dd58 100644 --- a/Pipelines/HuddleLamp.hep +++ b/Pipelines/HuddleLamp.hep @@ -1 +1 @@ -0true0false0false0false0false0false0false0false0true0true0false0false0false0false1490.925984049896759.69077174635560BlobData0false0false0false0false753.11199587254623399.24961830260094050255true981.40869867287756402.101169864665450true221226.1738765895329405.41852316890618095ColorBlob4400falsetruetruefalsetruetruetruefalse95105853.5359501490.9468534664036400.1753634900465501852.7596645909418117.909154433533802198.9950774886006276.0801344503291800false0false0false0false0false299.98384407694147491.724621891866490colorRgbImageData514.73033087231079492.25222322403340truetruetruefalse72012800000000.5737.25383399934162594.7909674534552101041.0598457471974657.41836107728319725false10.41248.803775292092665.229416273148560Marker1975.0032285367117485.9418215697889400false0false1816.3724160408749803.808582762595620010019481860.7902882211235633.001020089068790Disconnected1375.0371209030777420.8286980749361400.0831168759728917881065.15891402933252.223052327437479098DepthBlob2102truetruetruefalsefalsetruetruetrue98107.525450338074737382135950771.6112785201455648.6476229123252540false11529.8471148092747946.135364005292310truetruetruetrue160283193600001312.13216167797611248.6116184081430rgbInDepthROIROI560.54368617296245228.089977254550520truetruefalsetrue160283193600001321.3107155322089294.4823357536470350depthGrayFloatImage37.095200541360718248.693234322488820521280 x 72008330092falsefalse52676324.80114561962665-53.4354429003112440confidence0000.69425870283257329 \ No newline at end of file +0true0false0false0false0false0false0false0false0true0true0false0false0false0false1490.925984049896759.69077174635560BlobData0false0false0false0false753.11199587254623399.24961830260094050255true981.40869867287756402.101169864665450true221226.1738765895329405.41852316890618095ColorBlob4400falsetruetruefalsetruetruetruefalse95105853.5359501490.9468534664036400.1753634900465501852.7596645909418117.909154433533802198.9950774886006276.0801344503291800false0true0false0false0false299.98384407694147491.724621891866490colorRgbImageData514.73033087231079492.25222322403340truetruetruefalse72012800000000.5737.25383399934162594.7909674534552101041.0598457471974657.41836107728319725false10.41248.803775292092665.229416273148560Marker1975.0032285367117485.9418215697889400false0false1816.3724160408749803.808582762595620010019481860.7902882211235633.001020089068790Disconnected1375.0371209030777420.8286980749361400.0831168759728917881065.15891402933252.223052327437479098DepthBlob2102truetruetruefalsefalsetruetruetrue98107.525450338074737382135950771.6112785201455648.6476229123252540false11529.8471148092747946.135364005292310truetruetruetrue158286104200001312.13216167797611248.6116184081430rgbInDepthROIROI560.54368617296245228.089977254550520truetruefalsetrue158286104200001321.3107155322089294.4823357536470350depthGrayFloatImage37.095200541360718248.6932343224888203261280 x 72008330010falsefalse52676324.80114561962665-53.4354429003112440confidence0000.69425870283257329 \ No newline at end of file diff --git a/Pipelines/final_pipeline.hep b/Pipelines/final_pipeline.hep new file mode 100644 index 0000000..3ac8635 --- /dev/null +++ b/Pipelines/final_pipeline.hep @@ -0,0 +1 @@ +0true0false0false0false0false0true0false65.5582216083551658.686429394077890originalDepthGrayFloatImage0false0false0false0false0false321.3107155322089294.4823357536470350depthGrayFloatImage0false0false0false0false0false1137.9566291005226-214.730425874455480truetruefalsefalse1100000011289.7616344996377-69.9364874740259380false110false0false0false0false0false1635.2511110171031-0.690148719516214370BlobData0false0false0false0false753.11199587254623399.24961830260094050255true981.40869867287756402.101169864665450true221242.5813641478039367.1343855329402095ColorBlob4400falsetruetruefalsetruetruetruefalse95105853.5359501638.2173911880432366.3031398140692501944.067397978358576.67340387147502198.9950774886006276.0801344503291800false0false0false0false582.893412695009908.60527251315830truetruetruefalse4806400000001785.10900604429946622.1367800505743201054.2087793947842598.9380071497309725false10.41308.9645630057523747.266854064504290Marker2166.4549275748432484.4691161925724700false0false1816.3724160408749803.808582762595620010019481860.7902882211235633.001020089068790Disconnected1414.2113285681269541.2707444225853810.0831168759728917881098.693616291807119.528517020182463098DepthBlob2102truetruetruefalsefalsetruetruetrue9825.717.5254503380747373821359501084.4606812051843231.023314919967930truetruefalsefalse110000001754.640295108760693.5439305729258170true562.01639155017949244.289736403930930falsefalsefalsetrue159287104100001312.13216167797611248.6116184081430rgbInDepthROIROI600.53697291566323-86.40811994418240falsefalsetruetrue159287104100001868.47029687101747-144.733609074064730true354.28840869122661770.779474235827930524.81799080479129688.63884129780570true299.98384407694147491.724621891866490colorRgbImageData-142.40087713101929322.561027787357320161280 x 7200833003falsefalse52676352.78254778673823-93.1984880851536560confidence0000.7313734023799765 \ No newline at end of file diff --git a/Pipelines/test.hep b/Pipelines/test.hep new file mode 100644 index 0000000..6875eae --- /dev/null +++ b/Pipelines/test.hep @@ -0,0 +1 @@ +0true0false0false0false0false0false0false65.5582216083551658.686429394077890originalDepthGrayFloatImage0false0false0false0false0false321.3107155322089294.4823357536470350depthGrayFloatImage0false0false0true1004.5074925367971-99.524886116586060truetruefalsefalse1100000011036.456309618412240.5164158172030110false110false0false0false0false0false1490.925984049896759.69077174635560BlobData0false0false0false0false753.11199587254623399.24961830260094050255true981.40869867287756402.101169864665450true221226.1738765895329405.41852316890618095ColorBlob4400falsetruetruefalsetruetruetruefalse95105853.5359501490.9468534664036400.1753634900465501852.7596645909418117.909154433533802198.9950774886006276.0801344503291800false0false0false0false514.73033087231079492.25222322403341truetruetruefalse4806400000001737.25383399934162594.7909674534552101041.0598457471974657.41836107728309725false10.41248.803775292092665.229416273148560Marker1975.0032285367117485.9418215697889400false0false1816.3724160408749803.808582762595620010019481860.7902882211235633.001020089068790Disconnected1500.3506382490502665.69419173947500.0831168759728917881252.176677217138446.415047259493008098DepthBlob2102truetruetruefalsefalsetruetruetrue98107.525450338074737382135950560.54368617296245228.089977254550520truetruefalsetrue176233353300001312.13216167797611248.6116184081430rgbInDepthROIROI546.046873958657112.2631403293154480falsefalsetruetrue176233353300001750.65386669370571-71.098340213245450349.87029255957736713.343964524388750591.35635964833057765.724887049050720299.98384407694147491.724621891866490colorRgbImageData37.095200541360718248.69323432248882045640 x 4800833002falsefalse52676324.80114561962665-53.4354429003112440confidence0000.67902244092442732 \ No newline at end of file diff --git a/lib/Aforge.NET/AForge.Imaging.dll b/lib/Aforge.NET/AForge.Imaging.dll deleted file mode 100644 index a3cf49d..0000000 Binary files a/lib/Aforge.NET/AForge.Imaging.dll and /dev/null differ diff --git a/lib/Aforge.NET/AForge.Math.dll b/lib/Aforge.NET/AForge.Math.dll deleted file mode 100644 index 0a4243e..0000000 Binary files a/lib/Aforge.NET/AForge.Math.dll and /dev/null differ diff --git a/lib/Aforge.NET/AForge.Vision.GlyphRecognition.dll b/lib/Aforge.NET/AForge.Vision.GlyphRecognition.dll index 9cdf907..067b528 100644 Binary files a/lib/Aforge.NET/AForge.Vision.GlyphRecognition.dll and b/lib/Aforge.NET/AForge.Vision.GlyphRecognition.dll differ diff --git a/lib/Aforge.NET/AForge.dll b/lib/Aforge.NET/AForge.dll deleted file mode 100644 index cfa351c..0000000 Binary files a/lib/Aforge.NET/AForge.dll and /dev/null differ