예제 #1
0
        byte[] byte_PlantGroup = new byte[Width * Height * 4]; //生長路面圖;

        #region 版本一 : 一口氣將所有典轉為世界座標

        /*
         * public void detect(Vector3D[,] RealWorldPoint, int Width, int Height, double rate)
         * {
         *  Block_W = Width / PlantSize; //區塊切割寬數量
         *  Block_H = Height / PlantSize; //區塊切割高數量
         *  PlantRate = (int)(rate * (Block_H * Block_W));
         *
         *  PlantMask = new int[Block_H, Block_W]; //路面生長遮罩
         *  PlantPoint  = new Vector3D[Block_H, Block_W, 3];
         *  PlantVector = new Vector3D[Block_H, Block_W];
         *
         #region  計算區塊的平面法向量
         *  for (int y = 0; y < Block_H - 1; y++)
         *  {
         *      for (int x = 0; x < Block_W - 1; x++)
         *      {
         *          int PosIndex = y * Block_W + x;
         *          Point[] point = new Point[3];
         *          Vector3D tempPoint = new Vector3D();
         *
         *          point[0].X = x * PlantSize;
         *          point[0].Y = y * PlantSize;
         *          point[1].X = (x + 1) * PlantSize;
         *          point[1].Y = y * PlantSize;
         *          point[2].X = x * PlantSize;
         *          point[2].Y = (y + 1) * PlantSize;
         *
         *          //篩選平面區塊
         *          for (int i = 0; i < 3; i++)
         *          {
         *              //取得世界空間座標
         *              tempPoint = RealWorldPoint[point[i].X, point[i].Y];
         *              if (tempPoint.Z < 0.8 || tempPoint.Z > 4)  //取不到深度資訊時把遮罩設定為error
         *              {
         *                  PlantMask[y, x] = ERROR_IDX;
         *                  RecordPlantFlag = false; //取消紀錄PlantVector[]
         *                  break;
         *              }
         *              else
         *              {
         *                  RecordPlantFlag = true;
         *                  PlantPoint[y, x, i] = tempPoint;
         *              }
         *          }
         *
         *          //紀錄區塊平面法向量
         *          if (RecordPlantFlag == true)
         *          {
         *              PlantVector[y, x] = CalculateMethod.PlaneVector(PlantPoint[y, x, 0], PlantPoint[y, x, 1], PlantPoint[y, x, 2]);
         *
         *              if (PlantVector[y, x].Y > 0 && PlantVector[y, x].Z < 0)
         *              {
         *                  PlantVector[y, x].Normalize();
         *              }
         *              else
         *              {
         *                  PlantMask[y, x] = ERROR_IDX;
         *              }
         *          }
         *      }
         *  }
         *
         #endregion
         *
         #region 計算路面方程式
         *
         *  Vector3D PlantNormal = new Vector3D();
         *  Vector3D MinPoint = new Vector3D();
         *  Vector3D[,] RecordPlant = new Vector3D[Block_H * Block_W, 2];
         *
         *  double minDistance = 0;
         *
         *  for (int y = 0; y < Block_H; y++)
         *      for (int x = 0; x < Block_W; x++)
         *      {
         *
         *          if (PlantMask[y, x] == DETECT_IDX)
         *          {
         *              GrowCount = 0;
         *              TempVector = PlantVector[y, x];
         *              RefPoint = PlantPoint[y, x, 0];
         *              GrowPlant(x, y);
         *
         *              if (GrowCount >= PlantRate)
         *              {
         *                  RefPoint = RefPoint / GrowCount;
         *
         *                  double distance = Math.Abs(Vector3D.DotProduct(TempVector, RefPoint) / Math.Pow((TempVector.X * TempVector.X + TempVector.Y * TempVector.Y + TempVector.Z * TempVector.Z), 0.5));
         *
         *                  if (distance > minDistance) //找最低平面
         *                  {
         *                      minDistance = distance;
         *                      MinPoint = RefPoint;
         *                      PlantNormal = TempVector;
         *                      PlantNormal.Normalize();  //應該可以省略
         *                  }
         *
         *              }
         *              else
         *              {
         *                  PlantMask[y, x] = ERROR_IDX;
         *              }
         *
         *          }
         *      }
         *
         *  double plantParameter_a, plantParameter_b, plantParameter_c, plantParameter_d, plantParameter_ABC;  //aX+bY+cZ = d
         *
         *  plantParameter_a = PlantNormal.X;
         *  plantParameter_b = PlantNormal.Y;
         *  plantParameter_c = PlantNormal.Z;
         *  plantParameter_d = Vector3D.DotProduct(PlantNormal, MinPoint);
         *  plantParameter_ABC = Math.Pow(Math.Pow(PlantNormal.X, 2) + Math.Pow(PlantNormal.Y, 2) + Math.Pow(PlantNormal.Z, 2), 0.5); //到平面距離公式(分母)
         *  PlantNormal.Normalize();
         *
         *  plantParameter[0] = plantParameter_a;
         *  plantParameter[1] = plantParameter_b;
         *  plantParameter[2] = plantParameter_c;
         *  plantParameter[3] = plantParameter_d;
         *  plantParameter[4] = plantParameter_ABC;
         *
         *  Console.WriteLine(plantParameter[0] + "," + plantParameter[1] + "," + plantParameter[2] + ".  d = " + plantParameter[3]);
         *
         #endregion
         *
         * }
         *
         * //畫地面
         * public void paint(Vector3D[,] RealWorldPoint, ColorImagePoint[] _mappedDepthLocations, ref byte[] draw_Floor)
         * {
         *  int Width  = Block_W * PlantSize;
         *  int Height = Block_H * PlantSize;
         *
         *  for (int y = 0; y < 480; y++)
         *      for (int x = 0; x < 640; x++)
         *      {
         *          Vector3D RealPos = RealWorldPoint[x, y];
         *          double d = (plantParameter[0] * RealPos.X + plantParameter[1] * RealPos.Y + plantParameter[2] * RealPos.Z - plantParameter[3]) / plantParameter[4];
         *
         *          if (d < 0)    d = 0 - d;
         *          if (d < Distance && RealPos.Z > 0.8 && RealPos.Z < 4)
         *          {
         *              ColorImagePoint point = _mappedDepthLocations[y * 640 + x];
         *              if ((point.X >= 0 && point.X < 640) && (point.Y >= 0 && point.Y < 480))
         *              {
         *                  draw_Floor[(point.Y * Width + point.X) * 4] = (byte)(0);
         *                  draw_Floor[(point.Y * Width + point.X) * 4 + 1] = (byte)(255);
         *                  draw_Floor[(point.Y * Width + point.X) * 4 + 2] = (byte)(255);
         *              }
         *          }
         *      }
         * }
         */
        #endregion

        #region 版本二 : 只轉需要用的點到世界座標

        //全部偵測
        public List <Object3D> Detect(double rate, SkeletonPoint[] skeleton, byte[] colorPixel)
        {
            Block_W   = Width / PlantSize;  //區塊切割寬數量
            Block_H   = Height / PlantSize; //區塊切割高數量
            PlantRate = (int)(rate * (Block_H * Block_W));

            PlantMask   = new int[Block_H, Block_W]; //路面生長遮罩
            PlantPoint  = new Vector3D[Block_H, Block_W, 3];
            PlantVector = new Vector3D[Block_H, Block_W];

            List <Object3D> plants = new List <Object3D>();

            #region  計算區塊的平面法向量
            for (int y = 0; y < Block_H - 1; y++)
            {
                for (int x = 0; x < Block_W - 1; x++)
                {
                    //int PosIndex = y * Block_W + x;
                    Point[]  point     = new Point[3];
                    Vector3D tempPoint = new Vector3D();

                    point[0].X = x * PlantSize;
                    point[0].Y = y * PlantSize;
                    point[1].X = (x + 1) * PlantSize;
                    point[1].Y = y * PlantSize;
                    point[2].X = x * PlantSize;
                    point[2].Y = (y + 1) * PlantSize;

                    //篩選平面區塊
                    for (int i = 0; i < 3; i++)
                    {
                        //取得世界空間座標
                        tempPoint = CalculateMethod.TransPortVector(skeleton[point[i].X + point[i].Y * Width]);
                        if (tempPoint.Y == Ythreshold) //Vector3D.Equals(tempPoint, new Vector3D(0, 0, 0)) ||
                        {
                            PlantMask[y, x] = ERROR_IDX;
                            RecordPlantFlag = false; //取消紀錄PlantVector[] '

                            byte_PlantGroup[4 * (x + y * Width) + 3] = (byte)0;
                            for (int _i = 0; _i < PlantSize; _i++)
                            {
                                for (int _j = 0; _j < PlantSize; _j++)
                                {
                                    byte_PlantGroup[((point[i].Y + _i) * Width * 4) + (point[i].X + _j) * 4 + 3] = (byte)0;
                                }
                            }

                            break;
                        }
                        else
                        {
                            RecordPlantFlag     = true;
                            PlantPoint[y, x, i] = tempPoint;
                        }
                    }

                    //紀錄區塊平面法向量
                    if (RecordPlantFlag == true)
                    {
                        tempPoint = CalculateMethod.TransPortVector(skeleton[((x + 1) * PlantSize) + ((y + 1) * PlantSize) * Width]);
                        Vector3D tempVector = CalculateMethod.PlaneVector(tempPoint, PlantPoint[y, x, 1], PlantPoint[y, x, 2]);

                        PlantVector[y, x] = CalculateMethod.PlaneVector(PlantPoint[y, x, 0], PlantPoint[y, x, 1], PlantPoint[y, x, 2]);

                        //統一方向
                        if (PlantVector[y, x].Y < 0)
                        {
                            PlantVector[y, x] = PlantVector[y, x] * -1;
                        }

                        if (tempVector.Y < 0)
                        {
                            tempVector = tempPoint * -1;
                        }

                        //PlantVector[y, x] += tempVector;

                        PlantVector[y, x].Normalize();
                    }
                }
            }

            #endregion

            #region 計算路面方程式

            Vector3D PlantNormal = new Vector3D();
            Vector3D MinPoint    = new Vector3D();
            Vector3D[,] RecordPlant = new Vector3D[Block_H * Block_W, 2];
            Object3D obj = new Object3D();

            double minDistance = 5;
            int    id          = 0;

            for (int y = 0; y < Block_H; y++)
            {
                for (int x = 0; x < Block_W; x++)
                {
                    if (PlantMask[y, x] == DETECT_IDX)
                    {
                        Group_R = (byte)random.Next(100, 255);
                        Group_G = (byte)random.Next(100, 255);
                        Group_B = (byte)random.Next(100, 255);
                        obj     = new Object3D();

                        maxDis     = Double.MinValue;
                        GrowCount  = 0;
                        TempVector = PlantVector[y, x];
                        RefPoint   = PlantPoint[y, x, 0];
                        PrePoint   = PlantPoint[y, x, 0];
                        GrowPlant(x, y, ref obj, skeleton, colorPixel);

                        if (GrowCount >= PlantRate)
                        {
                            Console.WriteLine("plant id " + id + "," + maxDis);

                            RefPoint = RefPoint / GrowCount;

                            //因為皆為平面,故法向量相差不大,直接取其平均點的Y值作為高度判斷,取最低點
                            double distance = RefPoint.Y;
                            if (distance < minDistance) //找最低平面
                            {
                                minDistance = distance;
                                MinPoint    = RefPoint;  //嚴格講起來是取平均點
                                PlantNormal = TempVector;
                                PlantNormal.Normalize(); //應該可以省略
                            }

                            if (obj.points.Count != 0)
                            {
                                obj.max_plant.normal_vector = TempVector;
                                obj.max_plant.normal_vector.Normalize();
                                obj.max_plant.d             = Vector3D.DotProduct(obj.max_plant.normal_vector, RefPoint);
                                obj.max_plant.height        = Math.Abs(RefPoint.Y);
                                obj.max_plant.draw_range[0] = new System.Windows.Point(obj.points.Min(p => p.X), obj.points.Min(p => p.Y));
                                obj.max_plant.draw_range[1] = new System.Windows.Point(obj.points.Max(p => p.X), obj.points.Max(p => p.Y));

                                //取得平面的範圍
                                obj.GetRange(Ythreshold, true);
                                obj.max_plant.plant_range[0] = new Point3D(obj.cube_range[0].X, RefPoint.Y, obj.cube_range[0].Z);
                                obj.max_plant.plant_range[1] = new Point3D(obj.cube_range[1].X, RefPoint.Y, obj.cube_range[1].Z);
                                obj.cube_prange[0]           = obj.max_plant.draw_range[0];
                                obj.cube_prange[1]           = obj.max_plant.draw_range[1];
                                obj.ID = id;


                                for (int i = 0; i < 256; i++)
                                {
                                    obj.xhistrogram[i] /= obj.points.Count;
                                }

                                plants.Add(obj);

                                id++;
                            }
                        }
                        else
                        {
                            PlantMask[y, x] = ERROR_IDX;
                            byte_PlantGroup[4 * (x + y * Width) + 3] = (byte)0;

                            for (int i = 0; i < PlantSize; i++)
                            {
                                for (int j = 0; j < PlantSize; j++)
                                {
                                    byte_PlantGroup[((y * PlantSize + i) * Width * 4) + (x * PlantSize + j) * 4 + 3] = (byte)255;
                                }
                            }
                        }
                    }
                }
            }

            double plantParameter_a, plantParameter_b, plantParameter_c, plantParameter_d, plantParameter_ABC;  //aX+bY+cZ = d

            plantParameter_a   = PlantNormal.X;
            plantParameter_b   = PlantNormal.Y;
            plantParameter_c   = PlantNormal.Z;
            plantParameter_d   = Vector3D.DotProduct(PlantNormal, MinPoint);
            plantParameter_ABC = Math.Pow(Math.Pow(PlantNormal.X, 2) + Math.Pow(PlantNormal.Y, 2) + Math.Pow(PlantNormal.Z, 2), 0.5); //到平面距離公式(分母)
            //PlantNormal.Normalize();

            plantParameter[0] = plantParameter_a;
            plantParameter[1] = plantParameter_b;
            plantParameter[2] = plantParameter_c;
            plantParameter[3] = plantParameter_d;
            plantParameter[4] = plantParameter_ABC;

            Floor.roadFunction = new Vector3D(plantParameter_a, plantParameter_b, plantParameter_c);
            Floor.d            = plantParameter_d;

            return(plants);

            #endregion
        }
        byte[] byte_PlantGroup = new byte[Width * Height * 4]; //生長路面圖;

        #region 版本一 : 一口氣將所有典轉為世界座標
        /*
        public void detect(Vector3D[,] RealWorldPoint, int Width, int Height, double rate)
        {           
            Block_W = Width / PlantSize; //區塊切割寬數量
            Block_H = Height / PlantSize; //區塊切割高數量
            PlantRate = (int)(rate * (Block_H * Block_W));

            PlantMask = new int[Block_H, Block_W]; //路面生長遮罩                     
            PlantPoint  = new Vector3D[Block_H, Block_W, 3];
            PlantVector = new Vector3D[Block_H, Block_W];
          
            #region  計算區塊的平面法向量
            for (int y = 0; y < Block_H - 1; y++)
            {
                for (int x = 0; x < Block_W - 1; x++)
                {
                    int PosIndex = y * Block_W + x;
                    Point[] point = new Point[3];
                    Vector3D tempPoint = new Vector3D();

                    point[0].X = x * PlantSize;
                    point[0].Y = y * PlantSize;
                    point[1].X = (x + 1) * PlantSize;
                    point[1].Y = y * PlantSize;
                    point[2].X = x * PlantSize;
                    point[2].Y = (y + 1) * PlantSize;

                    //篩選平面區塊
                    for (int i = 0; i < 3; i++)
                    {
                        //取得世界空間座標
                        tempPoint = RealWorldPoint[point[i].X, point[i].Y];
                        if (tempPoint.Z < 0.8 || tempPoint.Z > 4)  //取不到深度資訊時把遮罩設定為error
                        {
                            PlantMask[y, x] = ERROR_IDX;
                            RecordPlantFlag = false; //取消紀錄PlantVector[]
                            break;
                        }
                        else
                        {
                            RecordPlantFlag = true;
                            PlantPoint[y, x, i] = tempPoint;
                        }
                    }

                    //紀錄區塊平面法向量
                    if (RecordPlantFlag == true)
                    {
                        PlantVector[y, x] = CalculateMethod.PlaneVector(PlantPoint[y, x, 0], PlantPoint[y, x, 1], PlantPoint[y, x, 2]);

                        if (PlantVector[y, x].Y > 0 && PlantVector[y, x].Z < 0)
                        {
                            PlantVector[y, x].Normalize();
                        }
                        else
                        {
                            PlantMask[y, x] = ERROR_IDX;
                        }
                    }
                }
            }

            #endregion
    
            #region 計算路面方程式

            Vector3D PlantNormal = new Vector3D();
            Vector3D MinPoint = new Vector3D();
            Vector3D[,] RecordPlant = new Vector3D[Block_H * Block_W, 2];          
     
            double minDistance = 0;

            for (int y = 0; y < Block_H; y++)
                for (int x = 0; x < Block_W; x++)
                {

                    if (PlantMask[y, x] == DETECT_IDX)
                    {
                        GrowCount = 0;
                        TempVector = PlantVector[y, x];
                        RefPoint = PlantPoint[y, x, 0];
                        GrowPlant(x, y);

                        if (GrowCount >= PlantRate)
                        {
                            RefPoint = RefPoint / GrowCount;

                            double distance = Math.Abs(Vector3D.DotProduct(TempVector, RefPoint) / Math.Pow((TempVector.X * TempVector.X + TempVector.Y * TempVector.Y + TempVector.Z * TempVector.Z), 0.5));

                            if (distance > minDistance) //找最低平面
                            {
                                minDistance = distance;
                                MinPoint = RefPoint;
                                PlantNormal = TempVector;
                                PlantNormal.Normalize();  //應該可以省略
                            }

                        }
                        else
                        {
                            PlantMask[y, x] = ERROR_IDX;
                        }

                    }
                }            

            double plantParameter_a, plantParameter_b, plantParameter_c, plantParameter_d, plantParameter_ABC;  //aX+bY+cZ = d   
      
            plantParameter_a = PlantNormal.X;
            plantParameter_b = PlantNormal.Y;
            plantParameter_c = PlantNormal.Z;
            plantParameter_d = Vector3D.DotProduct(PlantNormal, MinPoint);
            plantParameter_ABC = Math.Pow(Math.Pow(PlantNormal.X, 2) + Math.Pow(PlantNormal.Y, 2) + Math.Pow(PlantNormal.Z, 2), 0.5); //到平面距離公式(分母)
            PlantNormal.Normalize();

            plantParameter[0] = plantParameter_a;
            plantParameter[1] = plantParameter_b;
            plantParameter[2] = plantParameter_c;
            plantParameter[3] = plantParameter_d;
            plantParameter[4] = plantParameter_ABC;

            Console.WriteLine(plantParameter[0] + "," + plantParameter[1] + "," + plantParameter[2] + ".  d = " + plantParameter[3]);

            #endregion

        }
        
        //畫地面
        public void paint(Vector3D[,] RealWorldPoint, ColorImagePoint[] _mappedDepthLocations, ref byte[] draw_Floor)
        {
            int Width  = Block_W * PlantSize;
            int Height = Block_H * PlantSize;

            for (int y = 0; y < 480; y++)
                for (int x = 0; x < 640; x++)
                {
                    Vector3D RealPos = RealWorldPoint[x, y];
                    double d = (plantParameter[0] * RealPos.X + plantParameter[1] * RealPos.Y + plantParameter[2] * RealPos.Z - plantParameter[3]) / plantParameter[4];

                    if (d < 0)    d = 0 - d;
                    if (d < Distance && RealPos.Z > 0.8 && RealPos.Z < 4)
                    {
                        ColorImagePoint point = _mappedDepthLocations[y * 640 + x];
                        if ((point.X >= 0 && point.X < 640) && (point.Y >= 0 && point.Y < 480))
                        {
                            draw_Floor[(point.Y * Width + point.X) * 4] = (byte)(0);
                            draw_Floor[(point.Y * Width + point.X) * 4 + 1] = (byte)(255);
                            draw_Floor[(point.Y * Width + point.X) * 4 + 2] = (byte)(255);
                        }
                    }
                }
        }
        */
        #endregion

        #region 版本二 : 只轉需要用的點到世界座標

        //全部偵測
        public List<Object3D> Detect(double rate, SkeletonPoint[] skeleton, byte[] colorPixel)
        {
            Block_W = Width / PlantSize;  //區塊切割寬數量
            Block_H = Height / PlantSize; //區塊切割高數量
            PlantRate = (int)(rate * (Block_H * Block_W));

            PlantMask = new int[Block_H, Block_W]; //路面生長遮罩                     
            PlantPoint = new Vector3D[Block_H, Block_W, 3];
            PlantVector = new Vector3D[Block_H, Block_W];

            List<Object3D> plants = new List<Object3D>();

            #region  計算區塊的平面法向量
            for (int y = 0; y < Block_H - 1; y++)
            {
                for (int x = 0; x < Block_W - 1; x++)
                {
                    //int PosIndex = y * Block_W + x;
                    Point[] point = new Point[3];
                    Vector3D tempPoint = new Vector3D();

                    point[0].X = x * PlantSize;
                    point[0].Y = y * PlantSize;
                    point[1].X = (x + 1) * PlantSize;
                    point[1].Y = y * PlantSize;
                    point[2].X = x * PlantSize;
                    point[2].Y = (y + 1) * PlantSize;

                    //篩選平面區塊
                    for (int i = 0; i < 3; i++)
                    {
                        //取得世界空間座標
                        tempPoint = CalculateMethod.TransPortVector(skeleton[point[i].X + point[i].Y * Width]);
                        if (tempPoint.Y == Ythreshold) //Vector3D.Equals(tempPoint, new Vector3D(0, 0, 0)) || 
                        {
                            PlantMask[y, x] = ERROR_IDX;
                            RecordPlantFlag = false; //取消紀錄PlantVector[] '

                            byte_PlantGroup[4 * (x + y * Width) + 3] = (byte)0;
                            for (int _i = 0; _i < PlantSize; _i++)
                                for (int _j = 0; _j < PlantSize; _j++)
                                {
                                    byte_PlantGroup[((point[i].Y + _i) * Width * 4) + (point[i].X + _j) * 4 + 3] = (byte)0;
                                }

                            break;
                        }
                        else
                        {
                            RecordPlantFlag = true;
                            PlantPoint[y, x, i] = tempPoint;
                        }
                    }

                    //紀錄區塊平面法向量
                    if (RecordPlantFlag == true)
                    {
                        tempPoint = CalculateMethod.TransPortVector(skeleton[((x + 1) * PlantSize) + ((y + 1) * PlantSize) * Width]);
                        Vector3D tempVector = CalculateMethod.PlaneVector(tempPoint, PlantPoint[y, x, 1], PlantPoint[y, x, 2]);

                        PlantVector[y, x] = CalculateMethod.PlaneVector(PlantPoint[y, x, 0], PlantPoint[y, x, 1], PlantPoint[y, x, 2]);

                        //統一方向
                        if (PlantVector[y, x].Y < 0) PlantVector[y, x] = PlantVector[y, x] * -1;

                        if (tempVector.Y < 0) tempVector = tempPoint * -1;

                        //PlantVector[y, x] += tempVector;

                        PlantVector[y, x].Normalize();                        
                    }
                }
            }

            #endregion

            #region 計算路面方程式
            
            Vector3D PlantNormal = new Vector3D();
            Vector3D MinPoint = new Vector3D();
            Vector3D[,] RecordPlant = new Vector3D[Block_H * Block_W, 2];
            Object3D obj = new Object3D();

            double minDistance = 5;
            int id = 0;

            for (int y = 0; y < Block_H; y++)
                for (int x = 0; x < Block_W; x++)
                {
                    if (PlantMask[y, x] == DETECT_IDX)
                    {
                        Group_R = (byte)random.Next(100, 255);
                        Group_G = (byte)random.Next(100, 255);
                        Group_B = (byte)random.Next(100, 255);
                        obj = new Object3D();

                        maxDis = Double.MinValue;
                        GrowCount = 0;
                        TempVector = PlantVector[y, x];
                        RefPoint = PlantPoint[y, x, 0];
                        PrePoint = PlantPoint[y, x, 0];
                        GrowPlant(x, y, ref obj, skeleton, colorPixel);

                        if (GrowCount >= PlantRate)  
                        {
                            Console.WriteLine("plant id " + id + "," + maxDis);

                            RefPoint = RefPoint / GrowCount;

                            //因為皆為平面,故法向量相差不大,直接取其平均點的Y值作為高度判斷,取最低點
                            double distance = RefPoint.Y;
                            if (distance < minDistance) //找最低平面
                            {
                                minDistance = distance;
                                MinPoint = RefPoint;   //嚴格講起來是取平均點
                                PlantNormal = TempVector;
                                PlantNormal.Normalize();  //應該可以省略
                            }

                            if (obj.points.Count != 0)
                            {
                                obj.max_plant.normal_vector = TempVector;
                                obj.max_plant.normal_vector.Normalize();
                                obj.max_plant.d = Vector3D.DotProduct(obj.max_plant.normal_vector, RefPoint);
                                obj.max_plant.height = Math.Abs(RefPoint.Y);
                                obj.max_plant.draw_range[0] = new System.Windows.Point(obj.points.Min(p => p.X), obj.points.Min(p => p.Y));
                                obj.max_plant.draw_range[1] = new System.Windows.Point(obj.points.Max(p => p.X), obj.points.Max(p => p.Y));
                                
                                //取得平面的範圍
                                obj.GetRange(Ythreshold, true);
                                obj.max_plant.plant_range[0] = new Point3D(obj.cube_range[0].X, RefPoint.Y, obj.cube_range[0].Z);
                                obj.max_plant.plant_range[1] = new Point3D(obj.cube_range[1].X, RefPoint.Y, obj.cube_range[1].Z);
                                obj.cube_prange[0] = obj.max_plant.draw_range[0];
                                obj.cube_prange[1] = obj.max_plant.draw_range[1];
                                obj.ID = id;

                             
                                for (int i = 0; i < 256; i++)
                                {                                   
                                    obj.xhistrogram[i] /= obj.points.Count;                               
                                }

                                plants.Add(obj);

                                id++;
                            }
                        }
                        else
                        {
                            PlantMask[y, x] = ERROR_IDX;
                            byte_PlantGroup[4 * (x + y * Width) + 3] = (byte)0;

                            for (int i = 0; i < PlantSize; i++)
                                for (int j = 0; j < PlantSize; j++)
                                {
                                    byte_PlantGroup[((y * PlantSize + i) * Width * 4) + (x * PlantSize + j) * 4 + 3] = (byte)255;
                                }
                        }
                    }
                }
            
            double plantParameter_a, plantParameter_b, plantParameter_c, plantParameter_d, plantParameter_ABC;  //aX+bY+cZ = d   

            plantParameter_a = PlantNormal.X;
            plantParameter_b = PlantNormal.Y;
            plantParameter_c = PlantNormal.Z;
            plantParameter_d = Vector3D.DotProduct(PlantNormal, MinPoint);
            plantParameter_ABC = Math.Pow(Math.Pow(PlantNormal.X, 2) + Math.Pow(PlantNormal.Y, 2) + Math.Pow(PlantNormal.Z, 2), 0.5); //到平面距離公式(分母)
            //PlantNormal.Normalize();

            plantParameter[0] = plantParameter_a;
            plantParameter[1] = plantParameter_b;
            plantParameter[2] = plantParameter_c;
            plantParameter[3] = plantParameter_d;
            plantParameter[4] = plantParameter_ABC;

            Floor.roadFunction = new Vector3D(plantParameter_a, plantParameter_b, plantParameter_c);
            Floor.d = plantParameter_d;

            return plants;

            #endregion
        }