终于到第三个模块了，我们先来回顾下之前的工作：点云数据进来后，经过前两个节点的处理可以完成一个完整但粗糙的里程计，可以概略地估计出Lidar的相对运动。如果不受任何测量噪声的影响，这个运动估计的结果足够精确，没有任何漂移，那我们可以直接利用估计的Lidar位姿和对应时刻的量测值完成建图。但这就如同现实中不存在一个不受外力就能匀速直线运动的小球一样，量测噪声是不可避免的，因此Lidar位姿估计偏差一定存在。

Lidar里程计的结果不准确，拼起来的点也完全不成样子，且它会不断发散，因此误差也会越来越大。我们对特征的提取仅仅只是关注了他们的曲率，且点云中的点是离散的，无法保证上一帧的点在下一帧中仍会被扫到。因此，我们需要依靠别的方式去优化Lidar里程计的位姿估计精度。在SLAM领域，一般会采用与地图匹配的方式来优化这一结果。我们始终认为后一时刻的观测较前一时刻带有更多的误差，换而言之，我们更加信任前一时刻结果。因此我们对已经构建地图的信任程度远高于临帧点云配准后的Lidar运动估计。所以我们可以利用已构建地图对位姿估计结果进行修正。

LaserMapping

这一模块的功能：优化Lidar的位姿，在此基础上完成低频的环境建图

依旧从主函数开始

int main(int argc, char** argv)
{ros::init(argc, argv, "laserMapping");ros::NodeHandle nh;************订阅5个节点************ros::Subscriber subLaserCloudCornerLast = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_corner_last", 2, laserCloudCornerLastHandler);ros::Subscriber subLaserCloudSurfLast = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_surf_last", 2, laserCloudSurfLastHandler);ros::Subscriber subLaserOdometry = nh.subscribe<nav_msgs::Odometry> ("/laser_odom_to_init", 5, laserOdometryHandler);ros::Subscriber subLaserCloudFullRes = nh.subscribe<sensor_msgs::PointCloud2> ("/velodyne_cloud_3", 2, laserCloudFullResHandler);ros::Subscriber subImu = nh.subscribe<sensor_msgs::Imu> ("/imu/data", 50, imuHandler);*************发布3个节点*************ros::Publisher pubLaserCloudSurround = nh.advertise<sensor_msgs::PointCloud2> ("/laser_cloud_surround", 1);ros::Publisher pubLaserCloudFullRes = nh.advertise<sensor_msgs::PointCloud2> ("/velodyne_cloud_registered", 2);ros::Publisher pubOdomAftMapped = nh.advertise<nav_msgs::Odometry> ("/aft_mapped_to_init", 5);nav_msgs::Odometry odomAftMapped;odomAftMapped.header.frame_id = "/camera_init";odomAftMapped.child_frame_id = "/aft_mapped";tf::TransformBroadcaster tfBroadcaster;tf::StampedTransform aftMappedTrans;aftMappedTrans.frame_id_ = "/camera_init";aftMappedTrans.child_frame_id_ = "/aft_mapped";

在订阅器订阅到了laserOdometry发布的消息后即可开始进行处理。

while (status) {ros::spinOnce();if (newLaserCloudCornerLast && newLaserCloudSurfLast && newLaserCloudFullRes && newLaserOdometry &&fabs(timeLaserCloudCornerLast - timeLaserOdometry) < 0.005 &&fabs(timeLaserCloudSurfLast - timeLaserOdometry) < 0.005 &&fabs(timeLaserCloudFullRes - timeLaserOdometry) < 0.005) {newLaserCloudCornerLast = false;newLaserCloudSurfLast = false;newLaserCloudFullRes = false;newLaserOdometry = false;//frameCount让优化处理的部分与laserodometry的发布速度一致？frameCount++;if (frameCount >= stackFrameNum) {// 将坐标转移到世界坐标系下->得到可用于建图的Lidar坐标，即修改了transformTobeMapped的值transformAssociateToMap();// 将上一时刻所有角特征转到世界坐标系下int laserCloudCornerLastNum = laserCloudCornerLast->points.size();for (int i = 0; i < laserCloudCornerLastNum; i++) {pointAssociateToMap(&laserCloudCornerLast->points[i], &pointSel);laserCloudCornerStack2->push_back(pointSel);}// 将上一时刻所有边特征转到世界坐标系下int laserCloudSurfLastNum = laserCloudSurfLast->points.size();for (int i = 0; i < laserCloudSurfLastNum; i++) {pointAssociateToMap(&laserCloudSurfLast->points[i], &pointSel);laserCloudSurfStack2->push_back(pointSel);}}

接下来就是较为复杂的优化处理部分,我们先看看论文怎么说的

To find correspondences for the feature points, we store the point cloud on the map, $Q_{k-1}$ , in 10m cubic areas. The points in the cubes that intersect with $\bar{Q}_{k}$ are extracted and stored in a 3D KD-tree in {W}. We find the points in $Q_{k-1}$ within a certain region (10cm × 10cm × 10cm) around the feature points.

就是说：将地图 $Q_{k-1}$ 保存在一个10m的立方体中，若cube中的点与当前帧中的点云 $\bar{Q}_{k}$ 有重叠部分就把他们提取出来保存在KD树中。我们找地图 $Q_{k-1}$ 中的点时，要在特征点附近宽为10cm的立方体邻域内搜索

if (frameCount >= stackFrameNum) {frameCount = 0;//pointOnYAxis应该是lidar中心在当前坐标系下的位置PointType pointOnYAxis;pointOnYAxis.x = 0.0;pointOnYAxis.y = 10.0;pointOnYAxis.z = 0.0;//变换到世界坐标系下pointAssociateToMap(&pointOnYAxis, &pointOnYAxis);//transformTobeMapped记录的是lidar的位姿，在transformAssociateToMap（）函数中进行了更新//下面计算的i,j，k是一种索引，指明当前收到的点云所在的cube的（中心？）位置int centerCubeI = int((transformTobeMapped[3] + 25.0) / 50.0) + laserCloudCenWidth;int centerCubeJ = int((transformTobeMapped[4] + 25.0) / 50.0) + laserCloudCenHeight;int centerCubeK = int((transformTobeMapped[5] + 25.0) / 50.0) + laserCloudCenDepth;if (transformTobeMapped[3] + 25.0 < 0) centerCubeI--;if (transformTobeMapped[4] + 25.0 < 0) centerCubeJ--;if (transformTobeMapped[5] + 25.0 < 0) centerCubeK--;

接下来对做一些调整，确保位姿在cube中的相对位置（centerCubeI，centerCubeJ，centerCubeK）能够有一个5*5*5的邻域。

        while (centerCubeI < 3) {for (int j = 0; j < laserCloudHeight; j++) {for (int k = 0; k < laserCloudDepth; k++) {int i = laserCloudWidth - 1;pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];for (; i >= 1; i--) {laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =laserCloudCornerArray[i - 1 + laserCloudWidth*j + laserCloudWidth * laserCloudHeight * k];laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =laserCloudSurfArray[i - 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];}laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = laserCloudCubeCornerPointer;laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = laserCloudCubeSurfPointer;laserCloudCubeCornerPointer->clear();laserCloudCubeSurfPointer->clear();}}centerCubeI++;laserCloudCenWidth++;}while (centerCubeI >= laserCloudWidth - 3) {for (int j = 0; j < laserCloudHeight; j++) {for (int k = 0; k < laserCloudDepth; k++) {int i = 0;pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];for (; i < laserCloudWidth - 1; i++) {laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =laserCloudCornerArray[i + 1 + laserCloudWidth*j + laserCloudWidth * laserCloudHeight * k];laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =laserCloudSurfArray[i + 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];}laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = laserCloudCubeCornerPointer;laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = laserCloudCubeSurfPointer;laserCloudCubeCornerPointer->clear();laserCloudCubeSurfPointer->clear();}}centerCubeI--;laserCloudCenWidth--;}while (centerCubeJ < 3) {for (int i = 0; i < laserCloudWidth; i++) {for (int k = 0; k < laserCloudDepth; k++) {int j = laserCloudHeight - 1;pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];for (; j >= 1; j--) {laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =laserCloudCornerArray[i + laserCloudWidth*(j - 1) + laserCloudWidth * laserCloudHeight*k];laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =laserCloudSurfArray[i + laserCloudWidth * (j - 1) + laserCloudWidth * laserCloudHeight*k];}laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = laserCloudCubeCornerPointer;laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = laserCloudCubeSurfPointer;laserCloudCubeCornerPointer->clear();laserCloudCubeSurfPointer->clear();}}centerCubeJ++;laserCloudCenHeight++;} while (centerCubeJ >= laserCloudHeight - 3) {for (int i = 0; i < laserCloudWidth; i++) {for (int k = 0; k < laserCloudDepth; k++) {int j = 0;pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];for (; j < laserCloudHeight - 1; j++) {laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =laserCloudCornerArray[i + laserCloudWidth*(j + 1) + laserCloudWidth * laserCloudHeight*k];laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =laserCloudSurfArray[i + laserCloudWidth * (j + 1) + laserCloudWidth * laserCloudHeight*k];}laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = laserCloudCubeCornerPointer;laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = laserCloudCubeSurfPointer;laserCloudCubeCornerPointer->clear();laserCloudCubeSurfPointer->clear();}}centerCubeJ--;laserCloudCenHeight--;}while (centerCubeK < 3) {for (int i = 0; i < laserCloudWidth; i++) {for (int j = 0; j < laserCloudHeight; j++) {int k = laserCloudDepth - 1;pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];for (; k >= 1; k--) {laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =laserCloudCornerArray[i + laserCloudWidth*j + laserCloudWidth * laserCloudHeight*(k - 1)];laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight*(k - 1)];}laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = laserCloudCubeCornerPointer;laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = laserCloudCubeSurfPointer;laserCloudCubeCornerPointer->clear();laserCloudCubeSurfPointer->clear();}}centerCubeK++;laserCloudCenDepth++;}while (centerCubeK >= laserCloudDepth - 3) {for (int i = 0; i < laserCloudWidth; i++) {for (int j = 0; j < laserCloudHeight; j++) {int k = 0;pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];for (; k < laserCloudDepth - 1; k++) {laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =laserCloudCornerArray[i + laserCloudWidth*j + laserCloudWidth * laserCloudHeight*(k + 1)];laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight*(k + 1)];}laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = laserCloudCubeCornerPointer;laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = laserCloudCubeSurfPointer;laserCloudCubeCornerPointer->clear();laserCloudCubeSurfPointer->clear();}}centerCubeK--;laserCloudCenDepth--;}

处理完毕边沿点，接下来就是在取到的子cube的5*5*5的邻域内找对应的配准点了。

        int laserCloudValidNum = 0;int laserCloudSurroundNum = 0;//5*5*5的邻域里进行循环寻找for (int i = centerCubeI - 2; i <= centerCubeI + 2; i++) {for (int j = centerCubeJ - 2; j <= centerCubeJ + 2; j++) {for (int k = centerCubeK - 2; k <= centerCubeK + 2; k++) {if (i >= 0 && i < laserCloudWidth && j >= 0 && j < laserCloudHeight && k >= 0 && k < laserCloudDepth) {//int centerCubeI = int((transformTobeMapped[3] + 25.0) / 50.0) + laserCloudCenWidth;//int centerCubeJ = int((transformTobeMapped[4] + 25.0) / 50.0) + laserCloudCenHeight;//int centerCubeK = int((transformTobeMapped[5] + 25.0) / 50.0) + laserCloudCenDepth;//centerX,Y,Z = transformTobeMapped[3,4,5]+25float centerX = 50.0 * (i - laserCloudCenWidth);float centerY = 50.0 * (j - laserCloudCenHeight);float centerZ = 50.0 * (k - laserCloudCenDepth);bool isInLaserFOV = false;//确定邻域的点是否可用（是否在lidar的视角内）for (int ii = -1; ii <= 1; ii += 2) {for (int jj = -1; jj <= 1; jj += 2) {for (int kk = -1; kk <= 1; kk += 2) {float cornerX = centerX + 25.0 * ii;float cornerY = centerY + 25.0 * jj;float cornerZ = centerZ + 25.0 * kk;float squaredSide1 = (transformTobeMapped[3] - cornerX) * (transformTobeMapped[3] - cornerX) + (transformTobeMapped[4] - cornerY) * (transformTobeMapped[4] - cornerY)+ (transformTobeMapped[5] - cornerZ) * (transformTobeMapped[5] - cornerZ);float squaredSide2 = (pointOnYAxis.x - cornerX) * (pointOnYAxis.x - cornerX) + (pointOnYAxis.y - cornerY) * (pointOnYAxis.y - cornerY)+ (pointOnYAxis.z - cornerZ) * (pointOnYAxis.z - cornerZ);// 根据余弦定理进行判断float check1 = 100.0 + squaredSide1 - squaredSide2- 10.0 * sqrt(3.0) * sqrt(squaredSide1);float check2 = 100.0 + squaredSide1 - squaredSide2+ 10.0 * sqrt(3.0) * sqrt(squaredSide1);//视角在60°范围内if (check1 < 0 && check2 > 0) {isInLaserFOV = true;}}}}//将选择好的点存入数组中if (isInLaserFOV) {laserCloudValidInd[laserCloudValidNum] = i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k;laserCloudValidNum++;}laserCloudSurroundInd[laserCloudSurroundNum] = i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k;laserCloudSurroundNum++;}}}}

接下来就准备精度更加高的配准了，首先是准备工作，我们需要两堆进行配准的点云

        laserCloudCornerFromMap->clear();laserCloudSurfFromMap->clear();//已选择好的上一时刻的用来进行配准的点for (int i = 0; i < laserCloudValidNum; i++) {*laserCloudCornerFromMap += *laserCloudCornerArray[laserCloudValidInd[i]];*laserCloudSurfFromMap += *laserCloudSurfArray[laserCloudValidInd[i]];}int laserCloudCornerFromMapNum = laserCloudCornerFromMap->points.size();int laserCloudSurfFromMapNum = laserCloudSurfFromMap->points.size();//当前时刻的点，转到世界坐标系下int laserCloudCornerStackNum2 = laserCloudCornerStack2->points.size();for (int i = 0; i < laserCloudCornerStackNum2; i++) {pointAssociateTobeMapped(&laserCloudCornerStack2->points[i], &laserCloudCornerStack2->points[i]);}int laserCloudSurfStackNum2 = laserCloudSurfStack2->points.size();for (int i = 0; i < laserCloudSurfStackNum2; i++) {pointAssociateTobeMapped(&laserCloudSurfStack2->points[i], &laserCloudSurfStack2->points[i]);}//降采样laserCloudCornerStack->clear();downSizeFilterCorner.setInputCloud(laserCloudCornerStack2);downSizeFilterCorner.filter(*laserCloudCornerStack);int laserCloudCornerStackNum = laserCloudCornerStack->points.size();laserCloudSurfStack->clear();downSizeFilterSurf.setInputCloud(laserCloudSurfStack2);downSizeFilterSurf.filter(*laserCloudSurfStack);int laserCloudSurfStackNum = laserCloudSurfStack->points.size();laserCloudCornerStack2->clear();laserCloudSurfStack2->clear();

这样，我们就得到了用来配准的点云，接下来步入正题。我们再次拿出KD树，来寻找最邻近的5个点。对点云协方差矩阵进行主成分分析：若这五个点分布在直线上，协方差矩阵的特征值包含一个元素显著大于其余两个，与该特征值相关的特征向量表示所处直线的方向；若这五个点分布在平面上，协方差矩阵的特征值存在一个显著小的元素，与该特征值相关的特征向量表示所处平面的法线方向。

if (laserCloudCornerFromMapNum > 10 && laserCloudSurfFromMapNum > 100) {//数量足够多才进行处理//kd树寻找最近点kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMap);kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMap);for (int iterCount = 0; iterCount < 10; iterCount++) {laserCloudOri->clear();coeffSel->clear();for (int i = 0; i < laserCloudCornerStackNum; i++) {pointOri = laserCloudCornerStack->points[i];pointAssociateToMap(&pointOri, &pointSel);kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);if (pointSearchSqDis[4] < 1.0) {float cx = 0;float cy = 0; float cz = 0;for (int j = 0; j < 5; j++) {cx += laserCloudCornerFromMap->points[pointSearchInd[j]].x;cy += laserCloudCornerFromMap->points[pointSearchInd[j]].y;cz += laserCloudCornerFromMap->points[pointSearchInd[j]].z;}//五个点坐标的算术平均值cx /= 5;cy /= 5; cz /= 5;float a11 = 0;float a12 = 0; float a13 = 0;float a22 = 0;float a23 = 0; float a33 = 0;for (int j = 0; j < 5; j++) {float ax = laserCloudCornerFromMap->points[pointSearchInd[j]].x - cx;float ay = laserCloudCornerFromMap->points[pointSearchInd[j]].y - cy;float az = laserCloudCornerFromMap->points[pointSearchInd[j]].z - cz;a11 += ax * ax;a12 += ax * ay;a13 += ax * az;a22 += ay * ay;a23 += ay * az;a33 += az * az;}a11 /= 5;a12 /= 5; a13 /= 5;a22 /= 5;a23 /= 5; a33 /= 5;//协方差矩阵matA1.at<float>(0, 0) = a11;matA1.at<float>(0, 1) = a12;matA1.at<float>(0, 2) = a13;matA1.at<float>(1, 0) = a12;matA1.at<float>(1, 1) = a22;matA1.at<float>(1, 2) = a23;matA1.at<float>(2, 0) = a13;matA1.at<float>(2, 1) = a23;matA1.at<float>(2, 2) = a33;//求特征值及特征向量cv::eigen(matA1, matD1, matV1);

之后则是和LaserOdometry中一样的优化步骤，这里就不再贴出代码了。

在更新了位姿之后，将当前时刻的点云存入cube中，为下一次的配准做准备

for (int i = 0; i < laserCloudCornerStackNum; i++) {pointAssociateToMap(&laserCloudCornerStack->points[i], &pointSel);int cubeI = int((pointSel.x + 25.0) / 50.0) + laserCloudCenWidth;int cubeJ = int((pointSel.y + 25.0) / 50.0) + laserCloudCenHeight;int cubeK = int((pointSel.z + 25.0) / 50.0) + laserCloudCenDepth;if (pointSel.x + 25.0 < 0) cubeI--;if (pointSel.y + 25.0 < 0) cubeJ--;if (pointSel.z + 25.0 < 0) cubeK--;if (cubeI >= 0 && cubeI < laserCloudWidth && cubeJ >= 0 && cubeJ < laserCloudHeight && cubeK >= 0 && cubeK < laserCloudDepth) {int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK;laserCloudCornerArray[cubeInd]->push_back(pointSel);}}for (int i = 0; i < laserCloudSurfStackNum; i++) {pointAssociateToMap(&laserCloudSurfStack->points[i], &pointSel);int cubeI = int((pointSel.x + 25.0) / 50.0) + laserCloudCenWidth;int cubeJ = int((pointSel.y + 25.0) / 50.0) + laserCloudCenHeight;int cubeK = int((pointSel.z + 25.0) / 50.0) + laserCloudCenDepth;if (pointSel.x + 25.0 < 0) cubeI--;if (pointSel.y + 25.0 < 0) cubeJ--;if (pointSel.z + 25.0 < 0) cubeK--;if (cubeI >= 0 && cubeI < laserCloudWidth && cubeJ >= 0 && cubeJ < laserCloudHeight && cubeK >= 0 && cubeK < laserCloudDepth) {int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK;laserCloudSurfArray[cubeInd]->push_back(pointSel);}}

最后，将点云数据发布出去

        mapFrameCount++;if (mapFrameCount >= mapFrameNum) {mapFrameCount = 0;laserCloudSurround2->clear();for (int i = 0; i < laserCloudSurroundNum; i++) {int ind = laserCloudSurroundInd[i];*laserCloudSurround2 += *laserCloudCornerArray[ind];*laserCloudSurround2 += *laserCloudSurfArray[ind];}laserCloudSurround->clear();downSizeFilterCorner.setInputCloud(laserCloudSurround2);downSizeFilterCorner.filter(*laserCloudSurround);sensor_msgs::PointCloud2 laserCloudSurround3;pcl::toROSMsg(*laserCloudSurround, laserCloudSurround3);laserCloudSurround3.header.stamp = ros::Time().fromSec(timeLaserOdometry);laserCloudSurround3.header.frame_id = "/camera_init";pubLaserCloudSurround.publish(laserCloudSurround3);}int laserCloudFullResNum = laserCloudFullRes->points.size();for (int i = 0; i < laserCloudFullResNum; i++) {pointAssociateToMap(&laserCloudFullRes->points[i], &laserCloudFullRes->points[i]);}sensor_msgs::PointCloud2 laserCloudFullRes3;pcl::toROSMsg(*laserCloudFullRes, laserCloudFullRes3);laserCloudFullRes3.header.stamp = ros::Time().fromSec(timeLaserOdometry);laserCloudFullRes3.header.frame_id = "/camera_init";pubLaserCloudFullRes.publish(laserCloudFullRes3);geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw(transformAftMapped[2], -transformAftMapped[0], -transformAftMapped[1]);odomAftMapped.header.stamp = ros::Time().fromSec(timeLaserOdometry);odomAftMapped.pose.pose.orientation.x = -geoQuat.y;odomAftMapped.pose.pose.orientation.y = -geoQuat.z;odomAftMapped.pose.pose.orientation.z = geoQuat.x;odomAftMapped.pose.pose.orientation.w = geoQuat.w;odomAftMapped.pose.pose.position.x = transformAftMapped[3];odomAftMapped.pose.pose.position.y = transformAftMapped[4];odomAftMapped.pose.pose.position.z = transformAftMapped[5];odomAftMapped.twist.twist.angular.x = transformBefMapped[0];odomAftMapped.twist.twist.angular.y = transformBefMapped[1];odomAftMapped.twist.twist.angular.z = transformBefMapped[2];odomAftMapped.twist.twist.linear.x = transformBefMapped[3];odomAftMapped.twist.twist.linear.y = transformBefMapped[4];odomAftMapped.twist.twist.linear.z = transformBefMapped[5];pubOdomAftMapped.publish(odomAftMapped);aftMappedTrans.stamp_ = ros::Time().fromSec(timeLaserOdometry);aftMappedTrans.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w));aftMappedTrans.setOrigin(tf::Vector3(transformAftMapped[3], transformAftMapped[4], transformAftMapped[5]));tfBroadcaster.sendTransform(aftMappedTrans);}}status = ros::ok();rate.sleep();}

LOAM_velodyne学习（三）相关推荐

统计学习三要素模型＋策略＋算法
统计学习方法都是由模型. 策略和算法构成的. 即统计学习方法由三要素构成, 可以简单地表示为:方法＝模型+策略+算法模型统计学习首要考虑的问题是学习什么样的模型. 在监督学习过程中, 模型就是所要 ...
深度学习三巨头也成了大眼萌，这个一键转换动画电影形象的网站竟因「太火」而下线...
机器之心报道作者:魔王.杜伟想不想在动画电影中拥有自己的角色?这个网站一键满足你的需求,不过竟因流量太大成本过高而下线. 近期热映的电影<花木兰>总是让人回想起 1998 年上映的同名 ...
2020届 AAAI Fellow名单新鲜出炉！！！深度学习三巨头终于齐聚
点击上方"深度学习技术前沿",选择"星标"公众号资源干货,第一时间送达 AAAI 是国际人工智能领域最权威的学术组织,Fellow 是该学会给予会员的最高荣誉 ...
HTTP学习三：HTTPS
HTTP学习三:HTTPS 1 HTTP安全问题 HTTP1.0/1.1在网络中是明文传输的,因此会被黑客进行攻击. 1.1 窃取数据因为HTTP1.0/1.1是明文的,黑客很容易获得用户的重要数据 ...
python爬虫正则表达式实例-python爬虫学习三：python正则表达式
python爬虫学习三:python正则表达式 1.正则表达式基础 a.正则表达式的大致匹配过程: 1.依次拿出表达式和文本中的字符比较 2.如果每一个字符都能匹配,则匹配成功:一旦有匹配不成功的字符 ...
TweenMax动画库学习(三)
目录 TweenMax动画库学习(一) TweenMax动画库学习(二) TweenMax动画库学习(三) ...
实至名归！ACM宣布深度学习三巨头共同获得图灵奖
昨日晚间,ACM(国际计算机学会)宣布,有"深度学习三巨头"之称的Yoshua Bengio.Yann LeCun.Geoffrey Hinton共同获得了2018年的图灵奖,这是 ...
【技术综述】图像与CNN发家简史，集齐深度学习三巨头
文章首发于微信公众号<有三AI> [技术综述]图像与CNN发家简史,集齐深度学习三巨头没有一个经典的发现会是突然之间横空出世,它总是需要一些积淀. 提起卷积神经网络,我们总会从LeNet ...
spring security 学习三-rememberMe
spring security 学习三-rememberMe 功能:登录时的"记住我"功能原理: rememberMeAuthenticationFilter在security过 ...
C#多线程学习(三) 生产者和消费者
C#多线程学习(三) 生产者和消费者原文链接:http://kb.cnblogs.com/page/42530/ 本系列文章导航 C#多线程学习(一) 多线程的相关概念 C#多线程学习(二) 如何操 ...

LOAM_velodyne学习（三）

LaserMapping

LOAM_velodyne学习（三）相关推荐

最新文章

热门文章