# ORB_SLAM2_standalone

**Repository Path**: CodeZWQ/orb_slam2_standalone

## Basic Information

- **Project Name**: ORB_SLAM2_standalone
- **Description**: cartographer移植和优化项目视觉重定位部分
- **Primary Language**: Unknown
- **License**: GPL-3.0
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 0
- **Forks**: 4
- **Created**: 2022-02-22
- **Last Updated**: 2024-05-31

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

				cartographer移植和优化项目视觉重定位部分
产品描述：

基于雷达SLAM输出轨迹真值的视觉全局定位。定位有两种模式：基于特征地图定位和基于关键帧定位。优先基于特征地图定位，在特征地图稀疏或没有成功建立特征地图的区域使用基于关键帧定位。其中基于特征地图的定位结果更准确。

目前算法足够鲁棒，在严苛环境下能够连续给出全局定位的pose值。

Input：

保存视觉帧的开始和结束信号

激光雷达高频的tf话题

激光雷达低频的优化后的轨迹

Output：

如果基于特征地图定位，则给出唯一的定位结果pose值

如果基于关键帧定位，则给出BOW查询结果中相似的几个pose值，其中第一个pose为最好的pose

一、RK3399+Ubuntu16.04部署方法
I、Ubuntu16.04系统安装

官网参考链接：http://wiki.t-firefly.com/zh_CN/Firefly-RK3399/01-bootmode.html （主要是“更新固件”内容）
    1、 方法概述：使用一台安装ubuntu（我的是ubuntu 16.04）的电脑通过
     Type-C data cable数据线与Firefly-RK3399 开发板相连，在电脑上运行命令通过官方“Linux_Upgrade_Tool”工具烧录系统。硬件连接如下图

    2、 硬件设备：
    • Firefly-RK3399 开发板；
    • 良好的Type-C data cable数据线；
    • HDMI转VGA转接头（安装系统时不用连，安装完系统显示用）。
    3、 安装步骤：
    1) 通过官网链接下载Ubuntu 16.04镜像文件压缩包（https://pan.baidu.com/s/17xsxTXmVig4t6U3qlvFVBQ#list/path=%2F&parentPath=%2Fsharelink1414141670-882445172942659，提取码：byl8），然后下载“Ubuntu->Ubuntu16.04->Public-> FIREFLY-RK3399-UBUNTU16.04-GPT-20190910-1001.img.7z”。
另外，也可以从firefly官网链接（http://www.t-firefly.com/doc/download/page/id/3.html）中的“固件->Ubuntu(GPT)”进入下载链接
    2) 在安装ubuntu的电脑解压“FIREFLY-RK3399-UBUNTU16.04-GPT-20190910-1001.img.7z”，文件中包含升级工具“Linux_Upgrade_Tool”文件和镜像文件（pack(2)中文件，.img文件大小2.3G）。
    3) 设备先断开电源适配器和 Type-C 数据线的连接：
    a) USB 一端连接主机，Type-C 一端连接开发板 Type-C 母口
    b) 按住设备上的 RECOVERY （恢复）键并保持
    c) 接上电源（蓝色指示灯不亮）
    d) 大约两秒钟后，松开 RECOVERY 键
    4) 在“Linux_Upgrade_Tool”文件夹下打开终端，然后运行如下命令：
    a) sudo chmod a+x upgrade_tool
    b) sudo upgrade_tool uf FIREFLY-RK3399-UBUNTU16.04-GPT-20190910-1001.img
    5) 等待安装，安装需要几分钟，安装完成会显示“Upgrade fireware ok.”，等待1分钟左右系统会自动重启，蓝色指示灯亮起，可连接显示器。若安装失败参见官网参考链接寻找解决办法
    6) 更新
    a) sudo apt-get update
    b) sudo apt-get upgrade


II、 环境安装
参考链接：
https://blog.csdn.net/nannan7777/article/details/107161720
https://blog.csdn.net/weixin_44436677/article/details/105587986
https://blog.csdn.net/weixin_45137708/article/details/105565800

    1、 安装cmake
sudo apt-get install cmake
    2、 安装gcc、g++、git
sudo apt-get install gcc g++ git
    3、 安装opencv 3.2.0
参考链接：
http://dev.t-firefly.com/forum.php?mod=viewthread&tid=12143
https://blog.csdn.net/radiantjeral/article/details/82193370
    1) 安装依赖
sudo apt-get install build-essential

# 下一条中libgtk2.0-dev的依赖
sudo apt-get install libpng12-dev
sudo apt-get install libgdk-pixbuf2.0-dev

sudo apt-get install cmake git libgtk2.0-dev pkg-config libavcodec-dev libavformat-dev libswscale-dev

sudo apt-get install python-dev python-numpy libtbb2 libtbb-dev libjpeg-dev libpng-dev libtiff-dev libjasper-dev libdc1394-22-dev

    2) 下载源码
    1） opencv官网手动下载3.2.0版本：
https://opencv.org/releases/page/5/ 
    2） 命令行下载
git clone https://github.com/opencv/opencv.git

cd opencv/

git checkout 3.2.0

    3) 编译与安装
首先进入安装包的根目录
mkdir build

cd build

cmake -D CMAKE_BUILD_TYPE=RELEASE -D CMAKE_INSTALL_PREFIX=/usr/local .. 

make

sudo make install

    4、 安装pangolin
    1) 安装依赖
sudo apt-get install libglew-dev libpython2.7-dev libboost-dev libboost-thread-dev libboost-filesystem-dev

    2) 下载源码
git clone https://github.com/stevenlovegrove/Pangolin

    3) 编译与安装
进入Pangolin根目录
mkdir build

cd build

cmake ..

make -j4

sudo make install

    5、 安装eigen3
    1) 下载源码
wget https://bitbucket.org/eigen/eigen/get/3.2.10.tar.bz2

tar -xjf 3.2.10.tar.bz2

# 重命名
mv eigen-eigen-b9cd8366d4e8/ eigen-3.2.10

    2) 编译与安装
进入eigen-3.2.10根目录
mkdir build

cd build

cmake ..

make

sudo make install

    6、 安装ROS kinetic，参照官网链接
安装步骤如下：
使用清华镜像：
sudo sh -c '. /etc/lsb-release && echo "deb http://mirrors.tuna.tsinghua.edu.cn/ros/ubuntu/ `lsb_release -cs` main" > /etc/apt/sources.list.d/ros-latest.list'

sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654

sudo apt update

sudo apt-get install ros-kinetic-desktop-full
	#如果报错unable to …,就再更新一次，再安装一下
	sudo apt update
	sudo apt-get install ros-kinetic-desktop-full

apt-cache search ros-kinetic

echo "source /opt/ros/kinetic/setup.bash" >> ~/.bashrc

source ~/.bashrc

sudo apt install python-rosdep python-rosinstall python-rosinstall-generator python-wstool build-essential

sudo apt install python-rosdep

sudo rosdep init

rosdep update

# 开启主节点验证是否安装完成
roscore

III、 安装ROS的uvc_camera包
sudo apt-get install ros-kinetic-uvc-camera

IV、 下载、编译与运行ORB_SLAM2_standlone
    1、 新建ROS工作空间
mkdir -p ~/orb_slam2_ws/src

cd ~/orb_slam2_ws/src

in_init_workspace

cd ~/orb_slam2_ws/

catkin_make

source devel/setup.bash

    2、 下载ORB_SLAM2_standlone源码
cd ~/orb_slam2_ws/src

git clone https://gitee.com/yiluzhang/orb_slam2_standalone.git

    3、 编译
cd orb_slam2_standalone

mkdir -p data/Frames

chmod +x build.sh

./build.sh

cd ../..

catkin_make

    4、 运行
    1) 更改配置文件（orb_slam2_standalone/Examples/Monocular/ iisg_carto.yaml）里设置参数
    a) 是否开启可视化界面
# 0-close viewer 1-open viewer
bUseViewer: 0

    b) 相机参数
# 相机topic
Camera_topic: “/camera/image_raw”

# 相机参数
Camera.width:640
Camera.height:480

# Camera calibration and distortion parameters (OpenCV) 
Camera.fx: 644.148203
Camera.fy: 641.932757
Camera.cx: 454.125051
Camera.cy: 263.812763
Camera.k1: -0.390540
Camera.k2: 0.147062
Camera.p1: 0.000460
Camera.p2: -0.001930

# Camera frames per second 
Camera.fps: 30.0

# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
Camera.RGB: 1

    c) tf话题参数
# 话题名
TF_topic: "/tf"
# 与激光雷达pose对应的tf话题中的“frmae_id”名
Odom_frame: "odom"
# 与激光雷达pose对应的tf话题中的“child_frame_id”名
Tracking_frame: "laser_link"

    d) 相机和激光雷达的外参
# 相机坐标系到激光雷达坐标系的相对位姿
T_laser_cam:

    e) 文件夹路径
# 程序运行中需要保存和加载文件的data文件夹路径，更换data文件夹的路径，相对路径和绝对路径都可以

data_dir: "src/orb_slam2_standalone/data/"

    2) 启动主程序
# 打开新终端，启动ROS主节点
roscore

# 启动相机，用bag测试时不用启动相机, /dev/video*为相机设备名
roslaunch orb_slam2_ros uvc_cam.launch device:=/dev/video*

# 打开新终端，启动ORB_SLAM2_standlone节点
 cd ~/orb_slam2_ws

source devel/setup.bash

# 第一个参数为ORB词袋文件，第二个参数为配置参数文件
rosrun orb_slam2_ros Mono src/orb_slam2_standalone/Vocabulary/ORBvoc.txt src/orb_slam2_standalone/Examples/Monocular/iisg_carto.yaml

    3) 启动测试节点：在没有lidar制图板时，使用bag包单独测试视觉板
# 打开新终端，启动接收topic和service测试节点
功能：1.接收主程序建图完成发出的service
      2.显示主程序重定位成功发出的topic
cd ~/orb_slam2_ws

source devel/setup.bash

rosrun orb_slam2_ros test_sub_srver

# 打开新终端，启动发送topic和service测试节点，path为lidar pose的TUM格式.txt文件，该节点可用于向主程序发送topic和service，键盘输入数字‘x’，然后按下“enter”键发送命令，‘x’对应命令如下：
1-发送保存帧请求，然后打开新终端使用rosbag play [包路径]回放
2-发送停止保存帧请求，待回放完bag包后发送
3-发送建图请求，从下面启动测试节点[path]中读取lidar poses发送
4-发送停止建图请求，建图完成后会自动停止，可以不用手动发送
5-发送重定位请求，然后打开新终端使用rosbag play [包路径]回放
6-发送停止重定位请求，重定位可一直运行，无需停止
注：
    a) 目前主程序只支持顺序切换，往回切需重启主程序，帧和地图会自动保存到存储，无需重复保存帧和建图
    b) 保存帧时使用的bag包需要包含相机和tf话题

cd ~/orb_slam2_ws

source devel/setup.bash

rosrun orb_slam2_ros test_node [path]

注意事项：
1、更改ORB_SLAM2的源码后，需要更新ORB_SLAM2动态库，方法如下
    a、在ORB_SLAM2/build下用命令行：make ORB_SLAM2
    b、在ORB_SLAM2根目录下用命令行：./build.sh
2、关键参数说明
调节以下参数可以改善建图结果。
    1) 初始化参数
# 选择单应矩阵H或基础矩阵F的阈值，取值0.4-0.45，越大越偏向H
init_RH: 0.4
# 三角化时剔除异常点的重投影误差阈值, 取值4.0-10.0
init_th2: 8.0
# 初始化成功所需最小点数占经过随机采样一致性筛选后点数的比例，取值# 0.7-0.9
nMinGood_ratio: 0.9
# 初始化成功所需的最小3D点数，取值80-100
init_tracked_mappoints: 100
    2) 跟踪参数
# 关键帧跟踪成功所需当前帧与参考关键帧最小匹配点数，取值0-15
refer_nmatches: 5
# 关键帧跟踪成功所需当前帧与参考关键帧共视的最小3D点数, 取值0-10
refer_nmatchesMap: 3

# 普通帧跟踪成功所需当前帧与参考帧最小匹配点数，取值0-20
motion_nmatches: 5
# 普通帧跟踪成功所需当前帧与参考帧共视的最小3D点数, 取值0-10
motion_nmatchesMap: 3

# 重定位后跟踪局部地图成功所需当前帧与局部地图共视的最小3D点数，#_取值0-50
local_map_reloc_nmatches: 10
# 正常跟踪局部地图成功所需当前帧与局部地图共视的最小3D点数, 取值# 0-30
local_map_nmatches: 5

# 3D点跟踪丢失后插入关键帧的距离间隔，取值0.2-1.0，单位：米
KF_delta_distance: 0.2
# 3D点跟踪丢失后插入关键帧的航向角间隔，取值2.0-10.0，单位：度
KF_delta_yaw: 2.0

二、待解决问题：
1、切掉tracking线程中pose优化部分，直接注释掉pose优化部分报opencv错;
可能原因：打印数据发现优化还是会剔除一部分outlier
2、更改localmapping线程，源代码超过15共视帧的才会加入局部地图三角化和优化，为增加程序鲁棒性，
应使在跟踪较少或丢失情况下也能继续三角化。若更改牵扯代码较多;
3、前端需要具备接收有pose图片2个话题和无pose图片1个话题的功能,无pose时应该不进行初始化，只有重定位;
解决：更改ros_mono.cc文件，track（）函数最后代码造成重定位时崩溃，已设置条件
4、更新ORB_SLAM2动态库后重定位失败，以及直接使用加载地图重定位异常退出;
解决：关闭闭环检测后DB添加关键帧被关掉，更改闭环检测部分后正常。
5、寻找两帧之间的匹配点方法待优化
6、ros接收到pose和原pose有点差别

三、工作日志：
1、使用VINS双目+IMU录制好EuRoc数据集MH_01_easy轨迹rosbag（包含/cam0/image_raw及其pose两个topic）;
2、完成ORBSLAM2环境安装，在ROS环境下测试录制的rosbag;
3、完成pose话题的接收和传递，测试完成;
4、理清代码更改思路，尽可能不改动源码，完成初始化代码更改，待测试;
5、完善初始化代码更改并完成测试;
6、固定tracking和local mapping线程中pose，初步完成建图，固定pose后容易丢失，待进一步完善和调试。
7、完成单帧序列重定位(不跟踪)，并测试;
8、完成地图信息保存，并测试;
遇到问题：std::map<MapPoint*,long unsigned int> mmpnMapPointsIdx;定义在Map.h的public报错，定义
在protected正常。
9、完成地图信息加载，并完成加载测试;
10、完成系统雏形，待进一步完善和调试;
解决残留bug：之前更改代码后未更新ORB_SLAM2动态库，造成一系列奇怪的bug。
11、优化代码，解决问题3、4,实现建图、地图保存和加载及重定位基本功能，并使用EuRoC数据集MH_01轨迹建图并保存地图，然后加载地图
使用MH_01,MH_02,MH_03,MH_04,MH_05测试重定位效果，从MH_01数据和其他轨迹重定位视频来看，定位效果不错。
12、完成Frame的保存和加载，并完成测试;
问题：viewer.cc中imshow，配置文件需添加width和height
13、更改并调试丢失后重新初始化程序;
问题：重新初始化后总是不能跟踪多帧
解决：将tracking.cc中int nMinObs = 3;改为int nMinObs = 2;
14、调试并完成重定位功能;
15、定义通信协议;
15、将rosbuild编译方式更改为catkin方式;
16、调整输入数据格式，等待数据调试;
17、优化代码，完成我们数据集测试;
18、增加与制图板通信接口及测试节点，完成测试;
19、实现使用service切换保存帧、建图和重定位三个功能;
20、增加外参和路径到设置.yaml文件，测试完成;
21、优化接口和功能切换;
22、给firefly-RK339安装ubuntu 16.04系统和ORB_SLAM2环境
问题：ubuntu 18.04安装opencv和pangolin时报openGL错误
22、完成由笔记本到firefly-RK339(ubuntu 16.04)移植并测试
问题：make -j板子会出现内核错误，
orb_slam2_ros的CMakeLists.txt中include_directories需要添加${EIGEN3_INCLUDE_DIR}
23、协助激光组测试
问题：建图效果差，初始化难成功
24、解决建图效果差问题，查找问题原因，优化配置参数，新增无3D点关键帧地图及定位功能，并完成在录制数据集的测试
25、在新电脑部署ubuntu 16.04运行环境，解决同时原cartographer和orbslam2兼容性问题及一些琐碎问题，安装相机和激光雷达驱动程序
问题：一些opencv版本对原cartographer和orbslam2不通用
解决：安装opencv 3.3.4解决
26、了解和控制turtlebot2
27、学习cartographer源码，了解其参数配置，增加保存tf和优化后轨迹功能，调整参数使用雷神LS01B完成建图
28、在笔记本电脑同时开启相机和激光雷达，实时运行原cartographer和orbslam2得到闭环轨迹用于外参标定
29、使用邸老师外参标定程序完成外参标定，在实验室测试我们改进算法，建图与定位效果得到改善
30、优化参数，完成组内测试报告
31、在算法增加保存tf轨迹功能，并使用优化后轨迹进行校正
32、关键帧定位使用PoseArray发布重定位结果
33、优化参数，将重要参数更改为由yaml文件设置，便于调参
34、整理产品文件，向激光组交付
35、重装ubuntu 18.04系统，安装激光组改进的cartographer，与立群沟通完成1.1.0版本部署
问题：安装激光组远程库proto依赖后安装turtlebot2驱动程序报错，编译cartographer也报错
解决：再次重装ubuntu 18.04，先安装turtlebot2驱动，再安装激光组远程库carto的abseil和ceres依赖，再按carto官方安装proto
36、更新README.md
37、写测试用python脚本，向激光组交付测试报告
问题：1）用录制的bag包跑carto发布的tf的时间戳不是原时间戳；2）把算法阈值调低，特征地图变好了，但用测试脚本测试发现成功率变低
解决：1）写校正时间戳程序粗校正并保存tf内容，再利用evo评测工具细校正时间戳
      2）改回原来参数
38、阅读cartographer源码
39、修改cartographer源码，使用cartographer的Branch and Bound方法筛选和优化视觉重定位pose，完成测试




四、通信协议
视觉板与制图板通信的具体描述如下。
1) 保存帧
使用SaveFrames.srv服务，服务定义如下, 服务名字“save_frames”。步骤：制图板发送保存帧请求(save_flag=true)，视觉板将建图标志位置位后返回完成标志（finish_flag=true）；制图板完成数据采集后，发送停止保存帧请求(save_flag=false)通知视觉板停止采集图像，视觉板将建图标志位复位后返回完成标志（finish_flag=true）。
另外，在保存帧时，视觉板会订阅制图板发出的话题名为“/tf”的话题，并将激光雷达的位姿以TUM格式保存为本地文件。


SaveFrames.srv:
bool save_flag  # true-发送保存帧请求；false-发送停止保存帧请求
---
bool finish_flag # 接收到开启或停止保存信号

注：
    a) 每次开启保存帧自动删除上一次保存的帧。
    b) “/tf”话题为“tf2_msgs/TFMessage.msg”，定义如下：
https://github.com/ros/geometry2/blob/noetic-devel/tf2_msgs/msg/TFMessage.msg 
2) 建图
使用CartoToVisionMap.srv服务，服务定义如下。
    1） 制图板为client，视觉板为server,服务名字“carto_to_vision_map”
制图板完成位姿优化后向视觉板发送建图请求（map_flag=true），并且将所有scan帧的时间戳和pose（poses）发送给视觉板，视觉板返回接收到信号标志（finish_flag=true）,然后视觉板开始建图。
2）制图板为server，视觉板为client, 服务名字“carto_to_vision_map_rep”
视觉板建完图后发送建图完成标志（map_flag=true）

CartoToVisionMap.srv:
bool map_flag  # true-发送建图请求； 
geometry_msgs/PoseStamped[] poses # 激光雷达所有scan帧的时间戳和位姿(雷达到世界坐标系)
---
bool finish_flag # true-接收到建图信号
注：geometry_msgs/PoseStamped定义如下：
https://github.com/ros/common_msgs/blob/noetic-devel/geometry_msgs/msg/PoseStamped.msg 

3) 重定位
使用Relocalization.srv,服务定义如下, 服务名字“relocalization”。步骤：制图板向视觉板发送重定位请求（reloc_flag=true），视觉板返回接收到信号标志（finish_flag=true）。另外，若存储中已有地图，返回map_built_flag=true，开启重定位，定位成功结果后以话题形式发出；若存储中没有地图，返回map_built_flag=false，重定位未开启，需要保存好地图后再次发送重定位请求。

Relocalization.srv:
bool reloc_flag # true-发送重定位请求;
---
bool finish_flag # true-接收到重定位信号
bool map_built_flag # true-已有地图 false-没有地图

注：定位成功后发送的话题为“geometry_msgs/ PoseArray”，名字为“reloc_pose”。topic定义如下链接：
https://github.com/ros/common_msgs/blob/noetic-devel/geometry_msgs/msg/PoseArray.msg
话题详解：
    a) 时间戳为图像帧的时间戳，pose为雷达到世界坐标系（T_wl）。
    b) pose数组的第一个pose为最佳结果，其余为次优结果，次优结果随机排序。
    c) 话题的“frame_id”用于指示哪种定位结果，若其为“map_reloc”则表示为特征地图定位结果；若其为“kf_reloc”则表示为关键帧定位结果。

五、周报：详细周报见“周报”文件夹
8.18-8.23：
1、使用VINS双目+IMU录制好EuRoc数据集MH_01_easy轨迹rosbag（包含/cam0/image_raw及其pose两个topic）;
2、完成ORBSLAM2环境安装，在ROS环境下完成录制的rosbag测试;
3、确定初步整体方案和代码更改思路。
初步方案：在ORBSLAM2上进行更改，主要更改tracking线程，去掉loop线程。tracking线程不再优化pose，主要用于
提取特征点、数据关联、去除误匹配、选择关键帧给局部地图以及通过标志位切换建图和重定位模式。另外还需增加地图保
存和加载程序。
代码更改思路：保持原有框架。先完成建图方案，再根据重定位（保持原有方法）需求完成地图保存和加载，最后整体调试。

8.24-8.30:（这周工作日上班，周末旅游，时间较少，下周二回家后补上进度，现在思路基本清晰了，一周左右应该能完成雏形）
1、完成pose话题的接收和传递，测试完成;
2、理清代码更改思路，尽可能不改动源码，完成初始化代码更改，待测试。

8.31-9.4：
1、完善初始化代码更改并完成测试;
2、固定tracking和local mapping线程中pose，初步完成建图，固定pose后容易丢失，待进一步完善和调试。

9.5-9.11：
1、完成地图信息的保存和加载，并测试;
2、完成系统雏形，待进一步完善、调试和解决“一”中列出问题。

9.12-9.18：
1、实现建图、地图保存和加载及重定位基本功能，并使用EuRoC数据集MH_01轨迹建图并保存地图，然后加载地图
使用MH_01,MH_02,MH_03,MH_04,MH_05测试重定位效果，从MH_01数据和其他轨迹重定位视频来看，定位效果不错。

9.19-9.25：
1、完成Frame的保存和加载，并完成测试。

9.26-10.9:
1、更改并调试丢失后重新初始化程序。

10.10-10.16:
1、调试并完成重定位功能;
2、定义通信协议;

10.17-10.23：
1、将rosbuild编译方式更改为catkin方式;

10.24-10.30：
1、调整输入数据格式，等待数据调试;
2、协助写技术交底书。

10.31-11.6：
1、优化代码，完成我们数据集测试;

11.7-11.13：
1、增加与制图板通信接口及测试节点，完成测试;
2、实现使用service切换保存帧、建图和重定位三个功能;

11.14-11.20:
1、给firefly-RK339安装ubuntu 16.04系统和ORB_SLAM2环境
2、完成由笔记本到firefly-RK339(ubuntu 16.04)移植并测试

11.21-11.27:
1、协助激光组测试

11.28-12.04:
1、协助激光组测试

12.05-12.11:
1、查找原因
2、优化初始化和跟踪参数
3、新增无3D点关键帧地图及定位功能

12.12-12.18:
1、优化参数和测试新定位功能

12.19-12.25:
1、在新电脑部署ubuntu 16.04运行环境，解决同时原cartographer和orbslam2兼容性问题及一些琐碎问题，安装相机和激光雷达驱动程序
2、了解和控制turtlebot2

12.26-1.1:
1、学习cartographer源码，了解其参数配置，增加保存tf和优化后轨迹功能，调整参数使用雷神LS01B完成建图
2、在笔记本电脑同时开启相机和激光雷达，实时运行原cartographer和orbslam2得到闭环轨迹用于外参标定
3、使用邸老师外参标定程序完成外参标定
4、在实验室测试算法，建图与定位效果得到改善

1.1-1.8:
1、文献调研
2、优化参数，完成组内测试报告

1.9-1.15:
1、在算法增加保存tf轨迹功能，并使用优化后轨迹进行校正
2、关键帧定位使用PoseArray发布重定位结果
3、优化参数，将重要参数更改为由yaml文件设置，便于调参
4、整理产品文件，向激光组交付
5、重装ubuntu 18.04系统，安装激光组改进的cartographer，与立群沟通完成1.1.0版本部署

1.16-1.22:
1、整理文档
2、写测试脚本和时间戳校正程序，向激光组交付测试报告
3、阅读cartographer源码

1.23-1.29：
1、修改cartographer源码，使用cartographer的Branch and Bound方法筛选和优化视觉重定位pose，完成测试

1.30-2.5:
1、完成新办公室（300平方米）视觉+cartographer分支定界算法测试，并撰写测试报告。