def GetLine3DIntersectionWithDEM(sensorPt, targetPt):
''' Obtain height for points,intersecting with DEM '''
pt = []
sensorLat = sensorPt[0]
sensorLon = sensorPt[1]
sensorAlt = sensorPt[2]
targetLat = targetPt[0]
targetLon = targetPt[1]
try:
targetAlt = targetPt[2]
except Exception:
targetAlt = GetFrameCenter()[2]
distance = sphere.distance([sensorLat, sensorLon], [targetLat, targetLon])
distance = sqrt(distance**2 + (targetAlt - sensorAlt)**2)
dLat = (targetLat - sensorLat) / distance
dLon = (targetLon - sensorLon) / distance
dAlt = (targetAlt - sensorAlt) / distance
xOrigin = dtm_transform[0]
yOrigin = dtm_transform[3]
pixelWidth = dtm_transform[1]
pixelHeight = -dtm_transform[5]
pixelWidthMeter = pixelWidth * (pi / 180.0) * 6378137.0
# start at k = sensor point, then test every pixel a point on the 3D line
# until we cross the dtm (diffAlt >= 0).
diffAlt = -1
for k in range(0, int(dtm_buffer * pixelWidthMeter), int(pixelWidthMeter)):
point = [
sensorLon + k * dLon, sensorLat + k * dLat, sensorAlt + k * dAlt
]
col = int((point[0] - xOrigin) / pixelWidth)
row = int((yOrigin - point[1]) / pixelHeight)
try:
diffAlt = point[2] - dtm_data[row -
dtm_rowLowerBound][col -
dtm_colLowerBound]
except Exception:
qgsu.showUserAndLogMessage(
"", "DEM point not found after all iterations.", onlyLog=True)
break
if diffAlt <= 0:
pt = [point[1], point[0], point[2]]
break
if not pt:
qgsu.showUserAndLogMessage(
"",
"DEM point not found, last computed delta high: " + str(diffAlt),
onlyLog=True)
return pt
def drawRulerOnVideo(pt, idx, painter, surface, gt, drawRuler):
''' Draw Ruler on Video '''
if hasElevationModel():
pt = GetLine3DIntersectionWithPlane(
GetSensor(), pt, GetFrameCenter()[2])
scr_x, scr_y = vut.GetInverseMatrix(
pt[1], pt[0], gt, surface)
center_pt = pt
radius_pt = 5
center = QPoint(scr_x, scr_y)
if len(drawRuler) > 1:
try:
pen = QPen(Qt.red)
pen.setWidth(3)
painter.setPen(pen)
end_pt = drawRuler[idx + 1]
if hasElevationModel():
end_pt = GetLine3DIntersectionWithPlane(
GetSensor(), end_pt, GetFrameCenter()[2])
scr_x, scr_y = vut.GetInverseMatrix(
end_pt[1], end_pt[0], gt, surface)
end = QPoint(scr_x, scr_y)
painter.drawLine(center, end)
font12 = QFont("Arial", 12, weight=QFont.Bold)
painter.setFont(font12)
distance = round(sphere.distance(
(center_pt[0], center_pt[1]), (end_pt[0], end_pt[1])), 2)
text = str(distance) + " m"
global RulerTotalMeasure
RulerTotalMeasure += distance
# Draw Start/End Points
pen = QPen(Qt.white)
pen.setWidth(radius_pt)
pen.setCapStyle(Qt.RoundCap)
painter.setPen(pen)
painter.drawPoint(center)
painter.drawPoint(end)
painter.drawText(end + QPoint(5, -5), text)
pen = QPen(QColor(255, 51, 153))
painter.setPen(pen)
painter.drawText(end + QPoint(5, 10),
str(round(RulerTotalMeasure, 2)) + " m")
except Exception:
None
return
def UpdateTrajectoryData(packet):
''' Update Trajectory Values '''
global tLastLon
global tLastLat
lat = packet.GetSensorLatitude()
lon = packet.GetSensorLongitude()
alt = packet.GetSensorTrueAltitude()
try:
if tLastLon == 0.0 and tLastLat == 0.0:
tLastLon = lon
tLastLat = lat
else:
# little check to see if telemetry data are plausible before
# drawing.
distance = sphere.distance((tLastLon, tLastLat), (lon, lat))
if distance > 1000: # 1 km is the best value for prevent draw trajectory when start video again
return
except Exception:
None
tLastLon = lon
tLastLat = lat
trajectoryLyr = qgsu.selectLayerByName(Trajectory_lyr)
try:
if all(v is not None for v in [trajectoryLyr, lat, lon, alt]):
trajectoryLyr.startEditing()
f = QgsFeature()
if trajectoryLyr.featureCount() == 0:
f.setAttributes([lon, lat, alt])
surface = QgsGeometry.fromPolylineXY(
[QgsPointXY(lon, lat),
QgsPointXY(lon, lat)])
f.setGeometry(surface)
trajectoryLyr.addFeatures([f])
else:
f_last = trajectoryLyr.getFeature(trajectoryLyr.featureCount())
f.setAttributes([lon, lat, alt])
surface = QgsGeometry.fromPolylineXY([
QgsPointXY(lon, lat),
QgsPointXY(f_last.attribute(0), f_last.attribute(1))
])
f.setGeometry(surface)
trajectoryLyr.addFeatures([f])
CommonLayer(trajectoryLyr)
except Exception as e:
qgsu.showUserAndLogMessage(
QCoreApplication.translate("QgsFmvUtils",
"Failed Update Trajectory Layer! : "),
str(e))
return
def drawMeasureDistanceOnVideo(pt, idx, painter, surface, gt,
drawMDistance):
''' Draw Measure Distance on Video '''
if hasElevationModel():
pt = GetLine3DIntersectionWithPlane(GetSensor(), pt,
GetFrameCenter()[2])
scr_x, scr_y = vut.GetInverseMatrix(pt[1], pt[0], gt, surface)
center = QPoint(scr_x, scr_y)
if len(drawMDistance) > 1:
try:
painter.setPen(MeasurePen)
end_pt = drawMDistance[idx + 1]
if hasElevationModel():
end_pt = GetLine3DIntersectionWithPlane(
GetSensor(), end_pt,
GetFrameCenter()[2])
scr_x, scr_y = vut.GetInverseMatrix(end_pt[1], end_pt[0], gt,
surface)
end = QPoint(scr_x, scr_y)
painter.drawLine(center, end)
painter.setFont(DrawToolBar.bold_12)
distance = round(
sphere.distance((pt[0], pt[1]), (end_pt[0], end_pt[1])), 2)
text = str(distance) + " m"
global RulerTotalMeasure
RulerTotalMeasure += distance
# Line lenght
painter.setPen(MeasurePen)
painter.drawText(end + QPoint(5, -10), text)
painter.setPen(DrawToolBar.white_pen)
# Total lenght
painter.drawText(end + QPoint(5, 10),
str(round(RulerTotalMeasure, 2)) + " m")
# Draw Start/End Points
painter.drawPoint(center)
painter.drawPoint(end)
except Exception:
None
return
def GetLine3DIntersectionWithPlane(sensorPt, demPt, planeHeight):
''' Get Altitude from DEM '''
sensorLat = sensorPt[0]
sensorLon = sensorPt[1]
sensorAlt = sensorPt[2]
demPtLat = demPt[1]
demPtLon = demPt[0]
demPtAlt = demPt[2]
distance = sphere.distance([sensorLat, sensorLon], [demPtLat, demPtLon])
distance = sqrt(distance ** 2 + (demPtAlt - demPtAlt) ** 2)
dLat = (demPtLat - sensorLat) / distance
dLon = (demPtLon - sensorLon) / distance
dAlt = (demPtAlt - sensorAlt) / distance
k = ((demPtAlt - planeHeight) / (sensorAlt - demPtAlt)) * distance
pt = [sensorLon + (distance + k) * dLon, sensorLat +
(distance + k) * dLat, sensorAlt + (distance + k) * dAlt]
return pt
def CornerEstimationWithoutOffsets(packet):
''' Corner estimation without Offsets '''
try:
sensorLatitude = packet.SensorLatitude
sensorLongitude = packet.SensorLongitude
sensorTrueAltitude = packet.SensorTrueAltitude
frameCenterLat = packet.FrameCenterLatitude
frameCenterLon = packet.FrameCenterLongitude
frameCenterElevation = packet.FrameCenterElevation
sensorVerticalFOV = packet.SensorVerticalFieldOfView
sensorHorizontalFOV = packet.SensorHorizontalFieldOfView
headingAngle = packet.PlatformHeadingAngle
sensorRelativeAzimut = packet.SensorRelativeAzimuthAngle
targetWidth = packet.targetWidth
slantRange = packet.SlantRange
# If target width = 0 (occurs on some platforms), compute it with the slate range.
# Otherwise it leaves the footprint as a point.
# In some case targetWidth don't have value then equal to 0
if targetWidth is None:
targetWidth = 0
if targetWidth == 0 and slantRange != 0:
targetWidth = 2.0 * slantRange * \
tan(radians(sensorHorizontalFOV / 2.0))
elif targetWidth == 0 and slantRange == 0:
# default target width to not leave footprint as a point.
targetWidth = defaultTargetWidth
qgsu.showUserAndLogMessage(
QCoreApplication.translate(
"QgsFmvUtils", "Target width unknown, defaults to: " +
str(targetWidth) + "m."))
# compute distance to ground
if frameCenterElevation != 0:
sensorGroundAltitude = sensorTrueAltitude - frameCenterElevation
else:
qgsu.showUserAndLogMessage(
QCoreApplication.translate(
"QgsFmvUtils",
"Sensor ground elevation narrowed to true altitude: " +
str(sensorTrueAltitude) + "m."))
sensorGroundAltitude = sensorTrueAltitude
if sensorLatitude == 0:
return False
if sensorLongitude is None or sensorLatitude is None:
return False
initialPoint = (sensorLongitude, sensorLatitude)
if frameCenterLon is None or frameCenterLat is None:
return False
destPoint = (frameCenterLon, frameCenterLat)
distance = sphere.distance(initialPoint, destPoint)
if distance == 0:
return False
if sensorVerticalFOV > 0 and sensorHorizontalFOV > sensorVerticalFOV:
aspectRatio = sensorVerticalFOV / sensorHorizontalFOV
else:
aspectRatio = 0.75
value2 = (headingAngle + sensorRelativeAzimut) % 360.0 # Heading
value3 = targetWidth / 2.0
value5 = sqrt(pow(distance, 2.0) + pow(sensorGroundAltitude, 2.0))
value6 = targetWidth * aspectRatio / 2.0
degrees = rad2deg(atan(value3 / distance))
value8 = rad2deg(atan(distance / sensorGroundAltitude))
value9 = rad2deg(atan(value6 / value5))
value10 = value8 + value9
value11 = sensorGroundAltitude * tan(radians(value10))
value12 = value8 - value9
value13 = sensorGroundAltitude * tan(radians(value12))
value14 = distance - value13
value15 = value11 - distance
value16 = value3 - value14 * tan(radians(degrees))
value17 = value3 + value15 * tan(radians(degrees))
distance2 = sqrt(pow(value14, 2.0) + pow(value16, 2.0))
value19 = sqrt(pow(value15, 2.0) + pow(value17, 2.0))
value20 = rad2deg(atan(value16 / value14))
value21 = rad2deg(atan(value17 / value15))
# CP Up Left
bearing = (value2 + 360.0 - value21) % 360.0
cornerPointUL = list(
reversed(sphere.destination(destPoint, value19, bearing)))
# CP Up Right
bearing = (value2 + value21) % 360.0
cornerPointUR = list(
reversed(sphere.destination(destPoint, value19, bearing)))
# CP Low Right
bearing = (value2 + 180.0 - value20) % 360.0
cornerPointLR = list(
reversed(sphere.destination(destPoint, distance2, bearing)))
# CP Low Left
bearing = (value2 + 180.0 + value20) % 360.0
cornerPointLL = list(
reversed(sphere.destination(destPoint, distance2, bearing)))
if hasElevationModel():
pCornerPointUL = GetLine3DIntersectionWithDEM(
GetSensor(), cornerPointUL)
pCornerPointUR = GetLine3DIntersectionWithDEM(
GetSensor(), cornerPointUR)
pCornerPointLR = GetLine3DIntersectionWithDEM(
GetSensor(), cornerPointLR)
pCornerPointLL = GetLine3DIntersectionWithDEM(
GetSensor(), cornerPointLL)
UpdateFootPrintData(packet, pCornerPointUL, pCornerPointUR,
pCornerPointLR, pCornerPointLL,
hasElevationModel())
UpdateBeamsData(packet, pCornerPointUL,
pCornerPointUR, pCornerPointLR, pCornerPointLL,
hasElevationModel())
else:
UpdateFootPrintData(packet, cornerPointUL, cornerPointUR,
cornerPointLR, cornerPointLL,
hasElevationModel())
UpdateBeamsData(packet, cornerPointUL, cornerPointUR,
cornerPointLR, cornerPointLL, hasElevationModel())
SetGCPsToGeoTransform(cornerPointUL, cornerPointUR, cornerPointLR,
cornerPointLL, frameCenterLon, frameCenterLat)
except Exception as e:
qgsu.showUserAndLogMessage(
QCoreApplication.translate(
"QgsFmvUtils", "CornerEstimationWithoutOffsets failed! : "),
str(e))
return False
return True
def CornerEstimationWithoutOffsets(packet):
''' Corner estimation without Offsets '''
try:
sensorLatitude = packet.GetSensorLatitude()
sensorLongitude = packet.GetSensorLongitude()
sensorTrueAltitude = packet.GetSensorTrueAltitude()
frameCenterLat = packet.GetFrameCenterLatitude()
frameCenterLon = packet.GetFrameCenterLongitude()
sensorVerticalFOV = packet.GetSensorVerticalFieldOfView()
sensorHorizontalFOV = packet.GetSensorHorizontalFieldOfView()
headingAngle = packet.GetPlatformHeadingAngle()
sensorRelativeAzimut = packet.GetSensorRelativeAzimuthAngle()
targetWidth = packet.GettargetWidth()
if sensorLatitude == 0:
return False
initialPoint = (sensorLongitude, sensorLatitude)
destPoint = (frameCenterLon, frameCenterLat)
distance = sphere.distance(initialPoint, destPoint)
if distance == 0:
return False
if sensorVerticalFOV > 0 and sensorHorizontalFOV > sensorVerticalFOV:
aspectRatio = sensorVerticalFOV / sensorHorizontalFOV
else:
aspectRatio = 0.75
value2 = (headingAngle + sensorRelativeAzimut) % 360.0 # Heading
value3 = targetWidth / 2.0
value5 = sqrt(pow(distance, 2.0) + pow(sensorTrueAltitude, 2.0))
value6 = targetWidth * aspectRatio / 2.0
degrees = rad2deg(atan(value3 / distance))
value8 = rad2deg(atan(distance / sensorTrueAltitude))
value9 = rad2deg(atan(value6 / value5))
value10 = value8 + value9
value11 = sensorTrueAltitude * tan(radians(value10))
value12 = value8 - value9
value13 = sensorTrueAltitude * tan(radians(value12))
value14 = distance - value13
value15 = value11 - distance
value16 = value3 - value14 * tan(radians(degrees))
value17 = value3 + value15 * tan(radians(degrees))
distance2 = sqrt(pow(value14, 2.0) + pow(value16, 2.0))
value19 = sqrt(pow(value15, 2.0) + pow(value17, 2.0))
value20 = rad2deg(atan(value16 / value14))
value21 = rad2deg(atan(value17 / value15))
# CP Up Left
bearing = (value2 + 360.0 - value21) % 360.0
cornerPointUL = sphere.destination(destPoint, value19, bearing)
# TODO: Use Geopy?
# from geopy import Point
# from geopy.distance import distance, VincentyDistance
#
# # given: lat1, lon1, bearing, distMiles
# lat2, lon2 = VincentyDistance(kilometers=value19).destination(Point(destPoint[1], destPoint[0]), bearing)
# CP Up Right
bearing = (value2 + value21) % 360.0
cornerPointUR = sphere.destination(destPoint, value19, bearing)
# CP Low Right
bearing = (value2 + 180.0 - value20) % 360.0
cornerPointLR = sphere.destination(destPoint, distance2, bearing)
# CP Low Left
bearing = (value2 + 180.0 + value20) % 360.0
cornerPointLL = sphere.destination(destPoint, distance2, bearing)
UpdateFootPrintData(cornerPointUL, cornerPointUR, cornerPointLR,
cornerPointLL)
UpdateBeamsData(packet, cornerPointUL, cornerPointUR, cornerPointLR,
cornerPointLL)
SetGCPsToGeoTransform(cornerPointUL, cornerPointUR, cornerPointLR,
cornerPointLL, frameCenterLon, frameCenterLat)
except:
return False
return True
