# SLAM

**Repository Path**: d-twelve/SLAM

## Basic Information

- **Project Name**: SLAM
- **Description**: 基于多传感器融合（激光雷达、视觉、反光柱、IMU 、等）的 SLAM 算法
- **Primary Language**: Unknown
- **License**: Not specified
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 0
- **Forks**: 0
- **Created**: 2025-10-25
- **Last Updated**: 2025-10-25

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

# README - 反光柱定位系统 (Reflector-based Localization)

## 1. 系统概览
这个 ROS2 包 `fanguangbantrilateration` 实现了一个**基于激光反光柱的定位系统**，适合在有明显人工反光柱（比如测量柱）的场景下使用。系统由三个主要节点组成：

1. **scan_node**  
   - 从激光雷达点云/扫描数据中提取反光柱（reflectors）。  
   - 发布话题：`/reflectors`  

2. **reflector_match_node**  
   - 读取地图中预定义的反光柱位置（JSON 文件）。  
   - 将 `scan_node` 提取的反光柱与地图进行匹配。  
   - 自动维护一个“地标数据库”（Landmark DB），包括：  
     - 命中计数（hits）  
     - 衰减机制（decay）  
     - 删除机制（remove）  
   - 输出定位结果：`/matched_pose`（机器人在地图中的位姿）。  
   - 可视化：在 RViz 中用 Marker 显示地标和匹配关系。  

3. **pose_estimation_node**  
   - 基于 `reflector_match_node` 的匹配结果，进一步估计和优化机器人位姿。  

---

## 2. 数据流
```
激光雷达 → scan_node → /reflectors
          /reflectors → reflector_match_node → /matched_pose
                                          ↘ 可视化(RViz Markers)
```

---

## 3. 地标数据库 (Landmark DB) 自动维护机制

在 `reflector_match_node` 中，系统维护了一个地标库 `lmdb_`，里面存储了机器人认为“可靠”的反光柱。

每个地标包含：
- `id`：地标的唯一 ID  
- `x, y, z`：位置  
- `hits`：命中次数（表示可信度）  
- `miss`：未命中次数  
- `last_seen`：最后一次被观测到的时间  

### (1) 命中逻辑
- 如果观测到的反光柱成功与某个地标匹配：  
  - `hits` **增加 1**（上限 `landmark_init_hits`，避免无限增长）。  
  - 位置用**指数平滑**更新，减少跳变：  
    ```
    L.x = (1 - α) * L.x + α * obs_x
    L.y = (1 - α) * L.y + α * obs_y
    ```
  - `last_seen` 更新为当前时刻。  

- 如果观测到新反光柱（没有匹配上任何地标）：  
  - 新建一个 Landmark，初始 hits 为 `landmark_init_hits / 2`（避免误检直接成为强地标）。  

### (2) 衰减逻辑
- 每一帧都会调用 `decayLandmarks()`，所有地标的 `hits` 按 `decay_step` 衰减：  
  ```
  hits = max(0, hits - decay_step)
  ```

### (3) 删除逻辑
- 如果某个地标的 `hits` 下降到 `remove_threshold` 以下 → **从地标库中删除**。  
- 这样可以自动清理误检或长期未见到的反光柱。  

### (4) 可视化逻辑
- 在 RViz 中使用 Marker 显示地标：  
  - hits 高 = **绿色**  
  - hits 低 = **红色**  
- 这样可以直观地看到哪些地标是稳定的，哪些即将被删除。  

---

## 4. 启动流程
1. **准备地图文件**  
   - JSON 格式，例如：
     ```json
     [
       {"id": "R1", "x": 1.0, "y": 2.0, "z": 0.0, "radius": 0.1},
       {"id": "R2", "x": 2.5, "y": 3.8, "z": 0.0, "radius": 0.1}
     ]
     ```

2. **参数配置（YAML 文件）**
   例如 `params/reflector_match_node.yaml`：
   ```yaml
   reflector_match_node:
     ros__parameters:
       map_reflectors_file: "/path/to/map.json"
       map_frame_id: "map"
       base_frame_id: "base_link"
       landmark_init_hits: 20
       decay_step: 1
       remove_threshold: 2
       publish_rviz: true
   ```

3. **启动节点**  
   使用 launch 文件：
   ```bash
   ros2 launch fanguangbantrilateration system.launch.py
   ```

---

## 5. RViz 可视化
- `/landmarks` → 显示当前地标（绿色=强，红色=弱）。  
- `/assoc` → 显示观测点与地标的连线（绿色=匹配成功，灰色=未匹配）。  
- `/matched_pose` → 显示机器人估计的位姿。  

---

## 6. 改进建议（针对 hits 衰减/删除机制）
1. **自适应衰减**  
   - 可以让 `decay_step` 随时间动态调整，比如机器人运动速度越快 → 衰减越快。  

2. **双阈值策略**  
   - 增加一个 “冻结阈值”，避免 hits 在高可信度时轻易掉下去。  
   - 比如：  
     - hits ≥ 15 → 永久保存，不再衰减。  
     - hits < 15 → 正常衰减。  

3. **基于时间的删除**  
   - 除了 hits，还可以检查 `last_seen`，如果超过 30s 没再出现 → 删除。  

4. **可视化调试**  
   - 建议在 RViz Marker 文本里显示 hits & last_seen，方便观察地标维护的状态。  

---

## 7. 总结
- `scan_node`：提取反光柱。  
- `reflector_match_node`：匹配、维护地标库、估计位姿。  
- `pose_estimation_node`：融合结果，得到最终定位。  
- 地标库自动维护机制保证了系统能在**长期运行中保持鲁棒性**，既能记住常见的反光柱，也能自动忘记消失或误检的反光柱。