SLAM 前端匹配算法 NICP

ICP算法利用点云之间距离构建误差函数,PL-ICP利用点到线的距离构建误差函数,可以总结为都是利用欧式距离来进行匹配,而获得的数据中还包括法向量及曲率数据。实际上,以在点云匹配前,可以使用法向量和曲率数预先淘汰一部分质量不高的畸点,达到提高精确度和匹配速度的目的,NICP就是这个原理。

1.基本思想

  • a.替换原始ICP对应点匹配的方法
  • b. 充分的利用实际的曲面的一些特征来对错误的匹配点进行滤除,主要是法向量(车体坐标系下的法向量)及曲率。
  • c.误差项除了对应点的欧氏距离外,还加入了对应点的法向量的角度差

b:曲率类似于语义信息,直角跟半圆、圆弧的曲率不相同,因此肯定不能匹配;而法向量的作用如图所示:当误差比较大时,ab表示的直角跟a’b’表示的直角是同一个物体,因此他们的法向量肯定是对应相等的,用a与a’的法向量进行匹配是可以的,但用长边b与短边a’进行匹配,肯定就是错的。
在这里插入图片描述
c.在误差函数中添加法向量角度误差,是因为对于轮式里程计来说,旋转比平移更重要,因为旋转会导致很大的平移误差,而平移则不会导致角度误差。

2.数学描述

在这里插入图片描述
Ω:表示信息矩阵,可以理解为权重,即看重欧式距离的误差,还是更看重法向量的角度差。

3.求解法向量和曲率

找到点p周围半径为R的范围内的所有点云
假设点云分布在一条直线上,这些点云服从高斯分布,则沿着直线的曲率比较大,垂直于曲线的曲率小。

计算点云的中心:
在这里插入图片描述
计算方差:
在这里插入图片描述
对方差进行特征值分解
在这里插入图片描述
则曲率可以定义为:
在这里插入图片描述
其中在这里插入图片描述是较小的特征值

法向量:最小的特征值对应的特征向量,就是法向量。

4.点匹配的规则

  • 没有 well define的点就拒绝,就是杂点拒绝

  • 两点之间的距离大于阈值,拒绝在这里插入图片描述

  • 两点之间的曲率之差大于阈值,拒绝在这里插入图片描述

  • 两点之间的法向量之差大于阈值,拒绝
    在这里插入图片描述

5.目标函数的求解

在这里插入图片描述
当迭代过程收敛,则得到最终解

6.算法流程

在这里插入图片描述

小节

  • 由于在寻找点匹配的过程中考虑到了环境中的曲面的曲率和法向量,因此能排除很多明显是错误的点
  • 误差函数除了考虑欧氏距离,还考虑到了法向量的角度差,因此角度更准确
  • 开源领域最优的ICP匹配方法,开源的是3D的NICP
ORB-SLAM是一种基于特征点的视觉SLAM算法,它使用ORB特征点来进行特征匹配。ORB特征点是一种既能够快速检测又能够准确描述图像特征的算法。ORB特征点的检测和描述分别使用了FAST角点检测算法和BRIEF描述子算法。ORB-SLAM使用了基于词袋模型的方法来进行特征匹配,具体来说,ORB-SLAM将所有的ORB特征点分成若干个不同的视觉词汇,然后使用BoW(Bag of Words)模型来描述每个图像。在ORB-SLAM中,特征匹配主要分为两个步骤:词袋匹配和光流法匹配。词袋匹配是通过计算两个图像的词袋向量之间的距离来进行的,而光流法匹配则是通过计算两个图像中的特征点在相邻帧之间的运动来进行的。 以下是一个ORB-SLAM特征匹配算法的示例代码: ```c++ // ORB特征点检测和描述 cv::Ptr<cv::ORB> orb = cv::ORB::create(); std::vector<cv::KeyPoint> keypoints1, keypoints2; cv::Mat descriptors1, descriptors2; orb->detectAndCompute(img1, cv::noArray(), keypoints1, descriptors1); orb->detectAndCompute(img2, cv::noArray(), keypoints2, descriptors2); // 词袋匹配 cv::Ptr<cv::DescriptorMatcher> matcher = cv::DescriptorMatcher::create("BruteForce-Hamming"); std::vector<cv::DMatch> matches; matcher->match(descriptors1, descriptors2, matches); // 光流法匹配 std::vector<cv::Point2f> points1, points2; for (auto match : matches) { points1.push_back(keypoints1[match.queryIdx].pt); points2.push_back(keypoints2[match.trainIdx].pt); } std::vector<uchar> status; std::vector<float> err; cv::calcOpticalFlowPyrLK(img1, img2, points1, points2, status, err); // 输出匹配结果 for (int i = 0; i < matches.size(); i++) { if (status[i]) { cv::DMatch match = matches[i]; cv::Point2f pt1 = keypoints1[match.queryIdx].pt; cv::Point2f pt2 = keypoints2[match.trainIdx].pt; std::cout << "Match " << i << ": (" << pt1.x << ", " << pt1.y << ") -> (" << pt2.x << ", " << pt2.y << ")" << std::endl; } } ```
评论 2
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值