"""
Camera associate
written by <NAME> on 2019-03-21, version 1.0
Updated by <NAME> on Sep 29, 2020, version 1.1.0
"""

import os
import sys
import math
import pandas as pd
import numpy as np
import cv2
import json
import matplotlib.pyplot as plt
from sklearn.linear_model import LinearRegression
import seaborn as sns
from Common import SRC_IMAGES, DET_RESULT, ROI_RESULT, SCT_RESULT, MCT_RESULT


# ==== Default files path setting, you could comment the code line below and set your own path in new project ====
from Common import cam_names, data_path, box_info, roi_info, save_path, track_info, associate_info

# ========== CONFIGURATION ==========
# style set for figure
sns.set_palette('deep', desat=.6)
sns.set_context(rc={'figure.figsize': (8, 5)})

# ==== FILE PATH ====
# Default files path setting is in Common.py



# camera associate
associate_cam = {
    0: 1,
    1: 0,
    2: 3,
    3: 4,
    4: 3
}

associate_dict_c1_c2 = {
    0: 0,
    1: 1,
    2: 2,
    3: 3,
    4: 4
}
# camera associate
associate_dict_c3_c4 = {
    0: 0,
    1: 1,
    2: 2,
    3: 3,
    4: 4
}

def get_associate_dict(filepath):
    with open(filepath, 'r') as doc:
        data = json.load(doc)
        return data

# ========== BASIC CLASS ==========


class GaussDistribution(object):
    def __init__(self, mu, sigma):
        self.mu = mu
        self.sigma = sigma
        
    def get_probability(self, x):
        
        if type(self.mu) is np.float64:
            return self._1D_gaussian(x)
        if type(self.mu) is np.ndarray:
            return self._2D_gaussian(x)
        return None
        
    def _2D_gaussian(self, x):
        '''Get the probability of x with 2D gaussian'''
        n = self.mu.shape[0]
        
        sigma_det = np.linalg.det(self.sigma)
        sigma_inv = np.linalg.inv(self.sigma)
        
        # print("sigma_det,sigma_inv:",sigma_det,sigma_inv)
        
        den = np.sqrt((2*np.pi)**n*sigma_det)
        num = np.einsum('...k,kl,...l->...', x-self.mu, sigma_inv, x-self.mu)

        return np.exp(-num/2)/den
        
    def _1D_gaussian(self, x):
        '''Get the probability of x with 2D gaussian'''
        den = np.sqrt(2*np.pi*np.power(self.sigma, 2))
        num = np.power(x-self.mu, 2)/np.power(self.sigma, 2)
        # print("den,num:",den,num)
        return np.exp(-num/2)/den

    def get_normal_1D_gaussian(self, x):
        '''Get the probability of x with 2D gaussian'''
        x_norm = (x-self.mu)/self.sigma
        den = np.sqrt(2*np.pi*np.power(1, 2))
        num = np.power(x_norm, 2)/np.power(1, 2)
        # print("den,num:",den,num)
        return np.exp(-num/2)/den


class SingleCameraSTP(object):
    def __init__(   self,
                    tracking_record=None,
                    perspective_transformer=None,
                    time_interval=25,
                    var_beta_x=1.0,
                    var_beta_y=1.0):
        self.perspective_transformer = perspective_transformer
        self.tracking_record = tracking_record
        self.time_interval = time_interval

        self.perspective_trace = None
        self.distance_dict = None
        self.motion_params_4_all = None
        self.motion_params_4_each = None
        
        self.prob_alpha_x = 1       # x amplitude multiplier
        self.prob_alpha_y = 1       # y amplitude multiplier
        
        self.var_beta_x = var_beta_x         # x range multiplier
        self.var_beta_y = var_beta_y            # y range multiplier
        
        self.get_motion_params()
        self.nx_predictor = GaussDistribution(
                                    self.time_interval*self.motion_params_4_all['mean_x'],
                                    self.time_interval*self.var_beta_x*self.motion_params_4_all['var_x'])
        self.ny_predictor = GaussDistribution(
                                    self.time_interval*self.motion_params_4_all['mean_y'],
                                    self.time_interval*self.var_beta_y*self.motion_params_4_all['var_y'])

    @staticmethod
    def version(self):
        return print("Written by sunzhu, 2019-03-21, version 1.0")
    
    def get_motion_params(self):
        self.perspective_trace = get_pt_box_info(self.tracking_record, self.perspective_transformer)
        self.distance_dict = get_dist_in_deltaT(self.perspective_trace)
        self.motion_params_4_each, self.motion_params_4_all = get_statistical_paras_of_dist_in_deltaT(self.distance_dict)
        return self.motion_params_4_each, self.motion_params_4_all
        
    def update_predictor(self, tracking_record=None, time_interval=None, var_bata_x=1, var_bate_y=1):
        if tracking_record is not None:
            self.tracking_record = tracking_record
            self.get_motion_params()
        if time_interval is not None:
            self.time_interval = time_interval
        self.nx_predictor = GaussDistribution(
                                    self.time_interval*self.motion_params_4_all['mean_x'],
                                    self.time_interval*self.var_beta_x*self.motion_params_4_all['var_x'])
        self.ny_predictor = GaussDistribution(
                                    self.time_interval*self.motion_params_4_all['mean_y'],
                                    self.time_interval*self.var_beta_y*self.motion_params_4_all['var_y'])
            
    def get_probability_map(self, base_x, base_y, start_x=0, start_y=0, length_x=10, length_y=10, height=100, width=100):
        return get_probability_map(self.nx_predictor, self.ny_predictor, base_x, base_y, start_x, start_y, length_x, length_y, height, width)
        
    def get_probability(self, x, y, base_x, base_y):
        """Calculate the probability of pos
            x, y: pos to be calculated;
            base_x, base_y: predicted pos
        """
        delta_x = x - base_x 
        delta_y = y - base_y
        prob_x = self.nx_predictor.get_probability(delta_x)
        prob_y = self.ny_predictor.get_probability(delta_y)
        prob_xy = self.prob_alpha_x*prob_x*self.prob_alpha_y*prob_y
        return prob_x, prob_y, prob_xy
        
    def get_distance(self, x, y, base_x, base_y, alpha=0.5):
        pred_x = base_x+self.time_interval*self.motion_params_4_all['mean_x']
        pred_y = base_y+self.time_interval*self.motion_params_4_all['mean_y']
        comp_distance = abs(x-pred_x)*(1-alpha)+abs(y-pred_y)*alpha
        return comp_distance


class MultiCamerasSTP(object):
    def __init__(   self,
                    obj_single_camera_stp_cam_1=None,
                    obj_single_camera_stp_cam_2=None,
                    associate_dict=None):
        self.obj_single_camera_stp_cam_1 = obj_single_camera_stp_cam_1
        self.obj_single_camera_stp_cam_2 = obj_single_camera_stp_cam_2
        self.associate_dict = associate_dict
        
        self.coord_pairs = None
        self.coord_transformer = None
        self.starting_point = None
        
        self.prob_alpha_x = 1       # x multiplier for position probability calculating
        self.prob_alpha_y = 1       # y multiplier for position probability calculating
        
        self.var_beta_x = 5         # x range multiplier
        self.var_beta_y = 1         # y range multiplier
        
        self.get_coord_transformer()
        self.get_start_point_transform()
        
    def version(self):
        return print("Written by sunzhu, 2019-03-21, version 1.0")
    
    def get_coord_transformer(self):
        self.coord_pairs = get_STP_in_multi_cameras(
                                    self.obj_single_camera_stp_cam_1,
                                    self.obj_single_camera_stp_cam_2,
                                    self.associate_dict
                                    )
                                    
        cam_1_x, cam_2_x, cam_1_y, cam_2_y = self.coord_pairs
        
        self.coord_transformer = {}
        
        # Front to back transformer
        F2B_transformer = {}
        
        model_F2B_x = LinearRegression()
        model_F2B_x.fit(cam_1_x, cam_2_x)
        model_F2B_y = LinearRegression()
        model_F2B_y.fit(cam_1_y, cam_2_y)
        
        F2B_transformer['x'] = model_F2B_x
        F2B_transformer['y'] = model_F2B_y
        
        # Back to front transformer
        B2F_transformer = {}
        
        model_B2F_x = LinearRegression()
        model_B2F_x.fit(cam_2_x, cam_1_x)
        model_B2F_y = LinearRegression()
        model_B2F_y.fit(cam_2_y, cam_1_y)
        
        B2F_transformer['x'] = model_B2F_x
        B2F_transformer['y'] = model_B2F_y
        
        # Coordinate transformer
        self.coord_transformer['F2B'] = F2B_transformer
        self.coord_transformer['B2F'] = B2F_transformer
        
        return self.coord_transformer

    def get_start_point_transform(self, start_x_in_cam_2=0, start_y_in_cam_2=0):
        """Transform the coordinate value of starting point in cam 2 into the coordinate value in cam 1
            The coordinate value refers the position of object after perspective transforming.
        """
        corresponding_start_x_in_cam_1 = self.coord_transformer['B2F']['x'].predict([[start_x_in_cam_2]])[0][0]
        corresponding_start_y_in_cam_1 = self.coord_transformer['B2F']['y'].predict([[start_y_in_cam_2]])[0][0]
        # print(corresponding_start_x_in_cam_1,corresponding_start_y_in_cam_1)
        self.starting_point = [corresponding_start_x_in_cam_1, corresponding_start_y_in_cam_1]
        return self.starting_point
    
    def get_probability_map(self, base_x, base_y, t_interval=None, height=300, width=80):
        nx_predictor = GaussDistribution(  t_interval*self.obj_single_camera_stp_cam_1.motion_params_4_all['mean_x'],
                                            t_interval*self.var_beta_x*self.obj_single_camera_stp_cam_1.motion_params_4_all['var_x'])
        ny_predictor = GaussDistribution(  t_interval*self.obj_single_camera_stp_cam_1.motion_params_4_all['mean_y'],
                                            t_interval*self.var_beta_y*self.obj_single_camera_stp_cam_1.motion_params_4_all['var_y'])
        # print("starting_points:",self.starting_point[0],self.starting_point[1])
        
        return get_probability_map( nx_predictor,
                                    ny_predictor,
                                    base_x,
                                    base_y,
                                    start_x=self.starting_point[0],
                                    start_y=self.starting_point[1],
                                    )
                                    
    def get_probability(self, x, y, base_x, base_y, t_interval=None):
        trans_x = self.coord_transformer['B2F']['x'].predict([[x]])[0][0]
        trans_y = self.coord_transformer['B2F']['y'].predict([[y]])[0][0]
        delta_x = trans_x - base_x
        delta_y = trans_y - base_y

        # print("delta_x:",delta_x,"delta_y:",delta_y)
        nx_predictor = GaussDistribution(
                        t_interval*self.obj_single_camera_stp_cam_1.motion_params_4_all['mean_x'],
                        t_interval*self.var_beta_x*self.obj_single_camera_stp_cam_1.motion_params_4_all['var_x'])
        ny_predictor = GaussDistribution(
                        t_interval*self.obj_single_camera_stp_cam_1.motion_params_4_all['mean_y'],
                        t_interval*self.var_beta_y*self.obj_single_camera_stp_cam_1.motion_params_4_all['var_y'])
        # print('p_x',nx_predictor.get_probability(delta_x))      
        # print('p_y',ny_predictor.get_probability(delta_y))
        prob_x = nx_predictor.get_probability(delta_x)
        prob_y = ny_predictor.get_probability(delta_y)
        prob_xy = self.prob_alpha_x*prob_x*self.prob_alpha_y*prob_y
        return prob_x, prob_y, prob_xy
        
    def get_distance(self, x, y, base_x, base_y, t_interval=None, alpha=0.5):
        trans_x = self.coord_transformer['B2F']['x'].predict([[x]])[0][0]
        trans_y = self.coord_transformer['B2F']['y'].predict([[y]])[0][0]
        pred_x = base_x+t_interval*self.obj_single_camera_stp_cam_1.motion_params_4_all['mean_x']
        pred_y = base_y+t_interval*self.obj_single_camera_stp_cam_1.motion_params_4_all['mean_y']
        comp_distance = abs(x-pred_x)*(1-alpha)+abs(y-pred_y)*alpha
        return comp_distance
        
        
# =========== UTILS FUNCTIONS ============
def get_dist_in_deltaT(pt_trace):
    """Get moving distance of object within delta t"""
    delta_loc = {}
    for k in pt_trace:
        obj_trace = pt_trace[k]
        obj_delta_loc = []
        for i in range(len(obj_trace)-1):
            x1, y1, t1 = obj_trace[i][0][0], obj_trace[i][0][1], obj_trace[i][1]
            x2, y2, t2 = obj_trace[i+1][0][0], obj_trace[i+1][0][1], obj_trace[i+1][1]
            delta_x = x2 - x1
            delta_y = y2 - y1
            delta_t = t2 - t1
            if delta_t > 0:
                delta_x = delta_x/delta_t
                delta_y = delta_y/delta_t
                obj_delta_loc.append([delta_x, delta_y])
        delta_loc[k] = obj_delta_loc
    return delta_loc


def get_statistical_paras_of_dist_in_deltaT(delta_loc):
    res_4_each = {}  # parameters of each object
    res_4_all = {}  # parameters of all objects
    x_list = []
    y_list = []
    # Calculate the mean,var and cov of x,y for each object
    for k in delta_loc:
        tmp_dict = {}
        if len(delta_loc[k]) > 1:
            loc_x, loc_y = zip(*delta_loc[k])
            print(loc_x)
            tmp_dict['mean_x'] = np.mean(loc_x)
            tmp_dict['var_x'] = np.var(loc_x)
            tmp_dict['mean_y'] = np.mean(loc_y)
            tmp_dict['var_y'] = np.var(loc_y)
            tmp_dict['mean_xy'] = np.array([np.mean(loc_x), np.mean(loc_y)])
            print("k:", k)
            tmp_dict['cov_xy'] = np.cov(loc_x, loc_y)
            res_4_each[k] = tmp_dict
            x_list.extend(loc_x)
            y_list.extend(loc_y)
        
    # Calculate the mean, var and cov of x,y for all objects
    res_4_all['mean_x'] = np.mean(x_list)
    res_4_all['var_x'] = np.var(x_list)
    res_4_all['mean_y'] = np.mean(y_list)
    res_4_all['var_y'] = np.var(y_list)
    res_4_all['mean_xy'] = np.array([np.mean(x_list), np.mean(y_list)])
    res_4_all['cov_xy'] = np.cov(x_list, y_list)
    
    return res_4_each, res_4_all


def get_STP_in_single_camera(pt_trace):
    """
    Parameters:
        pt_trace:trace after perspective transformation in a single camera
    """
    dist_dict_1 = get_dist_in_deltaT(pt_trace)
    res_4_each, res_4_all = get_statistical_paras_of_dist_in_deltaT(dist_dict_1)
    return res_4_each, res_4_all


def get_STP_in_multi_cameras(obj_single_camera_stp_cam_1, obj_single_camera_stp_cam_2, associate_dict):
    """
    Parameters:
        obj_single_camera_stp_cam_1: STP in cam 1
        obj_single_camera_stp_cam_2: STP in cam 2
        associate_dict:mapping relation between objects in cam_1 and cam_2
    """
    # chosen_id_cam_1 = 2
    # chosen_id_cam_2 = associate_dict[chosen_id_cam_1]
    # pt_box_1,frame_cam_1 = zip(*pt_trace_1[str(chosen_id_cam_1)])
    # pt_box_2,frame_cam_2 = zip(*pt_trace_2[str(chosen_id_cam_2)])
   
    STP_S_4_each_1 = obj_single_camera_stp_cam_1.motion_params_4_each
    STP_S_4_all_1 = obj_single_camera_stp_cam_1.motion_params_4_all
    STP_S_4_each_2 = obj_single_camera_stp_cam_2.motion_params_4_each
    STP_S_4_all_2 = obj_single_camera_stp_cam_2.motion_params_4_all
    
    pt_trace_1 = obj_single_camera_stp_cam_1.perspective_trace
    pt_trace_2 = obj_single_camera_stp_cam_2.perspective_trace
    
    # # ===== Method 1: Predict location using motion parameters for all objects =====
    # objs_associate_info = []
    # for k in associate_dict:
        # for i in range(min(len(pt_trace_1[str(k)]),len(pt_trace_2[str(k)]))):
            # objs_associate_info.append([pt_trace_1[str(k)][i],pt_trace_2[str(k)][i]])


    # for i,v in enumerate(objs_associate_info):
        # x,y = v[0][0][0],v[0][0][1]
        # delta_t = v[1][1]-v[0][1]
        # pred_x = x + delta_t*STP_S_4_all_1['mean_x']
        # pred_y = y + delta_t*STP_S_4_all_1['mean_y']
        # v.append([pred_x,pred_y])
    # src_x,src_y = np.array([elem[2][0] for elem in objs_associate_info]),np.array([elem[2][1] for elem in objs_associate_info])
    # dst_x,dst_y = np.array([elem[1][0][0] for elem in objs_associate_info]),np.array([elem[1][0][1] for elem in objs_associate_info])
    
    # ===== Method 2 : Predict location using each object's parameter
    objs_associate_info = []
    for k in associate_dict:    # mapping dict
        if isinstance(k, (str)):
            if k in pt_trace_1 and k in pt_trace_2:
                for i in range(min(len(pt_trace_1[k]), len(pt_trace_2[k]))):    # 考虑路径长短不同，取两路径最短的，要不会有控制
                    objs_associate_info.append([k, pt_trace_1[k][i], pt_trace_2[k][i]])
        else:
            for i in range(min(len(pt_trace_1[str(k)]), len(pt_trace_2[str(k)]))):
                objs_associate_info.append([k, pt_trace_1[str(k)][i], pt_trace_2[str(k)][i]])
            
    for i, v in enumerate(objs_associate_info):
        x, y = v[1][0][0], v[1][0][1]
        delta_t = v[2][1]-v[1][1]
        try:
            pred_x = x + delta_t*STP_S_4_each_1[str(objs_associate_info[i][0])]['mean_x']
            pred_y = y + delta_t*STP_S_4_each_1[str(objs_associate_info[i][0])]['mean_y']
            v.append([pred_x, pred_y])
        except:
            pass
        
    cam_1_x, cam_1_y = np.array([elem[3][0] for elem in objs_associate_info]), np.array([elem[3][1] for elem in objs_associate_info])
    cam_2_x, cam_2_y = np.array([elem[2][0][0] for elem in objs_associate_info]), np.array([elem[2][0][1] for elem in objs_associate_info])
    
    # for i in range(len(src_y)):
        # print(cam_1_y[i],cam_2_y[i])
    # plt.scatter(src_y,dst_y)
    # plt.show()
    
    cam_1_x = np.reshape(cam_1_x, (-1, 1))
    cam_2_x = np.reshape(cam_2_x, (-1, 1))
    cam_1_y = np.reshape(cam_1_y, (-1, 1))
    cam_2_y = np.reshape(cam_2_y, (-1, 1))
    
    # pred_x = model_x.predict(src_x)
    # pred_y = model_y.predict(src_y)
    
    # plt.plot(src_y,dst_y,'b-',linewidth=3)
    # plt.scatter(pred_x,dst_x)
    # plt.show()
    
    # for i in range(len(pred_y)):
        # print(pred_y[i],dst_y[i])
    
    # err = pred_x-dst_x
    # e_i = np.linspace(1,len(src_x),len(src_x))
    # plt.plot(e_i,err,'r',linewidth=3)
    # plt.show()
    
    
    # # ===== FAIL: 误差较大,原因未查明,暂时放弃 =====
    # # Get affine transform matrix using least square algorithm
    # trans_matrix,_ = cv2.findHomography(src_pts,dst_pts,cv2.RANSAC,1.0)
    # input_vector = np.array(src_pts)
    # pt = cv2.transform(input_vector[None,:,:],trans_matrix)
    # M = np.mat(trans_matrix[:2])
    # for elem in src_pts:
        # im = np.mat((elem[0],elem[1],1)).T
        # rim = M*im
        # print(rim)

    return cam_1_x, cam_2_x, cam_1_y, cam_2_y


def get_box_center(box_list):
    center_box_list = []
    for elem in box_list:
        center_box_list.append([(elem[0]+elem[2])*1.0/2, elem[3]*1.0])
    return center_box_list


def get_pt_box_info(box_info, pt_obj):
    """Get perspective transformation of box bottom center"""
    if type(box_info) is dict:
        pt_box_info = {}
        for elem in box_info:
            box_list, frame_list = zip(*box_info[elem]['list'])
            center_box_list = get_box_center(box_list)
            pt_box_list = pt_obj.get_pred_transform(center_box_list)
            center_box_l = pt_obj.get_inverse_pred_transform(pt_box_list[0])
            pt_box_info[elem] = list(zip(pt_box_list[0], frame_list))
        return pt_box_info

    if type(box_info) is list:
        pt_box_info = []
        box_list, frame_list = zip(*box_info)
        center_box_list = get_box_center(box_list)
        pt_box_list = pt_obj.get_pred_transform(center_box_list)
        return list(zip(pt_box_list[0], frame_list))


def get_probability_map(obj_nx, obj_ny, base_x, base_y, start_x=0, start_y=0, length_x=10, length_y=10, height=100, width=100):

    x = np.linspace(start_x, start_x+length_x, width)
    y = np.linspace(start_y, start_y+length_y, height)

    delta_x = x - base_x
    delta_y = y - base_y
    
    p_x = obj_nx.get_probability(delta_x)
    p_y = obj_ny.get_probability(delta_y)
    # # ----- TEST:单变量分布显示 -----
    # plt.plot(base_x, p_x, 'b-', linewidth=3)
    # plt.plot(base_y, p_y, 'r-', linewidth=3)
    # plt.show()
    
    # 生成分布概率矩阵
    mat_p_x = np.mat(p_x)
    mat_p_y = np.mat(p_y).T
    mat_p = mat_p_y*mat_p_x
    
    # 返回分布概率显示图像
    return get_normalize_mat(mat_p)

def get_normalize_mat(data):
    '''Convert the probability map into color image'''
    mx = np.max(data)
    mn = np.min(data)
    mx_mat = np.ones_like(data)*mx
    mn_mat = np.ones_like(data)*mn
    if mx-mn > 0:
        n_data = np.array((data-mn_mat)*255/(mx-mn), dtype=np.uint8)
    else:
        n_data = np.zeros_like(data)
    
    # # # ----- TEST: n_data disp -----
    # print("max:",np.max(n_data))
    # print("min:",np.min(n_data))
    # print("shape:",n_data.shape) 
    # print("where:",len(np.where(n_data>1)[0]))
    # cv2.imshow('n_data',n_data)
    # cv2.waitKey()

    return n_data
    
def get_rational_value(x,low,high):
    '''Limit input in range'''
    y = np.max([np.min([x,high]),0])
    return y

# ========== TEST FUNCTIONS ===========
def gaussian_test():
    '''Useless for this task, just for testing the effect of gaussian distribution'''
    print("===== Get in the gaussian_test! ===== ")
    
    # 1D gauss testing
    obj_1D = GaussDistribution(0, 2)

    x = np.linspace(-10, 10, 100)
    y = obj_1D.get_probability(x)
    
    plt.plot(x, y, 'b-', linewidth=3)
    
    # 2D gauss testing
    X = np.linspace(-3, 3, 60)
    Y = np.linspace(-4, 4, 60)
    X, Y = np.meshgrid(X, Y)
    mu = np.array([0., 0.])
    Sigma = np.array([[1., -0.5], [-0.5, 1.5]])
    pos = np.empty(X.shape+(2,))
    pos[:, :, 0] = X
    pos[:, :, 1] = Y

    obj_2D = GaussDistribution(mu, Sigma)
    Z = obj_2D.get_probability(pos)

    fig =plt.figure()
    ax = fig.gca(projection='3d')
    ax.plot_surface(X, Y, Z, rstride=3, cstride=3, linewidth=1, antialiased=True)
    cset = ax.contour(X, Y, Z, zdir='z', offset=-0.15)

    ax.set_zlim(-0.15, 0.2)
    ax.set_zticks(np.linspace(0, 0.2, 5))
    ax.view_init(27, -21)
    plt.show()

def match_based_on_spatial_temperal_prior_test_1(tracker_record, pt_obj, t_interval=30):
    '''test predicting location of object,
    Predict object location in future based on the current object location, movement characteristics and time interval setting
    Parameters:
        tracker_record: tracking information
        pt_obj: perspective transformer object
        t_interval: time interval(frames)
    '''
    print("===== Get in the match_based_on_spatial_temperal_prior_test_1! ===== ")
    
    test_id = '0'
    # file path
    img_root = data_path[device_id]
    save_root = r"E:\Project\CV\trajectory\VehicleTracking\results\sct\001\test"

    # SingleCameraSTP
    obj_STP = SingleCameraSTP(tracker_record, pt_obj, t_interval, var_beta_x=1, var_beta_y=1)
    # obj_STP.update_predictor(tracker_record, 30, var_bata_x=2, var_bate_y=4)
       
    # test on id 1
    for v, elem in enumerate(obj_STP.perspective_trace[test_id]):
        if v+t_interval > len(obj_STP.perspective_trace[test_id])-1:
            break
        fname_1 = '{:0>4d}.jpg'.format(elem[1])
        fname_2 = '{:0>4d}.jpg'.format(elem[1]+t_interval)
        

        test_x, test_y = obj_STP.perspective_trace[test_id][v+t_interval][0][0], obj_STP.perspective_trace[test_id][v+t_interval][0][1]
        print("PrePos:", elem[0][0], ";", elem[0][1], ";", test_x, ";", test_y)
        print("Probability:", obj_STP.get_probability(test_x, test_y, elem[0][0], elem[0][1]))
        print("Distance:", obj_STP.get_distance(test_x, test_y, elem[0][0], elem[0][1]))
        p_map = obj_STP.get_probability_map(base_x=elem[0][0], base_y=elem[0][1], start_x=0, start_y=0, length_x=15, length_y=110, height=110, width=15)
        p_map = cv2.applyColorMap(p_map, cv2.COLORMAP_JET)

        color_p_map = cv2.resize(p_map, (int(pt_obj.transformed_width_for_disp), int(pt_obj.transformed_height_for_disp)))
        color_p_map = cv2.flip(color_p_map, 0)   # 0:vertical flip
        pt_color_p_map = pt_obj.get_inverse_disp_transform(color_p_map)
        
        
        img_1 = cv2.imread(os.path.join(img_root, fname_1))  # current frame
        img_2 = cv2.imread(os.path.join(img_root, fname_2))  # frame after fixed time interval
        diff_img = cv2.absdiff(img_1, img_2)                 # For displaying same object from different frames in one image.

        alpha = 0.5
        inverse_elem = pt_obj.get_inverse_pred_transform([elem[0]])
        img_3 = cv2.addWeighted(img_1, alpha, diff_img, 1-alpha, 0)
        alpha = 0.7
        img_3 = cv2.addWeighted(img_3, alpha, pt_color_p_map, 1-alpha, 0)

        cv2.namedWindow('img_3', cv2.WINDOW_NORMAL)
        cv2.imshow('img_3', img_3)
        # cv2.imwrite(os.path.join(save_root,fname_1),img_3)
        
        cv2.waitKey()
    return


def match_based_on_spatial_temperal_prior_test_2(tracker_record_1, tracker_record_2, pt_obj_1, pt_obj_2, associate_dict, t_interval=30):
    """test location predicting of object,
    Predict object location in future based on current object location, movement characteristics and time interval setting
    Parameters:
        tracker_record_1: Trace information in cam 1(front one);
        tracker_record_2: Trace information in cam 2(back one);
        pt_obj_1: perspective transformer object of cam 1;
        pt_obj_2: perspective transformer object of cam 2;
        t_interval: time interval(frames)
    """
    print("===== Get in the match_based_on_spatial_temperal_prior_test_2! ===== ")
    
    # file path
    device_id_1 = 0
    device_id_2 = 1
    img_root_1 = data_path[device_id_1]
    img_root_2 = data_path[device_id_2]
    # save_root =
 
    obj_single_camera_stp_cam_1 = SingleCameraSTP(tracker_record_1, pt_obj_1)
    obj_single_camera_stp_cam_2 = SingleCameraSTP(tracker_record_2, pt_obj_2)
    
    print(obj_single_camera_stp_cam_1.perspective_trace)
    print(obj_single_camera_stp_cam_1.motion_params_4_each)
    obj_multi_cameras_stp_c1c2 = MultiCamerasSTP(
                                    obj_single_camera_stp_cam_1,
                                    obj_single_camera_stp_cam_2,
                                    associate_dict)

    # # ===== TEST:coord_transformer_test =====
    # coord_transformer_test(obj_multi_cameras_stp_c1c2)
    # obj_multi_cameras_stp_c1c2.get_start_point_transform()
    
    pt_box_info_1 = obj_multi_cameras_stp_c1c2.obj_single_camera_stp_cam_1.perspective_trace
    pt_box_info_2 = obj_multi_cameras_stp_c1c2.obj_single_camera_stp_cam_2.perspective_trace
    
    # Test on object id '1'
    object_id = '0'
    
    for i in range(np.min([len(pt_box_info_1[object_id]), len(pt_box_info_2[object_id])])):
        f1 = i
        f2 = i
        fname_1 = str(pt_box_info_1[object_id][f1][1])+'.jpg'
        fname_2 = str(pt_box_info_2[object_id][f2][1])+'.jpg'
        
        img_1 = cv2.imread(os.path.join(img_root_1, fname_1))
        img_2 = cv2.imread(os.path.join(img_root_2, fname_2))
        
        cam_1_x = pt_box_info_1[object_id][f1][0][0]
        cam_1_y = pt_box_info_1[object_id][f1][0][1]
        
        cam_2_x = pt_box_info_2[object_id][f2][0][0]
        cam_2_y = pt_box_info_2[object_id][f2][0][1]
        
        t_interval = pt_box_info_2[object_id][f2][1]-pt_box_info_1[object_id][f1][1]
        
        print(cam_1_x, cam_1_y)
        print(cam_2_x, cam_2_y)
        print(t_interval)
        # print(obj_multi_cameras_stp_c1c2.starting_point)
        
        p_map = obj_multi_cameras_stp_c1c2.get_probability_map(cam_1_x, cam_1_y, t_interval, height=210, width=80)
        p_map = cv2.applyColorMap(p_map, cv2.COLORMAP_JET)
        p = obj_multi_cameras_stp_c1c2.get_probability(cam_2_x, cam_2_y, cam_1_x, cam_1_y, t_interval)
        print(p)
        # dist = obj_multi_cameras_stp_c1c2.get_distance(cam_2_x,cam_2_y,cam_1_x,cam_1_y,t_interval)
        p_map = cv2.resize(p_map, (int(pt_obj_2.transformed_width_for_disp), int(pt_obj_2.transformed_height_for_disp)))
        p_map = cv2.flip(p_map, 0)   # 0:vertical flip
        pt_color_p_map = pt_obj_2.get_inverse_disp_transform(p_map)
        
        alpha = 0.5
        img_3 = cv2.addWeighted(img_2, alpha, pt_color_p_map, 1-alpha, 0)
        
        img_4 = np.zeros((int(img_2.shape[0]), int(img_2.shape[1]*2), 3), np.uint8)
        img_4[:, :img_1.shape[1], :] = img_1
        img_4[:, img_1.shape[1]:, :] = img_3

        # cv2.namedWindow('img_1',cv2.WINDOW_NORMAL)
        # cv2.namedWindow('img_2',cv2.WINDOW_NORMAL)
        cv2.namedWindow('img_4', cv2.WINDOW_NORMAL)
        
        # cv2.imshow('img_1',img_1)
        # cv2.imshow('img_2',img_2)
        cv2.imshow('img_4', img_4)
        
        cv2.imwrite(os.path.join(save_root, fname_1), img_4)
        
        cv2.waitKey()
    return 


def trace_display_test(tracker_record, obj_id='0', img_path=None):
    """Display object trajectory on chosen image"""
    BBox, BFrame = zip(*tracker_record[obj_id]['list'])
    Center_BBox = get_box_center(BBox)
    print(Center_BBox)
    img = cv2.imread(img_path)
    for elem in Center_BBox:
        print(elem)
        cv2.circle(img, (int(elem[0]), int(elem[1])), 3, (0, 0, 255), 3)
    cv2.namedWindow('img_1', cv2.WINDOW_NORMAL)
    cv2.imshow('img_1', img)
    cv2.waitKey()
   
def coord_transformer_test(obj_multi_cameras_stp):
    transformers = obj_multi_cameras_stp.get_coord_transformer()
    print("===== Get in coord_transformer_test！=====")
    print(transformers['B2F']['y'].predict(obj_multi_cameras_stp.coord_pairs[3]))
   
def useless():
    # ===== TEST: 1D gassian display =====
    # x = np.linspace(-1,1,100)
    # y = obj.get_probability(x)

    # plt.plot(x,y,'b-',linewidth=3)
    # plt.show()

    # ===== TEST: 2D gaussian display =====
    # X = np.linspace(-1,1,60)
    # Y = np.linspace(-1,1,60)
    # X,Y = np.meshgrid(X,Y)

    # pos = np.empty(X.shape+(2,))
    # pos[:,:,0]= X
    # pos[:,:,1] = Y

    # Z = obj.get_probability(pos)
    # print(np.max(Z))

    # fig =plt.figure()
    # ax = fig.gca(projection='3d')
    # ax.plot_surface(X,Y,Z,rstride=3,cstride=3,linewidth=1,antialiased =True)
    # cset = ax.contour(X,Y,Z,zdir='z',offset=-0.15)

    # ax.set_zlim(-0.0001,57)
    # ax.set_zticks(np.linspace(0,0.2,5))
    # ax.view_init(27,-21)
    # plt.show()
    
    # ===== TEST: 2D gaussian display in 3D effect =====
    # with sns.axes_style("dark"):
        # sns.jointplot(display_x, display_y, kind="kde",space=0)
    # plt.show()
    
    # pt_tracker_record_1 = {}
    # pt_tracker_record_2 = {}
    pass
    
# ========== MAIN FUNCTIONS ===========


if __name__ == "__main__":
    # print('==== Cameras_associate ====')
    # # cam A
    # device_id = 0
    #
    # img_filepath_1 = data_path[device_id]
    # img_savepath_1 = save_path[device_id]
    # tk_json_filepath_1 = track_info[device_id]
    # pt_savepath_1 = roi_info[device_id]
    #
    # # cam B
    # associate_device_id = associate_cam[device_id]
    #
    # img_filepath_2 = data_path[associate_device_id]
    # img_savepath_2 = save_path[associate_device_id]
    # tk_json_filepath_2 = track_info[associate_device_id]
    # pt_savepath_2 = roi_info[associate_device_id]
    #
    # from data_generator import load_tracking_info
    # tracker_record_1 = load_tracking_info(tk_json_filepath_1)
    # tracker_record_2 = load_tracking_info(tk_json_filepath_2)
    #
    # import Perspective_transform as pt
    # pt_obj_1 = pt.Perspective_transformer(pt_savepath_1)
    # pt_obj_2 = pt.Perspective_transformer(pt_savepath_2)
    #
    # pt_box_info_1 = get_pt_box_info(tracker_record_1, pt_obj_1)
    # pt_box_info_2 = get_pt_box_info(tracker_record_2, pt_obj_2)
    #
    # # # ======= TEST: trace display ======
    # # trace_display_test(tracker_record_1, obj_id='0', img_path=os.path.join(data_path[0], "0001.jpg"))
    #
    # # ====== TEST: location predicting ======
    # # match_based_on_spatial_temperal_prior_test_1( tracker_record_1,
    # #                                                 pt_obj_1,
    # #                                                 5)
    #
    # associate_dict_c1_c2 = get_associate_dict(associate_info)[cam_names[1]]
    # match_based_on_spatial_temperal_prior_test_2(   tracker_record_1,
    #                                                 tracker_record_2,
    #                                                 pt_obj_1,
    #                                                 pt_obj_2,
    #                                                 associate_dict_c1_c2,
    #                                                 )
    #
    # # ====== TEST: Spatial-Temperal Prior (STP) in single camera ======
    # # d = get_STP_in_single_camera(pt_box_info_1)
    # # d = get_STP_in_multi_cameras(pt_box_info_1,pt_box_info_2,associate_dict_c1_c2)

    print("Save me!!!")
"""
Camera associate
written by <NAME> on 2019-03-21, version 1.0
Updated by <NAME> on Sep 29, 2020, version 1.1.0
"""

import os
import sys
import math
import pandas as pd
import numpy as np
import cv2
import json
import matplotlib.pyplot as plt
from sklearn.linear_model import LinearRegression
import seaborn as sns
from Common import SRC_IMAGES, DET_RESULT, ROI_RESULT, SCT_RESULT, MCT_RESULT


# ==== Default files path setting, you could comment the code line below and set your own path in new project ====
from Common import cam_names, data_path, box_info, roi_info, save_path, track_info, associate_info

# ========== CONFIGURATION ==========
# style set for figure
sns.set_palette('deep', desat=.6)
sns.set_context(rc={'figure.figsize': (8, 5)})

# ==== FILE PATH ====
# Default files path setting is in Common.py



# camera associate
associate_cam = {
    0: 1,
    1: 0,
    2: 3,
    3: 4,
    4: 3
}

associate_dict_c1_c2 = {
    0: 0,
    1: 1,
    2: 2,
    3: 3,
    4: 4
}
# camera associate
associate_dict_c3_c4 = {
    0: 0,
    1: 1,
    2: 2,
    3: 3,
    4: 4
}

def get_associate_dict(filepath):
    with open(filepath, 'r') as doc:
        data = json.load(doc)
        return data

# ========== BASIC CLASS ==========


class GaussDistribution(object):
    def __init__(self, mu, sigma):
        self.mu = mu
        self.sigma = sigma
        
    def get_probability(self, x):
        
        if type(self.mu) is np.float64:
            return self._1D_gaussian(x)
        if type(self.mu) is np.ndarray:
            return self._2D_gaussian(x)
        return None
        
    def _2D_gaussian(self, x):
        '''Get the probability of x with 2D gaussian'''
        n = self.mu.shape[0]
        
        sigma_det = np.linalg.det(self.sigma)
        sigma_inv = np.linalg.inv(self.sigma)
        
        # print("sigma_det,sigma_inv:",sigma_det,sigma_inv)
        
        den = np.sqrt((2*np.pi)**n*sigma_det)
        num = np.einsum('...k,kl,...l->...', x-self.mu, sigma_inv, x-self.mu)

        return np.exp(-num/2)/den
        
    def _1D_gaussian(self, x):
        '''Get the probability of x with 2D gaussian'''
        den = np.sqrt(2*np.pi*np.power(self.sigma, 2))
        num = np.power(x-self.mu, 2)/np.power(self.sigma, 2)
        # print("den,num:",den,num)
        return np.exp(-num/2)/den

    def get_normal_1D_gaussian(self, x):
        '''Get the probability of x with 2D gaussian'''
        x_norm = (x-self.mu)/self.sigma
        den = np.sqrt(2*np.pi*np.power(1, 2))
        num = np.power(x_norm, 2)/np.power(1, 2)
        # print("den,num:",den,num)
        return np.exp(-num/2)/den


class SingleCameraSTP(object):
    def __init__(   self,
                    tracking_record=None,
                    perspective_transformer=None,
                    time_interval=25,
                    var_beta_x=1.0,
                    var_beta_y=1.0):
        self.perspective_transformer = perspective_transformer
        self.tracking_record = tracking_record
        self.time_interval = time_interval

        self.perspective_trace = None
        self.distance_dict = None
        self.motion_params_4_all = None
        self.motion_params_4_each = None
        
        self.prob_alpha_x = 1       # x amplitude multiplier
        self.prob_alpha_y = 1       # y amplitude multiplier
        
        self.var_beta_x = var_beta_x         # x range multiplier
        self.var_beta_y = var_beta_y            # y range multiplier
        
        self.get_motion_params()
        self.nx_predictor = GaussDistribution(
                                    self.time_interval*self.motion_params_4_all['mean_x'],
                                    self.time_interval*self.var_beta_x*self.motion_params_4_all['var_x'])
        self.ny_predictor = GaussDistribution(
                                    self.time_interval*self.motion_params_4_all['mean_y'],
                                    self.time_interval*self.var_beta_y*self.motion_params_4_all['var_y'])

    @staticmethod
    def version(self):
        return print("Written by sunzhu, 2019-03-21, version 1.0")
    
    def get_motion_params(self):
        self.perspective_trace = get_pt_box_info(self.tracking_record, self.perspective_transformer)
        self.distance_dict = get_dist_in_deltaT(self.perspective_trace)
        self.motion_params_4_each, self.motion_params_4_all = get_statistical_paras_of_dist_in_deltaT(self.distance_dict)
        return self.motion_params_4_each, self.motion_params_4_all
        
    def update_predictor(self, tracking_record=None, time_interval=None, var_bata_x=1, var_bate_y=1):
        if tracking_record is not None:
            self.tracking_record = tracking_record
            self.get_motion_params()
        if time_interval is not None:
            self.time_interval = time_interval
        self.nx_predictor = GaussDistribution(
                                    self.time_interval*self.motion_params_4_all['mean_x'],
                                    self.time_interval*self.var_beta_x*self.motion_params_4_all['var_x'])
        self.ny_predictor = GaussDistribution(
                                    self.time_interval*self.motion_params_4_all['mean_y'],
                                    self.time_interval*self.var_beta_y*self.motion_params_4_all['var_y'])
            
    def get_probability_map(self, base_x, base_y, start_x=0, start_y=0, length_x=10, length_y=10, height=100, width=100):
        return get_probability_map(self.nx_predictor, self.ny_predictor, base_x, base_y, start_x, start_y, length_x, length_y, height, width)
        
    def get_probability(self, x, y, base_x, base_y):
        """Calculate the probability of pos
            x, y: pos to be calculated;
            base_x, base_y: predicted pos
        """
        delta_x = x - base_x 
        delta_y = y - base_y
        prob_x = self.nx_predictor.get_probability(delta_x)
        prob_y = self.ny_predictor.get_probability(delta_y)
        prob_xy = self.prob_alpha_x*prob_x*self.prob_alpha_y*prob_y
        return prob_x, prob_y, prob_xy
        
    def get_distance(self, x, y, base_x, base_y, alpha=0.5):
        pred_x = base_x+self.time_interval*self.motion_params_4_all['mean_x']
        pred_y = base_y+self.time_interval*self.motion_params_4_all['mean_y']
        comp_distance = abs(x-pred_x)*(1-alpha)+abs(y-pred_y)*alpha
        return comp_distance


class MultiCamerasSTP(object):
    def __init__(   self,
                    obj_single_camera_stp_cam_1=None,
                    obj_single_camera_stp_cam_2=None,
                    associate_dict=None):
        self.obj_single_camera_stp_cam_1 = obj_single_camera_stp_cam_1
        self.obj_single_camera_stp_cam_2 = obj_single_camera_stp_cam_2
        self.associate_dict = associate_dict
        
        self.coord_pairs = None
        self.coord_transformer = None
        self.starting_point = None
        
        self.prob_alpha_x = 1       # x multiplier for position probability calculating
        self.prob_alpha_y = 1       # y multiplier for position probability calculating
        
        self.var_beta_x = 5         # x range multiplier
        self.var_beta_y = 1         # y range multiplier
        
        self.get_coord_transformer()
        self.get_start_point_transform()
        
    def version(self):
        return print("Written by sunzhu, 2019-03-21, version 1.0")
    
    def get_coord_transformer(self):
        self.coord_pairs = get_STP_in_multi_cameras(
                                    self.obj_single_camera_stp_cam_1,
                                    self.obj_single_camera_stp_cam_2,
                                    self.associate_dict
                                    )
                                    
        cam_1_x, cam_2_x, cam_1_y, cam_2_y = self.coord_pairs
        
        self.coord_transformer = {}
        
        # Front to back transformer
        F2B_transformer = {}
        
        model_F2B_x = LinearRegression()
        model_F2B_x.fit(cam_1_x, cam_2_x)
        model_F2B_y = LinearRegression()
        model_F2B_y.fit(cam_1_y, cam_2_y)
        
        F2B_transformer['x'] = model_F2B_x
        F2B_transformer['y'] = model_F2B_y
        
        # Back to front transformer
        B2F_transformer = {}
        
        model_B2F_x = LinearRegression()
        model_B2F_x.fit(cam_2_x, cam_1_x)
        model_B2F_y = LinearRegression()
        model_B2F_y.fit(cam_2_y, cam_1_y)
        
        B2F_transformer['x'] = model_B2F_x
        B2F_transformer['y'] = model_B2F_y
        
        # Coordinate transformer
        self.coord_transformer['F2B'] = F2B_transformer
        self.coord_transformer['B2F'] = B2F_transformer
        
        return self.coord_transformer

    def get_start_point_transform(self, start_x_in_cam_2=0, start_y_in_cam_2=0):
        """Transform the coordinate value of starting point in cam 2 into the coordinate value in cam 1
            The coordinate value refers the position of object after perspective transforming.
        """
        corresponding_start_x_in_cam_1 = self.coord_transformer['B2F']['x'].predict([[start_x_in_cam_2]])[0][0]
        corresponding_start_y_in_cam_1 = self.coord_transformer['B2F']['y'].predict([[start_y_in_cam_2]])[0][0]
        # print(corresponding_start_x_in_cam_1,corresponding_start_y_in_cam_1)
        self.starting_point = [corresponding_start_x_in_cam_1, corresponding_start_y_in_cam_1]
        return self.starting_point
    
    def get_probability_map(self, base_x, base_y, t_interval=None, height=300, width=80):
        nx_predictor = GaussDistribution(  t_interval*self.obj_single_camera_stp_cam_1.motion_params_4_all['mean_x'],
                                            t_interval*self.var_beta_x*self.obj_single_camera_stp_cam_1.motion_params_4_all['var_x'])
        ny_predictor = GaussDistribution(  t_interval*self.obj_single_camera_stp_cam_1.motion_params_4_all['mean_y'],
                                            t_interval*self.var_beta_y*self.obj_single_camera_stp_cam_1.motion_params_4_all['var_y'])
        # print("starting_points:",self.starting_point[0],self.starting_point[1])
        
        return get_probability_map( nx_predictor,
                                    ny_predictor,
                                    base_x,
                                    base_y,
                                    start_x=self.starting_point[0],
                                    start_y=self.starting_point[1],
                                    )
                                    
    def get_probability(self, x, y, base_x, base_y, t_interval=None):
        trans_x = self.coord_transformer['B2F']['x'].predict([[x]])[0][0]
        trans_y = self.coord_transformer['B2F']['y'].predict([[y]])[0][0]
        delta_x = trans_x - base_x
        delta_y = trans_y - base_y

        # print("delta_x:",delta_x,"delta_y:",delta_y)
        nx_predictor = GaussDistribution(
                        t_interval*self.obj_single_camera_stp_cam_1.motion_params_4_all['mean_x'],
                        t_interval*self.var_beta_x*self.obj_single_camera_stp_cam_1.motion_params_4_all['var_x'])
        ny_predictor = GaussDistribution(
                        t_interval*self.obj_single_camera_stp_cam_1.motion_params_4_all['mean_y'],
                        t_interval*self.var_beta_y*self.obj_single_camera_stp_cam_1.motion_params_4_all['var_y'])
        # print('p_x',nx_predictor.get_probability(delta_x))      
        # print('p_y',ny_predictor.get_probability(delta_y))
        prob_x = nx_predictor.get_probability(delta_x)
        prob_y = ny_predictor.get_probability(delta_y)
        prob_xy = self.prob_alpha_x*prob_x*self.prob_alpha_y*prob_y
        return prob_x, prob_y, prob_xy
        
    def get_distance(self, x, y, base_x, base_y, t_interval=None, alpha=0.5):
        trans_x = self.coord_transformer['B2F']['x'].predict([[x]])[0][0]
        trans_y = self.coord_transformer['B2F']['y'].predict([[y]])[0][0]
        pred_x = base_x+t_interval*self.obj_single_camera_stp_cam_1.motion_params_4_all['mean_x']
        pred_y = base_y+t_interval*self.obj_single_camera_stp_cam_1.motion_params_4_all['mean_y']
        comp_distance = abs(x-pred_x)*(1-alpha)+abs(y-pred_y)*alpha
        return comp_distance
        
        
# =========== UTILS FUNCTIONS ============
def get_dist_in_deltaT(pt_trace):
    """Get moving distance of object within delta t"""
    delta_loc = {}
    for k in pt_trace:
        obj_trace = pt_trace[k]
        obj_delta_loc = []
        for i in range(len(obj_trace)-1):
            x1, y1, t1 = obj_trace[i][0][0], obj_trace[i][0][1], obj_trace[i][1]
            x2, y2, t2 = obj_trace[i+1][0][0], obj_trace[i+1][0][1], obj_trace[i+1][1]
            delta_x = x2 - x1
            delta_y = y2 - y1
            delta_t = t2 - t1
            if delta_t > 0:
                delta_x = delta_x/delta_t
                delta_y = delta_y/delta_t
                obj_delta_loc.append([delta_x, delta_y])
        delta_loc[k] = obj_delta_loc
    return delta_loc


def get_statistical_paras_of_dist_in_deltaT(delta_loc):
    res_4_each = {}  # parameters of each object
    res_4_all = {}  # parameters of all objects
    x_list = []
    y_list = []
    # Calculate the mean,var and cov of x,y for each object
    for k in delta_loc:
        tmp_dict = {}
        if len(delta_loc[k]) > 1:
            loc_x, loc_y = zip(*delta_loc[k])
            print(loc_x)
            tmp_dict['mean_x'] = np.mean(loc_x)
            tmp_dict['var_x'] = np.var(loc_x)
            tmp_dict['mean_y'] = np.mean(loc_y)
            tmp_dict['var_y'] = np.var(loc_y)
            tmp_dict['mean_xy'] = np.array([np.mean(loc_x), np.mean(loc_y)])
            print("k:", k)
            tmp_dict['cov_xy'] = np.cov(loc_x, loc_y)
            res_4_each[k] = tmp_dict
            x_list.extend(loc_x)
            y_list.extend(loc_y)
        
    # Calculate the mean, var and cov of x,y for all objects
    res_4_all['mean_x'] = np.mean(x_list)
    res_4_all['var_x'] = np.var(x_list)
    res_4_all['mean_y'] = np.mean(y_list)
    res_4_all['var_y'] = np.var(y_list)
    res_4_all['mean_xy'] = np.array([np.mean(x_list), np.mean(y_list)])
    res_4_all['cov_xy'] = np.cov(x_list, y_list)
    
    return res_4_each, res_4_all


def get_STP_in_single_camera(pt_trace):
    """
    Parameters:
        pt_trace:trace after perspective transformation in a single camera
    """
    dist_dict_1 = get_dist_in_deltaT(pt_trace)
    res_4_each, res_4_all = get_statistical_paras_of_dist_in_deltaT(dist_dict_1)
    return res_4_each, res_4_all


def get_STP_in_multi_cameras(obj_single_camera_stp_cam_1, obj_single_camera_stp_cam_2, associate_dict):
    """
    Parameters:
        obj_single_camera_stp_cam_1: STP in cam 1
        obj_single_camera_stp_cam_2: STP in cam 2
        associate_dict:mapping relation between objects in cam_1 and cam_2
    """
    # chosen_id_cam_1 = 2
    # chosen_id_cam_2 = associate_dict[chosen_id_cam_1]
    # pt_box_1,frame_cam_1 = zip(*pt_trace_1[str(chosen_id_cam_1)])
    # pt_box_2,frame_cam_2 = zip(*pt_trace_2[str(chosen_id_cam_2)])
   
    STP_S_4_each_1 = obj_single_camera_stp_cam_1.motion_params_4_each
    STP_S_4_all_1 = obj_single_camera_stp_cam_1.motion_params_4_all
    STP_S_4_each_2 = obj_single_camera_stp_cam_2.motion_params_4_each
    STP_S_4_all_2 = obj_single_camera_stp_cam_2.motion_params_4_all
    
    pt_trace_1 = obj_single_camera_stp_cam_1.perspective_trace
    pt_trace_2 = obj_single_camera_stp_cam_2.perspective_trace
    
    # # ===== Method 1: Predict location using motion parameters for all objects =====
    # objs_associate_info = []
    # for k in associate_dict:
        # for i in range(min(len(pt_trace_1[str(k)]),len(pt_trace_2[str(k)]))):
            # objs_associate_info.append([pt_trace_1[str(k)][i],pt_trace_2[str(k)][i]])


    # for i,v in enumerate(objs_associate_info):
        # x,y = v[0][0][0],v[0][0][1]
        # delta_t = v[1][1]-v[0][1]
        # pred_x = x + delta_t*STP_S_4_all_1['mean_x']
        # pred_y = y + delta_t*STP_S_4_all_1['mean_y']
        # v.append([pred_x,pred_y])
    # src_x,src_y = np.array([elem[2][0] for elem in objs_associate_info]),np.array([elem[2][1] for elem in objs_associate_info])
    # dst_x,dst_y = np.array([elem[1][0][0] for elem in objs_associate_info]),np.array([elem[1][0][1] for elem in objs_associate_info])
    
    # ===== Method 2 : Predict location using each object's parameter
    objs_associate_info = []
    for k in associate_dict:    # mapping dict
        if isinstance(k, (str)):
            if k in pt_trace_1 and k in pt_trace_2:
                for i in range(min(len(pt_trace_1[k]), len(pt_trace_2[k]))):    # 考虑路径长短不同，取两路径最短的，要不会有控制
                    objs_associate_info.append([k, pt_trace_1[k][i], pt_trace_2[k][i]])
        else:
            for i in range(min(len(pt_trace_1[str(k)]), len(pt_trace_2[str(k)]))):
                objs_associate_info.append([k, pt_trace_1[str(k)][i], pt_trace_2[str(k)][i]])
            
    for i, v in enumerate(objs_associate_info):
        x, y = v[1][0][0], v[1][0][1]
        delta_t = v[2][1]-v[1][1]
        try:
            pred_x = x + delta_t*STP_S_4_each_1[str(objs_associate_info[i][0])]['mean_x']
            pred_y = y + delta_t*STP_S_4_each_1[str(objs_associate_info[i][0])]['mean_y']
            v.append([pred_x, pred_y])
        except:
            pass
        
    cam_1_x, cam_1_y = np.array([elem[3][0] for elem in objs_associate_info]), np.array([elem[3][1] for elem in objs_associate_info])
    cam_2_x, cam_2_y = np.array([elem[2][0][0] for elem in objs_associate_info]), np.array([elem[2][0][1] for elem in objs_associate_info])
    
    # for i in range(len(src_y)):
        # print(cam_1_y[i],cam_2_y[i])
    # plt.scatter(src_y,dst_y)
    # plt.show()
    
    cam_1_x = np.reshape(cam_1_x, (-1, 1))
    cam_2_x = np.reshape(cam_2_x, (-1, 1))
    cam_1_y = np.reshape(cam_1_y, (-1, 1))
    cam_2_y = np.reshape(cam_2_y, (-1, 1))
    
    # pred_x = model_x.predict(src_x)
    # pred_y = model_y.predict(src_y)
    
    # plt.plot(src_y,dst_y,'b-',linewidth=3)
    # plt.scatter(pred_x,dst_x)
    # plt.show()
    
    # for i in range(len(pred_y)):
        # print(pred_y[i],dst_y[i])
    
    # err = pred_x-dst_x
    # e_i = np.linspace(1,len(src_x),len(src_x))
    # plt.plot(e_i,err,'r',linewidth=3)
    # plt.show()
    
    
    # # ===== FAIL: 误差较大,原因未查明,暂时放弃 =====
    # # Get affine transform matrix using least square algorithm
    # trans_matrix,_ = cv2.findHomography(src_pts,dst_pts,cv2.RANSAC,1.0)
    # input_vector = np.array(src_pts)
    # pt = cv2.transform(input_vector[None,:,:],trans_matrix)
    # M = np.mat(trans_matrix[:2])
    # for elem in src_pts:
        # im = np.mat((elem[0],elem[1],1)).T
        # rim = M*im
        # print(rim)

    return cam_1_x, cam_2_x, cam_1_y, cam_2_y


def get_box_center(box_list):
    center_box_list = []
    for elem in box_list:
        center_box_list.append([(elem[0]+elem[2])*1.0/2, elem[3]*1.0])
    return center_box_list


def get_pt_box_info(box_info, pt_obj):
    """Get perspective transformation of box bottom center"""
    if type(box_info) is dict:
        pt_box_info = {}
        for elem in box_info:
            box_list, frame_list = zip(*box_info[elem]['list'])
            center_box_list = get_box_center(box_list)
            pt_box_list = pt_obj.get_pred_transform(center_box_list)
            center_box_l = pt_obj.get_inverse_pred_transform(pt_box_list[0])
            pt_box_info[elem] = list(zip(pt_box_list[0], frame_list))
        return pt_box_info

    if type(box_info) is list:
        pt_box_info = []
        box_list, frame_list = zip(*box_info)
        center_box_list = get_box_center(box_list)
        pt_box_list = pt_obj.get_pred_transform(center_box_list)
        return list(zip(pt_box_list[0], frame_list))


def get_probability_map(obj_nx, obj_ny, base_x, base_y, start_x=0, start_y=0, length_x=10, length_y=10, height=100, width=100):

    x = np.linspace(start_x, start_x+length_x, width)
    y = np.linspace(start_y, start_y+length_y, height)

    delta_x = x - base_x
    delta_y = y - base_y
    
    p_x = obj_nx.get_probability(delta_x)
    p_y = obj_ny.get_probability(delta_y)
    # # ----- TEST:单变量分布显示 -----
    # plt.plot(base_x, p_x, 'b-', linewidth=3)
    # plt.plot(base_y, p_y, 'r-', linewidth=3)
    # plt.show()
    
    # 生成分布概率矩阵
    mat_p_x = np.mat(p_x)
    mat_p_y = np.mat(p_y).T
    mat_p = mat_p_y*mat_p_x
    
    # 返回分布概率显示图像
    return get_normalize_mat(mat_p)

def get_normalize_mat(data):
    '''Convert the probability map into color image'''
    mx = np.max(data)
    mn = np.min(data)
    mx_mat = np.ones_like(data)*mx
    mn_mat = np.ones_like(data)*mn
    if mx-mn > 0:
        n_data = np.array((data-mn_mat)*255/(mx-mn), dtype=np.uint8)
    else:
        n_data = np.zeros_like(data)
    
    # # # ----- TEST: n_data disp -----
    # print("max:",np.max(n_data))
    # print("min:",np.min(n_data))
    # print("shape:",n_data.shape) 
    # print("where:",len(np.where(n_data>1)[0]))
    # cv2.imshow('n_data',n_data)
    # cv2.waitKey()

    return n_data
    
def get_rational_value(x,low,high):
    '''Limit input in range'''
    y = np.max([np.min([x,high]),0])
    return y

# ========== TEST FUNCTIONS ===========
def gaussian_test():
    '''Useless for this task, just for testing the effect of gaussian distribution'''
    print("===== Get in the gaussian_test! ===== ")
    
    # 1D gauss testing
    obj_1D = GaussDistribution(0, 2)

    x = np.linspace(-10, 10, 100)
    y = obj_1D.get_probability(x)
    
    plt.plot(x, y, 'b-', linewidth=3)
    
    # 2D gauss testing
    X = np.linspace(-3, 3, 60)
    Y = np.linspace(-4, 4, 60)
    X, Y = np.meshgrid(X, Y)
    mu = np.array([0., 0.])
    Sigma = np.array([[1., -0.5], [-0.5, 1.5]])
    pos = np.empty(X.shape+(2,))
    pos[:, :, 0] = X
    pos[:, :, 1] = Y

    obj_2D = GaussDistribution(mu, Sigma)
    Z = obj_2D.get_probability(pos)

    fig =plt.figure()
    ax = fig.gca(projection='3d')
    ax.plot_surface(X, Y, Z, rstride=3, cstride=3, linewidth=1, antialiased=True)
    cset = ax.contour(X, Y, Z, zdir='z', offset=-0.15)

    ax.set_zlim(-0.15, 0.2)
    ax.set_zticks(np.linspace(0, 0.2, 5))
    ax.view_init(27, -21)
    plt.show()

def match_based_on_spatial_temperal_prior_test_1(tracker_record, pt_obj, t_interval=30):
    '''test predicting location of object,
    Predict object location in future based on the current object location, movement characteristics and time interval setting
    Parameters:
        tracker_record: tracking information
        pt_obj: perspective transformer object
        t_interval: time interval(frames)
    '''
    print("===== Get in the match_based_on_spatial_temperal_prior_test_1! ===== ")
    
    test_id = '0'
    # file path
    img_root = data_path[device_id]
    save_root = r"E:\Project\CV\trajectory\VehicleTracking\results\sct\001\test"

    # SingleCameraSTP
    obj_STP = SingleCameraSTP(tracker_record, pt_obj, t_interval, var_beta_x=1, var_beta_y=1)
    # obj_STP.update_predictor(tracker_record, 30, var_bata_x=2, var_bate_y=4)
       
    # test on id 1
    for v, elem in enumerate(obj_STP.perspective_trace[test_id]):
        if v+t_interval > len(obj_STP.perspective_trace[test_id])-1:
            break
        fname_1 = '{:0>4d}.jpg'.format(elem[1])
        fname_2 = '{:0>4d}.jpg'.format(elem[1]+t_interval)
        

        test_x, test_y = obj_STP.perspective_trace[test_id][v+t_interval][0][0], obj_STP.perspective_trace[test_id][v+t_interval][0][1]
        print("PrePos:", elem[0][0], ";", elem[0][1], ";", test_x, ";", test_y)
        print("Probability:", obj_STP.get_probability(test_x, test_y, elem[0][0], elem[0][1]))
        print("Distance:", obj_STP.get_distance(test_x, test_y, elem[0][0], elem[0][1]))
        p_map = obj_STP.get_probability_map(base_x=elem[0][0], base_y=elem[0][1], start_x=0, start_y=0, length_x=15, length_y=110, height=110, width=15)
        p_map = cv2.applyColorMap(p_map, cv2.COLORMAP_JET)

        color_p_map = cv2.resize(p_map, (int(pt_obj.transformed_width_for_disp), int(pt_obj.transformed_height_for_disp)))
        color_p_map = cv2.flip(color_p_map, 0)   # 0:vertical flip
        pt_color_p_map = pt_obj.get_inverse_disp_transform(color_p_map)
        
        
        img_1 = cv2.imread(os.path.join(img_root, fname_1))  # current frame
        img_2 = cv2.imread(os.path.join(img_root, fname_2))  # frame after fixed time interval
        diff_img = cv2.absdiff(img_1, img_2)                 # For displaying same object from different frames in one image.

        alpha = 0.5
        inverse_elem = pt_obj.get_inverse_pred_transform([elem[0]])
        img_3 = cv2.addWeighted(img_1, alpha, diff_img, 1-alpha, 0)
        alpha = 0.7
        img_3 = cv2.addWeighted(img_3, alpha, pt_color_p_map, 1-alpha, 0)

        cv2.namedWindow('img_3', cv2.WINDOW_NORMAL)
        cv2.imshow('img_3', img_3)
        # cv2.imwrite(os.path.join(save_root,fname_1),img_3)
        
        cv2.waitKey()
    return


def match_based_on_spatial_temperal_prior_test_2(tracker_record_1, tracker_record_2, pt_obj_1, pt_obj_2, associate_dict, t_interval=30):
    """test location predicting of object,
    Predict object location in future based on current object location, movement characteristics and time interval setting
    Parameters:
        tracker_record_1: Trace information in cam 1(front one);
        tracker_record_2: Trace information in cam 2(back one);
        pt_obj_1: perspective transformer object of cam 1;
        pt_obj_2: perspective transformer object of cam 2;
        t_interval: time interval(frames)
    """
    print("===== Get in the match_based_on_spatial_temperal_prior_test_2! ===== ")
    
    # file path
    device_id_1 = 0
    device_id_2 = 1
    img_root_1 = data_path[device_id_1]
    img_root_2 = data_path[device_id_2]
    # save_root =
 
    obj_single_camera_stp_cam_1 = SingleCameraSTP(tracker_record_1, pt_obj_1)
    obj_single_camera_stp_cam_2 = SingleCameraSTP(tracker_record_2, pt_obj_2)
    
    print(obj_single_camera_stp_cam_1.perspective_trace)
    print(obj_single_camera_stp_cam_1.motion_params_4_each)
    obj_multi_cameras_stp_c1c2 = MultiCamerasSTP(
                                    obj_single_camera_stp_cam_1,
                                    obj_single_camera_stp_cam_2,
                                    associate_dict)

    # # ===== TEST:coord_transformer_test =====
    # coord_transformer_test(obj_multi_cameras_stp_c1c2)
    # obj_multi_cameras_stp_c1c2.get_start_point_transform()
    
    pt_box_info_1 = obj_multi_cameras_stp_c1c2.obj_single_camera_stp_cam_1.perspective_trace
    pt_box_info_2 = obj_multi_cameras_stp_c1c2.obj_single_camera_stp_cam_2.perspective_trace
    
    # Test on object id '1'
    object_id = '0'
    
    for i in range(np.min([len(pt_box_info_1[object_id]), len(pt_box_info_2[object_id])])):
        f1 = i
        f2 = i
        fname_1 = str(pt_box_info_1[object_id][f1][1])+'.jpg'
        fname_2 = str(pt_box_info_2[object_id][f2][1])+'.jpg'
        
        img_1 = cv2.imread(os.path.join(img_root_1, fname_1))
        img_2 = cv2.imread(os.path.join(img_root_2, fname_2))
        
        cam_1_x = pt_box_info_1[object_id][f1][0][0]
        cam_1_y = pt_box_info_1[object_id][f1][0][1]
        
        cam_2_x = pt_box_info_2[object_id][f2][0][0]
        cam_2_y = pt_box_info_2[object_id][f2][0][1]
        
        t_interval = pt_box_info_2[object_id][f2][1]-pt_box_info_1[object_id][f1][1]
        
        print(cam_1_x, cam_1_y)
        print(cam_2_x, cam_2_y)
        print(t_interval)
        # print(obj_multi_cameras_stp_c1c2.starting_point)
        
        p_map = obj_multi_cameras_stp_c1c2.get_probability_map(cam_1_x, cam_1_y, t_interval, height=210, width=80)
        p_map = cv2.applyColorMap(p_map, cv2.COLORMAP_JET)
        p = obj_multi_cameras_stp_c1c2.get_probability(cam_2_x, cam_2_y, cam_1_x, cam_1_y, t_interval)
        print(p)
        # dist = obj_multi_cameras_stp_c1c2.get_distance(cam_2_x,cam_2_y,cam_1_x,cam_1_y,t_interval)
        p_map = cv2.resize(p_map, (int(pt_obj_2.transformed_width_for_disp), int(pt_obj_2.transformed_height_for_disp)))
        p_map = cv2.flip(p_map, 0)   # 0:vertical flip
        pt_color_p_map = pt_obj_2.get_inverse_disp_transform(p_map)
        
        alpha = 0.5
        img_3 = cv2.addWeighted(img_2, alpha, pt_color_p_map, 1-alpha, 0)
        
        img_4 = np.zeros((int(img_2.shape[0]), int(img_2.shape[1]*2), 3), np.uint8)
        img_4[:, :img_1.shape[1], :] = img_1
        img_4[:, img_1.shape[1]:, :] = img_3

        # cv2.namedWindow('img_1',cv2.WINDOW_NORMAL)
        # cv2.namedWindow('img_2',cv2.WINDOW_NORMAL)
        cv2.namedWindow('img_4', cv2.WINDOW_NORMAL)
        
        # cv2.imshow('img_1',img_1)
        # cv2.imshow('img_2',img_2)
        cv2.imshow('img_4', img_4)
        
        cv2.imwrite(os.path.join(save_root, fname_1), img_4)
        
        cv2.waitKey()
    return 


def trace_display_test(tracker_record, obj_id='0', img_path=None):
    """Display object trajectory on chosen image"""
    BBox, BFrame = zip(*tracker_record[obj_id]['list'])
    Center_BBox = get_box_center(BBox)
    print(Center_BBox)
    img = cv2.imread(img_path)
    for elem in Center_BBox:
        print(elem)
        cv2.circle(img, (int(elem[0]), int(elem[1])), 3, (0, 0, 255), 3)
    cv2.namedWindow('img_1', cv2.WINDOW_NORMAL)
    cv2.imshow('img_1', img)
    cv2.waitKey()
   
def coord_transformer_test(obj_multi_cameras_stp):
    transformers = obj_multi_cameras_stp.get_coord_transformer()
    print("===== Get in coord_transformer_test！=====")
    print(transformers['B2F']['y'].predict(obj_multi_cameras_stp.coord_pairs[3]))
   
def useless():
    # ===== TEST: 1D gassian display =====
    # x = np.linspace(-1,1,100)
    # y = obj.get_probability(x)

    # plt.plot(x,y,'b-',linewidth=3)
    # plt.show()

    # ===== TEST: 2D gaussian display =====
    # X = np.linspace(-1,1,60)
    # Y = np.linspace(-1,1,60)
    # X,Y = np.meshgrid(X,Y)

    # pos = np.empty(X.shape+(2,))
    # pos[:,:,0]= X
    # pos[:,:,1] = Y

    # Z = obj.get_probability(pos)
    # print(np.max(Z))

    # fig =plt.figure()
    # ax = fig.gca(projection='3d')
    # ax.plot_surface(X,Y,Z,rstride=3,cstride=3,linewidth=1,antialiased =True)
    # cset = ax.contour(X,Y,Z,zdir='z',offset=-0.15)

    # ax.set_zlim(-0.0001,57)
    # ax.set_zticks(np.linspace(0,0.2,5))
    # ax.view_init(27,-21)
    # plt.show()
    
    # ===== TEST: 2D gaussian display in 3D effect =====
    # with sns.axes_style("dark"):
        # sns.jointplot(display_x, display_y, kind="kde",space=0)
    # plt.show()
    
    # pt_tracker_record_1 = {}
    # pt_tracker_record_2 = {}
    pass
    
# ========== MAIN FUNCTIONS ===========


if __name__ == "__main__":
    # print('==== Cameras_associate ====')
    # # cam A
    # device_id = 0
    #
    # img_filepath_1 = data_path[device_id]
    # img_savepath_1 = save_path[device_id]
    # tk_json_filepath_1 = track_info[device_id]
    # pt_savepath_1 = roi_info[device_id]
    #
    # # cam B
    # associate_device_id = associate_cam[device_id]
    #
    # img_filepath_2 = data_path[associate_device_id]
    # img_savepath_2 = save_path[associate_device_id]
    # tk_json_filepath_2 = track_info[associate_device_id]
    # pt_savepath_2 = roi_info[associate_device_id]
    #
    # from data_generator import load_tracking_info
    # tracker_record_1 = load_tracking_info(tk_json_filepath_1)
    # tracker_record_2 = load_tracking_info(tk_json_filepath_2)
    #
    # import Perspective_transform as pt
    # pt_obj_1 = pt.Perspective_transformer(pt_savepath_1)
    # pt_obj_2 = pt.Perspective_transformer(pt_savepath_2)
    #
    # pt_box_info_1 = get_pt_box_info(tracker_record_1, pt_obj_1)
    # pt_box_info_2 = get_pt_box_info(tracker_record_2, pt_obj_2)
    #
    # # # ======= TEST: trace display ======
    # # trace_display_test(tracker_record_1, obj_id='0', img_path=os.path.join(data_path[0], "0001.jpg"))
    #
    # # ====== TEST: location predicting ======
    # # match_based_on_spatial_temperal_prior_test_1( tracker_record_1,
    # #                                                 pt_obj_1,
    # #                                                 5)
    #
    # associate_dict_c1_c2 = get_associate_dict(associate_info)[cam_names[1]]
    # match_based_on_spatial_temperal_prior_test_2(   tracker_record_1,
    #                                                 tracker_record_2,
    #                                                 pt_obj_1,
    #                                                 pt_obj_2,
    #                                                 associate_dict_c1_c2,
    #                                                 )
    #
    # # ====== TEST: Spatial-Temperal Prior (STP) in single camera ======
    # # d = get_STP_in_single_camera(pt_box_info_1)
    # # d = get_STP_in_multi_cameras(pt_box_info_1,pt_box_info_2,associate_dict_c1_c2)

    print("Save me!!!")
Camera associate written by <NAME> on 2019-03-21, version 1.0 Updated by <NAME> on Sep 29, 2020, version 1.1.0 # ==== Default files path setting, you could comment the code line below and set your own path in new project ==== # ========== CONFIGURATION ========== # style set for figure # ==== FILE PATH ==== # Default files path setting is in Common.py # camera associate # camera associate # ========== BASIC CLASS ========== Get the probability of x with 2D gaussian # print("sigma_det,sigma_inv:",sigma_det,sigma_inv) Get the probability of x with 2D gaussian # print("den,num:",den,num) Get the probability of x with 2D gaussian # print("den,num:",den,num) # x amplitude multiplier # y amplitude multiplier # x range multiplier # y range multiplier Calculate the probability of pos             x, y: pos to be calculated;             base_x, base_y: predicted pos # x multiplier for position probability calculating # y multiplier for position probability calculating # x range multiplier # y range multiplier # Front to back transformer # Back to front transformer # Coordinate transformer Transform the coordinate value of starting point in cam 2 into the coordinate value in cam 1             The coordinate value refers the position of object after perspective transforming. # print(corresponding_start_x_in_cam_1,corresponding_start_y_in_cam_1) # print("starting_points:",self.starting_point[0],self.starting_point[1]) # print("delta_x:",delta_x,"delta_y:",delta_y) # print('p_x',nx_predictor.get_probability(delta_x)) # print('p_y',ny_predictor.get_probability(delta_y)) # =========== UTILS FUNCTIONS ============ Get moving distance of object within delta t # parameters of each object # parameters of all objects # Calculate the mean,var and cov of x,y for each object # Calculate the mean, var and cov of x,y for all objects Parameters:         pt_trace:trace after perspective transformation in a single camera Parameters:         obj_single_camera_stp_cam_1: STP in cam 1         obj_single_camera_stp_cam_2: STP in cam 2         associate_dict:mapping relation between objects in cam_1 and cam_2 # chosen_id_cam_1 = 2 # chosen_id_cam_2 = associate_dict[chosen_id_cam_1] # pt_box_1,frame_cam_1 = zip(*pt_trace_1[str(chosen_id_cam_1)]) # pt_box_2,frame_cam_2 = zip(*pt_trace_2[str(chosen_id_cam_2)]) # # ===== Method 1: Predict location using motion parameters for all objects ===== # objs_associate_info = [] # for k in associate_dict: # for i in range(min(len(pt_trace_1[str(k)]),len(pt_trace_2[str(k)]))): # objs_associate_info.append([pt_trace_1[str(k)][i],pt_trace_2[str(k)][i]]) # for i,v in enumerate(objs_associate_info): # x,y = v[0][0][0],v[0][0][1] # delta_t = v[1][1]-v[0][1] # pred_x = x + delta_t*STP_S_4_all_1['mean_x'] # pred_y = y + delta_t*STP_S_4_all_1['mean_y'] # v.append([pred_x,pred_y]) # src_x,src_y = np.array([elem[2][0] for elem in objs_associate_info]),np.array([elem[2][1] for elem in objs_associate_info]) # dst_x,dst_y = np.array([elem[1][0][0] for elem in objs_associate_info]),np.array([elem[1][0][1] for elem in objs_associate_info]) # ===== Method 2 : Predict location using each object's parameter # mapping dict # 考虑路径长短不同，取两路径最短的，要不会有控制 # for i in range(len(src_y)): # print(cam_1_y[i],cam_2_y[i]) # plt.scatter(src_y,dst_y) # plt.show() # pred_x = model_x.predict(src_x) # pred_y = model_y.predict(src_y) # plt.plot(src_y,dst_y,'b-',linewidth=3) # plt.scatter(pred_x,dst_x) # plt.show() # for i in range(len(pred_y)): # print(pred_y[i],dst_y[i]) # err = pred_x-dst_x # e_i = np.linspace(1,len(src_x),len(src_x)) # plt.plot(e_i,err,'r',linewidth=3) # plt.show() # # ===== FAIL: 误差较大,原因未查明,暂时放弃 ===== # # Get affine transform matrix using least square algorithm # trans_matrix,_ = cv2.findHomography(src_pts,dst_pts,cv2.RANSAC,1.0) # input_vector = np.array(src_pts) # pt = cv2.transform(input_vector[None,:,:],trans_matrix) # M = np.mat(trans_matrix[:2]) # for elem in src_pts: # im = np.mat((elem[0],elem[1],1)).T # rim = M*im # print(rim) Get perspective transformation of box bottom center # # ----- TEST:单变量分布显示 ----- # plt.plot(base_x, p_x, 'b-', linewidth=3) # plt.plot(base_y, p_y, 'r-', linewidth=3) # plt.show() # 生成分布概率矩阵 # 返回分布概率显示图像 Convert the probability map into color image # # # ----- TEST: n_data disp ----- # print("max:",np.max(n_data)) # print("min:",np.min(n_data)) # print("shape:",n_data.shape) # print("where:",len(np.where(n_data>1)[0])) # cv2.imshow('n_data',n_data) # cv2.waitKey() Limit input in range # ========== TEST FUNCTIONS =========== Useless for this task, just for testing the effect of gaussian distribution # 1D gauss testing # 2D gauss testing test predicting location of object,     Predict object location in future based on the current object location, movement characteristics and time interval setting     Parameters:         tracker_record: tracking information         pt_obj: perspective transformer object         t_interval: time interval(frames) # file path # SingleCameraSTP # obj_STP.update_predictor(tracker_record, 30, var_bata_x=2, var_bate_y=4) # test on id 1 # 0:vertical flip # current frame # frame after fixed time interval # For displaying same object from different frames in one image. # cv2.imwrite(os.path.join(save_root,fname_1),img_3) test location predicting of object,     Predict object location in future based on current object location, movement characteristics and time interval setting     Parameters:         tracker_record_1: Trace information in cam 1(front one);         tracker_record_2: Trace information in cam 2(back one);         pt_obj_1: perspective transformer object of cam 1;         pt_obj_2: perspective transformer object of cam 2;         t_interval: time interval(frames) # file path # save_root = # # ===== TEST:coord_transformer_test ===== # coord_transformer_test(obj_multi_cameras_stp_c1c2) # obj_multi_cameras_stp_c1c2.get_start_point_transform() # Test on object id '1' # print(obj_multi_cameras_stp_c1c2.starting_point) # dist = obj_multi_cameras_stp_c1c2.get_distance(cam_2_x,cam_2_y,cam_1_x,cam_1_y,t_interval) # 0:vertical flip # cv2.namedWindow('img_1',cv2.WINDOW_NORMAL) # cv2.namedWindow('img_2',cv2.WINDOW_NORMAL) # cv2.imshow('img_1',img_1) # cv2.imshow('img_2',img_2) Display object trajectory on chosen image # ===== TEST: 1D gassian display ===== # x = np.linspace(-1,1,100) # y = obj.get_probability(x) # plt.plot(x,y,'b-',linewidth=3) # plt.show() # ===== TEST: 2D gaussian display ===== # X = np.linspace(-1,1,60) # Y = np.linspace(-1,1,60) # X,Y = np.meshgrid(X,Y) # pos = np.empty(X.shape+(2,)) # pos[:,:,0]= X # pos[:,:,1] = Y # Z = obj.get_probability(pos) # print(np.max(Z)) # fig =plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X,Y,Z,rstride=3,cstride=3,linewidth=1,antialiased =True) # cset = ax.contour(X,Y,Z,zdir='z',offset=-0.15) # ax.set_zlim(-0.0001,57) # ax.set_zticks(np.linspace(0,0.2,5)) # ax.view_init(27,-21) # plt.show() # ===== TEST: 2D gaussian display in 3D effect ===== # with sns.axes_style("dark"): # sns.jointplot(display_x, display_y, kind="kde",space=0) # plt.show() # pt_tracker_record_1 = {} # pt_tracker_record_2 = {} # ========== MAIN FUNCTIONS =========== # print('==== Cameras_associate ====') # # cam A # device_id = 0 # # img_filepath_1 = data_path[device_id] # img_savepath_1 = save_path[device_id] # tk_json_filepath_1 = track_info[device_id] # pt_savepath_1 = roi_info[device_id] # # # cam B # associate_device_id = associate_cam[device_id] # # img_filepath_2 = data_path[associate_device_id] # img_savepath_2 = save_path[associate_device_id] # tk_json_filepath_2 = track_info[associate_device_id] # pt_savepath_2 = roi_info[associate_device_id] # # from data_generator import load_tracking_info # tracker_record_1 = load_tracking_info(tk_json_filepath_1) # tracker_record_2 = load_tracking_info(tk_json_filepath_2) # # import Perspective_transform as pt # pt_obj_1 = pt.Perspective_transformer(pt_savepath_1) # pt_obj_2 = pt.Perspective_transformer(pt_savepath_2) # # pt_box_info_1 = get_pt_box_info(tracker_record_1, pt_obj_1) # pt_box_info_2 = get_pt_box_info(tracker_record_2, pt_obj_2) # # # # ======= TEST: trace display ====== # # trace_display_test(tracker_record_1, obj_id='0', img_path=os.path.join(data_path[0], "0001.jpg")) # # # ====== TEST: location predicting ====== # # match_based_on_spatial_temperal_prior_test_1( tracker_record_1, # #                                                 pt_obj_1, # #                                                 5) # # associate_dict_c1_c2 = get_associate_dict(associate_info)[cam_names[1]] # match_based_on_spatial_temperal_prior_test_2(   tracker_record_1, #                                                 tracker_record_2, #                                                 pt_obj_1, #                                                 pt_obj_2, #                                                 associate_dict_c1_c2, #                                                 ) # # # ====== TEST: Spatial-Temperal Prior (STP) in single camera ====== # # d = get_STP_in_single_camera(pt_box_info_1) # # d = get_STP_in_multi_cameras(pt_box_info_1,pt_box_info_2,associate_dict_c1_c2)