<reponame>l756302098/ros_practice<gh_stars>0
#!/usr/bin/env python
#coding=utf-8
import cv2  
import numpy as np
import matplotlib.pyplot as plt
import csv

# 2702
root = "/home/li/bag/0827/03/"
image_root = root + "image/"
base_root = "/home/li/bag/0827/base/"

def load_imu():
    dlist = np.loadtxt(open(root+"/imu2.txt","rb"),delimiter=" ",skiprows=0)
    print(dlist)
    print(dlist.shape)
    return dlist

def load_platform():
    dlist = np.loadtxt(open(root+"/platform2.txt","rb"),delimiter=" ",skiprows=0)
    #print(dlist)
    #print(dlist.shape)
    return dlist

def load_image():
    data = np.loadtxt(open(image_root+"/image.txt","rb"),dtype = np.str,delimiter=" ",skiprows=0)
    time = data[:,0].astype(np.float)
    label = data[:,1]
    return time,label

def show_images(angle,timestamps,images):
    print("show_images")
    x = 1080
    y = 1920
    name1 = images[0] 
    img1 = cv2.imread(name1)
    img11 = cv2.resize(img1, (int(y / 4), int(x / 4)))
    text1 = "base"
    addText1 =  img11.copy()
    cv2.putText(addText1,text1,(20,50),cv2.FONT_HERSHEY_COMPLEX,1.0,(0,0,255),2)

    name2 = images[1]
    img2 = cv2.imread(name2)
    img22 = cv2.resize(img2, (int(y / 4), int(x / 4)))
    text2 = "imu"
    addText2 =  img22.copy()
    cv2.putText(addText2,text2,(20,50),cv2.FONT_HERSHEY_COMPLEX,1.0,(0,0,255),2)

    name3 = images[2]
    img3 = cv2.imread(name3)
    img33 = cv2.resize(img3, (int(y / 4), int(x / 4)))
    text3 = "hk"
    addText3 =  img33.copy()
    cv2.putText(addText3,text3,(20,50),cv2.FONT_HERSHEY_COMPLEX,1.0,(0,0,255),2)

    imags = np.vstack([addText1,addText2,addText3])
    cv2.imwrite("/home/li/"+str(angle)+".png",imags,[int(cv2.IMWRITE_PNG_COMPRESSION),9])
    cv2.imshow("multi picture",imags)
    cv2.waitKey(5000)

def load_images():
    print("load_image_data")
    image_x_data,image_y_data = load_image()
    # for image in image_y_data:
    #     print(image)
    # image_x_data = image_data[:,0]
    image_x_data = np.divide(image_x_data,100.0)
    image_x_data = np.floor(image_x_data)
    # image_y_data = image_data[:,1]
    print("load_imu_data")
    imu_data = load_imu()
    imu_x_data = imu_data[:,0]
    imu_x_data = np.divide(imu_x_data,100)
    imu_x_data = np.floor(imu_x_data)
    imu_y_data = imu_data[:,3]
    imu_y_data = np.around(imu_y_data, decimals =  1)
    print("load_platform_data")
    pt_data = load_platform()
    pt_x_data = pt_data[:,0]
    pt_x_data = np.divide(pt_x_data,100)
    pt_x_data = np.floor(pt_x_data)
    pt_y_data = pt_data[:,1]
    pt_y_data = np.around(pt_y_data, decimals =  1)
    #test
    # imu_index_array = np.where(imu_y_data == 30.0)
    # if len(imu_index_array) > 0:
    #     imu_index = imu_index_array[0]
    #     timestamp = imu_x_data[imu_index]
    #     angle = imu_y_data[imu_index]
    #     print(imu_index,timestamp,angle)
    for i in range(270,360):
        # load base
        # find timestamp
        base_name = base_root + str(i) + ".jpg"
        imu_query_angle = 360 - i * 1.0
        #imu_query_angle = i * 1.0
        pt_query_angle = i * 1.0
        print(i," imu_query_angle ",imu_query_angle," pt_query_angle ",pt_query_angle)
        pt_image = ""
        pt_timestamp = 0
        imu_image = ""
        imu_timestamp = 0
        pt_index_array = np.where(pt_y_data == pt_query_angle)
        if len(pt_index_array) > 0:
            pt_index = pt_index_array[0]
            print("pt_index:",pt_index)
            if pt_index.size > 0:
                pt_timestamp = pt_x_data[pt_index[0]]
                print("pt_timestamp:",pt_timestamp)
                #print(imu_index,timestamp,angle)
                #query image
                image_index_array = np.where(image_x_data == pt_timestamp)
                if len(image_index_array) > 0:
                    image_index = image_index_array[0]
                    print("image_index:",image_index)
                    if image_index.size > 0:
                        pt_image = image_y_data[image_index[0]]
        imu_index_array = np.where(imu_y_data == imu_query_angle)
        if len(imu_index_array) > 0:
            imu_index = imu_index_array[0]
            if imu_index.size > 0:
                imu_timestamp = imu_x_data[imu_index[0]]
                # angle = imu_y_data[imu_index]
                # print(imu_index,timestamp,angle)
                #query image
                image_index_array = np.where(image_x_data == imu_timestamp)
                if len(image_index_array) > 0:
                    image_index = image_index_array[0]
                    if image_index.size > 0:
                        imu_image = image_y_data[image_index[0]]
                        print(base_name,imu_image)
                        # imu_img = cv2.imread(image_value)
                        # cv2.imshow("imu picture",imu_img)
                        # cv2.waitKey(2000)
        print(base_name,imu_image,pt_image)
        if len(pt_image) != 0 and len(imu_image) != 0:
            show_images(i,[0,imu_timestamp,pt_timestamp],[base_name,imu_image,pt_image])
if __name__ == '__main__':
    load_images()