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.pswp.cn/news/253942.shtml
繁體地址，請注明出處：http://hk.pswp.cn/news/253942.shtml
英文地址，請注明出處：http://en.pswp.cn/news/253942.shtml

如若內容造成侵權/違法違規/事實不符，請聯系多彩編程網進行投訴反饋email:809451989@qq.com，一經查實，立即刪除！

相關文章

監控視頻線種類 視頻信號傳輸介紹及各種視頻接口的傳輸距離

監控視頻線種類 視頻信號傳輸介紹及各種視頻接口的傳輸距離

一.視頻信號接口 監控視頻線種類介紹&#xff1a; 按照材料區分有SYV及SYWV兩種&#xff0c;絕緣層的物理材料結構不同&#xff0c;SYV是實心聚乙烯電纜&#xff0c;SYWV是高物理發泡電纜&#xff0c;物理發泡電纜傳輸性能優于聚乙烯。 S--同軸電纜 Y--聚乙烯 V--聚氯乙烯 W…
閱讀更多...
免費節假日API 更新新功能了 新增農歷信息返回

免費節假日API 更新新功能了 新增農歷信息返回

感謝大家對免費節假日API的支持.最近看了別家的api于是增加了一些新功能即獲取日期的農歷信息. 這個新功能還處于測試階段如有問題歡迎反饋 檢查一個日期是詳細信息 https://tool.bitefu.net/jiari/?d20180101&info1 返回值 {"status": 1,"type": 1,…
閱讀更多...
新手算法學習之路----二叉樹（二叉樹最大路徑和）

新手算法學習之路----二叉樹（二叉樹最大路徑和）

摘抄自&#xff1a;https://segmentfault.com/a/1190000003554858#articleHeader2 題目&#xff1a; Given a binary tree, find the maximum path sum. The path may start and end at any node in the tree. For example: Given the below binary tree, 1/ \2 3Return 6. 思…
閱讀更多...
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;…
閱讀更多...
PHP數據庫類

PHP數據庫類

<?phpclass Db{//私有靜態屬性存儲實例化對象自身private static $instance;//存儲PDO類的實例化private $pdo;//PDOStatement類private $stmt;//禁止外部實例化對象&#xff0c;鏈接數據庫private function __construct($config,$port,$charset){try{$this->pdo new P…
閱讀更多...
oracle參數文件、控制文件、數據文件、日志文件的位置及查詢方法

oracle參數文件、控制文件、數據文件、日志文件的位置及查詢方法

參數文件&#xff1a;所有參數文件一般在 $ORACLE_HOME/dbs 下 sqlplus查詢語句&#xff1a;show parameter spfile; 網絡連接文件&#xff1a; $ORACLE_HOME/dbs/network/admin 目錄中 控制文件&#xff1a;select * from v$controlfile; 數據文件&#xff1a;一般在oracleda…
閱讀更多...
Bishops Alliance—— 最大上升子序列

Bishops Alliance—— 最大上升子序列

原題鏈接&#xff1a;http://codeforces.com/gym/101147/problem/F 題意&#xff1a;n*n的棋盤&#xff0c;給m個主教的坐標及其私有距離p&#xff0c;以及常數C&#xff0c;求位于同一對角線上滿足條件&#xff1a;dist(i, j) > p[i]^2 p[j]^2 C 的主教集合的元素個數最…
閱讀更多...
LeGO-LOAM學習

LeGO-LOAM學習

前言 在學習了LOAM之后&#xff0c;了解到LeGO-LOAM&#xff08;面向復雜情況的輕量級優化地面的雷達里程計&#xff09;&#xff0c;進行了一個學習整理。 Github&#xff1a;https://github.com/RobustFieldAutonomyLab/LeGO-LOAM 論文&#xff1a;https://github.com/Robu…
閱讀更多...
char data[0]用法總結

char data[0]用法總結

struct MyData { int nLen; char data[0]; }; 開始沒有理解紅色部分的內容&#xff0c;上網搜索下&#xff0c;發現用處很大&#xff0c;記錄下來。 在結構中&#xff0c;data是一個數組名&#xff1b;但該數組沒有元素&#xff1b;該數組…
閱讀更多...
（一）低功耗設計目的與功耗的類型

（一）低功耗設計目的與功耗的類型

一、低功耗設計的目的 1.便攜性設備等需求 電子產品在我們生活中扮演了極其重要的作用&#xff0c;便攜性的電子設備便是其中一種。便攜性設備需要電池供電、需要消耗電池的能量。在同等電能提供下&#xff0c;低功耗設計的產品就能夠工作更長的時間。時間的就是生命&#xff…
閱讀更多...
(轉)徹底學會使用epoll(一)——ET模式實現分析

(轉)徹底學會使用epoll(一)——ET模式實現分析

注&#xff1a;之前寫過兩篇關于epoll實現的文章&#xff0c;但是感覺懂得了實現原理并不一定會使用&#xff0c;所以又決定寫這一系列文章&#xff0c;希望能夠對epoll有比較清楚的認識。是請大家轉載務必注明出處&#xff0c;算是對我勞動成果的一點點尊重吧。另外&#xff0…
閱讀更多...
MFC的消息映射有什么作用

MFC的消息映射有什么作用

絕對以下這三個解釋的比較簡潔&#xff0c;特此做個記錄&#xff01;以感謝回答的這些人&#xff01; MFC的消息映射有什么作用: Windows操作系統主要是有消息來處理的&#xff0c;每個程序都有自己的消息隊列&#xff0c;并且這些消息是有優先級的&#xff0c;也就是誰會先…
閱讀更多...
線性表的鏈式存儲結構

線性表的鏈式存儲結構

鏈式存儲結構的定義 1.概念定義&#xff1a; - n個結點離散分配 - 彼此通過指針相連 - 每個結點只有一個前驅結點和一個后繼結點 - 首結點沒有前驅結點&#xff0c;尾結點沒有后繼結點 2.專業術語 -首結點&#xff1a;第一個有有效數據的結點 -尾結點&#xff1a;最后一個有有效…
閱讀更多...
Apache 設置http跳轉至HTTPS訪問

Apache 設置http跳轉至HTTPS訪問

為什么80%的碼農都做不了架構師&#xff1f;>>> <VirtualHost>...</VirtualHost> 中添加如下配置 <IfModule mod_rewrite.c>RewriteEngine onRewriteCond %{SERVER_PORT} 80RewriteRule ^(.*)$ https://域名/$1 [R301,L] </IfModule> 轉…
閱讀更多...
JAVA線程概念

JAVA線程概念

一、程序與進程 1、程序&#xff1a;一段靜態的代碼。 2、進程&#xff1a;程序的一次動態執行過程&#xff0c;它對應從代碼加載、執行到執行完畢的一個完整過程。 3、進程也稱任務&#xff0c;支持多個進程同時執行的OS就被稱為多進程OS或多任務OS。 二、進程與線程 在一…
閱讀更多...
（二）功耗的分析

（二）功耗的分析

前面學習了進行低功耗的目的個功耗的構成&#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篇, 將介紹全局狀態引起的問題. 全局狀態 全局狀態, 也可以叫做應用程序狀態, 它是一組變量, 這些變量維護著…
閱讀更多...
最新文章

Copyright @ 2022~2023