前面介绍了多传感器后融合算法系统的框架设计、多传感器目标关联算法以及求解状态估计问题常用的卡尔曼滤波算法，这里使用一些真实传感器的目标数据来进行相关内容的实战。仅做学习用途。本节核心就是基于卡尔曼滤波实现LiDAR和Camera的后融合检测算法（支持Radar扩展）。
1. 融合器数据输入格式
这里简单介绍一下不同传感器目标数据的输入格式。
LiDARObject
主要包含3D的位置、速度和一些其他属性（如尺寸、类别和置信度等）。
// 单个LiDAR目标信息
struct LiDARObject {
double time_ns = 0.0f;
int id = -1;
float x = 0.0f;
float y = 0.0f;
float z = 0.0f;
float length = 0.0f;
float width = 0.0f;
float height = 0.0f;
float velo_x = 0.0f;
float velo_y = 0.0f;
float velo_z = 0.0f;
int label = -1;
float confidence = 0.0f;
};
using LiDARObjectPtr = std::shared_ptr<LiDARObject>;
// 单帧多个LiDAR目标信息
struct LiDARObjectList {
double time_ns = 0.0f;
std::string frame_id = "";
std::vector<LiDARObjectPtr> objs;
};
using LiDARObjectListPtr = std::shared_ptr<LiDARObjectList>;
CameraObject
主要包含2D的目标属性信息。
// 单个Camera目标信息
struct CameraObject {
double time_ns = 0.0f;
int id = -1;
float ux = 0.0;
float vy = 0.0;
float width = 0.0f;
float height = 0.0f;
int label = -1;
float confidence = 0.0f;
};
using CameraObjectPtr = std::shared_ptr<CameraObject>;
// 单帧多个Camera目标信息
struct CameraObjectList {
double time_ns = 0.0f;
std::string frame_id = "";
std::vector<CameraObjectPtr> objs;
};
using CameraObjectListPtr = std::shared_ptr<CameraObjectList>;
RadarObject
主要包含位置、速度和雷达截面反射强度RCS信息。
// 单个Radar目标信息
struct RadarObject {
double time_ns = 0.0f;
int id = -1;
float x = 0.0f;
float y = 0.0f;
float z = 0.5f;
float velo_x = 0.0f;
float velo_y = 0.0f;
float velo_z = 0.0f;
float angle = 0.0f;
float rcs = 0.0f;
};
using RadarObjectPtr = std::shared_ptr<RadarObject>;
// 单帧多个Radar目标信息
struct RadarObjectList {
double time_ns = 0.0f;
std::string frame_id = "";
std::vector<RadarObjectPtr> objs;
};
using RadarObjectListPtr = std::shared_ptr<RadarObjectList>;
FusionObject
基于多模传感器目标数据融合之后完整的3D bounding box数据，即不同目标信息的并集。
// 单个Fusion目标信息
struct FusionObject {
double time_ns = 0.0f; // 存储对象的时间戳（单位可能是纳秒）
int id = -1; // 描述对象ID
float x = 0.0f; // 目标的三维空间位置
float y = 0.0f;
float z = 0.0f;
float length = 0.0f; // 目标的三维尺寸
float width = 0.0f;
float height = 0.0f;
float velo_x = 0.0f; // 目标的三维速度
float velo_y = 0.0f;
float velo_z = 0.0f;
int label = -1; // 目标的类别标签
float confidence = 0.0f; // 目标的置信度
float ux = 0.0f; // 目标在二维图像中的横坐标(像素坐标系)
float vy = 0.0f; // 目标在二维图像中的纵坐标(像素坐标系)
float width_2d = 0.0f; // 目标在二维图像中的宽度
float height_2d = 0.0f; // 目标在二维图像中的高度
int life_duration = 0; // 目标的生命周期
float rcs = 0.0f; // 目标的雷达截面积反射强度RCS
// 通过一个仿射变换（Affine transformation）来更新对象的位置和速度：线性变换”+“平移”
void transformBy(const Eigen::Affine3d& trans) {
Eigen::Vector3d pos(x, y, z); Eigen::Vector3d velo(velo_x, velo_y, velo_z);
// 使用仿射变换 trans 来更新对象的位置。
pos = trans * pos;
// 仅使用仿射变换的线性部分（不包括平移）来更新对象的速度.
velo = trans.linear() * velo;
x = pos(0);
y = pos(1);
z = pos(2);
velo_x = velo(0);
velo_y = velo(1);
velo_z = velo(2);
}
};
using FusionObjectPtr = std::shared_ptr<FusionObject>;
// 单帧多个Fusion目标信息
struct FusionObjectList {
double time_ns = 0.0f;
std::string frame_id = "";
std::vector<FusionObjectPtr> objs;
};
using FusionObjectListPtr = std::shared_ptr<FusionObjectList>;
Frame
除了传感器各自的数据格式，以及融合数据格式外，我们是希望将近似同一时刻的传感器数据进行融合的，这里可以定义一个帧（包含不同传感器同一时刻的检测数据）。
struct Frame {
double time_ns = 0.0f;
LiDARObjectListPtr lidar_objs;
CameraObjectListPtr camera_objs;
RadarObjectListPtr radar_objs;
};
using FramePtr = std::shared_ptr<Frame>;
2. 融合器架构设计
在后融合系统问题建模中提到，多模数据融合需要考虑如下三个问题：观测量时空对齐问题、目标关联问题和多观测条件下的系统状态估计问题。
因此融合器也可以封装成三部分：预测模块（Predictor）、匹配模块（Matcher）、融合更新模块（Tracker）。这里需要强调的一点是，融合框架采用主从传感器的设计方式，即
- 主传感器（LiDAR）可创建状态量中的目标，主从传感器（LiDAR/Camera/Radar等）能更新状态量；
- 融合触发方式为支持所有传感器触发，即任何模态数据更新，立即触发多传感器融合动作；
class FusionWrapper {
public:
FusionWrapper(const std::string& config_file);
// 融合器主入口：输入多模传感器帧数据
bool Update(const FramePtr &frame);
bool GetFusionResult(const FusionObjectListPtr &res);
private:
std::shared_ptr<fusion::Tracker> tracker_; // 跟踪更新模块（KF/EKF算法）
std::shared_ptr<fusion::Matcher> matcher_; // 匹配模块(KM算法)
std::shared_ptr<fusion::Predictor> predictor_; // 预测模块（时空外推对齐）
};
2.1 预测模块
对于不同频率的LiDAR、Camera、Radar检测结果，将其输入到预测模块中，根据自车的运动和目标的状态估计，进行时空对齐，使得融合时的状态量和观测量是位于同一时空下的。这里自车使用匀速运动模型来进行简单估计，将
S
t
a
t
e
t
−
1
State_{t-1}
Statet−1时刻数据预测到观测所在的 t 时刻。
S
t
a
t
e
t
=
T
e
g
o
t
t
−
1
∗
S
t
a
t
e
t
−
1
State_t={T_{ego}}^{t-1}_t*State_{t-1}
Statet=Tegott−1∗Statet−1
- S t a t e t − 1 State_{t-1} Statet−1包含位置、速度等信息；
- T e g o t t − 1 {T_{ego}}^{t-1}_t Tegott−1表示从 t-1 时刻到 t 时刻，自车的位姿变化量；
class Predictor {
public:
Predictor() = default;
bool Predict(const FusionObjectListPtr &fusion_obj_list, double ts);
}; // class Predictor
/* 函数功能：用于将输入目标数据预测对齐到指定时间戳 ts 位置
参数：需要预测的目标数据 fusion_obj_list，以及 目标时间戳
输出：预测操作执行结果
*/
bool Predictor::Predict(const FusionObjectListPtr &fusion_obj_list, double ts) {
// Predict global objects to local timestamp
if(fusion_obj_list->objs.empty()) return true;
// 计算时间差（obj -> ts）及自车位姿变换矩阵
float dt = ts - fusion_obj_list->time_ns;
Eigen::MatrixXd T_ego;
T_ego = Eigen::MatrixXd(5,5);
T_ego << 1, 0, dt, 0, 0,
0, 1, 0, dt, 0,
0, 0, 1, 0, 0,
0, 0, 0, 1, 0,
0, 0, 0, 0, 1;
for(size_t i = 0; i < fusion_obj_list->objs.size(); ++i){
Eigen::VectorXd state(5);
// 目标运动补偿值计算
state << fusion_obj_list->objs[i]->x,
fusion_obj_list->objs[i]->y,
fusion_obj_list->objs[i]->velo_x,
fusion_obj_list->objs[i]->velo_y,
fusion_obj_list->objs[i]->label;
state = T_ego * state;
// 目标运动套补偿值更新
fusion_obj_list->objs[i]->x = state[0];
fusion_obj_list->objs[i]->y = state[1];
fusion_obj_list->objs[i]->velo_x = state[2];
fusion_obj_list->objs[i]->velo_y = state[3];
fusion_obj_list->objs[i]->label = state[4];
}
// 预测结束后需要将目标帧的时间戳与预测时刻保持一致
fusion_obj_list->time_ns = ts;
return true;
}
2.2 目标关联模块
不同模态的目标关联代价可以使用如下方式进行度量，在帧和帧目标得到代价权重矩阵之后，可以使用KM算法求解带权二分图的最优匹配。进而得到目标与目标之间的匹配关联关系。
LiDAR目标和Fusion目标匹配度量
通过3D目标中心点的距离度量即可。（KM求解是最小权匹配问题）
float Matcher::IoULiDARToFusion(const LiDARObjectPtr& lidar_obj, const FusionObjectPtr& fusion_obj) {
return std::sqrt(std::pow(lidar_obj->x - fusion_obj->x, 2) +
std::pow(lidar_obj->y - fusion_obj->y, 2));
}
Radar目标和Fusion目标匹配度量
通过2D目标中心店与3D目标中心点距离度量即可。（KM求解是最小权匹配问题）
float Matcher::IoURadarToFusion(const RadarObjectPtr& radar_obj, const FusionObjectPtr& fusion_obj) {
float dist = 0.0f;
dist = std::sqrt(std::pow(radar_obj->x - fusion_obj->x, 2) +
std::pow(radar_obj->y - fusion_obj->y, 2));
return dist;
}
Camera目标和Fusion目标匹配度量
需要先将融合目标的3D Box信息通过内外参转换投影到像素坐标系下，计算2D Box的交并比。
struct BBox2D {
float x = 0.0f;
float y = 0.0f;
float width = 0.0f;
float height = 0.0f;
};
struct BBox3D {
float x = 0.0f;
float y = 0.0f;
float z = 0.0f;
float width = 0.0f;
float height = 0.0f;
float length = 0.0f;
};
float Matcher::IoUCamToFusion(
const CameraObjectPtr& cam_obj, const FusionObjectPtr& fusion_obj) {
float dist = 0.0f;
BBox2D pred;
pred.x = cam_obj->ux;
pred.y = cam_obj->vy;
pred.width = cam_obj->width;
pred.height = cam_obj->height;
BBox3D tgt;
tgt.x = fusion_obj->x;
tgt.y = fusion_obj->y;
tgt.z = fusion_obj->z;
tgt.width = fusion_obj->width;
tgt.height = fusion_obj->height;
tgt.length = fusion_obj->length;
dist = IoUIn2D(pred, tgt, extrinsic_camera_to_baselink_.inverse(), cam_intrinsic_);
return dist;
}
float IoUIn2D(const BBox2D &box_2d, const BBox3D &box_3d,
const Eigen::Affine3d& extrinsic, const Vector6d& intrinsic) {
BBox2D tgt;
// 将3D Box投影成 2D Box
Project3DBoxTo2DBox(box_3d, extrinsic, intrinsic, tgt);
return IoUIn2D(box_2d, tgt);
}
float IoUIn2D(const BBox2D &pred, const BBox2D &tgt) {
float x1_min = pred.x - pred.width * 0.5;
float y1_min = pred.y - pred.height * 0.5;
float x1_max = pred.x + pred.width * 0.5;
float y1_max = pred.y + pred.height * 0.5;
float x2_min = tgt.x - tgt.width * 0.5;
float y2_min = tgt.y - tgt.height * 0.5;
float x2_max = tgt.x + tgt.width * 0.5;
float y2_max = tgt.y + tgt.height * 0.5;
if ((x1_max <= x2_min || y1_max <= y2_min) ||
(x2_max <= x1_min || y2_max <= y1_min)) {
return 0.0;
}
float x_min = std::max(x1_min, x2_min);
float y_min = std::max(y1_min, y2_min);
float x_max = std::min(x1_max, x2_max);
float y_max = std::min(y1_max, y2_max);
float inter = (y_max - y_min) * (x_max - x_min);
float uni = (pred.width * pred.height + tgt.width * tgt.height) - inter;
return inter / uni;
}
2.2.1 KM算法求解目标匹配度
匹配模块的算法流程：
- 根据输入数据类型的不同计算目标之间的匹配权重，构建带权二分图；
- 利用KM算法求解二分图的最优匹配方式；
- 输出匹配结果
// 以LiDARObject和FusionObject的匹配为例：
bool Matcher::Match(const LiDARObjectListPtr& lidar_obj_list,
const FusionObjectListPtr& global_obj_list,
std::map<size_t, int>& lidar_global_map) {
lidar_global_map.clear();
for (size_t i = 0; i < lidar_obj_list->objs.size(); ++i) {
lidar_global_map[i] = -1;
}
if (global_obj_list->objs.size() == 0) {
return true;;
}
// 构建带权二分图
std::vector<std::vector<float>> weights;
for (size_t i = 0; i < lidar_obj_list->objs.size(); ++i) {
std::vector<float> row(global_obj_list->objs.size(), 0);
weights.push_back(row);
for (size_t j = 0; j < global_obj_list->objs.size(); ++j) {
weights[i][j] = IoULiDARToFusion(lidar_obj_list->objs[i], global_obj_list->objs[j]);
}
}
// 调用KM算法进行目标匹配
auto f = [&](size_t r, size_t c) {return weights[r][c];};
auto res = munkres_algorithm<float>(lidar_obj_list->objs.size(),
global_obj_list->objs.size(),
f);
auto is_matching = [&](const size_t r, const size_t c) {
const auto ii = std::find_if(begin(res), end(res), [&](const auto& x) {
return (x.first == r and x.second == c);
});
return ii != end(res);
};
// 输出匹配结果
for (size_t i = 0; i < lidar_obj_list->objs.size(); ++i) {
for (size_t j = 0; j < global_obj_list->objs.size(); ++j) {
if (is_matching(i, j)) {
lidar_global_map[i] = j;
}
}
}
return true;
}
2.3 状态估计模块
在得到目标状态量与观测量之间的匹配关系之后，就可以使用卡尔曼滤波来执行融合操作了。这里以LiDAR观测与全局目标的融合更新为例介绍主要流程：
- 取出匹配成功的状态目标和观测目标，并将其输入到对应的滤波器模块；
- 注意，如果是首次输入，这里需要通过主传感器LiDAR的观测来构建全局目标状态量；同时为对应目标创建滤波器；
- 滤波器依次执行 预测 -> 更新 操作；
- 状态目标更新。
bool Tracker::Update(const LiDARObjectListPtr &lidar_obj_list,
const std::map<size_t, int>& map) {
for (auto m : map) {
// 如果存在满足匹配的 全局目标
if (m.second >= 0) {
Measurement lidar_mea;
const auto &lidar_obj = lidar_obj_list->objs[m.first];
const auto &global_obj = global_obj_list_->objs[m.second];
lidar_mea.sensor_type = SensorType::LIDAR;
Eigen::VectorXd meas(5);
meas << lidar_obj->x,
lidar_obj->y,
lidar_obj->velo_x,
lidar_obj->velo_y,
lidar_obj->label;
lidar_mea.time_ns = lidar_obj->time_ns;
lidar_mea.meas = meas;
State state;
meas << global_obj->x,
global_obj->y,
global_obj->velo_x,
global_obj->velo_y,
global_obj->label;
state.time_ns = global_obj->time_ns;
state.meas = meas;
// 通过卡尔曼滤波器融合 全局状态量 和 lidar 观测量
motion_filters_[m.second]->UpdateMotion(state, lidar_mea);
global_obj->x = motion_filters_[m.second]->GetState()[0];
global_obj->y = motion_filters_[m.second]->GetState()[1];
global_obj->z = lidar_obj->z;
global_obj->velo_x = motion_filters_[m.second]->GetState()[2];
global_obj->velo_y = motion_filters_[m.second]->GetState()[3];
global_obj->label = motion_filters_[m.second]->GetState()[4];
global_obj->length = lidar_obj->length;
global_obj->width = lidar_obj->width;
global_obj->height = lidar_obj->height;
int dura = global_obj_list_->objs[m.second]->life_duration;
global_obj_list_->objs[m.second]->life_duration = (dura < startup_duration_ ? dura+1 : startup_duration_);
std::cout << "updating exist global object: " << global_obj_list_->objs[m.second]->ToString() << std::endl;
} else {
// 通过 lidar 数据创建新的 全局状态量
FusionObjectPtr new_obj =
std::make_shared<FusionObject>(FusionObject());
const auto &lidar_obj = lidar_obj_list->objs[m.first];
new_obj->time_ns = lidar_obj->time_ns;
new_obj->id = lidar_obj->id;
new_obj->x = lidar_obj->x;
new_obj->y = lidar_obj->y; new_obj->z = lidar_obj->z;
new_obj->length = lidar_obj->length;
new_obj->width = lidar_obj->width;
new_obj->height = lidar_obj->height;
new_obj->velo_x = lidar_obj->velo_x;
new_obj->velo_y = lidar_obj->velo_y;
new_obj->velo_z = lidar_obj->velo_z;
new_obj->label = lidar_obj->label;
new_obj->confidence = lidar_obj->confidence;
new_obj->life_duration = startup_duration_;
global_obj_list_->objs.push_back(new_obj);
std::shared_ptr<FusionEKF> motion_filter =
std::make_shared<FusionEKF>(FusionEKF());
motion_filters_.push_back(motion_filter);
std::cout << "create new global object: " << new_obj->ToString() << std::endl;
}
global_obj_list_->time_ns = lidar_obj_list->time_ns;
}
return true;
}
2.3.1 卡尔曼滤波器-预测
预测就是输入前一时刻的最优状态
(
x
^
k
−
1
,
P
^
)
(\hat{x}_{k-1},\hat{P})
(x^k−1,P^)，外界对过程的影响
u
k
u_k
uk，环境的不确定度
Q
k
Q_k
Qk后，通过状态转移方程来预测当前时刻系统状态
(
x
k
ˉ
,
P
k
ˉ
)
(\bar{x_k},\bar{P_k})
(xkˉ,Pkˉ)的过程；
x
k
ˉ
=
F
k
x
^
k
−
1
+
B
k
u
k
(1)
\tag{1} \bar{x_k}=F_k\hat{x}_{k-1}+B_ku_k
xkˉ=Fkx^k−1+Bkuk(1)
P
k
ˉ
=
F
k
P
^
k
−
1
F
k
T
+
Q
k
(2)
\tag{2} \bar{P_k} = F_k\hat{P}_{k-1}F_k^T+Q_k
Pkˉ=FkP^k−1FkT+Qk(2)
其中：
- x k T = [ x , y , v x , v y , l a b e l ] x_k^T=[x,y,v_x,v_y,label] xkT=[x,y,vx,vy,label]表示系统在 k 时刻的状态量； P k P_k Pk表示状态量 x k x_k xk对应的协方差信息；
- u k u_k uk表示外界输入，如油门刹车等； B k B_k Bk表示控制矩阵（描述油门刹车影响运动量改变的变换矩阵）；
- F k F_k Fk是状态转移矩阵，描述系统前一个时刻到下一个时刻的状态转移情况。
- Q k Q_k Qk是过程噪声协方差矩阵（如风，地面摩擦等）。一般是形式为 w k ∼ N ( 0 , Q k ) w_k\sim N(0, Q_k) wk∼N(0,Qk)的高斯噪声。
void KalmanFilter::Predict() {
// 状态预测及协方差更新
x_ = F_ * x_;
MatrixXd Ft = F_.transpose();
P_ = F_ * P_ * Ft + Q_;
}
2.3.2 卡尔曼滤波器-更新
更新就是输入预测得到的当前时刻系统状态量
(
x
ˉ
k
,
P
k
ˉ
)
(\bar{x}_{k}, \bar{P_k})
(xˉk,Pkˉ)，以及传感器得到的当前时刻的观测量
(
z
k
,
R
k
)
(z_k,R_k)
(zk,Rk)后，通过两者协方差信息得到卡尔曼增益K后，利用观测值对系统状态进行修正的过程。修正值一般认为是当前时刻估计出的最优状态
(
x
^
k
,
P
k
^
)
(\hat{x}_{k}, \hat{P_k})
(x^k,Pk^)。
K
=
P
k
ˉ
H
k
T
(
H
k
P
k
ˉ
H
k
T
+
R
k
)
−
1
(3)
\tag{3} K = \bar{P_k}H_k^T(H_k\bar{P_k}H_k^T+R_k)^{-1}
K=PkˉHkT(HkPkˉHkT+Rk)−1(3)
x
k
^
=
x
k
ˉ
+
K
(
z
k
−
H
k
x
k
ˉ
)
(4)
\tag{4} \hat{x_k} = \bar{x_k} + K(z_k-H_k\bar{x_k})
xk^=xkˉ+K(zk−Hkxkˉ)(4)
P
k
^
=
P
k
ˉ
−
K
H
k
P
k
ˉ
=
(
I
−
K
H
k
)
P
k
ˉ
(5)
\tag{5} \hat{P_k} = \bar{P_k}-KH_k\bar{P_k}=(I-KH_k)\bar{P_k}
Pk^=Pkˉ−KHkPkˉ=(I−KHk)Pkˉ(5)
其中：
- H k H_k Hk是观测矩阵，描述是传感器观测量与状态量的转化关系，如传感器测的是cm，状态量表示为m等。
- R k R_k Rk是观测噪声协方差矩阵（如传感器测量误差）。一般是形式为 v k ∼ N ( 0 , R k ) v_k\sim N(0, R_k) vk∼N(0,Rk)的高斯噪声。
- z k z_k zk是传感器观测量，其形式与传感器类型及测量方式有关。
- K 是卡尔曼增益，描述的是系统状态量与传感器观测量之间的权重关系，大小受各自协方差信息的影响。
void KalmanFilter::Update(const VectorXd &z) {
VectorXd z_pred = H_ * x_;
VectorXd y = z - z_pred;
MatrixXd Ht = H_.transpose();
MatrixXd PHt = P_ * Ht;
MatrixXd S = H_ * PHt + R_;
MatrixXd Si = S.inverse();
MatrixXd K = PHt * Si;
// 后验状态估计
x_ = x_ + (K * y);
long x_size = x_.size();
MatrixXd I = MatrixXd::Identity(x_size, x_size);
P_ = (I - K * H_) * P_;
}
2.3.3 卡尔曼滤波器主流程
在使用预测和更新模块之前，我们还需要明确一下除状态量
x
k
x_k
xk和观测量
z
k
z_k
zk以外的输入输出部分。除上图中可以看到，在卡尔曼滤波器-预测模块工作时，我们需要进行一些关键信息的初始化，如F、Q、H和R等，其中
- F 为状态转移矩阵，一般与系统选择的运动模型有关，这里采用匀速运动模型，因此外部输入Bu为0；
- Q为过程噪声协方差矩阵，一般时间越长不确定性越高；
- H为观测矩阵，与传感器类型及测量方式有关。如 lidar融合时本身就是3D box，可以选择单位阵；
- R观测噪声协方差矩阵，与传感器测量特性有关。如 lidar测距测速都比较准确，那么对应位置元素的方差可以设置的小一些。
{R_laser_ = MatrixXd(5, 5);
H_laser_ = MatrixXd(5, 5);
P_ = MatrixXd(5, 5);
R_laser_ << 0.0225, 0, 0, 0, 0,
0, 0.0225, 0, 0, 0,
0, 0, 0.0225, 0, 0,
0, 0, 0, 0.0225, 0,
0, 0, 0, 0, 5; // 经验值-lidar观测噪声协方差矩阵
H_laser_ << 1, 0, 0, 0, 0,
0, 1, 0, 0, 0,
0, 0, 1, 0, 0,
0, 0, 0, 1, 0,
0, 0, 0, 0, 1;
// 经验值：过程噪声，时间越长不确定性越高
noise_ax = 9;
noise_ay = 9;
noise_attr_ = 5;
}
当外部信息输入确认之后，就可以使用状态量 x k − 1 ^ \hat{x_{k-1}} xk−1^和观测量 z k z_k zk进行状态量的预测和更新了。主流程如下：
// 卡尔曼滤波器工作主流程
void FusionEKF::UpdateMotion(const State &state, const Measurement &measurement) {
/*****************************************************************************
* Initialization
****************************************************************************/
motion_filter_.x_ = VectorXd(5);
motion_filter_.P_ = P_;
if (measurement.sensor_type == SensorType::LIDAR) {
// Initialize state.
motion_filter_.x_ << state.meas[0], state.meas[1], state.meas[2], state.meas[3],state.meas[4];
}
/*****************************************************************************
* Prediction-预测
****************************************************************************/
float dt = measurement.time_ns - state.time_ns; // in seconds
float dt_2 = dt * dt;
float dt_3 = dt_2 * dt;
float dt_4 = dt_3 * dt;
// 更新状态转移矩阵
motion_filter_.F_ = MatrixXd(5, 5);
motion_filter_.F_ << 1, 0, dt, 0, 0,
0, 1, 0, dt, 0,
0, 0, 1, 0, 0,
0, 0, 0, 1, 0,
0, 0, 0, 0, 1;
// 更新过程噪声协方差矩阵
// Use noise_ax = 9 and noise_ay = 9 for your Q matrix.(经验值)
motion_filter_.Q_ = MatrixXd(5, 5);
motion_filter_.Q_ << dt*noise_ax, 0, 0, 0, 0,
0, dt*noise_ay, 0, 0, 0,
0, 0, dt*noise_ax, 0, 0,
0, 0, 0, dt*noise_ay, 0,
0, 0, 0, 0, dt*noise_attr_;
motion_filter_.Predict();
/*****************************************************************************
* Update-更新
****************************************************************************/
if(measurement.sensor_type == SensorType::LIDAR){
motion_filter_.H_ = H_laser_;
motion_filter_.R_ = R_laser_;
motion_filter_.Update(measurement.meas);
}
return;
}
3. 框架主流程
前面分别介绍了后融合模块的输入数据格式、后融合框架部分（预测、目标关联和状态估计模块），对相关内容整合之后，就可以实现完整的数据处理链路了。即
- 输入多模态数据，构建观测帧Frame，并将观测帧数据输入到Fusion融合模块中。
int main(int argc, char** argv)
{
//argv[1] config, argv[2] location, argv[3] camera, argv[4] lidar, argv[5] ground truth
ros::init(argc, argv, "fusion_node");
if (argc != 5)
{
std::cout << "Need 4 args!\n";
return 0;
}
std::vector<std::tuple<double, proto_input::FileTag, std::string> >&& data_frame_seq = proto_input::mergeInputSequence({
proto_input::buildSensorInputSequence(argv[2], "Location", proto_input::FileTag::LOCATION),
proto_input::buildSensorInputSequence(argv[3], "Lidar", proto_input::FileTag::CAMERA),
proto_input::buildSensorInputSequence(argv[4], "Lidar", proto_input::FileTag::LIDAR),
//proto_input::buildSensorInputSequence(argv[5], "Radar", FileTag::RADAR),
});
ros::NodeHandle nh;
ros::Publisher lidar_pub = nh.advertise<sensor_msgs::PointCloud>("lidar_det", 50);
ros::Publisher res_pub = nh.advertise<sensor_msgs::PointCloud>("fusion_result", 50);
ros::Rate r(2);
ezcfg::Interpreter itp;
kit::perception::fusion::FusionWrapper fusion{ argv[1] };
for (auto it = data_frame_seq.begin(); it != data_frame_seq.end();)
{
//get vehicle position and orientation
itp.loadFile(std::get<2>(*it));
proto_input::Location pose;
itp.lex.matchID("header");
itp.parse(pose.header);
itp.lex.matchID("pose");
itp.parse(pose.pose);
++it;
std::shared_ptr<kit::perception::fusion::Frame> frame = std::make_shared<kit::perception::fusion::Frame>();
// 获取 camera 检测的目标数据
if (std::get<1>(*it) == proto_input::FileTag::CAMERA)
{
proto_input::CameraObject raw_co;
kit::perception::fusion::CameraObjectListPtr co_list = std::make_shared<kit::perception::fusion::CameraObjectList>();
itp.loadFile(std::get<2>(*it));
while (itp.lex.getToken() == ezcfg::Token::ID && itp.lex.getTokenText() == "perception_obstacle")
{
itp.lex.next();
itp.parse(raw_co);
co_list->objs.push_back(makeCameraObjectPtr(raw_co, std::get<0>(*it)));
}
co_list->time_ns = std::get<0>(*it);
frame->camera_objs = std::move(co_list);
++it;
}
// 获取 lidar 检测的目标数据
if (std::get<1>(*it) == proto_input::FileTag::LIDAR)
{
proto_input::LidarObject raw_lo;
kit::perception::fusion::LiDARObjectListPtr lo_list = std::make_shared<kit::perception::fusion::LiDARObjectList>();
itp.loadFile(std::get<2>(*it));
while (itp.lex.getToken() == ezcfg::Token::ID && itp.lex.getTokenText() == "perception_obstacle")
{
itp.lex.next();
itp.parse(raw_lo);
raw_lo.anchor_point.x = raw_lo.anchor_point.x - pose.pose.position.x;
raw_lo.anchor_point.y = raw_lo.anchor_point.y - pose.pose.position.y;
raw_lo.anchor_point.z = raw_lo.anchor_point.z - pose.pose.position.z;
lo_list->objs.push_back(makeLiDARObjectPtr(raw_lo, std::get<0>(*it)));
}
lidar_pub.publish(transToPointCloud(*lo_list));
lo_list->time_ns = std::get<0>(*it);
frame->lidar_objs = std::move(lo_list);
++it;
}
// 获取 radar 检测的目标数据（因参考资料中的数据原因，暂不实现，但可在此处添加扩展）
if (std::get<1>(*it) == proto_input::FileTag::RADAR){}
// 融合器Fusion
fusion.Update(frame);
kit::perception::fusion::FusionObjectListPtr fusion_res = std::make_shared<kit::perception::fusion::FusionObjectList>(kit::perception::fusion::FusionObjectList());
fusion.GetFusionResult(fusion_res);
//result output
std::cout << "********************************************************************\n";
for (size_t i = 0; i < fusion_res->objs.size(); i++)
std::cout << fusion_res->objs[i]->ToString() << std::endl;
res_pub.publish(transToPointCloud(*fusion_res));
std::cout << "********************************************************************\n";
r.sleep();
}
ros::shutdown();
return 0;
}
- 融合模块对Frame数据进行处理。
bool FusionWrapper::Update(const FramePtr &frame) {
fusion::FusionObjectListPtr global_obj_list =
std::make_shared<fusion::FusionObjectList>(fusion::FusionObjectList());
// 融合 lidar 观测数据
if (frame->lidar_objs->objs.size() > 0) {
std::cout << "updating with lidar objects..." << std::endl;
tracker_->GetGlobalObjects(global_obj_list);
if (!predictor_->Predict(global_obj_list, frame->lidar_objs->time_ns)) {
std::cout << "predict error for lidar measurement." << std::endl;
return false;
}
std::map<size_t, int> local_global_map;
if (!matcher_->Match(frame->lidar_objs, global_obj_list, local_global_map)) {
std::cout << "match failed for lidar and global." << std::endl;
return false;
}
tracker_->Update(frame->lidar_objs, local_global_map);
}
// 融合 radar 观测数据
if (frame->radar_objs->size() > 0) {
PreProcess(frame->radar_objs, extrinsic_radar_to_baselink_);
tracker_->GetGlobalObjects(global_obj_list);
if (!predictor->Predict(global_obj_list, frame->lidar_objs->time_ns)) {
std::cout << "predict error for radar measurement." << std::endl;
return false;
}
std::map<size_t, int> associate_arr;
matcher_->Match(frame->radar_objs, global_obj_list, associate_arr);
tracker_->Update(frame->radar_objs, global_obj_list, associate_arr);
}
// 融合 camera 观测数据
if (frame->camera_objs->objs.size() > 0) {
std::cout << "updating with camera objects..." << std::endl;
tracker_->GetGlobalObjects(global_obj_list);
if (!predictor_->Predict(global_obj_list, frame->camera_objs->time_ns)) {
std::cout << "predict error for camera measurement." << std::endl;
return false;
}
std::map<size_t, int> local_global_map;
if (!matcher_->Match(frame->camera_objs, global_obj_list, local_global_map)) {
std::cout << "match failed for camera and global." << std::endl;
return false;
}
tracker_->Update(frame->camera_objs, local_global_map);
}
return true;
}
运行效果-单目标：
单目标匹配跟踪可视化
运行效果-多目标：
多目标匹配跟踪可视化
4. 问题记录：
在熟悉代码过程中发现，虽然初始维护的全局目标和滤波器数量是一致的，但是当目标增多之后，两者数量并不相同。分析发现是在进行全局目标的声明周期管理时，只进行了全局目标的擦除操作，并没有对该目标对应的滤波器进行操作，导致滤波器数量一致在上升，这也意味着状态量的协方差和滤波器中维护的前一时刻的后验状态协方差是不一致的。因此在进行全局目标声明周期管理时，除了需要擦除全局目标外，还需要将其相应的滤波器进行擦除，以保证全局目标数量和滤波器数量的一致性，以及全局目标协方差信息的连续性。
修改前：
修改后：
扫一扫
1194
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
