Main-Repo/SourceCode/AIReconstruction/Reconstruction3DHelper/ContourHelper.cs

using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Linq;
using System.Runtime.InteropServices;
using AI.Common;
using AI.Common.Tools;
using AI.Common.Log;


namespace Reconstruction3DHelper
{
    /// <summary>
    /// 参照opencv里的freeman链码
    /// 由于freeman链码里只有0~7,分别表示8个不同方向
    /// 但我们的输入点可能不满足上述关系，因此增加了-1和8，分别表示重叠和间距较大的情况
    /// </summary>
    internal enum EnumChainCode
    {
        // 参考点和当前点重叠
        Overlap = -1,
        Up = 0,
        UpRight = 1,
        Right = 2,
        DownRight = 3,
        Down = 4,
        DownLeft = 5,
        Left = 6,
        UpLeft = 7,
        // 当前点不在参考点8邻域内
        Gap = 8,
    }

    /// <summary>
    /// 线的类型
    /// </summary>
    public enum EnumLineType
    {
        EightNeighborhood = 0,
        FourNeighborhood = 1,
    }

    /// <summary>
    /// 轮廓相关帮助类
    /// </summary>
    public class ContourHelper
    {
        /// <summary>
        /// float的精度
        /// </summary>
        public const float FloatPrecision = 0.000001f;

        #region

        /// <summary>
        /// 参照opencv里的cv::DistanceTypes定义的
        /// </summary>
        public enum EnumDistType
        {
            User=-1,
            L1=1,
            L2=2,
            C=3,
            L12=4,
            Fair=5,
            Welsch=6,
            Huber=7,
        }

        #endregion


        #region dllimport

        [DllImport(@"PlaqueProcessingCpp.dll", CallingConvention = CallingConvention.Cdecl)]
        public static extern bool FitLine(IntPtr roiVec, int len, float[] matrix, EnumDistType distType,
            double param, double reps, double aeps);
        #endregion


        #region public

        /// <summary>
        /// 去除轮廓毛刺
        /// </summary>
        /// <param name="points"></param>
        /// <param name="filterRadius"></param>
        /// <returns></returns>
        public static Point2D[] ContourDeburr(Point2D[] points,int filterRadius=3)
        {
            // 将轮廓点重采样，使其符合freeman chain code的要求
            var resampledPoints = ContourResample(points);
            // 重采样后点数过少，直接返回
            int resampledPointCount = resampledPoints.Length;
            if (resampledPointCount <= 2)
            {
                return resampledPoints;
            }
            // 分别计算当前点和前一个点，后一个点的chain code，以此来判断当前点是否为毛刺点
            List<int> burrPointIndexes = new List<int>();
            for (int ni = 0; ni < resampledPointCount; ni++)
            {
                int indexPre = ni;
                int indexBack = ni;
                DecrementInCirculation(ref indexPre, resampledPointCount);
                IncrementInCirculation(ref indexBack, resampledPointCount);
                if (IsBurrPoint(resampledPoints[indexPre], resampledPoints[ni], resampledPoints[indexBack]))
                {
                    burrPointIndexes.Add(ni);
                }
            }
            // 如果没有毛刺点，可以直接返回
            if (burrPointIndexes.Count <= 0)
            {
                return resampledPoints;
            }
            // 有毛刺点，需要去除毛刺
            // 考虑到毛刺可能突出不止一个像素，因此从每个毛刺点左右分别找与已有轮廓线重合的点
            // 只删除与毛刺相邻的重叠点（避免影响其他轮廓交叠位置处）
            List<int> indexesToRemove = new List<int>();
            foreach (var burrPointIndex in burrPointIndexes)
            {
                bool preSearch = true;
                bool backSearch = true;
                int indexPre = burrPointIndex;
                int indexBack = burrPointIndex;
                int searchedCountPre = 0;
                int searchedCountBack = 0;
                while (preSearch || backSearch)
                {
                    // 向前搜索
                    if (preSearch)
                    {
                        DecrementInCirculation(ref indexPre, resampledPointCount);
                        searchedCountPre += 1;
                    }
                    // 向后搜索
                    if (backSearch)
                    {
                        IncrementInCirculation(ref indexBack, resampledPointCount);
                        searchedCountBack += 1;
                    }
                    // 判断indexPre处的点，是否和毛刺点后方点相邻，如果相邻，则继续搜索
                    bool neighborPre = false;
                    for (int ni = 0; ni <= searchedCountBack; ni++)
                    {
                        int index = burrPointIndex;
                        IncrementInCirculation(ref index, resampledPointCount, ni);
                        var chainCodePre = ChainCodeToReferencePoint(resampledPoints[indexPre], resampledPoints[index]);
                        if (chainCodePre != EnumChainCode.Gap)
                        {
                            neighborPre = true;
                            break;
                        }
                    }
                    if (!neighborPre)
                    {
                        preSearch = false;
                    }
                    // 判断indexBack处的点，是否和毛刺点前方的点相邻，如果相邻，则继续搜索
                    bool neighborBack = false;
                    for (int ni = 0; ni <= searchedCountPre; ni++)
                    {
                        int index = burrPointIndex;
                        DecrementInCirculation(ref index, resampledPointCount, ni);
                        var chainCodeBack = ChainCodeToReferencePoint(resampledPoints[indexBack], resampledPoints[index]);
                        if (chainCodeBack != EnumChainCode.Gap)
                        {
                            neighborBack = true;
                            break;
                        }
                    }
                    if (!neighborBack)
                    {
                        backSearch = false;
                    }
                    if (searchedCountPre + searchedCountBack +1 >= resampledPointCount)
                    {
                        break;
                    }
                }
                // 逐个将当前位置处该删除的点添加到总的indexesToRemove里
                int indexStart = burrPointIndex;
                int indexEnd = burrPointIndex;
                if (searchedCountPre >= 2)
                {
                    DecrementInCirculation(ref indexStart, resampledPointCount, searchedCountPre-1);
                }
                if (searchedCountBack >= 2)
                {
                    IncrementInCirculation(ref indexEnd, resampledPointCount, searchedCountBack);
                }
                int indexRemove = indexStart;
                while (true)
                {
                    if (!indexesToRemove.Contains(indexRemove))
                    {
                        indexesToRemove.Add(indexRemove);
                    }
                    IncrementInCirculation(ref indexRemove, resampledPointCount);
                    if (indexRemove == indexEnd || indexStart == indexEnd)
                    {
                        break;
                    }
                }
            }
            // 没有找到要移除的点，直接返回
            if (indexesToRemove.Count <= 0)
            {
                return resampledPoints;
            }
            // 将待删除的点的序号，排序，从大到小排
            indexesToRemove.Sort();
            indexesToRemove.Reverse();
            List<Point2D> removedPoints = resampledPoints.ToList();
            foreach (var index in indexesToRemove)
            {
                removedPoints.RemoveAt(index);
            }
            // 删除后的点，可能会有空缺，补齐
            var pointsDeburred = ContourResample(removedPoints.ToArray());
            // 最后再对去毛刺后的点进行滤波
            int pointCount = pointsDeburred.Length;
            if (pointCount <= 2*filterRadius)
            {
                return pointsDeburred;
            }
            // 得到要滤波的x和y
            int len = pointCount + 2 * filterRadius;
            int idx = pointCount - filterRadius;
            double[] contourX = new double[len];
            double[] contourY = new double[len];
            for (int ni = 0; ni < len; ni++)
            {
                int index = (idx + ni) % pointCount;
                contourX[ni] = pointsDeburred[index].X;
                contourY[ni] = pointsDeburred[index].Y;
            }
            // 进行一维滤波
            double[] contourXFilt = GaussianBlur(contourX, filterRadius);
            double[] contourYFilt = GaussianBlur(contourY, filterRadius);
            // 转为输出
            Point2D[] pointReturn = new Point2D[pointCount];
            for (int ni = 0; ni < pointCount; ni++)
            {
                pointReturn[ni].X = (int)contourXFilt[ni + filterRadius];
                pointReturn[ni].Y = (int)contourYFilt[ni + filterRadius];
            }
            return pointReturn;
        }

        /// <summary>
        /// 轮廓重采样
        /// </summary>
        /// <param name="points"></param>
        /// <param name="lineType"></param>
        /// <returns></returns>
        public static Point2D[] ContourResample(Point2D[] points, EnumLineType lineType = EnumLineType.EightNeighborhood)
        {
            int pointNum = points.Length;
            if (pointNum <= 0)
            {
                return Array.Empty<Point2D>();
            }
            // 点数过少，直接复制
            if (pointNum <= 2)
            {
                Point2D[] result = new Point2D[pointNum];
                Array.Copy(result, points, pointNum);
                return result;
            }
            // 输入的轮廓点应该是逐像素相连的（8邻域），如果有点重叠或间距较大，则应先重采样成所需格式
            List<Point2D> pointsResampled = new List<Point2D>();
            Point2D pointPre, pointNi;
            // 第0个点的前一个点是最后一个点
            pointPre = points[pointNum - 1];
            for (int ni = 0; ni < pointNum; ni++)
            {
                pointNi = points[ni];
                // 计算当前点到前一个点之间的连线上所有的点
                var linePoints = PointsOnTheLine(pointPre, pointNi, lineType);
                // 由于前一个点已添加到pointsResampled里，但PointsOnTheLine的返回值是包含起点和终点的，所以从第1个点开始添加
                // 当前点和前一个点重叠时，PointsOnTheLine的返回值长度为1，因此不会重复添加重叠点
                for (int nj = 1; nj < linePoints.Length; nj++)
                {
                    pointsResampled.Add(linePoints[nj]);
                }
                pointPre = pointNi;
            }
            return pointsResampled.ToArray();
        }

        /// <summary>
        /// 循环自减
        /// </summary>
        /// <param name="index"></param>
        /// <param name="circleLen"></param>
        /// <returns></returns>
        public static void DecrementInCirculation(ref int index, int circleLen, int delta = 1)
        {
            delta = delta % circleLen;
            index -= delta;
            if (index < 0)
            {
                index += circleLen;
            }
        }

        /// <summary>
        /// 循环自增
        /// </summary>
        /// <param name="index"></param>
        /// <param name="circleLen"></param>
        /// <returns></returns>
        public static void IncrementInCirculation(ref int index, int circleLen, int delta = 1)
        {
            delta = delta % circleLen;
            index += delta;
            if (index >= circleLen)
            {
                index -= circleLen;
            }
        }

        /// <summary>
        /// 一维高斯滤波
        /// </summary>
        /// <param name="datas"></param>
        /// <param name="radius"></param>
        /// <returns></returns>
        public static double[] GaussianBlur(double[] datas, int radius)
        {
            double[] kernel = GaussianKernel(radius);
            int len = datas.Length;
            double[] datasFilt = new double[len];
            Array.Copy(datas, datasFilt, len);
            int kernelLen = kernel.Length;
            double[] temp = new double[kernelLen];
            for (int ni = radius; ni < len - radius; ni++)
            {
                double sum = 0;
                for (int nj = 0; nj < kernelLen; nj++)
                {
                    temp[nj] = datas[ni + nj - radius] * kernel[nj];
                    sum += temp[nj];
                }
                datasFilt[ni] = sum;
            }
            return datasFilt;
        }

        /// <summary>
        /// 获取两点之间的连线
        /// 主要参考了opencv的画线算法（该方法用整数的加减计算即可实现，效率较高，属于改进的Bresenham方法）
        /// （如果pointStart和pointEnd重叠，返回结果只有一个点）
        /// </summary>
        /// <param name="pointStart"></param>
        /// <param name="pointEnd"></param>
        /// <param name="lineType"></param>
        /// <returns></returns>
        public static Point2D[] PointsOnTheLine(Point2D pointStart, Point2D pointEnd,
            EnumLineType lineType = EnumLineType.EightNeighborhood)
        {
            if (lineType != EnumLineType.EightNeighborhood && lineType != EnumLineType.FourNeighborhood)
            {
                throw new ArgumentException("lineType should be 'EightNeighborhood' or 'FourNeighborhood'in this function.");
            }
            // 判断是否需要交换xy或交换起始和结束点
            int deltaX = 1;
            int deltaY = 1;
            int dx = pointEnd.X - pointStart.X;
            int dy = pointEnd.Y - pointStart.Y;
            int x = pointStart.X;
            int y = pointStart.Y;
            bool vertStartEnd = dx < 0;
            if (vertStartEnd)
            {
                // 交换开始点和结束点
                // 注: 这里用if dx<0 则 dx=-dx 来替代Math.abs
                dx = -dx;
                dy = -dy;
                x = pointEnd.X;
                y = pointEnd.Y;
            }
            if (dy < 0)
            {
                // 注: 这里用if dy<0 则 dy=-dy 来替代Math.abs
                dy = -dy;
                deltaY = -1;
            }
            bool vertXY = dy > dx;
            if (vertXY)
            {
                // 交换x和y
                int temp = dx;
                dx = dy;
                dy = temp;
                temp = deltaX;
                deltaX = deltaY;
                deltaY = temp;
            }
            // 得到控制循环的一些参数
            int error =0, count = 0;
            int plusErr =0, plusX =0, plusY =0;
            int minusErr = 0, minusX = 0, minusY = 0;
            // 注: 这里用 dy+dy 和 dx+dx 代替 2*dy 和 2*dx, 也是为了提高计算效率
            switch (lineType)
            {
                case EnumLineType.EightNeighborhood:
                    count = dx + 1;
                    error = dx - (dy + dy);
                    plusErr = dx + dx;
                    plusX = 0;
                    plusY = deltaY;
                    minusErr = -(dy + dy);
                    minusX = deltaX;
                    minusY = 0;
                    break;
                case EnumLineType.FourNeighborhood:
                    count = dx + dy + 1;
                    error = 0;
                    plusErr = (dx + dx) + (dy + dy);
                    plusX = -deltaX;
                    plusY = deltaY;
                    minusErr = -(dy + dy);
                    minusX = deltaX;
                    minusY = 0;
                    break;
            }
            if (vertXY)
            {
                // 交换plusX和plusY、minusX和minusY
                int temp = plusX;
                plusX = plusY;
                plusY = temp;
                temp = minusX;
                minusX = minusY;
                minusY = temp;
            }
            // 循环，得到线上所有点的坐标
            Point2D[] points = new Point2D[count];
            for (int ni = 0; ni < count; ni++)
            {
                int index = ni;
                if (vertStartEnd)
                {
                    index = count - 1 - ni;
                }
                points[index].X = x;
                points[index].Y = y;
                if (error < 0)
                {
                    y += plusY;
                    error += plusErr;
                    x += plusX;
                }
                error += minusErr;
                x += minusX;
                y += minusY;
            }
            return points;
        }

        /// <summary>
        /// 获取一段弧线上的所有点
        /// 如果 endAngle大于startAngle 则逆时针绘制，如果endAngle小于startAngle则顺时针绘制
        /// 用多边形逼近法（该方法虽然计算量相对较大，但可以较为方便地生成任意的圆弧）
        /// </summary>
        /// <param name="pointCenter"></param>
        /// <param name="radius"></param>
        /// <param name="startAngle">起始角度（角度，0~360）</param>
        /// <param name="angleSpan">角度范围（角度，-360~360，为正则从起始角度逆时针转，为负则顺时针转）</param>
        /// <returns></returns>
        public static Point2D[] PointsOnTheArc(Point2D pointCenter, float radius, double startAngle, double angleSpan)
        {
            // 设置多边形逼近的步长，目前暂定边长约为5个像素
            int polygonSideLen = 5;
            double endAngle = startAngle + angleSpan;
            // 判断是否需要交换start和end
            bool vertStartEnd = angleSpan < 0;
            if (vertStartEnd)
            {
                double temp = startAngle;
                startAngle = endAngle;
                endAngle = temp;
            }
            // 把角度转换到0~360范围内
            while (startAngle < 0)
            {
                startAngle += 360;
                endAngle += 360;
            }
            while (endAngle > 360)
            {
                endAngle -= 360;
                startAngle -= 360;
            }
            if (endAngle - startAngle > 360)
            {
                startAngle = 0;
                endAngle = 360;
            }
            // 迭代次数（根据radius和弧度范围，大致确定迭代次数即可）
            int count = Math.Max(1, Convert.ToInt32(2 * Math.PI * radius * (endAngle - startAngle) / 360 / polygonSideLen));
            double alpha = (endAngle - startAngle) / count;
            double alphaRad = MathHelper.DegreeToRadian(alpha);
            double startAngleRad = MathHelper.DegreeToRadian(startAngle);
            double cos = Math.Cos(alphaRad);
            double sin = Math.Sin(alphaRad);
            double xi = radius * Math.Cos(startAngleRad);
            double yi = radius * Math.Sin(startAngleRad);
            double x, y;
            List<Point2D> points = new List<Point2D>();
            Point2D pointStart = new Point2D();
            // 图像坐标上y轴是向下的，所以y应该在pointCenter.Y的基础上减掉yi, 而x方向，因为图像坐标上x轴也是向右，所以是加
            pointStart.X = pointCenter.X + (int)xi;
            pointStart.Y = pointCenter.Y-(int)yi;
            // 将起始点加到points里
            points.Add(pointStart);
            Point2D pointEnd = new Point2D();
            for (int ni = 0; ni < count; ni++)
            {
                x = (xi * cos - yi * sin);
                y = (xi * sin + yi * cos);
                pointEnd.X = pointCenter.X +(int)x;
                pointEnd.Y = pointCenter.Y-(int)y;
                // 两点之间用直线连接
                var linePoints = PointsOnTheLine(pointStart, pointEnd);
                for (int nj = 1; nj < linePoints.Length; nj++)
                {
                    points.Add(linePoints[nj]);
                }
                // 递推
                xi = x;
                yi = y;
                pointStart = pointEnd;
            }
            // 如果交换了起始点和结束点，则翻转一下
            if (vertStartEnd)
            {
                points.Reverse();
            }
            return points.ToArray();
        }

        /// <summary>
        /// 获取一段椭圆上所有点
        /// 如果 endAngle大于startAngle 则逆时针绘制，如果endAngle小于startAngle则顺时针绘制
        /// 用多边形逼近法（该方法虽然计算量相对较大，但可以较为方便地生成任意的圆弧）
        /// </summary>
        /// <param name="pointCenter"></param>
        /// <param name="width"></param>
        /// <param name="height"></param>
        /// <param name="angle"></param>
        /// <param name="startAngle"></param>
        /// <param name="angleSpan"></param>
        /// <returns></returns>
        public static Point2D[] PointsOnTheEllipse(Point2D pointCenter, float width, float height, double angle,
            double startAngle, double angleSpan)
        {
            float halfWidth = width / 2;
            float halfHeight = height / 2;
            // 设置多边形逼近的步长，目前暂定边长约为5个像素
            int polygonSideLen = 5;
            // 将angle调整到合适的范围
            while (angle < 0)
            {
                angle += 360;
            }
            while (angle > 360)
            {
                angle -= 360;
            }
            double endAngle = startAngle + angleSpan;
            // 判断是否需要交换start和end
            bool vertStartEnd = angleSpan < 0;
            if (vertStartEnd)
            {
                double temp = startAngle;
                startAngle = endAngle;
                endAngle = temp;
            }
            // 把角度转换到0~360范围内
            while (startAngle < 0)
            {
                startAngle += 360;
                endAngle += 360;
            }
            while (endAngle > 360)
            {
                endAngle -= 360;
                startAngle -= 360;
            }
            if (endAngle - startAngle > 360)
            {
                startAngle = 0;
                endAngle = 360;
            }
            // 迭代次数（根据radius和弧度范围，大致确定迭代次数即可）
            int count = Math.Max(1, Convert.ToInt32(2 * Math.PI * Math.Sqrt(Math.Pow(halfWidth, 2) + Math.Pow(halfHeight, 2)) * (endAngle - startAngle) / 360 / polygonSideLen));
            double alpha = (endAngle - startAngle) / count;
            double angleRad = MathHelper.DegreeToRadian(angle);
            double startAngleRad = MathHelper.DegreeToRadian(startAngle);
            double cosAngle = Math.Cos(angleRad);
            double sinAngle = Math.Sin(angleRad);
            double xi = halfWidth * Math.Cos(startAngleRad);
            double yi = halfHeight * Math.Sin(startAngleRad);
            double tempAngle,tempRad;
            List<Point2D> points = new List<Point2D>();
            Point2D pointStart = new Point2D();
            // 图像坐标上y轴是向下的，所以y应该在pointCenter.Y的基础上减掉yi, 而x方向，因为图像坐标上x轴也是向右，所以是加
            pointStart.X = pointCenter.X+(int)(xi * cosAngle - yi * sinAngle);
            pointStart.Y = pointCenter.Y-(int)(xi * sinAngle + yi * cosAngle);
            // 将起始点加到points里
            points.Add(pointStart);
            Point2D pointEnd = new Point2D();
            for (int ni = 1; ni <= count; ni++)
            {
                tempAngle = startAngle + alpha * ni;
                tempRad = MathHelper.DegreeToRadian(tempAngle);
                xi = halfWidth * Math.Cos(tempRad);
                yi = halfHeight * Math.Sin(tempRad);
                pointEnd.X = pointCenter.X+(int)(xi * cosAngle - yi * sinAngle);
                pointEnd.Y = pointCenter.Y-(int)(xi * sinAngle + yi * cosAngle);
                // 两点之间用直线连接
                var linePoints = PointsOnTheLine(pointStart, pointEnd);
                for (int nj = 1; nj < linePoints.Length; nj++)
                {
                    points.Add(linePoints[nj]);
                }
                pointStart = pointEnd;
            }
            // 如果交换了起始点和结束点，则翻转一下
            if (vertStartEnd)
            {
                points.Reverse();
            }
            return points.ToArray();
        }

        /// <summary>
        /// 计算点到直线的距离
        /// </summary>
        public static double GetPointToLineDistance(Point2D lineStart, Point2D lineEnd, Point2D point)
        {
            if (lineStart.X == lineEnd.X)
            {
                return Math.Abs(point.X - lineStart.X);
            }
            double slope = (lineEnd.Y - lineStart.Y) / (lineEnd.X - lineStart.X);
            double intercept = (lineEnd.X * lineStart.Y - lineStart.X * lineEnd.Y) / (lineEnd.X - lineStart.X);
            return Math.Abs(slope * point.X - point.Y + intercept) / (Math.Sqrt(slope * slope + 1));
        }

        /// <summary>
        /// 计算点到直线的距离
        /// </summary>
        /// <param name="lineParam"></param>
        /// <param name="point"></param>
        /// <returns></returns>
        public static double GetPointToLineDistance(double[] lineParam, Point2D point)
        {
            if (lineParam[1] == 0)
            {
                return Math.Abs(point.X - lineParam[2]) / lineParam[0];
            }
            var A = lineParam[0];
            var C = lineParam[2];
            var a = point.X;
            var b = point.Y;
            var n = (A * a + A * A * b + C) / (A * A + 1);
            var m = a - A * n + A * b;
            if (Math.Abs(m - a) < 0.00001)
            {
                return 0;
            }
            var disX = m - a;
            var disY = n - b;
            return Math.Sqrt(disX * disX + disY * disY);
        }

        /// <summary>
        /// 计算两点间的直线斜率与截距
        /// </summary>
        public static void GetSlopeAndIntercept(Point2D FirstPoint, Point2D SeconsPoint, out double k, out double b)
        {
            double x1 = FirstPoint.X;
            double y1 = FirstPoint.Y;
            double x2 = SeconsPoint.X;
            double y2 = SeconsPoint.Y;

            if (x1 == x2)
            {
                k = double.NaN;
                b = double.NaN;
                return;
            }
            if (y1 == y2)
            {
                k = 0.0;
                b = y1;
                return;
            }
            k = ((y1 - y2) / (x1 - x2));
            b = y1 - x1 * k;
            return;
        }

        /// <summary>
        /// 射线法判断点是否在多边形内，即计算射线与多边形各边的交点，如果是奇数，则点在多边形内，否则在多边形外
        /// <param name="polygonPointList"></param> 多边形上的点，注意要按照顺序保存
        /// <param name="objectPoint"></param> 目标点,判断该点是否在多边形内的
        /// </summary>
        public static bool IsPointInPolygonRayMethod(Point2D[] polygonPointList, Point2D objectPoint)
        {
            if (polygonPointList.Length == 0)
            {
                return false;
            }
            int num = 0;
            // 用当前点(i)和前一个点(j)拉一条直线，计算与objectPoint点发出的射线（指向x轴正方向）的交点的个数
            int j = polygonPointList.Length - 1;
            for (int i = 0; i < polygonPointList.Length; i++)
            {
                Point2D start = new Point2D(polygonPointList[i].X, polygonPointList[i].Y);
                Point2D end = new Point2D(polygonPointList[j].X, polygonPointList[j].Y);
                // 点与多边形的顶点重合
                if (((start.X == objectPoint.X) && (start.Y == objectPoint.Y)) || ((end.X == objectPoint.X) && (end.Y == objectPoint.Y)))
                {
                    return true;
                }
                // 判断线段两端点是否在射线两侧
                if (((objectPoint.Y >= start.Y) && (objectPoint.Y < end.Y)) || ((objectPoint.Y >= end.Y) && (objectPoint.Y < start.Y)))
                {
                    // 线段上与射线 Y 坐标相同的点的 X 坐标
                    var x = start.X + (objectPoint.Y - start.Y) * (end.X - start.X) / (end.Y - start.Y);
                    // 点在多边形的边上
                    if (objectPoint.X == x)
                    {
                        return true;   
                    }
                    // 射线穿过多边形的边界
                    if (x > objectPoint.X)
                    {
                        ++num;
                    }
                }
                j = i;
            }
            // 射线穿过多边形边界的次数num为偶数时点在多边形外,返回false，否则返回true
            return (num % 2 == 0) ? false : true;
        }

        /// <summary>
        /// 拟合直线
        /// </summary>
        /// <param name="points"></param>
        /// <param name="removeAbnormal">是否移除异常点</param>
        /// <returns></returns>
        public static bool FitLines(Point2D[] points,out double[] lineParam, bool removeAbnormal = false)
        {
            lineParam = new double[3];
            try
            {
                // 为了避免直接删除输入数组points里的元素，复制一份出来
                var pointsCal = new Point2D[points.Length];
                Array.Copy(points, pointsCal,points.Length);
                // 移除异常数据
                if (removeAbnormal)
                {
                    // 原来颈动脉代码里的意思，应该是反复做三次移除异常数据
                    var removeTimes = 0;
                    while (removeTimes < 3)
                    {
                        pointsCal = RemoveAbnormalData(pointsCal);
                        removeTimes++;
                    }
                    // 因为这里的Point2D,X,Y里存的分别是Point3D的X和Z，所以这里将Point2D按Y排序，即为将投影点仍旧按照其帧顺序排序
                    // （因为在RemoveAbnormalData里根据点到直线的距离排序后，投影点的帧顺序会被打乱）
                    pointsCal = pointsCal.OrderBy(x => x.Y).ToArray();
                }
                int pointCount = pointsCal.Length;
                float[] matrix = new float[4];
                GCHandle hObject = GCHandle.Alloc(pointsCal, GCHandleType.Pinned);
                IntPtr pObject = hObject.AddrOfPinnedObject();
                // ToDo: 是否需要将拟合直线的参数开放出来（之前颈动脉代码里也是写死的）
              
                var ret = FitLine(pObject, pointCount, matrix, EnumDistType.L2, 0, 0.001, 0.001);
                                     
                float x0 = matrix[0];
                float x1 = matrix[1];
                float x2 = matrix[2];
                float x3 = matrix[3];
                if (x0 < FloatPrecision)
                {
                    lineParam[0] = 1;
                    lineParam[1] = 0;
                    lineParam[2] = x2;
                }
                else
                {
                    double k = x1 / x0;
                    double b = x3 - k * x2;
                    lineParam[0] = k;
                    lineParam[1] = -1;
                    lineParam[2] = b;
                }
                return true;
            }
            catch (Exception excep)
            {
                LogHelper.ErrorLog("ContourHelper.FitLine Failed:"+excep);
                return false;
            }
        }
        

        #endregion


        #region private

        /// <summary>
        /// 计算当前点相对于参考点的chain code
        /// </summary>
        /// <param name="pointRef"></param>
        /// <param name="pointToAdd"></param>
        /// <returns></returns>
        private static EnumChainCode ChainCodeToReferencePoint(Point2D pointRef, Point2D pointToAdd)
        {
            int deltaX = pointToAdd.X - pointRef.X;
            int deltaY = pointToAdd.Y - pointRef.Y;
            if (deltaX == 0 && deltaY == 0)
            {
                return EnumChainCode.Overlap;
            }
            if (deltaX == -1 && deltaY == 0)
            {
                return EnumChainCode.Left;
            }
            if (deltaX == 1 && deltaY == 0)
            {
                return EnumChainCode.Right;
            }
            if (deltaX == -1 && deltaY == -1)
            {
                return EnumChainCode.UpLeft;
            }
            if (deltaX == 0 && deltaY == -1)
            {
                return EnumChainCode.Up;
            }
            if (deltaX == 1 && deltaY == -1)
            {
                return EnumChainCode.UpRight;
            }
            if (deltaX == -1 && deltaY == 1)
            {
                return EnumChainCode.DownLeft;
            }
            if (deltaX == 0 && deltaY == 1)
            {
                return EnumChainCode.Down;
            }
            if (deltaX == 1 && deltaY == 1)
            {
                return EnumChainCode.DownRight;
            }
            return EnumChainCode.Gap;
        }

        /// <summary>
        /// 计算当前点是否为毛刺点
        /// </summary>
        /// <param name="pointBefore"></param>
        /// <param name="pointCur"></param>
        /// <param name="pointAfter"></param>
        /// <returns></returns>
        private static bool IsBurrPoint(Point2D pointBefore, Point2D pointCur, Point2D pointAfter)
        {
            int chainCodeWithBef = (int)ChainCodeToReferencePoint(pointCur, pointBefore);
            int chainCodeWithAft = (int)ChainCodeToReferencePoint(pointCur, pointAfter);
            // 只有符合freeman chain code 的相邻点才能这样判断
            if (chainCodeWithBef >= 0 && chainCodeWithBef <= 7 && chainCodeWithAft >= 0 && chainCodeWithAft <= 7)
            {
                int delta = Math.Abs(chainCodeWithBef - chainCodeWithAft);
                if (delta<=1 || delta>=7)
                {
                    return true;
                }
            }
            return false;
        }

        /// <summary>
        /// 高斯核
        /// </summary>
        /// <param name="radius"></param>
        /// <returns></returns>
        private static double[] GaussianKernel(int radius)
        {
            int count = radius * 2 + 1;
            // sigma 决定高斯核的形态，经验值，选择让核边缘的权重较小，中心权重较大的sigma值
            double sigma = radius / 1.5;
            double[] kernel = new double[count];
            double sum = 0;
            for (int ni = 0; ni < count; ni++)
            {
                kernel[ni] = Math.Exp(-Math.Pow(ni - radius, 2) / (2 * Math.Pow(sigma, 2))) /
                             (sigma * Math.Sqrt(2 * Math.PI));
                sum += kernel[ni];
            }
            // 除以sum
            for (int ni = 0; ni < count; ni++)
            {
                kernel[ni] /= sum;
            }
            return kernel;
        }

        /// <summary>
        /// 移除异常数据
        /// </summary>
        /// <param name="points"></param>
        /// <returns></returns>
        private static Point2D[] RemoveAbnormalData(Point2D[] points)
        {
            // 如果点数过少，则直接返回
            var count = points.Length;
            if (count < 10)
            {
                return points;
            }
            if (!FitLines(points, out var line, false))
            {
                return points;
            }
            // 计算每个点到直线的距离
            var diffs = new double[count];
            double sumDiff = 0;
            for (int ni = 0; ni < count; ni++)
            {
                var diff = GetPointToLineDistance(line, points[ni]);
                sumDiff += diff;
                diffs[ni] = diff;
            }
            // 计算距离值的平均值和标准差
            var averageDiff = sumDiff / count;
            double sumStd = 0;
            for (int ni = 0; ni < count; ni++)
            {
                var diff = diffs[ni];
                sumStd += Math.Pow(diff - averageDiff, 2);
            }
            sumStd = Math.Sqrt(sumStd / count);

            var minDiff = averageDiff - 3 * sumStd;
            var maxDiff = averageDiff + 3 * sumStd;
            // 删除距离大于平均值加减3倍的标准差的点
            for (var ni = 0; ni < count; ni++)
            {
                var diff = diffs[ni];
                if (diff < minDiff || diff > maxDiff)
                {
                    diffs[ni] = double.MinValue;
                }
            }
            // 去掉10%的数据（注：颈动脉原来代码里是这样写的，这里先直接照搬，后续再考虑优化）
            var countRemove = count / 10;
            var diffList = new List<KeyValuePair<int, double>>();
            for (var ni = 0; ni < count; ni++)
            {
                diffList.Add(new KeyValuePair<int, double>(ni, diffs[ni]));
            }
            var sortedDiffList = diffList.OrderByDescending(o => o.Value);
            var result = new List<Point2D>();
            for (var ni = countRemove; ni < count; ni++)
            {
                // 在上一步中，距离大于平均值加减3倍标准差的点（被赋为了double.MinValue），不管在不在10%以内，都应该被舍弃
                if (sortedDiffList.ElementAt(ni).Value >=0)
                {
                    result.Add(points[sortedDiffList.ElementAt(ni).Key]);
                }
            }
            return result.ToArray();
        }


        #endregion


    }

}