forked from zbw/yiliao2026
新增俯瞰图变换包，类似车辆环视图
This commit is contained in:
44
src/origincar_birdseye/CMakeLists.txt
Normal file
44
src/origincar_birdseye/CMakeLists.txt
Normal file
@@ -0,0 +1,44 @@
|
|||||||
|
cmake_minimum_required(VERSION 3.8)
|
||||||
|
project(origincar_birdseye)
|
||||||
|
|
||||||
|
if(NOT CMAKE_CXX_STANDARD)
|
||||||
|
set(CMAKE_CXX_STANDARD 17)
|
||||||
|
endif()
|
||||||
|
|
||||||
|
find_package(ament_cmake REQUIRED)
|
||||||
|
find_package(cv_bridge REQUIRED)
|
||||||
|
find_package(geometry_msgs REQUIRED)
|
||||||
|
find_package(OpenCV REQUIRED)
|
||||||
|
find_package(rclcpp REQUIRED)
|
||||||
|
find_package(sensor_msgs REQUIRED)
|
||||||
|
find_package(tf2 REQUIRED)
|
||||||
|
find_package(tf2_geometry_msgs REQUIRED)
|
||||||
|
find_package(tf2_ros REQUIRED)
|
||||||
|
|
||||||
|
add_executable(birdseye_node src/birdseye_node.cpp)
|
||||||
|
target_include_directories(birdseye_node PRIVATE include)
|
||||||
|
ament_target_dependencies(birdseye_node
|
||||||
|
cv_bridge
|
||||||
|
geometry_msgs
|
||||||
|
rclcpp
|
||||||
|
sensor_msgs
|
||||||
|
tf2
|
||||||
|
tf2_geometry_msgs
|
||||||
|
tf2_ros)
|
||||||
|
target_link_libraries(birdseye_node ${OpenCV_LIBS} tf2_geometry_msgs::tf2_geometry_msgs)
|
||||||
|
|
||||||
|
add_executable(synthetic_camera_node src/synthetic_camera_node.cpp)
|
||||||
|
ament_target_dependencies(synthetic_camera_node rclcpp sensor_msgs)
|
||||||
|
|
||||||
|
if(BUILD_TESTING)
|
||||||
|
find_package(ament_cmake_gtest REQUIRED)
|
||||||
|
ament_add_gtest(test_ground_projection test/test_ground_projection.cpp)
|
||||||
|
target_include_directories(test_ground_projection PRIVATE include)
|
||||||
|
endif()
|
||||||
|
|
||||||
|
install(TARGETS birdseye_node synthetic_camera_node DESTINATION lib/${PROJECT_NAME})
|
||||||
|
install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME})
|
||||||
|
install(DIRECTORY include/ DESTINATION include)
|
||||||
|
install(FILES README.md DESTINATION share/${PROJECT_NAME})
|
||||||
|
|
||||||
|
ament_package()
|
||||||
288
src/origincar_birdseye/README.md
Normal file
288
src/origincar_birdseye/README.md
Normal file
@@ -0,0 +1,288 @@
|
|||||||
|
# origincar_birdseye 使用教程
|
||||||
|
|
||||||
|
`origincar_birdseye` 将车头斜向下安装的单目相机图像投影到地面鸟瞰平面，并提供两种输出：
|
||||||
|
|
||||||
|
- 车体视角：车辆固定在画布中，车辆前后移动时地面图像相对车辆流动。
|
||||||
|
- 地图视角：通过 TF 查询 `odom_combined -> base_footprint`，把每帧地面图像放到固定世界画布中。
|
||||||
|
|
||||||
|
运行时不需要棋盘格。棋盘格只在标定阶段用于得到相机内参以及 `camera_from_base_*` 外参。
|
||||||
|
|
||||||
|
## 1. 与 car_usb_cam 的关系
|
||||||
|
|
||||||
|
本包不会修改 `car_usb_cam`，也不会代替它控制相机。联合启动文件只是原样包含：
|
||||||
|
|
||||||
|
```text
|
||||||
|
car_usb_cam/launch/hobot_usb_cam.launch.py
|
||||||
|
```
|
||||||
|
|
||||||
|
并只传递该启动文件当前公开的 `camera_config` 参数。因此相机设备、分辨率、帧率、像素格式以及相机自身的标定矫正仍由下面的文件负责：
|
||||||
|
|
||||||
|
```text
|
||||||
|
car_usb_cam/config/usb_camera.yaml
|
||||||
|
car_usb_cam/config/usb_camera_calibration.yaml
|
||||||
|
```
|
||||||
|
|
||||||
|
当前实车检查到的 `usb_camera.yaml` 配置为：
|
||||||
|
|
||||||
|
```yaml
|
||||||
|
hobot_usb_cam:
|
||||||
|
ros__parameters:
|
||||||
|
frame_id: default_usb_cam
|
||||||
|
framerate: 30
|
||||||
|
image_height: 720
|
||||||
|
image_width: 1280
|
||||||
|
io_method: mmap
|
||||||
|
pixel_format: mjpeg
|
||||||
|
video_device: /dev/video0
|
||||||
|
zero_copy: false
|
||||||
|
```
|
||||||
|
|
||||||
|
`hobot_usb_cam` 根据自己的参数和标定文件产生已矫正图像。当前 `pixel_format: mjpeg` 模式实测在 `/image` 发布 `sensor_msgs/msg/CompressedImage`；本包通过 `image_message_type: compressed` 订阅并解码 MJPEG。若相机改为发布 `sensor_msgs/msg/Image`，只需把该参数改成 `raw`。
|
||||||
|
|
||||||
|
本包默认配置 `input_is_rectified: true`，不会再次执行畸变矫正，避免双重矫正。
|
||||||
|
|
||||||
|
如果以后订阅的是未经矫正的原始图像，应将 `input_is_rectified` 改为 `false`，此时本包才使用 `fx/fy/cx/cy` 和 `distortion_coefficients` 执行 `cv::undistort`。
|
||||||
|
|
||||||
|
## 2. 数据流
|
||||||
|
|
||||||
|
```text
|
||||||
|
/dev/video0
|
||||||
|
-> hobot_usb_cam + usb_camera.yaml + usb_camera_calibration.yaml
|
||||||
|
-> /image（当前为已矫正 sensor_msgs/msg/CompressedImage）
|
||||||
|
-> 本包解码 MJPEG
|
||||||
|
-> birdseye_node
|
||||||
|
-> /birdseye/vehicle/image
|
||||||
|
-> /birdseye/world/image
|
||||||
|
^
|
||||||
|
| TF: odom_combined -> base_footprint
|
||||||
|
```
|
||||||
|
|
||||||
|
世界视角缺少 TF 时不会影响车体视角输出；节点会跳过当前世界视角帧并节流打印警告。
|
||||||
|
|
||||||
|
## 3. 编译
|
||||||
|
|
||||||
|
WSL 开发环境：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
cd /home/hikos/Yiliao2026
|
||||||
|
source /opt/ros/humble/setup.bash
|
||||||
|
colcon build --packages-select origincar_birdseye
|
||||||
|
source install/setup.bash
|
||||||
|
```
|
||||||
|
|
||||||
|
RDKx5 实车环境：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
cd /home/sunrise/yiliao_ws
|
||||||
|
source /opt/tros/humble/setup.bash
|
||||||
|
colcon build --packages-select origincar_birdseye
|
||||||
|
source install/setup.bash
|
||||||
|
```
|
||||||
|
|
||||||
|
## 4. 无摄像头离线验证
|
||||||
|
|
||||||
|
离线启动会发布 YAML 指定尺寸的彩色网格图，并关闭世界视角：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
ros2 launch origincar_birdseye offline_validation.launch.py
|
||||||
|
```
|
||||||
|
|
||||||
|
另开终端检查：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
source /opt/ros/humble/setup.bash
|
||||||
|
source /home/hikos/Yiliao2026/install/setup.bash
|
||||||
|
ros2 topic info /birdseye/vehicle/image
|
||||||
|
ros2 topic echo /birdseye/vehicle/image --once --field width
|
||||||
|
ros2 topic echo /birdseye/vehicle/image --once --field height
|
||||||
|
```
|
||||||
|
|
||||||
|
离线图像发布器的输入话题和尺寸位于 `config/offline_camera.yaml`。它们必须与鸟瞰 YAML 对应参数一致。离线发布器使用原始 `sensor_msgs/msg/Image`，所以 `config/offline_birdseye_overrides.yaml` 会把 `image_message_type` 覆盖为 `raw`，并关闭世界视角。
|
||||||
|
|
||||||
|
## 5. 只启动鸟瞰节点
|
||||||
|
|
||||||
|
相机已由其他启动文件运行时：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
ros2 launch origincar_birdseye birdseye_runtime.launch.py
|
||||||
|
```
|
||||||
|
|
||||||
|
指定另一份鸟瞰配置：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
ros2 launch origincar_birdseye birdseye_runtime.launch.py \
|
||||||
|
birdseye_config:=/absolute/path/to/birdseye.yaml
|
||||||
|
```
|
||||||
|
|
||||||
|
## 6. 联合启动 car_usb_cam 和鸟瞰节点
|
||||||
|
|
||||||
|
使用两个包的默认 YAML：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
ros2 launch origincar_birdseye car_usb_birdseye.launch.py
|
||||||
|
```
|
||||||
|
|
||||||
|
使用自定义相机和鸟瞰配置：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
ros2 launch origincar_birdseye car_usb_birdseye.launch.py \
|
||||||
|
camera_config:=/absolute/path/to/usb_camera.yaml \
|
||||||
|
birdseye_config:=/absolute/path/to/birdseye.yaml
|
||||||
|
```
|
||||||
|
|
||||||
|
`camera_config` 被原样传给 `hobot_usb_cam.launch.py`，本包不会读取或重写它。如果修改了相机输出尺寸，还必须同步修改鸟瞰 YAML 的 `input_width`、`input_height` 和相应内参。
|
||||||
|
|
||||||
|
## 7. 鸟瞰 YAML 参数
|
||||||
|
|
||||||
|
默认文件：`config/birdseye_defaults.yaml`。
|
||||||
|
|
||||||
|
| 参数 | 含义 |
|
||||||
|
|---|---|
|
||||||
|
| `image_topic` | 输入图像话题，可在 YAML 中修改 |
|
||||||
|
| `image_message_type` | `compressed` 订阅 `CompressedImage`；`raw` 订阅 `Image` |
|
||||||
|
| `input_width` / `input_height` | 标定所对应的输入图像尺寸；每帧会严格校验 |
|
||||||
|
| `input_is_rectified` | `true` 表示上游已经矫正，本包不再去畸变 |
|
||||||
|
| `vehicle_output_topic` | 车体视角输出话题 |
|
||||||
|
| `world_output_topic` | 地图视角输出话题 |
|
||||||
|
| `base_frame` | 车辆基准坐标系，默认 `base_footprint` |
|
||||||
|
| `world_frame` | 世界坐标系，默认 `odom_combined` |
|
||||||
|
| `enable_world_view` | 是否生成地图视角 |
|
||||||
|
| `fx/fy/cx/cy` | 与实际输入尺寸对应的相机内参 |
|
||||||
|
| `distortion_coefficients` | 原始图像畸变参数；仅 `input_is_rectified: false` 时使用 |
|
||||||
|
| `camera_from_base_rotation` | 从车体坐标到相机坐标的 3x3 旋转矩阵，按行展开 |
|
||||||
|
| `camera_from_base_translation` | 从车体坐标到相机坐标的平移向量，单位米 |
|
||||||
|
| `output_width/height` | 车体视角输出像素尺寸 |
|
||||||
|
| `meters_per_pixel` | 每像素代表的米数，两个输出共用 |
|
||||||
|
| `rear_meters` | 车体原点距车体视角画布下边缘的距离 |
|
||||||
|
| `left_meters` | 车体视角画布左边界对应的车体左侧距离 |
|
||||||
|
| `world_origin_x/y` | 世界画布局部原点在 `world_frame` 中的位置 |
|
||||||
|
| `world_width/height` | 世界视角输出像素尺寸 |
|
||||||
|
| `world_left_meters` | 世界局部原点距画布左边缘的距离 |
|
||||||
|
| `world_rear_meters` | 世界局部原点距画布下边缘的距离 |
|
||||||
|
|
||||||
|
输入话题和尺寸只需修改 YAML，例如：
|
||||||
|
|
||||||
|
```yaml
|
||||||
|
birdseye_node:
|
||||||
|
ros__parameters:
|
||||||
|
image_topic: /front_camera/rectified_image
|
||||||
|
image_message_type: compressed
|
||||||
|
input_width: 1920
|
||||||
|
input_height: 1080
|
||||||
|
input_is_rectified: true
|
||||||
|
```
|
||||||
|
|
||||||
|
注意：改变话题只会改变鸟瞰节点的订阅目标，不会擅自改变 `car_usb_cam` 的发布话题。上游必须已经发布同名话题，或由系统级 ROS remap 将其映射到该名称。
|
||||||
|
|
||||||
|
## 8. 分辨率与标定的约束
|
||||||
|
|
||||||
|
下面四项必须一致：
|
||||||
|
|
||||||
|
1. `car_usb_cam` 实际发布图像的宽高；
|
||||||
|
2. `birdseye_defaults.yaml` 的 `input_width/input_height`；
|
||||||
|
3. `fx/fy/cx/cy` 所属的标定分辨率；
|
||||||
|
4. `camera_from_base_*` 标定时所使用的成像模式和安装姿态。
|
||||||
|
|
||||||
|
收到尺寸不一致的图像时，本包不会继续投影，而是打印类似错误：
|
||||||
|
|
||||||
|
```text
|
||||||
|
input image size mismatch: expected 1280x720, received 800x450
|
||||||
|
```
|
||||||
|
|
||||||
|
默认内参是把项目中原来的 `800x450` 内参按相同 16:9 视场线性缩放到 `1280x720` 的过渡值。它可用于软件联调，但实车最终精度应以同一相机模式下重新确认的矫正后内参为准。
|
||||||
|
|
||||||
|
## 9. 标定到运行的流程
|
||||||
|
|
||||||
|
1. 固定相机在车头，铺设地面棋盘格。
|
||||||
|
2. 用最终运行时相同的 `car_usb_cam` 分辨率、格式和矫正设置采集图像。
|
||||||
|
3. 获取矫正后图像对应的 `fx/fy/cx/cy`。
|
||||||
|
4. 运行 `camera_to_base_calibrator`，由棋盘格计算固定的 `T_camera_from_base`。
|
||||||
|
5. 把结果写入 `birdseye_defaults.yaml`，检查实时鸟瞰图中的直线、比例和车辆位置。
|
||||||
|
6. 固定结果后移除棋盘格；运行阶段只需要相机图像和地图模式所需的 TF。
|
||||||
|
|
||||||
|
`camera_to_base_calibrator` 的职责只是求相机相对于车体/地面的固定外参。它不需要在正常行驶时持续运行；相机安装没有松动时也不需要反复拍棋盘格。
|
||||||
|
|
||||||
|
## 10. 两种运行模式
|
||||||
|
|
||||||
|
### 车体居中模式
|
||||||
|
|
||||||
|
设置：
|
||||||
|
|
||||||
|
```yaml
|
||||||
|
enable_world_view: false
|
||||||
|
```
|
||||||
|
|
||||||
|
使用 `/birdseye/vehicle/image`。车辆矩形始终位于固定像素位置，地面随车辆运动向后或向前流动。
|
||||||
|
|
||||||
|
### 地图模式
|
||||||
|
|
||||||
|
设置：
|
||||||
|
|
||||||
|
```yaml
|
||||||
|
enable_world_view: true
|
||||||
|
world_frame: odom_combined
|
||||||
|
base_frame: base_footprint
|
||||||
|
```
|
||||||
|
|
||||||
|
系统必须提供可查询的 TF：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
ros2 run tf2_ros tf2_echo odom_combined base_footprint
|
||||||
|
```
|
||||||
|
|
||||||
|
节点按每帧图像时间戳查询车辆位置和航向，再把该帧映射到 `/birdseye/world/image`。
|
||||||
|
|
||||||
|
## 11. 话题检查
|
||||||
|
|
||||||
|
```bash
|
||||||
|
ros2 topic list
|
||||||
|
ros2 topic info /image -v
|
||||||
|
ros2 topic hz /image
|
||||||
|
ros2 topic info /birdseye/vehicle/image -v
|
||||||
|
ros2 topic info /birdseye/world/image -v
|
||||||
|
```
|
||||||
|
|
||||||
|
主要话题：
|
||||||
|
|
||||||
|
| 默认话题 | 类型 | 方向 |
|
||||||
|
|---|---|---|
|
||||||
|
| `/image` | 当前为 `sensor_msgs/msg/CompressedImage` | 输入 |
|
||||||
|
| `/birdseye/vehicle/image` | `sensor_msgs/msg/Image` | 输出 |
|
||||||
|
| `/birdseye/world/image` | `sensor_msgs/msg/Image` | 输出 |
|
||||||
|
| `/tf`、`/tf_static` | `tf2_msgs/msg/TFMessage` | 地图模式输入 |
|
||||||
|
|
||||||
|
## 12. 测试
|
||||||
|
|
||||||
|
```bash
|
||||||
|
cd /home/hikos/Yiliao2026
|
||||||
|
source /opt/ros/humble/setup.bash
|
||||||
|
colcon build --packages-select origincar_birdseye
|
||||||
|
colcon test --packages-select origincar_birdseye
|
||||||
|
colcon test-result --test-result-base build/origincar_birdseye --verbose
|
||||||
|
```
|
||||||
|
|
||||||
|
## 13. 常见问题
|
||||||
|
|
||||||
|
### 没有收到图像
|
||||||
|
|
||||||
|
确认 `image_topic` 与实际话题完全一致，并用 `ros2 topic info <话题> -v` 检查类型。发布 `sensor_msgs/msg/CompressedImage` 时使用 `image_message_type: compressed`；发布 `sensor_msgs/msg/Image` 时使用 `raw`。WebSocket 数据不能作为输入。
|
||||||
|
|
||||||
|
### 报输入尺寸不一致
|
||||||
|
|
||||||
|
原始 `Image` 可直接查看 `width/height`；`CompressedImage` 不携带独立宽高字段，本包解码第一帧后会打印 `accepted input frame size=...`。让相机配置、鸟瞰输入尺寸和内参使用同一分辨率，不能只改宽高而保留另一分辨率的内参。
|
||||||
|
|
||||||
|
### 鸟瞰图扭曲或地面直线不直
|
||||||
|
|
||||||
|
检查是否重复矫正、内参是否对应当前分辨率，以及 `camera_from_base_*` 是否来自相机当前安装姿态。使用 `car_usb_cam` 已矫正输出时应保持 `input_is_rectified: true`。
|
||||||
|
|
||||||
|
### 车体视角正常但没有世界视角
|
||||||
|
|
||||||
|
检查 `enable_world_view`，然后确认图像时间戳处存在 `odom_combined -> base_footprint` TF。没有 TF 时节点会明确输出 `world view skipped`。
|
||||||
|
|
||||||
|
### `/dev/video0` 打不开
|
||||||
|
|
||||||
|
通常是另一个相机进程占用了设备。不要同时启动两份 `hobot_usb_cam`；联合启动前先停止旧相机节点。
|
||||||
|
|
||||||
|
### 输出大面积黑色
|
||||||
|
|
||||||
|
检查外参旋转/平移方向、相机是否真的朝向地面，以及参考地面范围是否落在相机 FOV 内。当前默认外参只是离线近似值，实车必须使用 `camera_to_base_calibrator` 的最终结果。
|
||||||
41
src/origincar_birdseye/config/birdseye_defaults.yaml
Normal file
41
src/origincar_birdseye/config/birdseye_defaults.yaml
Normal file
@@ -0,0 +1,41 @@
|
|||||||
|
birdseye_node:
|
||||||
|
ros__parameters:
|
||||||
|
image_topic: /image
|
||||||
|
# Current car_usb_cam MJPEG mode publishes sensor_msgs/msg/CompressedImage.
|
||||||
|
# Use "raw" when the selected upstream publisher uses sensor_msgs/msg/Image.
|
||||||
|
image_message_type: compressed
|
||||||
|
# Must match the image actually published by car_usb_cam and the calibration matrix below.
|
||||||
|
input_width: 1280
|
||||||
|
input_height: 720
|
||||||
|
# hobot_usb_cam applies its configured camera calibration before publishing /image.
|
||||||
|
input_is_rectified: true
|
||||||
|
vehicle_output_topic: /birdseye/vehicle/image
|
||||||
|
world_output_topic: /birdseye/world/image
|
||||||
|
base_frame: base_footprint
|
||||||
|
world_frame: odom_combined
|
||||||
|
enable_world_view: true
|
||||||
|
|
||||||
|
# Scaled from the existing 800x450 intrinsics to the current 1280x720 stream.
|
||||||
|
# Replace these values with the final rectified 1280x720 calibration when available.
|
||||||
|
fx: 450.55244482
|
||||||
|
fy: 457.08553096
|
||||||
|
cx: 583.36914605
|
||||||
|
cy: 327.88183768
|
||||||
|
distortion_coefficients: [0.03532389, -0.05576607, 0.00027529, 0.00241747, 0.01024998]
|
||||||
|
|
||||||
|
# Approximate T_camera_from_base for offline validation. Replace with calibrator output on the vehicle.
|
||||||
|
camera_from_base_rotation: [0.0, -1.0, 0.0, -0.70710678, 0.0, -0.70710678, 0.70710678, 0.0, -0.70710678]
|
||||||
|
camera_from_base_translation: [0.0, 0.28284271, 0.14142136]
|
||||||
|
|
||||||
|
output_width: 600
|
||||||
|
output_height: 800
|
||||||
|
meters_per_pixel: 0.01
|
||||||
|
rear_meters: 2.0
|
||||||
|
left_meters: 3.0
|
||||||
|
|
||||||
|
world_origin_x: 0.0
|
||||||
|
world_origin_y: 0.0
|
||||||
|
world_width: 800
|
||||||
|
world_height: 800
|
||||||
|
world_left_meters: 4.0
|
||||||
|
world_rear_meters: 4.0
|
||||||
@@ -0,0 +1,4 @@
|
|||||||
|
birdseye_node:
|
||||||
|
ros__parameters:
|
||||||
|
image_message_type: raw
|
||||||
|
enable_world_view: false
|
||||||
5
src/origincar_birdseye/config/offline_camera.yaml
Normal file
5
src/origincar_birdseye/config/offline_camera.yaml
Normal file
@@ -0,0 +1,5 @@
|
|||||||
|
synthetic_camera_node:
|
||||||
|
ros__parameters:
|
||||||
|
image_topic: /image
|
||||||
|
input_width: 1280
|
||||||
|
input_height: 720
|
||||||
@@ -0,0 +1,80 @@
|
|||||||
|
# car_usb_cam YAML Adaptation Implementation Plan
|
||||||
|
|
||||||
|
> **For agentic workers:** REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (`- [ ]`) syntax for tracking.
|
||||||
|
|
||||||
|
**Goal:** Make the bird's-eye node accept its image topic and expected input size from YAML, consume the already-rectified output of the current `car_usb_cam`, and document the complete workflow.
|
||||||
|
|
||||||
|
**Architecture:** `car_usb_cam` remains an independent upstream package and is included through its public `camera_config` launch argument without modification. `origincar_birdseye` loads all of its own parameters from `birdseye_defaults.yaml`, validates every received frame against the configured calibration size, and optionally skips local undistortion when the upstream image is already rectified.
|
||||||
|
|
||||||
|
**Tech Stack:** ROS 2 Humble, C++17, OpenCV, ROS 2 Python launch, YAML, ament/GoogleTest.
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
### Task 1: Testable input-size validation
|
||||||
|
|
||||||
|
**Files:**
|
||||||
|
- Create: `include/origincar_birdseye/image_validation.hpp`
|
||||||
|
- Modify: `test/test_ground_projection.cpp`
|
||||||
|
|
||||||
|
- [ ] Add tests showing that an exact configured size succeeds and a mismatched frame returns a diagnostic containing both sizes.
|
||||||
|
- [ ] Run `colcon test --packages-select origincar_birdseye` and verify the new test fails before implementation.
|
||||||
|
- [ ] Add the minimal pure validation helper.
|
||||||
|
- [ ] Rebuild and verify the unit test passes.
|
||||||
|
|
||||||
|
### Task 2: YAML-controlled runtime input
|
||||||
|
|
||||||
|
**Files:**
|
||||||
|
- Modify: `src/birdseye_node.cpp`
|
||||||
|
- Modify: `config/birdseye_defaults.yaml`
|
||||||
|
|
||||||
|
- [ ] Declare `input_width`, `input_height`, and `input_is_rectified` as node parameters.
|
||||||
|
- [ ] Reject non-positive configured dimensions during construction.
|
||||||
|
- [ ] Store the configured dimensions in `CameraIntrinsics` and reject mismatching frames with a throttled error before projection.
|
||||||
|
- [ ] Skip `cv::undistort` when `input_is_rectified: true`; retain the optional path for a raw camera source.
|
||||||
|
- [ ] Configure the current `car_usb_cam` defaults as `/image`, `1280`, `720`, and already rectified.
|
||||||
|
|
||||||
|
### Task 3: Current car_usb_cam launch compatibility
|
||||||
|
|
||||||
|
**Files:**
|
||||||
|
- Modify: `launch/car_usb_birdseye.launch.py`
|
||||||
|
- Modify: `launch/birdseye_runtime.launch.py`
|
||||||
|
|
||||||
|
- [ ] Replace obsolete `usb_*` launch arguments with the current single `camera_config` argument.
|
||||||
|
- [ ] Add `camera_config` and `birdseye_config` launch arguments with package defaults.
|
||||||
|
- [ ] Include `car_usb_cam/launch/hobot_usb_cam.launch.py` unchanged and pass only `camera_config`.
|
||||||
|
- [ ] Start the bird's-eye node using `birdseye_config`; do not modify or remap the camera package internally.
|
||||||
|
- [ ] Verify both launch files are importable and expose the documented arguments.
|
||||||
|
|
||||||
|
### Task 4: Package tutorial
|
||||||
|
|
||||||
|
**Files:**
|
||||||
|
- Create: `README.md`
|
||||||
|
|
||||||
|
- [ ] Document architecture, current car configuration, prerequisites, build commands, offline validation, bird's-eye-only launch, joint real-camera launch, and YAML overrides.
|
||||||
|
- [ ] Document every input/output topic and parameter, vehicle/world coordinate behavior, upstream rectification, calibration-size constraints, checkerboard-to-runtime workflow, and troubleshooting.
|
||||||
|
- [ ] State explicitly that `car_usb_cam` is not modified and its camera YAML/calibration remain authoritative for capture and upstream rectification.
|
||||||
|
|
||||||
|
### Task 5: Local and robot verification
|
||||||
|
|
||||||
|
**Files:**
|
||||||
|
- Verify only; no planned source additions.
|
||||||
|
|
||||||
|
- [ ] Build the package in `/home/hikos/Yiliao2026` and run all package tests with full failure output.
|
||||||
|
- [ ] Run the synthetic offline pipeline and verify `/birdseye/vehicle/image` is published at the configured output size.
|
||||||
|
- [ ] Copy only `origincar_birdseye` to `/home/sunrise/yiliao_ws/src/`, preserving `car_usb_cam`.
|
||||||
|
- [ ] Build and test on RDKx5.
|
||||||
|
- [ ] Start the existing real camera through the joint launch, verify the current MJPEG `/image` is `sensor_msgs/msg/CompressedImage` decoded as `1280x720`, verify bird's-eye output, then stop the launch and confirm `/dev/video0` is released.
|
||||||
|
|
||||||
|
### Task 6: MJPEG message compatibility discovered on the vehicle
|
||||||
|
|
||||||
|
**Files:**
|
||||||
|
- Modify: `include/origincar_birdseye/image_validation.hpp`
|
||||||
|
- Modify: `src/birdseye_node.cpp`
|
||||||
|
- Modify: `config/birdseye_defaults.yaml`
|
||||||
|
- Create: `config/offline_birdseye_overrides.yaml`
|
||||||
|
- Modify: `README.md`
|
||||||
|
|
||||||
|
- [ ] Test and validate the YAML value `image_message_type` as `raw` or `compressed`.
|
||||||
|
- [ ] Subscribe to `sensor_msgs/msg/CompressedImage` for the current MJPEG camera configuration and decode it before the common processing path.
|
||||||
|
- [ ] Retain configurable `sensor_msgs/msg/Image` support for raw publishers.
|
||||||
|
- [ ] Verify the decoded frame is `1280x720` and the vehicle output is published on the robot.
|
||||||
@@ -0,0 +1,42 @@
|
|||||||
|
#ifndef ORIGINCAR_BIRDSEYE__BIRDSEYE_GEOMETRY_HPP_
|
||||||
|
#define ORIGINCAR_BIRDSEYE__BIRDSEYE_GEOMETRY_HPP_
|
||||||
|
|
||||||
|
#include <optional>
|
||||||
|
|
||||||
|
#include "origincar_birdseye/ground_projection.hpp"
|
||||||
|
|
||||||
|
namespace origincar_birdseye
|
||||||
|
{
|
||||||
|
|
||||||
|
struct BirdseyeCanvas
|
||||||
|
{
|
||||||
|
int width;
|
||||||
|
int height;
|
||||||
|
double meters_per_pixel;
|
||||||
|
double rear_meters;
|
||||||
|
double left_meters;
|
||||||
|
};
|
||||||
|
|
||||||
|
class BirdseyeGeometry
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
explicit BirdseyeGeometry(BirdseyeCanvas canvas)
|
||||||
|
: canvas_(canvas)
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
std::optional<PixelPoint> basePointToCanvasPixel(const GroundPoint & point) const
|
||||||
|
{
|
||||||
|
const PixelPoint pixel{
|
||||||
|
(canvas_.left_meters - point.y) / canvas_.meters_per_pixel,
|
||||||
|
(canvas_.rear_meters - point.x) / canvas_.meters_per_pixel};
|
||||||
|
return pixel;
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
BirdseyeCanvas canvas_;
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace origincar_birdseye
|
||||||
|
|
||||||
|
#endif // ORIGINCAR_BIRDSEYE__BIRDSEYE_GEOMETRY_HPP_
|
||||||
@@ -0,0 +1,65 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <algorithm>
|
||||||
|
#include <cmath>
|
||||||
|
#include <optional>
|
||||||
|
#include <string>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
|
#include "origincar_birdseye/birdseye_geometry.hpp"
|
||||||
|
#include "origincar_birdseye/ground_projection.hpp"
|
||||||
|
|
||||||
|
namespace origincar_birdseye
|
||||||
|
{
|
||||||
|
|
||||||
|
inline bool allFinite(const std::vector<double> & values)
|
||||||
|
{
|
||||||
|
return std::all_of(
|
||||||
|
values.begin(), values.end(), [](double value) {
|
||||||
|
return std::isfinite(value);
|
||||||
|
});
|
||||||
|
}
|
||||||
|
|
||||||
|
inline std::optional<std::string> validateCameraConfiguration(
|
||||||
|
const CameraIntrinsics & intrinsics, const std::vector<double> & rotation,
|
||||||
|
const std::vector<double> & translation)
|
||||||
|
{
|
||||||
|
if (intrinsics.width <= 0 || intrinsics.height <= 0 ||
|
||||||
|
!std::isfinite(intrinsics.fx) || !std::isfinite(intrinsics.fy) ||
|
||||||
|
!std::isfinite(intrinsics.cx) || !std::isfinite(intrinsics.cy) ||
|
||||||
|
intrinsics.fx <= 0.0 || intrinsics.fy <= 0.0)
|
||||||
|
{
|
||||||
|
return "camera intrinsics and input dimensions must be finite and positive";
|
||||||
|
}
|
||||||
|
if (rotation.size() != 9 || translation.size() != 3 ||
|
||||||
|
!allFinite(rotation) || !allFinite(translation))
|
||||||
|
{
|
||||||
|
return "T_camera_from_base must contain finite 3x3 rotation and 3D translation values";
|
||||||
|
}
|
||||||
|
return std::nullopt;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline std::optional<std::string> validateCanvas(
|
||||||
|
const BirdseyeCanvas & canvas, const std::string & name)
|
||||||
|
{
|
||||||
|
if (canvas.width <= 0 || canvas.height <= 0) {
|
||||||
|
return name + " canvas width and height must be positive";
|
||||||
|
}
|
||||||
|
if (!std::isfinite(canvas.meters_per_pixel) || canvas.meters_per_pixel <= 0.0 ||
|
||||||
|
!std::isfinite(canvas.rear_meters) || !std::isfinite(canvas.left_meters))
|
||||||
|
{
|
||||||
|
return name + " canvas metric parameters must be finite and meters_per_pixel must be positive";
|
||||||
|
}
|
||||||
|
return std::nullopt;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline std::optional<std::string> validateCoefficientVector(
|
||||||
|
const std::vector<double> & values, size_t minimum_size, const std::string & name)
|
||||||
|
{
|
||||||
|
if (values.size() < minimum_size || !allFinite(values)) {
|
||||||
|
return name + " must contain the required number of finite values";
|
||||||
|
}
|
||||||
|
return std::nullopt;
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace origincar_birdseye
|
||||||
@@ -0,0 +1,81 @@
|
|||||||
|
#ifndef ORIGINCAR_BIRDSEYE__GROUND_PROJECTION_HPP_
|
||||||
|
#define ORIGINCAR_BIRDSEYE__GROUND_PROJECTION_HPP_
|
||||||
|
|
||||||
|
#include <array>
|
||||||
|
#include <optional>
|
||||||
|
|
||||||
|
namespace origincar_birdseye
|
||||||
|
{
|
||||||
|
|
||||||
|
struct CameraIntrinsics
|
||||||
|
{
|
||||||
|
double fx;
|
||||||
|
double fy;
|
||||||
|
double cx;
|
||||||
|
double cy;
|
||||||
|
int width;
|
||||||
|
int height;
|
||||||
|
};
|
||||||
|
|
||||||
|
struct GroundPoint
|
||||||
|
{
|
||||||
|
double x;
|
||||||
|
double y;
|
||||||
|
};
|
||||||
|
|
||||||
|
struct PixelPoint
|
||||||
|
{
|
||||||
|
double x;
|
||||||
|
double y;
|
||||||
|
};
|
||||||
|
|
||||||
|
class RigidTransform
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
RigidTransform(std::array<double, 9> rotation, std::array<double, 3> translation)
|
||||||
|
: rotation_(rotation), translation_(translation)
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
std::array<double, 3> apply(const std::array<double, 3> & point) const
|
||||||
|
{
|
||||||
|
return {
|
||||||
|
rotation_[0] * point[0] + rotation_[1] * point[1] + rotation_[2] * point[2] + translation_[0],
|
||||||
|
rotation_[3] * point[0] + rotation_[4] * point[1] + rotation_[5] * point[2] + translation_[1],
|
||||||
|
rotation_[6] * point[0] + rotation_[7] * point[1] + rotation_[8] * point[2] +
|
||||||
|
translation_[2]};
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
std::array<double, 9> rotation_;
|
||||||
|
std::array<double, 3> translation_;
|
||||||
|
};
|
||||||
|
|
||||||
|
class GroundProjection
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
GroundProjection(CameraIntrinsics intrinsics, RigidTransform camera_from_base)
|
||||||
|
: intrinsics_(intrinsics), camera_from_base_(camera_from_base)
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
std::optional<PixelPoint> projectBaseGroundPoint(const GroundPoint & point) const
|
||||||
|
{
|
||||||
|
const auto camera_point = camera_from_base_.apply({point.x, point.y, 0.0});
|
||||||
|
if (camera_point[2] <= 0.0) {
|
||||||
|
return std::nullopt;
|
||||||
|
}
|
||||||
|
|
||||||
|
return PixelPoint{
|
||||||
|
intrinsics_.fx * camera_point[0] / camera_point[2] + intrinsics_.cx,
|
||||||
|
intrinsics_.fy * camera_point[1] / camera_point[2] + intrinsics_.cy};
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
CameraIntrinsics intrinsics_;
|
||||||
|
RigidTransform camera_from_base_;
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace origincar_birdseye
|
||||||
|
|
||||||
|
#endif // ORIGINCAR_BIRDSEYE__GROUND_PROJECTION_HPP_
|
||||||
@@ -0,0 +1,31 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <optional>
|
||||||
|
#include <sstream>
|
||||||
|
#include <string>
|
||||||
|
|
||||||
|
namespace origincar_birdseye
|
||||||
|
{
|
||||||
|
|
||||||
|
inline std::optional<std::string> validateImageSize(
|
||||||
|
int expected_width, int expected_height, int actual_width, int actual_height)
|
||||||
|
{
|
||||||
|
if (expected_width == actual_width && expected_height == actual_height) {
|
||||||
|
return std::nullopt;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::ostringstream message;
|
||||||
|
message << "input image size mismatch: expected " << expected_width << 'x' << expected_height
|
||||||
|
<< ", received " << actual_width << 'x' << actual_height;
|
||||||
|
return message.str();
|
||||||
|
}
|
||||||
|
|
||||||
|
inline std::optional<std::string> validateImageMessageType(const std::string & value)
|
||||||
|
{
|
||||||
|
if (value == "raw" || value == "compressed") {
|
||||||
|
return std::nullopt;
|
||||||
|
}
|
||||||
|
return "image_message_type must be raw or compressed";
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace origincar_birdseye
|
||||||
26
src/origincar_birdseye/launch/birdseye_runtime.launch.py
Normal file
26
src/origincar_birdseye/launch/birdseye_runtime.launch.py
Normal file
@@ -0,0 +1,26 @@
|
|||||||
|
from ament_index_python.packages import get_package_share_directory
|
||||||
|
from launch import LaunchDescription
|
||||||
|
from launch.actions import DeclareLaunchArgument
|
||||||
|
from launch.substitutions import LaunchConfiguration
|
||||||
|
from launch_ros.actions import Node
|
||||||
|
|
||||||
|
import os
|
||||||
|
|
||||||
|
|
||||||
|
def generate_launch_description():
|
||||||
|
default_config = os.path.join(
|
||||||
|
get_package_share_directory('origincar_birdseye'),
|
||||||
|
'config',
|
||||||
|
'birdseye_defaults.yaml')
|
||||||
|
return LaunchDescription([
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
'birdseye_config',
|
||||||
|
default_value=default_config,
|
||||||
|
description='Birdseye node ROS parameter YAML'),
|
||||||
|
Node(
|
||||||
|
package='origincar_birdseye',
|
||||||
|
executable='birdseye_node',
|
||||||
|
name='birdseye_node',
|
||||||
|
output='screen',
|
||||||
|
parameters=[LaunchConfiguration('birdseye_config')])
|
||||||
|
])
|
||||||
40
src/origincar_birdseye/launch/car_usb_birdseye.launch.py
Normal file
40
src/origincar_birdseye/launch/car_usb_birdseye.launch.py
Normal file
@@ -0,0 +1,40 @@
|
|||||||
|
import os
|
||||||
|
|
||||||
|
from ament_index_python.packages import get_package_share_directory
|
||||||
|
from launch import LaunchDescription
|
||||||
|
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
|
||||||
|
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||||
|
from launch.substitutions import LaunchConfiguration
|
||||||
|
from launch_ros.actions import Node
|
||||||
|
|
||||||
|
|
||||||
|
def generate_launch_description():
|
||||||
|
birdseye_share = get_package_share_directory('origincar_birdseye')
|
||||||
|
camera_share = get_package_share_directory('car_usb_cam')
|
||||||
|
default_birdseye_config = os.path.join(
|
||||||
|
birdseye_share, 'config', 'birdseye_defaults.yaml')
|
||||||
|
default_camera_config = os.path.join(
|
||||||
|
camera_share, 'config', 'usb_camera.yaml')
|
||||||
|
camera_launch = os.path.join(camera_share, 'launch', 'hobot_usb_cam.launch.py')
|
||||||
|
|
||||||
|
return LaunchDescription([
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
'camera_config',
|
||||||
|
default_value=default_camera_config,
|
||||||
|
description='Path passed unchanged to car_usb_cam'),
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
'birdseye_config',
|
||||||
|
default_value=default_birdseye_config,
|
||||||
|
description='Birdseye node ROS parameter YAML'),
|
||||||
|
IncludeLaunchDescription(
|
||||||
|
PythonLaunchDescriptionSource(camera_launch),
|
||||||
|
launch_arguments={
|
||||||
|
'camera_config': LaunchConfiguration('camera_config'),
|
||||||
|
}.items()),
|
||||||
|
Node(
|
||||||
|
package='origincar_birdseye',
|
||||||
|
executable='birdseye_node',
|
||||||
|
name='birdseye_node',
|
||||||
|
output='screen',
|
||||||
|
parameters=[LaunchConfiguration('birdseye_config')]),
|
||||||
|
])
|
||||||
42
src/origincar_birdseye/launch/offline_validation.launch.py
Normal file
42
src/origincar_birdseye/launch/offline_validation.launch.py
Normal file
@@ -0,0 +1,42 @@
|
|||||||
|
import os
|
||||||
|
|
||||||
|
from ament_index_python.packages import get_package_share_directory
|
||||||
|
from launch import LaunchDescription
|
||||||
|
from launch.actions import DeclareLaunchArgument
|
||||||
|
from launch.substitutions import LaunchConfiguration
|
||||||
|
from launch_ros.actions import Node
|
||||||
|
|
||||||
|
|
||||||
|
def generate_launch_description():
|
||||||
|
share = get_package_share_directory('origincar_birdseye')
|
||||||
|
birdseye_config = os.path.join(
|
||||||
|
share,
|
||||||
|
'config',
|
||||||
|
'birdseye_defaults.yaml')
|
||||||
|
synthetic_config = os.path.join(share, 'config', 'offline_camera.yaml')
|
||||||
|
birdseye_overrides = os.path.join(
|
||||||
|
share, 'config', 'offline_birdseye_overrides.yaml')
|
||||||
|
return LaunchDescription([
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
'birdseye_config',
|
||||||
|
default_value=birdseye_config,
|
||||||
|
description='Birdseye node ROS parameter YAML'),
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
'synthetic_config',
|
||||||
|
default_value=synthetic_config,
|
||||||
|
description='Synthetic image publisher ROS parameter YAML'),
|
||||||
|
Node(
|
||||||
|
package='origincar_birdseye',
|
||||||
|
executable='synthetic_camera_node',
|
||||||
|
name='synthetic_camera_node',
|
||||||
|
output='screen',
|
||||||
|
parameters=[LaunchConfiguration('synthetic_config')]),
|
||||||
|
Node(
|
||||||
|
package='origincar_birdseye',
|
||||||
|
executable='birdseye_node',
|
||||||
|
name='birdseye_node',
|
||||||
|
output='screen',
|
||||||
|
parameters=[
|
||||||
|
LaunchConfiguration('birdseye_config'),
|
||||||
|
birdseye_overrides]),
|
||||||
|
])
|
||||||
27
src/origincar_birdseye/package.xml
Normal file
27
src/origincar_birdseye/package.xml
Normal file
@@ -0,0 +1,27 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<package format="3">
|
||||||
|
<name>origincar_birdseye</name>
|
||||||
|
<version>0.1.0</version>
|
||||||
|
<description>Single front camera ground-plane bird's-eye projection for Origincar.</description>
|
||||||
|
<maintainer email="2314753575@qq.com">sunrise</maintainer>
|
||||||
|
<license>Apache-2.0</license>
|
||||||
|
|
||||||
|
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||||
|
<depend>cv_bridge</depend>
|
||||||
|
<depend>geometry_msgs</depend>
|
||||||
|
<depend>libopencv-dev</depend>
|
||||||
|
<depend>rclcpp</depend>
|
||||||
|
<depend>sensor_msgs</depend>
|
||||||
|
<depend>tf2</depend>
|
||||||
|
<depend>tf2_geometry_msgs</depend>
|
||||||
|
<depend>tf2_ros</depend>
|
||||||
|
<exec_depend>ament_index_python</exec_depend>
|
||||||
|
<exec_depend>car_usb_cam</exec_depend>
|
||||||
|
<exec_depend>launch</exec_depend>
|
||||||
|
<exec_depend>launch_ros</exec_depend>
|
||||||
|
<test_depend>ament_cmake_gtest</test_depend>
|
||||||
|
|
||||||
|
<export>
|
||||||
|
<build_type>ament_cmake</build_type>
|
||||||
|
</export>
|
||||||
|
</package>
|
||||||
340
src/origincar_birdseye/src/birdseye_node.cpp
Normal file
340
src/origincar_birdseye/src/birdseye_node.cpp
Normal file
@@ -0,0 +1,340 @@
|
|||||||
|
#include <array>
|
||||||
|
#include <cmath>
|
||||||
|
#include <functional>
|
||||||
|
#include <memory>
|
||||||
|
#include <optional>
|
||||||
|
#include <stdexcept>
|
||||||
|
#include <string>
|
||||||
|
#include <utility>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
|
#include <cv_bridge/cv_bridge.h>
|
||||||
|
#include <geometry_msgs/msg/transform_stamped.hpp>
|
||||||
|
#include <opencv2/calib3d.hpp>
|
||||||
|
#include <opencv2/imgcodecs.hpp>
|
||||||
|
#include <opencv2/imgproc.hpp>
|
||||||
|
#include <rclcpp/rclcpp.hpp>
|
||||||
|
#include <sensor_msgs/image_encodings.hpp>
|
||||||
|
#include <sensor_msgs/msg/compressed_image.hpp>
|
||||||
|
#include <sensor_msgs/msg/image.hpp>
|
||||||
|
#include <tf2/utils.h>
|
||||||
|
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
|
||||||
|
#include <tf2_ros/buffer.h>
|
||||||
|
#include <tf2_ros/transform_listener.h>
|
||||||
|
|
||||||
|
#include "origincar_birdseye/birdseye_geometry.hpp"
|
||||||
|
#include "origincar_birdseye/configuration_validation.hpp"
|
||||||
|
#include "origincar_birdseye/ground_projection.hpp"
|
||||||
|
#include "origincar_birdseye/image_validation.hpp"
|
||||||
|
|
||||||
|
namespace origincar_birdseye
|
||||||
|
{
|
||||||
|
|
||||||
|
class BirdseyeNode : public rclcpp::Node
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
BirdseyeNode()
|
||||||
|
: Node("birdseye_node"),
|
||||||
|
tf_buffer_(this->get_clock()),
|
||||||
|
tf_listener_(tf_buffer_)
|
||||||
|
{
|
||||||
|
const auto image_topic = this->declare_parameter<std::string>("image_topic", "/image");
|
||||||
|
const auto image_message_type = this->declare_parameter<std::string>(
|
||||||
|
"image_message_type", "raw");
|
||||||
|
if (const auto error = validateImageMessageType(image_message_type)) {
|
||||||
|
throw std::runtime_error(*error);
|
||||||
|
}
|
||||||
|
const auto vehicle_output_topic = this->declare_parameter<std::string>(
|
||||||
|
"vehicle_output_topic", "/birdseye/vehicle/image");
|
||||||
|
const auto world_output_topic = this->declare_parameter<std::string>(
|
||||||
|
"world_output_topic", "/birdseye/world/image");
|
||||||
|
base_frame_ = this->declare_parameter<std::string>("base_frame", "base_footprint");
|
||||||
|
world_frame_ = this->declare_parameter<std::string>("world_frame", "odom_combined");
|
||||||
|
enable_world_view_ = this->declare_parameter<bool>("enable_world_view", true);
|
||||||
|
|
||||||
|
const int input_width = this->declare_parameter<int>("input_width", 0);
|
||||||
|
const int input_height = this->declare_parameter<int>("input_height", 0);
|
||||||
|
input_is_rectified_ = this->declare_parameter<bool>("input_is_rectified", true);
|
||||||
|
if (input_width <= 0 || input_height <= 0) {
|
||||||
|
throw std::runtime_error("input_width and input_height must be positive YAML parameters");
|
||||||
|
}
|
||||||
|
|
||||||
|
const CameraIntrinsics intrinsics{
|
||||||
|
this->declare_parameter<double>("fx", 0.0),
|
||||||
|
this->declare_parameter<double>("fy", 0.0),
|
||||||
|
this->declare_parameter<double>("cx", 0.0),
|
||||||
|
this->declare_parameter<double>("cy", 0.0),
|
||||||
|
input_width,
|
||||||
|
input_height};
|
||||||
|
const auto rotation = this->declare_parameter<std::vector<double>>(
|
||||||
|
"camera_from_base_rotation", std::vector<double>{});
|
||||||
|
const auto translation = this->declare_parameter<std::vector<double>>(
|
||||||
|
"camera_from_base_translation", std::vector<double>{});
|
||||||
|
if (const auto error = validateCameraConfiguration(intrinsics, rotation, translation)) {
|
||||||
|
throw std::runtime_error(*error);
|
||||||
|
}
|
||||||
|
|
||||||
|
intrinsics_ = intrinsics;
|
||||||
|
camera_from_base_ = RigidTransform(
|
||||||
|
{rotation[0], rotation[1], rotation[2], rotation[3], rotation[4], rotation[5], rotation[6],
|
||||||
|
rotation[7], rotation[8]},
|
||||||
|
{translation[0], translation[1], translation[2]});
|
||||||
|
projection_ = std::make_unique<GroundProjection>(intrinsics_, camera_from_base_);
|
||||||
|
|
||||||
|
const BirdseyeCanvas vehicle_canvas{
|
||||||
|
static_cast<int>(this->declare_parameter<int>("output_width", 600)),
|
||||||
|
static_cast<int>(this->declare_parameter<int>("output_height", 800)),
|
||||||
|
this->declare_parameter<double>("meters_per_pixel", 0.01),
|
||||||
|
this->declare_parameter<double>("rear_meters", 2.0),
|
||||||
|
this->declare_parameter<double>("left_meters", 3.0)};
|
||||||
|
if (const auto error = validateCanvas(vehicle_canvas, "vehicle")) {
|
||||||
|
throw std::runtime_error(*error);
|
||||||
|
}
|
||||||
|
vehicle_geometry_ = std::make_unique<BirdseyeGeometry>(vehicle_canvas);
|
||||||
|
vehicle_canvas_ = vehicle_canvas;
|
||||||
|
|
||||||
|
world_canvas_.width = static_cast<int>(this->declare_parameter<int>("world_width", 800));
|
||||||
|
world_canvas_.height = static_cast<int>(this->declare_parameter<int>("world_height", 800));
|
||||||
|
world_canvas_.meters_per_pixel = vehicle_canvas.meters_per_pixel;
|
||||||
|
world_canvas_.rear_meters = this->declare_parameter<double>("world_rear_meters", 4.0);
|
||||||
|
world_canvas_.left_meters = this->declare_parameter<double>("world_left_meters", 4.0);
|
||||||
|
world_origin_x_ = this->declare_parameter<double>("world_origin_x", 0.0);
|
||||||
|
world_origin_y_ = this->declare_parameter<double>("world_origin_y", 0.0);
|
||||||
|
if (const auto error = validateCanvas(world_canvas_, "world")) {
|
||||||
|
throw std::runtime_error(*error);
|
||||||
|
}
|
||||||
|
if (!std::isfinite(world_origin_x_) || !std::isfinite(world_origin_y_)) {
|
||||||
|
throw std::runtime_error("world origin must contain finite values");
|
||||||
|
}
|
||||||
|
|
||||||
|
const auto distortion = this->declare_parameter<std::vector<double>>(
|
||||||
|
"distortion_coefficients", std::vector<double>{0.0, 0.0, 0.0, 0.0, 0.0});
|
||||||
|
if (const auto error = validateCoefficientVector(
|
||||||
|
distortion, 4, "distortion_coefficients"))
|
||||||
|
{
|
||||||
|
throw std::runtime_error(*error);
|
||||||
|
}
|
||||||
|
camera_matrix_ = (cv::Mat_<double>(3, 3) << intrinsics.fx, 0.0, intrinsics.cx, 0.0,
|
||||||
|
intrinsics.fy, intrinsics.cy, 0.0, 0.0, 1.0);
|
||||||
|
distortion_ = cv::Mat(distortion).clone();
|
||||||
|
|
||||||
|
vehicle_publisher_ = this->create_publisher<sensor_msgs::msg::Image>(vehicle_output_topic, 10);
|
||||||
|
world_publisher_ = this->create_publisher<sensor_msgs::msg::Image>(world_output_topic, 10);
|
||||||
|
if (image_message_type == "compressed") {
|
||||||
|
compressed_image_subscription_ =
|
||||||
|
this->create_subscription<sensor_msgs::msg::CompressedImage>(
|
||||||
|
image_topic, rclcpp::SensorDataQoS(),
|
||||||
|
std::bind(&BirdseyeNode::onCompressedImage, this, std::placeholders::_1));
|
||||||
|
} else {
|
||||||
|
raw_image_subscription_ = this->create_subscription<sensor_msgs::msg::Image>(
|
||||||
|
image_topic, rclcpp::SensorDataQoS(),
|
||||||
|
std::bind(&BirdseyeNode::onRawImage, this, std::placeholders::_1));
|
||||||
|
}
|
||||||
|
|
||||||
|
RCLCPP_INFO(
|
||||||
|
this->get_logger(),
|
||||||
|
"birdseye input=%s message_type=%s expected_size=%dx%d rectified=%s "
|
||||||
|
"world_frame=%s base_frame=%s",
|
||||||
|
image_topic.c_str(), image_message_type.c_str(), intrinsics_.width, intrinsics_.height,
|
||||||
|
input_is_rectified_ ? "true" : "false", world_frame_.c_str(), base_frame_.c_str());
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
using BaseToCanvas = std::function<std::optional<PixelPoint>(const GroundPoint &)>;
|
||||||
|
|
||||||
|
cv::Mat render(
|
||||||
|
const cv::Mat & image, const BaseToCanvas & base_to_canvas, int width,
|
||||||
|
int height) const
|
||||||
|
{
|
||||||
|
const std::array<GroundPoint, 4> reference_points{{
|
||||||
|
{0.5, -2.0}, {0.5, 2.0}, {4.0, 2.0}, {4.0, -2.0}}};
|
||||||
|
std::vector<cv::Point2f> source;
|
||||||
|
std::vector<cv::Point2f> destination;
|
||||||
|
source.reserve(reference_points.size());
|
||||||
|
destination.reserve(reference_points.size());
|
||||||
|
for (const auto & point : reference_points) {
|
||||||
|
const auto image_pixel = projection_->projectBaseGroundPoint(point);
|
||||||
|
const auto output_pixel = base_to_canvas(point);
|
||||||
|
if (!image_pixel || !output_pixel) {
|
||||||
|
return cv::Mat::zeros(height, width, CV_8UC3);
|
||||||
|
}
|
||||||
|
source.emplace_back(image_pixel->x, image_pixel->y);
|
||||||
|
destination.emplace_back(output_pixel->x, output_pixel->y);
|
||||||
|
}
|
||||||
|
cv::Mat output;
|
||||||
|
cv::warpPerspective(
|
||||||
|
image, output, cv::getPerspectiveTransform(source, destination), cv::Size(width, height),
|
||||||
|
cv::INTER_LINEAR, cv::BORDER_CONSTANT);
|
||||||
|
return output;
|
||||||
|
}
|
||||||
|
|
||||||
|
void drawVehicle(cv::Mat & image, const BaseToCanvas & base_to_canvas) const
|
||||||
|
{
|
||||||
|
const std::array<GroundPoint, 4> footprint{{
|
||||||
|
{0.138, -0.095}, {0.138, 0.095}, {-0.138, 0.095}, {-0.138, -0.095}}};
|
||||||
|
std::vector<cv::Point> pixels;
|
||||||
|
for (const auto & point : footprint) {
|
||||||
|
const auto pixel = base_to_canvas(point);
|
||||||
|
if (!pixel) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
pixels.emplace_back(static_cast<int>(pixel->x), static_cast<int>(pixel->y));
|
||||||
|
}
|
||||||
|
cv::fillConvexPoly(image, pixels, cv::Scalar(0, 220, 255));
|
||||||
|
}
|
||||||
|
|
||||||
|
std::optional<PixelPoint> worldPixel(const GroundPoint & world_point) const
|
||||||
|
{
|
||||||
|
const GroundPoint local{world_point.x - world_origin_x_, world_point.y - world_origin_y_};
|
||||||
|
return BirdseyeGeometry(world_canvas_).basePointToCanvasPixel(local);
|
||||||
|
}
|
||||||
|
|
||||||
|
static GroundPoint baseToWorld(
|
||||||
|
const GroundPoint & point, const geometry_msgs::msg::TransformStamped & transform)
|
||||||
|
{
|
||||||
|
const double yaw = tf2::getYaw(transform.transform.rotation);
|
||||||
|
const double cosine = std::cos(yaw);
|
||||||
|
const double sine = std::sin(yaw);
|
||||||
|
return {
|
||||||
|
transform.transform.translation.x + cosine * point.x - sine * point.y,
|
||||||
|
transform.transform.translation.y + sine * point.x + cosine * point.y};
|
||||||
|
}
|
||||||
|
|
||||||
|
void publish(
|
||||||
|
const cv::Mat & image, const std_msgs::msg::Header & header,
|
||||||
|
const std::string & frame_id,
|
||||||
|
const rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr & publisher) const
|
||||||
|
{
|
||||||
|
auto message =
|
||||||
|
cv_bridge::CvImage(header, sensor_msgs::image_encodings::BGR8, image).toImageMsg();
|
||||||
|
message->header.frame_id = frame_id;
|
||||||
|
publisher->publish(*message);
|
||||||
|
}
|
||||||
|
|
||||||
|
void processImage(const cv::Mat & source, const std_msgs::msg::Header & header)
|
||||||
|
{
|
||||||
|
const auto size_error = validateImageSize(
|
||||||
|
intrinsics_.width, intrinsics_.height,
|
||||||
|
source.cols, source.rows);
|
||||||
|
if (size_error) {
|
||||||
|
RCLCPP_ERROR_THROTTLE(
|
||||||
|
this->get_logger(), *this->get_clock(), 2000, "%s; update input_width/input_height "
|
||||||
|
"and matching camera intrinsics in birdseye YAML", size_error->c_str());
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!logged_input_size_) {
|
||||||
|
RCLCPP_INFO(this->get_logger(), "accepted input frame size=%dx%d", source.cols, source.rows);
|
||||||
|
logged_input_size_ = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
cv::Mat projection_input;
|
||||||
|
if (input_is_rectified_) {
|
||||||
|
projection_input = source;
|
||||||
|
} else {
|
||||||
|
cv::undistort(source, projection_input, camera_matrix_, distortion_);
|
||||||
|
}
|
||||||
|
|
||||||
|
const auto vehicle_mapping = [this](const GroundPoint & point) {
|
||||||
|
return vehicle_geometry_->basePointToCanvasPixel(point);
|
||||||
|
};
|
||||||
|
auto vehicle = render(
|
||||||
|
projection_input, vehicle_mapping, vehicle_canvas_.width, vehicle_canvas_.height);
|
||||||
|
drawVehicle(vehicle, vehicle_mapping);
|
||||||
|
publish(vehicle, header, base_frame_, vehicle_publisher_);
|
||||||
|
|
||||||
|
if (!enable_world_view_) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
geometry_msgs::msg::TransformStamped world_from_base;
|
||||||
|
try {
|
||||||
|
world_from_base = tf_buffer_.lookupTransform(
|
||||||
|
world_frame_, base_frame_, header.stamp, tf2::durationFromSec(0.05));
|
||||||
|
} catch (const tf2::TransformException & error) {
|
||||||
|
RCLCPP_WARN_THROTTLE(
|
||||||
|
this->get_logger(),
|
||||||
|
*this->get_clock(), 2000, "world view skipped: %s", error.what());
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
const auto world_mapping = [this, &world_from_base](const GroundPoint & point) {
|
||||||
|
return worldPixel(baseToWorld(point, world_from_base));
|
||||||
|
};
|
||||||
|
auto world = render(
|
||||||
|
projection_input, world_mapping, world_canvas_.width, world_canvas_.height);
|
||||||
|
drawVehicle(world, world_mapping);
|
||||||
|
publish(world, header, world_frame_, world_publisher_);
|
||||||
|
}
|
||||||
|
|
||||||
|
void onRawImage(const sensor_msgs::msg::Image::SharedPtr message)
|
||||||
|
{
|
||||||
|
try {
|
||||||
|
const auto image = cv_bridge::toCvCopy(message, sensor_msgs::image_encodings::BGR8);
|
||||||
|
processImage(image->image, message->header);
|
||||||
|
} catch (const cv_bridge::Exception & error) {
|
||||||
|
RCLCPP_ERROR_THROTTLE(
|
||||||
|
this->get_logger(), *this->get_clock(), 2000,
|
||||||
|
"raw image decode failed: %s", error.what());
|
||||||
|
} catch (const cv::Exception & error) {
|
||||||
|
RCLCPP_ERROR_THROTTLE(
|
||||||
|
this->get_logger(), *this->get_clock(), 2000,
|
||||||
|
"raw image processing failed: %s", error.what());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void onCompressedImage(const sensor_msgs::msg::CompressedImage::SharedPtr message)
|
||||||
|
{
|
||||||
|
if (message->data.empty()) {
|
||||||
|
RCLCPP_ERROR_THROTTLE(
|
||||||
|
this->get_logger(), *this->get_clock(), 2000,
|
||||||
|
"compressed image decode failed: empty data");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
try {
|
||||||
|
const cv::Mat image = cv::imdecode(message->data, cv::IMREAD_COLOR);
|
||||||
|
if (image.empty()) {
|
||||||
|
RCLCPP_ERROR_THROTTLE(
|
||||||
|
this->get_logger(), *this->get_clock(), 2000,
|
||||||
|
"compressed image decode failed: empty decoded frame");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
processImage(image, message->header);
|
||||||
|
} catch (const cv::Exception & error) {
|
||||||
|
RCLCPP_ERROR_THROTTLE(
|
||||||
|
this->get_logger(), *this->get_clock(), 2000,
|
||||||
|
"compressed image processing failed: %s", error.what());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
std::string base_frame_;
|
||||||
|
std::string world_frame_;
|
||||||
|
bool enable_world_view_{true};
|
||||||
|
bool input_is_rectified_{true};
|
||||||
|
bool logged_input_size_{false};
|
||||||
|
CameraIntrinsics intrinsics_{};
|
||||||
|
RigidTransform camera_from_base_{{}, {}};
|
||||||
|
std::unique_ptr<GroundProjection> projection_;
|
||||||
|
BirdseyeCanvas vehicle_canvas_{};
|
||||||
|
BirdseyeCanvas world_canvas_{};
|
||||||
|
std::unique_ptr<BirdseyeGeometry> vehicle_geometry_;
|
||||||
|
double world_origin_x_{0.0};
|
||||||
|
double world_origin_y_{0.0};
|
||||||
|
cv::Mat camera_matrix_;
|
||||||
|
cv::Mat distortion_;
|
||||||
|
tf2_ros::Buffer tf_buffer_;
|
||||||
|
tf2_ros::TransformListener tf_listener_;
|
||||||
|
rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr raw_image_subscription_;
|
||||||
|
rclcpp::Subscription<sensor_msgs::msg::CompressedImage>::SharedPtr
|
||||||
|
compressed_image_subscription_;
|
||||||
|
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr vehicle_publisher_;
|
||||||
|
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr world_publisher_;
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace origincar_birdseye
|
||||||
|
|
||||||
|
int main(int argc, char ** argv)
|
||||||
|
{
|
||||||
|
rclcpp::init(argc, argv);
|
||||||
|
rclcpp::spin(std::make_shared<origincar_birdseye::BirdseyeNode>());
|
||||||
|
rclcpp::shutdown();
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
68
src/origincar_birdseye/src/synthetic_camera_node.cpp
Normal file
68
src/origincar_birdseye/src/synthetic_camera_node.cpp
Normal file
@@ -0,0 +1,68 @@
|
|||||||
|
#include <chrono>
|
||||||
|
#include <memory>
|
||||||
|
#include <stdexcept>
|
||||||
|
#include <string>
|
||||||
|
|
||||||
|
#include <rclcpp/rclcpp.hpp>
|
||||||
|
#include <sensor_msgs/image_encodings.hpp>
|
||||||
|
#include <sensor_msgs/msg/image.hpp>
|
||||||
|
|
||||||
|
namespace origincar_birdseye
|
||||||
|
{
|
||||||
|
|
||||||
|
class SyntheticCameraNode : public rclcpp::Node
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
SyntheticCameraNode()
|
||||||
|
: Node("synthetic_camera_node")
|
||||||
|
{
|
||||||
|
const auto image_topic = this->declare_parameter<std::string>("image_topic", "/image");
|
||||||
|
width_ = this->declare_parameter<int>("input_width", 0);
|
||||||
|
height_ = this->declare_parameter<int>("input_height", 0);
|
||||||
|
if (width_ <= 0 || height_ <= 0) {
|
||||||
|
throw std::runtime_error("synthetic input_width and input_height must be positive");
|
||||||
|
}
|
||||||
|
publisher_ = this->create_publisher<sensor_msgs::msg::Image>(image_topic, 10);
|
||||||
|
timer_ = this->create_wall_timer(std::chrono::milliseconds(100), [this]() {publishFrame();});
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
void publishFrame()
|
||||||
|
{
|
||||||
|
const auto width = static_cast<uint32_t>(width_);
|
||||||
|
const auto height = static_cast<uint32_t>(height_);
|
||||||
|
sensor_msgs::msg::Image message;
|
||||||
|
message.header.stamp = this->now();
|
||||||
|
message.header.frame_id = "front_camera_optical_frame";
|
||||||
|
message.height = height;
|
||||||
|
message.width = width;
|
||||||
|
message.encoding = sensor_msgs::image_encodings::RGB8;
|
||||||
|
message.step = width * 3;
|
||||||
|
message.data.resize(static_cast<size_t>(message.step) * height);
|
||||||
|
for (uint32_t row = 0; row < height; ++row) {
|
||||||
|
for (uint32_t column = 0; column < width; ++column) {
|
||||||
|
const bool line = row % 40 < 2 || column % 40 < 2;
|
||||||
|
const size_t index = static_cast<size_t>(row) * message.step + column * 3;
|
||||||
|
message.data[index] = line ? 240 : 50;
|
||||||
|
message.data[index + 1] = line ? 240 : 100;
|
||||||
|
message.data[index + 2] = line ? 240 : 50;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
publisher_->publish(message);
|
||||||
|
}
|
||||||
|
|
||||||
|
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr publisher_;
|
||||||
|
rclcpp::TimerBase::SharedPtr timer_;
|
||||||
|
int width_{0};
|
||||||
|
int height_{0};
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace origincar_birdseye
|
||||||
|
|
||||||
|
int main(int argc, char ** argv)
|
||||||
|
{
|
||||||
|
rclcpp::init(argc, argv);
|
||||||
|
rclcpp::spin(std::make_shared<origincar_birdseye::SyntheticCameraNode>());
|
||||||
|
rclcpp::shutdown();
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
114
src/origincar_birdseye/test/test_ground_projection.cpp
Normal file
114
src/origincar_birdseye/test/test_ground_projection.cpp
Normal file
@@ -0,0 +1,114 @@
|
|||||||
|
#include <gtest/gtest.h>
|
||||||
|
|
||||||
|
#include <limits>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
|
#include "origincar_birdseye/ground_projection.hpp"
|
||||||
|
#include "origincar_birdseye/birdseye_geometry.hpp"
|
||||||
|
#include "origincar_birdseye/configuration_validation.hpp"
|
||||||
|
#include "origincar_birdseye/image_validation.hpp"
|
||||||
|
|
||||||
|
namespace origincar_birdseye
|
||||||
|
{
|
||||||
|
|
||||||
|
TEST(GroundProjection, ProjectsGroundPointFromVehicleCoordinates)
|
||||||
|
{
|
||||||
|
CameraIntrinsics intrinsics{800.0, 800.0, 400.0, 225.0, 800, 450};
|
||||||
|
RigidTransform camera_from_base(
|
||||||
|
{0.0, -1.0, 0.0,
|
||||||
|
0.0, 0.0, -1.0,
|
||||||
|
1.0, 0.0, 0.0},
|
||||||
|
{0.0, 0.0, 0.0});
|
||||||
|
GroundProjection projection(intrinsics, camera_from_base);
|
||||||
|
|
||||||
|
const auto pixel = projection.projectBaseGroundPoint({1.0, 0.0});
|
||||||
|
|
||||||
|
ASSERT_TRUE(pixel.has_value());
|
||||||
|
EXPECT_NEAR(pixel->x, 400.0, 1e-6);
|
||||||
|
EXPECT_NEAR(pixel->y, 225.0, 1e-6);
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace origincar_birdseye
|
||||||
|
|
||||||
|
namespace origincar_birdseye
|
||||||
|
{
|
||||||
|
|
||||||
|
TEST(BirdseyeGeometry, KeepsVehicleAtConfiguredCanvasPosition)
|
||||||
|
{
|
||||||
|
BirdseyeGeometry geometry({600, 800, 0.01, 2.0, 3.0});
|
||||||
|
|
||||||
|
const auto vehicle = geometry.basePointToCanvasPixel({0.0, 0.0});
|
||||||
|
const auto one_meter_ahead = geometry.basePointToCanvasPixel({1.0, 0.0});
|
||||||
|
|
||||||
|
ASSERT_TRUE(vehicle.has_value());
|
||||||
|
ASSERT_TRUE(one_meter_ahead.has_value());
|
||||||
|
EXPECT_NEAR(vehicle->x, 300.0, 1e-6);
|
||||||
|
EXPECT_NEAR(vehicle->y, 200.0, 1e-6);
|
||||||
|
EXPECT_NEAR(one_meter_ahead->x, 300.0, 1e-6);
|
||||||
|
EXPECT_NEAR(one_meter_ahead->y, 100.0, 1e-6);
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(BirdseyeGeometry, PreservesPointsOutsideCanvasForHomography)
|
||||||
|
{
|
||||||
|
BirdseyeGeometry geometry({600, 800, 0.01, 2.0, 3.0});
|
||||||
|
|
||||||
|
const auto four_meters_ahead = geometry.basePointToCanvasPixel({4.0, 2.0});
|
||||||
|
|
||||||
|
ASSERT_TRUE(four_meters_ahead.has_value());
|
||||||
|
EXPECT_NEAR(four_meters_ahead->x, 100.0, 1e-6);
|
||||||
|
EXPECT_NEAR(four_meters_ahead->y, -200.0, 1e-6);
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(ConfigurationValidation, RejectsInvalidCanvas)
|
||||||
|
{
|
||||||
|
const auto zero_scale = validateCanvas({600, 800, 0.0, 2.0, 3.0}, "vehicle");
|
||||||
|
const auto invalid_width = validateCanvas({0, 800, 0.01, 2.0, 3.0}, "vehicle");
|
||||||
|
|
||||||
|
EXPECT_TRUE(zero_scale.has_value());
|
||||||
|
EXPECT_TRUE(invalid_width.has_value());
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(ConfigurationValidation, RejectsNonFiniteCameraParameters)
|
||||||
|
{
|
||||||
|
CameraIntrinsics intrinsics{800.0, 800.0, 400.0, 225.0, 800, 450};
|
||||||
|
std::vector<double> rotation{1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
|
||||||
|
std::numeric_limits<double>::quiet_NaN()};
|
||||||
|
const std::vector<double> translation{0.0, 0.0, 0.2};
|
||||||
|
|
||||||
|
EXPECT_TRUE(validateCameraConfiguration(intrinsics, rotation, translation).has_value());
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace origincar_birdseye
|
||||||
|
|
||||||
|
namespace origincar_birdseye
|
||||||
|
{
|
||||||
|
|
||||||
|
TEST(ImageValidation, AcceptsConfiguredInputSize)
|
||||||
|
{
|
||||||
|
EXPECT_FALSE(validateImageSize(1280, 720, 1280, 720).has_value());
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(ImageValidation, ReportsConfiguredAndActualInputSizes)
|
||||||
|
{
|
||||||
|
const auto error = validateImageSize(1280, 720, 800, 450);
|
||||||
|
|
||||||
|
ASSERT_TRUE(error.has_value());
|
||||||
|
EXPECT_NE(error->find("expected 1280x720"), std::string::npos);
|
||||||
|
EXPECT_NE(error->find("received 800x450"), std::string::npos);
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(ImageValidation, AcceptsRawAndCompressedMessageTypes)
|
||||||
|
{
|
||||||
|
EXPECT_FALSE(validateImageMessageType("raw").has_value());
|
||||||
|
EXPECT_FALSE(validateImageMessageType("compressed").has_value());
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(ImageValidation, RejectsUnknownMessageType)
|
||||||
|
{
|
||||||
|
const auto error = validateImageMessageType("mjpeg");
|
||||||
|
|
||||||
|
ASSERT_TRUE(error.has_value());
|
||||||
|
EXPECT_NE(error->find("raw or compressed"), std::string::npos);
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace origincar_birdseye
|
||||||
Reference in New Issue
Block a user