lio-sam实车调试之定位测试

1.首先阅读一下recolization的源码，看一下和mapping有啥区别：

-imageProjection.cpp中：

cloudInfo.imuPreintegrationResetId = round(startOdomMsg.pose.covariance[0]);

计算相对变换（从雷达的起始时刻到当前imu数据来的时候）

-imuPreintegration.cpp中：

int imuPreintegrationResetId = 0;
订阅odom的里程计？猜测应该是方便显示
    ros::Subscriber subPoseOdomToMap;
    geometry_msgs::PoseStamped poseOdomToMap;
    subPoseOdomToMap = nh.subscribe("lio_sam/mapping/pose_odomTo_map", 1,&IMUPreintegration::odomToMapPoseHandler,      this, ros::TransportHints().tcpNoDelay());
    pubImuPath = nh.advertise("lio_sam/imu/path", 1);
    map_to_odom    = tf::Transform(tf::createQuaternionFromRPY(0, 0, 0), tf::Vector3(0, 0, 0));

void odomToMapPoseHandler(const geometry_msgs::PoseStamped::ConstPtr& poseOdomToMapmsg)
    {
        tf::Quaternion q_tem;
        q_tem.setX(poseOdomToMapmsg->pose.orientation.x);
        q_tem.setY(poseOdomToMapmsg->pose.orientation.y);
        q_tem.setZ(poseOdomToMapmsg->pose.orientation.z);
        q_tem.setW(poseOdomToMapmsg->pose.orientation.w);
        tf::Vector3 p_tem(poseOdomToMapmsg->pose.position.x, poseOdomToMapmsg->pose.position.y, poseOdomToMapmsg->pose.position.z);

        map_to_odom    = tf::Transform(q_tem, p_tem);

    }

int currentResetId = round(odomMsg->pose.covariance[0]);
if (currentResetId != imuPreintegrationResetId)
{
       resetParams();
       imuPreintegrationResetId = currentResetId;
       return;
}     
单独发布了imu的path
 // publish imu path
        static nav_msgs::Path imuPath;
        static double last_path_time = -1;
        if (imuTime - last_path_time > 0.1)
        {
            last_path_time = imuTime;
            geometry_msgs::PoseStamped pose_stamped;
            pose_stamped.header.stamp = thisImu.header.stamp;
            pose_stamped.header.frame_id = "odom";
            pose_stamped.pose = odometry.pose.pose;
            imuPath.poses.push_back(pose_stamped);
            while(!imuPath.poses.empty() && abs(imuPath.poses.front().header.stamp.toSec() - imuPath.poses.back().header.stamp.toSec()) > 3.0)
                imuPath.poses.erase(imuPath.poses.begin());
            if (pubImuPath.getNumSubscribers() != 0)
            {
                imuPath.header.stamp = thisImu.header.stamp;
                imuPath.header.frame_id = "odom";
                pubImuPath.publish(imuPath);
            }
        }   

-mapOptimization.cpp:这个文件没参与到执行过程中。。。

没有回环？或者说没有发布回环约束边；

添加了最近的关键帧点云：

pcl::PointCloud::Ptr latestKeyFrameCloud;
pcl::PointCloud::Ptr nearHistoryKeyFrameCloud;

执行了闭环检测，去掉了线程池

#pragma omp parallel for num_threads(numberOfCores)

-globalLocalize.cpp：

读入整个地图在哪啊！！！在这！
 

    void cloudGlobalLoad()
    {
        pcl::io::loadPCDFile(std::getenv("HOME") + savePCDDirectory + "cloudGlobal.pcd", *cloudGlobalMap);

        pcl::PointCloud::Ptr cloud_temp(new pcl::PointCloud());
        downSizeFilterICP.setInputCloud(cloudGlobalMap);
        downSizeFilterICP.filter(*cloud_temp);
        //*cloudGlobalMap = *cloud_temp;
        *cloudGlobalMapDS = *cloud_temp;

        std::cout << "test 0.01  the size of global cloud: " << cloudGlobalMap->points.size() << std::endl;
        std::cout << "test 0.02  the size of global map after filter: " << cloudGlobalMapDS->points.size() << std::endl;
    }

测试结果：

 测试分析：

1.地图从默认目录中存储和读取。

2.

已经测试：

①目前必须从起始位置附近（不超过3m范围内，朝向与初始方向一致）才能完成初始配准。

②之后的路径可以偏离，即使在长走廊环境也不会因为重复特征漂移，因为有imu的积分带来的位姿估计。

未测试：

室外环境+颠簸环境+长距离