SLAM笔记-hector

内容来源：http://t.csdn.cn/is8ga

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

构造函数

里面包含的功能：参数初始化；多层地图的初始化（这个涉及到路线规划，代码这里有最高精度的地图）；新建线程实现地图发布；要在五次中找到base_link->front_laser_link的tf关系

// 构造函数
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.");
        }
    }
}

回调函数

将scan转换为pointcloud；再把pointcloud转换hector内部的数据结构，更新并储存baselink在map中的位姿；发布odom的topic

/**
 * 激光数据处理回调函数，将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_));
        }
    }
}

转换SCAN格式的函数（把pointcloud转换hector内部的数据结构）

// 将点云数据转换成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;
}

 update()

    /**
    * 对每一帧的激光数据进行处理
    * @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;
        }
    }

matchData()_1

    /**
     * 地图匹配，通过多分辨率地图求解当前激光帧的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;
    }

matchData()_2

    /**
   * 给定位姿初值，估计当前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);
    }

 matchData()_3

    /**
     * 实际进行位姿估计的函数
     * @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;
    }

estimateTransformationLogLh()

    /**
     *  高斯牛顿估计位姿
     * @param estimate      位姿初始值
     * @param gridMapUtil   网格地图相关计算工具
     * @param dataPoints    激光数据
     * @return  提示是否有解　－－－　貌似没用上
    */
    bool estimateTransformationLogLh(Eigen::Vector3f &estimate,
                                     ConcreteOccGridMapUtil &gridMapUtil,
                                     const DataContainer &dataPoints)
    {
        /** 核心函数，计算H矩阵和dTr向量(ｂ列向量)---- 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;
    }