LOAM_velodyne学习（三）

终于到第三个模块了，我们先来回顾下之前的工作：点云数据进来后，经过前两个节点的处理可以完成一个完整但粗糙的里程计，可以概略地估计出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();}

 

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.mzph.cn/news/253942.shtml

如若内容造成侵权/违法违规/事实不符，请联系多彩编程网进行投诉反馈email:809451989@qq.com，一经查实，立即删除！

相关文章

监控视频线种类 视频信号传输介绍及各种视频接口的传输距离

监控视频线种类 视频信号传输介绍及各种视频接口的传输距离

一.视频信号接口 监控视频线种类介绍&#xff1a; 按照材料区分有SYV及SYWV两种&#xff0c;绝缘层的物理材料结构不同&#xff0c;SYV是实心聚乙烯电缆&#xff0c;SYWV是高物理发泡电缆&#xff0c;物理发泡电缆传输性能优于聚乙烯。 S--同轴电缆 Y--聚乙烯 V--聚氯乙烯 W…
阅读更多...
Ajax工作原理

Ajax工作原理

详见&#xff1a;http://blog.yemou.net/article/query/info/tytfjhfascvhzxcyt238 在这篇文章中&#xff0c;我将从10个方面来对AJAX技术进行系统的讲解。 1、ajax技术的背景 不可否认&#xff0c;ajax技术的流行得益于google的大力推广&#xff0c;正是由于google earth、go…
阅读更多...
各种视频信号格式及端子介绍/VGA DVI HDMI区别

各种视频信号格式及端子介绍/VGA DVI HDMI区别

视频信号是我们接触最多的显示信号&#xff0c;但您并不一定对各种视频信号有所了解。因为国内用到的视频信号格式和端子非常有限&#xff0c;一般就是复合视频和S端子&#xff0c;稍高级一些的就是色差及VGA。对于那些经常接触国外电器和二手设备的朋友&#xff0c;就会遇到各…
阅读更多...
LOAM_velodyne学习（四）

LOAM_velodyne学习（四）

TransformMaintenance 来到了最后一个模块&#xff0c;代码不是很长&#xff0c;我们在看完代码之后&#xff0c;再详细说明这个模块的功能 依然主函数开始 int main(int argc, char** argv) {ros::init(argc, argv, "transformMaintenance");ros::NodeHandle nh;…
阅读更多...
oracle参数文件、控制文件、数据文件、日志文件的位置及查询方法

oracle参数文件、控制文件、数据文件、日志文件的位置及查询方法

参数文件&#xff1a;所有参数文件一般在 $ORACLE_HOME/dbs 下 sqlplus查询语句&#xff1a;show parameter spfile; 网络连接文件&#xff1a; $ORACLE_HOME/dbs/network/admin 目录中 控制文件&#xff1a;select * from v$controlfile; 数据文件&#xff1a;一般在oracleda…
阅读更多...
LeGO-LOAM学习

LeGO-LOAM学习

前言 在学习了LOAM之后&#xff0c;了解到LeGO-LOAM&#xff08;面向复杂情况的轻量级优化地面的雷达里程计&#xff09;&#xff0c;进行了一个学习整理。 Github&#xff1a;https://github.com/RobustFieldAutonomyLab/LeGO-LOAM 论文&#xff1a;https://github.com/Robu…
阅读更多...
（一）低功耗设计目的与功耗的类型

（一）低功耗设计目的与功耗的类型

一、低功耗设计的目的 1.便携性设备等需求 电子产品在我们生活中扮演了极其重要的作用&#xff0c;便携性的电子设备便是其中一种。便携性设备需要电池供电、需要消耗电池的能量。在同等电能提供下&#xff0c;低功耗设计的产品就能够工作更长的时间。时间的就是生命&#xff…
阅读更多...
(转)彻底学会使用epoll(一)——ET模式实现分析

(转)彻底学会使用epoll(一)——ET模式实现分析

注&#xff1a;之前写过两篇关于epoll实现的文章&#xff0c;但是感觉懂得了实现原理并不一定会使用&#xff0c;所以又决定写这一系列文章&#xff0c;希望能够对epoll有比较清楚的认识。是请大家转载务必注明出处&#xff0c;算是对我劳动成果的一点点尊重吧。另外&#xff0…
阅读更多...
Apache 设置http跳转至HTTPS访问

Apache 设置http跳转至HTTPS访问

为什么80%的码农都做不了架构师&#xff1f;>>> <VirtualHost>...</VirtualHost> 中添加如下配置 <IfModule mod_rewrite.c>RewriteEngine onRewriteCond %{SERVER_PORT} 80RewriteRule ^(.*)$ https://域名/$1 [R301,L] </IfModule> 转…
阅读更多...
（二）功耗的分析

（二）功耗的分析

前面学习了进行低功耗的目的个功耗的构成&#xff0c;今天就来分享一下功耗的分析。由于是面向数字IC前端设计的学习&#xff0c;所以这里的功耗分析是基于DC中的power compiler工具&#xff1b;更精确的功耗分析可以采用PT&#xff0c;关于PT的功耗分析可以查阅其他资料&#…
阅读更多...
Hibernate创建hqll时报错

Hibernate创建hqll时报错

Hibernate 问题,在执行Query session.createQuery(hql) 报错误 出错截图&#xff1a; 这条语句在java运行环境下&#xff0c;直接连数据库不出错&#xff0c;如果在hiberante,struts环境下就出错 出错原因&#xff1a;jar包冲突&#xff0c;struts2和hibernate框架中都有antlr包…
阅读更多...
.NET Core TDD 前传: 编写易于测试的代码 -- 全局状态

.NET Core TDD 前传: 编写易于测试的代码 -- 全局状态

第1篇: 讲述了如何创造"缝". "缝"(seam)是需要知道的概念. 第2篇, 避免在构建对象时写出不易测试的代码. 第3篇, 依赖项和迪米特法则. 本文是第4篇, 将介绍全局状态引起的问题. 全局状态 全局状态, 也可以叫做应用程序状态, 它是一组变量, 这些变量维护着…
阅读更多...
（三）系统与架构级低功耗设计

（三）系统与架构级低功耗设计

前面讲解了使用EDA工具&#xff08;主要是power compiler&#xff09;进行功耗分析的流程&#xff0c;这里我们将介绍在数字IC中进行低功耗设计的方法&#xff0c;同时也结合EDA工具&#xff08;主要是Design Compiler&#xff09;如何实现。我们的讲解的低功耗设计主要是自顶向…
阅读更多...
（四）RTL级低功耗设计

（四）RTL级低功耗设计

前面介绍了系统级的低功耗设计&#xff0c;换句话说就是在系统级降低功耗可以考虑的方面。系统级的低功耗设计&#xff0c;主要是由系统级设计、具有丰富经验的人员实现&#xff0c;虽然还轮不到我们设计&#xff0c;我们了解一下还是比较好的。我们前端设计人员的重点不在系统…
阅读更多...
Unity3D 游戏前端开发技能树(思维导图)

Unity3D 游戏前端开发技能树(思维导图)

如果做游戏也是一种游戏,那么这个游戏的自由度实在是太高了.(导图源文件链接&#xff1a;http://pan.baidu.com/s/1eSHpH5o 密码&#xff1a;qzl5) 最近要用思维导图软件Xmind把自己的思路好好捋一捋,算是温故知新吧. 转载于:https://www.cnblogs.com/qiaogaojian/p/6098962.ht…
阅读更多...
SQL Server 死锁的告警监控

SQL Server 死锁的告警监控

原文:SQL Server 死锁的告警监控今天这篇文章总结一下如何监控SQL Server的死锁&#xff0c;其实以前写过MS SQL 监控错误日志的告警信息&#xff0c;这篇文章着重介绍如何监控数据库的死锁&#xff0c;当然这篇文章不分析死锁产生的原因、以及如何解决死锁。死锁&#xff08;D…
阅读更多...
关于web性能一些特性汇总

关于web性能一些特性汇总

关于web性能一些特性汇总 DOMContentLoaded & load load事件是window对象上的事件。指的是网页资源已经加载完毕&#xff08;包括但不限于DOM、图片、音频、脚本、插件资源以及CSS&#xff09;。 DOMContentLoaded事件是document对象上的事件。指的是DOM已经加载完毕。IE中…
阅读更多...
（五）门级电路低功耗设计优化

（五）门级电路低功耗设计优化

&#xff08;1&#xff09;门级电路的功耗优化综述 门级电路的功耗优化(Gate Level Power Optimization&#xff0c;简称GLPO)是从已经映射的门级网表开始&#xff0c;对设计进行功耗的优化以满足功耗的约束&#xff0c;同时设计保持其性能&#xff0c;即满足设计规则和时序的要…
阅读更多...
Spring3向Spring4升级过程中quartz修改

Spring3向Spring4升级过程中quartz修改

为什么80%的码农都做不了架构师&#xff1f;>>> 问题 nested exception is org.springframework.beans.factory.CannotLoadBeanClassException: Cannot find class [org.springframework.scheduling.quartz.CronTriggerBean] for bean with name ... 原因 org.spri…
阅读更多...
EasyUI--messager

EasyUI--messager

1.　　　　alert 方法 <script type"text/javascript">$( function(){$.messager.alert("调用messager","文本内容") ;});</script> 这里还可以通过icon添加相应的图标及info加入回调函数 <script type"text/javascript&quo…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023