You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
79 lines
1.8 KiB
79 lines
1.8 KiB
2 months ago
|
import os
|
||
|
|
import sys
|
||
|
|
import ctypes
|
||
|
|
from pathlib import Path
|
||
|
|
import numpy as np
|
||
|
|
|
||
|
|
# 获取项目根目录（当前文件所在目录的父目录）
|
||
|
|
project_root = Path(__file__).parent.parent.parent
|
||
|
|
bin_dir = project_root / "Bin"
|
||
|
|
|
||
|
|
# 添加Bin目录到Python模块搜索路径
|
||
|
|
if str(bin_dir) not in sys.path:
|
||
|
|
sys.path.insert(0, str(bin_dir))
|
||
|
|
|
||
|
|
# 预加载依赖库，确保动态链接器能找到它们
|
||
|
|
lib_guide_stitch = bin_dir / "libGuideStitch.so"
|
||
|
|
if lib_guide_stitch.exists():
|
||
|
|
try:
|
||
|
|
ctypes.CDLL(str(lib_guide_stitch), mode=ctypes.RTLD_GLOBAL)
|
||
|
|
except Exception as e:
|
||
|
|
print(f"警告: 预加载libGuideStitch.so失败: {e}")
|
||
|
|
|
||
|
|
# 导入模块
|
||
|
|
from UStitcher import API_UnderStitch, FrameInfo
|
||
|
|
|
||
|
|
import cv2
|
||
|
|
|
||
|
|
|
||
|
|
frame_info = FrameInfo()
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
frame_info.nFrmID = 1
|
||
|
|
frame_info.craft.stPos.B = 39
|
||
|
|
frame_info.craft.stPos.L = 120
|
||
|
|
frame_info.craft.stPos.H = 1000
|
||
|
|
frame_info.camInfo.nFocus = 40
|
||
|
|
frame_info.camInfo.fPixelSize = 12
|
||
|
|
|
||
|
|
frame_info.servoInfo.fServoAz = 90
|
||
|
|
frame_info.servoInfo.fServoPt = -45
|
||
|
|
|
||
|
|
frame_info.nEvHeight = 1200
|
||
|
|
|
||
|
|
frame_info.nWidth = 1280
|
||
|
|
frame_info.nHeight = 1024
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
stitcher = API_UnderStitch.Create()
|
||
|
|
|
||
|
|
|
||
|
|
# 先初始化（设置原点）
|
||
|
|
pan_info = stitcher.Init(frame_info)
|
||
|
|
print(f"初始化成功，全景图尺寸: {pan_info.m_pan_width} x {pan_info.m_pan_height}")
|
||
|
|
|
||
|
|
def warpPointWithH(H, pt):
|
||
|
|
wp = H @ np.array([pt[0],pt[1],1]).T
|
||
|
|
return wp / wp[2]
|
||
|
|
|
||
|
|
for i in range(100):
|
||
|
|
frame_info.nFrmID = i
|
||
|
|
frame_info.craft.stPos.B = 39
|
||
|
|
frame_info.craft.stPos.L = 120
|
||
|
|
frame_info.craft.stPos.H = 1000
|
||
|
|
frame_info.camInfo.nFocus = 40
|
||
|
|
frame_info.camInfo.fPixelSize = 12
|
||
|
|
|
||
|
|
frame_info.servoInfo.fServoAz = 90
|
||
|
|
frame_info.servoInfo.fServoPt = -45
|
||
|
|
|
||
|
|
H = stitcher.getHomography(frame_info)
|
||
|
|
print(f"单应性矩阵 H:\n{H}")
|
||
|
|
|
||
|
|
wp = warpPointWithH(H, np.array([100,111]))
|
||
|
|
print(f"物方坐标:\n{wp}")
|
||
|
|
|
||
|
|
print("done")
|