# Loc_fuse

**Repository Path**: remvs/loc_fuse

## Basic Information

- **Project Name**: Loc_fuse
- **Description**: 基于多源数据和地图方向先验的定位
- **Primary Language**: C++
- **License**: BSD-3-Clause
- **Default Branch**: alldev
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 2
- **Forks**: 0
- **Created**: 2021-10-25
- **Last Updated**: 2024-06-04

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

# 并发建图与定位

本工程受**国家重点研发计划“云计算与大数据”重点专项项目——“多源数据驱动的智能化高效场景建模与绘制引擎”(2017YFB1002601)**资助，为该项目中课题一“复杂场景多源数据分析融合与联合优化”的子课题。

## 介绍
我们提供了一种基于多源数据和方向先验联合优化的定位方法，融合了多源数据和地图方向先验信息。通过融合多种几何约束进行联合优化，利用多源数据和地图方向先验所蕴含的多重几何线索设计二维、三维和方向先验的能量函数，实现车载平台同步定位与地图构建，从而求解出准确的车载平台定位结果。本方法可应用于车载平台同步定位与地图构建，通过从灭点得到少量地图方向先验，并结合二维、三维、方向先验的能量函数并进行联合优化，从图像、激光点云序列和少量地图方向得到每个时刻的六自由度位姿，提高车载平台定位精度。

实验结果表明，我们在KITTI，KITTI-360，Oxford Radar RobotCar 和 POSS 数据上都得到了比单一模态方法更好的结果。其中在KITTI 00-10上总的相对平移误差小于0.5%，旋转误差小于0.001°/米。

## 背景
同步定位与地图构建（SLAM）是当前计算机视觉和机器人领域中的热门技术。它是人类和移动智能体的基本需求，得到了相关产业界的关注和应用，成为各类机器人、无人驾驶、虚拟现实等新兴应用的核心基础算法。
SLAM算法想要解决的问题是，在未知环境中利用传感器（比如相机、激光雷达、惯性测量单元等）运动时获取的观测数据，求解出自身的六自由度位姿，与此同时建立周围环境的三维地图。
现有经典视觉SLAM系统仅从图像上提取颜色或几何信息作为视觉线索来求解位姿，但仅依靠相机会面临单目相机的尺度漂移，剧烈光照变化，纹理缺失等挑战性场景下定位失败的问题。也有方法尝试将图像和激光雷达点云进行数据关联的融合，主要是把激光雷达点云的深度信息结合到视觉线索上，但该方法可能受限于激光雷达的线束数量限制，图像特征并非总能得到完全对应的质量好的深度值。这类方法属于松耦合，没有能充分利用多源数据、多重线索的互补性，所以方法的定位精度不高，需要提升。

## 总体流程

<!-- ![totalpipeline](doc/totalFlow.png "总体流程") -->
&nbsp;
<center>
    <img style="border-radius: 0.3125em;
    box-shadow: 0 2px 4px 0 rgba(34,36,38,.12),0 2px 10px 0 rgba(34,36,38,.08);" 
    src="doc/totalFlow.png">
    <br>
    <div style="color:orange; border-bottom: 1px solid #d9d9d9;
    display: inline-block;
    color: #999;
    padding: 2px;">总体流程</div>
</center>
&nbsp;

本方法包括以下三大模块：

1）对每帧激光雷达点云计算特征，基于隐式移动最小二乘（IMLS），通过点云-地图的点云配准得到每帧点云的初始位姿，输入到视觉同步定位与地图构建（SLAM）系统ORB-SLAM2中，建立关键帧3d点云间基于隐式移动最小二乘（IMLS）约束；

2）通过ORB-SLAM2使用上述1）的点云的初始位姿来得到包含图像和点云的关键帧数据结构，并建立稀疏视觉ORB特征点地图，其中含有关键帧间共视图，与之对应的是2d特征-3d地图点之间的对应关系，即建立2d特征-3d地图点之间的重投影误差约束；

3）检测图像灭点，并利用少量的地图三维方向先验信息和灭点建立三维方向的投影约束，即建立地图三维方向先验的投影约束；最终联合以上三类约束：2d特征-3d地图点之间的重投影误差约束、关键帧3d点云间基于隐式移动最小二乘（IMLS）约束、少量地图三维方向先验的投影约束一同构建能量函数并进行优化。

## 一. 安装依赖
本项目支持在**Ubuntu 16.04**, **18.04**操作系统上运行。 

### C++11 or C++0x Compiler
我们使用 C++11 的多线程和计时功能。

### Pangolin
我们使用 [Pangolin](https://github.com/stevenlovegrove/Pangolin) 进行可视化和用户界面。 可以在以下位置找到下载和安装说明：https://github.com/stevenlovegrove/Pangolin。

### OpenCV
我们使用 [OpenCV](http://opencv.org) 来操作图像和特征。 可以在以下网址找到下载和安装说明：http://opencv.org。 **支持opencv-3.2**。

### Eigen3
g2o 的依赖库（见下文）。 可以在以下网址找到下载和安装说明：http://eigen.tuxfamily.org。 **支持eigen-3.2.10**。

### DBoW2 and g2o (已经包含在 Thirdparty/ 文件夹)
我们使用 [DBoW2](https://github.com/dorian3d/DBoW2) 库来执行地点识别和 [g2o](https://github.com/RainerKuemmerle/g2o) 库来执行非线性优化。

### PCL
我们使用[PCL](https://pointclouds.org/)库来进行三维点云数据的处理，安装参见：https://pcl.readthedocs.io/projects/tutorials/en/latest/compiling_pcl_posix.html。 **支持pcl-1.8.1**。

### python
涉及到使用python来作图，地图先验方向聚类
```shell
conda create -n loc python=3.8
conda activate loc

import argparse
conda install sklearn==0.23.1 argparse numpy yaml matplotlib
```


## 二. 测试流程

#### 工程编译
```shell
git clone https://toscode.gitee.com/remvs/loc_fuse.git Loc_fuse
cd Loc_fuse
chmod +x build.sh
./build.sh
```

> 说明：本工程对于不同的测试数据集，使用不同的工程分支（to do），分支名称包含测试数据集名称，各分支内含有各自数据集接口的运行脚本。下面均以KITTI odometry 00-10为示例数据集。

### 基于点云的初始轨迹生成
<!-- ![lidarodo](doc/IMLSodo.png  " ") -->
&nbsp;
<center>
    <img style="border-radius: 0.3125em;
    box-shadow: 0 2px 4px 0 rgba(34,36,38,.12),0 2px 10px 0 rgba(34,36,38,.08);" 
    src="doc/IMLSodo.png">
    <br>
    <div style="color:orange; border-bottom: 1px solid #d9d9d9;
    display: inline-block;
    color: #999;
    padding: 2px;">基于点云的初始轨迹生成</div>
</center>

&nbsp;

>输入：激光雷达点云
>
>输出：位姿

```shell
./runIMLSodo.sh
```

### 基于图像和点云等多源数据的联合优化

>输入：图像+激光雷达点云+点云初始轨迹+(可选：imu轨迹)
>
>输出：位姿

```shell
# 切换分支

./kitti_wovp.sh

```

### 基于多源数据和场景结构联合优化的数据配准
>输入：图像+激光雷达点云+点云初始轨迹+(可选：imu轨迹)+少量地图先验方向
>
>输出：位姿

```shell
./kitti_benchmark.sh
```

#### 主要结果
&nbsp;
<center>
    <img style="border-radius: 0.3125em;
    box-shadow: 0 2px 4px 0 rgba(34,36,38,.12),0 2px 10px 0 rgba(34,36,38,.08);" 
    src="doc/kitti_ysresult.png">
    <br>
    <div style="color:orange; border-bottom: 1px solid #d9d9d9;
    display: inline-block;
    color: #999;
    padding: 2px;">KITTI 00-10 定位结果</div>
</center>

<!-- ![KITTI 00-10 定位结果](doc/kitti_ysresult.png "KITTI 00-10 定位结果") -->


<!-- ![KITTI 02 定性结果](doc/kitti02.png "KITTI 02 定性结果") -->
&nbsp;
<center>
    <img style="border-radius: 0.3125em;
    box-shadow: 0 2px 4px 0 rgba(34,36,38,.12),0 2px 10px 0 rgba(34,36,38,.08);" 
    src="doc/kitti02.png">
    <br>
    <div style="color:orange; border-bottom: 1px solid #d9d9d9;
    display: inline-block;
    color: #999;
    padding: 2px;">KITTI 02 轨迹结果</div>
</center>