cartographr之占据格地图

最新推荐文章于 2024-03-17 20:36:15 发布
最新推荐文章于 2024-03-17 20:36:15 发布
阅读量1.5k 收藏 6
点赞数 1
分类专栏： Cartographer 机器人
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/caizou123123/article/details/86698824
版权

文章目录


这里有个 工程主要就是为了简单表示cartographer中占据格地图如何生成以及原理

在这里插入图片描述
ProbabilityGrid->Grid2D->MapLimits->CellLimits

1, grid_2d

class Grid2D
{
	//成员变量
	MapLimits limits_;  // 表示２d地图的分辨率，大小
	std::vector<uint16> correspondenct_cost　// 对应的值大小为celllimits.num_x_cells * celllimits.num_y_cells
	float min_correspondece_cost_;
	float max_correspondece_cost_;
	std::vector<int> update_indices_;  
    Eigen::AlignedBox2i known_cells_box_;
	// 维护更新时候用的值
	protected:
     const std::vector<uint16>& correspondence_cost_cells() const
     {
       return correspondence_cost_cells_;
     }
     const std::vector<int>& update_indices() const { return update_indices_; }
     const Eigen::AlignedBox2i& known_cells_box() const {return known_cells_box_;}

     std::vector<uint16>* mutable_correspondence_cost_cells() {return &correspondence_cost_cells_;}
     std::vector<int>* mutable_update_indices() { return &update_indices_; }
     Eigen::AlignedBox2i* mutable_known_cells_box() { return &known_cells_box_; }

    // Converts a 'cell_index' into an index into 'cells_'.
    int ToFlatIndex(const Eigen::Array2i& cell_index) const;
};

2,probability_grid

注意就是应用查找表更新地图

bool ProbabilityGrid::ApplyLookupTable(const Eigen::Array2i& cell_index,
                                       const std::vector<uint16>& table)
 {
	  DCHECK_EQ(table.size(), kUpdateMarker);
	  const int flat_index = ToFlatIndex(cell_index);
	  uint16* cell = &(*mutable_correspondence_cost_cells())[flat_index];
	  if (*cell >= kUpdateMarker) 
	  {
	   　　 return false;
	  }
	  mutable_update_indices()->push_back(flat_index);
	  *cell = table[*cell];
	  DCHECK_GE(*cell, kUpdateMarker);
	  mutable_known_cells_box()->extend(cell_index.matrix());
	  return true;
}

3, probability_values

这里面主要维护几个表和有几个转换关于Probability和CorrespondenceCost

１,Probability->CorrespondenceCost

inline float ProbabilityToCorrespondenceCost(const float probability)
{
  	return 1.f - probability;
}

2, CorrespondenceCost->probability

inline float CorrespondenceCostToProbability(const float correspondence_cost) 
{
    return 1.f - correspondence_cost;
}

3, Probability->Odds

// odds = p/(1-p)
inline float Odds(float probability)
{
    return probability / (1.f - probability);
}

4, Odds -> Probability

// p = odds/(odds+1)
inline float ProbabilityFromOdds(const float odds)
 {
    return odds / (odds + 1.f);
}

5, 几个全局变量，最大，小概率，最大最小CorrespondenceCost

constexpr float kMinProbability = 0.1f;         // min probability
constexpr float kMaxProbability = 1.f - kMinProbability; // max probability
constexpr float kMinCorrespondenceCost = 1.f - kMaxProbability; // minc = 1-maxp
constexpr float kMaxCorrespondenceCost = 1.f - kMinProbability; // maxc = 1-minp

6,几个截断概率和Cost的函数

inline float ClampProbability(const float probability)
{
    return Clamp(probability, kMinProbability, kMaxProbability);
}

inline float ClampCorrespondenceCost(const float correspondence_cost)
{
    return Clamp(correspondence_cost, kMinCorrespondenceCost,kMaxCorrespondenceCost);
}

7, 初始化和更新用的参数

constexpr uint16 kUnknownProbabilityValue = 0;
constexpr uint16 kUnknownCorrespondenceValue = kUnknownProbabilityValue;
constexpr uint16 kUpdateMarker = 1u << 15; // 32768

８，将float的概率或者correspondenceCost转为Value　uint16限制范围[1, 32767]

inline uint16  CorrespondenceCostToValue(const float correspondence_cost)
{
        return BoundedFloatToValue(correspondence_cost, kMinCorrespondenceCost, kMaxCorrespondenceCost);
}
//converts a probability to a uint16 in the range[1, 32767] range
inline uint16 ProbabilityToValue(const float probability)
{
    return BoundedFloatToValue(probability, kMinProbability, kMaxProbability);
}

9, 用于将value转为对应概率或者correspondenceCost的查找表，并且需要提前计算kValueToProbability

kValueToCorrespondenceCost

extern const std::vector<float>* const kValueToProbability;
extern const std::vector<float>* const kValueToCorrespondenceCost;

const std::vector<float>* const kValueToProbability = PrecomputeValueToProbability().release();
const std::vector<float>* const kValueToCorrespondenceCost = PrecomputeValueToCorrespondenceCost().release();

重点介绍
PrecomputeValueToProbability
PrecomputeValueToCorrespondenceCost
均使用了PrecomputeValueToBoundedFloat函数继续计算用于保留更新和不更新时候的表
计算出结果result为６５５３６个元素，里面对应float值
在这里插入图片描述

std::unique_ptr<std::vector<float>> PrecomputeValueToBoundedFloat(const uint16 unknown_value,
                                                                  const float unknown_result,
                                                                  const float lower_bound,
                                                                  const float upper_bound)
{
    auto result = make_unique<std::vector<float>>();
    // Repeat two times, so that both values with and without the update marker
    // can be converted to a probability.
    for (int repeat = 0; repeat != 2; ++repeat)
    {
        for (int value = 0; value != 32768; ++value)
        {
            result->push_back(SlowValueToBoundedFloat(value, unknown_value, unknown_result, lower_bound, upper_bound));
        }
    }
    return result;
}

１0查表从uint16得到float值

// converts a uint16  to a probability in the range[kMinProbability, kMaxProbability]
// loop up table
inline float ValueToProbability(const uint16 value)
{
    return (*kValueToProbability)[value];
}


// Converts a uint16 (which may or may not have the update marker set) to a
// correspondence cost in the range [kMinCorrespondenceCost,
// kMaxCorrespondenceCost].
inline float ValueToCorrespondenceCost(const uint16 value)
{
  return (*kValueToCorrespondenceCost)[value];
}

11,(1-4)属于float转换，接下来两个是uint16的转换而且存在一些技巧用于更新值

举一个例子，CorrespondenceCostValueToProbabilityValue同理

inline uint16 ProbabilityValueToCorrespondenceCostValue(uint16 probability_value)
{
    if (probability_value == kUnknownProbabilityValue)
    {
        return kUnknownCorrespondenceValue;
    }
    // ！！！！！！！！！！注意
    bool update_carry = false;
    if (probability_value > kUpdateMarker)　// 概率值大于32768
        probability_value -= kUpdateMarker;　// 
        update_carry = true;
    }
    uint16 result = CorrespondenceCostToValue( //              correspondcost->uint16
                    ProbabilityToCorrespondenceCost(         // probability->correspondcost
                    ValueToProbability(probability_value))); // value->probability
    if (update_carry) result += kUpdateMarker;　
    return result;
}

12 ComputeLookupTableToApplyCorrespondenceCostOdds和ComputeLookupTableToApplyOdds两者一个１-p的关系

std::vector<uint16> ComputeLookupTableToApplyCorrespondenceCostOdds(float odds)
{
   std::vector<uint16> result;
   result.push_back(CorrespondenceCostToValue(
                    ProbabilityToCorrespondenceCost(
                        ProbabilityFromOdds(odds))) +
                    kUpdateMarker);
      ／／公式３              
   for (int cell = 1; cell != 32768; ++cell)
   {
       result.push_back(CorrespondenceCostToValue(
                      ProbabilityToCorrespondenceCost(ProbabilityFromOdds(
                      odds * Odds(CorrespondenceCostToProbability((*kValueToCorrespondenceCost)[cell]))))) +
                      kUpdateMarker);
   }
   return result;
}

4 example

int main(int argc, char *argv[])
{
//模拟一帧激光数据
    RangeData range_data;
    range_data.returns.emplace_back(-3.5f, 0.5f, 0.f);
    range_data.returns.emplace_back(-2.5f, 1.5f, 0.f);
    range_data.returns.emplace_back(-1.5f, 2.5f, 0.f);
    range_data.returns.emplace_back(-0.5f, 3.5f, 0.f);
    range_data.returns.emplace_back(-13.5f, 1.5f, 0.f);
    range_data.returns.emplace_back(-20.5f, 1.5f, 0.f);
    range_data.returns.emplace_back(-1.5f, 9.5f, 0.f);
    range_data.returns.emplace_back(-5.5f, 7.5f, 0.f);

    range_data.returns.emplace_back(30.5f, 20.5f, 0.f);
    range_data.returns.emplace_back(200.5f, 100.5f, 0.f);
    range_data.returns.emplace_back(1.5f, 2.5f, 0.f);
    range_data.returns.emplace_back(0.5f, 3.5f, 0.f);
    range_data.returns.emplace_back(13.5f, 1.5f, 0.f);
    range_data.returns.emplace_back(20.5f, 1.5f, 0.f);
    range_data.returns.emplace_back(1.5f, 9.5f, 0.f);
    range_data.returns.emplace_back(5.5f, 7.5f, 0.f);

    range_data.returns.emplace_back(30.5f, 20.5f, 0.f);
    range_data.returns.emplace_back(200.5f, 100.5f, 0.f);
    range_data.returns.emplace_back(100.5f, 200.5f, 0.f);
    range_data.returns.emplace_back(60.5f, 30.5f, 0.f);
    range_data.returns.emplace_back(13.5f, 15.f, 0.f);
    range_data.returns.emplace_back(20.5f, 150.f, 0.f);
    range_data.returns.emplace_back(5.f, 49.5f, 0.f);
    range_data.returns.emplace_back(56.5f, 17.5f, 0.f);
    range_data.origin.x() = 70.5f;
    range_data.origin.y() = 72.5f;

    ProbabilityGrid grid(MapLimits(1., Eigen::Vector2d(150., 150.), CellLimits(150, 150)));　／／定义一个ｍａｐ
    //将这帧激光数据插入地图中
    Insert(range_data, &grid);
    constexpr int kUnKnown = 128;
    const CellLimits& celllimits = grid.limits().cell_limits();
    const int width = celllimits.num_x_cells;
    const int height = celllimits.num_y_cells;
    cv::Mat image = cv::Mat(width, height, CV_8UC3);
    for(const Eigen::Array2i& xy_index: XYIndexRangeIterator(grid.limits().cell_limits()))
    {
        CHECK(grid.limits().Contains(xy_index));
        const unsigned char value = grid.IsKnown(xy_index)? RoundToInt((1-grid.GetProbability(xy_index))*255+0):kUnKnown;
        image.at<cv::Vec3b>(xy_index.x(), xy_index.y()) = cv::Vec3b(value, value, value);
    }
        cv::imshow("image", image);
        cv::imwrite("image.png", image);
        cv::waitKey(0);

    return 1;
}

Insert函数，　两个表维护hit和miss

void Insert(const RangeData& range_data, ProbabilityGrid* const probability_grid)
{

    const std::vector<uint16> hit_table_(ComputeLookupTableToApplyCorrespondenceCostOdds(
                                             Odds(0.9)));
    const std::vector<uint16> miss_table_(ComputeLookupTableToApplyCorrespondenceCostOdds(
                                              Odds(0.1)));
  // By not finishing the update after hits are inserted, we give hits priority
  // (i.e. no hits will be ignored because of a miss in the same cell).
  CastRays(range_data, hit_table_, miss_table_, true,
           CHECK_NOTNULL(probability_grid));
  probability_grid->FinishUpdate();
}
小西服红色
关注
专栏目录
栅格地图转代价地图，栅格地图转cv::mat图片,代价地图转换为cv::Mat
苏凯的博客
04-04 579
栅格地图转代价地图，栅格地图转cv::mat图片,代价地图转换为cv::Mat
(02)Cartographer源码无死角解析-(43) 2D栅格地图→ValueConversionTables、Grid2D
江南才尽江南山，年少无知年少狂！
01-01 1330
(02)Cartographer源码无死角解析-(03) 新数据运行与地图保存、加载地图启动仅定位模式这篇博客钟保存的地图：该栅格地图可以理解为一个灰度图，颜色约白的像素，说明该像素对应地图坐标系中 cell 被占用的概率越少，这里的占用可以理解为障碍物。也就是说，越黑的区域其是障碍物的概率越大，如上图中墙壁是黑色的，等价于障碍物。另外，在地图的外面，也就是上图中的灰色区域，表示没有探测的区域，其对应的 cell 区域有可能没有被占用，也可能被占用了。
cartographer之GridInterface、Grid_2d、ProbabilityGrid
南北粉面馆
07-30 452
GridInterface基本废弃 namespace cartographer { namespace mapping { class GridInterface { // todo(kdaun) move mutual functions of Grid2D/3D here public: virtual ~GridInterface() {} }; } // namespace mapping } // namespace cartographer
cartographer的栅格地图更新
最新发布
mumapeople的博客
03-17 701
InsertRangeData（）函数具体实现在sumap_2d.cc，核心功能是调用了 submap->InsertRangeData(range_data, range_data_inserter_.get());跳转到Insert（）函数的具体实现，在probability_grid_range_data_inserter_2d.cc中，在ProbabilityGridRangeDataInserter2D类中定义。通过函数Odds转换为更新系数Chit和Cmiss。
Cartographer源码解析——2D栅格地图构建
m0_67976789的博客
04-22 3893
到目前为止，对于点云数据的预处理过程已经介绍完毕，如：点云数据多传感器时间同步、运动畸变校正、重力校正、体素滤波等。做完这一系列的预备工作之后，实际上呢，就可以进行点云的扫描匹配了。在讲解扫描匹配之前，先来看看 Cartographer 2D 的栅格地图，其不像3D点云地图有很多成熟的库可以调用，具有统一的标准。大多数 2D Slam 的栅格地图都是需要自己编写代码进行构建的。下面就来看看 Cartographer 中时如何构建的。
cartographer（9） probability grid
chilian12321的博客
03-25 1083
cartographer 源码中实际上提供grid map两种形式，一种为probability，另一种为tsdf。 grid map MapLimits limits_; // 地图大小边界，包括x和y最大值， 分辨率， x和yu方向栅格数 std::vector<uint16> correspondence_cost_cells_; //grid 地图存储值，采用uint16类型， float min_correspondence_cost_; // 对应实际意义的最...
Cartograoher算法学习与源码阅读（1）
lxjack123的博客
06-15 993
Cartographer:算法的基本思路：Scan是激光扫描的单帧数据，通过累加Scan来构建局部地图（Submap）。Cartographer的实现并没有采用滤波方法，而是采用了Graph-based SLAM的模型进行Pose estimation，具体的实现是用了Ceres scan matching(Ceres是Google自家的库) 。 这样构造出来的很多很多submap是会产生累计误差...
protobuf-master
11-27
Cartographer是一个实时SLAM（Simultaneous Localization and Mapping，即同时定位与建图）解决方案，常用于机器人技术中，用于构建环境地图并确定自身位置。由于protobuf是用于数据通信和存储，所以它是...
OccupancyGrid珊格地图转cv::mat图片，转点云地图
苏凯的博客
12-27 685
OccupancyGrid珊格地图转cv::mat图片
Cartographer源码阅读2D-前端Submap生成及数据插入
yeluohanchan的博客
09-28 1297
Cartographer源码阅读-2D前端Submap生成及数据插入 前端CSM位姿计算后，将激光点云转换到Local SLAM的坐标系下，插入submaps中，该submaps指的是前端LocalTrajectoryBuilder2D中维护的ActiveSubmap2d。 ActiveSubmap2D class ActiveSubmaps2D { public: explicit ActiveSubmaps2D(const proto::SubmapsOptions2D& options)
(02)Cartographer源码无死角解析-(44) 2D栅格地图→ProbabilityGrid 与 ProbabilityToLogOddsInteger()
江南才尽江南山，年少无知年少狂！
01-02 1335
上一篇博客中，首先介绍了 ValueConversionTables，其最终的目的是生成一个转换表，该转换的主要的功能是把[0,1∽32767][0,1∽32767]的数值映射至[0.9,0.1∽0.9][0.9,0.1∽0.9]。虽然后重点讲解 Grid2D::GrowLimits() 函数，该函数主要功能就是判断传入的 point 是否位目前的子图之中，如果不在，则会把地图扩大至原来的四倍，直到 point 处于子图之中。
ROS2下catographer源码参数解析及工作原理
m0_73843839的博客
03-09 2458
cartographer解析
cartographer_probability_grid
fb_941219的博客
06-15 1742
首先从 上层进入，由于cartographer支持 2D 和 3D，所以在代码基本是面向接口编程的， 不完整类图如下(画得不是很规范，将就看）：ActiveSubmaps2D 类：核心函数就是 .这部分代码辨别坐标时： 默认resolution:0.05–>5cm/pixel.分辨率还是挺高的，比较耗内存。 2.栅格地图的更新 这一小节主要搬迁至这篇文章，备忘。 相关常量和数值的计算在 里面。 举例：假设 looccu = 0.9，lofree = -0.7。那么，显而易见，一个栅格状态的
(02)Cartographer源码无死角解析-(41) 2D栅格地图→ActiveSubmaps2D
江南才尽江南山，年少无知年少狂！
12-25 1591
通过前面一系列的博客，PoseExtrapolator 进行了比较细致的分析。到目前为止，对于点云数据的预处理过程可以说时十分了解了，如：点云数据多传感器时间同步、运动畸变校正、重力校正、体素滤波等。做完这一系列的预备工作之后，实际上呢，就可以进行点云的扫描匹配了。在讲解扫描匹配之前，先来看看 Cartographer 2D 的栅格地图，其不像3D点云地图有很多成熟的库可以调用，具有统一的标准。大多数 2D Slam 的栅格地图都是需要自己编写代码进行构建的。
cartographer 生成子图
xiaoma_bk的博客
01-04 7073
红色代表接下來准备解释的函数。。 1、生成子图调用函数 在local_trajectory_builder_2d.ccInsertIntoSubmap函数 中 std::vector&lt;std::shared_ptr&lt;const Submap2D&gt;&gt; insertion_submaps = active_submaps_.InsertRa...
Eigen aligned_allocator
龍的传人
02-23 670
在使用Eigen的时候，如果STL容器中的元素是Eigen数据库结构，比如下面用vector容器存储Eigen::Matrix4f类型或用map存储Eigen::Vector4f数据类型时： vector<Eigen::Matrix4d>; std::map<int, Eigen::Vector4f> 编译能通过，当运行时会报段错误。 对eigen中的固定大小的类使用STL容器的时候，如果直接使用会出错，所谓固定大小（fixed-size）的类是指在编译过程中就已经分配.
cartographer阅读笔记(自用)
南柯一梦的博客
06-03 437
cartographer阅读笔记 ????(自用) 一些不熟悉的C++知识 std::move() std::move 右值引用 左值是指表达式结束后依然存在的持久化对象，右值是指表达式结束时就不再存在的临时对象 在C++11之前，右值是不能引的，如 int &a = 1; // error: 非常量的引用必须为左值 const int &a = 1; // 我们最多只能用常量引用来绑定一个右值 在C++11中我们可以引用右值，使用&&实现： int &&amp
cartographer 占据概率相关
xiaoma_bk的博客
07-20 4065
1. 所处 probablity_Value.h/cc probability_grid.h/c probability_grid_range_data_insert.h/cc 2.相关公式 2.1 基本概念 对于栅格化地图中的一个cell, 或者说一个pixel，它的状态可能有两个状态： s= 1:表示该点被occupied的 s=0:表示该点是Free的 对于同一个点用两个...
cartographer源码浅析（一）-新鲜出炉
liuxiaofei823的博客
04-17 7771
cartographer_ros 入口文件：node_main.cc 入口函数main,如下图： 初始化节点：cartographer_node； Run函数加载配置文件LoadOptions()；根据配置文件处理所订阅的Topic。例如IMU 注意cartographer是否接受imu数据，取决于if判断的条件，这些条件是在“backpack_2d.lu
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值