{"id":19855874,"url":"https://github.com/cuixing158/costmap_pathplan","last_synced_at":"2025-09-18T14:32:02.155Z","repository":{"id":247211271,"uuid":"825272496","full_name":"cuixing158/costmap_pathplan","owner":"cuixing158","description":"A simple scheme of costmap+path planning for low-speed automated parking","archived":false,"fork":false,"pushed_at":"2024-09-22T01:23:47.000Z","size":70973,"stargazers_count":3,"open_issues_count":0,"forks_count":2,"subscribers_count":1,"default_branch":"main","last_synced_at":"2025-04-01T19:38:16.959Z","etag":null,"topics":["costmap","costmap-2d-layer","mapping","odometry","pathplanning","simulation"],"latest_commit_sha":null,"homepage":"","language":"MATLAB","has_issues":true,"has_wiki":null,"has_pages":null,"mirror_url":null,"source_name":null,"license":null,"status":null,"scm":"git","pull_requests_enabled":true,"icon_url":"https://github.com/cuixing158.png","metadata":{"files":{"readme":"README.md","changelog":null,"contributing":null,"funding":null,"license":null,"code_of_conduct":null,"threat_model":null,"audit":null,"citation":null,"codeowners":null,"security":null,"support":null,"governance":null,"roadmap":null,"authors":null,"dei":null,"publiccode":null,"codemeta":null}},"created_at":"2024-07-07T10:18:11.000Z","updated_at":"2025-02-07T02:16:53.000Z","dependencies_parsed_at":"2025-01-11T13:58:38.012Z","dependency_job_id":null,"html_url":"https://github.com/cuixing158/costmap_pathplan","commit_stats":null,"previous_names":["cuixing158/costmap_pathplan"],"tags_count":0,"template":false,"template_full_name":null,"purl":"pkg:github/cuixing158/costmap_pathplan","repository_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/cuixing158%2Fcostmap_pathplan","tags_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/cuixing158%2Fcostmap_pathplan/tags","releases_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/cuixing158%2Fcostmap_pathplan/releases","manifests_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/cuixing158%2Fcostmap_pathplan/manifests","owner_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/owners/cuixing158","download_url":"https://codeload.github.com/cuixing158/costmap_pathplan/tar.gz/refs/heads/main","sbom_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/cuixing158%2Fcostmap_pathplan/sbom","scorecard":null,"host":{"name":"GitHub","url":"https://github.com","kind":"github","repositories_count":275781371,"owners_count":25527359,"icon_url":"https://github.com/github.png","version":null,"created_at":"2022-05-30T11:31:42.601Z","updated_at":"2022-07-04T15:15:14.044Z","status":"online","status_checked_at":"2025-09-18T02:00:09.552Z","response_time":77,"last_error":null,"robots_txt_status":"success","robots_txt_updated_at":"2025-07-24T06:49:26.215Z","robots_txt_url":"https://github.com/robots.txt","online":true,"can_crawl_api":true,"host_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub","repositories_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories","repository_names_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repository_names","owners_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/owners"}},"keywords":["costmap","costmap-2d-layer","mapping","odometry","pathplanning","simulation"],"created_at":"2024-11-12T14:13:59.288Z","updated_at":"2025-09-18T14:31:57.141Z","avatar_url":"https://github.com/cuixing158.png","language":"MATLAB","funding_links":[],"categories":[],"sub_categories":[],"readme":"# 基于先验里程计+车位检测实时构建costmap 2D地图及路径规划\n\n[![MATLAB Build](https://img.shields.io/badge/MATLAB_Build-passing-success)](https://matlab.mathworks.com/)\n\n[![Test](https://img.shields.io/badge/tested_with-R2024a-blue)](https://matlab.mathworks.com/)\n\n![cost map](./data/mapOnLine.JPG)\n\n## Overview\n\n\u003e 本repo旨在根据**4副鱼眼原始环视图+先验`egoVehicle`里程计($x$,$y$,$\\theta$)+俯视图车位检测**，依旧逐步完成多相机标定、图像拼接/融合、BirdEyeView俯视图生成、多目标车位检测跟踪，最终实时构建2D cost map地图和全局路径规划。\n\n\u003c!-- @import \"[TOC]\" {cmd=\"toc\" depthFrom=1 depthTo=6 orderedList=false} --\u003e\n\n\u003c!-- code_chunk_output --\u003e\n\n- [基于先验里程计+车位检测实时构建costmap 2D地图及路径规划](#基于先验里程计车位检测实时构建costmap-2d地图及路径规划)\n  - [Overview](#overview)\n  - [Features](#features)\n  - [Workflow](#workflow)\n    - [1. Fisheye Camera Calibration](#1-fisheye-camera-calibration)\n    - [2. Multi-Object Parking Detection And Track](#2-multi-object-parking-detection-and-track)\n    - [3. Cost Map](#3-cost-map)\n    - [4. Path Planning](#4-path-planning)\n  - [C++ implementation](#c-implementation)\n  - [Reference](#reference)\n\n\u003c!-- /code_chunk_output --\u003e\n\n## Features\n\n- [x] 支持实时动态任意构建，依旧无需预先指定地图总大小\n- [x] 像素地图与物理地图一一对应，查询/定位方便\n- [x] 支持多目标车位跟踪关联\n- [x] :star:(new)支持任意给定costmap地图和起止2D点坐标，规划一条无碰撞全局路径\n- [x] :star:(new)支持地图加载和保存(xml,bin,json等任意格式，优势明显)，非两年前保存的像素地图\n- [x] :star:(new)支持地图动态增补\n- [x] :star:(new)支持地图多语义标签表达，非之前的二进制(0或者1)模式\n- [x] :star:(new)支持全局地图和当前局部costmap地图随时获取\n- [x] 高效支持嵌入式C/C++代码生成\n- [ ] 支持车道线、导流线、柱子等各类障碍物感知信息的输入\n- [ ] 支持3层地下停车场地图设计\n- [ ] 支持提供给UI全局地图路径规划接口\n- [ ] 支持提供给UI多层地图(全局和局部)接口\n- [ ] 支持全局路径在拐弯、跨层、十字路口等描述\n- [ ] 支持全局地图处理误差累计修正\n- [ ] 支持地图误差实时输出\n- [ ] 支持地图分区域/分段\n- [ ] 支持泊车后地图自动更新\n\n## Workflow\n\n### 1. Fisheye Camera Calibration\n\n根据车载周围的四副鱼眼畸变图像和里程计等相关数据，可以不用传统的“棋盘标定板”检测每张图像的每个角点的方法标定鱼眼图像，本项目亮点做法是：\n\n- 利用畸变表多项式拟合求解内参和畸变系数\n- **仅利用地面一个矩形的物理尺寸计算内外参**\n\n详细实现标定流程文件请参阅[`fisheyeCameraCalibration.m`](./fisheyeCameraCalibration.m)或者[pdf](https://github.com/cuixing158/Visual-Based-Odometry-Estimation/blob/main/fisheyeCameraCalibration.pdf)。\n\n标定大体流程图从上到下如下过程：\n\n![flow](./data/flow.png)\n\nFig. 第1行为原始鱼眼畸变图像,第2行为对应的去畸变图像，第3行为对应的俯视图像\n\n最后计算各自视图相对\"front\"的转换矩阵，融合拼接全景俯视图像即可。\n\n### 2. Multi-Object Parking Detection And Track\n\n目前是使用深度学习经典的yolo模型在BEV图像下做车位检测，多目标跟踪关联策略输出稳定的车位序号。这个是非常成熟的技术，主要看三点：\n\n1. 跟踪到的车位，即assignments分配情况；\n\n2. 跟踪丢失的车位，即missingTracks；\n\n3. 新进入的车位，即unassignedDetection;\n\n![cost matrix](./data/costMatrix.JPG)\n\nFig. Cost代价矩阵，每行代表跟踪，每列代表检测\n\n说明如下：对于第1种情况，跟踪与某个检测关联上了，其历史计数应当加1；第2种情况，跟踪丢失，跟踪器不一定删除，但若超过给定阈值，则删除此跟踪；第3种情况，新进入的车位，一般没有与历史跟踪匹配的上，那么新建一个跟踪。\n\nCost代价矩阵的计算根据自己定义的距离函数进行度量，由于目前是车位跟踪，以车位中心点的世界坐标欧式距离为指标计算即可。**要输出车位costmap地图，无需对跟丢失的车位进行删除，问题其实简化了很多，只需关注要上面第1,3点即可。车位序号即跟踪器序号，可以稳定输出，即使以后走回圈重识别到以前的车位，依旧可以稳定输出车位序号。**\n\n- 车位的正确表达方式\n\n上面说了多目标跟踪器，那么自然就联想到上面一个“检测目标”应该怎么表示，车位应当作为一个单独“类”来进行管理，做到跟踪器与车位无关！比如本项目目前用的是深度模型输出的2个顶点和夹角表示法，其可以转换为我们任何想要的表示方式，最简单直观的是输出4个顶点坐标。\n\n其实，常用的车位类型有很多种，比如下面几种，而我们遇到更多的是封闭式车位和车位线上的独有车位序号，这个应当作为车位这个类型的附加属性管理。车位当然还有其他更多属性，比如宽度，长度，车位线类型等等。\n\n![parking Type](./data/parkingType.JPG)\n\nFig. 车位类型，图源[link](https://ww2.mathworks.cn/help/driving/ref/parkingspace.html?s_tid=doc_ta)\n\n根据问题需要，本Repo使用简化的表示方式，即[$centerX$,$centerY$,$width$,$length$,$\\theta$]表示一个车位，只考虑位姿，不考虑其他车位属性。\n\n### 3. Cost Map\n\n由上面几个步骤后，就要建立`Cost Map`地图了，其意义在于为后续的规控提供搜索空间，地图包含了障碍物和`ego vehicle`可行驶区域，以网格占据概率进行表征。要得到一条无碰撞行驶轨迹，适当时候算法会把地图转换成一张二值像素图像，以车辆的最小包围半径进行基本的图像处理操作，比如膨胀，腐蚀等。然后结合全局路径规划器（比如A*,RRT，PRM等等），最终通过地图，构建一条无碰撞的行驶轨迹。\n\n当前我们感知算法内容仅有车位是否占据网格，即只有障碍物和自由空间之分，故简化成了`binaryCostMap`地图。\n\n- 正确的地图构建方式\n\n常规构建`Cost Map`，一般会考虑到停车场大小，像素与物理世界的分辨率，还有考虑到动态扩展地图大小等因素。在程序实践上，一般要预分配一块内存用于保存地图总大小空间(比如[binaryOccupancyMap](https://ww2.mathworks.cn/help/nav/ref/binaryoccupancymap.html)，当然我也可以做到不预分配，动态扩增方式)不预分配总大小，随着地图扩张，按照自适应方向以`padArray`方式逐步扩张边界像素图像达到动态建图目的。\n\n要正确并实时方式构建地图，也要考虑到`ego vehicle`的任意走向和未知的停车场大小，所以前期**不要用像素地图扩张的方式构建，而是以非常自然的世界物理坐标方式表示传递，到真正需要路径规划用到`Cost Map`的时候，最后一步如有必要时直接转换为像素地图。其优势还在于此方法几乎没有任何计算开销和地图内存占用开销！仅仅用到几个坐标系转换矩阵乘法而已！**\n\n以近期的数据为例，其最终实时构建的动态costmap地图如下：\n\n![cost map gif](./data/mapOnLine.gif)\n\nFig. 左侧为当前帧检测车位，右侧为对应的costmap地图\n\n### 4. Path Planning\n\n这里主要是指地图的全局路径规划，只有在**泊车模式**下才启用。在建图模式下给定的`costmap`地图+起始和终点坐标点，生成一条无碰撞的全局路径，目的用于后续局部路径规划作为参考线。另外关于局部路径规划请参阅郭倩之前的相关工作。\n\n根据年前自己完成的部分`C++`版`costmap`地图工作，结合近期的逐步完善，现在可以得到如下的多语义地图：\n\n![cost map](./data/map_on_line.JPG)\n\nFig. 多语义地图\n\n简单解释如下：如果以像素地图表征`costmap`，那么图像中是以概率网格占据的形式，在[0,1]范围之间，概率越接近1就越代表非freespace区域，越接近0就越代表可行驶区域。\n\n关于一段路径的路径规划示例如下：给定上述`costmap`地图+起始点和终止点坐标分别为`startPose = [0,0,0]`,`goalPose = [65,-17,-pi/2]`，那么通过混合A*算法可以得到如下规划路径。在MATLAB中可视化如下：\n\n![cost map](./data/plannedPath.png)\n\nFig. 路径规划\n\n## C++ implementation\n\nC++实现了上面同等部分功能，请移步到我的此[Repo](https://github.com/cuixing158/costmap_pathplan_cpp),欢迎批评指正！\n\n## Reference\n\n1. [Create 360° Bird's-Eye-View Image Around a Vehicle](https://www.mathworks.com/help/driving/ug/create-360-birds-eye-view-image.html)\n1. [Automated Parking Valet](https://www.mathworks.com/help/driving/ug/automated-parking-valet.html)\n1. [Automated Parking Systems](https://www.mathworks.com/help/driving/automated-parking-systems.html)\n","project_url":"https://awesome.ecosyste.ms/api/v1/projects/github.com%2Fcuixing158%2Fcostmap_pathplan","html_url":"https://awesome.ecosyste.ms/projects/github.com%2Fcuixing158%2Fcostmap_pathplan","lists_url":"https://awesome.ecosyste.ms/api/v1/projects/github.com%2Fcuixing158%2Fcostmap_pathplan/lists"}