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,?
?, in 10m cubic areas. The points in the cubes that intersect with?
?are extracted and stored in a 3D KD-tree in {W}. We find the points in?
?within a certain region (10cm × 10cm × 10cm) around the feature points.
就是說:將地圖??保存在一個10m的立方體中,若cube中的點與當前幀中的點云?
有重疊部分就把他們提取出來保存在KD樹中。我們找地圖?
?中的點時,要在特征點附近寬為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();}
?