微网站设计方案,建设一个商务网站的步骤,网站制作时间,网站开发软件的选择目录 1. 给ICP和NDT配准添加柯西核函数1.1 代码实现 2. 将第1部分的robust loss引入IncNDTLO和LooselyLIO#xff0c;给出实现和运行效果3. 从概率层面解释NDT残差和协方差矩阵的关系#xff0c;说明为什么NDT协方差矩阵可以用于最小二乘4. 为LOAM like LO设计一个地面点云提… 目录 1. 给ICP和NDT配准添加柯西核函数1.1 代码实现 2. 将第1部分的robust loss引入IncNDTLO和LooselyLIO给出实现和运行效果3. 从概率层面解释NDT残差和协方差矩阵的关系说明为什么NDT协方差矩阵可以用于最小二乘4. 为LOAM like LO设计一个地面点云提取流程并单独为这些点使用点面ICP4.1 代码实现4.2 对地面点进行ICP 5. 也欢迎大家来我的微信公众号--过千帆来读书提高我们的认知。 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::pairMat6d, Vec6d(Mat6d::Zero(), Vec6d::Zero()),[jacobians, errors, effect_pts, total_res, effective_num](const std::pairMat6d, Vec6d pre,int idx) - std::pairMat6d, 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::pairMat6d, 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_castsize_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::vectorEigen::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::JacobiSVDEigen::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::vectorint nn_indices;kdtree_ground_.GetClosestPoint(ToPointType(qs), nn_indices, 5);effect_ground[idx] false;if (nn_indices.size() 5) {std::vectorVec3d 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.head3().dot(qs) n[3];if (fabs(dis) options_.max_plane_distance_) return;effect_ground[idx] true;// build residualEigen::Matrixdouble, 1, 6 J;J.block1, 3(0, 0) -n.head3().transpose() * pose.so3().matrix() * SO3::hat(q);J.block1, 3(0, 3) n.head3().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. 也欢迎大家来我的微信公众号–过千帆来读书提高我们的认知。
