Direct-stereo-slam开源算法记录

Fast Direct Stereo Visual SLAM

明尼苏达大学，快速准确立体视觉SLAM，不依赖于特征探测与匹配。作者从单目DSO扩展到双目系统，通过3D点最小光度误差优化双目配置尺度。

论文地址：https://arxiv.org/pdf/2112.01890.pdf
源码地址：https://github.com/IRVLab/direct_stereo_slam
相关工作：Direct Sparse Odometry，A Photometrically Calibrated Benchmark For Monocular Visual Odometry，https://github.com/JakobEngel/dso

1.安装DSO

使用作者给的库来安装 https://github.com/JakobEngel/dso

sudo apt install libsuitesparse-dev libeigen3-dev libboost-all-dev libopencv-dev
cd dso
mkdir build && cd build
cmake ..
make -j4

安装完成后可以使用DSO来测试数据集，如图

Run on a dataset from https://vision.in.tum.de/mono-dataset using

bin/dso_dataset \
	files=XXXXX/sequence_XX/images.zip \
	calib=XXXXX/sequence_XX/camera.txt \
	gamma=XXXXX/sequence_XX/pcalib.txt \
	vignette=XXXXX/sequence_XX/vignette.png \
	preset=0 \
	mode=0

还需要配置DSO_PATH,在cmakelists.txt中修改默认DSO路径即可

2.kitti.launch配置

1.首先下载kitti vo数据集，拆分后使用该链接的python文件将image转成rosbag
https://github.com/JotellyBarros/image2rosbag_KITTIodometry

2.使用rosbageditor修改rosbag info
链接 https://github.com/facontidavide/rosbag_editor

3.在kitti.launch中修改bag路径

使用roslaunch direct_stereo_slam kitti.launch 测试kitti数据集
如果出现以下错误，还需要修改对应的camera0和camera1.txt中图像的大小

3.修改源码使输出的真值格式为tum格式

tum格式为:timestamp,tx,ty,tz,qx,qy,qz,qw

在代码的src/loop_closure的LoopHandler.cpp中将dslam_file的输出结构改为

// lf->tfm_w_c是g2o::SE3Quat 使用tovector输出tx,ty,tz,qx,qy,qz,qw
auto r_wc = lf->tfm_w_c.toVector();
    dslam_file << lf->incoming_id << " ";
    dslam_file << r_wc[0] << " " << r_wc[1] << " " << r_wc[2] << " " << r_wc[3] << " "
    << r_wc[4] << " " << r_wc[5] << " " << r_wc[6] << std::endl;

incoming_id对应真值时间戳的序号
使用python修改下dslam.txt即可

import os,sys

def id2times(args):
    # print(args[0],args[1])
    tumfile = open(args[0],"r")
    timesfile = open(args[1],"r")
    result = open("id2times_result.txt","w")
    timeslist = []
    for line in timesfile.readlines():
        line = line.strip()
        timeslist.append(line)
    for line in tumfile.readlines():
        vector = line.split(" ")
        # print(vector[0])
        vector[0] = timeslist[int(vector[0])]
        # str=vector[0].append(" ")
        for i in range(7):
            vector[i] = ''.join([vector[i],' '])
        result.writelines(vector)
    tumfile.close()
    timesfile.close()
    result.close()

if __name__ == "__main__":
    if len( sys.argv ) == 3:
        id2times(sys.argv[1:])
    else:
        print( "Usage: python id2times.py tumfilename timesfilename")