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++) { } }