1. 理论推导
2.代码实现
由于第7章已经有个icp_3d类了,并且有了点面ICP功能,这里直接在其基础上添加实现基于点面ICP的IEKF LIO系统对观测方程的处理方式,即:
void Icp3d::ComputeResidualAndJacobians(const SE3& input_pose, Mat18d& HTVH, Vec18d& HTVr)
对观测方程的处理代码实现(核心):
void Icp3d::ComputeResidualAndJacobians(const SE3& input_pose, Mat18d& HTVH, Vec18d& HTVr)
{
LOG(INFO) << "aligning with point to plane";
assert(target_ != nullptr && source_ != nullptr);
SE3 pose = input_pose;
int cnt_pts = source_->points.size();
std::vector<int> index(cnt_pts);
for (int i = 0; i < index.size(); ++i)
index[i] = i;
std::vector<bool> effect_pts(index.size(), false); // 用于标记有效点
std::vector<Eigen::Matrix<double, 1, 18>> jacobians(index.size()); // 用于存储雅可比矩阵
std::vector<double> errors(index.size()); // 用于存储残差
// gauss-newton 迭代
// 最近邻,可以并发
std::for_each(std::execution::par_unseq, index.begin(), index.end(), [&](int idx) {
// 并发遍历到点云中的某个点,不是按顺序遍历的
auto q = ToVec3d(source_->points[idx]);
Vec3d qs = pose * q; // 雷达系转换到IMU系:P_I = R_IL * P_L + T_IL
std::vector<int> nn;
kdtree_->GetClosestPoint(ToPointType(qs), nn, 5); // 这里取5个最近邻
if (nn.size() > 3) {
// convert to eigen
std::vector<Vec3d> nn_eigen;
for (int i = 0; i < nn.size(); ++i) {
nn_eigen.emplace_back(ToVec3d(target_->points[nn[i]]));
}
Vec4d n;
if (!math::FitPlane(nn_eigen, n)) {
// 失败的不要
effect_pts[idx] = false;
return;
}
double dis = n.head<3>().dot(qs) + n[3];
if (fabs(dis) > options_.max_plane_distance_) {
// 点离的太远了不要
effect_pts[idx] = false;
return;
}
effect_pts[idx] = true;
// build residual
Eigen::Matrix<double, 1, 18> J;
J.setZero();
J.block<1, 3>(0, 0) = n.head<3>().transpose();
J.block<1, 3>(0, 6) = -n.head<3>().transpose() * pose.so3().matrix() * SO3::hat(q);
jacobians[idx] = J;
errors[idx] = dis;
} else {
effect_pts[idx] = false;
}
});
double total_res = 0;
int effective_num = 0;
const double info_ratio = 0.01; // 每个点反馈的info因子
auto H_and_err = std::accumulate(
index.begin(), index.end(), std::pair<Mat18d, Vec18d>(Mat18d::Zero(), Vec18d::Zero()),
[&jacobians, &errors, &effect_pts, &total_res, &effective_num](const std::pair<Mat18d, Vec18d>& pre,
int idx) -> std::pair<Mat18d, Vec18d> {
if (!effect_pts[idx]) {
return pre;
} else {
total_res += errors[idx] * errors[idx];
effective_num++;
return std::pair<Mat18d, Vec18d>(pre.first + jacobians[idx].transpose() * jacobians[idx] * info_ratio,
pre.second - jacobians[idx].transpose() * errors[idx] * info_ratio);
}
});
HTVH = H_and_err.first;
HTVr = H_and_err.second;
LOG(INFO) << "effective: " << effective_num;
}
结果展示: