//上色
public void paint(ref byte[] draw_Floor, SkeletonPoint[] skeleton)
{
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 = CalculateMethod.TransPortVector(skeleton[x + y * Width]);
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)
{
draw_Floor[(y * 640 + x) * 4] = (byte)(0);
draw_Floor[(y * 640 + x) * 4 + 1] = (byte)(255);
draw_Floor[(y * 640 + x) * 4 + 2] = (byte)(255);
}
}
}
}
public static Vector3D TransPortVector(Vector3D realPoint) //單點做世界座標轉換
{
Vector3D result = CalculateMethod.MatrixMulVector(realPoint, Tmatrix);
Vector3D after = new Vector3D();
after.X = Rmatrix.M11 * result.X + Rmatrix.M12 * result.Y + Rmatrix.M13 * result.Z;
after.Y = Rmatrix.M21 * result.X + Rmatrix.M22 * result.Y + Rmatrix.M23 * result.Z;
after.Z = Rmatrix.M31 * result.X + Rmatrix.M32 * result.Y + Rmatrix.M33 * result.Z;
return(after);
}
public List <Vector3D> Detect(Object3D obj, List <Object3D> objects)
{
for (int o = 0; o < objects.Count; o++)
{
if (objects[o].ID != obj.ID)
{
for (int i = 0; i < objects[o].points.Count; i++)
{
double x = objects[o].points[i].X;
double y = objects[o].points[i].Y;
detectmarker[(int)(x + y * W)] = ERROR;
}
}
}
avgDis = 0;
maxDis = 0;
cdis = 0;
ctotal = 0;
bigavgDis = 0;
maxdisPoint = new Vector3D();
int SX = (int)(obj.max_plant.draw_range[0].X + obj.max_plant.draw_range[1].X) / 2;
int SY = (int)(obj.max_plant.draw_range[0].Y + obj.max_plant.draw_range[1].Y) / 2;
pB = colorPixels[4 * (int)(SX + SY * W) + 0];
pG = colorPixels[4 * (int)(SX + SY * W) + 1];
pR = colorPixels[4 * (int)(SX + SY * W) + 2];
pdepth = depth[SX, SY];
p3dpoint = CalculateMethod.TransPortVector(skeleton[(int)(SX + SY * W)]);
cSY = 0.299 * pR + 0.587 * pG + 0.114 * pB;
double dx = obj.max_plant.draw_range[1].X - obj.max_plant.draw_range[0].X;
double dy = obj.max_plant.draw_range[1].Y - obj.max_plant.draw_range[0].Y;
try
{
if (pdepth != Ythreshold)
{
GrowColor(SX, SY);
}
else
{
Console.WriteLine("start error");
}
Console.WriteLine("avgDis = " + (double)avgDis / cdis + ", maxdis " + maxDis + " count " + cdis + " ctotal " + ctotal + ", bigavg " + (double)bigavgDis / (ctotal - cdis));
//Console.WriteLine("max point " + maxdisPoint);
avgDis = 0;
maxDis = 0;
cdis = 0;
ctotal = 0;
bigavgDis = 0;
maxdisPoint = new Vector3D();
}
catch (StackOverflowException error)
{
Console.WriteLine(error);
}
return(newpoints);
}
private void GrowPlant(int x, int y, ref Object3D obj, SkeletonPoint[] skeleton, byte[] colorPixel)
{
Vector3D temp = TempVector;
temp.Normalize();
double DP = Vector3D.DotProduct(temp, PlantVector[y, x]); //內積 限制角度範圍 30度0.86 45度 0.7
double Dis = Tools.Distance(PrePoint, PlantPoint[y, x, 0]);
if ((x > 0 && x < Block_W - 1) && (y > 0 && y < Block_H - 1) && (DP > 0.86) && Dis <= 0.3)//
{
obj.points.Add(new System.Windows.Point(x * PlantSize, y * PlantSize));
obj.points3d.Add(PlantPoint[y, x, 0]);
PlantMask[y, x] = TAKE_IDX;
RefPoint = RefPoint + PlantPoint[y, x, 0];
TempVector = TempVector + PlantVector[y, x];
PrePoint = PlantPoint[y, x, 0];
GrowCount++; //計算平面數量
if (Dis > maxDis)
{
maxDis = Dis; //測試
}
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 + 2] = (byte)Group_R;
byte_PlantGroup[((y * PlantSize + i) * Width * 4) + (x * PlantSize + j) * 4 + 1] = (byte)Group_G;
byte_PlantGroup[((y * PlantSize + i) * Width * 4) + (x * PlantSize + j) * 4 + 0] = (byte)Group_B;
//加上高度值踢掉物體部分 , 避免抓到物體高度
Vector3D tmp = CalculateMethod.TransPortVector(skeleton[(int)(x * PlantSize + j) + (int)(y * PlantSize + i) * Width]);
double _Dis = Tools.Distance(PrePoint, tmp);
if (tmp.Y != Ythreshold && tmp.Y >= 0.08 && _Dis <= 0.3)
{
obj.points.Add(new System.Windows.Point(x * PlantSize + j, y * PlantSize + i));
obj.points3d.Add(tmp);
}
int B = colorPixel[(int)((y * PlantSize + i) * Width + x * PlantSize + j) * 4 + 0];
int G = colorPixel[(int)((y * PlantSize + i) * Width + x * PlantSize + j) * 4 + 1];
int R = colorPixel[(int)((y * PlantSize + i) * Width + x * PlantSize + j) * 4 + 2];
double U = (-0.169 * R - 0.331 * G + 0.500 * B + 128); //量化
double Y = 0.299 * R + 0.587 * G + 0.114 * B;
obj.xhistrogram[(int)U]++;
}
}
if (PlantMask[y, (x - 1)] == DETECT_IDX) //left
{
GrowPlant(x - 1, y, ref obj, skeleton, colorPixel);
}
if (PlantMask[(y - 1), x] == DETECT_IDX) //up
{
GrowPlant(x, y - 1, ref obj, skeleton, colorPixel);
}
if (PlantMask[(y + 1), x] == DETECT_IDX) //down
{
GrowPlant(x, y + 1, ref obj, skeleton, colorPixel);
}
if (PlantMask[y, (x + 1)] == DETECT_IDX) //right
{
GrowPlant(x + 1, y, ref obj, skeleton, colorPixel);
}
//if (PlantMask[(y - 1), (x - 1)] == DETECT_IDX) //left
//{
// GrowPlant(x - 1, y - 1, ref obj, skeleton, colorPixel);
//}
//if (PlantMask[(y - 1), (x + 1)] == DETECT_IDX) //up
//{
// GrowPlant((x + 1), y - 1, ref obj, skeleton, colorPixel);
//}
//if (PlantMask[(y + 1), (x - 1)] == DETECT_IDX) //down
//{
// GrowPlant(x - 1, y + 1, ref obj, skeleton, colorPixel);
//}
//if (PlantMask[(y + 1), (x + 1)] == DETECT_IDX) //right
//{
// GrowPlant(x + 1, y + 1, ref obj, skeleton, colorPixel);
//}
}
}
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
}
private int GrowColor(int x, int y)
{
if (detectmarker[x + y * W] == UNDETECT)
{
int B = colorPixels[4 * (int)(x + y * W) + 0];
int G = colorPixels[4 * (int)(x + y * W) + 1];
int R = colorPixels[4 * (int)(x + y * W) + 2];
short newdepth = depth[x, y];
Vector3D tmp = CalculateMethod.TransPortVector(skeleton[(int)(x + y * W)]);
double U = (-0.169 * R - 0.331 * G + 0.500 * B + 128); //量化
double pU = (-0.169 * pR - 0.331 * pG + 0.500 * pB + 128); //量化
double Y = 0.299 * R + 0.587 * G + 0.114 * B;
double pY = 0.299 * pR + 0.587 * pG + 0.114 * pB;
double Dis = Tools.Distance(tmp, p3dpoint);
ctotal++;
if (Dis > maxDis)
{
maxDis = Dis;
maxdisPoint = tmp;
}
if (Dis > 0.5)
{
bigavgDis += Dis;
}
else
{
cdis++;
avgDis += Dis;
}
if ((Math.Abs(R - pR) < ColorThreshold && Math.Abs(G - pG) < ColorThreshold && Math.Abs(B - pB) < ColorThreshold) && Math.Abs(Y - cSY) < 50 && (tmp.Y != Ythreshold && Dis <= 0.3)) // || (Math.Abs(R - pR) + Math.Abs(G - pG) + Math.Abs(B - pB)) < 50 &&
{
newpoints.Add(tmp);
detectmarker[x + y * W] = DETECTED;
pB = B;
pG = G;
pR = R;
pdepth = newdepth;
p3dpoint = tmp;
for (int i = 0; i < 5; i++)
{
for (int j = 0; j < 5; j++)
{
objectMask[4 * ((x + i) + (y + j) * W) + 0] = (byte)B;
objectMask[4 * ((x + i) + (y + j) * W) + 1] = (byte)G;
objectMask[4 * ((x + i) + (y + j) * W) + 2] = (byte)R;
}
}
if (x > 0 && detectmarker[(x - 5) + y * W] == UNDETECT)
{
GrowColor(x - 5, y);
}
if (y > 0 && detectmarker[x + (y - 5) * W] == UNDETECT)
{
GrowColor(x, y - 5);
}
if (x < W - 1 && detectmarker[(x + 5) + y * W] == UNDETECT)
{
GrowColor(x + 5, y);
}
if (y < H - 1 && detectmarker[x + (y + 5) * W] == UNDETECT)
{
GrowColor(x, y + 5);
}
}
else
{
detectmarker[x + y * W] = DETECTED;
if (!(Math.Abs(R - pR) < ColorThreshold && Math.Abs(G - pG) < ColorThreshold && Math.Abs(B - pB) < ColorThreshold && Math.Abs(Y - cSY) < 50))//後者好像可以拿掉
{
if (!(Math.Abs(R - pR) < ColorThreshold))
{
for (int i = 0; i < 5; i++)
{
for (int j = 0; j < 5; j++)
{
objectMask[4 * ((x + i) + (y + j) * W) + 0] = (byte)0;
objectMask[4 * ((x + i) + (y + j) * W) + 1] = (byte)0;
objectMask[4 * ((x + i) + (y + j) * W) + 2] = (byte)255;
}
}
}
if (!(Math.Abs(G - pG) < ColorThreshold))
{
for (int i = 0; i < 5; i++)
{
for (int j = 0; j < 5; j++)
{
objectMask[4 * ((x + i) + (y + j) * W) + 0] = (byte)0;
objectMask[4 * ((x + i) + (y + j) * W) + 1] = (byte)255;
objectMask[4 * ((x + i) + (y + j) * W) + 2] = (byte)0;
}
}
}
if (!(Math.Abs(B - pB) < ColorThreshold))
{
for (int i = 0; i < 5; i++)
{
for (int j = 0; j < 5; j++)
{
objectMask[4 * ((x + i) + (y + j) * W) + 0] = (byte)255;
objectMask[4 * ((x + i) + (y + j) * W) + 1] = (byte)0;
objectMask[4 * ((x + i) + (y + j) * W) + 2] = (byte)0;
}
}
}
else if (Math.Abs(Y - cSY) >= 50)
{
for (int i = 0; i < 5; i++)
{
for (int j = 0; j < 5; j++)
{
objectMask[4 * ((x + i) + (y + j) * W) + 0] = (byte)0;
objectMask[4 * ((x + i) + (y + j) * W) + 1] = (byte)255;
objectMask[4 * ((x + i) + (y + j) * W) + 2] = (byte)255;
}
}
}
}
if (tmp.Y != Ythreshold && Dis > 0.3) //Math.Abs(pdepth - newdepth) < 800)
{
for (int i = 0; i < 5; i++)
{
for (int j = 0; j < 5; j++)
{
objectMask[4 * ((x + i) + (y + j) * W) + 0] = (byte)255;
objectMask[4 * ((x + i) + (y + j) * W) + 1] = (byte)0;
objectMask[4 * ((x + i) + (y + j) * W) + 2] = (byte)255;
}
}
}
return(0);
}
}
return(0);
}
