|
|
import numpy as np
|
|
|
import json
|
|
|
from typing import Dict, Any
|
|
|
|
|
|
class CameraModel:
|
|
|
def __init__(self, config_path: str = None):
|
|
|
"""
|
|
|
初始化相机模型，支持从配置文件加载参数。
|
|
|
:param config_path: 配置文件的路径（JSON/YAML）。如果为 None，则使用默认参数。
|
|
|
"""
|
|
|
# 默认参数
|
|
|
self._default_params = {
|
|
|
'camera_width': 1280,
|
|
|
'camera_height': 960,
|
|
|
"origin_K": np.array([[1086.58411235010, 0.00000000e+00, 656.379279306796],
|
|
|
[0.00000000e+00, 1086.65442545872, 960-489.101890262552],
|
|
|
[0.00000000e+00, 0.00000000e+00, 1.00000000e+00]]),
|
|
|
"undistor_K": np.array([[838.19297892, 0, 658.51580973],
|
|
|
[0, 836.93529706, 960-484.42225009],
|
|
|
[0, 0, 1]]),
|
|
|
"dist_coeffs": np.array([-0.450688803, 0.260760637, 0.000210619624, -0.000669674309, -0.0447846385]),
|
|
|
"focal_length": 3.6,
|
|
|
"height": 1007.0633042608584, # 1.007e+03 - 9
|
|
|
"rotation_alpha": 0.8892272342384955,
|
|
|
"rotation_beta": 0.6786627929104145,
|
|
|
"position_offset_x": 507,
|
|
|
"position_offset_y": 32,
|
|
|
"rotation_camera": 0.03985022053087201,
|
|
|
|
|
|
"filter": True,
|
|
|
"filt_percent": 0.05,
|
|
|
"outlier_num": 10,
|
|
|
"ransac_residual_threshold": 2.5,
|
|
|
"grid_downsample": True,
|
|
|
"cell_size": 10,
|
|
|
"zoom":1.1,
|
|
|
|
|
|
}
|
|
|
|
|
|
# 如果提供了配置文件，则从文件加载参数
|
|
|
if config_path:
|
|
|
self.load_config(config_path)
|
|
|
else:
|
|
|
self._set_parameters(self._default_params)
|
|
|
|
|
|
def _set_parameters(self, params: Dict[str, Any]) -> None:
|
|
|
"""根据参数字典设置成员变量"""
|
|
|
self.camera_width = params["camera_width"]
|
|
|
self.camera_height = params["camera_height"]
|
|
|
self.origin_K = np.array(params["origin_K"])
|
|
|
self.undistor_K = np.array(params["undistor_K"])
|
|
|
self.dist_coeffs = np.array(params["dist_coeffs"])
|
|
|
self.focal_length = params["focal_length"]
|
|
|
self.height = params["height"]
|
|
|
self.rotation_alpha = params["rotation_alpha"]
|
|
|
self.rotation_beta = params["rotation_beta"]
|
|
|
self.position_offset_x = params["position_offset_x"]
|
|
|
self.position_offset_y = params["position_offset_y"]
|
|
|
self.rotation_camera = params["rotation_camera"]
|
|
|
self.filter = params["filter"]
|
|
|
self.filt_percent = params["filt_percent"]
|
|
|
self.outlier_num = params["outlier_num"]
|
|
|
self.ransac_residual_threshold = params["ransac_residual_threshold"]
|
|
|
self.grid_downsample = params["grid_downsample"]
|
|
|
self.cell_size = params["cell_size"]
|
|
|
self.zoom = params["zoom"]
|
|
|
# 计算派生属性
|
|
|
self.pixel_size_x = self.focal_length / self.undistor_K[0, 0]
|
|
|
self.pixel_size_y = self.focal_length / self.undistor_K[1, 1]
|
|
|
self.principal_point = (self.undistor_K[0, 2], self.undistor_K[1, 2])
|
|
|
|
|
|
def load_config(self, file_path: str) -> None:
|
|
|
"""从 JSON 文件加载参数"""
|
|
|
with open(file_path, 'r') as f:
|
|
|
params = json.load(f)
|
|
|
self._set_parameters(params)
|
|
|
|
|
|
def save_config(self, file_path: str) -> None:
|
|
|
"""将当前参数保存到 JSON 文件"""
|
|
|
params = {
|
|
|
"camera_width": self.camera_width,
|
|
|
"camera_height": self.camera_height,
|
|
|
"origin_K": self.origin_K.tolist(),
|
|
|
"undistor_K": self.undistor_K.tolist(),
|
|
|
"dist_coeffs": self.dist_coeffs.tolist(),
|
|
|
"focal_length": self.focal_length,
|
|
|
"height": self.height,
|
|
|
"rotation_alpha": self.rotation_alpha,
|
|
|
"rotation_beta": self.rotation_beta,
|
|
|
"position_offset_x": self.position_offset_x,
|
|
|
"position_offset_y": self.position_offset_y,
|
|
|
"rotation_camera": self.rotation_camera,
|
|
|
"filter": self.filter,
|
|
|
"filt_percent": self.filt_percent,
|
|
|
"outlier_num": self.outlier_num,
|
|
|
"grid_downsample": self.grid_downsample,
|
|
|
"cell_size": self.cell_size,
|
|
|
"ransac_residual_threshold": self.ransac_residual_threshold,
|
|
|
"zoom": self.zoom
|
|
|
}
|
|
|
with open(file_path, 'w') as f:
|
|
|
json.dump(params, f, indent=4)
|
|
|
|
|
|
def update_parameter(self, key: str, value: Any) -> None:
|
|
|
"""动态更新单个参数"""
|
|
|
if hasattr(self, key):
|
|
|
setattr(self, key, value)
|
|
|
# 如果修改了 K 或焦距，需要重新计算派生属性
|
|
|
if key in ["undistor_K", "focal_length"]:
|
|
|
self.pixel_size_x = self.focal_length / self.undistor_K[0, 0]
|
|
|
self.pixel_size_y = self.focal_length / self.undistor_K[1, 1]
|
|
|
self.principal_point = (self.undistor_K[0, 2], self.undistor_K[1, 2])
|
|
|
else:
|
|
|
raise AttributeError(f"参数 {key} 不存在")
|
|
|
|
|
|
# model = CameraModel(None)
|
|
|
# model.save_config("updated_config.json") |
