1. 给ICP和NDT配准添加柯西核函数

鲁邦核函数的意义：

在实际优化过程中，很可能会出现误匹配的项，这些误匹配的项被算法当做要降低的误差对待，由于误匹配的“误差”会很大，降低它能够明显使总的误差降低，但是显然这些项不应该被当做误差项对待，因为他会抹平其它正确边的影响，使得优化算法专注于调整一个错误的值。鲁邦核函数正好可以解决这个问题。核函数保证每条边的误差不会大的没边而掩盖其它的边。具体的方式就是，把原先误差的二范数度量替换成一个增长没那么快的函数，同时保证自己的光滑性质。

$下面是柯西核函数在最小二乘问题上的应用$: 此处可以看一下这篇博文，里面讲了非线性最小二乘问题以及核函数。


g2o中柯西函数的实现：

可以去看看g2o是如何使用核函数进行优化的源码，通过看g2o源码对核函数的实现可以粗略总结出，要做的工作分两部分，
1.实现柯西核函数（控制阈值c选择, 残差在正态分布情况下选择2.3849，非正态分布下没有一定值，这里直接设置为1.0）；
2. 根据上面的公式推导拼正规方程。

1.1 代码实现

point2point:

auto H_and_err = std::accumulate(index.begin(), index.end(), std::pair<Mat6d, Vec6d>(Mat6d::Zero(), Vec6d::Zero()),[&jacobians, &errors, &effect_pts, &total_res, &effective_num](const std::pair<Mat6d, Vec6d>& pre,int idx) -> std::pair<Mat6d, Vec6d> {if (!effect_pts[idx]) {return pre;} else {double e2 = errors[idx].dot(errors[idx]);total_res += e2;effective_num++;// Cauchy 鲁棒核函数double delta =  1.0;                    // 控制阈值设置为1.0double delta2 = delta * delta;double delta2_inv = 1.0 / delta2;double aux = delta2_inv * e2 + 1.0;Vec3d rho;rho[0] = delta2  * log(aux);            // Cauchy核函数rho[1] = 1.0 / aux;                     // Cauchy核函数的一阶导数rho[2] = -delta2_inv * pow(rho[1],2);   // Cauchy核函数的二阶导数Mat3d weighted_infos = rho[1] * Mat3d::Identity() + 2 * rho[2] * errors[idx] * errors[idx].transpose();return std::pair<Mat6d, Vec6d>(pre.first + jacobians[idx].transpose() * weighted_infos * jacobians[idx],pre.second - rho[1] * jacobians[idx].transpose() * errors[idx]);}});

几种配准方法代码大同小异这里只贴出point2point的吧。其他ICP类型和NDT可以遵照point2point实现。以下是四种配准方法加了柯西核函数之后的表现。

2. 将第1部分的robust loss引入IncNDTLO和LooselyLIO，给出实现和运行效果

这里在看LooselyLIO代码时，发现其调用的方法就是IncNDTLO，而IncNDTLO在AddCloud中的配准方法就是IncNdt3d。只需要修改src/ch7/ndt_inc.cc文件中的AlignNdt()函数即可。修改和第一题中的代码一致。
运行效果：（加核函数前后表现差异不大）

3. 从概率层面解释NDT残差和协方差矩阵的关系，说明为什么NDT协方差矩阵可以用于最小二乘

4. 为LOAM like LO设计一个地面点云提取流程，并单独为这些点使用点面ICP

论文参考：Fast segmentation of 3D point clouds: A paradigm on LiDAR data for autonomous vehicle applications
地面提取思路：一帧3D点云包含的点数众多，我们要提取地面，可以只选出接近地面的一些点拿来拟合平面，可以降低大量计算。

论文中的LPR算法流程如下：（种子点可以理解为近地点，不过种子点的提取要求雷达与地面大体垂直，不然无法按照论文中思路提取出种子点，因为近地点提取原理就是对激光点云排序，选Z值小的点，如果激光雷达正对着地面，此方法失效。–当然大多数雷达不会这么做。）

4.1 代码实现

void FeatureExtraction::ExtractGroundPlane(CloudPtr point_input_xyz, CloudPtr pc_ground)
{int lpr_max_iters = 100; // lpr算法 最大迭代次数double lpr_least_gpoints_rate = 0.3; // 用于计算近地点阈值的一个比率，按照整帧点云点数来确定点数计算近地点阈值，这个阈值用来提取种子点double lpr_fit_dist_thre = 0.05; // 到平面距离小于此阈值的点就属于平面点// find the lpr planeCloudPtr sort_cloud = point_input_xyz; // 对点云按Z值排序std::sort(sort_cloud->points.begin(), sort_cloud->points.end(),[&](const PointType& p1, const PointType& p2) { return p1.z < p2.z; });// extract init ground seedsdouble lpr_avg_height = 0;size_t lpr_num = static_cast<size_t>(lpr_least_gpoints_rate *sort_cloud->points.size());for (size_t i = 0; i < lpr_num; i++) {lpr_avg_height += sort_cloud->points[i].z;}lpr_avg_height /= lpr_num;CloudPtr pc_for_ground(new sad::PointCloudType);for (size_t i = 0; i < sort_cloud->points.size(); i++) {if (sort_cloud->points[i].z <= lpr_avg_height) {pc_for_ground->points.emplace_back(sort_cloud->points[i]);} else {break;}}// ransac fitting iterativelyPlaneParam plane(Eigen::Vector3d::Zero(), 0);PlaneParam last_pp(Eigen::Vector3d::Zero(), 0);for (int iter_cnt = 0; iter_cnt < lpr_max_iters; iter_cnt++) {// fitting plane{// calculate the mean and covstd::vector<Eigen::Vector3d> points;Eigen::Vector3d mean(0.0, 0.0, 0.0);for (size_t j = 0; j < pc_for_ground->points.size(); j++) {PointType point = pc_for_ground->points[j];Eigen::Vector3d temp(point.x, point.y, point.z);mean += temp;points.emplace_back(temp);}mean = mean / pc_for_ground->points.size();Eigen::Matrix3d cov = Eigen::Matrix3d::Zero();for (size_t j = 0; j < points.size(); j++) {Eigen::Vector3d temp = points[j] - mean;cov = cov + temp * temp.transpose();}// svdEigen::JacobiSVD<Eigen::MatrixXd> svd(cov, Eigen::DecompositionOptions::ComputeFullU);plane.normal = (svd.matrixU().col(2));plane.intercept = -(plane.normal.transpose() * mean)(0, 0);}pc_ground->points.clear();for (size_t j = 0; j < pc_for_ground->points.size(); j++) {PointType point = pc_for_ground->points[j];double point_to_plane_dis = std::fabs(plane.normal(0) * point.x + plane.normal(1) * point.y +plane.normal(2) * point.z + plane.intercept);if (point_to_plane_dis <= lpr_fit_dist_thre) {pc_ground->points.emplace_back(point);} }// convergence checkEigen::Vector3d dlt_norm = plane.normal - last_pp.normal;double dlt_intcpt = plane.intercept - last_pp.intercept;if (dlt_norm.norm() < 0.001 && dlt_intcpt < 0.01 && iter_cnt > lpr_max_iters / 2){LOG(INFO) << "ExtractGroundPlane success!!!";break;}last_pp = plane;}return;
}

$地面提取效果：$

4.2 对地面点进行ICP

相关变量声明：


核心代码实现：

if (options_.use_ground_points_) {std::for_each(std::execution::par_unseq, index_ground.begin(), index_ground.end(), [&](int idx) {Vec3d q = ToVec3d(ground->points[idx]);Vec3d qs = pose * q;// 检查最近邻std::vector<int> nn_indices;kdtree_ground_.GetClosestPoint(ToPointType(qs), nn_indices, 5);effect_ground[idx] = false;if (nn_indices.size() == 5) {std::vector<Vec3d> nn_eigen;for (auto& n : nn_indices) nn_eigen.emplace_back(ToVec3d(local_map_ground_->points[n]));// 点面残差Vec4d n;if (!math::FitPlane(nn_eigen, n)) return;double dis = n.head<3>().dot(qs) + n[3];if (fabs(dis) > options_.max_plane_distance_) return;effect_ground[idx] = true;// build residualEigen::Matrix<double, 1, 6> J;J.block<1, 3>(0, 0) = -n.head<3>().transpose() * pose.so3().matrix() * SO3::hat(q);J.block<1, 3>(0, 3) = n.head<3>().transpose();jacob_ground[idx] = J;errors_ground[idx] = dis;}});}for (const auto& idx : index_ground) {if (effect_ground[idx]) {H += jacob_ground[idx].transpose() * jacob_ground[idx];err += -jacob_ground[idx].transpose() * errors_ground[idx];effective_num++;total_res += errors_ground[idx] * errors_ground[idx];}}

实现效果：
①仅使用提取的地面点来进行定位：（可以看到效果非常不好，地面提取算法可能还需要优化，也有可能本身只使用地面点来配准就效果很差，这个可以继续尝试优化。）

②使用角点和地面点一起定位：（明显角点的匹配很好的把地面匹配的结果矫正了，但是一开始部分还是不行。）

5. 也欢迎大家来我的微信公众号–过千帆，来读书，提高我们的认知。