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

Posted 2021-09-05 COCO_PEAK_NOODLE

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

    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<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 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);
        }
    }

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