适当修改LIO-SAM_based_relocalization解决初始重定位显示错误

去掉初始化成功后才进行里程计。

    void laserCloudInfoHandler(const lio_sam::cloud_infoConstPtr& msgIn){// extract time stamp//addedtimeLaserInfoStamp = msgIn->header.stamp;timeLaserCloudInfoLast = msgIn->header.stamp.toSec();// extract info and feature cloudcloudInfo = *msgIn;pcl::fromROSMsg(msgIn->cloud_corner,  *laserCloudCornerLast);pcl::fromROSMsg(msgIn->cloud_surface, *laserCloudSurfLast);/************************************added by gc*****************************///if the sysytem is not initialized ffer the first scan for the system to initialize//the LIO system start working only when the localization initializing is finished/*if(initializedFlag == NonInitialized || initializedFlag == Initializing){if(cloudScanForInitialize->points.size() == 0){downsampleCurrentScan();mtx_general.lock();*cloudScanForInitialize += *laserCloudCornerLastDS;*cloudScanForInitialize += *laserCloudSurfLastDS;mtx_general.unlock();laserCloudCornerLastDS->clear();laserCloudSurfLastDS->clear();laserCloudCornerLastDSNum = 0;laserCloudSurfLastDSNum = 0;transformTobeMapped[0] = cloudInfo.imuRollInit;transformTobeMapped[1] = cloudInfo.imuPitchInit;transformTobeMapped[2] = cloudInfo.imuYawInit;if (!useImuHeadingInitialization)//gc: if not use the heading of init_IMU as InitializationtransformTobeMapped[2] = 0;}return;}frameNum++;*//************************************added by gc*****************************/std::lock_guard<std::mutex> lock(mtx);if (timeLaserCloudInfoLast - timeLastProcessing >= mappingProcessInterval) {//gc:control the rate of mapping processtimeLastProcessing = timeLaserCloudInfoLast;updateInitialGuess();//gc: update initial value for statesextractSurroundingKeyFrames();//gc:downsampleCurrentScan();//gc:down sample the current corner points and surface pointsscan2MapOptimization();//gc: calculate the tranformtion using lidar measurement with the Imu preintegration as initial values//and then interpolate roll and pitch angle using IMU measurement and above measurementsaveKeyFramesAndFactor();//gc: save corner cloud and surface cloud of this scan, and add odom and GPS factors//correctPoses();publishOdometry();publishFrames();}}

在接收到重定位位姿后直接进行全局匹配，然后更新odomToWorld

void initialpose_callback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& pose_msg){//first calculate global pose//x-y-zif(initializedFlag == Initialized)return;float x = pose_msg->pose.pose.position.x;float y = pose_msg->pose.pose.position.y;float z = pose_msg->pose.pose.position.z;//roll-pitch-yawtf::Quaternion q_global;double roll_global; double pitch_global; double yaw_global;q_global.setX(pose_msg->pose.pose.orientation.x);q_global.setY(pose_msg->pose.pose.orientation.y);q_global.setZ(pose_msg->pose.pose.orientation.z);q_global.setW(pose_msg->pose.pose.orientation.w);tf::Matrix3x3(q_global).getRPY(roll_global, pitch_global, yaw_global);//global transformationtransformInTheWorld[0] = roll_global;transformInTheWorld[1] = pitch_global;transformInTheWorld[2] = yaw_global;transformInTheWorld[3] = x;transformInTheWorld[4] = y;transformInTheWorld[5] = z;PointTypePose thisPose6DInWorld = trans2PointTypePose(transformInTheWorld);Eigen::Affine3f T_thisPose6DInWorld = pclPointToAffine3f(thisPose6DInWorld);//Odom transformationPointTypePose thisPose6DInOdom = trans2PointTypePose(transformTobeMapped);Eigen::Affine3f T_thisPose6DInOdom = pclPointToAffine3f(thisPose6DInOdom);//transformation: Odom to MapEigen::Affine3f T_OdomToMap = T_thisPose6DInWorld * T_thisPose6DInOdom.inverse();float delta_x, delta_y, delta_z, delta_roll, delta_pitch, delta_yaw;pcl::getTranslationAndEulerAngles (T_OdomToMap, delta_x, delta_y, delta_z, delta_roll, delta_pitch, delta_yaw);mtxtranformOdomToWorld.lock();//keep for co-operate the initializing and lio, not useful for the presenttranformOdomToWorld[0] = delta_roll;tranformOdomToWorld[1] = delta_pitch;tranformOdomToWorld[2] = delta_yaw;tranformOdomToWorld[3] = delta_x;tranformOdomToWorld[4] = delta_y;tranformOdomToWorld[5] = delta_z;mtxtranformOdomToWorld.unlock();initializedFlag = NonInitialized;//okagvstd::cout << "transformInTheWorld R P Y X Y Z " << transformInTheWorld[0] << " "<< transformInTheWorld[1] << " "<< transformInTheWorld[2] << " "<< transformInTheWorld[3] << " "<< transformInTheWorld[4] << " "<< transformInTheWorld[5] << std::endl;std::cout << "tranformOdomToWorld R P Y X Y Z "<< tranformOdomToWorld[0] << " "<< tranformOdomToWorld[1] << " "<< tranformOdomToWorld[2] << " "<< tranformOdomToWorld[3] << " "<< tranformOdomToWorld[4] << " "<< tranformOdomToWorld[5] << std::endl; ICPscanMatchGlobal();initializedFlag = Initialized;//globalLocalizeInitialiized = false;}

新增一个bool变量global_match_state用于检测是否重定位成功

 void ICPscanMatchGlobal(){//initializing
/*if(initializedFlag == NonInitialized){ICPLocalizeInitialize();return;}*/if (cloudKeyPoses3D->points.empty() == true)return;//change-5/******************added by gc************************/mtxWin.lock();int latestFrameIDGlobalLocalize;latestFrameIDGlobalLocalize = win_cloudKeyPoses3D.size() - 1;pcl::PointCloud<PointType>::Ptr latestCloudIn(new pcl::PointCloud<PointType>());*latestCloudIn += *transformPointCloud(win_cornerCloudKeyFrames[latestFrameIDGlobalLocalize], &win_cloudKeyPoses6D[latestFrameIDGlobalLocalize]);*latestCloudIn += *transformPointCloud(win_surfCloudKeyFrames[latestFrameIDGlobalLocalize],   &win_cloudKeyPoses6D[latestFrameIDGlobalLocalize]);std::cout << "the size of input cloud: " << latestCloudIn->points.size() <<std::endl;mtxWin.unlock();/******************added by gc************************///        int latestFrameIDGlobalLocalize;
//        latestFrameIDGlobalLocalize = cloudKeyPoses3D->size() - 1;
//
//
//        //latest Frame cloudpoints In the odom Frame
//
//        pcl::PointCloud<PointType>::Ptr latestCloudIn(new pcl::PointCloud<PointType>());
//        *latestCloudIn += *transformPointCloud(cornerCloudKeyFrames[latestFrameIDGlobalLocalize], &cloudKeyPoses6D->points[latestFrameIDGlobalLocalize]);
//        *latestCloudIn += *transformPointCloud(surfCloudKeyFrames[latestFrameIDGlobalLocalize],   &cloudKeyPoses6D->points[latestFrameIDGlobalLocalize]);
//        std::cout << "the size of input cloud: " << latestCloudIn->points.size() <<std::endl;pcl::NormalDistributionsTransform<PointType, PointType> ndt;ndt.setTransformationEpsilon(0.01);ndt.setResolution(1.0);pcl::IterativeClosestPoint<PointType, PointType> icp;icp.setMaxCorrespondenceDistance(100);icp.setMaximumIterations(100);icp.setTransformationEpsilon(1e-6);icp.setEuclideanFitnessEpsilon(1e-6);icp.setRANSACIterations(0);// Align cloud//3. calculate the tranform of odom relative to world//Eigen::Affine3f transodomToWorld_init = pcl::getTransformation(0,0,0,0,0,0);mtxtranformOdomToWorld.lock();std::cout << "ICPscanMatchGlobal R P Y X Y Z "<< tranformOdomToWorld[0] << " "<< tranformOdomToWorld[1] << " "<< tranformOdomToWorld[2] << " "<< tranformOdomToWorld[3] << " "<< tranformOdomToWorld[4] << " "<< tranformOdomToWorld[5] << std::endl;Eigen::Affine3f transodomToWorld_init = pcl::getTransformation(tranformOdomToWorld[3], tranformOdomToWorld[4], tranformOdomToWorld[5], tranformOdomToWorld[0], tranformOdomToWorld[1], tranformOdomToWorld[2]);mtxtranformOdomToWorld.unlock();//okagv//PointTypePose thisPose6DInWorld = trans2PointTypePose(transformInTheWorld);//Eigen::Affine3f T_thisPose6DInWorld = pclPointToAffine3f(thisPose6DInWorld);Eigen::Matrix4f matricInitGuess = transodomToWorld_init.matrix();//std::cout << "matricInitGuess: " << matricInitGuess << std::endl;//Firstly perform ndt in coarse resolutionndt.setInputSource(latestCloudIn);ndt.setInputTarget(cloudGlobalMapDS);pcl::PointCloud<PointType>::Ptr unused_result_0(new pcl::PointCloud<PointType>());ndt.align(*unused_result_0, matricInitGuess);//use the outcome of ndt as the initial guess for ICPicp.setInputSource(latestCloudIn);icp.setInputTarget(cloudGlobalMapDS);pcl::PointCloud<PointType>::Ptr unused_result(new pcl::PointCloud<PointType>());icp.align(*unused_result, ndt.getFinalTransformation());std::cout << "ICP converg flag:" << icp.hasConverged() << ". Fitness score: " << icp.getFitnessScore() << std::endl << std::endl;//if (icp.hasConverged() == false || icp.getFitnessScore() > historyKeyframeFitnessScore)// return;Eigen::Affine3f transodomToWorld_New;transodomToWorld_New = icp.getFinalTransformation();float x, y, z, roll, pitch, yaw;pcl::getTranslationAndEulerAngles (transodomToWorld_New, x, y, z, roll, pitch, yaw);//std::cout << " in test 0.03: deltaX = " << x << " deltay = " << y << " deltaZ = " << z << std::endl;mtxtranformOdomToWorld.lock();//renew tranformOdomToWorldtranformOdomToWorld[0] = roll;tranformOdomToWorld[1] = pitch;tranformOdomToWorld[2] = yaw;tranformOdomToWorld[3] = x;tranformOdomToWorld[4] = y;tranformOdomToWorld[5] = z;mtxtranformOdomToWorld.unlock();//publish the laserpointcloud in world frame//publish global mappublishCloud(&pubMapWorld, cloudGlobalMapDS, timeLaserInfoStamp, "map");//publish world mapif (icp.hasConverged() == true && icp.getFitnessScore() < historyKeyframeFitnessScore){geometry_msgs::PoseStamped pose_odomTo_map;tf::Quaternion q_odomTo_map = tf::createQuaternionFromRPY(roll, pitch, yaw);pose_odomTo_map.header.stamp = timeLaserInfoStamp;pose_odomTo_map.header.frame_id = "map";pose_odomTo_map.pose.position.x = x; pose_odomTo_map.pose.position.y = y; pose_odomTo_map.pose.position.z = z;pose_odomTo_map.pose.orientation.x = q_odomTo_map.x();pose_odomTo_map.pose.orientation.y = q_odomTo_map.y();pose_odomTo_map.pose.orientation.z = q_odomTo_map.z();pose_odomTo_map.pose.orientation.w = q_odomTo_map.w();pubOdomToMapPose.publish(pose_odomTo_map);global_match_state = true;}else{initializedFlag = NonInitialized;}//publish the trsformation between map and odom}

检测重定位成功后才发布匹配后的位姿

    void publishFrames(){if (cloudKeyPoses3D->points.empty())return;// publish key posespublishCloud(&pubKeyPoses, cloudKeyPoses3D, timeLaserInfoStamp, "odom");// Publish surrounding key framespublishCloud(&pubRecentKeyFrames, laserCloudSurfFromMapDS, timeLaserInfoStamp, "odom");// publish registered key frame//gc: feature pointsif (pubRecentKeyFrame.getNumSubscribers() != 0){pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>());PointTypePose thisPose6D = trans2PointTypePose(transformTobeMapped);*cloudOut += *transformPointCloud(laserCloudCornerLastDS,  &thisPose6D);*cloudOut += *transformPointCloud(laserCloudSurfLastDS,    &thisPose6D);publishCloud(&pubRecentKeyFrame, cloudOut, timeLaserInfoStamp, "odom");}//added *****************by gcif(pubLaserCloudInWorld.getNumSubscribers() != 0 && global_match_state == true){/*std::cout << "transformTobeMapped R P Y X Y Z"<< " " << transformTobeMapped[0] << " " << transformTobeMapped[1] << " " << transformTobeMapped[2] << " " << transformTobeMapped[3] << " " << transformTobeMapped[4] << " " << transformTobeMapped[5] << std::endl;*/pcl::PointCloud<PointType>::Ptr cloudInBase(new pcl::PointCloud<PointType>());pcl::PointCloud<PointType>::Ptr cloudOutInWorld(new pcl::PointCloud<PointType>());PointTypePose thisPose6DInOdom = trans2PointTypePose(transformTobeMapped);Eigen::Affine3f T_thisPose6DInOdom = pclPointToAffine3f(thisPose6DInOdom);mtxtranformOdomToWorld.lock();PointTypePose pose_Odom_Map = trans2PointTypePose(tranformOdomToWorld);mtxtranformOdomToWorld.unlock();Eigen::Affine3f T_pose_Odom_Map = pclPointToAffine3f(pose_Odom_Map);Eigen::Affine3f T_poseInMap = T_pose_Odom_Map * T_thisPose6DInOdom;*cloudInBase += *laserCloudCornerLastDS;*cloudInBase += *laserCloudSurfLastDS;pcl::transformPointCloud(*cloudInBase, *cloudOutInWorld, T_poseInMap.matrix());publishCloud(&pubLaserCloudInWorld, cloudOutInWorld, timeLaserInfoStamp, "map");}//added *********************by gc// publish registered high-res raw cloud//gc: whole point_cloud of the scanif (pubCloudRegisteredRaw.getNumSubscribers() != 0){pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>());pcl::fromROSMsg(cloudInfo.cloud_deskewed, *cloudOut);PointTypePose thisPose6D = trans2PointTypePose(transformTobeMapped);*cloudOut = *transformPointCloud(cloudOut,  &thisPose6D);publishCloud(&pubCloudRegisteredRaw, cloudOut, timeLaserInfoStamp, "odom");}// publish pathif (pubPath.getNumSubscribers() != 0){globalPath.header.stamp = timeLaserInfoStamp;globalPath.header.frame_id = "odom";pubPath.publish(globalPath);}}

这样，一开启软件后程序会直接执行里程计计算，然后给一个初始位姿后会进行重定位，然后定位成功后就会发布匹配后的当前激光数据了。

适当修改LIO-SAM_based_relocalization解决初始重定位显示错误相关推荐

注册表桌面显示计算机,电脑开机后不显示桌面图标怎么办？修改注册表解决开机后不显示桌面图标的3种方法...
解决方法一: 1.按Win+R打开运行,输入regedit并回车: 2.在注册表展开:HKEY_CURRENT_USER\Software\Microsoft\Windows\CurrentVersi ...
微信修改文件路径后任务栏图标显示错误
如下图,在修改了微信的文件保存路径后,图标就变成了这个样子,看着烦死了,然后我就试着卸载重装,表示并没有什么用,因为我受不了呀,然后就试上面都是废话,嘻嘻解决办法: 右键失效图标,点将此程序锁定到 ...
（解决）mysql1366中文显示错误的终极解决方案
在创建表格后录入数据时,数据库发生1366错误,百度后发现是中文编码问题,不过按照百度的将my.ini文件中的default-character-set改成utf8,重启后依然没有任何作用, 使用该 ...
S5PV210体系结构与接口04：代码重定位 SDRAM初始化
目录 1. C语言环境初始化 1.1 C语言运行所需环境 1.2 初始化栈 1.2.1 栈的概念 1.2.2 栈的作用 1.2.3 如何初始化 1.3 初始化bss段 1.3.1 bss段的作用 1. ...
韦东山第一期学习笔记——重定位
重定位说明必须知道的几个概念什么是代码重定位? 什么是位置无关码什么是运行地址为什么要代码重定位? nand flash启动的情况 nor flash启动的情况两种方式的重定位代码重定位 ...
【Android 逆向】逆向修改游戏应用 ( 分析应用结构 | 定位动态库位置 | 定位动态库中的修改点 | 修改动态库 | 重打包 )
文章目录一.应用结构分析二.定位动态库位置三.定位动态库中的修改点四.修改动态库五.重打包一.应用结构分析分析上一篇博客 [Android 逆向]逆向修改游戏应用 ( APK 解析工具 ...
PE文件-手工修改重定位表-WinHex-CFF Explorer
文章目录 1.CFF Explorer 2.计算添加后的重定位大小 3.作者答疑 1.CFF Explorer 如果需要修改exe,dll等的二进制代码,遇到添加绝对地址时,需要将绝对地址的位置添 ...
uboot2012(一)分析重定位
目录引入环境配置编译体验入口查找代码分析 board_init_f pie 内存分布分析 SP设置 board_init_f 重定位代码段重定位实现变量地址修改参考引入关于移植,搜 ...
第四天：关看门狗、设置栈、控制icache、重定位、链接脚本
1.汇编写启动代码:关看门狗什么是看门狗? 看门狗(watch dog timer看门狗定时器),比如:家门口有一只狗,这个狗定时会饿(譬如两小时一饿),狗饿了就会胡乱咬人,人进进出出要想保证安全必 ...
《5.SDRAM和重定位relocate》
转自 https://edu.csdn.net/lecturer/505 朱老师物联网大讲堂 <5.SDRAM和重定位relocate> 第一部分.章节目录 1.5.1.汇编写启动代码之关 ...

适当修改LIO-SAM_based_relocalization解决初始重定位显示错误

适当修改LIO-SAM_based_relocalization解决初始重定位显示错误相关推荐

最新文章

热门文章