LOAM

LOAM"/>

LOAM

参考=qq&utm_medium=social&utm_oi=1039233414704160768

针对b情况，A点本是一个面点，在这个时候会被误认为成一个角点。对应代码中的内容为：

遍历所有点（除去前五个和后六个），判断该点及其周边点是否可以作为特征点位：当某点及其后点间的距离平方大于某阈值a（说明这两点有一定距离），且两向量夹角小于某阈值b时（夹角小就可能存在遮挡），将其一侧的临近6个点设为不可标记为特征点的点；若某点到其前后两点的距离均大于c倍的该点深度，则该点判定为不可标记特征点的点（入射角越小，点间距越大，即激光发射方向与投射到的平面越近似水平）。
(int i = 5; i < cloudSize - 6; i++) {
float diffX = laserCloud->points[i + 1].x - laserCloud->points[i].x;
float diffY = laserCloud->points[i + 1].y - laserCloud->points[i].y;
float diffZ = laserCloud->points[i + 1].z - laserCloud->points[i].z;
float diff = diffX * diffX + diffY * diffY + diffZ * diffZ; // points[i+1]与points[i]间的距离的平方

if (diff > 0.1) {      //判断A和B两点间距离的平方是否大于阈值0.1float depth1 = sqrt(laserCloud->points[i].x * laserCloud->points[i].x + laserCloud->points[i].y * laserCloud->points[i].y +laserCloud->points[i].z * laserCloud->points[i].z); // points[i]的深度float depth2 = sqrt(laserCloud->points[i + 1].x * laserCloud->points[i + 1].x + laserCloud->points[i + 1].y * laserCloud->points[i + 1].y +laserCloud->points[i + 1].z * laserCloud->points[i + 1].z); // points[i+1]的深度/* 针对论文中(b)情况 *//*depth1>depth2 X[i+1]距离更近，远侧点标记不特征；depth1<depth2 X[i]距离更近，远侧点标记不特征*/if (depth1 > depth2) {// X(i+1)-X(i)*（|X(i+1)|/|X(i)|）diffX = laserCloud->points[i + 1].x - laserCloud->points[i].x * depth2 / depth1;diffY = laserCloud->points[i + 1].y - laserCloud->points[i].y * depth2 / depth1;diffZ = laserCloud->points[i + 1].z - laserCloud->points[i].z * depth2 / depth1; // 构建了一个等腰三角形的底向量if (sqrt(diffX * diffX + diffY * diffY + diffZ * diffZ) / depth2 < 0.1) { // 根据等腰三角形性质，这一判断threshold=0.1实际表示X[i]向量与X[i+1]的夹角小于5.732度// cloudNeighborPicked 是考虑一个特征点周围不能再设置成特征约束的判断标志位cloudNeighborPicked[i - 5] = 1;cloudNeighborPicked[i - 4] = 1;cloudNeighborPicked[i - 3] = 1;cloudNeighborPicked[i - 2] = 1;cloudNeighborPicked[i - 1] = 1;cloudNeighborPicked[i] = 1;}} else {diffX = laserCloud->points[i + 1].x * depth1 / depth2 - laserCloud->points[i].x;diffY = laserCloud->points[i + 1].y * depth1 / depth2 - laserCloud->points[i].y;diffZ = laserCloud->points[i + 1].z * depth1 / depth2 - laserCloud->points[i].z;if (sqrt(diffX * diffX + diffY * diffY + diffZ * diffZ) / depth1 < 0.1) {cloudNeighborPicked[i + 1] = 1;cloudNeighborPicked[i + 2] = 1;cloudNeighborPicked[i + 3] = 1;cloudNeighborPicked[i + 4] = 1;cloudNeighborPicked[i + 5] = 1;cloudNeighborPicked[i + 6] = 1;}}
}
画图解释