Metis_xctg/Adjust/CameraImageStitchMethod.cs

/*
    拼接的方法封装到 MatchPicRealtime(Mat matNew, Mat matBig)函数中。
    函数执行流程：
    1、采用OpenCvSharp.Feature2D.ORB的DetectAndCompute算法，检测和计算特征点。
    2、采用OpenCvSharp.DescriptorMatcher.BFMatcher的KnnMatch算法，对检测出来的特征点进行匹配，得到DMatch对象的集合。
    3、筛选DMatch对象的集合，计算得到横向距离差和纵向距离差。
    4、对匹配的结果做过滤，将拼接好的图像返回。 
 */

using OpenCvSharp;
using System;
using System.Collections.Generic;
using System.Linq;

namespace PaintDotNet.Adjust
{
    /// <summary>
    /// 图像处理
    /// </summary>
    public unsafe class AdjustMethods
    {
        public static Mat LastMat;
        public static double LastX;
        public static double LastY;
        private static Mat BackupMat;
        private static Rect LastRect;

        /// <summary>
        /// 图像拼接方法，返回拼接好的mat
        /// </summary>
        /// <param name="matSrc">需要拼接的mat1</param>
        /// <param name="matTo">需要拼接的mat2</param>
        /// <param name="errorFlag">拼接成功/失败</param>
        /// <param name="rect">拼接的矩形边框</param>
        /// <returns></returns>
        public static Mat MatchPicRealtime(Mat matSrc, Mat matTo, out Boolean errorFlag, out Rect rect)
        {
            errorFlag = false;
            Mat mat = matSrc.Clone();
            try
            {
                BackupMat = LastMat;
                mat = MatchPicRealtime(matSrc, matTo);
            }
            catch (Exception)
            {
                LastMat = BackupMat;
                errorFlag = true;
            }
            rect = LastRect;
            return mat;
        }
        public static Mat MatchPicRealtime(Mat matNew, Mat matBig)
        {
            if (LastMat == null)
            {
                LastMat = matNew;
                return matNew.Clone();
            }
            using (Mat matSrcRet = new Mat())
            using (Mat matToRet = new Mat())
            {
                KeyPoint[] keyPointsSrc, keyPointsTo;
                using (var surf = OpenCvSharp.ORB.Create(800))
                {
                    surf.DetectAndCompute(LastMat, null, out keyPointsSrc, matSrcRet);
                    surf.DetectAndCompute(matNew, null, out keyPointsTo, matToRet);
                    LastMat = matNew;
                }

                using (var bfMatcher = new OpenCvSharp.BFMatcher())//FlannBasedMatcher
                {
                    var matches = bfMatcher.KnnMatch(matSrcRet, matToRet, k: 2);

                    var pointsSrc = new List<Point2f>();
                    var pointsDst = new List<Point2f>();
                    var goodMatches = new List<DMatch>();
                    //数组按照元素个数由多到少排序
                    var arrXMatches = new List<int>();
                    var arrYMatches = new List<int>();
                    float XCompute = 0; float YCompute = 0; int coutPt = 0;
                    foreach (DMatch[] items in matches.Where(x => x.Length > 1))
                    {
                        if (items[0].Distance < 0.5 * items[1].Distance)
                        {
                            var X_dis = (keyPointsSrc[items[0].QueryIdx].Pt.X - keyPointsTo[items[0].TrainIdx].Pt.X);
                            var Y_dis = (keyPointsSrc[items[0].QueryIdx].Pt.Y - keyPointsTo[items[0].TrainIdx].Pt.Y);
                            arrXMatches.Add((int)X_dis);
                            arrYMatches.Add((int)Y_dis);
                        }
                    }
                    int[] arrX = arrXMatches.ToArray();
                    int[] arrY = arrYMatches.ToArray();
                    Array.Sort(arrX);
                    Array.Sort(arrY);
                    int valX1 = arrX[0];//当前
                    int valX2 = 0;//上一个
                    int timeX1 = 1;//当前
                    int timeX2 = 0;//上一个
                    for (int sortI = 1; sortI < arrX.Length; sortI++)
                    {
                        if (arrX[sortI] == valX1)
                        {
                            timeX1++;
                        }
                        else
                        {
                            if (timeX1 > timeX2)
                            {
                                valX2 = valX1;
                                timeX2 = timeX1;
                            }
                            valX1 = arrX[sortI];
                            timeX1 = 1;
                        }
                    }
                    if (timeX1 > timeX2)
                    {
                        valX2 = valX1;
                        timeX2 = timeX1;
                    }
                    int valY1 = arrY[0];//当前
                    int valY2 = 0;//上一个
                    int timeY1 = 1;//当前
                    int timeY2 = 0;//上一个
                    for (int sortI = 1; sortI < arrY.Length; sortI++)
                    {
                        if (arrY[sortI] == valY1)
                        {
                            timeY1++;
                        }
                        else
                        {
                            if (timeY1 > timeY2)
                            {
                                valY2 = valY1;
                                timeY2 = timeY1;
                            }
                            valY1 = arrY[sortI];
                            timeY1 = 1;
                        }
                    }
                    if (timeY1 > timeY2)
                    {
                        valY2 = valY1;
                        timeY2 = timeY1;
                    }

                    foreach (DMatch[] items in matches.Where(x => x.Length > 1))
                    {
                        if (items[0].Distance < 0.5 * items[1].Distance)
                        {
                            //pointsSrc.Add(keyPointsSrc[items[0].QueryIdx].Pt);
                            //pointsDst.Add(keyPointsTo[items[0].TrainIdx].Pt);
                            //goodMatches.Add(items[0]);
                            var X_dis = (keyPointsSrc[items[0].QueryIdx].Pt.X - keyPointsTo[items[0].TrainIdx].Pt.X);
                            var Y_dis = (keyPointsSrc[items[0].QueryIdx].Pt.Y - keyPointsTo[items[0].TrainIdx].Pt.Y);
                            if ((valX2 >= X_dis - 5 && valX2 <= X_dis + 5) && (valY2 >= Y_dis - 5 && valY2 <= Y_dis + 5))
                            {
                                Console.WriteLine("横向距离差：{0} 纵向距离差：{1}", X_dis, Y_dis);
                                pointsSrc.Add(keyPointsSrc[items[0].QueryIdx].Pt);
                                pointsDst.Add(keyPointsTo[items[0].TrainIdx].Pt);
                                goodMatches.Add(items[0]);
                                //arrXMatches.Add((int)X_dis);
                                ////arrYMatches.Add((int)Y_dis);
                                coutPt++;
                                XCompute += X_dis; YCompute += Y_dis;
                            }
                            else
                            {
                                Console.WriteLine("横向距离差：{0} 纵向距离差：{1} 舍弃", X_dis, Y_dis);
                            }
                        }
                    }
                    LastX += XCompute / coutPt;
                    LastY += YCompute / coutPt;// 600;//
                    //Console.WriteLine("平均横向距离差：{0}  平均纵向距离差：{1}", currentX, currentY);//计算原图像相对于新图像的(距离差)、-->变换矩阵

                    var outMat = new Mat();

                    // 算法RANSAC对匹配的结果做过滤
                    var pSrc = pointsSrc.ConvertAll(Point2fToPoint2d);
                    var pDst = pointsDst.ConvertAll(Point2fToPoint2d);
                    var outMask = new Mat();
                    var result = new Mat();
                    // 如果原始的匹配结果为空, 则跳过过滤步骤
                    if (pSrc.Count > 0 && pDst.Count > 0)
                    {
                        int srcOriX = 0;
                        int srcOriY = 0;
                        int newOriX = 0;
                        int newOriY = 0;

                        // Cv2.FindHomography(pSrc, pDst, HomographyMethods.Ransac, mask: outMask);

                        //得到变换矩阵后拼接图像
                        int viewWidth = matBig.Width;// 2448;
                        int viewHeight = matBig.Height;// 2040;
                        if (LastX <= 0)
                        {
                            viewWidth = (int)(viewWidth - LastX);
                            srcOriX = (int)(-LastX);
                            newOriX = 0;
                            LastX = 0;
                        }
                        else
                        {
                            viewWidth = (int)Math.Max(LastX + matNew.Width, matBig.Width);
                            newOriX = (int)LastX;
                            //for (int i = 0; i < pDst.Count; i++)
                            //{
                            //    pDst[i] = new Point2d(pSrc[i].X, pDst[i].Y);
                            //}
                        }

                        if (LastY <= 0)
                        {
                            viewHeight = (int)(viewHeight - LastY);
                            srcOriY = (int)(-LastY);
                            newOriY = 0;
                            LastY = 0;
                        }
                        else
                        {
                            //for (int i = 0; i < pDst.Count; i++)
                            //{
                            //    pDst[i] = new Point2d(pDst[i].X, pSrc[i].Y);
                            //}
                            newOriY = (int)LastY;
                            viewHeight = (int)Math.Max(matBig.Height, matNew.Height + LastY);
                        }

                        result = new Mat(new OpenCvSharp.Size(viewWidth, viewHeight), matNew.Type());

                        //Rect rectSrc = new Rect(0, 0, matNew.Width, matNew.Height);
                        //Mat tempMatSrc = matNew.Clone(rectSrc);
                        Mat tempMatSrc = matNew.Clone();
                        //复制原有区域part1(<--用户选中的区域)到新图像(<--原图像)
                        Mat mask = tempMatSrc.CvtColor(ColorConversionCodes.RGBA2GRAY);

                        Mat posSrc = new Mat(result, new Rect(newOriX, newOriY, matNew.Width, matNew.Height));
                        tempMatSrc.CopyTo(posSrc, mask);//原图像复制到新图像


                        //Rect rect = new Rect(0, 0, matBig.Width, matBig.Height);
                        //Mat tempMat = matBig.Clone(rect);
                        Mat tempMat = matBig.Clone();
                        //复制原有区域part2(<--用户选中的区域)到新图像(<--原图像)
                        mask = tempMat.CvtColor(ColorConversionCodes.RGBA2GRAY);
                        Mat pos = new Mat(result, new Rect(srcOriX, srcOriY, matBig.Width, matBig.Height));
                        tempMat.CopyTo(pos, mask);//原图像复制到新图像
                        LastRect = new Rect(newOriX, newOriY, matNew.Width, matNew.Height);
                        //Bitmap part2 = OpenCvSharp.Extensions.BitmapConverter.ToBitmap(result);
                        ////、、R3a
                        //part2.Save(@"ResSurfA" + indexRes + ".jpg", System.Drawing.Imaging.ImageFormat.Jpeg);
                        return result;
                    }
                    result = matNew.Clone();
                    return result;
                }
            }
        }

        private static Point2d Point2fToPoint2d(Point2f input)
        {
            Point2d p2 = new Point2d(input.X, input.Y);
            return p2;
        }
    }
}