鲁邦核函数的意义 ：
在实际优化过程中，很可能会出现误匹配的项，这些误匹配的项被算法当做要降低的误差对待，由于误匹配的“误差”会很大，降低它能够明显使总的误差降低，但是显然这些项不应该被当做误差项对待，因为他会抹平其它正确边的影响，使得优化算法专注于调整一个错误的值。鲁邦核函数正好可以解决这个问题。核函数保证每条边的误差不会大的没边而掩盖其它的边。具体的方式就是，把原先误差的二范数度量替换成一个增长没那么快的函数，同时保证自己的光滑性质。
下面是柯西核函数在最小二乘问题上的应用
下面是柯西核函数在最小二乘问题上的应用
下面是柯西核函数在最小二乘问题上的应用
: 此处可以看一下
这篇博文
，里面讲了非线性最小二乘问题以及核函数。
g2o中柯西函数的实现：
可以去看看g2o是如何使用核函数进行优化的源码，通过看g2o源码对核函数的实现可以粗略总结出，要做的工作分两部分，
1.实现柯西核函数（控制阈值c选择, 残差在正态分布情况下选择2.3849，非正态分布下没有一定值，这里直接设置为1.0）；
2. 根据上面的公式推导拼正规方程。
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.0
double 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实现。 以下是四种配准方法加了柯西核函数之后的表现。
这里在看LooselyLIO代码时，发现其调用的方法就是IncNDTLO，而IncNDTLO在AddCloud中的配准方法就是IncNdt3d。只需要修改src/ch7/ndt_inc.cc文件中的AlignNdt()函数即可。修改和第一题中的代码一致。
运行效果
：（加核函数前后表现差异不大）
论文参考： Fast segmentation of 3D point clouds: A paradigm on LiDAR data for autonomous vehicle applications
地面提取思路：一帧3D点云包含的点数众多，我们要提取地面，可以只选出接近地面的一些点拿来拟合平面，可以降低大量计算。
论文中的LPR算法流程如下：
（种子点可以理解为近地点，不过种子点的提取要求雷达与地面大体垂直，不然无法按照论文中思路提取出种子点，因为近地点提取原理就是对激光点云排序，选Z值小的点，如果激光雷达正对着地面，此方法失效。–当然大多数雷达不会这么做。）
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 plane
CloudPtr 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 seeds
double 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 iteratively
PlaneParam 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 cov
std::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();
}
// svd
Eigen::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 check
Eigen::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;
}
地面提取效果：
地面提取效果：
地面提取效果：
相关变量声明：
核心代码实现：
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 residual
Eigen::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];
}
}
实现效果：
①仅使用提取的地面点来进行定位：（可以看到效果非常不好，地面提取算法可能还需要优化，也有可能本身只使用地面点来配准就效果很差，这个可以继续尝试优化。）
②使用角点和地面点一起定位：（明显角点的匹配很好的把地面匹配的结果矫正了，但是一开始部分还是不行。）
