多传感器融合感知中的视锥体创建
·
在机器人 SLAM / costmap / OpenVDB / 3D 避障中，传感器视锥模型的作用是描述：
当前传感器在某一时刻“能看到哪里、看不到哪里、哪些空间可以被更新为 free、哪些点可能是 obstacle”。
它通常用于：
1. 点云 / 深度图裁剪
2. Occupancy Grid 更新
3. OpenVDB / OctoMap / TSDF 融合
4. costmap clearing / marking
5. 多传感器融合中的可见性判断
6. 语义地图中的观测置信度建模
1. 视锥模型在系统中的位置
Camera / LiDAR / Depth Sensor
↓
Sensor Intrinsics + Extrinsics
↓
Frustum / FOV Model
↓
Visible Space Query
↓
Voxel Update / Costmap Update / Obstacle Fusion
↓
Planner / Controller
简单说：
视锥模型不是地图本身，而是地图更新时的“观测范围约束”。
2. 坐标系定义
机器人系统里至少有 3 个关键坐标系：
map
└── odom
└── base_link
└── camera_link / lidar_link
常见变换：
T_map_base
T_base_sensor
T_map_sensor = T_map_base * T_base_sensor
如果一个点在传感器坐标系中为：
p_sensor = [x, y, z, 1]^T
转换到 map 坐标系：
p_map = T_map_sensor * p_sensor
反过来，如果要判断地图中的某个 voxel 是否在传感器视野内：
p_sensor = inverse(T_map_sensor) * p_map
3. 相机 / 深度相机视锥模型
对于 RGB-D 相机、双目相机、深度相机，最常用的是 针孔相机模型。
相机内参矩阵：
K = [ fx 0 cx
0 fy cy
0 0 1 ]
其中：
| 参数 | 含义 |
|---|---|
fx |
x 方向焦距 |
fy |
y 方向焦距 |
cx |
主点 x |
cy |
主点 y |
width |
图像宽度 |
height |
图像高度 |
near |
最近有效距离 |
far |
最远有效距离 |
3.1 像素点反投影到 3D
像素点：
u, v
深度：
z
反投影到相机坐标系：
x = (u - cx) * z / fx
y = (v - cy) * z / fy
z = z
也可以写成：
p_camera = z * K^-1 * [u, v, 1]^T
3.2 构造视锥 8 个角点
用图像四个角点分别在 near 和 far 深度处反投影。
像素角点：
(0, 0)
(width, 0)
(width, height)
(0, height)
近裁剪面：
near_z
远裁剪面：
far_z
得到 8 个角点：
near_tl, near_tr, near_br, near_bl
far_tl, far_tr, far_br, far_bl
示意：
camera origin
/|\
/ | \
/ | \
/ | \
near plane
\ | /
\ | /
\ | /
far plane
更准确的空间结构：
far_tl -------- far_tr
/| /|
/ | / |
near_tl -------- near_tr |
| | | |
| far_bl -------|-- far_br
| / | /
| / | /
near_bl -------- near_br
4. 通过 FOV 构建视锥
如果没有相机内参，也可以用水平 / 垂直 FOV 构建。
参数：
horizontal_fov = hfov
vertical_fov = vfov
near
far
远平面半宽：
far_half_w = far * tan(hfov / 2)
far_half_h = far * tan(vfov / 2)
近平面半宽：
near_half_w = near * tan(hfov / 2)
near_half_h = near * tan(vfov / 2)
相机坐标系下一般假设：
z forward
x right
y down / up depending convention
则远平面四角可以写成：
[-far_half_w, -far_half_h, far]
[ far_half_w, -far_half_h, far]
[ far_half_w, far_half_h, far]
[-far_half_w, far_half_h, far]
近平面同理。
5. 判断一个 3D 点是否在视锥内
对于相机，最常用方法是：
方法一：投影判断
给定地图点：
p_map = [x, y, z]
先转到传感器坐标系：
p_sensor = T_sensor_map * p_map
得到：
X, Y, Z
然后判断深度：
near <= Z <= far
投影到像素：
u = fx * X / Z + cx
v = fy * Y / Z + cy
判断是否在图像范围内：
0 <= u < width
0 <= v < height
如果都满足，则点在视锥内。
伪代码：
bool isInsideCameraFrustum(
const Eigen::Vector3f& p_map,
const Eigen::Matrix4f& T_sensor_map,
float fx, float fy, float cx, float cy,
int width, int height,
float near_z, float far_z)
{
Eigen::Vector4f p(p_map.x(), p_map.y(), p_map.z(), 1.0f);
Eigen::Vector4f ps = T_sensor_map * p;
float X = ps.x();
float Y = ps.y();
float Z = ps.z();
if (Z < near_z || Z > far_z) {
return false;
}
float u = fx * X / Z + cx;
float v = fy * Y / Z + cy;
return u >= 0 && u < width && v >= 0 && v < height;
}
方法二：平面判断
视锥可以表示为 6 个平面：
left plane
right plane
top plane
bottom plane
near plane
far plane
每个平面：
n · p + d >= 0
如果点同时在 6 个平面的内部，则在视锥内。
适合：
大量 voxel culling
OpenVDB tree 裁剪
GPU frustum culling
局部地图范围快速筛选
6. LiDAR 视锥 / 视场模型
严格来说，2D LiDAR 不是视锥，而是 扇形扫描区域。
参数：
angle_min
angle_max
range_min
range_max
判断点是否在 LiDAR 可见范围：
r = sqrt(x^2 + y^2)
theta = atan2(y, x)
条件：
range_min <= r <= range_max
angle_min <= theta <= angle_max
伪代码：
bool isInsideLidarFov(
const Eigen::Vector3f& p_sensor,
float angle_min,
float angle_max,
float range_min,
float range_max)
{
float x = p_sensor.x();
float y = p_sensor.y();
float r = std::sqrt(x * x + y * y);
float theta = std::atan2(y, x);
if (r < range_min || r > range_max) {
return false;
}
return theta >= angle_min && theta <= angle_max;
}
7. 3D LiDAR 视场模型
3D LiDAR 可以建模为：
水平 FOV + 垂直 FOV + 距离范围
点在 LiDAR 坐标系下：
x, y, z
距离：
r = sqrt(x^2 + y^2 + z^2)
水平角：
azimuth = atan2(y, x)
垂直角：
elevation = atan2(z, sqrt(x^2 + y^2))
判断条件：
range_min <= r <= range_max
azimuth_min <= azimuth <= azimuth_max
elevation_min <= elevation <= elevation_max
这类模型适合：
Livox
Velodyne
Ouster
禾赛
速腾聚创
固态 / 半固态 LiDAR
8. 用视锥更新 Occupancy / OpenVDB
视锥在地图更新中主要有两个作用：
| 操作 | 含义 |
|---|---|
| marking | 传感器看到的点标记为 occupied |
| clearing | 传感器射线经过但没有命中的区域标记为 free |
8.1 点云 marking
PointCloud
↓
Transform to map
↓
Voxel index
↓
occupied += log_odds_hit
伪代码：
for (const auto& p_sensor : pointcloud) {
if (!validRange(p_sensor)) {
continue;
}
Eigen::Vector3f p_map = transformPoint(T_map_sensor, p_sensor);
VoxelIndex idx = worldToVoxel(p_map);
grid[idx].log_odds += log_odds_hit;
}
8.2 射线 clearing
从传感器原点到障碍物点之间的 voxel，都可以认为是 free：
sensor origin -------------- obstacle point
free free free free occupied
伪代码：
for (const auto& p_sensor : pointcloud) {
Eigen::Vector3f origin_map = getSensorOrigin(T_map_sensor);
Eigen::Vector3f hit_map = transformPoint(T_map_sensor, p_sensor);
auto ray_voxels = raycastVoxels(origin_map, hit_map);
for (size_t i = 0; i + 1 < ray_voxels.size(); ++i) {
grid[ray_voxels[i]].log_odds += log_odds_miss;
}
grid[ray_voxels.back()].log_odds += log_odds_hit;
}
9. OpenVDB 中的视锥更新思路
OpenVDB 适合做稀疏 3D voxel map。
推荐设计：
Depth / PointCloud
↓
Frustum Model
↓
Ray Casting
↓
OpenVDB Occupancy Grid
↓
Projection to Costmap / SDF Query
OpenVDB 中可以存：
openvdb::FloatGrid::Ptr occupancy_grid;
openvdb::FloatGrid::Ptr sdf_grid;
openvdb::Int32Grid::Ptr semantic_grid;
其中 occupancy 可以使用 log-odds：
log_odds > threshold => occupied
log_odds < threshold => free
unknown => inactive voxel
9.1 OpenVDB 伪代码
class SensorFrustum {
public:
Eigen::Matrix4f T_map_sensor;
float near_z = 0.2f;
float far_z = 5.0f;
float fx = 525.0f;
float fy = 525.0f;
float cx = 320.0f;
float cy = 240.0f;
int width = 640;
int height = 480;
bool containsMapPoint(const Eigen::Vector3f& p_map) const {
Eigen::Matrix4f T_sensor_map = T_map_sensor.inverse();
Eigen::Vector4f p(
p_map.x(), p_map.y(), p_map.z(), 1.0f
);
Eigen::Vector4f ps = T_sensor_map * p;
float X = ps.x();
float Y = ps.y();
float Z = ps.z();
if (Z < near_z || Z > far_z) {
return false;
}
float u = fx * X / Z + cx;
float v = fy * Y / Z + cy;
return u >= 0 && u < width && v >= 0 && v < height;
}
};
OpenVDB 更新接口可以这样设计：
class VDBOccupancyMapper {
public:
void updateWithPointCloud(
const std::vector<Eigen::Vector3f>& points_sensor,
const SensorFrustum& frustum)
{
Eigen::Vector3f origin_map =
frustum.T_map_sensor.block<3, 1>(0, 3);
for (const auto& p_sensor : points_sensor) {
if (p_sensor.z() < frustum.near_z ||
p_sensor.z() > frustum.far_z) {
continue;
}
Eigen::Vector3f hit_map =
transformPoint(frustum.T_map_sensor, p_sensor);
updateFreeAlongRay(origin_map, hit_map);
updateOccupied(hit_map);
}
}
private:
void updateFreeAlongRay(
const Eigen::Vector3f& origin,
const Eigen::Vector3f& hit)
{
// Raycast voxel traversal
// log_odds += miss
}
void updateOccupied(const Eigen::Vector3f& hit)
{
// log_odds += hit
}
};
10. 投影到 2D costmap
如果要和 Nav2 costmap 结合，可以把 3D 视锥更新后的 voxel map 投影成 2D。
核心逻辑：
遍历 local costmap 范围内每个 x, y
检查 z_min ~ z_max 内是否存在 occupied voxel
有 → lethal obstacle
无 → free
示意：
OpenVDB 3D Map
↓
z height filter
↓
XY projection
↓
2D Costmap
伪代码：
for (int mx = 0; mx < costmap_size_x; ++mx) {
for (int my = 0; my < costmap_size_y; ++my) {
double wx, wy;
costmap.mapToWorld(mx, my, wx, wy);
bool occupied = false;
for (float z = z_min; z <= z_max; z += voxel_resolution) {
if (vdb_map.isOccupied(wx, wy, z)) {
occupied = true;
break;
}
}
if (occupied) {
costmap.setCost(mx, my, LETHAL_OBSTACLE);
} else {
costmap.setCost(mx, my, FREE_SPACE);
}
}
}
11. 视锥模型在 costmap 中的作用
在 Nav2 / costmap 里，视锥主要对应：
observation source
raytrace clearing
obstacle marking
max_obstacle_height
raytrace_max_range
obstacle_max_range
示例配置：
local_costmap:
local_costmap:
ros__parameters:
global_frame: odom
robot_base_frame: base_link
rolling_window: true
width: 5.0
height: 5.0
resolution: 0.05
plugins:
- obstacle_layer
- inflation_layer
obstacle_layer:
plugin: "nav2_costmap_2d::ObstacleLayer"
observation_sources: scan depth_points
scan:
topic: /scan
data_type: "LaserScan"
marking: true
clearing: true
obstacle_max_range: 3.0
raytrace_max_range: 4.0
depth_points:
topic: /camera/depth/points
data_type: "PointCloud2"
marking: true
clearing: true
min_obstacle_height: 0.05
max_obstacle_height: 1.5
obstacle_max_range: 2.5
raytrace_max_range: 3.0
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
inflation_radius: 0.4
cost_scaling_factor: 3.0
其中：
| 参数 | 对应视锥含义 |
|---|---|
obstacle_max_range |
marking 最大距离 |
raytrace_max_range |
clearing 最大距离 |
min_obstacle_height |
最小有效高度 |
max_obstacle_height |
最大有效高度 |
marking |
命中点标记障碍 |
clearing |
射线清除自由空间 |
12. 深度相机视锥更新流程
对于 RGB-D / RealSense / Orbbec / ZED：
Depth Image
↓
按像素反投影
↓
生成点云
↓
视锥内点过滤
↓
raycast clearing
↓
occupied marking
↓
更新 voxel map / costmap
工程建议：
不要每个像素都更新
可以做：
1. depth stride 采样，例如每 2~4 个像素采样一次
2. 跳过无效深度
3. 限制 near / far
4. 限制 z 高度范围
5. 对点云做 voxel downsample
6. 使用局部 rolling map，而不是无限大地图
13. 多传感器视锥融合
如果有多个传感器：
front_depth_camera
rear_depth_camera
left_lidar
right_lidar
top_3d_lidar
每个传感器都应该有自己的视锥模型：
struct SensorModel {
std::string name;
std::string frame_id;
SensorType type;
Eigen::Matrix4f T_base_sensor;
float range_min;
float range_max;
float hfov;
float vfov;
CameraIntrinsics intrinsics;
};
融合时：
for each sensor:
get T_map_sensor
build frustum
update map
架构：
┌──────────────┐
│ front camera │
└──────┬───────┘
↓
front frustum
┌──────────────┐
│ 3D LiDAR │
└──────┬───────┘
↓
lidar fov model
┌──────────────┐
│ rear camera │
└──────┬───────┘
↓
rear frustum
↓
Multi-sensor Visibility Fusion
↓
OpenVDB / Costmap / ESDF
14. 视锥模型的工程类设计
可以设计一个统一接口：
enum class SensorType {
Camera,
DepthCamera,
Lidar2D,
Lidar3D
};
struct CameraIntrinsics {
float fx = 0.0f;
float fy = 0.0f;
float cx = 0.0f;
float cy = 0.0f;
int width = 0;
int height = 0;
};
struct FrustumConfig {
SensorType type;
float range_min = 0.1f;
float range_max = 5.0f;
float hfov = 0.0f;
float vfov = 0.0f;
float azimuth_min = -3.14f;
float azimuth_max = 3.14f;
float elevation_min = -0.5f;
float elevation_max = 0.5f;
CameraIntrinsics camera;
};
class SensorFrustumModel {
public:
explicit SensorFrustumModel(const FrustumConfig& config)
: config_(config) {}
void setPoseInMap(const Eigen::Matrix4f& T_map_sensor) {
T_map_sensor_ = T_map_sensor;
T_sensor_map_ = T_map_sensor.inverse();
}
bool containsMapPoint(const Eigen::Vector3f& p_map) const {
Eigen::Vector4f p(
p_map.x(), p_map.y(), p_map.z(), 1.0f
);
Eigen::Vector4f ps4 = T_sensor_map_ * p;
Eigen::Vector3f ps = ps4.head<3>();
switch (config_.type) {
case SensorType::Camera:
case SensorType::DepthCamera:
return containsCameraPoint(ps);
case SensorType::Lidar2D:
return containsLidar2DPoint(ps);
case SensorType::Lidar3D:
return containsLidar3DPoint(ps);
default:
return false;
}
}
private:
bool containsCameraPoint(const Eigen::Vector3f& ps) const {
float X = ps.x();
float Y = ps.y();
float Z = ps.z();
if (Z < config_.range_min || Z > config_.range_max) {
return false;
}
const auto& cam = config_.camera;
float u = cam.fx * X / Z + cam.cx;
float v = cam.fy * Y / Z + cam.cy;
return u >= 0 && u < cam.width &&
v >= 0 && v < cam.height;
}
bool containsLidar2DPoint(const Eigen::Vector3f& ps) const {
float x = ps.x();
float y = ps.y();
float r = std::sqrt(x * x + y * y);
float theta = std::atan2(y, x);
return r >= config_.range_min &&
r <= config_.range_max &&
theta >= config_.azimuth_min &&
theta <= config_.azimuth_max;
}
bool containsLidar3DPoint(const Eigen::Vector3f& ps) const {
float x = ps.x();
float y = ps.y();
float z = ps.z();
float xy = std::sqrt(x * x + y * y);
float r = std::sqrt(x * x + y * y + z * z);
float azimuth = std::atan2(y, x);
float elevation = std::atan2(z, xy);
return r >= config_.range_min &&
r <= config_.range_max &&
azimuth >= config_.azimuth_min &&
azimuth <= config_.azimuth_max &&
elevation >= config_.elevation_min &&
elevation <= config_.elevation_max;
}
private:
FrustumConfig config_;
Eigen::Matrix4f T_map_sensor_ = Eigen::Matrix4f::Identity();
Eigen::Matrix4f T_sensor_map_ = Eigen::Matrix4f::Identity();
};
15. 配置文件示例
可以用 YAML 管理多个传感器：
sensors:
front_depth:
type: depth_camera
frame_id: camera_front_link
range_min: 0.2
range_max: 4.0
intrinsics:
width: 640
height: 480
fx: 525.0
fy: 525.0
cx: 319.5
cy: 239.5
update:
marking: true
clearing: true
voxel_downsample: 0.05
min_obstacle_height: 0.05
max_obstacle_height: 1.5
front_lidar:
type: lidar2d
frame_id: laser_link
range_min: 0.1
range_max: 12.0
azimuth_min: -2.356
azimuth_max: 2.356
update:
marking: true
clearing: true
top_lidar3d:
type: lidar3d
frame_id: lidar_top_link
range_min: 0.3
range_max: 30.0
azimuth_min: -3.14159
azimuth_max: 3.14159
elevation_min: -0.45
elevation_max: 0.35
update:
marking: true
clearing: true
voxel_downsample: 0.1
16. 机器人导航中推荐的视锥更新策略
16.1 对 local costmap
推荐：
近距离、高频、低延迟
参数建议：
| 参数 | 建议 |
|---|---|
| update frequency | 10Hz ~ 20Hz |
| range | 2m ~ 5m |
| resolution | 0.03m ~ 0.1m |
| clearing | 必须开启 |
| marking | 必须开启 |
| unknown | 局部可按 free / unknown 策略处理 |
16.2 对 global map
推荐：
中低频、稳定、长期累积
参数建议：
| 参数 | 建议 |
|---|---|
| update frequency | 1Hz ~ 5Hz |
| range | 5m ~ 30m |
| resolution | 0.05m ~ 0.2m |
| log-odds | 使用概率融合 |
| dynamic objects | 做时间衰减 |
17. 常见问题
17.1 视锥太大
问题：
错误 clearing
误删障碍物
CPU 开销大
解决：
限制 range_max
限制高度范围
只更新局部 rolling window
点云降采样
17.2 视锥太小
问题：
障碍物残留
局部地图更新慢
机器人前方 free 区域不足
解决：
增大 raytrace_max_range
检查 TF
检查相机内参
检查 FOV 配置
17.3 TF 不准
问题：
障碍物漂移
视锥方向错误
clearing 清错区域
重点检查：
map -> odom
odom -> base_link
base_link -> sensor_link
17.4 深度相机噪声导致误障碍
解决：
深度中值滤波
点云 statistical outlier removal
高度过滤
连续帧确认
log-odds 融合
小连通域删除
18. 推荐最终架构
结合 OpenVDB / costmap / SLAM，可以这样设计：
┌────────────────────┐
│ SLAM / Odometry │
└─────────┬──────────┘
│
T_map_base
│
┌──────────────┐ ┌────────────▼────────────┐
│ Depth Camera │─────▶│ Sensor Frustum Model │
└──────────────┘ └────────────┬────────────┘
│
┌──────────────┐ │
│ 3D LiDAR │─────▶ visibility / ray casting │
└──────────────┘ │
▼
┌────────────────────┐
│ OpenVDB 3D Map │
│ occupancy / SDF │
└─────────┬──────────┘
│
┌──────────────┴──────────────┐
▼ ▼
2D Costmap Projection 3D Collision Query
│ │
▼ ▼
Nav2 Local Planner Motion Planner / VLA Safety
19. 结论
构建传感器视锥模型的核心步骤是：
1. 定义传感器内参 / FOV / range
2. 定义外参 T_base_sensor
3. 通过 TF 得到 T_map_sensor
4. 构建 near / far 平面或 FOV 判断函数
5. 判断点 / voxel 是否在视锥内
6. 对可见区域执行 marking / clearing
7. 更新 OpenVDB / OctoMap / costmap
总结：
视锥模型是传感器观测空间的几何约束，它决定哪些 voxel 可以被当前传感器更新，以及如何把点云、深度图、LiDAR 数据可靠地融合进 3D 地图和 costmap。
AtomGit 是由开放原子开源基金会联合 CSDN 等生态伙伴共同推出的新一代开源与人工智能协作平台。平台坚持“开放、中立、公益”的理念，把代码托管、模型共享、数据集托管、智能体开发体验和算力服务整合在一起，为开发者提供从开发、训练到部署的一站式体验。
更多推荐
8
0- 0
已为社区贡献71条内容
所有评论(0)