######################################################################
# O o o o-o o--o o-o o o o--o o-o o--o #
# / \ |\ | | \ | | o o |\ /| | | \ | #
# o---o| \ | | O O-Oo | | | O | O-o | O O-o #
# | || \| | / | \ o o | | | | / | #
# o oo o o-o o o o-o o o o--o o-o o--o #
######################################################################
#
# ANDROMEDE
# Copyright (C) 2023 Toulouse INP
#
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
# See the GNU General Public License for more details :
# <http://www.gnu.org/licenses/>.
#
######################################################################
import numpy as np
import cv2
from ThirdParty import PhotogrammetryFunctions as PhotoFuncs
from Core import ReadFlightData
[docs]
def compute_homography(r1, t_vec1, R2, t_vec2, d_inv, normal):
"""Compute the homography from one given position to another.
:param r1: rotational vector of position 1 (matrix 3*3 format)
:param t_vec1: translation vector of position 1
:param R2: rotational vector of position 2 (rodrigues format)
:param t_vec2: translation vector of position 2
:param d_inv: distance to the projected plane for position 1
:param normal: normale au plan de projection
:return: matrix of the homography
:rtype: array
"""
homography = R2 @ r1.transpose() + d_inv * (-R2 @ r1.transpose() @ t_vec1 + t_vec2) @ normal.transpose()
return homography
[docs]
def estimate_interior(interior_orient):
"""Convert camera parameters of undistorted image to pixel value for opencv
:param interior_orient: intrinsic camera parameter
:return: intrinsic camera
:rtype: array
:return: distortion correction factor
:rtype: array
"""
pixel_size = interior_orient.sensor_size_x / interior_orient.resolution_x
ck = -1 * interior_orient.ck / pixel_size
xh = interior_orient.resolution_x / 2
yh = interior_orient.resolution_y / 2
cam_file = [ck, xh, yh, 0, 0, 0, 0, 0]
# read camera file with interior orientation information
# transform metric values to pixel values
ck, cx, cy, k1, k2, k3, p1, p2 = cam_file
# give information about interior camera geometry
# camera matrix opencv
cam_matrix = np.zeros((3, 3), dtype=np.float32)
cam_matrix[0][0] = ck
cam_matrix[0][2] = cx
cam_matrix[1][1] = ck
cam_matrix[1][2] = cy
cam_matrix[2][2] = 1.0
dist_coefficient = np.asarray([k1, k2, p1, p2, k3], dtype=np.float32)
return cam_matrix, dist_coefficient, cam_file
def estimate_exterior(gcpCoo, imgCoo_GCP, interior_orient, need_estimate_exterior,
unit_gcp, max_orientation_deviation, ransacApprox, angles_eor, pos_eor,
directoryOutput):
eor_mat = np.zeros((0, 0))
"""Compute the homography matrix for 3D , pose camera and UAV movement. More details on
https://github.com/AnetteEltner/FlowVeloTool
:param gcpCoo: coordinates of ground reference points (real word)
:param imgCoo_GCP: coordinates of ground reference points in pixel on the image
:param interior_orient: intrinsic camera parameter
:param need_estimate_exterior: estimation of the camera position (True/False)
:param unit_gcp: converting factor for data in intrinsic matrix (default value 1000 i.e data in mm)
:param ransacApprox: first estimation of the camera position with Ransac method (True/False)
:param angles_eor: camera angle (if known)
:param pos_eor: camera angle (if known)
:return: matrix of the homography
:rtype: array
"""
if need_estimate_exterior:
if len(gcpCoo) > 1:
gcp_img_pts_to_undistorted = imgCoo_GCP
# undistorted image measurements of GCP
gcp_img_pts_undistorted = PhotoFuncs.undistort_img_coos(gcp_img_pts_to_undistorted, interior_orient, False)
gcp_pts_ids = np.array([np.linspace(1, len(gcp_img_pts_undistorted), len(gcp_img_pts_undistorted))])
gcp_img_pts_undistorted = np.hstack((gcp_pts_ids.T, gcp_img_pts_undistorted))
gcp_obj_pts_table = np.hstack((gcp_pts_ids.T, gcpCoo))
# get exterior orientation
if ransacApprox:
exterior_approx = np.asarray([0, 0, 0, 0, 0, 0]).reshape(6, 1)
else:
exterior_approx = np.vstack((pos_eor, angles_eor)) # * unit_gcp
eor_mat = PhotoFuncs.get_exterior_camera_geometry(gcp_img_pts_undistorted, gcp_obj_pts_table,
interior_orient, unit_gcp, max_orientation_deviation,
ransacApprox, exterior_approx, True, directoryOutput)
# ...or use predefined camera pose information
else:
rot_mat = PhotoFuncs.rot_matrix(angles_eor[0], angles_eor[1], angles_eor[2], 'radians').T
rot_mat = rot_mat * np.array([[-1, -1, -1], [1, 1, 1], [-1, -1, -1]])
eor_mat = np.hstack((rot_mat.T, pos_eor)) # if rotation matrix received from opencv transpose rot_mat
eor_mat = np.vstack((eor_mat, [0, 0, 0, 1]))
eor_mat[0:3, 3] = eor_mat[0:3, 3] * unit_gcp
return eor_mat
[docs]
def compute_projection_points(video, method, transformation_3D):
"""Design ground data in the right format as a function of the chosen method. Shift minimal position to (0,0).
:param video: analysed video object
:param method: type of Homography
:return: position of reference points on image (pixel)
:rtype: array
:return: position of reference points in the real word (m)
:rtype: array
"""
proj = video.process_dict['proj_table']
min_coord = proj[:, 2] + proj[:, 3]
ind_min = np.where(min_coord == min(min_coord))
originXY = [float(proj[ind_min, 2]), float(proj[ind_min, 3]), 0, 'A', 0, 0] # geo-referencing of positions
proj[:, 2] = proj[:, 2] - (proj[ind_min, 2]) # zeroing of an origin point,
proj[:, 3] = proj[:, 3] - (proj[ind_min, 3])
if len(proj) > 3:
if method == 'Homography' and not transformation_3D:
if len(proj) > 4:
video.outputConsole.appendPlainText('Only the first 4 points are taken into account')
src_points = np.float32([proj[i, :2] for i in range(4)])
dst_points = np.float32([proj[i, 2:4] for i in range(4)])
return src_points, dst_points, originXY
else:
if len(proj[0]) == 4:
video.outputConsole.appendPlainText(' no elevation data for ground points')
else:
src_points = np.float32([proj[i, :2] for i in range(proj.shape[0])])
dst_points = np.float32([proj[i, 2:] for i in range(proj.shape[0])])
return src_points, dst_points, originXY
else:
video.outputConsole.appendPlainText('No enough points in projection file')
[docs]
def compute_projective_matrix(video, params):
"""Compute projective matrix as a function of the chosen homography method.
:param video: analysed video object
:param method: type of Homography
:param pixels_per_metre: resolution of the transformed image
:param ROIXXX: pixel of the transformed ROI
:param angle: camera angle (from UAV position or user-defined)
:param camera_position: camera position (from UAV position or user-defined)
:param unit_gcp: converting factor for data in intrinsic matrix (default value 1000 i.e. data in mm)
:return: matrix of the homography
:rtype: array
"""
method = params.dict['PP']['transformation']
transformation_3D = params.dict['PP']['3D']
if method != 'No transformation':
video.outputConsole.appendPlainText('Transformation initialization...')
if method == 'Homography' and not transformation_3D:
src_points, dst_points, originXY = compute_projection_points(video, method, transformation_3D)
video.projective_matrix = cv2.getPerspectiveTransform(src_points,
dst_points * params.dict['PP']['pixels_per_metre'])
video.process_dict['originXY'] = originXY
else:
camera_matrix, dist_coefficient, cam_file = estimate_interior(video.process_dict['interior_orient'])
params.dict['PP']['camera_matrix'] = camera_matrix
params.dict['PP']['Ymax'] = video.prop['original_resolution'][0]
max_orientation_deviation = 1
ransac_approx = True
angles_eor = np.zeros((3, 1)) # estimation a priori
pos_eor = np.zeros((3, 1))
need_estimate_exterior = False
dst_points, src_points = [np.empty((0, 0)) for _ in range(2)]
ROI_points = np.array(
[[params.dict['IP']['ROIxmin'], video.prop['original_resolution'][0] - params.dict['IP']['ROIymin']],
[params.dict['IP']['ROIxmax'], video.prop['original_resolution'][0] - params.dict['IP']['ROIymin']],
[params.dict['IP']['ROIxmax'], video.prop['original_resolution'][0] - params.dict['IP']['ROIymax']],
[params.dict['IP']['ROIxmin'], video.prop['original_resolution'][0] - params.dict['IP']['ROIymax']]])
if method == 'Homography' and transformation_3D: # calculation of the homography from the 3D points
src_points, dst_points, originXY = compute_projection_points(video, method, transformation_3D)
video.process_dict['originXY'] = originXY
need_estimate_exterior = True
eor_mat = estimate_exterior(dst_points, src_points, video.process_dict['interior_orient'],
need_estimate_exterior,
params.dict['IP']['unit_gcp'], max_orientation_deviation,
ransac_approx, angles_eor, pos_eor,
video.filepath)
rvec = eor_mat[:3, :3]
position_camera = np.zeros((3, 1))
for ii in range(3):
position_camera[ii] = eor_mat[ii, 3] / params.dict['IP']['unit_gcp']
# calcul de tvec à partir de la distance au point (0,0) sur le rayon optique
tvec_axis = np.matmul(rvec, position_camera)
X_proj = groundProjectPoint(ROI_points, rvec, tvec_axis * params.dict['IP']['unit_gcp'],
params.dict['PP']['camera_matrix'], params.dict['PP']['water_level'] *
params.dict['IP']['unit_gcp']) / params.dict['IP']['unit_gcp']
X_proj[:, 0] = X_proj[:, 0] * params.dict['PP']['pixels_per_metre']
X_proj[:, 1] = X_proj[:, 1] * params.dict['PP']['pixels_per_metre']
src2_points = np.float32([ROI_points[i, :2] for i in range(len(ROI_points[:, 0]))])
dst_points = np.float32([X_proj[i, :2] for i in range(len(ROI_points[:, 0]))])
video.projective_matrix = cv2.getPerspectiveTransform(src2_points, dst_points)
elif method == 'Camera position':
params.dict['PP']['before'] = False
# transformation DJI type to Flowvelotool type
angles_eor[0] = (np.pi / 2 + params.dict['PP']['angle'][1] / 180 * np.pi)
angles_eor[1] = params.dict['PP']['angle'][0] / 180 * np.pi
angles_eor[2] = params.dict['PP']['angle'][2] / 180 * np.pi
# estimation a priori
for j in range(3):
pos_eor[j] = params.dict['PP']['camera_position'][j] # position of the camera
eor_mat = estimate_exterior(dst_points, src_points, video.process_dict['interior_orient'],
need_estimate_exterior,
params.dict['IP']['unit_gcp'], max_orientation_deviation,
ransac_approx, angles_eor, pos_eor,
None)
rvec = eor_mat[:3, :3]
pos_eor[2] = pos_eor[2] - params.dict['PP']['water_level']
tvec_axis = np.matmul(rvec, pos_eor)
X_proj = groundProjectPoint(ROI_points, rvec, tvec_axis * params.dict['IP']['unit_gcp'],
params.dict['PP']['camera_matrix'], 0) / params.dict['IP']['unit_gcp']
X_proj[:, 0] = X_proj[:, 0] * params.dict['PP']['pixels_per_metre']
X_proj[:, 1] = X_proj[:, 1] * params.dict['PP']['pixels_per_metre']
src2_points = np.float32([ROI_points[i, :2] for i in range(len(ROI_points[:, 0]))])
dst_points = np.float32([X_proj[i, :2] for i in range(len(ROI_points[:, 0]))])
video.projective_matrix = cv2.getPerspectiveTransform(src2_points, dst_points)
elif method == 'UAV':
tstart = video.prop['window_frame'][0] / video.prop['fps']
params.dict['PP']['before'] = False
Coord_uav, originXY, angle_uav = ReadFlightData.read_flight_data(video.flight_name, tstart,
1 / video.prop['fps'] * video.prop['frame_step'])
video.process_dict['originXY'] = originXY
Nstep = len(Coord_uav[0])
params.dict['PP']['angle'] = np.zeros((Nstep, 3))
params.dict['PP']['camera_position'] = np.zeros((Nstep, 3))
params.dict['PP']['originXY'] = originXY
for i in range(Nstep):
params.dict['PP']['angle'][i, 1] = angle_uav[0][i] / 180 * np.pi
params.dict['PP']['angle'][i, 0] = (np.pi / 2 + angle_uav[1][i] / 180 * np.pi) #
params.dict['PP']['angle'][i, 2] = angle_uav[2][i] / 180 * np.pi
# params_dict_file['anglePdv'][i,2]=angle_uav[2][i]/180*np.pi-angle_uav[2][0]/180*np.pi
params.dict['PP']['camera_position'][i, 0] = Coord_uav[0][i]
params.dict['PP']['camera_position'][i, 1] = Coord_uav[1][i]
params.dict['PP']['camera_position'][i, 2] = Coord_uav[2][i]
angles_eor = np.zeros((3, 1)) # estimation a priori
pos_eor = np.zeros((3, 1))
for j in range(3):
angles_eor[j] = params.dict['PP']['angle'][i, j] # estimation a priori
pos_eor[j] = params.dict['PP']['camera_position'][
i, j] # position de la camera lue dans le plan de vol à l'instant initial
eor_mat = estimate_exterior(dst_points, src_points, params.process_dict['interior_orient'],
need_estimate_exterior,
params.dict['IP']['unit_gcp'], max_orientation_deviation,
ransac_approx, angles_eor, pos_eor,
None)
video.process_dict['EOR'].append(eor_mat)
[docs]
def shift_image(video, params):
"""Compute the translation of tranfsformed image to get positive value.
:param video: analysed video object
:param transformation: type of Homography
:param ROIXXX: pixel of the transformed ROI
:return: matrix of the translation to positive value for the transformed original image
:rtype: array
:return: matrix of the translation to positive value for the transformed ROI
:rtype: array
:return: resolution of the transformed original image
:rtype: list of int (height,width)
:return: resolution of the transformed ROI
:rtype: list of int (height,width)
"""
# Compute image size original
method = params.dict['PP']['transformation']
if (method != 'No transformation') and (method != 'UAV'):
pts_bnd = np.float32(
[[0, 0], [0, video.prop['original_resolution'][0]], [video.prop['original_resolution'][1], video.prop['original_resolution'][0]],
[video.prop['original_resolution'][1], 0]]).reshape(-1, 1, 2)
pts_bnd_ = cv2.perspectiveTransform(pts_bnd, video.projective_matrix)
[xmin, ymin] = np.int32(pts_bnd_.min(axis=0).ravel() - 0.5)
[xmax, ymax] = np.int32(pts_bnd_.max(axis=0).ravel() + 0.5)
pts_bnd_ROI = np.float32([[params.dict['IP']['ROIxmin'], params.dict['IP']['ROIymin']],
[params.dict['IP']['ROIxmin'], params.dict['IP']['ROIymax']],
[params.dict['IP']['ROIxmax'], params.dict['IP']['ROIymax']],
[params.dict['IP']['ROIxmax'], params.dict['IP']['ROIymin']]]).reshape(-1, 1, 2)
pts_bnd_ROI = cv2.perspectiveTransform(pts_bnd_ROI, video.projective_matrix)
[ROIxmin, ROIymin] = np.int32(pts_bnd_ROI.min(axis=0).ravel() - 0.5)
[ROIxmax, ROIymax] = np.int32(pts_bnd_ROI.max(axis=0).ravel() + 0.5)
t = [-xmin, -ymin] # offset to have positive co-data (pixel)
tROI = [-ROIxmin, -ROIymin] # offset to have positive co-data (pixel)
video.process_dict['Ht'] = np.array([[1, 0, t[0]], [0, 1, t[1]], [0, 0, 1]]) # Translate
video.process_dict['Ht_ROI'] = np.array([[1, 0, tROI[0]], [0, 1, tROI[1]], [0, 0, 1]]) # Translate
# projection of the extreme points of the ROI
video.process_dict['ROI_min'] = (ROIxmin, ROIymin)
video.process_dict['resolution_transform_ROI'] = (ROIymax - ROIymin, ROIxmax - ROIxmin)
[docs]
def groundProjectPoint(coord_point, rvec, tvec, camera_matrix, z=0.0):
"""Project the points in pixel coordinate into real world coordinate. The origin is the intersection between camera
axis and choosen plane.
:param coord_point: Points in pixel
:param rvec: rotational matrix from pose camera angle
:param tvec: translational vector from pose camera
:param camera_matrix: intrinsic camera matrix
:return: all points in real word coordinates
:rtype: array
"""
# projet dans le monde réel les coordonnées images dans coord_points
# if len(coord_point.shape)==1:
# coord=coord_point.copy()
# coord_point =np.zeros((1,2))
# coord_point[0, 0] = coord[0]
# coord_point[0, 1] = coord[1]
coord_proj = np.float32(np.zeros((len(coord_point[:, 0]), len(coord_point[0, :]) + 1)))
# rotMat, _ = cv2.Rodrigues(rvec)
rotMat = rvec
camMat = np.asarray(camera_matrix)
_, iRot = cv2.invert(rotMat)
_, iCam = cv2.invert(camMat)
for i in range(len(coord_point)):
uvPoint = np.ones((3, 1))
# Image point
uvPoint[0] = coord_point[i, 0]
uvPoint[1] = coord_point[i, 1]
tempMat = np.matmul(np.matmul(iRot, iCam), uvPoint)
tempMat2 = np.matmul(iRot, tvec)
s = (z + tempMat2[2]) / tempMat[2]
wcPoint = np.float32(np.matmul(iRot, (np.matmul(s * iCam, uvPoint) - tvec))) # world coordinate
# wcPoint = np.float32(np.matmul((np.matmul(s * uvPoint, iCam) - tvec),iRot)) # world coordinate
# wcPoint[2] will not be exactly equal to z, but very close to it
# assert int(abs(wcPoint[2] - z) * (10 ** 8)) == 0
wcPoint[2] = z
coord_proj[i, 0] = wcPoint[0]
coord_proj[i, 1] = wcPoint[1]
coord_proj[i, 2] = wcPoint[2]
return coord_proj