概率栅格地图更新过程

一、前言

“在宇宙中，曲率驱动航迹既可以成为危险标志，也能成为安全声明。 如果航迹在一个世界旁边，是前者；如果把这个世界包裹在其中，则是后者。就像一个手拿绞索的人，他是危险的；但如果他把绞索套到自己的脖子上，他就变成安全的了。”
同一事物在不同情境下可能具有截然相反的属性，这是一种既对立又统一的哲学，万事万物都不可能是独立存在，放眼整个宇宙也是在这种矛盾中周而复始！

二、概率栅格地图的概念

概率栅格地图是一种用于表示环境的二维或三维网格地图，每个栅格单元表示该位置被占用的概率。概率栅格地图广泛应用于机器人导航、自动驾驶和SLAM等领域。

三、概率栅格地图的推导

四、实例

五、简单代码实现

#include <vector>
#include <cmath>
#include <iostream>
#include <algorithm>
#include <math.h>
#include <cstring>
#include "opencv2/highgui.hpp"
#include "opencv2/imgproc.hpp"
using namespace std;
using namespace cv;

// 概率栅格单元定义
struct GridCell
{
    GridCell() : log_odds(0.0), occ_pro(0.5)
    {
    }

    void updateOdds(float odds)
    {
        log_odds = odds;
        occ_pro = 1.0f / (1.0f + std::exp(-log_odds));
    }

    float getLogOdds()
    {
        return log_odds;
    }

    float getOccPro()
    {
        return occ_pro;
    }

private:
    float log_odds{0.0}; // 对数几率值
    float occ_pro{0.5};  // 障碍物概率
};

// 概率栅格地图类
class ProbabilityGrid
{
public:
    // 初始化地图参数
    ProbabilityGrid(int width, int height, float resolution)
        : width_(width), height_(height), resolution_(resolution)
    {
        cells_ = new GridCell[width * height];
    }

    // 更新激光扫描数据
    void updateLaserScan(const cv::Point &sensor_origin,
                         const std::vector<cv::Point> &hits);
    // 查询栅格概率
    float getProbability(int x, int y) const;
    // 转换成栅格地图
    void transformCvMap();

private:
    // Bresenham直线算法
    std::vector<cv::Point> bresenhamLine(cv::Point start,
                                         cv::Point end);
    // 栅格值有效判断
    bool isValidGrid(int mx, int my) const;
    // 获取index
    inline int getIndex(int mx, int my) const;
    // 更新单个栅格的log odds
    void updateCell(int x, int y, float delta);

    // 常量参数
    static constexpr float kHitOdds = 0.7f;     // 对应log(0.7/(1-0.7)) ≈ 0.85
    static constexpr float kMissOdds = -0.4f;   // 对应log(0.3/(1-0.3)) ≈ -0.85
    static constexpr float kMaxLogOdds = 20.0f; // 对数几率最大绝对值

    // 地图参数
    int width_, height_; // 栅格数量
    float resolution_;   // 米/栅格
    GridCell *cells_;    // 栅格数据
};

// --------------------------------------------------------------------------

// 更新激光扫描数据
void ProbabilityGrid::updateLaserScan(const cv::Point &sensor_origin,
                                      const std::vector<cv::Point> &hits)
{
    for (const auto &hit : hits)
    {
        // 遍历光束路径上的所有栅格
        auto ray = bresenhamLine(sensor_origin, hit);

        // 更新Miss区域（路径上的栅格，排除终点）
        for (size_t i = 0; i < ray.size() - 1; ++i)
        {
            updateCell(ray[i].x, ray[i].y, kMissOdds);
        }

        // 更新Hit区域（终点）
        updateCell(hit.x, hit.y, kHitOdds);
    }
}

float ProbabilityGrid::getProbability(int x, int y) const
{
    if (isValidGrid(x, y))
    {
        return cells_[getIndex(x, y)].getOccPro();
    }
    else
    {
        return 0;
    }
}

inline int ProbabilityGrid::getIndex(int mx, int my) const { return my * width_ + mx; }

bool ProbabilityGrid::isValidGrid(int mx, int my) const
{
    return (mx < width_ && mx >= 0 && my < height_ && my >= 0);
}

std::vector<cv::Point> ProbabilityGrid::bresenhamLine(cv::Point start,
                                                      cv::Point end)
{
    std::vector<cv::Point> points;

    int x0 = start.x;
    int y0 = start.y;
    int x1 = end.x;
    int y1 = end.y;

    // 处理起点和终点重合的情况
    if (x0 == x1 && y0 == y1)
    {
        points.emplace_back(x0, y0);
        return points;
    }

    // 计算差分和步长
    int dx = std::abs(x1 - x0);
    int dy = -std::abs(y1 - y0);
    int sx = (x0 < x1) ? 1 : -1;
    int sy = (y0 < y1) ? 1 : -1;

    int err = dx + dy; // 误差项

    while (true)
    {
        points.emplace_back(x0, y0);

        // 到达终点时终止
        if (x0 == x1 && y0 == y1)
            break;

        int e2 = 2 * err;

        // 误差累积判断
        if (e2 >= dy)
        { // 水平步进
            if (x0 == x1)
                break; // 防止在垂直线上无限循环
            err += dy;
            x0 += sx;
        }

        if (e2 <= dx)
        { // 垂直步进
            if (y0 == y1)
                break; // 防止在水平线上无限循环
            err += dx;
            y0 += sy;
        }
    }

    return points;
}

void ProbabilityGrid::updateCell(int x, int y, float delta)
{
    if (isValidGrid(x, y))
    {
        int idx = getIndex(x, y);

        float new_log_odds = clamp(cells_[idx].getLogOdds() + delta, -kMaxLogOdds, kMaxLogOdds);
        cells_[idx].updateOdds(new_log_odds);
    }
    else
    {
        return;
    }
}

void ProbabilityGrid::transformCvMap()
{
    cv::Mat obs_map(height_, width_, CV_8UC1, cv::Scalar(100));
    for (int i = 0; i < width_; ++i)
    {
        for (int j = 0; j < height_; ++j)
        {
            int cost = getProbability(i, j) > 0.5 ? 0 : 255;
            obs_map.at<uchar>(j, i) = cost;
        }
    }
}
/

//  --------------------- 示例使用 ---------------------------
int main()
{
    using namespace std;
    // 创建1米分辨率，10x10米的地图
    ProbabilityGrid grid(10, 10, 1.0f);
    // 模拟激光扫描数据：原点(5,5)，命中点(7,7)
    cv::Point origin = {5, 5};
    std::vector<cv::Point> hits = {{7, 7}};

    // 更新地图
    grid.updateLaserScan(origin, hits);

    // 查询命中点概率
    std::cout << "Hit cell probability: "
              << grid.getProbability(7, 7) << std::endl; // 应接近0.7

    // 多次更新模拟
    for (int i = 0; i < 8; ++i)
    {
        grid.updateLaserScan(origin, hits);
    }
    std::cout << "After 8 hits: "
              << grid.getProbability(7, 7) << std::endl; // 概率应接近0.97

    std::cout << "After 8 hits: "
              << grid.getProbability(6, 6) << std::endl;

    return 0;
}

结语：Cartographer中为了提高效率，用查表操作直接代替运算操作，以空间换时间。