video-location/video_location.py

# -*- coding: utf-8 -*-
"""
最终交付版 - Python后端GeoJSON处理器 (v11.2)

功能:
- 完全基于您提供的、投影方向正确的 project_lonlat_to_pixel 函数。
- 核心函数 process_geojson_for_frontend 直接输出一个“干净”的、可供前端JS绘制的【线段列表】。
- 包含一个本地OpenCV预览，用于即时验证返回给前端的数据。
"""
import numpy as np
from scipy.spatial.transform import Rotation as R
import json

## -------------------------------------------------------------------------- ##
## 1. 核心算法函数 (基于您的版本)
## -------------------------------------------------------------------------- ##
EARTH_RADIUS = 6378137.0

def lonlat_to_enu(lon, lat, center_lon, center_lat):
    delta_lon = np.radians(lon - center_lon)
    delta_lat = np.radians(lat - center_lat)
    x = EARTH_RADIUS * delta_lon * np.cos(np.radians(center_lat))
    y = EARTH_RADIUS * delta_lat
    return np.array([x, y, 0])

def project_lonlat_to_pixel(target_lon, target_lat, resolution_w, resolution_h,
                            cam_lon, cam_lat, cam_height, p, t, hfov, north_p_value=0.0):
    """
    [最终正确版] 反向投影函数：从经纬度计算像素坐标(u, v)。
    - 正确处理 cz < 0 的情况，通过创建一个方向正确但位置在无穷远的虚拟点来实现线段裁剪。
    """
    # 步骤 1 & 2: ... (这部分代码不变)
    target_enu = lonlat_to_enu(target_lon, target_lat, cam_lon, cam_lat)
    cam_pos_enu = np.array([0, 0, cam_height])
    vec_enu = target_enu - cam_pos_enu
    norm = np.linalg.norm(vec_enu)
    if norm < 1e-6:
        return None, None, False
    vec_enu /= norm

    # 步骤 3: ... (这部分代码不变)
    p_corrected = p - north_p_value
    r_base = R.from_euler('x', -90, degrees=True)
    r_pan = R.from_euler('z', p_corrected, degrees=True)
    r_tilt = R.from_euler('x', -t, degrees=True)
    total_rotation = r_pan * r_tilt * r_base
    inverse_rotation = total_rotation.inv()
    vec_cam = inverse_rotation.apply(vec_enu)
    cx, cy, cz = vec_cam

    center_u = resolution_w / 2
    center_v = resolution_h / 2

    # ===================== [ 核心修正逻辑 V3.0 ] =====================
    # 如果点在相机后方 (cz <= 0)
    if cz <= 1e-9:
        # 我们不再尝试投影它。
        # 而是根据 cx, cy 的方向，在屏幕中心的基础上，向该方向延伸一个巨大的距离，
        # 创建一个“无穷远”的虚拟点。这个点的方向是正确的。
        # 这个巨大的数值确保了该点一定在屏幕外。
        MEGA_DISTANCE = 1_000_000.0
        u = center_u + cx * MEGA_DISTANCE
        v = center_v + cy * MEGA_DISTANCE
        return u, v, False # 这个点本身是不可见的
    # =================================================================

    # 步骤 4: (正常投影逻辑) 如果点在相机前方，则正常计算
    f_x = resolution_w / (2 * np.tan(np.radians(hfov) / 2))
    f_y = f_x

    u = center_u + f_x * (cx / cz)
    v = center_v + f_y * (cy / cz)

    is_visible = (0 <= u < resolution_w) and (0 <= v < resolution_h)

    return u, v, is_visible

## -------------------------------------------------------------------------- ##
## 2. GeoJSON解析 与 线段裁剪算法
## -------------------------------------------------------------------------- ##
def parse_geojson_coordinates(geojson_data):
    coordinates = []
    try:
        if geojson_data.get('type') == 'FeatureCollection': geometry = geojson_data['features'][0]['geometry']
        else: geometry = geojson_data
        if geometry.get('type') == 'Polygon':
            exterior_ring = geometry['coordinates'][0]
            coordinates = [(coord[0], coord[1]) for coord in exterior_ring]
            print(f"成功从GeoJSON中提取了 {len(coordinates)} 个边界点。")
            return coordinates
    except (KeyError, IndexError, TypeError): pass
    return []

INSIDE, LEFT, RIGHT, BOTTOM, TOP = 0, 1, 2, 4, 8
def compute_outcode(x, y, xmin, ymin, xmax, ymax):
    code = INSIDE
    if x < xmin: code |= LEFT
    elif x > xmax: code |= RIGHT
    if y < ymin: code |= BOTTOM
    elif y > ymax: code |= TOP
    return code
def cohen_sutherland_clip(x1, y1, x2, y2, xmin, ymin, xmax, ymax):
    code1 = compute_outcode(x1, y1, xmin, ymin, xmax, ymax); code2 = compute_outcode(x2, y2, xmin, ymin, xmax, ymax)
    accept = False
    while True:
        if not (code1 | code2): accept = True; break
        elif code1 & code2: break
        else:
            x, y = 0.0, 0.0; code_out = code1 if code1 else code2
            if code_out & TOP:
                if y2 != y1: x = x1 + (x2 - x1) * (ymax - y1) / (y2 - y1)
                else: x = x1
                y = ymax
            elif code_out & BOTTOM:
                if y2 != y1: x = x1 + (x2 - x1) * (ymin - y1) / (y2 - y1)
                else: x = x1
                y = ymin
            elif code_out & RIGHT:
                if x2 != x1: y = y1 + (y2 - y1) * (xmax - x1) / (x2 - x1)
                else: y = y1
                x = xmax
            elif code_out & LEFT:
                if x2 != x1: y = y1 + (y2 - y1) * (xmin - x1) / (x2 - x1)
                else: y = y1
                x = xmin
            if code_out == code1: x1, y1 = x, y; code1 = compute_outcode(x1, y1, xmin, ymin, xmax, ymax)
            else: x2, y2 = x, y; code2 = compute_outcode(x2, y2, xmin, ymin, xmax, ymax)
    if accept: return [[int(x1), int(y1)], [int(x2), int(y2)]]
    return None

## -------------------------------------------------------------------------- ##
## 3. 核心处理函数
## -------------------------------------------------------------------------- ##
def process_geojson_for_frontend(geojson_data, camera_params, calibrated_params):
    geo_coords = parse_geojson_coordinates(geojson_data)
    if not geo_coords: return []
    if len(geo_coords) > 1 and geo_coords[0] != geo_coords[-1]:
        geo_coords.append(geo_coords[0]) # 确保多边形闭合

    full_params = {**camera_params, **calibrated_params}
    raw_pixel_points = []
    # 1. Batch convert all geographic points to pixel points
    for lon, lat in geo_coords:
        u, v, is_visible = project_lonlat_to_pixel(
            lon, lat,
            resolution_w=camera_params['resolution_w'], resolution_h=camera_params['resolution_h'],
            cam_lon=camera_params['cam_lon'], cam_lat=camera_params['cam_lat'],
            cam_height=calibrated_params['cam_height'], p=camera_params['p'],
            t=camera_params['t'], hfov=calibrated_params['hfov'],
            north_p_value=camera_params['north_p_value']
        )
        raw_pixel_points.append([int(u), int(v)])

    visible_segments = []
    screen_w = camera_params['resolution_w']
    screen_h = camera_params['resolution_h']

    for i in range(len(raw_pixel_points) - 1):
        p1 = raw_pixel_points[i]
        p2 = raw_pixel_points[i+1]
        if p1[0] == float('inf') or p2[0] == float('inf'): continue
        u1, v1 = p1; u2, v2 = p2
        clipped_segment = cohen_sutherland_clip(u1, v1, u2, v2, 0, 0, screen_w - 1, screen_h - 1)
        if clipped_segment:
            visible_segments.append(clipped_segment)

    return visible_segments

## -------------------------------------------------------------------------- ##
## 4. 主程序
## -------------------------------------------------------------------------- ##
if __name__ == '__main__':
    geojson_string = """
{
  "type": "FeatureCollection",
  "features": [
    {
      "type": "Feature",
      "properties": {
        "name": "",
        "id": "m-7d43a6d0-a7bd-4fc1-84b2-deab51757f8e",
        "type": "polygon",
        "style": {
          "color": "#29cf34",
          "opacity": 0.5,
          "outline": true,
          "outlineWidth": 3,
          "outlineColor": "#ffffff",
          "clampToGround": true,
          "materialType": "Color",
          "label": {
            "text": "",
            "font_size": 18,
            "color": "#ffffff",
            "outline": true,
            "outlineColor": "#000000",
            "pixelOffsetY": -10,
            "distanceDisplayCondition": true,
            "distanceDisplayCondition_far": 500000,
            "distanceDisplayCondition_near": 0
          }
        },
        "options": {}
      },
      "geometry": {
        "type": "Polygon",
        "coordinates": [
          [
            [
              112.894539,
              28.221327,
              0
            ],
            [
              112.894712,
              28.221333,
              0
            ],
            [
              112.894709,
              28.221267,
              0
            ],
            [
              112.894533,
              28.221267,
              0
            ]
          ]
        ]
      }
    }
  ]
}
    """
    geojson_data = json.loads(geojson_string)
    calibrated_params = {"cam_height": 28.0000, "hfov": 61.0000}
    camera_params = {
        "resolution_w": 1280, "resolution_h": 720,
        "cam_lon": 112.893799, "cam_lat": 28.221701,
        "north_p_value": 39.0, "p": 271.9, "t": 26.2
    }

    final_screen_segments = process_geojson_for_frontend(geojson_data, camera_params, calibrated_params)

    print(json.dumps(final_screen_segments, indent=2))