public void sortDetectPoint(Vector3[] dp_3D, RealCamera camera, int[] sort_index)
{
for (int i = 0; i < sort_index.Length; i++)
{
sort_index [i] = i;
}
float[] distance = new float[dp_3D.Length];
for (int i = 0; i < dp_3D.Length; i++)
{
distance [i] = Vector3.Distance(dp_3D [i], camera.transform.position);
}
for (int i = 0; i < dp_3D.Length; i++)
{
for (int j = i + 1; j < dp_3D.Length; j++)
{
if (distance [i] > distance [j])
{
int temp;
temp = sort_index [i];
sort_index [i] = sort_index [j];
sort_index[j] = temp;
float tempf;
tempf = distance [i];
distance [i] = distance [j];
distance [j] = tempf;
}
}
}
Array.Clear(distance, 0, distance.Length);
}
/*
* 將normalized的點
* 轉換成pixel座標
* Input : normalize 後的點
*/
public Vector3[] transformNormalizeVerticeToTexture(Vector3[] input_vertice, RealCamera camera)
{
Vector3[] result = new Vector3[input_vertice.Length];
for (int i = 0; i < input_vertice.Length; i++)
{
Vector3 pos_on_texture;
pos_on_texture.x = input_vertice[i].x * camera.pixel_width;
pos_on_texture.y = input_vertice[i].y * camera.pixel_height;
pos_on_texture.z = 0;
result[i] = pos_on_texture;
}
return(result);
}
//將3D骨架投影到投影面
public Vector3[] projectVertice3DToProjectPlane(Vector3[] vertex3D, RealCamera camera)
{
int vertex_num = vertex3D.Length;
Vector3[] vertex2D = new Vector3[vertex_num];
Vector3 camera_position = camera.transform.position;
Vector3 camera_direction = camera.transform.forward.normalized;
Vector3 cameraToPlane = camera_direction * camera.camera_distance;
for (int i = 0; i < vertex_num; i++)
{
//計算投影後位置
Vector3 cameraToVertex = vertex3D[i] - camera_position;
float t = Vector3.Dot(cameraToPlane, camera_direction) / Vector3.Dot(cameraToVertex, camera_direction);
vertex2D[i] = camera_position + cameraToVertex * t;
//計算投影後大小
}
return(vertex2D);
}
/*
* 將投影面上的點
* 以左下角為原點
* 投影面的長和寬為1單位
* 做normalize
* (0,1,0)_____________(1,1,0)
* | |
* | 投影面 |
* (0,0,0)|___________|(1,0,0)
*
*/
public Vector3[] normalizeVertice2D(Vector3[] input_vertice, RealCamera camera)
{
Vector3[] result = new Vector3[input_vertice.Length];
//origin 為 相機成像 左下角的點
Vector3 origin = camera.transform.position + camera.transform.forward * camera.camera_distance
- camera.transform.right * camera.camera_width / 2
- camera.transform.up * camera.camera_height / 2;
for (int i = 0; i < input_vertice.Length; i++)
{
Vector3 normalized_pos;
Vector3 diff = input_vertice[i] - origin;
normalized_pos.x = Vector3.Project(diff, camera.transform.right).magnitude / camera.camera_width * (Vector3.Dot(Vector3.Project(diff, camera.transform.right).normalized, camera.transform.right));
normalized_pos.y = Vector3.Project(diff, camera.transform.up).magnitude / camera.camera_height * (Vector3.Dot(Vector3.Project(diff, camera.transform.up).normalized, camera.transform.up));
normalized_pos.z = 0;
result[i] = normalized_pos;
}
return(result);
}
public void updateHumanPose3D()
{
int camera_count = Main.instance.cameras.Length;
//score confidence for each 2d joint
float[,] confidence = new float[camera_count, joint_count];
//detect occlusiion
for (int skele_i = 0; skele_i < skeleton2D.Length; skele_i++)
{
RealCamera rcam = Main.instance.cameras[skele_i].GetComponent <RealCamera>();
for (int limb_i = 0; limb_i < limbs_index.GetLength(0); limb_i++)
{
for (int joint_i = 0; joint_i < skeleton2D[skele_i].joint_position.Length; joint_i++)
{
float proj_limb_w = limbs_width[limb_i] * rcam.proj_diatence / Vector3.Distance(joints3D_transform[joint_i].position, rcam.transform.position);
int pixel_limb_w = (int)((proj_limb_w / rcam.proj_width) * rcam.pixel_width);
Vector3 pixel_limb_0 = skeleton2D[skele_i].joint_position[limbs_index[limb_i, 0]];
Vector3 pixel_limb_1 = skeleton2D[skele_i].joint_position[limbs_index[limb_i, 1]];
Vector3 pixel_joint_i = skeleton2D[skele_i].joint_position[joint_i];
Vector3 normal_limb_v = Vector3.Normalize(pixel_limb_1 - pixel_limb_0);
Vector3 normal_limb_w_v = new Vector3(normal_limb_v[1], normal_limb_v[0], 0);
if (Vector3.Dot(normal_limb_v, pixel_joint_i - pixel_limb_0) < Vector3.Distance(pixel_limb_1, pixel_limb_0) &&
Vector3.Dot(normal_limb_v, pixel_joint_i - pixel_limb_0) > 0 &&
Mathf.Abs(Vector3.Dot(normal_limb_w_v, pixel_joint_i - pixel_limb_0)) < pixel_limb_w &&
Vector3.Distance(joints3D_transform[joint_i].position, rcam.transform.position) > Vector3.Distance(joints3D_transform[limbs_index[limb_i, 0]].position, rcam.transform.position))
{
confidence[skele_i, joint_i] = 1;
}
else
{
confidence[skele_i, joint_i] = 0;
}
}
}
}
Vector3[] final_position = new Vector3[joint_count];
float[] totoal_confidence = new float[joint_count];
//only use high confidence 2d joint to fuse the 3d human skeleton model
for (int skele_i = 0; skele_i < skeleton2D.Length; skele_i++)
{
RealCamera rcam = Main.instance.cameras[skele_i].GetComponent <RealCamera>();
Vector3[] nor_joints_position = skeleton2D[skele_i].getNormalizeJointsPosition();
for (int joint_i = 0; joint_i < skeleton2D[skele_i].joint_position.Length; joint_i++)
{
Vector3 proj_joint = rcam.transform.position + rcam.transform.forward * rcam.proj_diatence + rcam.transform.right * rcam.proj_width * (0.5f - nor_joints_position[joint_i].x) + rcam.transform.up * rcam.proj_height * (0.5f - nor_joints_position[joint_i].y);
Vector3 vec1 = proj_joint - rcam.transform.position;
Vector3 vec2 = joints3D_transform[joint_i].position - rcam.transform.position;
Vector3 new_pos = rcam.transform.position + Vector3.Project(vec2, vec1);
final_position[joint_i] += new_pos * confidence[skele_i, joint_i];
totoal_confidence[joint_i] += confidence[skele_i, joint_i];
//joints3D_transform[i].localPosition = new Vector3(joints2D_transform0[i].localPosition.x, joints2D_transform1[i].localPosition.y, joints2D_transform1[i].localPosition.x);
}
}
for (int joint_i = 0; joint_i < skeleton2D[0].joint_position.Length; joint_i++)
{
joints3D_transform[joint_i].localPosition = final_position[joint_i] / totoal_confidence[joint_i];
}
for (int i = 0; i < skeleton2D[0].joint_position.Length; i++)
{
}
}
