Example #1
0
        public static (KxTransform Transform, List <Marker> Markers) Calibrate(CvColor cvColor, CvCameraSpace cs)
        {
            //Define Board
            var cube = CoordinateDefinition.Microcube();
            //Look for Board
            var markers = Vision.FindAruco(cvColor);

            if (!markers.Any())
            {
                throw new Exception("No calibration pattern could be found in the image!");
            }
            //Calculate Camera Pose
            return(Vision.GetPoseFromImage(cube, cs, markers), markers);
        }
Example #2
0
        public static KxTransform GetPoseFromXef(string xefPath)
        {
            //Create a defined registration pattern - in this case a cube
            var cube = CoordinateDefinition.Microcube();
            //Find registration
            var xef     = new Xef(xefPath);
            var colorCv = xef.LoadCvColorFrame(0);
            //Find and draw (make sure it can be found)
            var markers = Vision.FindAruco(colorCv);
            //Vision.DrawAruco(colorCv).Show();

            //Calculate pose
            var _3dImage    = xef.LoadCVCameraSpace(5);
            var kxTransform = Vision.GetPoseFromImage(cube, _3dImage, markers);

            return(kxTransform);
        }
Example #3
0
        /// <summary>
        /// Applies the input transform in reverse of the KxPosition to real to determine
        /// the quality of the pose estimation.
        /// </summary>
        /// <param name="pose">the input pose 4x4 matrix</param>
        /// <param name="def">the definition of the ARUCO pattern in real space</param>
        /// <param name="markers">the markers found in the image</param>
        /// <returns>a list of deltas of each transformed center vs the real center</returns>
        public static List <float> ValidatePose(MatOfFloat pose, CoordinateDefinition def, List <Marker> markers)
        {
            List <float> deltas = new List <float>();

            markers.ForEach(m =>
            {
                if (def.ContainsCode(m.Id))
                {
                    var realPos = def.CenterDefinitions[m.Id];
                    var kPos    = m.KxCenter;
                    var ptTx    = pose.TransformPoint3f(kPos);
                    var delta   = (ptTx - realPos).Magnitude();
                    if (!float.IsNaN(delta))
                    {
                        deltas.Add(delta);
                    }
                }
            });
            return(deltas);
        }
Example #4
0
        public static CoordinateDefinition Microcube()
        {
            var cubeDepth   = 0.052f;
            var markerFar   = 0.0475f;
            var markerClose = 0.0025f;
            var width       = 0.045f;

            var bd = new CoordinateDefinition();

            //Side 1 (1,2,3,4) - Proximal Cube Face
            bd.CenterDefinitions.Add(0, new Point3f(0.025f, 0.025f, -cubeDepth));
            bd.CenterDefinitions.Add(1, new Point3f(-0.025f, 0.025f, -cubeDepth));
            bd.CenterDefinitions.Add(2, new Point3f(0.025f, -0.025f, -cubeDepth));
            bd.CenterDefinitions.Add(3, new Point3f(-0.025f, -0.025f, -cubeDepth));

            //Side 2 (4,5,6,7) - Right Cube Face
            bd.CenterDefinitions.Add(4, new Point3f(-cubeDepth, 0.025f, -0.025f));
            bd.CenterDefinitions.Add(5, new Point3f(-cubeDepth, 0.025f, 0.025f));
            bd.CenterDefinitions.Add(6, new Point3f(-cubeDepth, -0.025f, -0.025f));
            bd.CenterDefinitions.Add(7, new Point3f(-cubeDepth, -0.025f, 0.025f));

            //Side 3 (8,9,10,11) - Distal Cube Face
            bd.CenterDefinitions.Add(8, new Point3f(-0.025f, 0.025f, cubeDepth));
            bd.CenterDefinitions.Add(9, new Point3f(0.025f, 0.025f, cubeDepth));
            bd.CenterDefinitions.Add(10, new Point3f(-0.025f, -0.025f, cubeDepth));
            bd.CenterDefinitions.Add(11, new Point3f(0.025f, -0.025f, cubeDepth));

            //Side 4 (12,13,14,15) - Left Cube Face
            bd.CenterDefinitions.Add(12, new Point3f(cubeDepth, 0.025f, 0.025f));
            bd.CenterDefinitions.Add(13, new Point3f(cubeDepth, 0.025f, -0.025f));
            bd.CenterDefinitions.Add(14, new Point3f(cubeDepth, -0.025f, 0.025f));
            bd.CenterDefinitions.Add(15, new Point3f(cubeDepth, -0.025f, -0.025f));

            //Side 5  - Top Cube Face
            bd.CenterDefinitions.Add(16, new Point3f(0.025f, cubeDepth, 0.025f));
            bd.CenterDefinitions.Add(17, new Point3f(-0.025f, cubeDepth, 0.025f));
            bd.CenterDefinitions.Add(18, new Point3f(0.025f, cubeDepth, -0.025f));
            bd.CenterDefinitions.Add(19, new Point3f(-0.025f, cubeDepth, -0.025f));
            return(bd);
        }
Example #5
0
        /// <summary>
        /// Calculates the camera pose (Kinect space to real space transform) based on coordinate definition
        /// and detected markers
        /// </summary>
        /// <param name="def">the definition of the visible coordinates</param>
        /// <param name="_3dImage">the 3d points mapped to Kinect color coordinates</param>
        /// <param name="markers">the detected markers in the color image</param>
        /// <returns>a 4x4 matrix of the camera pose</returns>
        public static KxTransform GetPoseFromImage(CoordinateDefinition def, CvCameraSpace cvcs, List <Marker> markers)
        {
            MatOfPoint3f sourcePts = new MatOfPoint3f();
            MatOfPoint3f destPts   = new MatOfPoint3f();

            if (markers != null)
            {
                //For each marker found, look up Kinect position (2D -> 3D), find corresponding real position
                //Add KPos and RealPos to two arrays to calculate the transform between
                markers.ForEach(m =>
                {
                    if (def.ContainsCode(m.Id))
                    {
                        m.KxCenter = MarkerProcessor.FindCenter(m, cvcs);
                    }
                });

                //Todo: Take N best markers (highest mask sum means better 3d data)
                var        ordered   = markers.OrderByDescending(m => m.MaskSum.Val0).ToList();
                MatOfFloat tx        = null;
                var        lastDelta = float.MinValue;
                double     lastStd   = float.MinValue;

                foreach (var m in ordered)
                {
                    //ADD CENTER
                    var realPos = def.CenterDefinitions[m.Id];
                    if (!float.IsInfinity(m.KxCenter.X))
                    {
                        //Source is Kinect position
                        sourcePts.Add(m.KxCenter);
                        //Destination is actual physical location
                        destPts.Add(realPos);
                        _logger.Info($"Adding point {m.Id} : {m.KxCenter} =>");
                        _logger.Info($"{realPos}");
                    }

                    //ADD CORNERS
                    for (int p = 0; p < m.Points.Length; p++)
                    {
                        var corn             = m.Points[p];
                        var kxCornerPosition = MarkerProcessor.FindLocation(corn, cvcs);
                        if (!float.IsInfinity(kxCornerPosition.X))
                        {
                            //Source is Kinect position
                            sourcePts.Add(kxCornerPosition);
                            //Destination is actual physical location
                            destPts.Add(def.CornerDefinitions[m.Id][p]);
                            _logger.Info($"Adding point {m.Id} : {kxCornerPosition} =>");
                            _logger.Info($"{def.CornerDefinitions[m.Id][p]}");

                            if (sourcePts.Total() >= 3)
                            {
                                _logger.Info($"Using {sourcePts.Count} markers to find pose...");
                                var newTx    = Transform.TransformBetween(sourcePts, destPts);
                                var deltas   = ValidatePose(newTx, def, markers);
                                var avgDelta = deltas.Average();
                                var std      = deltas.StdDev();

                                //If it is better update
                                if (tx == null || Math.Abs(avgDelta) < Math.Abs(lastDelta))
                                {
                                    lastDelta = avgDelta;
                                    lastStd   = std;
                                    tx        = newTx;
                                }
                                else
                                {
                                    break;
                                }
                            }
                        }
                    }

                    if (sourcePts.Total() >= 15)
                    {
                        _logger.Info($"Using {sourcePts.Count} markers to find pose...");
                        var newTx    = Transform.TransformBetween(sourcePts, destPts);
                        var deltas   = ValidatePose(newTx, def, markers);
                        var avgDelta = deltas.Average();
                        var std      = deltas.StdDev();

                        //If it is better update
                        if (tx == null || Math.Abs(avgDelta) < Math.Abs(lastDelta))
                        {
                            lastDelta = avgDelta;
                            lastStd   = std;
                            tx        = newTx;
                        }
                        else
                        {
                            break;
                        }
                    }
                }

                _logger.Info($"Pose calculated with average delta of : ");
                _logger.Info($"{(lastDelta * 1000).ToString("N3")} ± {(lastStd * 1000).ToString("N3")} mm");


                //Need to flip Z to get into DICOM coordinates
                if (!sourcePts.Any() || !destPts.Any())
                {
                    throw new Exception("No points to transform!");
                }

                var kxTx = new KxTransform(tx.To2DArray());

                return(kxTx);
            }
            throw new ArgumentException("Markers cannot be null.");
        }
Example #6
0
        public static void BuildRegistrationCube(string path, double cmWidth = 10.4)
        {
            var definition = CoordinateDefinition.Cube(11 * 2.5, cmWidth);

            var dir   = Path.GetDirectoryName(path);
            var sides = new Dictionary <string, int>() //side, starting index id
            {
                { "Side1", 0 },
                { "Side2", 4 },
                { "Side3", 8 },
                { "Side4", 12 },
                { "Side5", 16 },
                { "Side6", 20 },
            };

            var pixelsPerCM = 300 / 2.54;

            //2 SQUARES WIDE, 2 SQUARES HIGH, 150 margin on width,
            //100 margin on height
            var paperPxWidth  = pixelsPerCM * cmWidth; //11in - 1in margin *300px/in
            var paperPxHeight = pixelsPerCM * cmWidth; //17in - 1in margin *300px/in
            var markerPxWidth = (int)(pixelsPerCM * 4.5 / 10.4 * cmWidth);
            var marginWidth   = (int)((paperPxWidth - 2.0 * markerPxWidth) / 4);
            var marginHeight  = marginWidth;
            var rows          = 2;
            var columns       = 2;

            var id = 0;

            foreach (var side in sides)
            {
                Rect roi = new Rect(0, 0, markerPxWidth, markerPxWidth);
                using (var outputImage = new Mat(new Size(paperPxWidth, paperPxHeight), MatType.CV_8UC1, Scalar.White))
                {
                    for (var y = 0; y < rows; y++)
                    {
                        roi.Top = y * markerPxWidth + marginHeight * (y * 2 + 1);

                        for (var x = 0; x < columns; x++)
                        {
                            roi.Left = x * markerPxWidth + marginWidth * (x * 2 + 1);

                            using (var roiMat = new Mat(outputImage, roi))
                                using (var markerImage = new Mat())
                                    using (var dict2 = CvAruco.GetPredefinedDictionary(PredefinedDictionaryName.Dict6X6_250))
                                    {
                                        CvAruco.DrawMarker(dict2, id++, markerPxWidth, markerImage, 1);
                                        markerImage.CopyTo(roiMat);
                                    }
                        }
                    }

                    var crossHairWidth = 1 * pixelsPerCM; // 1 cm
                    var crossHairColor = new Scalar(25, 25, 25);
                    Cv2.Line(outputImage, new Point((float)paperPxWidth / 2, (float)paperPxHeight / 2), new Point((float)paperPxWidth / 2, (float)(paperPxHeight / 2 - crossHairWidth)), crossHairColor);
                    Cv2.Line(outputImage, new Point((float)paperPxWidth / 2, (float)paperPxHeight / 2), new Point((float)paperPxWidth / 2, (float)(paperPxHeight / 2 + crossHairWidth)), crossHairColor);
                    Cv2.Line(outputImage, new Point((float)paperPxWidth / 2, (float)paperPxHeight / 2), new Point((float)(paperPxWidth / 2 - crossHairWidth), (float)(paperPxHeight / 2)), crossHairColor);
                    Cv2.Line(outputImage, new Point((float)paperPxWidth / 2, (float)paperPxHeight / 2), new Point((float)(paperPxWidth / 2 + crossHairWidth), (float)(paperPxHeight / 2)), crossHairColor);

                    Cv2.PutText(outputImage, side.Key, new Point((float)paperPxWidth / 2 - marginWidth / 1.3, (float)paperPxHeight), HersheyFonts.HersheyPlain, 1.5 / 10.4 * cmWidth, new Scalar(25, 25, 25), 1);
                    path = Path.Combine(dir, side.Key + ".png");
                    Cv2.ImWrite(path, outputImage);
                }
            }
        }
Example #7
0
        public static CoordinateDefinition Cube(double cubeDepthCM, double squareWidthCM)
        {
            var pixelsPerCM   = 300 / 2.54;
            var cmWidth       = squareWidthCM * 10.4 / 4.5;
            var paperPxWidth  = pixelsPerCM * cmWidth; //11in - 1in margin *300px/in
            var paperPxHeight = pixelsPerCM * cmWidth; //17in - 1in margin *300px/in
            var markerPxWidth = (int)(pixelsPerCM * 4.5 / 10.4 * cmWidth);
            var marginWidthPx = (int)((paperPxWidth - 2.0 * markerPxWidth) / 4);
            var marginHeight  = marginWidthPx;

            var   markerFar   = (float)((markerPxWidth + marginWidthPx) / pixelsPerCM);
            var   markerClose = (float)(marginWidthPx / pixelsPerCM);
            float cubeDepth   = (float)cubeDepthCM / 2.0f;
            var   width       = (float)squareWidthCM;

            var bd = new CoordinateDefinition();

            //Side 1 (1,2,3,4) - Proximal Cube Face
            bd.Add(0, new Point3f(markerFar, markerFar, -cubeDepth),
                   width, new Point3f(-1, 0, 0), new Point3f(0, -1, 0));
            bd.Add(1, new Point3f(-markerClose, markerFar, -cubeDepth),
                   width, new Point3f(-1, 0, 0), new Point3f(0, -1, 0));
            bd.Add(2, new Point3f(markerFar, -markerClose, -cubeDepth),
                   width, new Point3f(-1, 0, 0), new Point3f(0, -1, 0));
            bd.Add(3, new Point3f(-markerClose, -markerClose, -cubeDepth),
                   width, new Point3f(-1, 0, 0), new Point3f(0, -1, 0));

            //Side 2 (4,5,6,7) - Right Cube Face
            bd.Add(4, new Point3f(-cubeDepth, markerFar, -markerFar),
                   width, new Point3f(0, 0, 1), new Point3f(0, -1, 0));
            bd.Add(5, new Point3f(-cubeDepth, markerFar, markerClose),
                   width, new Point3f(0, 0, 1), new Point3f(0, -1, 0));
            bd.Add(6, new Point3f(-cubeDepth, -markerClose, -markerFar),
                   width, new Point3f(0, 0, 1), new Point3f(0, -1, 0));
            bd.Add(7, new Point3f(-cubeDepth, -markerClose, markerClose),
                   width, new Point3f(0, 0, 1), new Point3f(0, -1, 0));

            //Side 3 (8,9,10,11) - Distal Cube Face
            bd.Add(8, new Point3f(-markerFar, markerFar, cubeDepth),
                   width, new Point3f(1, 0, 0), new Point3f(0, -1, 0));
            bd.Add(9, new Point3f(markerClose, markerFar, cubeDepth),
                   width, new Point3f(1, 0, 0), new Point3f(0, -1, 0));
            bd.Add(10, new Point3f(-markerFar, -markerClose, cubeDepth),
                   width, new Point3f(1, 0, 0), new Point3f(0, -1, 0));
            bd.Add(11, new Point3f(markerClose, -markerClose, cubeDepth),
                   width, new Point3f(1, 0, 0), new Point3f(0, -1, 0));

            //Side 4 (12,13,14,15) - Left Cube Face
            bd.Add(12, new Point3f(cubeDepth, markerFar, markerFar),
                   width, new Point3f(0, 0, -1), new Point3f(0, -1, 0));
            bd.Add(13, new Point3f(cubeDepth, markerFar, -markerClose),
                   width, new Point3f(0, 0, -1), new Point3f(0, -1, 0));
            bd.Add(14, new Point3f(cubeDepth, -markerClose, markerFar),
                   width, new Point3f(0, 0, -1), new Point3f(0, -1, 0));
            bd.Add(15, new Point3f(cubeDepth, -markerClose, -markerClose),
                   width, new Point3f(0, 0, -1), new Point3f(0, -1, 0));

            //Side 5  - Top Cube Face
            bd.Add(16, new Point3f(markerFar, cubeDepth, markerFar),
                   width, new Point3f(-1, 0, 0), new Point3f(0, 0, -1));
            bd.Add(17, new Point3f(-markerClose, cubeDepth, markerFar),
                   width, new Point3f(-1, 0, 0), new Point3f(0, 0, -1));
            bd.Add(18, new Point3f(markerFar, cubeDepth, markerClose),
                   width, new Point3f(-1, 0, 0), new Point3f(0, 0, -1));
            bd.Add(19, new Point3f(-markerClose, cubeDepth, markerClose),
                   width, new Point3f(-1, 0, 0), new Point3f(0, 0, -1));

            return(bd);
        }