适当修改LIO-SAM_based_relocalization解决初始重定位显示错误
去掉初始化成功后才进行里程计。
void laserCloudInfoHandler(const lio_sam::cloud_infoConstPtr& msgIn)
{
// extract time stamp
//added
timeLaserInfoStamp = msgIn->header.stamp;
timeLaserCloudInfoLast = msgIn->header.stamp.toSec();
// extract info and feature cloud
cloudInfo = *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 Initialization
transformTobeMapped[2] = 0;
}
return;
}
frameNum++;
*/
/************************************added by gc*****************************/
std::lock_guard<std::mutex> lock(mtx);
if (timeLaserCloudInfoLast - timeLastProcessing >= mappingProcessInterval) {//gc:control the rate of mapping process
timeLastProcessing = timeLaserCloudInfoLast;
updateInitialGuess();//gc: update initial value for states
extractSurroundingKeyFrames();//gc:
downsampleCurrentScan();//gc:down sample the current corner points and surface points
scan2MapOptimization();//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 measurement
saveKeyFramesAndFactor();//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-z
if(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-yaw
tf::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 transformation
transformInTheWorld[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 transformation
PointTypePose thisPose6DInOdom = trans2PointTypePose(transformTobeMapped);
Eigen::Affine3f T_thisPose6DInOdom = pclPointToAffine3f(thisPose6DInOdom);
//transformation: Odom to Map
Eigen::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 present
tranformOdomToWorld[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;
//okagv
std::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::Ptr latestCloudIn(new pcl::PointCloud());
// *latestCloudIn += *transformPointCloud(cornerCloudKeyFrames[latestFrameIDGlobalLocalize], &cloudKeyPoses6D->points[latestFrameIDGlobalLocalize]);
// *latestCloudIn += *transformPointCloud(surfCloudKeyFrames[latestFrameIDGlobalLocalize], &cloudKeyPoses6D->points[latestFrameIDGlobalLocalize]);
// std::cout << "the size of input cloud: " << latestCloudIn->points.size() <
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 resolution
ndt.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 ICP
icp.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 tranformOdomToWorld
tranformOdomToWorld[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 map
publishCloud(&pubMapWorld, cloudGlobalMapDS, timeLaserInfoStamp, "map");//publish world map
if (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 poses
publishCloud(&pubKeyPoses, cloudKeyPoses3D, timeLaserInfoStamp, "odom");
// Publish surrounding key frames
publishCloud(&pubRecentKeyFrames, laserCloudSurfFromMapDS, timeLaserInfoStamp, "odom");
// publish registered key frame
//gc: feature points
if (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 gc
if(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 scan
if (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 path
if (pubPath.getNumSubscribers() != 0)
{
globalPath.header.stamp = timeLaserInfoStamp;
globalPath.header.frame_id = "odom";
pubPath.publish(globalPath);
}
}
这样，一开启软件后程序会直接执行里程计计算，然后给一个初始位姿后会进行重定位，然后定位成功后就会发布匹配后的当前激光数据了。