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 }