cross_view_tracking_for_3d_pose_estimation.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Imports"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "#==========================================\n",
    "# Title:  Multi person Cross view tracking\n",
    "# Author: Varun T\n",
    "# Date:   6 July 2023\n",
    "# Paper title: Cross-View Tracking for Multi-Human 3D Pose Estimation at over 100 FPS\n",
    "# Paper link: https://arxiv.org/abs/2003.03972\n",
    "# Original github repo: https://github.com/longcw/crossview_3d_pose_tracking/tree/master\n",
    "# Licesnse: Unknown\n",
    "# Note: Not an original author of the paper or code. No affiliation with AiFi Inc.\n",
    "#==========================================\n",
    "\n",
    "import argparse\n",
    "import cv2\n",
    "import numpy as np\n",
    "import pandas as pd\n",
    "from numpy.linalg import svd\n",
    "from tqdm import tqdm\n",
    "import os\n",
    "import matplotlib.pyplot as plt\n",
    "from dataset.crossview_dataset import data_utils\n",
    "from dataset.crossview_dataset.visualization.visualizer import Visualizer\n",
    "from dataset.crossview_dataset.calib.calibration import Calibration\n",
    "from matplotlib.patches import Circle\n",
    "\n",
    "%matplotlib widget\n",
    "from matplotlib.animation import FuncAnimation\n",
    "from mpl_toolkits.mplot3d import Axes3D\n",
    "import time\n",
    "\n",
    "from scipy.optimize import linear_sum_assignment\n",
    "from collections import defaultdict, OrderedDict\n",
    "\n",
    "from scipy.sparse import coo_matrix\n",
    "from bip_solver import GLPKSolver\n",
    "\n",
    "import logging\n",
    "#from tqdm import tqdm\n",
    "from tqdm.notebook import tqdm"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Config"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# path to the dataset. Change accordingly\n",
    "FOLDER = \".\\\\dataset\\\\Campus_Seq1\\\\\"\n",
    "frame_root = os.path.join(FOLDER,\"frames\")\n",
    "calibration_file = os.path.join(FOLDER,\"calibration.json\") \n",
    "pose_file = os.path.join(FOLDER,\"annotation_2d.json\")\n",
    "\n",
    "# frame iterator object\n",
    "frame_loader = data_utils.FrameLoader(frame_root, None)\n",
    "# pose iterator object\n",
    "pose_loader = data_utils.Pose2DLoader(pose_file)\n",
    "\n",
    "# load calibration\n",
    "calibration = Calibration.from_json(calibration_file)\n",
    "\n",
    "\n",
    "# change only after looking at the code below\n",
    "delta_time_threshold = 0.1  \n",
    "\n",
    "# config used in the paper for the campus dataset \n",
    "\n",
    "# 2D correspondence config\n",
    "w_2D = 0.4  # Weight of 2D correspondence\n",
    "alpha_2D = 25 # Threshold of 2D velocity\n",
    "lambda_a = 5  # Penalty rate of time interval\n",
    "lambda_t = 10\n",
    "\n",
    "# 3D correspondence confif\n",
    "w_3D = 0.6  # Weight of 3D correspondence\n",
    "alpha_3D = 0.1  # Threshold of distance\n",
    "#-----------------------------------------------------\n",
    "## Configure logger\n",
    "logging.basicConfig(filename='CVT_for_3d_pose_reconstruction.log',encoding='utf-8',level=logging.INFO, filemode = 'w') \n",
    "\n",
    "wait_time = 1\n",
    "\n",
    "no_of_cameras = len(calibration.camera_ids)\n",
    "world_ltrb = calibration.compute_world_ltrb()\n",
    "\n",
    "num_body_joints_detected_by_2d_pose_detector = 14\n",
    "# camera location\n",
    "camera_id_list = list(calibration.camera_ids)\n",
    "camera_3d_location = [] \n",
    "camera_look_at = []\n",
    "for cam_index in range(no_of_cameras):\n",
    "    cam_loc = calibration.cameras[camera_id_list[cam_index]].location\n",
    "    camera_3d_location.append(cam_loc)\n",
    "    camera_look_at.append(calibration.cameras[camera_id_list[cam_index]].look_at)\n",
    "    print(f'Camera id {camera_id_list[cam_index]} | location: {cam_loc}')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Helper funtions"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "## Vizualization functions\n",
    "\n",
    "def show_cam_image_with_pose_keypoints(frame_cam_0, frame_cam_0_pose, image_wh = [360, 288]):\n",
    "    # Create a figure. Equal aspect so circles look circular\n",
    "    fig = plt.figure()\n",
    "    ax = plt.axes()\n",
    "    ax.set_aspect('equal')\n",
    "\n",
    "    # Show the image\n",
    "    ax.imshow(frame_cam_0['image'])\n",
    "\n",
    "    # plot poses\n",
    "    if frame_cam_0_pose is not None:\n",
    "        point_radius = 2\n",
    "        score_thresh = 0.1\n",
    "        rendering = frame_cam_0['image'].copy()\n",
    "        render_h, render_w = rendering.shape[0:2]\n",
    "        scale_x = render_w / image_wh[0]\n",
    "        scale_y = render_h / image_wh[1]\n",
    "        for i, pose in enumerate(frame_cam_0_pose[\"poses\"]):\n",
    "            #color = color_generator.get_color(pose[\"id\"])\n",
    "\n",
    "            raw_points = np.copy(pose[\"points_2d\"])\n",
    "            keypoints_score = pose[\"scores\"]\n",
    "            raw_points[:, 0] *= scale_x\n",
    "            raw_points[:, 1] *= scale_y\n",
    "            keypoints = [tuple(map(int, point)) for point in raw_points]\n",
    "            for k, (point, score) in enumerate(zip(keypoints, keypoints_score)):\n",
    "                if score < score_thresh:\n",
    "                    continue\n",
    "                circ = Circle(point,point_radius)\n",
    "                ax.add_patch(circ)\n",
    "    \n",
    "        \n",
    "    # Show the image\n",
    "    plt.show()\n",
    "    \n",
    "def show_cam_location_on_3d_plot(camera_id_list, calibration, magnitude = 5, ax = None ):\n",
    "    \n",
    "    \n",
    "    if ax is None:\n",
    "        fig = plt.figure()\n",
    "        ax = fig.add_subplot(111, projection='3d')\n",
    "     \n",
    "    for cam_id in camera_id_list:\n",
    "        location = calibration.cameras[cam_id].location\n",
    "        look_at_point = calibration.cameras[cam_id].look_at\n",
    "        #look_at_point = calibration.cameras[cam_id].look_at_world_origin\n",
    "        ax.scatter(location[0], location[1], location[2], c='red', marker='o')\n",
    "        ax.text(location[0], location[1], location[2], f'{cam_id}', color='black')\n",
    "\n",
    "        axis_vector = (np.array(look_at_point) - np.array(location))/5\n",
    "        #magnitude = 5\n",
    "        #magnitude = np.linalg.norm(axis_vector)\n",
    "\n",
    "        pyramid_vertices = [\n",
    "            location,\n",
    "            (location[0] + axis_vector[0], location[1] + axis_vector[1], location[2] + axis_vector[2]),\n",
    "            (location[0] + axis_vector[0], location[1] + axis_vector[1], location[2] + axis_vector[2]),\n",
    "            (location[0] + axis_vector[0] -  magnitude, location[1] + axis_vector[1] -  magnitude, location[2] + axis_vector[2] -  magnitude),\n",
    "            (location[0] + axis_vector[0] +  magnitude, location[1] + axis_vector[1] -  magnitude, location[2] + axis_vector[2] -  magnitude),\n",
    "            (location[0] + axis_vector[0], location[1] + axis_vector[1], location[2] + axis_vector[2])\n",
    "        ]\n",
    "\n",
    "        pyramid_faces = [\n",
    "            [0, 1, 2],\n",
    "            [0, 3, 4],\n",
    "            [0, 1, 3],\n",
    "            [0, 4, 1],\n",
    "            [2, 1, 3],\n",
    "            [2, 1, 4],\n",
    "            [5, 2, 3],\n",
    "            [5, 2, 4]\n",
    "        ]\n",
    "\n",
    "        pyramid_vertices = np.array(pyramid_vertices)\n",
    "        for face in pyramid_faces:\n",
    "            ax.plot(pyramid_vertices[face, 0], pyramid_vertices[face, 1], pyramid_vertices[face, 2], c='blue')\n",
    "\n",
    "\n",
    "def calculate_perpendicular_distance(point, line_start, line_end):\n",
    "    \n",
    "    distance = np.linalg.norm(np.cross(line_end-line_start, line_start-point))/np.linalg.norm(line_end-line_start)\n",
    "    \n",
    "    return distance\n",
    "\n",
    "def get_velocity_at_this_timestamp_for_this_id_for_cur_timestamp(poses_3d_all_timestamps, timestamp_latest_pose, points_3d_latest_pose, id_latest_pose, delta_time_threshold = 0.1):\n",
    "    \"\"\"\n",
    "    poses_3d_at_cur_timstamp, poses_3d_at_last_timstamp: numpy array of shape (1 x no of joints)\n",
    "    \"\"\"\n",
    "    ## TODO: verify velocity estimation...\n",
    "    #  3D velocity estimated via a linear least-square method\n",
    "        \n",
    "    # go from the second last index in the window delta time threshold to the second last occurence of the points \n",
    "    # 3d for the ID id_latest_pose\n",
    "    velocity_t = np.zeros((len(points_3d_latest_pose)))\n",
    "    timestamp_tilde_frame = 0.0\n",
    "    for index in range(len(poses_3d_all_timestamps)-1,0,-1):\n",
    "        this_timestamp = list(poses_3d_all_timestamps.keys())[index]\n",
    "        if (timestamp_latest_pose - this_timestamp) > delta_time_threshold:\n",
    "            break\n",
    "        if this_timestamp >= timestamp_latest_pose or all(value is None for value in poses_3d_all_timestamps[this_timestamp]):\n",
    "            continue\n",
    "        # iterate through to the current timestamp and append values for the IDs which are not already covered before\n",
    "        for id_index in range(len(poses_3d_all_timestamps[this_timestamp])):\n",
    "            if poses_3d_all_timestamps[this_timestamp][id_index]['id'] == id_latest_pose:\n",
    "                points_3d_tilde_timestamp = np.array(poses_3d_all_timestamps[this_timestamp][id_index]['points_3d'])\n",
    "                timestamp_tilde_frame = this_timestamp\n",
    "                break\n",
    "    \n",
    "    if timestamp_tilde_frame > 0 and (timestamp_latest_pose > timestamp_tilde_frame):\n",
    "        assert len(points_3d_latest_pose) == len(points_3d_tilde_timestamp)\n",
    "        \n",
    "        \n",
    "        for k in range(len(points_3d_latest_pose)):\n",
    "            p_x1, p_y1, p_z1 = points_3d_latest_pose[k]\n",
    "            p_x2, p_y2, p_z2 = points_3d_tilde_timestamp[k]\n",
    "\n",
    "            # distance \n",
    "            #displacement_t = np.sqrt((p_x1 - p_x2) ** 2 + (p_y1 - p_y2) ** 2 + (p_z1 - p_z2) ** 2)\n",
    "            # displacement\n",
    "            displacement_t = (p_x1 - p_x2) + (p_y1 - p_y2) + (p_z1 - p_z2)\n",
    "            \n",
    "            # Divide displacements by corresponding time intervals to get velocities\n",
    "            assert float(timestamp_latest_pose) > float(timestamp_tilde_frame)\n",
    "            velocity_t[k] = displacement_t / ( float(timestamp_latest_pose) - float(timestamp_tilde_frame))\n",
    "         \n",
    "    return velocity_t.tolist()\n",
    "\n",
    "def get_latest_3D_poses_available_for_cur_timestamp(poses_3d_all_timestamps, timestamp_cur_frame, delta_time_threshold = 0.1):\n",
    "    \n",
    "    # Iterate through poses_3d_all_timestamps from the current timestamp to get the latest points 3D for IDs in \n",
    "    # the window of the delta_time_threshold> Note that time window from the current timestamp and not from the \n",
    "    # timestamp when points 3d were estimated\n",
    "    \n",
    "    # [[{'id': calculated, 'points_3d': list of target joints, 'timestamp': , 'velocity': }], [{}], ]\n",
    "    poses_3D_latest = []\n",
    "\n",
    "    id_list = []\n",
    "    \n",
    "    for index in range(len(poses_3d_all_timestamps)-1,0,-1):\n",
    "        this_timestamp = list(poses_3d_all_timestamps.keys())[index]\n",
    "        # time window ends return the ID\n",
    "        if (timestamp_cur_frame - this_timestamp) > delta_time_threshold:\n",
    "            break\n",
    "        # to get 3d pose at timestamp before the timestamp at the current frame\n",
    "        if this_timestamp >= timestamp_cur_frame or all(value is None for value in poses_3d_all_timestamps[this_timestamp]):\n",
    "            continue\n",
    "        if all(value is not None for value in poses_3d_all_timestamps[this_timestamp]):\n",
    "            # iterate through to the current timestamp and append values for the IDs which are not already covered before\n",
    "            for id_index in range(len(poses_3d_all_timestamps[this_timestamp])):\n",
    "                \n",
    "                if poses_3d_all_timestamps[this_timestamp][id_index]['id'] not in id_list:\n",
    "                    poses_3D_latest.append({'id': poses_3d_all_timestamps[this_timestamp][id_index]['id'],\n",
    "                                        'points_3d': poses_3d_all_timestamps[this_timestamp][id_index]['points_3d'],\n",
    "                                        'timestamp': this_timestamp,\n",
    "                                        'velocity': get_velocity_at_this_timestamp_for_this_id_for_cur_timestamp(poses_3d_all_timestamps,\n",
    "                                                                                                                 this_timestamp,\n",
    "                                                                                                                 poses_3d_all_timestamps[this_timestamp][id_index]['points_3d'],\n",
    "                                                                                                                 poses_3d_all_timestamps[this_timestamp][id_index]['id'])})\n",
    "                    id_list.append(poses_3d_all_timestamps[this_timestamp][id_index]['id'])\n",
    "        else:\n",
    "            continue\n",
    "        \n",
    "    if len(poses_3D_latest)>0:\n",
    "        poses_3D_latest = sorted(poses_3D_latest, key=lambda i: int(i['id']), reverse=False)\n",
    "    return poses_3D_latest\n",
    "\n",
    "\n",
    "def extract_key_value_pairs_from_poses_2d_list(data, id, timestamp_cur_frame, delta_time_threshold = 0.1):\n",
    "    camera_id_covered_list = []\n",
    "    result = []\n",
    "    \n",
    "    ## TODO: search the points in the delta time threshold window\n",
    "    # Find the latest timestamp for each camera\n",
    "    \n",
    "    for index in range(len(data)-1,0,-1):\n",
    "        \n",
    "        this_timestamp = data[index]['timestamp']\n",
    "        this_camera = data[index]['camera']\n",
    "        # time window ends return the ID\n",
    "        if (timestamp_cur_frame - this_timestamp) > delta_time_threshold:\n",
    "            break\n",
    "        if this_camera not in camera_id_covered_list:\n",
    "            \n",
    "            # iterate through to the current timestamp and append values for the IDs which are not already covered before\n",
    "            for pose_index in range(len(data[index]['poses'])):\n",
    "                if data[index]['poses'][pose_index]['id'] == id:\n",
    "                    result.append({\n",
    "                        'camera': this_camera,\n",
    "                        'timestamp': this_timestamp,\n",
    "                        'poses': data[index]['poses'][pose_index],\n",
    "                        'image_wh': data[index]['image_wh']\n",
    "                    })\n",
    "                    camera_id_covered_list.append(this_camera)\n",
    "                    break\n",
    "        else:\n",
    "            continue\n",
    "        \n",
    "    return result\n",
    "\n",
    "def separate_lists_for_incremental_triangulation(data):\n",
    "    result = {}\n",
    "    for item in data:\n",
    "        for key, value in item.items():\n",
    "            if key not in result:\n",
    "                result[key] = []\n",
    "            result[key].append(value)\n",
    "    return result\n",
    "\n",
    "def compute_affinity_epipolar_constraint_with_pairs(detections_pairs, alpha_2D, num_body_joints_detected_by_2d_pose_detector, calibration):\n",
    "    \n",
    "    Au_this_pair = 0\n",
    "    \n",
    "    # assuming D_i, D_j are each single matrix of 14x2\n",
    "    D_L = np.array(detections_pairs[0]['points_2d'])\n",
    "    D_R = np.array(detections_pairs[1]['points_2d'])\n",
    "    cam_L_id = detections_pairs[0]['camera_id']\n",
    "    cam_R_id = detections_pairs[1]['camera_id']\n",
    "    #image_wh_L = detections_pairs[0]['image_wh']\n",
    "    #image_wh_R = detections_pairs[1]['image_wh']\n",
    "    \n",
    "    assert len(D_L)==len(D_R)\n",
    "    assert cam_L_id != cam_R_id\n",
    "    F_matrix = calibration.get_fundamental_matrix([cam_L_id,cam_R_id])\n",
    "    \n",
    "    Au_this_pair = (1 -  ((calibration.distance_between_epipolar_lines(D_L, D_R, F_matrix))/ (2*alpha_2D)))\n",
    "    #Au_this_pair = (1 -  ((calibration.sampson_epipolar_distance(D_L, D_R, [cam_L_id, cam_R_id], F_matrix))/ (2*alpha_2D))) \n",
    "    #Au_this_pair = (1 -  ((calibration.symmetrical_epipolar_distance(D_L, D_R, [cam_L_id, cam_R_id], F_matrix))/ (2*alpha_2D)))\n",
    "    \n",
    "    \n",
    "    return Au_this_pair\n",
    "\n",
    "def get_affinity_matrix_epipolar_constraint(Du, alpha_2D, calibration):\n",
    "    \n",
    "    # Step 1: Get all unmatched detections per camera for the current timestamp\n",
    "    # Step 2: Generate pair of detections for all the detections of all cameras with \n",
    "    # detections of every other cameras  \n",
    "    # Step 3: For each camera: \n",
    "    #           for each pair of detection: \n",
    "    #               for each body joint in the detection:\n",
    "    #                   compute affinity matrix via epipolar contraint with the remaining detections in all other cameras\n",
    "    \n",
    "    \n",
    "    Du_cam_wise_split = {}\n",
    "    for entry in Du:\n",
    "        camera_id = entry['camera_id']\n",
    "        if camera_id not in Du_cam_wise_split:\n",
    "            Du_cam_wise_split[camera_id] = []\n",
    "        Du_cam_wise_split[camera_id].append(entry)\n",
    "    \n",
    "    \n",
    "    num_entries = sum(len(entries) for entries in Du_cam_wise_split.values())\n",
    "    Au = np.zeros((num_entries, num_entries), dtype=np.float32)\n",
    "    \n",
    "    # Create a dictionary to map each camera_id to an index\n",
    "    camera_id_to_index = {camera_id: i for i, camera_id in enumerate(Du_cam_wise_split.keys())}\n",
    "    \n",
    "    # Iterate over each camera\n",
    "    for camera_id, entries in Du_cam_wise_split.items():\n",
    "        # Iterate over each entry in the camera\n",
    "        for i in range(len(entries)):\n",
    "            # Generate pairs with entries from other cameras\n",
    "            for other_camera_id, other_entries in Du_cam_wise_split.items():\n",
    "                if other_camera_id != camera_id:\n",
    "                    for j in range(len(other_entries)):\n",
    "                        pair_ij = (entries[i], other_entries[j])\n",
    "                        pair_ji = (other_entries[j], entries[i])\n",
    "                        index_i = camera_id_to_index[camera_id] * len(entries) + i\n",
    "                        index_j = camera_id_to_index[other_camera_id] * len(other_entries) + j                        \n",
    "                        \n",
    "                        \n",
    "                        Au[index_i, index_j] = compute_affinity_epipolar_constraint_with_pairs(pair_ij,\n",
    "                                                                                               alpha_2D, \n",
    "                                                                                               num_body_joints_detected_by_2d_pose_detector,\n",
    "                                                                                               calibration)\n",
    "                        Au[index_j, index_i] = compute_affinity_epipolar_constraint_with_pairs(pair_ji,\n",
    "                                                                                               alpha_2D, \n",
    "                                                                                               num_body_joints_detected_by_2d_pose_detector,\n",
    "                                                                                               calibration)\n",
    "                        \n",
    "                        \n",
    "    return Au"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Algorithm\n",
    "Task: \n",
    "3D multiperson cross-view tracking. In each iteration we get a new frame of one of the available cameras. Our task is to get 2D poses in this frame. For each pose in this frame (detection) assign to a pre-existing tracked id (target) or assign a new ID if no matches are found. People are tracked only if they are in FoV of atleat 2 cameras. Once the person moves out of the FoV of the camera for more than a specified time window, we assign a new ID. Therefore on the occasion of a reentry the person is treated as new person. More formally, we have not implemented ReID as it was not provided in the paper.\n",
    "\n",
    "More formally,\n",
    "From Algorithm 1 in the paper tracking algo,\n",
    "Supposing there are M detections {Di,t,c|i =1, ..., M} in the new frame, \n",
    "we need to associate these detections to the last N tracked targets {Ti,t0 |i = 1, ..., N},\n",
    "and afterwards update the 3D locations of targets based on the matching results. \n",
    "\n",
    "Input: \n",
    "New 2D human poses Dt,c = {Dj,t,c|j = 1, ..., M}\n",
    "Previous targets Tt_tilde = {Ti,t_tilde |i = 1, ..., N} at time t_tilde\n",
    "Previous unmatched detections Du = {Dti,ci}\n",
    "\n",
    "Output: \n",
    "New targets with 3D poses Tt = {Ti,t} at time t"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "## TODO\n",
    "# 1) Implement ReID. For the case where the subjects renter the scene/ de-occluded after delta time threshold window\n",
    "# 2) Verify everything now.\n",
    "# 3) Find other TODOs\n",
    "\n",
    "\n",
    "# list of dictionary per frame/iteration with   [{'id': assigned by pose detector, 'points_2d': list of target joints} , {},...,{N}]\n",
    "poses_2d_all_frames = []\n",
    "\n",
    "# list of per frame/iteration dictionary with   [{'id': calculated, 'points_3d': list of target joints} , {},...,{M}, 'camera_ID': list of camera ID used for this pose]\n",
    "poses_3d_all_timestamps = defaultdict(list) \n",
    "\n",
    "# list of dictionary per frame/iteration with   [{'camera_id': , 'points_2d': list of target joints} , {},...,{<=N}]\n",
    "unmatched_detections_all_frames = defaultdict(list) \n",
    "\n",
    "\n",
    "new_id = -1 # So that ID number starts from 0\n",
    "iterations = 0  #  \n",
    "new_id_last_update_timestamp = 0  # latest timestamp at which we detected a new person.\n",
    "\n",
    "\n",
    "#### Loop through all the camera frames one by one\n",
    "\n",
    "for frame_index in tqdm(range(len(frame_loader))):\n",
    "    \n",
    "    #### (1) Intialize variable for each iterations\n",
    "    indices_T = [] \n",
    "    indices_D = []\n",
    "    logging.info('----'*100)\n",
    "    logging.info(iterations)\n",
    "    \n",
    "    #### (2) Get and preprocess data to use in algo. \n",
    "    frame_cam = frame_loader.__getitem__(frame_index)\n",
    "    camera_id_cur_frames = frame_cam['camera_name']\n",
    "    timestamp_cur_frame = frame_cam['timestamp']\n",
    "    logging.info(f'Current timestamp: {timestamp_cur_frame}')\n",
    "    \n",
    "    ## get 2D poses of all detections in the curent frames\n",
    "    if pose_file:\n",
    "        \n",
    "        points_2d_cur_frames = []\n",
    "        points_2d_scores_cur_frames = []\n",
    "        pose_json_cur_frame = pose_loader.get_data(frame_cam[\"frame_name\"].replace(\"\\\\\", \"/\"))\n",
    "        poses_cur_frames = pose_json_cur_frame['poses']   # poses list with multiple ids + points_2d + scores\n",
    "        \n",
    "        if not poses_cur_frames:\n",
    "            \n",
    "            iterations+=1\n",
    "            poses_3d_all_timestamps[timestamp_cur_frame].append(None)\n",
    "            continue \n",
    "        \n",
    "        for poses_index in range(len(poses_cur_frames)):\n",
    "            #print(poses_cur_frames[poses_index]['id'])\n",
    "            logging.info(f'Original ID : {poses_cur_frames[poses_index][\"id\"]}')\n",
    "            # deleting ID as they are already assigned in the current dataset\n",
    "            poses_cur_frames[poses_index]['id'] = '-1'\n",
    "            points_2d_cur_frames.append(poses_cur_frames[poses_index]['points_2d'])\n",
    "            points_2d_scores_cur_frames.append(poses_cur_frames[poses_index]['scores'])\n",
    "            \n",
    "        image_wh_cur_frames = pose_json_cur_frame['image_wh']\n",
    "        location_camera_center_cur_frames = calibration.cameras[camera_id_cur_frames].location\n",
    "        #print(f'poses cur frames: {poses_cur_frames}')\n",
    "        logging.info(f'poses cur frames: {poses_cur_frames}')\n",
    "    # Use pose detector on current frame YOLO!\n",
    "    # Currently not implemented     \n",
    "    else:\n",
    "        continue\n",
    "    \n",
    "    poses_2d_all_frames.append(pose_json_cur_frame)\n",
    "    \n",
    "    \n",
    "    ## get all available 3D poses from the last timestamp\n",
    "    poses_3D_latest = get_latest_3D_poses_available_for_cur_timestamp(poses_3d_all_timestamps, timestamp_cur_frame, delta_time_threshold = delta_time_threshold)\n",
    "    #print(f'poses 3d for this iter: {poses_3D_latest}')\n",
    "    logging.info(f'poses 3d for this iter: {poses_3D_latest}')\n",
    "    N_3d_poses_last_timestamp = len(poses_3D_latest)\n",
    "    M_2d_poses_this_camera_frame = len(points_2d_cur_frames)\n",
    "    \n",
    "    Dt_c = np.array(points_2d_cur_frames)  # Shape (M poses on frame , no of body points , 2)\n",
    "    Dt_c_scores = np.array(points_2d_scores_cur_frames)\n",
    "        \n",
    "    A = np.zeros((N_3d_poses_last_timestamp, M_2d_poses_this_camera_frame))  # Cross-view association matrix shape N x M\n",
    "    \n",
    "    # Cross-view association\n",
    "    for i in range(N_3d_poses_last_timestamp): # Iterate through prev N Target poses\n",
    "        \n",
    "        # x_t_tilde_tilde_c: projection of prev. detected ith 3d pose on a camera with ID camera_id_cur_frames \n",
    "        x_t_tilde_tilde_c = calibration.project(np.array(poses_3D_latest[i]['points_3d']), camera_id_cur_frames)\n",
    "        delta_t = timestamp_cur_frame - poses_3D_latest[i]['timestamp']     # Time interval\n",
    "        #print(f' delta_t: {delta_t}')\n",
    "        logging.info(f' delta_t: {delta_t}')\n",
    "        \n",
    "        for j in range(M_2d_poses_this_camera_frame): # Iterate through M poses \n",
    "            \n",
    "            # Each detection (Dj_t_c) in this frame will have k body points for every camera c\n",
    "            # x_t_c in image coordinates\n",
    "            # x_t_c_norm scale normalized image coordinates\n",
    "            x_t_c_norm = Dt_c[j].copy() \n",
    "            x_t_c_norm[:,0] = x_t_c_norm[:,0] / image_wh_cur_frames[0]\n",
    "            x_t_c_norm[:,1] = x_t_c_norm[:,1] / image_wh_cur_frames[1]\n",
    "            \n",
    "            K_joints_detected_this_person = len(x_t_c_norm)\n",
    "            \n",
    "            # use x_t_c vs x_t_c_norm? Verify...\n",
    "            back_proj_x_t_c_to_ground = calibration.cameras[camera_id_cur_frames].back_project(x_t_c_norm, z_worlds=np.zeros(K_joints_detected_this_person))\n",
    "            \n",
    "            for k in range(K_joints_detected_this_person):  # Iterate through K keypoints\n",
    "                \n",
    "                distance_2D = np.linalg.norm(x_t_c_norm[k] - x_t_tilde_tilde_c[k])  # Distance between joints\n",
    "\n",
    "                A_2D = w_2D * (1 - distance_2D / (alpha_2D*delta_t)) * np.exp(-lambda_a * delta_t)\n",
    "                \n",
    "                ## TODO: verify velocity estimation...\n",
    "                #  3D velocity to be estimated via a linear least-square method\n",
    "                velocity_t_tilde = poses_3D_latest[i]['velocity'][k]\n",
    "                \n",
    "                predicted_X_t = np.array(poses_3D_latest[i]['points_3d'][k]) + velocity_t_tilde * delta_t\n",
    "                \n",
    "                # Assuming that cameras will be pointed at the ground with z = 0\n",
    "                # 3d distance between vector given by camera center and ground point to predicted x_t\n",
    "                # All of the points lie in the same coordinate system? Verify...\n",
    "                dl = calculate_perpendicular_distance(point = predicted_X_t , line_start = location_camera_center_cur_frames , line_end = back_proj_x_t_c_to_ground[k])\n",
    "                \n",
    "                A_3D = w_3D * (1 - dl / alpha_3D) * np.exp(-lambda_a * delta_t)\n",
    "                \n",
    "                A[i,j] += A_2D + A_3D\n",
    "                \n",
    "    # Perform Hungarian algorithm for assignment for each camera\n",
    "    indices_T, indices_D = linear_sum_assignment(A, maximize = True)\n",
    "    \n",
    "    #print(f'Indices_T, Indices_D: {indices_T, indices_D}')\n",
    "    logging.info(f'Indices_T, Indices_D: {indices_T, indices_D}')\n",
    "    # redundant but checking one to one mapping\n",
    "    assert len(indices_D) == len(indices_T), \"number of detection should be equal to target for each iterations\"\n",
    "    \n",
    "    # Target update\n",
    "    for i,j in zip(indices_T, indices_D):\n",
    "        \n",
    "        # Update latest detections in the poses_2d_all_frames  \n",
    "        poses_2d_all_frames[-1]['poses'][j]['id'] = poses_3D_latest[i]['id']\n",
    "\n",
    "        # Get latest 2D Pose data from all the cameras for the detected ID \n",
    "        poses_2d_inc_rec_other_cam = extract_key_value_pairs_from_poses_2d_list(poses_2d_all_frames, \n",
    "                                                                                id = poses_3D_latest[i]['id'],\n",
    "                                                                                timestamp_cur_frame = timestamp_cur_frame,\n",
    "                                                                                delta_time_threshold = delta_time_threshold )\n",
    "        \n",
    "        # move following code in func extract_key_value_pairs_from_poses_2d_list to get *_inc_rec variables directly\n",
    "        # Get 2D poses of ID \n",
    "        dict_with_poses_for_n_cameras_for_latest_timeframe = separate_lists_for_incremental_triangulation(poses_2d_inc_rec_other_cam)\n",
    "        \n",
    "        camera_ids_inc_rec = []\n",
    "    \n",
    "        image_wh_inc_rec = []\n",
    "    \n",
    "        timestamps_inc_rec = []\n",
    "\n",
    "        points_2d_inc_rec = []\n",
    "        \n",
    "        camera_ids_inc_rec = dict_with_poses_for_n_cameras_for_latest_timeframe['camera']\n",
    "        image_wh_inc_rec = dict_with_poses_for_n_cameras_for_latest_timeframe['image_wh']\n",
    "        timestamps_inc_rec = dict_with_poses_for_n_cameras_for_latest_timeframe['timestamp']\n",
    "        \n",
    "        for dict_index in range(len(dict_with_poses_for_n_cameras_for_latest_timeframe['poses'])):\n",
    "            \n",
    "            points_2d_inc_rec.append(dict_with_poses_for_n_cameras_for_latest_timeframe['poses'][dict_index]['points_2d'])\n",
    "        \n",
    "        # migration to func ends here \n",
    "   \n",
    "        K_joints_detected_this_person = len(Dt_c[j])\n",
    "        \n",
    "        Ti_t = []\n",
    "        \n",
    "        for k in range(K_joints_detected_this_person): # iterate through k points\n",
    "            \n",
    "            # get all the 2d pose point from all the cameras where this target was detected last\n",
    "            # i.e. if current frame is from cam 1 then get last detected 2d pose of this target \n",
    "            # from all of the cameras. Do triangulation with all cameras with detected ID\n",
    "            \n",
    "            _, Ti_k_t = calibration.linear_ls_triangulate_weighted(np.array(points_2d_inc_rec)[:,k,:], \n",
    "                                                                    camera_ids_inc_rec, \n",
    "                                                                    image_wh_inc_rec, \n",
    "                                                                    lambda_t, \n",
    "                                                                    timestamps_inc_rec)\n",
    "            Ti_t.append(Ti_k_t.tolist())\n",
    "        \n",
    "        \n",
    "        # If there is no entry for the current id at the current timestamp\n",
    "        if i >= len(poses_3d_all_timestamps[timestamp_cur_frame]):\n",
    "            poses_3d_all_timestamps[timestamp_cur_frame].append({'id': poses_3D_latest[i]['id'],\n",
    "                                                        'points_3d': Ti_t,\n",
    "                                                        'camera_ID': [camera_id_cur_frames]})\n",
    "            \n",
    "        # If there exist an entry already overwrite as this would be contain updated timestamps\n",
    "        # from all cameras for points 3D. \n",
    "        else:\n",
    "            poses_3d_all_timestamps[timestamp_cur_frame][i]['points_3d'] = Ti_t\n",
    "            poses_3d_all_timestamps[timestamp_cur_frame][i]['camera_ID'].append(camera_id_cur_frames)\n",
    "    \n",
    "    # Try to have same format as points_2d \n",
    "    # Target initialization\n",
    "    \n",
    "    for j in range(M_2d_poses_this_camera_frame):\n",
    "        if j not in indices_D:\n",
    "            unmatched_detections_all_frames[timestamp_cur_frame].append({'camera_id': camera_id_cur_frames,\n",
    "                                                            'points_2d': Dt_c[j],\n",
    "                                                            'scores': Dt_c_scores[j],\n",
    "                                                            'image_wh': image_wh_cur_frames})\n",
    "    \n",
    "    # There is no previous 3D target for two timestamps\n",
    "    ## TODO:\n",
    "    # Memory of only two timestamps is not good. Change to memory of entire runtime? \n",
    "    # Think about corner as here only affinity is geometric consistency between two views\n",
    "    # and not apperance\n",
    "\n",
    "    iterations+=1\n",
    "    \n",
    "    # Assuming we get frame data for all the cameras sequentially\n",
    "    if iterations % no_of_cameras == 0:\n",
    "        # If the list is not empty\n",
    "        if unmatched_detections_all_frames[timestamp_cur_frame]:\n",
    "        # delete entries which ever are already used in matching\n",
    "    \n",
    "            unique_cameras_set_this_iter_with_unmatched_det = set(item['camera_id'] for item in unmatched_detections_all_frames[timestamp_cur_frame])\n",
    "            \n",
    "            num_cameras_this_iter_with_unmatched_det = len(unique_cameras_set_this_iter_with_unmatched_det)\n",
    "            \n",
    "            #print(f'num_cameras_this_iter_with_unmatched_det: {num_cameras_this_iter_with_unmatched_det}')\n",
    "            logging.info(f'num_cameras_this_iter_with_unmatched_det: {num_cameras_this_iter_with_unmatched_det}')\n",
    "            # if there is unmatched detection from atleast two different cameras for the current timestamp\n",
    "            if (num_cameras_this_iter_with_unmatched_det > 1):\n",
    "                \n",
    "                Au = get_affinity_matrix_epipolar_constraint(unmatched_detections_all_frames[timestamp_cur_frame],\n",
    "                                                            alpha_2D,\n",
    "                                                            calibration)  # Apply epipolar constraint\n",
    "                #clusters = graph_partition(Au)  # Perform graph partitioning\n",
    "                solver = GLPKSolver(min_affinity=0, max_affinity=1)\n",
    "                clusters, sol_matrix = solver.solve(Au.astype(np.double), rtn_matrix  = True)\n",
    "                \n",
    "                \n",
    "                # Target initialization from clusters\n",
    "                for Dcluster in clusters:\n",
    "                    points_2d_this_cluster = []\n",
    "                    camera_id_this_cluster = []\n",
    "                    image_wh_this_cluster = []\n",
    "                    \n",
    "                    if len(Dcluster) >= 2:\n",
    "                    \n",
    "                        #print(f'Inside cluster: {Dcluster} ')\n",
    "                        logging.info(f'Inside cluster: {Dcluster} ')\n",
    "                        \n",
    "                        # TODO: Adhoc Solution. Change in the future\n",
    "                        # If there a new person detected within delta time threshold then probably\n",
    "                        # this new person is belongs to the older id\n",
    "                        if timestamp_cur_frame - new_id_last_update_timestamp > delta_time_threshold:\n",
    "                            \n",
    "                            new_id_last_update_timestamp = timestamp_cur_frame\n",
    "                            new_id +=1\n",
    "                        \n",
    "                        for detection_index in Dcluster:\n",
    "                            points_2d_this_cluster.append(unmatched_detections_all_frames[timestamp_cur_frame][detection_index]['points_2d'])\n",
    "                            \n",
    "                            camera_id_this_cluster.append(unmatched_detections_all_frames[timestamp_cur_frame][detection_index]['camera_id'])\n",
    "                            \n",
    "                            image_wh_this_cluster.append(unmatched_detections_all_frames[timestamp_cur_frame][detection_index]['image_wh'])\n",
    "                            \n",
    "                            # Change ID for all the used points in poses 2D all frames for the current timestamp \n",
    "                            # Since points are added in order of the original poses_2d_all_frames thus simply \n",
    "                            # overwrite the ID to index of the Dcluster. Verify...\n",
    "                        \n",
    "                            for new_index_set_id in range(len(poses_2d_all_frames[-len(Dcluster):][detection_index]['poses'])):\n",
    "                                if str(poses_2d_all_frames[-len(Dcluster):][detection_index]['poses'][new_index_set_id]['id']) == '-1':\n",
    "                                    poses_2d_all_frames[-len(Dcluster):][detection_index]['poses'][new_index_set_id]['id'] = new_id\n",
    "                        \n",
    "                        # Overwriting the unmatched detection for the current timeframe with the indcies not present in the detection cluster\n",
    "                        unmatched_detections_all_frames[timestamp_cur_frame] = [unmatched_detections_all_frames[timestamp_cur_frame][i] for i in range(len(unmatched_detections_all_frames[timestamp_cur_frame])) if i not in Dcluster]\n",
    "                        \n",
    "                        Tnew_t = calibration.triangulate_complete_pose(points_2d_this_cluster,camera_id_this_cluster,image_wh_this_cluster)\n",
    "                        \n",
    "                        # Add the 3D points according to the ID \n",
    "                        poses_3d_all_timestamps[timestamp_cur_frame].append({'id': new_id,\n",
    "                                                                    'points_3d': Tnew_t.tolist(),\n",
    "                                                                    'camera_ID': camera_id_this_cluster})\n",
    "    \n",
    "    \n",
    "    #print(f'unmatched_detections_all_frames: {unmatched_detections_all_frames}')\n",
    "    logging.info(f'unmatched_detections_all_frames: {unmatched_detections_all_frames}')\n",
    "    #print(f'poses 3d calc for this timestamp: {poses_3d_all_timestamps[timestamp_cur_frame]}')\n",
    "    logging.info(f'poses 3d calc for this timestamp: {poses_3d_all_timestamps[timestamp_cur_frame]}')\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 3D plot for single timestamp with matplotlib"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "ori_wcx = np.mean(world_ltrb[0::2])\n",
    "ori_wcy = np.mean(world_ltrb[1::2])\n",
    "world_ltrb_mean_cen = world_ltrb.copy()\n",
    "world_ltrb_mean_cen[0::2] -= ori_wcx\n",
    "world_ltrb_mean_cen[1::2] -= ori_wcy\n",
    "camera_id_list = list(camera_id_list)\n",
    "\n",
    "#maximum_person_to_plot = 25\n",
    "#cmap = plt.get_cmap('viridis')\n",
    "#slicedCM = cmap(np.linspace(0, 1, maximum_person_to_plot)) \n",
    "\n",
    "fig = plt.figure()\n",
    "ax = fig.add_subplot(1, 1, 1, projection='3d')\n",
    "ax.set_xlabel('X')\n",
    "ax.set_ylabel('Y')\n",
    "ax.set_zlabel('Z')\n",
    "ax.set_xlim3d(world_ltrb_mean_cen[0], world_ltrb_mean_cen[2])\n",
    "ax.set_ylim3d(world_ltrb_mean_cen[1], world_ltrb_mean_cen[3])\n",
    "ax.set_zlim3d(0, 300)\n",
    "timestamp_to_plot = float(41.72)\n",
    "ax.set_title(f'Timestamp: {timestamp_to_plot}')\n",
    "show_cam_location_on_3d_plot(camera_id_list, calibration, magnitude=20, ax = ax)\n",
    "scatter_points_arr = []\n",
    "person_ID_list = []\n",
    "for pose_index in range(len(poses_3d_all_timestamps[timestamp_to_plot])):\n",
    "    this_pose = np.array(poses_3d_all_timestamps[timestamp_to_plot][pose_index]['points_3d'])\n",
    "    this_pose[:, 0] -= ori_wcx\n",
    "    this_pose[:, 1] -= ori_wcy\n",
    "    \n",
    "    keep = (\n",
    "        (this_pose[:, 0] > world_ltrb_mean_cen[0])\n",
    "        & (this_pose[:, 0] < world_ltrb_mean_cen[2])\n",
    "        & (this_pose[:, 1] > world_ltrb_mean_cen[1])\n",
    "        & (this_pose[:, 1] < world_ltrb_mean_cen[3])\n",
    "    )\n",
    "    this_pose = this_pose[keep]\n",
    "    scatter_points_arr.append(this_pose)\n",
    "    person_ID_list.append(str(poses_3d_all_timestamps[timestamp_to_plot][pose_index]['id']))\n",
    "    ax.text(this_pose[-1,0], this_pose[-1,1], this_pose[-1,2]+10, f'ID: {person_ID_list[pose_index]}', color='black')\n",
    "    \n",
    "# Only do the scatter inside the loop\n",
    "# top head is last keypoint \n",
    "scatter_points_arr = np.array(scatter_points_arr).reshape(-1,3)\n",
    "\n",
    "ax.scatter(scatter_points_arr[:,0], scatter_points_arr[:,1], scatter_points_arr[:,2], c = 'g', marker='o')\n",
    "# drawing updated values\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 3D animation with matplotlib"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "plot_3d_animation = True\n",
    "if plot_3d_animation:\n",
    "    ori_wcx = np.mean(world_ltrb[0::2])\n",
    "    ori_wcy = np.mean(world_ltrb[1::2])\n",
    "    world_ltrb_mean_cen = world_ltrb.copy()\n",
    "    world_ltrb_mean_cen[0::2] -= ori_wcx\n",
    "    world_ltrb_mean_cen[1::2] -= ori_wcy\n",
    "    camera_id_list = list(camera_id_list)\n",
    "\n",
    "    # Set up the figure and axis\n",
    "    fig = plt.figure()\n",
    "    ax = fig.add_subplot(1, 1, 1, projection='3d')\n",
    "    ax.set_xlabel('X')\n",
    "    ax.set_ylabel('Y')\n",
    "    ax.set_zlabel('Z')\n",
    "    #ax.set_xlim3d(world_ltrb_mean_cen[0], world_ltrb_mean_cen[2])\n",
    "    #ax.set_ylim3d(world_ltrb_mean_cen[1], world_ltrb_mean_cen[3])\n",
    "    ax.set_xlim3d(-2000, 2000)\n",
    "    ax.set_ylim3d(-1000, 1000)\n",
    "    ax.set_zlim3d(0, 300)\n",
    "        \n",
    "    \n",
    "    #show_cam_location_on_3d_plot(camera_3d_location, camera_look_at, magnitude=20, ax = ax)\n",
    "    show_cam_location_on_3d_plot(camera_id_list, calibration, magnitude=20, ax = ax)\n",
    "    scatter = ax.scatter([], [], [], c='g', marker='o')\n",
    "    #person_ID_text = ax.text(0,0,0, s = '', color='black')\n",
    "    \n",
    "    # Animation update function\n",
    "    def update(index):\n",
    "        # Clear the previous frame\n",
    "        scatter._offsets3d = ([], [], [])\n",
    "        #person_ID_text.set_text('')\n",
    "        #person_ID_text.set_position((0,0,0))\n",
    "    \n",
    "        timestamp_to_plot = list(poses_3d_all_timestamps.keys())[index]\n",
    "        scatter_points_list = []\n",
    "        # Get the 3D points for the current frame\n",
    "        if all(value is not None for value in poses_3d_all_timestamps[timestamp_to_plot]):\n",
    "            for pose_index in range(len(poses_3d_all_timestamps[timestamp_to_plot])):\n",
    "                this_pose = np.array(poses_3d_all_timestamps[timestamp_to_plot][pose_index]['points_3d'])\n",
    "                this_pose[:, 0] -= ori_wcx\n",
    "                this_pose[:, 1] -= ori_wcy\n",
    "                \n",
    "                keep = (\n",
    "                    (this_pose[:, 0] > world_ltrb_mean_cen[0])\n",
    "                    & (this_pose[:, 0] < world_ltrb_mean_cen[2])\n",
    "                    & (this_pose[:, 1] > world_ltrb_mean_cen[1])\n",
    "                    & (this_pose[:, 1] < world_ltrb_mean_cen[3])\n",
    "                )\n",
    "                #this_pose = this_pose[keep]\n",
    "                scatter_points_list.append(this_pose)\n",
    "                person_ID = str(poses_3d_all_timestamps[timestamp_to_plot][pose_index]['id'])\n",
    "                                \n",
    "                #person_ID_text.set_text(f'ID: {person_ID}')\n",
    "                #person_ID_text.set_position((this_pose[-1,0], this_pose[-1,1], this_pose[-1,2]+10))\n",
    "                \n",
    "            scatter_points_arr = np.array(scatter_points_list).reshape(-1,3)\n",
    "            scatter._offsets3d = (scatter_points_arr[:,0], scatter_points_arr[:,1], scatter_points_arr[:,2])\n",
    "        \n",
    "        # Set the plot title with the timestamp\n",
    "        ax.set_title(f'Timestamp: {timestamp_to_plot}')\n",
    "    \n",
    "    # Create the animation\n",
    "    animation = FuncAnimation(fig, update, len(poses_3d_all_timestamps), interval=20, repeat=False)\n",
    "\n",
    "    # Show the animation\n",
    "    plt.show()\n"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.10.6"
  },
  "orig_nbformat": 4
 },
 "nbformat": 4,
 "nbformat_minor": 2
}