professorfabioandrade
commented
6 months ago Hi,
First thing you should do is to check if the gimbal in the DJI drones already give the attitude (roll, pitch and yaw) with respect to the world frame. Therefore you do not need the rotation from gimbal to UAS neither the UAS attitude.
You can investigate that here: https://www.phantomhelp.com/LogViewer/Upload/
Regards, Fabio
On Thu, May 23, 2024, 9:46 AM Haroon Rasheed @.***> wrote:
I am performing the same procedure on both the DJI Mavic 2 Pro and the Mavic Mini, but I am unable to achieve consistent positional accuracy of the object. I have calculated the K matrix with the help of Agisoft Lens.
import cv2, math import numpy as np import utm class GeoReference: def init(self, image_path, gimbal_roll, gimbal_pitch, gimbal_yaw, uas_roll, uas_pitch, uas_yaw, latitude, longitude, altitude):
Input the image path for geo-reference
self.image_path = image_path # values of gimbal's roll pitch and yaw with w.r.t uas self.gimbal_roll = math.radians(gimbal_roll) self.gimbal_pitch = math.radians(gimbal_pitch) self.gimbal_yaw = math.radians(gimbal_yaw) # values of the uas with w.r.t ned frame self.uas_roll = math.radians(uas_roll) self.uas_pitch = math.radians(uas_pitch) self.uas_yaw = math.radians(uas_yaw) # latitude longitude and altitude values of the uas self.latitude = latitude self.longitude = longitude self.UTM_z = altitude # Intrinsic parameters camera matrix, Calculated by the resizing or the Mavic 2 Pro Images self.K = np.array([ [1664.59, 0, 935.755], [0, 1326.18, 487.819], [0, 0, 1] ]) # Rotation and Translation Matrix from Camera frame to Gimbal frame self.R_C_to_G = np.array([[0, 0, 1], [1, 0, 0], [0, 1, 0]]) self.T_C_to_G = np.array([[0], [0], [0]]) # Rotation and Translation Matrix from Gimbal frame to UAS frame self.R_G_to_UAS = math.nan self.T_G_to_UAS = np.array([[-0.002], [0.023], [0.002]]) # Rotation and Translation Matrix from UAS frame to NED frame self.R_UAS_to_NED = math.nan self.T_UAS_to_NED = np.array([[0], [0], [0]]) # Rotation and Translation Matrix from NED frame to ENU frame self.R_NED_to_ENU = np.array([[0, 1, 0], [1, 0, 0], [0, 0, -1]]) self.T_NED_to_ENU = math.nan def open_image_and_capture_coordinates(self): # Read the image image = cv2.imread(self.image_path) # Check if the image was successfully loaded if image is not None: # Display the image cv2.imshow('Image', image) # Set the callback function for mouse events cv2.setMouseCallback('Image', self.on_mouse_click) # Wait for a key event cv2.waitKey(0) cv2.destroyAllWindows() else: print(f"Error: Unable to load the image at '{self.image_path}'. Make sure the file path is correct.") def on_mouse_click(self, event, x, y, flags, param): if event == cv2.EVENT_LBUTTONDOWN: # Get the pixel values i-e x and y print(f"Clicked at pixel coordinates: ({x}, {y})") P_C_prime = self.image_to_camera_frame(self.K, x, y) # Camera frame to gimbal frame P_G_prime = self.camera_to_gimbal_frame(P_C_prime, self.R_C_to_G, self.T_C_to_G) # Gimbal frame to UAS frame self.R_G_to_UAS = self.rotation_matrix_gimbal_to_uas(self.gimbal_yaw, self.gimbal_pitch, self.gimbal_roll) P_UAS_prime = self.gimbal_to_uas_frame(P_G_prime, self.R_G_to_UAS, self.T_G_to_UAS) # UAS frame to NED frame self.R_UAS_to_NED = self.rotation_matrix_uas_to_ned(self.uas_yaw, self.uas_pitch, self.uas_roll) P_NED_prime = self.uas_to_ned_frame(P_UAS_prime, self.R_UAS_to_NED, self.T_UAS_to_NED) # NED frame to ENU frame UTM_x, UTM_y, letter, zone = self.latlon_to_utm(self.latitude, self.longitude) self.T_NED_to_ENU = np.array([[UTM_x], [UTM_y], [self.UTM_z]]) P_ENU_prime = self.ned_to_enu_frame(P_NED_prime, self.R_NED_to_ENU, self.T_NED_to_ENU) z_ENU = 0 T = self.T_NED_to_ENU + self.R_NED_to_ENU @ self.T_UAS_to_NED + self.R_NED_to_ENU @ self.R_UAS_to_NED @ self.T_G_to_UAS + self.R_NED_to_ENU @ self.R_UAS_to_NED @ self.R_G_to_UAS @ self.T_C_to_G z_T = T[2] z_ENU_prime = P_ENU_prime[2] z_C = self.calculate_z_C(z_ENU, z_T, z_ENU_prime) # Calculating P_ENU P_ENU = self.calculate_P_ENU(z_C, P_ENU_prime, T) P_ENU = list(map(str, P_ENU)) # Use list comprehension to apply the conversion function to each element float_list = [self.convert_to_float(s) for s in P_ENU] target_latitude, target_longitude = self.utm_to_latlon(float_list[0], float_list[1], letter, zone) print(target_latitude, target_longitude) def convert_to_float(self, s): # Strip the square brackets and convert to float return float(s.strip('[]')) def image_to_camera_frame(self, K, u, v): i = np.array([[u], [v], [1]]) P_C_prime = np.linalg.inv(K) @ i return P_C_prime def camera_to_gimbal_frame(self, P_C_prime, R_C_to_G, T_C_to_G): P_G_prime = R_C_to_G @ P_C_prime + T_C_to_G return P_G_prime def gimbal_to_uas_frame(self, P_G_prime, R_G_to_UAS, T_G_to_UAS): P_UAS_prime = R_G_to_UAS @ P_G_prime + T_G_to_UAS return P_UAS_prime def uas_to_ned_frame(self, P_UAS_prime, R_UAS_to_NED, T_UAS_to_NED): P_NED_prime = R_UAS_to_NED @ P_UAS_prime + T_UAS_to_NED return P_NED_prime def ned_to_enu_frame(self, P_NED_prime, R_NED_to_ENU, T_NED_to_ENU): P_ENU_prime = R_NED_to_ENU @ P_NED_prime + T_NED_to_ENU return P_ENU_prime def calculate_z_C(self, z_ENU, z_T, z_ENU_prime): return (z_ENU - z_T) / (z_ENU_prime - z_T) def calculate_P_ENU(self, z_C, P_ENU_prime, T): P_ENU = z_C * P_ENU_prime - z_C * T + T return P_ENU.round(4) def rotation_matrix_gimbal_to_uas(self, yaw, pitch, roll): R_G_to_UAS = np.array([ [math.cos(yaw) * math.cos(pitch), -math.sin(yaw) * math.cos(roll) + math.cos(yaw) * math.sin(pitch) * math.sin(roll), math.sin(yaw) * math.sin(roll) + math.cos(yaw) * math.cos(roll) * math.sin(pitch)], [math.sin(yaw) * math.cos(pitch), math.cos(yaw) * math.cos(roll) + math.sin(roll) * math.sin(pitch) * math.sin(yaw), -math.cos(yaw) * math.sin(roll) + math.sin(pitch) * math.sin(yaw) * math.cos(roll)], [-math.sin(pitch), math.cos(pitch) * math.sin(roll), math.cos(pitch) * math.cos(roll)] ]) return R_G_to_UAS def rotation_matrix_uas_to_ned(self, yaw, pitch, roll): R_UAS_to_NED = np.array([ [math.cos(yaw) * math.cos(pitch), -math.sin(yaw) * math.cos(roll) + math.cos(yaw) * math.sin(pitch) * math.sin(roll), math.sin(yaw) * math.sin(roll) + math.cos(yaw) * math.cos(roll) * math.sin(pitch)], [math.sin(yaw) * math.cos(pitch), math.cos(yaw) * math.cos(roll) + math.sin(roll) * math.sin(pitch) * math.sin(yaw), -math.cos(yaw) * math.sin(roll) + math.sin(pitch) * math.sin(yaw) * math.cos(roll)], [-math.sin(pitch), math.cos(pitch) * math.sin(roll), math.cos(pitch) * math.cos(roll)] ]) return R_UAS_to_NED def latlon_to_utm(self, latitude, longitude): # Convert latitude and longitude to UTM coordinates utm_coords = utm.from_latlon(latitude, longitude) return utm_coords def utm_to_latlon(self, utm_easting, utm_northing, utm_zone, utm_letter): # Convert UTM coordinates to latitude and longitude # zone number is 43, zone letter is 'S' lat, lon = utm.to_latlon(easting=utm_easting, northing=utm_northing, zone_number=utm_zone, zone_letter=utm_letter ) return lat, londef main(): geo = GeoReference(image_path='images/robo-flow/DJI_0753.MP4.jpg', longitude=-117.1697468,latitude=33.03167344,altitude=78.7000025, gimbal_yaw= 318.8, gimbal_roll=0, gimbal_pitch=-90, uas_pitch=-7.9, uas_roll= -4.9, uas_yaw= 334.5) geo.open_image_and_capture_coordinates()
if name == 'main': main()
I am experiencing a 30 to 40 meter error with some angle when attempting to determine the UTM coordinates of an object by clicking on the image to obtain the (u, v) values. The process involves converting these coordinates into UTM and then into latitude and longitude. This error persists across both DJI Mavic 2 Pro and Mavic Mini drones.
Am I doing something wrong ?
— Reply to this email directly, view it on GitHub https://github.com/professorfabioandrade/georef/issues/4, or unsubscribe https://github.com/notifications/unsubscribe-auth/ACEWBLAZB6KVAAHNG7LNPRTZDWNEHAVCNFSM6AAAAABIFCQB4CVHI2DSMVQWIX3LMV43ASLTON2WKOZSGMYTEMJZHEZDQNA . You are receiving this because you are subscribed to this thread.Message ID: @.***>
peakyquest
I am performing the same procedure on both the DJI Mavic 2 Pro and the Mavic Mini, but I am unable to achieve consistent positional accuracy of the object. I have calculated the K matrix with the help of Agisoft Lens.
I am experiencing a 30 to 40 meter error with some angle when attempting to determine the UTM coordinates of an object by clicking on the image to obtain the (u, v) values. The process involves converting these coordinates into UTM and then into latitude and longitude. This error persists across both DJI Mavic 2 Pro and Mavic Mini drones.
Am I doing something wrong ?