public Object3D Detect(int SX, int SY, int EX, int EY, double rate, Vector3D[] realpoints, double goal_height)
{
Block_W = (EX - SX) / PlantSize; //區塊切割寬數量
Block_H = (EY - SY) / 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> objects = 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 + SX);
Point[] point = new Point[3];
Vector3D tempPoint = new Vector3D();
point[0].X = x * PlantSize + SX;
point[0].Y = y * PlantSize + SY;
point[1].X = (x + 1) * PlantSize + SX;
point[1].Y = y * PlantSize + SY;
point[2].X = x * PlantSize + SX;
point[2].Y = (y + 1) * PlantSize + SY;
//篩選平面區塊
for (int i = 0; i < 3; i++)
{
//取得世界空間座標
tempPoint = realpoints[point[i].X + point[i].Y * Width];
if (Vector3D.Equals(tempPoint, new Vector3D(0, 0, 0)) || tempPoint.Y == Ythreshold)
{
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] = PlantVector[y, x] * -1;
}
PlantVector[y, x].Normalize();
}
}
}
#endregion
#region 求出區域最大平面
Vector3D[,] RecordPlant = new Vector3D[Block_H * Block_W, 2];
List <Point> plan_points = new List <Point>();
List <Point> relation = new List <Point>();
double maxGrowCount = 0;
int id = 0;
int newid = 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(0, 255);
Group_G = (byte)random.Next(0, 255);
Group_B = (byte)random.Next(0, 255);
plan_points.Clear();
GrowCount = 0;
TempVector = PlantVector[y, x];
RefPoint = PlantPoint[y, x, 0];
GrowPlant(x, y, ref plan_points, SX, SY);
if (GrowCount >= PlantRate)
{
RefPoint = RefPoint / GrowCount;
Vector3D test = TempVector;
test.Normalize();
if (plan_points.Count != 0)
{
//找最大平面 // 找高度最接近的平面
//if (goal_height != -1)
//{
// //if (GrowCount > maxGrowCount)
// if (Math.Abs(Math.Abs(RefPoint.Y) - goal_height) < maxGrowCount)
// {
// //maxGrowCount = GrowCount;
// maxGrowCount = Math.Abs(Math.Abs(RefPoint.Y) - goal_height);
// newid = id;
// }
//}
//else
double AngleIJ = Vector3D.AngleBetween(test, Floor.roadFunction);
{
if (GrowCount > maxGrowCount && Math.Abs(RefPoint.Y) > 0.1 && AngleIJ < 10) //
{
maxGrowCount = GrowCount;
newid = id;
}
}
relation.Add(new Point(GrowCount, id));
//求出範圍
double minx = Double.MaxValue, maxx = Double.MinValue, minz = Double.MaxValue, maxz = Double.MinValue;
foreach (Point p in plan_points)
{
Vector3D tmp = realpoints[p.X + p.Y * Width];
if (tmp.Y != Ythreshold)
{
if (tmp.X < minx)
{
minx = tmp.X;
}
if (tmp.X > maxx)
{
maxx = tmp.X;
}
if (tmp.Z < minz)
{
minz = tmp.Z;
}
if (tmp.Z > maxz)
{
maxz = tmp.Z;
}
}
}
//求出物體資訊
Object3D object3d = new Object3D();
object3d.max_plant.normal_vector = TempVector;
object3d.max_plant.normal_vector.Normalize();
object3d.max_plant.d = Vector3D.DotProduct(object3d.max_plant.normal_vector, RefPoint);
object3d.max_plant.draw_range[0] = new System.Windows.Point(plan_points.Min(p => p.X), plan_points.Min(p => p.Y));
object3d.max_plant.draw_range[1] = new System.Windows.Point(plan_points.Max(p => p.X), plan_points.Max(p => p.Y));
object3d.max_plant.center = new Vector3D((object3d.cube_range[0].X + object3d.cube_range[1].X) / 2, RefPoint.Y, (object3d.cube_range[1].Z + object3d.cube_range[0].Z) / 2);
object3d.max_plant.height = Math.Abs(RefPoint.Y);
object3d.cube_range[0] = object3d.max_plant.plant_range[0] = new Point3D(minx, RefPoint.Y, minz);
object3d.cube_range[1] = object3d.max_plant.plant_range[1] = new Point3D(maxx, RefPoint.Y, maxz);
object3d.ID = id;
objects.Add(object3d);
id++;
}
}
else
{
PlantMask[y, x] = ERROR_IDX;
}
}
}
}
#endregion
//relation.Sort(Tools.ComparePointX);
//relation.ToArray();
//if (goal_height != -1)
//{
// for (int i = 0; i < relation.Count; i++)
// {
// Object3D tmp = objects.Find(p => p.ID == relation[i].Y);
// if (Math.Abs(tmp.max_plant.height - goal_height) < 0.1)
// {
// newid = relation[i].Y;
// break;
// }
// }
//}
//else
//{
// for (int i = 0; i < relation.Count; i++)
// {
// Object3D tmp = objects.Find(p => p.ID == relation[i].Y);
// if (Math.Abs(tmp.max_plant.height - 0.1) >= 0.1)
// {
// newid = relation[i].Y;
// break;
// }
// }
//}
Object3D newobject = objects.Find(p => p.ID == newid);
if (newobject != null)
{
Console.WriteLine("max grow count " + maxGrowCount + ", " + Vector3D.AngleBetween(newobject.max_plant.normal_vector, Floor.roadFunction));
}
return(newobject);
}
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
}