MiVIP/UAVRoomLocalization.py at main · fredericklee602/MiVIP · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
# Part 2 UAV找April Tag，並求相機姿態
import cv2
import rospy
import numpy as np
import voxl_control
import math
import apriltag
# Rotation 轉換旋轉矩陣、歐拉角、四元數
from scipy.spatial.transform import Rotation as R
# inv輸出反矩陣
from numpy.linalg import inv
from read_config import config_


class RoomLocalization:
    def __init__(self):
        # config檔
        self.CONFIG = config_()

        self.drone = voxl_control.Drone()

        # apriltag分析tag碼tag36h11
        self.at_detector = apriltag.Detector(apriltag.DetectorOptions(families=self.CONFIG.apriltag_family))

        #  Apriltag世界座標
        objectPoints = np.array([[-1, 1, 0],
                                [1, 1, 0],
                                [1, -1, 0],
                                [-1, -1, 0]],
                                dtype=np.float32)
        self.objectPoints = objectPoints*float(self.CONFIG.apriltag_pic_length)


        self.FRAME_COUNT = 0

        self.apriltagdict = {}
    def Global_Coordinate_deck(self,obj_points,UAV_para,UAV_img_points):

        # UAV給定UAV座標及姿態
        UAV_pos = UAV_para[:3]
        UAV_orient = UAV_para[3:]
        Rmatrix_UAV = R.from_quat(UAV_orient)

        # UAV拍攝棋盤格影像點
        _, rvec, tvec = cv2.solvePnP(obj_points, UAV_img_points, self.CONFIG.UAV_Camera_intrinsic["mtx"], self.CONFIG.UAV_Camera_intrinsic["dist"])

        # UAV 相機坐標系 R T
        rvec_UAV = rvec
        tvec_UAV = tvec
        rvec_matrix_UAV = cv2.Rodrigues(rvec_UAV)[0]

        # UAV1 相機坐標系轉NED，並旋轉回UAV0相同基底坐標系
        m = np.dot(rvec_matrix_UAV,np.transpose(obj_points)) + np.transpose(tvec_UAV.repeat(4)).reshape(3,4)
        NED_chess_point = np.dot(Rmatrix_UAV.as_matrix(),np.dot(np.array([[0,0,1],[1,0,0],[0,1,0]]),m))
        x = UAV_pos[0]
        y = UAV_pos[1]
        z = UAV_pos[2]
        bias_UAV = np.array([[x*100],[y*100],[z*100]])
        #print("bias_UAV:",bias_UAV)
        # bias_UAV 機體中心座標移到左眼相機座標
        bias_UAV = bias_UAV + np.dot(Rmatrix_UAV.as_matrix(),np.array(self.CONFIG.broad_left_eye_coordinate))
        # ned_Chess為棋盤格所有點初始UAV坐標系座標點
        ned_Chess = bias_UAV.repeat(4).reshape(3,4) + NED_chess_point
        ned_Chess = np.mean(ned_Chess,axis=1)

        return ned_Chess

    def Global_Coordinate_seeker(self,obj_points,UAV_para,UAV_img_points):

        # UAV給定UAV座標及姿態
        UAV_pos = UAV_para[:3]
        UAV_orient = UAV_para[3:]
        Rmatrix_UAV = R.from_quat(UAV_orient)

        # UAV拍攝棋盤格影像點
        _, rvec, tvec = cv2.solvePnP(obj_points, UAV_img_points, self.CONFIG.UAV_Camera_intrinsic["mtx"], self.CONFIG.UAV_Camera_intrinsic["dist"])

        # UAV 相機坐標系 R T
        rvec_UAV = rvec
        tvec_UAV = tvec
        rvec_matrix_UAV = cv2.Rodrigues(rvec_UAV)[0]

        # UAV1 相機坐標系轉NED，並旋轉回UAV0相同基底坐標系
        m = np.dot(rvec_matrix_UAV,np.transpose(obj_points)) + np.transpose(tvec_UAV.repeat(4)).reshape(3,4)
        NED_chess_point = np.dot(np.array([[0,0,1],[1,0,0],[0,1,0]]),np.dot(Rmatrix_UAV.as_matrix(),m))
        x = UAV_pos[0]
        y = UAV_pos[1]
        z = UAV_pos[2]
        bias_UAV = np.array([[x*100],[y*100],[z*100]])
        #print("bias_UAV:",bias_UAV)
        # bias_UAV 機體中心座標移到左眼相機座標
        bias_UAV = bias_UAV + np.dot(Rmatrix_UAV.as_matrix(),np.dot(np.array([[0,1,0],[0,0,1],[1,0,0]]), np.array(self.CONFIG.seeker_left_eye_coordinate)))
        NED_bias_UAV = np.dot(np.array([[0,0,1],[1,0,0],[0,1,0]]),bias_UAV)
        # ned_Chess為棋盤格所有點初始UAV坐標系座標點
        ned_Chess = NED_bias_UAV.repeat(4).reshape(3,4) + NED_chess_point
        ned_Chess = np.mean(ned_Chess,axis=1)
        print("ned_Chess:",[ned_Chess[0][0],ned_Chess[1][0],ned_Chess[2][0]])
        return ned_Chess

    def image_callback(self,UAV_type):
        global FRAME_COUNT
        img = self.drone.l_img
        img = cv2.resize(img, self.CONFIG.img_size_640, interpolation=cv2.INTER_CUBIC)
        tags = self.at_detector.detect(img)
        cv2.imshow("drone",img)
        if len(tags)>0:
            UAV_img_points = tags[0].corners
            tag_ID = tags[0].tag_id
            k = cv2.waitKey(1)
            if self.FRAME_COUNT%30==0:
                print("有偵測到AprilTag")
                if UAV_type=="seeker":
                    result_dict = self.Global_Coordinate_seeker(self.objectPoints,self.drone.state.pose_array,UAV_img_points)
                    self.apriltagdict.update({tag_ID:result_dict})
                if UAV_type=="deck":
                    result_dict = self.Global_Coordinate_deck(self.objectPoints,self.drone.state.pose_array,UAV_img_points)
                    self.apriltagdict.update({tag_ID:result_dict})
                cv2.waitKey(100)
        else:
            if self.FRAME_COUNT%30==0:
                print("UAV無偵測到AprilTag")
    def start(self):
        rospy.init_node('my_node')
        rospy.sleep(0.1)
        self.FRAME_COUNT = 0
        UAV_type = self.CONFIG.UAV_type
        while True:
            self.FRAME_COUNT += 1
            self.image_callback(UAV_type)
            if cv2.waitKey(1) & 0xFF == 27: # 按ESC键退出
                cv2.destroyAllWindows()
                break
    def _print_(self):
        print(self.apriltagdict)
if __name__ == '__main__':
    RL = RoomLocalization()
    RL.start()
    RL._print_()