上篇文章讲解了如何在固定位置使用Hector构建单帧的栅格地图,以及知道了SLAM的本质就是将不同时刻的scan在正确的位置上写成栅格地图.

本篇文章将对 Hector 进行简单的重写,使得其代码更简单,更清晰.

这也是本系列教程第一次成功建出一张比较好的地图.

先放图,虽然有些瑕疵,但是整体还是不错的.
在这里插入图片描述
话不多说,先说明一下对代码做了那些改变,然后再着重讲解一下 Hector是如何做scan match的.

1 代码简单重构

相对于原版Hector做了哪些改变

  • 重写了原版的HectorMappingRos.cpp,使得代码变得更加简单与清晰
  • 发布了 map -> odom -> base_link 的 TF树
  • 去掉了原版的 DebugInfo 以及 Drawing 这两个功能以及代码文件.个人感觉这两个功能用不上,导致代码复杂
  • 更改了一些文件的位置,使得现在的.cc文件只有一个,其余全是头文件.原版的src文件夹下有很多文件
  • 对很多文件进行了注释

2 代码讲解

2.1 获取代码

代码已经提交在github上了,如果不知道github的地址的朋友, 请在我的公众号: 从零开始搭激光SLAM 中回复 开源地址 获得。

推荐使用 git clone 的方式进行下载, 因为代码是正处于更新状态的, git clone 下载的代码可以使用 git pull 很方便地进行更新.

本篇文章对应的代码为 Creating-2D-laser-slam-from-scratch/lesson4/src/hector_mapping/ 与 Creating-2D-laser-slam-from-scratch/lesson4/include/hector_mapping/hector_slam.cc

2.2 构造函数

在构造函数中 首先调用 InitParams 进行了 ROS参数的初始化.

之后对 HectorSlamProcessor 对象进行初始化.

再之后对多层地图进行了初始化,并调用 setMapInfo() 进行 ROS地图的内存的分配.

最后,查找从 base_link 到 雷达坐标系 间的坐标变换.

// 构造函数
HectorMappingRos::HectorMappingRos()
    : private_node_("~"), lastGetMapUpdateIndex(-100), tfB_(0), map_publish_thread_(0)
{
    ROS_INFO_STREAM("\033[1;32m----> Hector SLAM started.\033[0m");

    // 参数初始化
    InitParams();

    laser_scan_subscriber_ = node_handle_.subscribe(p_scan_topic_, p_scan_subscriber_queue_size_, &HectorMappingRos::scanCallback, this); // 雷达数据处理

    if (p_pub_odometry_)
    {
        odometryPublisher_ = node_handle_.advertise<nav_msgs::Odometry>("odom_hector", 50);
    }

    tfB_ = new tf::TransformBroadcaster();

    slamProcessor = new hectorslam::HectorSlamProcessor(static_cast<float>(p_map_resolution_),
                                                        p_map_size_, p_map_size_,
                                                        Eigen::Vector2f(p_map_start_x_, p_map_start_y_),
                                                        p_map_multi_res_levels_);

    slamProcessor->setUpdateFactorFree(p_update_factor_free_);                // 0.4
    slamProcessor->setUpdateFactorOccupied(p_update_factor_occupied_);        // 0.9
    slamProcessor->setMapUpdateMinDistDiff(p_map_update_distance_threshold_); // 0.4
    slamProcessor->setMapUpdateMinAngleDiff(p_map_update_angle_threshold_);   // 0.9

    // 多层地图的初始化
    int mapLevels = slamProcessor->getMapLevels();
    mapLevels = 1; // 这里设置成只发布最高精度的地图,如果有其他需求,如进行路径规划等等需要多层地图时,注释本行。

    std::string mapTopic_ = "map";
    for (int i = 0; i < mapLevels; ++i)
    {
        mapPubContainer.push_back(MapPublisherContainer());
        slamProcessor->addMapMutex(i, new HectorMapMutex());

        std::string mapTopicStr(mapTopic_);

        if (i != 0)
        {
            mapTopicStr.append("_" + boost::lexical_cast<std::string>(i));
        }

        std::string mapMetaTopicStr(mapTopicStr);
        mapMetaTopicStr.append("_metadata");

        MapPublisherContainer &tmp = mapPubContainer[i];
        tmp.mapPublisher_ = node_handle_.advertise<nav_msgs::OccupancyGrid>(mapTopicStr, 1, true);
        tmp.mapMetadataPublisher_ = node_handle_.advertise<nav_msgs::MapMetaData>(mapMetaTopicStr, 1, true);

        setMapInfo(tmp.map_, slamProcessor->getGridMap(i)); // 设置地图服务

        if (i == 0)
        {
            mapPubContainer[i].mapMetadataPublisher_.publish(mapPubContainer[i].map_.map.info);
        }
    }

	// 新建一个线程用来发布地图
    map_publish_thread_ = new boost::thread(boost::bind(&HectorMappingRos::publishMapLoop, this, p_map_pub_period_));
    map_to_odom_.setIdentity();

    // 查找 base_link -> front_laser_link 的tf,循环5次以确保其能找到
    int count = 5;
    ros::Duration dur(0.5);
    while (count-- != 0)
    {
        if (tf_.waitForTransform(p_base_frame_, p_scan_frame_, ros::Time(0), dur))
        {
            tf_.lookupTransform(p_base_frame_, p_scan_frame_, ros::Time(0), laserTransform_);
            break;
        }
        else
        {
            ROS_WARN("lookupTransform laser frame into base_link timed out.");
        }
    }
}

2.3 回调函数

首先进行了雷达数据格式的转换. 先通过 laser_geometry 这个包将 LaserScan 转换成 PointCloud 格式,这个很简单,我之前的文章已经实现过了,这里懒的改了.

之后是将雷达数据从点云形式转换到Hector自己的雷达数据存储格式.

再之后就是进行扫描匹配与地图更新.

扫描匹配计算完成之后再发布里程计 topic 与 tf,tf将根据不同的配置决定是发布 map -> base_link 还是 map -> odom -> base_link.


想要看输出的里程计信息 topic 就必须要有odom坐标系.

这里将 map 与 odom 的坐标系重合了( 没有对 map_to_odom_ 再赋值 ) ,所以 base_link 在 map 与 odom 下的坐标是相同的.

在发布TF的时候,将TF的时间戳都设置成了雷达的时间戳,这块好像有问题,会导致在rviz里 fixed frame 设置成map时看不到hector发布的odom数据。

我也将时间戳改成了 ros::Time::now() ,改完了之后有时能看到有时看不到,还不清楚原因,所以先在rviz里将 fixed frame 设置成了odom.

/**
 * 激光数据处理回调函数,将ros数据格式转换为算法中的格式,并转换成地图尺度,交由slamProcessor处理。
 * 算法中所有的计算都是在地图尺度下进行。  
 */
void HectorMappingRos::scanCallback(const sensor_msgs::LaserScan &scan)
{
    start_time_ = std::chrono::steady_clock::now();

    ros::WallTime startTime = ros::WallTime::now();

    // 将 scan 转换成 点云格式
    projector_.projectLaser(scan, laser_point_cloud_, 30.0);

    Eigen::Vector3f startEstimate(Eigen::Vector3f::Zero());

    // 将雷达数据的点云格式 更改成 hector 内部的数据格式
    if (rosPointCloudToDataContainer(laser_point_cloud_, laserTransform_, laserScanContainer, slamProcessor->getScaleToMap()))
    {
        // 首先获取上一帧的位姿,作为初值
        startEstimate = slamProcessor->getLastScanMatchPose();

        // 进入扫描匹配与地图更新
        slamProcessor->update(laserScanContainer, startEstimate);
    }
        
    end_time_ = std::chrono::steady_clock::now();
    time_used_ = std::chrono::duration_cast<std::chrono::duration<double>>(end_time_ - start_time_);
    std::cout << "数据转换与扫描匹配用时: " << time_used_.count() << " 秒。" << std::endl;

    if (p_timing_output_)
    {
        ros::WallDuration duration = ros::WallTime::now() - startTime;
        ROS_INFO("HectorSLAM Iter took: %f milliseconds", duration.toSec() * 1000.0f);
    }

    // 更新存储的位姿, 这里的位姿是 base_link 在 map 下的位姿
    poseInfoContainer_.update(slamProcessor->getLastScanMatchPose(), slamProcessor->getLastScanMatchCovariance(), scan.header.stamp, p_map_frame_);

    // 发布 odom topic
    if (p_pub_odometry_)
    {
        nav_msgs::Odometry tmp;
        tmp.pose = poseInfoContainer_.getPoseWithCovarianceStamped().pose;
        tmp.header = poseInfoContainer_.getPoseWithCovarianceStamped().header;
        tmp.child_frame_id = p_base_frame_;
        odometryPublisher_.publish(tmp);
    }

    if (pub_map_to_baselink_tf_)
    {
        // pub map -> odom -> base_link tf
        if (p_pub_map_odom_transform_)
        {
            tfB_->sendTransform(tf::StampedTransform(map_to_odom_, scan.header.stamp, p_map_frame_, p_odom_frame_));
            tfB_->sendTransform(tf::StampedTransform(poseInfoContainer_.getTfTransform(), scan.header.stamp, p_odom_frame_, p_tf_map_scanmatch_transform_frame_name_));
        }
        // pub map -> base_link tf
        else
        {
            tfB_->sendTransform(tf::StampedTransform(poseInfoContainer_.getTfTransform(), scan.header.stamp, p_map_frame_, p_tf_map_scanmatch_transform_frame_name_));
        }
    }
}

2.4 转换scan格式的函数

这里的 laserTransform 指的是从 base_link 到 雷达坐标系 间的坐标变换.

首先将base_link到雷达坐标系的坐标转换 乘以地图分辨率 设置为这帧数据的 origo.

之后对每个雷达数据进行距离判定,如果距离太近或者太远就跳过.

这里还进行了如果距离大于 雷达数据的最大使用距离 的判断,因为雷达数据太远时的数据跳动比较厉害,所以这里将 p_use_max_scan_range_ 之外的数据也过滤掉不进行使用.

之后就是将数据点进行坐标变换,通过左乘laserTransform得到在base_link下的正确的x y 值,因为只进行了一次左乘laserTransform,所以转换后的z坐标是不正确的,通过再次减去laserTransform才得到正确的z坐标.

最后将base_link下的xy乘以分辨率放入dataContainer中.

// 将点云数据转换成Hector中雷达数据的格式
bool HectorMappingRos::rosPointCloudToDataContainer(const sensor_msgs::PointCloud &pointCloud, const tf::StampedTransform &laserTransform, hectorslam::DataContainer &dataContainer, float scaleToMap)
{
    size_t size = pointCloud.points.size();
    dataContainer.clear();

    tf::Vector3 laserPos(laserTransform.getOrigin());

    // dataContainer.setOrigo(Eigen::Vector2f::Zero());
    // 将base_link到雷达坐标系的坐标转换 乘以地图分辨率 当成这帧数据的 origo
    dataContainer.setOrigo(Eigen::Vector2f(laserPos.x(), laserPos.y()) * scaleToMap);

    for (size_t i = 0; i < size; ++i)
    {
        const geometry_msgs::Point32 &currPoint(pointCloud.points[i]);

        float dist_sqr = currPoint.x * currPoint.x + currPoint.y * currPoint.y;
        if ((dist_sqr > p_sqr_laser_min_dist_) && (dist_sqr < p_sqr_laser_max_dist_))
        {
            if ((currPoint.x < 0.0f) && (dist_sqr < 0.50f))
            {
                continue;
            }

            // 距离太远的点跳动太大,如果距离大于使用距离(20m),就跳过
            if (dist_sqr > p_use_max_scan_range_ * p_use_max_scan_range_)
                continue;
            
            // 点的坐标左乘base_link->laser_link的tf 将得到该点在base_link下的 xy 坐标, 但是z坐标是不正确
            tf::Vector3 pointPosBaseFrame(laserTransform * tf::Vector3(currPoint.x, currPoint.y, currPoint.z));
            
            // 通过再减去 base_link->laser_link的tf的z的值,得到该点在base_link下正确的 z 坐标
            float pointPosLaserFrameZ = pointPosBaseFrame.z() - laserPos.z();

            if (pointPosLaserFrameZ > p_laser_z_min_value_ && pointPosLaserFrameZ < p_laser_z_max_value_)
            {
                // 将雷达数据的 x y 都乘地图的分辨率 0.05 再放入dataContainer中
                dataContainer.add(Eigen::Vector2f(pointPosBaseFrame.x(), pointPosBaseFrame.y()) * scaleToMap);
            }
        }
    }
    return true;
}

2.5 update()

update() 函数位于 Creating-2D-laser-slam-from-scratch/lesson4/include/lesson4/hector_mapping/slam_main/HectorSlamProcessor.h中.

将scan_match与地图更新分成了2步进行.

scan_match是调用mapRep->matchData()函数,返回了当前scan在与地图进行扫描匹配后的地图坐标系下的位姿.

之后调用了updateByScan()进行了地图的更新,这块代码已经在上一篇文章中讲过了,所以这里就不再进行讲解了.

    /**
    * 对每一帧的激光数据进行处理
    * @param dataContainer  激光数据存储容器,坐标已转换成地图尺度,为地图中激光系的坐标
    * @param poseHintWorld  激光系在地图中的初始pose
    * @param map_without_matching 是否进行匹配
    */
    void update(const DataContainer &dataContainer, const Eigen::Vector3f &poseHintWorld, bool map_without_matching = false)
    {
        //std::cout << "\nph:\n" << poseHintWorld << "\n";

        /** 1. 位姿匹配 **/
        Eigen::Vector3f newPoseEstimateWorld;

        if (!map_without_matching)
        {
            // 进行 scan to map 的地方
            newPoseEstimateWorld = (mapRep->matchData(poseHintWorld, dataContainer, lastScanMatchCov));
        }
        else
        {
            newPoseEstimateWorld = poseHintWorld;
        }

        lastScanMatchPose = newPoseEstimateWorld;

        /** 2.地图更新 **/
        if (util::poseDifferenceLargerThan(newPoseEstimateWorld, lastMapUpdatePose, paramMinDistanceDiffForMapUpdate, paramMinAngleDiffForMapUpdate) || map_without_matching)
        { // 仅在位姿变化大于阈值 或者 map_without_matching为真 的时候进行地图更新
            mapRep->updateByScan(dataContainer, newPoseEstimateWorld);

            mapRep->onMapUpdated();
            lastMapUpdatePose = newPoseEstimateWorld;
        }
    }

2.5.1 matchData()_1

第一个matchData() 位于 Creating-2D-laser-slam-from-scratch/lesson4/include/lesson4/hector_mapping/slam_main/MapRepMultiMap.h中.

假设一共3层多分辨率地图,这里首先使用 setFrom() 将雷达数据dataContainer进行长度的缩短,对每个原始数据都乘以 1/(2的n次方) , n是 2, 1, 0.

之后使用缩短后的雷达数据,与对应分辨率的地图进行扫描匹配.

也就是在粗分辨率地图上先进行扫描匹配,得出的结果 再传入 更精细分辨率地图上进行扫描匹配.

最终返回一个在map坐标系下的位姿.

    /**
     * 地图匹配,通过多分辨率地图求解当前激光帧的pose。
     */
    virtual Eigen::Vector3f matchData(const Eigen::Vector3f &beginEstimateWorld, const DataContainer &dataContainer, Eigen::Matrix3f &covMatrix)
    {
        size_t size = mapContainer.size();

        Eigen::Vector3f tmp(beginEstimateWorld);

        /// coarse to fine 的pose求精过程,i层的求解结果作为i-1层的求解初始值。
        for (int index = size - 1; index >= 0; --index)
        {
            //std::cout << " m " << i;
            if (index == 0)
            {
                tmp = (mapContainer[index].matchData(tmp, dataContainer, covMatrix, 5)); /// 输入数据dataContainer 对应 mapContainer[0]
            }
            else
            {
                // 根据地图层数对原始激光数据坐标进行缩小,得到对应图层尺寸的激光数据
                dataContainers[index - 1].setFrom(dataContainer, static_cast<float>(1.0 / pow(2.0, static_cast<double>(index))));
                tmp = (mapContainer[index].matchData(tmp, dataContainers[index - 1], covMatrix, 3));
                /// dataContainers[i]对应mapContainer[i+1]
            }
        }
        return tmp;
    }

2.5.2 matchData()_2

第二个machData() 位于 Creating-2D-laser-slam-from-scratch/lesson4/include/lesson4/hector_mapping/slam_main/MapProcContainer.h 中.

可以看到,这个函数中并没有额外的操作,只是将对应分辨率的地图也一起传入到第三个mathcData() 中.

    /**
   * 给定位姿初值,估计当前scan在当前图层中的位姿 ---- 位姿为世界系下的pose 、  dataContainer应与当前图层尺度匹配
   * @param beginEstimateWorld 世界系下的位姿
   * @param dataContainer       激光数据
   * @param covMatrix           scan-match的不确定性 -- 协方差矩阵
   * @param maxIterations       最大的迭代次数
   * @return
   */
    Eigen::Vector3f matchData(const Eigen::Vector3f &beginEstimateWorld, const DataContainer &dataContainer, Eigen::Matrix3f &covMatrix, int maxIterations)
    {
        return scanMatcher->matchData(beginEstimateWorld, *gridMapUtil, dataContainer, covMatrix, maxIterations);
    }

2.5.3 matchData()_3

第三个 matchData() 位于 Creating-2D-laser-slam-from-scratch/lesson4/include/lesson4/hector_mapping/matcher/ScanMatcher.h 中.

这回终于是真正进行扫描匹配的地方了.


首先说一下hector world坐标系与hector map坐标系

之前说的这个初始位姿一直都是world坐标系下的坐标,Hector中认为实际物理坐标表示成的坐标为world坐标,以左上角为原点的.

hector map坐标就是用 world坐标再除以地图的分辨率0.05(也就是乘以20),再加上map坐标系的原点 得到的.也就是像素坐标.


这段代码先将给定的初始位姿转换成Hector的map坐标系下的像素坐标.

之后调用了 estimateTransformationLogLh() 进行计算位姿,之后又通过多次调用这个函数使结果更加准确.

求解的过程就在 estimateTransformationLogLh() 里.

这里的H矩阵是这个类的私有变量,代表hessian, 也就是要返回的协方差矩阵.

然后,将estimate从地图坐标系下转到world坐标系下,再返回.

    /**
     * 实际进行位姿估计的函数
     * @param beginEstimateWorld  位姿初值
     * @param gridMapUtil         网格地图工具,这里主要是用来做坐标变换
     * @param dataContainer       激光数据
     * @param covMatrix           协方差矩阵
     * @param maxIterations       最大迭代次数
     * @return
     */
    Eigen::Vector3f matchData(const Eigen::Vector3f &beginEstimateWorld,
                              ConcreteOccGridMapUtil &gridMapUtil, 
                              const DataContainer &dataContainer,
                              Eigen::Matrix3f &covMatrix, int maxIterations)
    {
        if (dataContainer.getSize() != 0)
        {

            // 1. 初始pose转换为地图尺度下的pose --仿射变换,包括位姿的尺度和偏移,旋转在 X Y 同尺度变化时保持不变
            Eigen::Vector3f beginEstimateMap(gridMapUtil.getMapCoordsPose(beginEstimateWorld));
            Eigen::Vector3f estimate(beginEstimateMap);

            // 2. 第一次迭代
            estimateTransformationLogLh(estimate, gridMapUtil, dataContainer);

            int numIter = maxIterations;
            /** 3. 多次迭代求解 **/
            for (int i = 0; i < numIter; ++i)
            {
                estimateTransformationLogLh(estimate, gridMapUtil, dataContainer);
            }

            // 角度正则化
            estimate[2] = util::normalize_angle(estimate[2]);

            covMatrix = Eigen::Matrix3f::Zero();
            //covMatrix.block<2,2>(0,0) = (H.block<2,2>(0,0).inverse());
            //covMatrix.block<2,2>(0,0) = (H.block<2,2>(0,0));

            // 使用Hessian矩阵近似协方差矩阵
            covMatrix = H;

            // 结果转换回物理坐标系下 -- 转换回实际尺度
            return gridMapUtil.getWorldCoordsPose(estimate);
        }

        return beginEstimateWorld;
    }

2.6 estimateTransformationLogLh()

这个函数也位于 Creating-2D-laser-slam-from-scratch/lesson4/include/lesson4/hector_mapping/matcher/ScanMatcher.h 中.

首先,这个函数通过getCompleteHessianDerivs() 计算出了协方差矩阵H , 以及 dTr向量.

之后,通过高斯牛顿法最终推倒出的求根公式: H的逆 乘以 dtr 求出 先验位姿与后验位姿间的增量. 也就是 X = H.inverse() * dTr --> H * X = dTr

最后再将这个增量加上之前的预测值,得到匹配之后的位姿.

    /**
     *  高斯牛顿估计位姿
     * @param estimate      位姿初始值
     * @param gridMapUtil   网格地图相关计算工具
     * @param dataPoints    激光数据
     * @return  提示是否有解 --- 貌似没用上
    */
    bool estimateTransformationLogLh(Eigen::Vector3f &estimate,
                                     ConcreteOccGridMapUtil &gridMapUtil,
                                     const DataContainer &dataPoints)
    {
        /** 核心函数,计算H矩阵和dTr向量(b列向量)---- occGridMapUtil.h 中 **/
        gridMapUtil.getCompleteHessianDerivs(estimate, dataPoints, H, dTr);
        //std::cout << "\nH\n" << H  << "\n";
        //std::cout << "\ndTr\n" << dTr  << "\n";

        // 判断H是否可逆, 判断增量非0,避免无用计算
        if ((H(0, 0) != 0.0f) && (H(1, 1) != 0.0f))
        {
            // 求解位姿增量
            Eigen::Vector3f searchDir(H.inverse() * dTr);

            // 角度增量不能太大
            if (searchDir[2] > 0.2f)
            {
                searchDir[2] = 0.2f;
                std::cout << "SearchDir angle change too large\n";
            }
            else if (searchDir[2] < -0.2f)
            {
                searchDir[2] = -0.2f;
                std::cout << "SearchDir angle change too large\n";
            }

            /// 更新估计值 --- 结果在地图尺度下
            updateEstimatedPose(estimate, searchDir);
            return true;
        }
        return false;
    }

    void updateEstimatedPose(Eigen::Vector3f &estimate, const Eigen::Vector3f &change)
    {
        estimate += change;
    }

由于篇幅限制,getCompleteHessianDerivs() 函数的讲解与hector的公式推倒将在下篇文章中进行说明

3 运行

3.1 运行

本篇文章对应的数据包, 请在我的公众号中回复 lesson1 获得,并将launch中的bag_filename更改成您实际的目录名。

通过如下命令运行本篇文章对应的程序

roslaunch lesson4 hector_slam.launch

因为hector中依赖了 laser_geometry 这个包,如果有同学启动失败,提示缺少这个包,手动安装一下就好了.

3.2 运行结果与分析

启动之后,会在rviz中显示出如下画面.

首先,要说明一点的是rviz中的 Fixed Frame 我设置成了odom,因为我map与odom是重合的,所以这块设置成odom也是可以的.

如果设置成map,odom_hector这块会显示不出来,会提示找不到 TF,这个原因我还不太清楚,猜测是发布map->odom的tf的时间戳设置成雷达的时间戳这块存在问题,但是我没有去深扣它.

Rviz的 Fixed Frame 的意义是:所有显示出来的信息,都要通过 TF 进行坐标变换,变换到Fixed Frame 下,如果在规定的时间内没有查到 TF 就会报错,显示不了信息.

所以我将Fixed Frame设置成odom后,显示的2个odometry插件就不再需要进行坐标变换,可以直接显示出来.
在这里插入图片描述红色轨迹是小车上的编码器的结果,蓝色轨迹是hector_slam输出的里程计结果.

通过在建图的过程中对比雷达与地图的匹配,以及对比红色蓝色的轨迹,可以发现,hector计算出的里程计还是挺准确的,即使是回来的过程中,雷达与地图也是始终匹配上的.

但是,在回来的过程中,编码器产生的里程计与hector的里程计产生了较大的偏差.

通过最后小车停止时,雷达数据与地图依然能够很好的匹配这一现象,可以认为hector产生的里程计的准确程度大于小车自身的里程计.

至于走廊边缘那边界不清晰的问题,通过观看建图过程发现,主要是由于远距离的雷达点跳动产生的.

计算耗时

同时,终端会打印处如下信息,可以看到,执行一次回调的时间主要花费在了进行数据转换和扫描匹配上.如果时间打印的再详细些的话就会发现时间主要用在扫描匹配部分.

当运行时间长了之后,会发现进行扫描匹配的时间明显变长了.

数据转换与扫描匹配用时: 0.0942895 秒。
执行一次回调用时: 0.094357 秒。
数据转换与扫描匹配用时: 0.0934194 秒。
执行一次回调用时: 0.093491 秒。
# 一段时间之后
数据转换与扫描匹配用时: 0.119987 秒。
执行一次回调用时: 0.120077 秒。
数据转换与扫描匹配用时: 0.13254 秒。
执行一次回调用时: 0.133221 秒。

雷达是10hz的,处理一次回调就要花0.12秒了,这时就会在运算过程中丢弃雷达数据.**这个原因猜测是由于地图变大之后,与地图的匹配更加耗时导致的.**具体是哪一部分导致的还有待推敲。

我在代码里将多分辨率地图的层数设置成了3层,如果设置成2层的话也会减小时间消耗.但是匹配的效果就会变差.

hector里程计的频率

如果一直通过rostopic hz 来查看 雷达与odom_hector的频率的话,就会发现,到后期 odom_hector的频率已经从 10hz 下降到 7.9hz 了.

   topic        rate    min_delta   max_delta   std_dev    window
=================================================================
/laser_scan    9.986    0.09054     0.1107      0.003959   20    
/map           0.5228   1.913       1.913       0.0        20    
/odom_hector   10.2     0.08047     0.1209      0.01026    2     
# 一段时间之后
   topic        rate    min_delta   max_delta   std_dev    window
=================================================================
/laser_scan    10.0     0.08059     0.1166      0.004054   1984  
/map           0.5004   1.783       2.174       0.06475    1984  
/odom_hector   7.979    0.08047     0.3229      0.02474    100

4 总结与Next

本篇文章对Hecot的代码进行了简单的重构,发布了map与odom的tf,实现了一个基于scan_to_map的里程计,并构建出了第一张较成功的栅格地图.

下一篇文章将对 Hector 论文中的公式进行手动推倒,以及相应的代码讲解.

再下一篇文章将使用里程计数据进行SLAM,将基于 Karto 进行相关代码的实现.


文章将在 公众号: 从零开始搭SLAM 进行同步更新,欢迎大家关注,可以在公众号中添加我的微信,进激光SLAM交流群,大家一起交流SLAM技术。

同时,也希望您将这个公众号推荐给您身边做激光SLAM的人们,大家共同进步。

如果您对我写的文章有什么建议,或者想要看哪方面功能如何实现的,请直接在公众号中回复,我可以收到,并将认真考虑您的建议。

在这里插入图片描述