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