我要建设公司网站,壹搜网站建设,现在推广什么app最挣钱,学python学费大概多少1 地面点分离剔除方法
1.1 数学推导 LeGO-LOAM 中前端改进中很重要的一点就是充分利用了地面点,那首先自然是提取 对地面点的提取。 如上图,相邻的两个扫描线束的同一列打在地面上如 点所示,他们的垂直高度差 #xff0c;水平距离差 #xff0c;计算垂直高度差和水平高度差…1 地面点分离剔除方法
1.1 数学推导 LeGO-LOAM 中前端改进中很重要的一点就是充分利用了地面点,那首先自然是提取 对地面点的提取。 如上图,相邻的两个扫描线束的同一列打在地面上如 点所示,他们的垂直高度差 水平距离差 计算垂直高度差和水平高度差的角度 。这里为AB到水平面的夹角理想情况下应该接近 0考虑到一方面激光雷达安装也无法做到绝对水平。另一方面地面也不是绝对水平。因此这个角度会略微大于 0考虑到作者实际在草坪之类的场景下运动因此这个值被设置成 10 度。 实际上此种地面分离算法有一些简单我们可以结合激光雷达安装高度等其他先验信息进行优化。 1.2 Lego-LOAM代码实现 版本1LegoLOAM改版 void ImageProjection::groundRemoval() {// _ground_mat// -1, no valid info to check if ground of not// 0, initial value, after validation, means not ground// 1, ground// 同一列由下到上 _horizontal_scans 1800 _ground_scan_index7(地面点的上限)16线设置为7 因为水平地面点不可能在天上面对把for (size_t j 0; j _horizontal_scans; j){for (size_t i 0; i _ground_scan_index; i){// 取出同一列相邻点的索引size_t lowerInd j (i)*_horizontal_scans;size_t upperInd j (i 1) * _horizontal_scans;// 非有效点if (_full_cloud-points[lowerInd].intensity -1 ||_full_cloud-points[upperInd].intensity -1) {// no info to check, invalid points_ground_mat(i, j) -1;continue;}float dX _full_cloud-points[upperInd].x - _full_cloud-points[lowerInd].x;float dY _full_cloud-points[upperInd].y - _full_cloud-points[lowerInd].y;float dZ _full_cloud-points[upperInd].z - _full_cloud-points[lowerInd].z;float vertical_angle std::atan2(dZ , sqrt(dX * dX dY * dY dZ * dZ));// TODO: review this change// 和上文的公式相同 _sensor_mount_angle设置为_sensor_mount_angle * DEG_TO_RAD DEG_TO_RAD M_PI / 180.0if ( (vertical_angle - _sensor_mount_angle) 10 * DEG_TO_RAD){_ground_mat(i, j) 1;_ground_mat(i 1, j) 1;}}}// extract ground cloud (_ground_mat 1)// mark entry that doesnt need to label (ground and invalid point) for// segmentation note that ground remove is from 0~_N_scan-1, need _range_mat// for mark label matrix for the 16th scan// 遍历特征矩阵 1800 * 16 如果是地面点/无效点的话 设置为-1 不参与线特征面特征的提取for (size_t i 0; i _vertical_scans; i){for (size_t j 0; j _horizontal_scans; j){if (_ground_mat(i, j) 1 ||_range_mat(i, j) FLT_MAX){_label_mat(i, j) -1;}}}// RVIZ可视化的操作for (size_t i 0; i _ground_scan_index; i){for (size_t j 0; j _horizontal_scans; j){if (_ground_mat(i, j) 1)_ground_cloud-push_back(_full_cloud-points[j i * _horizontal_scans]);}}
} 版本2Lego-LOAM原生 void groundRemoval(){size_t lowerInd, upperInd;float diffX, diffY, diffZ, angle;// groundMat// -1, no valid info to check if ground of not// 0, initial value, after validation, means not ground// 1, groundfor (size_t j 0; j Horizon_SCAN; j){for (size_t i 0; i groundScanInd; i){lowerInd j ( i )*Horizon_SCAN;upperInd j (i1)*Horizon_SCAN;if (fullCloud-points[lowerInd].intensity -1 ||fullCloud-points[upperInd].intensity -1){// no info to check, invalid pointsgroundMat.atint8_t(i,j) -1;continue;}diffX fullCloud-points[upperInd].x - fullCloud-points[lowerInd].x;diffY fullCloud-points[upperInd].y - fullCloud-points[lowerInd].y;diffZ fullCloud-points[upperInd].z - fullCloud-points[lowerInd].z;angle atan2(diffZ, sqrt(diffX*diffX diffY*diffY) ) * 180 / M_PI;if (abs(angle - sensorMountAngle) 10){groundMat.atint8_t(i,j) 1;groundMat.atint8_t(i1,j) 1;}}}// extract ground cloud (groundMat 1)// mark entry that doesnt need to label (ground and invalid point) for segmentation// note that ground remove is from 0~N_SCAN-1, need rangeMat for mark label matrix for the 16th scanfor (size_t i 0; i N_SCAN; i){for (size_t j 0; j Horizon_SCAN; j){if (groundMat.atint8_t(i,j) 1 || rangeMat.atfloat(i,j) FLT_MAX){labelMat.atint(i,j) -1;}}}if (pubGroundCloud.getNumSubscribers() ! 0){for (size_t i 0; i groundScanInd; i){for (size_t j 0; j Horizon_SCAN; j){if (groundMat.atint8_t(i,j) 1)groundCloud-push_back(fullCloud-points[j i*Horizon_SCAN]);}}}} 2 基于BFS的点云聚类和外点剔除、树干点云提取算法
2.1 数学推导 广度优先遍历(BFS)和深度优先遍历(DFS)同属于两种经典的图遍历的算法。 具体到广度优先遍历算法来说,首先从某个节点出发一层一层地遍历下一层必须等到上一层节点全部遍历完成之后才会开始遍历。 比如在上面这个无向图中如果我们从 A 节点开始遍历那么首先访问和 A 节点相邻 的节点这里就是 S、B、D然后在访问和 S、B、D 相邻的其他节点这里就是 C。 因此遍历的顺序是 A-S-B-D-C如果我们从 S 开始遍历则顺序就是 S-A-B- C-D可以看到不同的起始点对应的遍历顺序是不同的。 通常我们使用 BFS 遍历图结构的时候会借助一个队列 std::queue 来帮助我们实现 其基本步骤如下 1. 将起始顶点 S 加入队列 2. 访问 S,同时把 S 标记为已访问 3. 循环从队列中取出节 1.如果队列为空则访问结束 2.否则类似 S将该节点标记为已访问同时将其子节点加入队列。 在 LeGO-LOAM 中BFS 算法用于实现一个简单的点云聚类功能。 BFS 算法适用于图数据结构为了把单帧 lidar 点云运用上 BFS 算法首先需要将其建模成一个图模型一个很简单有效的办法就是将其投影到一个平面图上以velodyne-16 为例我们将其投影到一个 16×1800 大小的图上(这里 16 是一共有 16跟线束1800 是因为水平分辨率是 0.2 度一个扫描周期有 1800 个点)如图 对于任何一个珊格点其上下左右四个相邻点视为图结构中的邻接节点这里要注意的是左右边界的点和边界另一侧也构成邻接因为水平方向是同一个扫描周期具有物理意义上的连续性。我们可以以任意一个点开始执行 BFS 搜索直到遍历完这部分近邻点聚类点数过少的就认为是 outlier可以被剔除。 具体实现 1. 遍历每个点如果该点已经被处理过了就不再处理 2. 如果没有被处理就说明这是一个新的聚类然后执行 BFS 的步骤 1.将队列里的首元素弹出然后将该元素近邻塞入队列末尾(这里没有使用std::queue使用的普通数组所以就使用双指针来替代) 这里的近邻就是上下左右 2.分别判断近邻和自身距离是否足够近 angle 越大则认为两点越可能是同一个聚类物体上的点则打上同样的 label。 3.如此循环 2.2 代码实现 在嘈杂环境中的扫描的特征提取过程。原始点云如图(a)所示。在图(b)中红色点被标记为地面点。其余的点是分割后保留的点。只保留树干..树叶点都被剔除掉了在图(c)中蓝色和黄色点表示F e 和 F p 中的边缘和平面特征。在图(d)中绿色和粉色点分别代表F e 和 F p 中的边缘和平面特征。 广度优先遍历代码 // 广度优先遍历void labelComponents(int row, int col){// use std::queue std::vector std::deque will slow the program down greatlyfloat d1, d2, alpha, angle;int fromIndX, fromIndY, thisIndX, thisIndY; bool lineCountFlag[N_SCAN] {false};// 用一个数组保存X坐标一个数组保存Y坐标queueIndX[0] row;queueIndY[0] col;// 队列里面有多少个点int queueSize 1;// 指向前端int queueStartInd 0;// 指向即将要加入的位置int queueEndInd 1;// 将这个点的(X,Y)坐标赋予给allPushedIndX[0]allPushedIndX[0] row;allPushedIndY[0] col;int allPushedIndSize 1;while(queueSize 0){// 将队列的首元素去除完成广度优先的操作fromIndX queueIndX[queueStartInd];fromIndY queueIndY[queueStartInd];// 队列中一共有多少点--queueSize;// 将指向队列首部的指针向前移动1queueStartInd;// 标记这个点云属于哪一类,当前聚类的ID,当前物体属于同一个ID在确定当前可以聚类的情况下更新 labelCount,labelCountlabelMat.atint(fromIndX, fromIndY) labelCount;// 遍历这个结点的邻接顶点,其实就是上下左右四个元素// std::vectorstd::pairint8_t, int8_t neighborIterator;// std::pairint8_t, int8_t neighbor;// neighbor.first -1; neighbor.second 0; neighborIterator.push_back(neighbor);// neighbor.first 0; neighbor.second 1; neighborIterator.push_back(neighbor);// neighbor.first 0; neighbor.second -1; neighborIterator.push_back(neighbor);// neighbor.first 1; neighbor.second 0; neighborIterator.push_back(neighbor);for (auto iter neighborIterator.begin(); iter ! neighborIterator.end(); iter){// 四个可能的邻接顶点的(X,Y)值thisIndX fromIndX (*iter).first;thisIndY fromIndY (*iter).second;// 索引必须在(N_SCAN * HORIZON_SCAN 16 * 1600中)// 行数不在 0 - 16 间(最上面最下面的点 第1根线或者第8根线的点)if (thisIndX 0 || thisIndX N_SCAN)continue;// 同一根线相距360度且左右相邻if (thisIndY 0)thisIndY Horizon_SCAN - 1;if (thisIndY Horizon_SCAN)thisIndY 0;// 这个顶点是否被访问过if (labelMat.atint(thisIndX, thisIndY) ! 0)continue;// 将两个点距离雷达较远的点的距离赋值为d1较近的点赋值为d2d1 std::max(rangeMat.atfloat(fromIndX, fromIndY), rangeMat.atfloat(thisIndX, thisIndY));d2 std::min(rangeMat.atfloat(fromIndX, fromIndY), rangeMat.atfloat(thisIndX, thisIndY));// (0,1) (0,-1) 角分辨率为0.2度--左右糯if ((*iter).first 0)alpha segmentAlphaX;// (1,0) (-1,0) 角分辨率为2度--上下挪动elsealpha segmentAlphaY;// d2*sin(alpha)为垂线距离 d2*cos(alpha))为垂线的对边 d1 -d2*cos(alpha))为小边 求tan值为angle角度angle atan2(d2*sin(alpha), (d1 -d2*cos(alpha)));// 60度(弧度制) 60.0/180.0*M_PI; ----减少这个值会增大精度if (angle segmentTheta){// 丁真 添加到队列中 queueEndInd指向即将要插入的位置中queueIndX[queueEndInd] thisIndX;queueIndY[queueEndInd] thisIndY;queueSize;queueEndInd;// 标记和前面一种的类型labelMat.atint(thisIndX, thisIndY) labelCount;// 标志位 是否这个点被遍历过设置为truelineCountFlag[thisIndX] true;// 保存聚类结果allPushedIndX[allPushedIndSize] thisIndX;allPushedIndY[allPushedIndSize] thisIndY;allPushedIndSize;}}}// check if this segment is validbool feasibleSegment false;// 如果聚类的点大于30,小于30认为是叶子点/噪声点if (allPushedIndSize 30)feasibleSegment true;// 可能是树杆(垂直于LIDAR扫描方向的)else if (allPushedIndSize segmentValidPointNum){int lineCount 0;for (size_t i 0; i N_SCAN; i)if (lineCountFlag[i] true)lineCount;if (lineCount segmentValidLineNum)feasibleSegment true; }// segment is valid, mark these pointsif (feasibleSegment true){// 更新标签值,表示下次是新的一陀东西labelCount;}else{// 聚类无效for (size_t i 0; i allPushedIndSize; i){labelMat.atint(allPushedIndX[i], allPushedIndY[i]) 999999;}}} 特征提取前预处理代码 // 对非地面点聚类void cloudSegmentation(){// 对所有点进行遍历 N_SCAN16线 Horizon_SCAN1800(每根线有1800个点)for (size_t i 0; i N_SCAN; i)for (size_t j 0; j Horizon_SCAN; j)if (labelMat.atint(i,j) 0)// 地面点会是-1我们不处理(在上面的代码讲解中已经说过了)// 以这个点为顶点进行广度优先遍历labelComponents(i, j);int sizeOfSegCloud 0;// 遍历本帧所有激光点for (size_t i 0; i N_SCAN; i){segMsg.startRingIndex[i] sizeOfSegCloud-1 5;for (size_t j 0; j Horizon_SCAN; j){// 要不是有效的距离点 要不是有效的地面点if (labelMat.atint(i,j) 0 || groundMat.atint8_t(i,j) 1){// 聚类无效的点if (labelMat.atint(i,j) 999999){if (i groundScanInd j % 5 0){outlierCloud-push_back(fullCloud-points[j i*Horizon_SCAN]);continue;}else{continue;}}// 1800列 做一个下采样 点云加密的时候可以用// groundMat// -1, no valid info to check if ground of not// 0, initial value, after validation, means not ground// 1, groundif (groundMat.atint8_t(i,j) 1){if (j%5!0 j5 jHorizon_SCAN-5)continue;}// 标记地面点这样在后续提取特征的时候不会被当作角点提取segMsg.segmentedCloudGroundFlag[sizeOfSegCloud] (groundMat.atint8_t(i,j) 1);// mark the points column index for marking occlusion latersegMsg.segmentedCloudColInd[sizeOfSegCloud] j;// 保存距离激光雷达的距离segMsg.segmentedCloudRange[sizeOfSegCloud] rangeMat.atfloat(i,j);// 保存点云segmentedCloud-push_back(fullCloud-points[j i*Horizon_SCAN]);// 更新下一帧的点的索引sizeOfSegCloud;}}// 同LIO-SAM特征提取的边界segMsg.endRingIndex[i] sizeOfSegCloud-1 - 5;}// 发布给RVIZif (pubSegmentedCloudPure.getNumSubscribers() ! 0){for (size_t i 0; i N_SCAN; i){for (size_t j 0; j Horizon_SCAN; j){if (labelMat.atint(i,j) 0 labelMat.atint(i,j) ! 999999){segmentedCloudPure-push_back(fullCloud-points[j i*Horizon_SCAN]);segmentedCloudPure-points.back().intensity labelMat.atint(i,j);}}}}} 3 两步优化的帧间里程计
3.1 理论推导 和原始 LOAM (或者 A-LOAM )一样通过前后两帧点云来估计两帧之间的运动从而累加得到前端里程记的输出和上述方法使用线面约束同时优化六自由度帧间位姿不同LeGO-LOAM 的前端分成两个步骤每个步骤估计三自由度的变量 第一步 利用地面点优化 地面点更符合面特征的性质。因此地面点的优化问题就使用点到面的约束来构建同时我们注意到地面点之间的约束对 x , y 和 yaw 这三个自由度是不能观的。换句话说当这三个自由度的值发生变化时点到面的残差不会发生显著变化。所以地面点之间的优化只会对 pitch , roll 以及 z 进行约束和优化 第二步 利用角点优化 第一步优化完 pitch 、 roll 以及 z 之后我们仍需对另外三个自由度的变量进行估计此时我们选用提取的角点进行优化由于多线激光雷达提取的角点通常是垂直的边缘特征。 因此,这些特征对 x 、 y 以及yaw 有着比较好的能观性通过角点的优化结合上地面点的结果可以得到六自由度的帧间优化结果。 激光里程计模块的两步优化。首先通过匹配从地面点提取的平面特征获得 [t z , θ roll , θ pitch ]。然后使用从分割点提取的边缘特征来估计 [t x , t y , θ yaw ]同时将 [t z , θ roll , θ pitch ] 作为约束。 3.2 代码实现 void updateTransformation(){if (laserCloudCornerLastNum 10 || laserCloudSurfLastNum 100)return;for (int iterCount1 0; iterCount1 25; iterCount1) {laserCloudOri-clear();coeffSel-clear();// 平面点优化findCorrespondingSurfFeatures(iterCount1);if (laserCloudOri-points.size() 10)continue;if (calculateTransformationSurf(iterCount1) false)break;}for (int iterCount2 0; iterCount2 25; iterCount2) {laserCloudOri-clear();coeffSel-clear();// 角点优化findCorrespondingCornerFeatures(iterCount2);if (laserCloudOri-points.size() 10)continue;if (calculateTransformationCorner(iterCount2) false)break;}} 每一个部分和LOAM一致请移步我的多传感器融合的博客。 void findCorrespondingSurfFeatures(int iterCount){int surfPointsFlatNum surfPointsFlat-points.size();for (int i 0; i surfPointsFlatNum; i) {TransformToStart(surfPointsFlat-points[i], pointSel);if (iterCount % 5 0) {kdtreeSurfLast-nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);int closestPointInd -1, minPointInd2 -1, minPointInd3 -1;if (pointSearchSqDis[0] nearestFeatureSearchSqDist) {closestPointInd pointSearchInd[0];int closestPointScan int(laserCloudSurfLast-points[closestPointInd].intensity);float pointSqDis, minPointSqDis2 nearestFeatureSearchSqDist, minPointSqDis3 nearestFeatureSearchSqDist;for (int j closestPointInd 1; j surfPointsFlatNum; j) {if (int(laserCloudSurfLast-points[j].intensity) closestPointScan 2.5) {break;}pointSqDis (laserCloudSurfLast-points[j].x - pointSel.x) * (laserCloudSurfLast-points[j].x - pointSel.x) (laserCloudSurfLast-points[j].y - pointSel.y) * (laserCloudSurfLast-points[j].y - pointSel.y) (laserCloudSurfLast-points[j].z - pointSel.z) * (laserCloudSurfLast-points[j].z - pointSel.z);if (int(laserCloudSurfLast-points[j].intensity) closestPointScan) {if (pointSqDis minPointSqDis2) {minPointSqDis2 pointSqDis;minPointInd2 j;}} else {if (pointSqDis minPointSqDis3) {minPointSqDis3 pointSqDis;minPointInd3 j;}}}for (int j closestPointInd - 1; j 0; j--) {if (int(laserCloudSurfLast-points[j].intensity) closestPointScan - 2.5) {break;}pointSqDis (laserCloudSurfLast-points[j].x - pointSel.x) * (laserCloudSurfLast-points[j].x - pointSel.x) (laserCloudSurfLast-points[j].y - pointSel.y) * (laserCloudSurfLast-points[j].y - pointSel.y) (laserCloudSurfLast-points[j].z - pointSel.z) * (laserCloudSurfLast-points[j].z - pointSel.z);if (int(laserCloudSurfLast-points[j].intensity) closestPointScan) {if (pointSqDis minPointSqDis2) {minPointSqDis2 pointSqDis;minPointInd2 j;}} else {if (pointSqDis minPointSqDis3) {minPointSqDis3 pointSqDis;minPointInd3 j;}}}}pointSearchSurfInd1[i] closestPointInd;pointSearchSurfInd2[i] minPointInd2;pointSearchSurfInd3[i] minPointInd3;}if (pointSearchSurfInd2[i] 0 pointSearchSurfInd3[i] 0) {tripod1 laserCloudSurfLast-points[pointSearchSurfInd1[i]];tripod2 laserCloudSurfLast-points[pointSearchSurfInd2[i]];tripod3 laserCloudSurfLast-points[pointSearchSurfInd3[i]];float pa (tripod2.y - tripod1.y) * (tripod3.z - tripod1.z) - (tripod3.y - tripod1.y) * (tripod2.z - tripod1.z);float pb (tripod2.z - tripod1.z) * (tripod3.x - tripod1.x) - (tripod3.z - tripod1.z) * (tripod2.x - tripod1.x);float pc (tripod2.x - tripod1.x) * (tripod3.y - tripod1.y) - (tripod3.x - tripod1.x) * (tripod2.y - tripod1.y);float pd -(pa * tripod1.x pb * tripod1.y pc * tripod1.z);float ps sqrt(pa * pa pb * pb pc * pc);pa / ps;pb / ps;pc / ps;pd / ps;float pd2 pa * pointSel.x pb * pointSel.y pc * pointSel.z pd;float s 1;if (iterCount 5) {s 1 - 1.8 * fabs(pd2) / sqrt(sqrt(pointSel.x * pointSel.x pointSel.y * pointSel.y pointSel.z * pointSel.z));}if (s 0.1 pd2 ! 0) {coeff.x s * pa;coeff.y s * pb;coeff.z s * pc;coeff.intensity s * pd2;laserCloudOri-push_back(surfPointsFlat-points[i]);coeffSel-push_back(coeff);}}}} void findCorrespondingCornerFeatures(int iterCount){int cornerPointsSharpNum cornerPointsSharp-points.size();for (int i 0; i cornerPointsSharpNum; i) {TransformToStart(cornerPointsSharp-points[i], pointSel);if (iterCount % 5 0) {kdtreeCornerLast-nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);int closestPointInd -1, minPointInd2 -1;if (pointSearchSqDis[0] nearestFeatureSearchSqDist) {closestPointInd pointSearchInd[0];int closestPointScan int(laserCloudCornerLast-points[closestPointInd].intensity);float pointSqDis, minPointSqDis2 nearestFeatureSearchSqDist;for (int j closestPointInd 1; j cornerPointsSharpNum; j) {if (int(laserCloudCornerLast-points[j].intensity) closestPointScan 2.5) {break;}pointSqDis (laserCloudCornerLast-points[j].x - pointSel.x) * (laserCloudCornerLast-points[j].x - pointSel.x) (laserCloudCornerLast-points[j].y - pointSel.y) * (laserCloudCornerLast-points[j].y - pointSel.y) (laserCloudCornerLast-points[j].z - pointSel.z) * (laserCloudCornerLast-points[j].z - pointSel.z);if (int(laserCloudCornerLast-points[j].intensity) closestPointScan) {if (pointSqDis minPointSqDis2) {minPointSqDis2 pointSqDis;minPointInd2 j;}}}for (int j closestPointInd - 1; j 0; j--) {if (int(laserCloudCornerLast-points[j].intensity) closestPointScan - 2.5) {break;}pointSqDis (laserCloudCornerLast-points[j].x - pointSel.x) * (laserCloudCornerLast-points[j].x - pointSel.x) (laserCloudCornerLast-points[j].y - pointSel.y) * (laserCloudCornerLast-points[j].y - pointSel.y) (laserCloudCornerLast-points[j].z - pointSel.z) * (laserCloudCornerLast-points[j].z - pointSel.z);if (int(laserCloudCornerLast-points[j].intensity) closestPointScan) {if (pointSqDis minPointSqDis2) {minPointSqDis2 pointSqDis;minPointInd2 j;}}}}pointSearchCornerInd1[i] closestPointInd;pointSearchCornerInd2[i] minPointInd2;}if (pointSearchCornerInd2[i] 0) {tripod1 laserCloudCornerLast-points[pointSearchCornerInd1[i]];tripod2 laserCloudCornerLast-points[pointSearchCornerInd2[i]];float x0 pointSel.x;float y0 pointSel.y;float z0 pointSel.z;float x1 tripod1.x;float y1 tripod1.y;float z1 tripod1.z;float x2 tripod2.x;float y2 tripod2.y;float z2 tripod2.z;float m11 ((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1));float m22 ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1));float m33 ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1));float a012 sqrt(m11 * m11 m22 * m22 m33 * m33);float l12 sqrt((x1 - x2)*(x1 - x2) (y1 - y2)*(y1 - y2) (z1 - z2)*(z1 - z2));float la ((y1 - y2)*m11 (z1 - z2)*m22) / a012 / l12;float lb -((x1 - x2)*m11 - (z1 - z2)*m33) / a012 / l12;float lc -((x1 - x2)*m22 (y1 - y2)*m33) / a012 / l12;float ld2 a012 / l12;float s 1;if (iterCount 5) {s 1 - 1.8 * fabs(ld2);}if (s 0.1 ld2 ! 0) {coeff.x s * la; coeff.y s * lb;coeff.z s * lc;coeff.intensity s * ld2;laserCloudOri-push_back(cornerPointsSharp-points[i]);coeffSel-push_back(coeff);}}}}
