高仙清洁机器人墙线检测与极致贴边-原理与代码实现

视频讲解

​​​​​​​墙线提取，线段提取与分割合并、圆弧检测、内外直角检测 · 语雀

背景，实现目的

用于改善长直墙近距离沿墙效果；

实现原理

step: 每间隔一定位移检测右侧直线段并生成segment历史序列，检测segment检测的连续性和稳定性；（lidar数据中检测直线段稳定出现条件严苛，避免误检测；）

step: 检测实时的右侧linelaser数据，判断是否为竖直线；

定义一个概念：稳定沿竖直墙；

触发稳定沿竖直墙的条件：

lidar检测出的水平segment稳定出现；（判断条件严苛，连续出现，平行，重叠）

机器位姿与稳定的segment近似平行；

机器位姿在segment左侧一定距离内；

右侧linelaser数据判断为竖直线；

当检测出稳定沿竖直墙时，屏蔽lidar带噪声实时数据计算出的wall_dis，防止抖动或者wall_dis太近触发的emergency；

参考

📎一种墙线检测方法、装置、电子设备、存储介质及机器人.pdf

📎一种障碍物标记方法、装置及可读非暂时性存储介质.pdf

使用历史数据的目的或者原因：

lidar近距离测得的点云会有严重的拖尾/类似拖尾问题（特别是针对tof雷达）（近距离时候两个激光脉冲的信号峰重叠）;

效果

📎20231204_145511.mp4

📎20231204_171310.mp4

上图中，当wall_line_check为1111时表示四个条件同时满足，触发 稳定沿竖直墙 状态；

segment size 表示缓存的检测到的线段的队列；最多八条，队列满了之后删除最初的数据；

vertical line 表示当前右侧线激光检测到的障碍物点是否为竖直线；（右侧线激光打出去的平面是一个竖直面，对于同一面直墙，线激光检测到的竖直线在地图中的投影为一个点而且落在lidar检测到的墙线上）

效果：

能够在沿长直墙时实现零贴边无抖动；

code附录

墙线检测

#pragma once
#include "local_zmap_mixed.h"
#include "pose2d.h"
#include "utils/singleton.h"
#include "zmq_simple/visualization_debug.h" // 用于发送地图和其他重要的调试信息到上位机
#include <list>
#include <mutex>

struct Point3f;
class WallLineDetector
{
    // 检测到的lidar线段
    struct LidarDataInfo {
        uint64_t TimeStampMs; // 时间戳
        Pose2D CarPose;       // 机器位姿
        // std::vector<std::pair<float, float>> Points; // 原始lidar点
        // bool ExistLineSegment;                       // 是否存在直线段
        std::pair<Pose2D, Pose2D> LineSegment; // 检测到的直线段,只存一段
        LidarDataInfo()
        {
            TimeStampMs = 0;
            // ExistLineSegment = false;
        }
    };
    // 检测右侧linelaser是否为竖直线
    struct LineLaserInfo {
        uint64_t TimeStampMs;               // 时间戳
        bool IsLineLaserRight_VerticalLine; // 右侧是否为竖直线
        LineLaserInfo()
        {
            TimeStampMs = 0;
        }
    };
    struct CheckResult {
        // 同时满足以下条件认为是在稳定的沿直墙
        bool steadySegment;     // 判断 segment 是否稳定
        bool carSegmentAngleOk; // 判断机器与最近的segment夹角是否合理
        bool carSegmentPosOk;   // 机器在segment起点坐标系下的位姿是否合理
        bool carVerticalOk;     // 竖直检测结果是否合理
        CheckResult()
        {
            steadySegment     = false;
            carSegmentAngleOk = false;
            carSegmentPosOk   = false;
            carVerticalOk     = false;
        }
        void Reset()
        {
            steadySegment     = false;
            carSegmentAngleOk = false;
            carSegmentPosOk   = false;
            carVerticalOk     = false;
        }
        // 判断两个状态是否没区别
        bool operator==(CheckResult &other)
        {
            return (steadySegment == other.steadySegment && carSegmentAngleOk == other.carSegmentAngleOk &&
                    carSegmentPosOk == other.carSegmentPosOk && carVerticalOk == other.carVerticalOk);
        }
        bool IsAllOk()
        {
            return (steadySegment == true && carSegmentAngleOk == true &&
                    carSegmentPosOk == true && carVerticalOk == true);
        }
    };

public:
    WallLineDetector() {}
    ~WallLineDetector() {}
    void Init()
    {
        mLidarInfoList.clear();
        mLineLaserInfoList.clear();
        mIsEnabled       = true;
        mLastCheckTickMs = GetCurrentTickMs();
    }
    // 失能
    void Disable()
    {
        mIsEnabled = false;
        mLidarInfoList.clear();
        mLineLaserInfoList.clear();
    }
    // 获取结果，右侧是否有墙面
    bool IsWallLineRightSide(Pose2D carPose);

    // 添加数据
    void AddLidarData(ObservationScan2D &obs, Pose2D currentPose);
    void AddLineLaserData(std::vector<Point3f> &points);

    // 可视化检测结果分量
    void VisualizeCheckResult(Pose2D carPose);
private:
    // 可视化中间结果，segment和竖直方向结果；只是在segment有变动的时候更新
    void VisualizeSegments();
    // 可视化中间结果，竖直检测结果
    void VisualizeLinelaserResult(Pose2D carPose);
    CheckResult mLastaCheckResult; // 上一次的检测结果
    CheckResult CurCheckResult;    // 当前的检测结果

private:
    // 记录历史数据, note 注意数据丢失的情况
    // lidar
    const float k_range_near_thresh        = 0.6f; // 需要检测的距离范围
    const float k_range_far_thresh         = 1.2f;
    const uint64_t k_lidar_detect_interval = 1000;  // [ms]相邻两次检测lidar数据的时间间隔
    const float k_dis_thresh               = 0.1f; // [ms]相邻两次检测lidar数据的距离间隔
    const size_t k_lidar_info_size_max     = 8;     // lidar历史数据序列长度最大值
    const float k_collinear_angle_thresh                  = DEG2RAD(30); // 判断机器与segment平行的角度阈值
    const float k_car_segment_othogonal_dis_thresh        = 0.45f;       // [m]投影落在线段上，且距离线段不超过0.45f
    const float k_segment_collinear_angle_thresh          = DEG2RAD(15); // 判断两条segment平行的角度阈值
    const float k_segment_collinear_orthogonal_dis_thresh = 0.15f;       // 判断两条segment平行的平行距离阈值
    std::list<WallLineDetector::LidarDataInfo> mLidarInfoList;           // lidar历史数据
    // linelaser
    const size_t k_linelaser_info_size_max = 3;                    // lidar历史数据序列长度最大值
    std::list<WallLineDetector::LineLaserInfo> mLineLaserInfoList; // lineLaser历史数据
    // 使能失能
    bool mIsEnabled           = false;
    uint64_t mLastCheckTickMs = 0; // 上一次查询的时间戳
    Pose2D mCheckPoseNoUse;        // 查询时机器的位姿
    // multiThread issue
    std::mutex mDataMutex; // 多线程调用加锁
};

// 单例，墙面检测
#define WALL_LINE_DETECTOR Singleton<WallLineDetector>::Instance()

#include "wall_line_detector.hpp"
#include "leg_detector/obstacle_extractor.h"
#include "local_zmap_linelaser.h"
#include <algorithm>
#include <vector>
// 获取结果，右侧是否有墙面
// 判断方法：右侧最近有长直线；右侧最近有竖直线；
bool WallLineDetector::IsWallLineRightSide(Pose2D carPose)
{
    mCheckPoseNoUse = carPose;
    // 多线程调用加锁
    std::lock_guard<std::mutex> lg(mDataMutex);
    //
    if (mIsEnabled == false) {
        // 忘了init容错
        mIsEnabled       = true;
        mLastCheckTickMs = GetCurrentTickMs();
        return false;
    }
    // 超过500ms没有调用，数据全部重置
    if (GetCurrentTickMs() - mLastCheckTickMs > 500) {
        Init();
        return false;
    }
    mLastCheckTickMs = GetCurrentTickMs();
    //
    CurCheckResult.Reset();
    // 判断 右侧最近有长直线，使用lidar数据
    bool is_exist_horizon_line = false;
    {
        if (mLidarInfoList.size() < 3) {
            return false; // 至少三次记录
        }
        std::vector<WallLineDetector::LidarDataInfo> mLidarInfoListCopy;
        mLidarInfoListCopy.assign(mLidarInfoList.begin(), mLidarInfoList.end());
        std::reverse(mLidarInfoListCopy.begin(), mLidarInfoListCopy.end()); // 把时间上最新的放到前面

        // func:Segment起点建立坐标系
        auto func_get_segment_refPose = [](std::pair<Pose2D, Pose2D> posePair) -> Pose2D {
            Pose2D start = posePair.first;
            Pose2D end   = posePair.second;
            float phi    = WrapToPi(atan2(end.Y - start.Y, end.X - start.X));
            start.SetPhi(phi);
            return start;
        };
        // 判断一个segment与前后记录的segment是否重合平行
        auto func_check_segment_steady_collinear_back_forth = [&](size_t i) -> bool {
            if (i <= 0 || i + 1 >= mLidarInfoListCopy.size() || mLidarInfoListCopy.size() <= 3) {
                return false; // 必须是中间的
            }
            WallLineDetector::LidarDataInfo curSegment_forth = mLidarInfoListCopy[i - 1]; // 时间戳更新的一个
            WallLineDetector::LidarDataInfo curSegment       = mLidarInfoListCopy[i];
            WallLineDetector::LidarDataInfo curSegment_back  = mLidarInfoListCopy[i + 1]; // 时间戳更旧的一个
            // curSegment起点建立坐标系
            Pose2D refPose = func_get_segment_refPose(curSegment.LineSegment);
            //
            Pose2D refPose_forth = func_get_segment_refPose(curSegment_forth.LineSegment);
            Pose2D refPose_back  = func_get_segment_refPose(curSegment_back.LineSegment);
            // 判断方向平行， 前后都平行说明至少没有抖动
            if (fabs(WrapToPi(refPose_forth.Phi() - refPose.Phi())) > k_segment_collinear_angle_thresh) {
                return false;
            }
            if (fabs(WrapToPi(refPose_back.Phi() - refPose.Phi())) > k_segment_collinear_angle_thresh) {
                return false;
            }
            // 判断平行距离
            if (fabs((refPose_forth - refPose).Y) > k_segment_collinear_orthogonal_dis_thresh ||
                fabs((refPose - refPose_back).Y) > k_segment_collinear_orthogonal_dis_thresh) {
                return false;
            }
            return true; // 稳定的segment
        };
        // 判断是否右侧有lidar长直线段；判断标准：距离,平行度collinear，多次出现segment,
        // carPose 是否在第i个稳定的segment左侧 平行
        auto func_check_car_leftside_steady_segment = [&](size_t i) -> bool {
            CurCheckResult.steadySegment     = false;
            CurCheckResult.carSegmentAngleOk = false;
            CurCheckResult.carSegmentPosOk   = false;
            //
            if (i <= 0 || i + 1 >= mLidarInfoListCopy.size() || mLidarInfoListCopy.size() <= 3) {
                return false; // 必须是中间的
            }
            // 判断segment是否稳定 steadySegment
            if (func_check_segment_steady_collinear_back_forth(i) == false) {
                return false; // segment不稳定直接返回false
            }
            CurCheckResult.steadySegment = true;
            // 可视化
            // carPose 是否在第i个segment左侧 平行
            {
                Pose2D refPose = func_get_segment_refPose(mLidarInfoListCopy[i].LineSegment);
                Pose2D end     = mLidarInfoListCopy[i].LineSegment.second;
                // 判断夹角 carSegmentAngleOk
                if (fabs(WrapToPi(carPose.Phi() - refPose.Phi())) > k_collinear_angle_thresh) {
                    return false;
                }
                CurCheckResult.carSegmentAngleOk = true;
                // 判断机器投影到refPose上的坐标
                Pose2D car_to_segment = carPose - refPose; // 机器在segment起点坐标系下的位姿
                // 投影落在线段上，且距离线段不超过0.45f
                if (car_to_segment.X > 0.f && car_to_segment.X < refPose.GetDistance(end) &&
                    car_to_segment.Y > 0 && car_to_segment.Y < k_car_segment_othogonal_dis_thresh) {
                    // 可视化 水平直线段检测结果
                    CurCheckResult.carSegmentPosOk = true;
                    return true; // 在refPose左侧有效范围内
                }
            }
            return false;
        };
        //  从最近一次检测的segment开始判断，机器与一条稳定的segment平行就返回true
        for (size_t i = 1; i < mLidarInfoListCopy.size() - 1; i++) {
            if (func_check_car_leftside_steady_segment(i) == true) {
                // 有一条合适就认为是在lidar直墙边
                is_exist_horizon_line = true;
                break;
            }
        }
    } // end // 判断 右侧最近有长直线，使用lidar数据
    if (is_exist_horizon_line == false) {
        // 可视化 水平直线段检测结果
        return false; // 不是与稳定的segment平行
    }
    // mLineLaserInfoList 记录数目不够 返回false
    if (mLineLaserInfoList.size() < k_linelaser_info_size_max) {
        return false;
    }
    // mLineLaserInfoList 里面三次有一次是竖直的就认为有linelaser竖直墙
    bool is_exist_vertical_line = false;
    {
        for (auto &item : mLineLaserInfoList) {
            if (item.IsLineLaserRight_VerticalLine == true && GetCurrentTickMs() - item.TimeStampMs < 800) {
                // 可视化 竖直检测结果
                is_exist_vertical_line = true;
                CurCheckResult.carVerticalOk = true;
                break;
            }
        }
    }
    // 可视化 竖直检测结果
    return is_exist_vertical_line;
}

/// @brief 更新当前的lidar数据，时间间隔或者距离间隔满足条件均可添加数据；检测直线段并记录，设置标志位；
/// @param obs lidar帧
/// @param currentPose 当前机器位姿
// todo/bug: 长时间没有segment加入需要删除过时数据，超时时长需要根据沿边速度计算；
void WallLineDetector::AddLidarData(ObservationScan2D &obs, Pose2D currentPose)
{
    // 多线程调用加锁
    std::lock_guard<std::mutex> lg(mDataMutex);
    //
    // 防止没有失能导致资源消耗
    {
        if (mIsEnabled == false) {
            return;
        }
        if (GetCurrentTickMs() - mLastCheckTickMs > 500) {
            mIsEnabled = false;
            return;
        }
    }
    // 判断是否需要添加
    do {
        if (mLidarInfoList.size() == 0) {
            break; // 时间间隔满足要求
        }
        {
            auto interval = obs.GetTimeStamp() / 1000 - mLidarInfoList.back().TimeStampMs;
            if (interval > k_lidar_detect_interval) {
                break; // 时间间隔满足要求， 这时应该是机器没有动或者长时间没有直线，因为k_dis_thresh比较小，会很快更新
            }
            if (currentPose.GetDistance(mLidarInfoList.back().CarPose) > k_dis_thresh) {
                break; // 距离间隔满足要求
            }
        }
        return; // 不添加
    } while (0);
    // // 需要检测的距离范围
    // const float k_range_near_thresh = 0.6f;
    // const float k_range_far_thresh  = 1.2f;
    // 只取机器右前方区域一定距离范围内的点；
    ObservationScan2D obs_filtered;
    int valid_count = 0;
    {
        const float *scan_data = obs.GetScanData();
        const bool *scan_valid = obs.GetValidArray();
        for (size_t i = 270; i < 360; i++) {
            // 只考虑 k_range_raidus_use 距离范围内的点 && scanDataCopy[i] < k_range_raidus_use
            if (scan_valid[i] && scan_data[i] > k_range_near_thresh && scan_data[i] < k_range_far_thresh) {
                obs_filtered.SetScanData(i, scan_data[i]);
                obs_filtered.SetValidArray(i, true);
                valid_count++;
            }
        }
        for (size_t i = 0; i < 30; i++) {
            // 只考虑 k_range_raidus_use 距离范围内的点 && scanDataCopy[i] < k_range_raidus_use
            if (scan_valid[i] && scan_data[i] > k_range_near_thresh && scan_data[i] < k_range_far_thresh) {
                obs_filtered.SetScanData(i, scan_data[i]);
                obs_filtered.SetValidArray(i, true);
                valid_count++;
            }
        }
        if (valid_count < 8) {
            mLidarInfoList.clear(); // 右前方没有lidar点，直接清空
            return;                 // lidar点太少, 无法检测，直接清空
        }
    }
    // 检测直线
    {
        leg_detector::ObstacleExtractor SegExtractor;
        SegExtractor.setMode(leg_detector::ObstacleExtractor::GET_LINE_SEG); // 设置为检测直线分割合并
        SegExtractor.ProcessLidarData(currentPose, obs_filtered);
        std::list<obstacle_detector::Segment> segments = SegExtractor.GetSegments(); // 提取出的线段
        // SegExtractor.GetInputPoints().size();
        // 判断并计算单条直线段
        {
            // 找到长度最长的segment
            // obstacle_detector::Segment max_segment = *(std::max_element(segments.begin(), segments.end(),
            //   [](std::list<obstacle_detector::Segment>::iterator one, std::list<obstacle_detector::Segment>::iterator other) -> bool { return (*one).length() < (*other).length(); }));
            obstacle_detector::Segment max_segment = *(std::max_element(segments.begin(), segments.end(),
                                                                        [](obstacle_detector::Segment &one, obstacle_detector::Segment &other) -> bool { return (one).length() < (other).length(); }));
            if (max_segment.length() < 0.3f) {
                return; // 最长线段太短了
            }
            // 判断是否大多数点都在直线上
            std::list<obstacle_detector::Point> points = SegExtractor.GetInputPoints();
            // points.size();
            // 记录离直线太远的点的个数；
            int far_from_segment_point_count = 0;
            for (auto &item : points) {
                double dis = max_segment.distanceTo(item);
                if (dis > 0.04) {
                    far_from_segment_point_count++;
                    // 离直线太远的点超过三个认为不是长直墙
                    if (far_from_segment_point_count > 3) {
                        return;
                    }
                }
            }
            // 记录单直线
            LidarDataInfo segmentInfo;
            {
                // segmentInfo.ExistLineSegment == true;
                segmentInfo.TimeStampMs = obs.GetTimeStamp() / 1000;
                segmentInfo.CarPose     = currentPose;
                // 线段的全局坐标,让线段朝向和机器一致
                if (max_segment.first_point.x < max_segment.last_point.x) {
                    segmentInfo.LineSegment = std::make_pair(currentPose + Pose2D(max_segment.first_point.x, max_segment.first_point.y, 0),
                                                             currentPose + Pose2D(max_segment.last_point.x, max_segment.last_point.y, 0));
                } else {
                    segmentInfo.LineSegment = std::make_pair(currentPose + Pose2D(max_segment.last_point.x, max_segment.last_point.y, 0),
                                                             currentPose + Pose2D(max_segment.first_point.x, max_segment.first_point.y, 0));
                }
                //
                mLidarInfoList.push_back(segmentInfo);
                if (mLidarInfoList.size() > k_lidar_info_size_max) {
                    mLidarInfoList.pop_front();
                }
                // 可视化
                VisualizeSegments();
            }
        } // end 判断是否是单直线
    }
}

/// @brief 更新当前的lineLaser数据(已经过滤高度和滤波之后的)，记录右侧沿墙数据是否是竖直线；每一帧都做检测
/// @param  points 右侧的linelaser数据
/// @param currentPose 当前机器位姿
void WallLineDetector::AddLineLaserData(std::vector<Point3f> &points)
{
    // 多线程调用加锁
    std::lock_guard<std::mutex> lg(mDataMutex);
    //
    // 防止没有失能导致资源消耗
    {
        if (mIsEnabled == false) {
            return;
        }
        if (GetCurrentTickMs() - mLastCheckTickMs > 500) {
            mIsEnabled = false;
            return;
        }
    }
    // 判断是否为竖直线，在yz平面内判断
    //
    // const float k_wash_board_heigth = 0.05f;
    // 百分之八十的点在半径2cm范围内认为是竖直墙
    // 判断y坐标，是否所有点都在2.5cm范围内
    auto func_is_vertical = [&]() -> bool {
        std::vector<float> y_list;
        for (Point3f &item : points) {
            if (item.z < 0.04f) {
                continue;
            }
            y_list.push_back(item.y);
        }
        // 是否所有点都在2.5cm范围内
        if (y_list.size() > 20 && *(std::max_element(y_list.begin(), y_list.end())) - *(std::min_element(y_list.begin(), y_list.end())) < 0.025f) {
            return true;
        }
        // 有噪点的情形
        // other 其他情况也认为是竖直墙,note : 对踢脚线墙面特殊处理
        return false;
    };
    LineLaserInfo linelaserInfo;
    linelaserInfo.IsLineLaserRight_VerticalLine = func_is_vertical();
    linelaserInfo.TimeStampMs                   = GetCurrentTickMs();
    mLineLaserInfoList.push_back(linelaserInfo);
    //
    if (mLineLaserInfoList.size() > k_linelaser_info_size_max) {
        mLineLaserInfoList.pop_front();
    }
    // 可视化
    VisualizeLinelaserResult(mCheckPoseNoUse);
}

void WallLineDetector::VisualizeSegments()
{
    if (mLidarInfoList.empty()) {
        return;
    }
    // mLidarInfoList
    std::vector<WallLineDetector::LidarDataInfo> infolist;
    infolist.assign(mLidarInfoList.begin(), mLidarInfoList.end());
    //
    graphics::G_LineSegments_Group lines;
    // 计算每条射线的起点终点
    for (size_t i = 0; i < infolist.size(); i++) {
        graphics::G_Linesegment_Base segment;
        Pose2D start    = infolist[i].LineSegment.first;
        Pose2D end      = infolist[i].LineSegment.second;
        segment.start.x = start.X;
        segment.start.y = start.Y;
        segment.end.x   = end.X;
        segment.end.y   = end.Y;
        lines.lineSegments.push_back(segment);
    }
    // 上传
    lines.width = 3;
    lines.color = graphics::COLOR_DARKGREY;
    // VisualizationUtil::Instance()->SendCarPose(car_pose);
    VisualizationUtil::Instance()->SendGraphicItem("mLidarInfoList", lines.GetBin());
    {
        graphics::G_Text text;
        text.origin.x = infolist[0].LineSegment.first.X + 0.4f;
        text.origin.y = infolist[0].LineSegment.first.Y + 0.4f;
        text.text     = std::string("segment size:") + std::to_string(mLidarInfoList.size());
        text.width    = 2;
        text.color    = graphics::COLOR_BLUE;
        VisualizationUtil::Instance()->SendGraphicItem("mLidarInfoList_size", text.GetBin());
    }
}
// 可视化中间结果，竖直检测结果
void WallLineDetector::VisualizeLinelaserResult(Pose2D carPose)
{
    // mLineLaserInfoList
    static bool lastResult      = false;
    bool is_exist_vertical_line = false;
    {
        for (auto &item : mLineLaserInfoList) {
            if (item.IsLineLaserRight_VerticalLine == true && GetCurrentTickMs() - item.TimeStampMs < 800) {
                // 可视化 竖直检测结果
                is_exist_vertical_line = true;
                break;
            }
        }
    }
    // visualize
    auto func_show = [&]() -> void {
        graphics::G_Text text;
        text.origin.x = carPose.X + 0.4f;
        text.origin.y = carPose.Y - 0.4f;
        text.text     = std::string("vertical_line:") + std::to_string((int)is_exist_vertical_line);
        text.width    = 2;
        text.color    = is_exist_vertical_line ? graphics::COLOR_BLUE : graphics::COLOR_RED;
        VisualizationUtil::Instance()->SendGraphicItem("vertical_line", text.GetBin());
    };
    if (is_exist_vertical_line == lastResult) {
        static uint64_t lastTick = GetCurrentTickMs();
        if (GetCurrentTickMs() - lastTick < 200) {
            return;
        } else {
            lastTick = GetCurrentTickMs(); // 检测结果无改变间隔时间上传
            func_show();
        }
    }
    func_show(); // 检测结果有改变一定上传
    // 更新
    lastResult = is_exist_vertical_line;
}
// 可视化检测结果分量
// todo: 将 线激光检测到的竖直线和lidar的segment做一个double check 
void WallLineDetector::VisualizeCheckResult(Pose2D carPose)
{
    // 判断两个状态是否没区别，有区别才上传
    if (CurCheckResult == mLastaCheckResult) {
        return;
    }
    // 可视化 ， 使用文字的形式
    std::string res = std::to_string(int(CurCheckResult.steadySegment)) + std::to_string(int(CurCheckResult.carSegmentAngleOk)) +
                      std::to_string(int(CurCheckResult.carSegmentPosOk)) + std::to_string(int(CurCheckResult.carVerticalOk));
    {
        graphics::G_Text text;
        text.origin.x = carPose.X;
        text.origin.y = carPose.Y;
        text.text     = std::string("wall_line_check:") + res;
        text.width    = 3;
        text.color    = CurCheckResult.IsAllOk() ? graphics::COLOR_BLUE : graphics::COLOR_RED;
        VisualizationUtil::Instance()->SendGraphicItem("wall_line_check", text.GetBin());
    }
    // 更新
    mLastaCheckResult = CurCheckResult;
}

lidar点线段提取，使用iepf和split and merge（适用于周向有序排列的点提取和合并线段）

算法的其他用途：检测环境退化（两条平行线段）与非退化（存在成角度的多条长直线段，用于scan to map匹配获得准确的定位位姿，可以用于退化环境中消除odo累计误差）特征

另外一个应用：检测回环时候增加权重，增大信息矩阵，在检测到很强的非退化回环匹配的时候增加残差计算的权重）

另一个应用：在粒子滤波中，通过检测直线计算退化方向，据此决定重采样粒子分布；本质上，粒子总误差的协方差矩阵的主方向（最大特征值的特征向量）与退化方向平行；此时融合wheel odo位姿的时候相信odo在主方向上的投影分量；

void ObstacleExtractor::detectSegments(const PointSet &point_set)
{
    std::cout << "detect segment now ,num points is: " << point_set.num_points << endl;
    if (point_set.num_points < p_min_group_points_)
        return;

    Segment segment(*point_set.begin, *point_set.end); // Use Iterative End Point Fit

    // if (p_use_split_and_merge_)
    //     segment = fitSegment(point_set);

    PointIterator set_divider;
    double max_distance = 0.0;
    double distance     = 0.0;

    int split_index = 0; // Natural index of splitting point (counting from 1)
    int point_index = 0; // Natural index of current point in the set

    // Seek the point of division
    // 离首尾直线段最远的作为分段点
    for (PointIterator point = point_set.begin; point != point_set.end; ++point) {
        ++point_index;

        if ((distance = segment.distanceTo(*point)) >= max_distance) {
            // double r = (*point).length();

            // todo:这里要对照论文调整，这里的值给的小一些提取的线段会更多；
            // if (distance > p_max_split_distance_ + r * p_distance_proportion_) {
            if (distance > 0.05) {
                max_distance = distance;
                set_divider  = point;
                split_index  = point_index;
            }
        }
    }

    // Split the set only if the sub-groups are not 'small'
    // 被分割的点集中点的个数不能少于3个 todo:可以调整
    if (max_distance > 0.0 && split_index > p_min_group_points_ && split_index < point_set.num_points - p_min_group_points_) {
        set_divider = input_points_.insert(set_divider, *set_divider); // Clone the dividing point for each group

        PointSet subset1, subset2;
        subset1.begin      = point_set.begin;
        subset1.end        = set_divider;
        subset1.num_points = split_index;
        subset1.is_visible = point_set.is_visible;

        subset2.begin      = ++set_divider;
        subset2.end        = point_set.end;
        subset2.num_points = point_set.num_points - split_index;
        subset2.is_visible = point_set.is_visible;

        detectSegments(subset1);
        detectSegments(subset2);
    } else { // Add the segment
        if (!p_use_split_and_merge_) {
            segment = fitSegment(point_set); // 使用最小二乘拟合直线
        }
        segments_.push_back(segment);
    }
}
/**
 * @brief 合并直线段，根据直线段间的法向距离或者真实距离
 * 
 */
void ObstacleExtractor::mergeSegments()
{
    for (auto i = segments_.begin(); i != segments_.end(); ++i) {
        for (auto j = i; j != segments_.end(); ++j) {
            Segment merged_segment;

            if (compareSegments(*i, *j, merged_segment)) {
                auto temp_itr = segments_.insert(i, merged_segment);
                segments_.erase(i);
                segments_.erase(j);
                i = --temp_itr; // Check the new segment against others
                break;
            }
        }
    }
}
bool ObstacleExtractor::compareSegments(const Segment &s1, const Segment &s2, Segment &merged_segment)
{
    if (&s1 == &s2)
        return false;

    // Segments must be provided counter-clockwise
    if (s1.first_point.cross(s2.first_point) < 0.0)
        return compareSegments(s2, s1, merged_segment);

    if (checkSegmentsProximity(s1, s2)) {
        vector<PointSet> point_sets;
        point_sets.insert(point_sets.end(), s1.point_sets.begin(), s1.point_sets.end());
        point_sets.insert(point_sets.end(), s2.point_sets.begin(), s2.point_sets.end());

        Segment segment = fitSegment2(point_sets);

        if (checkSegmentsCollinearity(segment, s1, s2)) {
            merged_segment = segment;
            return true;
        }
    }

    return false;
}

bool ObstacleExtractor::checkSegmentsProximity(const Segment &s1, const Segment &s2)
{
    //   return (s1.trueDistanceTo(s2.first_point) < p_max_merge_separation_ ||
    //           s1.trueDistanceTo(s2.last_point)  < p_max_merge_separation_ ||
    //           s2.trueDistanceTo(s1.first_point) < p_max_merge_separation_ ||
    //           s2.trueDistanceTo(s1.last_point)  < p_max_merge_separation_);
    // todo: 这里的相邻点距离阈值要调整
    return (s1.trueDistanceTo(s2.first_point) < 0.05 ||
            s1.trueDistanceTo(s2.last_point) < 0.05 ||
            s2.trueDistanceTo(s1.first_point) < 0.05 ||
            s2.trueDistanceTo(s1.last_point) < 0.05);
}

bool ObstacleExtractor::checkSegmentsCollinearity(const Segment &segment, const Segment &s1, const Segment &s2)
{
    return (segment.distanceTo(s1.first_point) < 0.05 &&
            segment.distanceTo(s1.last_point) < 0.05 &&
            segment.distanceTo(s2.first_point) < 0.05 &&
            segment.distanceTo(s2.last_point) < 0.05);
}