湖北专业的网站制作代理商,国内室内设计公司前十名,企业网站开发的目的,常用域名大全终于到第三个模块了#xff0c;我们先来回顾下之前的工作#xff1a;点云数据进来后#xff0c;经过前两个节点的处理可以完成一个完整但粗糙的里程计#xff0c;可以概略地估计出Lidar的相对运动。如果不受任何测量噪声的影响#xff0c;这个运动估计的结果足够精确…终于到第三个模块了我们先来回顾下之前的工作点云数据进来后经过前两个节点的处理可以完成一个完整但粗糙的里程计可以概略地估计出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.subscribesensor_msgs::PointCloud2(/laser_cloud_corner_last, 2, laserCloudCornerLastHandler);ros::Subscriber subLaserCloudSurfLast nh.subscribesensor_msgs::PointCloud2(/laser_cloud_surf_last, 2, laserCloudSurfLastHandler);ros::Subscriber subLaserOdometry nh.subscribenav_msgs::Odometry (/laser_odom_to_init, 5, laserOdometryHandler);ros::Subscriber subLaserCloudFullRes nh.subscribesensor_msgs::PointCloud2 (/velodyne_cloud_3, 2, laserCloudFullResHandler);ros::Subscriber subImu nh.subscribesensor_msgs::Imu (/imu/data, 50, imuHandler);*************发布3个节点*************ros::Publisher pubLaserCloudSurround nh.advertisesensor_msgs::PointCloud2 (/laser_cloud_surround, 1);ros::Publisher pubLaserCloudFullRes nh.advertisesensor_msgs::PointCloud2 (/velodyne_cloud_registered, 2);ros::Publisher pubOdomAftMapped nh.advertisenav_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,jk是一种索引指明当前收到的点云所在的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中的相对位置centerCubeIcenterCubeJcenterCubeK能够有一个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::PointCloudPointType::Ptr laserCloudCubeCornerPointer laserCloudCornerArray[i laserCloudWidth * j laserCloudWidth * laserCloudHeight * k];pcl::PointCloudPointType::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::PointCloudPointType::Ptr laserCloudCubeCornerPointer laserCloudCornerArray[i laserCloudWidth * j laserCloudWidth * laserCloudHeight * k];pcl::PointCloudPointType::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::PointCloudPointType::Ptr laserCloudCubeCornerPointer laserCloudCornerArray[i laserCloudWidth * j laserCloudWidth * laserCloudHeight * k];pcl::PointCloudPointType::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::PointCloudPointType::Ptr laserCloudCubeCornerPointer laserCloudCornerArray[i laserCloudWidth * j laserCloudWidth * laserCloudHeight * k];pcl::PointCloudPointType::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::PointCloudPointType::Ptr laserCloudCubeCornerPointer laserCloudCornerArray[i laserCloudWidth * j laserCloudWidth * laserCloudHeight * k];pcl::PointCloudPointType::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::PointCloudPointType::Ptr laserCloudCubeCornerPointer laserCloudCornerArray[i laserCloudWidth * j laserCloudWidth * laserCloudHeight * k];pcl::PointCloudPointType::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.atfloat(0, 0) a11;matA1.atfloat(0, 1) a12;matA1.atfloat(0, 2) a13;matA1.atfloat(1, 0) a12;matA1.atfloat(1, 1) a22;matA1.atfloat(1, 2) a23;matA1.atfloat(2, 0) a13;matA1.atfloat(2, 1) a23;matA1.atfloat(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();}
