Hector代码笔记

阅读Hector代码时的笔记，更多Hector内容请看

https://blog.csdn.net/tiancailx/article/details/112978031

1 入口

hector_mapping/src/HectorMappingRos.cpp

1.1 scanCallback()

/**
 * 激光数据处理回调函数，将ros数据格式转换为算法中的格式，并转换成地图尺度，交由slamProcessor处理。
 * 算法中所有的计算都是在地图尺度下进行。  ROS部分的内容根据实际需求添加删减
 * @param scan  激光的数据ROS消息包
 */
void HectorMappingRos::scanCallback(const sensor_msgs::LaserScan& scan)
{
    if (hectorDrawings)
    {
        hectorDrawings->setTime(scan.header.stamp);
    }

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

    // 不使用tf，默认雷达的坐标系就是base_link
    if (!p_use_tf_scan_transformation_) // 未使用tf
    {

        if (rosLaserScanToDataContainer(scan, laserScanContainer,slamProcessor->getScaleToMap()))
        {
            slamProcessor->update(laserScanContainer,slamProcessor->getLastScanMatchPose());
        }
    }
    else
    {
        ros::Duration dur (0.5);

        if (tf_.waitForTransform(p_base_frame_,scan.header.frame_id, scan.header.stamp,dur))
        {
            tf::StampedTransform laserTransform;
            tf_.lookupTransform(p_base_frame_,scan.header.frame_id, scan.header.stamp, laserTransform);

            //projector_.transformLaserScanToPointCloud(p_base_frame_ ,scan, pointCloud,tf_);
            /** 将laser scan 转换成 laser frame下的点云消息 **/
            projector_.projectLaser(scan, laser_point_cloud_,30.0);

            if (scan_point_cloud_publisher_.getNumSubscribers() > 0){
                scan_point_cloud_publisher_.publish(laser_point_cloud_);
            }

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

            if(rosPointCloudToDataContainer(laser_point_cloud_, laserTransform, laserScanContainer, slamProcessor->getScaleToMap()))
            {
                if (initial_pose_set_){
                    initial_pose_set_ = false;
                    startEstimate = initial_pose_;
                }else if (p_use_tf_pose_start_estimate_){

                    try
                    {
                        tf::StampedTransform stamped_pose;

                        tf_.waitForTransform(p_map_frame_,p_base_frame_, scan.header.stamp, ros::Duration(0.5));
                        tf_.lookupTransform(p_map_frame_, p_base_frame_,  scan.header.stamp, stamped_pose);

                        tfScalar yaw, pitch, roll;
                        stamped_pose.getBasis().getEulerYPR(yaw, pitch, roll);

                        startEstimate = Eigen::Vector3f(stamped_pose.getOrigin().getX(),stamped_pose.getOrigin().getY(), yaw);
                    }
                    catch(tf::TransformException e)
                    {
                        ROS_ERROR("Transform from %s to %s failed\n", p_map_frame_.c_str(), p_base_frame_.c_str());
                        startEstimate = slamProcessor->getLastScanMatchPose();
                    }

                }else{
                    startEstimate = slamProcessor->getLastScanMatchPose();
                }


                if (p_map_with_known_poses_){
                    slamProcessor->update(laserScanContainer, startEstimate, true);
                }else{
                    // 进入扫描匹配与地图更新
                    slamProcessor->update(laserScanContainer, startEstimate);
                }
            }

        }else{
            ROS_INFO("lookupTransform %s to %s timed out. Could not transform laser scan into base_frame.", p_base_frame_.c_str(), scan.header.frame_id.c_str());
            return;
        }
    }
    // ...
}

1.2 update()

hector_mapping/include/hector_slam_lib/slam_main/HectorSlamProcessor.h

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

2.1 matchData()

hector_mapping/include/hector_slam_lib/slam_main/MapRepMultiMap.h

    /**
     * 地图匹配，通过金字塔求解当前激光帧的pose。
     * @param beginEstimateWorld
     * @param dataContainer
     * @param covMatrix
     * @return
     */
    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.2 matchData()

hector_mapping/include/hector_slam_lib/slam_main/MapProcContainer.h

  /**
   * 给定位姿初值，估计当前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.3 matchData()

hector_mapping/include/hector_slam_lib/matcher/ScanMatcher.h

    /**
     * 实际进行位姿估计的函数
     * @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 (drawInterface)
        {
            drawInterface->setScale(0.05f);
            drawInterface->setColor(0.0f, 1.0f, 0.0f);
            drawInterface->drawArrow(beginEstimateWorld);

            Eigen::Vector3f beginEstimateMap(gridMapUtil.getMapCoordsPose(beginEstimateWorld));

            drawScan(beginEstimateMap, gridMapUtil, dataContainer);

            drawInterface->setColor(1.0, 0.0, 0.0);
        }

        if (dataContainer.getSize() != 0)
        {

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

            Eigen::Vector3f estimate(beginEstimateMap);

            /// 2. 一次迭代
            estimateTransformationLogLh(estimate, gridMapUtil, dataContainer);
            //bool notConverged = estimateTransformationLogLh(estimate, gridMapUtil, dataContainer);


            //std::cout << "\n cond: " << cond << " det: " << determinant << "\n";

            int numIter = maxIterations;

            /** 3. 多次迭代求解 **/
            for (int i = 0; i < numIter; ++i)
            {
                //std::cout << "\nest:\n" << estimate;

                estimateTransformationLogLh(estimate, gridMapUtil, dataContainer);
                //notConverged = estimateTransformationLogLh(estimate, gridMapUtil, dataContainer);

                if (drawInterface)
                {
                    float invNumIterf = 1.0f / static_cast<float>(numIter);
                    drawInterface->setColor(static_cast<float>(i) * invNumIterf, 0.0f, 0.0f);
                    drawInterface->drawArrow(gridMapUtil.getWorldCoordsPose(estimate));
                    //drawInterface->drawArrow(Eigen::Vector3f(0.0f, static_cast<float>(i)*0.05, 0.0f));
                }

                if (debugInterface)
                {
                    debugInterface->addHessianMatrix(H);
                }
            }

            if (drawInterface)
            {
                drawInterface->setColor(0.0, 0.0, 1.0);
                drawScan(estimate, gridMapUtil, dataContainer);
            }

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

            covMatrix = Eigen::Matrix3f::Zero();

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

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

        return beginEstimateWorld;
    }

2.4 estimateTransformationLogLh()

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

        //　判断H是否可逆
        if ((H(0, 0) != 0.0f) && (H(1, 1) != 0.0f))
        {

            //H += Eigen::Matrix3f::Identity() * 1.0f;
            /// 求解位姿增量
            Eigen::Vector3f searchDir(H.inverse() * dTr);

            //std::cout << "\nsearchdir\n" << searchDir  << "\n";

            /// 角度增量不能太大
            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;
    }

2.5 getCompleteHessianDerivs()

hector_mapping/include/hector_slam_lib/map/OccGridMapUtil.h

    /**
     * 使用当前pose投影dataPoints到地图，计算出 H 矩阵 b列向量， 理论部分详见Hector论文： 《A Flexible and Scalable SLAM System with Full 3D Motion Estimation》.
     * @param pose    地图系上的位姿
     * @param dataPoints  已转换为地图尺度的激光点数据
     * @param H   需要计算的 H矩阵
     * @param dTr  需要计算的 b列向量
     */
    void getCompleteHessianDerivs(const Eigen::Vector3f& pose, const DataContainer& dataPoints, Eigen::Matrix3f& H, Eigen::Vector3f& dTr)
    {
        int size = dataPoints.getSize();

        /** 1. 获取变换矩阵，用于将激光点转换到地图系上。 **/
        Eigen::Affine2f transform(getTransformForState(pose));

        float sinRot = sin(pose[2]);
        float cosRot = cos(pose[2]);

        H = Eigen::Matrix3f::Zero();
        dTr = Eigen::Vector3f::Zero();

        for (int i = 0; i < size; ++i) {

            /** 地图尺度下的激光body系激光点坐标 **/
            const Eigen::Vector2f& currPoint (dataPoints.getVecEntry(i));

            /** transform * currPoint 为地图系下的激光点坐标 **/
            Eigen::Vector3f transformedPointData(interpMapValueWithDerivatives(transform * currPoint));
            /// 这里获取的是 transformedPointData[0]--网格插值得分   transformedPointData[1]--x方向的梯度  transformedPointData[2] -- y方向梯度

            float funVal = 1.0f - transformedPointData[0];

            dTr[0] += transformedPointData[1] * funVal;
            dTr[1] += transformedPointData[2] * funVal;

            float rotDeriv = ((-sinRot * currPoint.x() - cosRot * currPoint.y()) * transformedPointData[1] + (cosRot * currPoint.x() - sinRot * currPoint.y()) * transformedPointData[2]);

            dTr[2] += rotDeriv * funVal;

            H(0, 0) += util::sqr(transformedPointData[1]);
            H(1, 1) += util::sqr(transformedPointData[2]);
            H(2, 2) += util::sqr(rotDeriv);

            H(0, 1) += transformedPointData[1] * transformedPointData[2];
            H(0, 2) += transformedPointData[1] * rotDeriv;
            H(1, 2) += transformedPointData[2] * rotDeriv;
        }
        /// 上面就是按照公式计算H、b，具体公式见论文。
        /// 其中H是对称矩阵，只算上三角就行， 减少计算量。
        H(1, 0) = H(0, 1);
        H(2, 0) = H(0, 2);
        H(2, 1) = H(1, 2);
    }

3 地图更新

3.1 updateByScan()

hector_mapping/include/hector_slam_lib/slam_main/MapProcContainer.h

  /**
   * 有Scan数据更新地图
   * @param dataContainer   当前scan激光数据
   * @param robotPoseWorld  当前scan世界系下位姿
   */
  void updateByScan(const DataContainer &dataContainer, const Eigen::Vector3f &robotPoseWorld)
  {
      if (mapMutex)
      {
          mapMutex->lockMap();
      } //加锁，禁止其他线程竞争地图资源

      /// 更新地图
      gridMap->updateByScan(dataContainer, robotPoseWorld);

      if (mapMutex)
      {
          mapMutex->unlockMap();
      } //地图解锁
  }

3.2 updateByScan()

hector_mapping/include/hector_slam_lib/map/OccGridMapBase.h

    /**
     * 地图更新函数！！！ 使用scan的位姿和激光数据更新地图。英文注释已经很清楚了==
     * Updates the map using the given scan data and robot pose
     * @param dataContainer Contains the laser scan data
     * @param robotPoseWorld The 2D robot pose in world coordinates
     */
    void updateByScan(const DataContainer& dataContainer, const Eigen::Vector3f& robotPoseWorld)
    {
        currMarkFreeIndex = currUpdateIndex + 1;
        currMarkOccIndex = currUpdateIndex + 2;

        //Get pose in map coordinates from pose in world coordinates
        Eigen::Vector3f mapPose(this->getMapCoordsPose(robotPoseWorld));

        //Get a 2D homogenous transform that can be left-multiplied to a robot coordinates vector to get world coordinates of that vector
        Eigen::Affine2f poseTransform((Eigen::Translation2f(mapPose[0], mapPose[1]) * Eigen::Rotation2Df(mapPose[2])));
        ///实际上就是变换矩阵

        //Get start point of all laser beams in map coordinates (same for all beams, stored in robot coords in dataContainer)
        Eigen::Vector2f scanBeginMapf(poseTransform * dataContainer.getOrigo());

        //Get integer vector of laser beams start point
        Eigen::Vector2i scanBeginMapi(scanBeginMapf[0] + 0.5f, scanBeginMapf[1] + 0.5f);

        //Get number of valid beams in current scan
        int numValidElems = dataContainer.getSize();

        //std::cout << "\n maxD: " << maxDist << " num: " << numValidElems << "\n";

        //Iterate over all valid laser beams
        for (int i = 0; i < numValidElems; ++i)
        {

            //Get map coordinates of current beam endpoint
            Eigen::Vector2f scanEndMapf(poseTransform * (dataContainer.getVecEntry(i)));
            //std::cout << "\ns\n" << scanEndMapf << "\n";

            //add 0.5 to beam endpoint vector for following integer cast (to round, not truncate)
            scanEndMapf.array() += (0.5f);

            //Get integer map coordinates of current beam endpoint
            Eigen::Vector2i scanEndMapi(scanEndMapf.cast<int>());

            //Update map using a bresenham variant for drawing a line from beam start to beam endpoint in map coordinates
            if (scanBeginMapi != scanEndMapi)
            {
                updateLineBresenhami(scanBeginMapi, scanEndMapi); /// Bresenhami 算法计算两点连线经过的所有格子
            }
        }

        //Tell the map that it has been updated
        this->setUpdated();/// 记录更新序号

        //Increase update index (used for updating grid cells only once per incoming scan)
        currUpdateIndex += 3; // currUpdateIndex 被吃掉了呗
    }

3.3 updateLineBresenhami()

    inline void updateLineBresenhami( const Eigen::Vector2i& beginMap, const Eigen::Vector2i& endMap, unsigned int max_length = UINT_MAX){

        int x0 = beginMap[0];
        int y0 = beginMap[1];

        //check if beam start point is inside map, cancel update if this is not the case
        if ((x0 < 0) || (x0 >= this->getSizeX()) || (y0 < 0) || (y0 >= this->getSizeY())) {
            return;
        }

        int x1 = endMap[0];
        int y1 = endMap[1];

        //std::cout << " x: "<< x1 << " y: " << y1 << " length: " << length << "     ";

        //check if beam end point is inside map, cancel update if this is not the case
        if ((x1 < 0) || (x1 >= this->getSizeX()) || (y1 < 0) || (y1 >= this->getSizeY())) {
            return;
        }

        int dx = x1 - x0;
        int dy = y1 - y0;

        unsigned int abs_dx = abs(dx);
        unsigned int abs_dy = abs(dy);

        int offset_dx = util::sign(dx);
        int offset_dy = util::sign(dy) * this->sizeX;

        unsigned int startOffset = beginMap.y() * this->sizeX + beginMap.x();

        //if x is dominant
        if(abs_dx >= abs_dy){ /// 激光束穿过的点更新一次Free状态
            int error_y = abs_dx / 2;
            bresenham2D(abs_dx, abs_dy, error_y, offset_dx, offset_dy, startOffset);
        }else{
            //otherwise y is dominant
            int error_x = abs_dy / 2;
            bresenham2D(abs_dy, abs_dx, error_x, offset_dy, offset_dx, startOffset);
        }

        unsigned int endOffset = endMap.y() * this->sizeX + endMap.x();
        this->bresenhamCellOcc(endOffset);/// 激光束端点更新一次Occupied状态

    }

3.4 bresenham2D()

    inline void bresenham2D( unsigned int abs_da, unsigned int abs_db, int error_b, int offset_a, int offset_b, unsigned int offset){

        this->bresenhamCellFree(offset);

        unsigned int end = abs_da-1;

        for(unsigned int i = 0; i < end; ++i){
            offset += offset_a;
            error_b += abs_db;

            if((unsigned int)error_b >= abs_da){
                offset += offset_b;
                error_b -= abs_da;
            }
            /// 激光束穿过的网格被更新一次free状态
            this->bresenhamCellFree(offset);
        }
    }

3.5 bresenhamCellFree()

    inline void bresenhamCellFree(unsigned int offset)
    {
        ConcreteCellType& cell (this->getCell(offset));

        /// 单次scan中,每个网格只允许更新一次free状态（可能有多条激光束穿过同一个网格）
        if (cell.updateIndex < currMarkFreeIndex) {
            concreteGridFunctions.updateSetFree(cell);
            cell.updateIndex = currMarkFreeIndex;
        }
    }

3.6 bresenhamCellOcc()

    inline void bresenhamCellOcc(unsigned int offset)
    {
        ConcreteCellType& cell (this->getCell(offset));
        /// 单次scan中，每个网格只允许更新一次occupied状态
        if (cell.updateIndex < currMarkOccIndex) {

            //if this cell has been updated as free in the current iteration, revert this
            /// 如果击中的网格在当前scan中曾被更新为free状态，那么撤销次free的更新，以occupied状态为准。
            if (cell.updateIndex == currMarkFreeIndex) {
                concreteGridFunctions.updateUnsetFree(cell);
            }

            concreteGridFunctions.updateSetOccupied(cell);
            //std::cout << " setOcc " << "\n";
            cell.updateIndex = currMarkOccIndex;
        }
    }