一塊兒作RGB-D SLAM 第二季 （一）

時間  2019-12-07
標籤 一塊兒 rgb slam 第二 欄目 CSS 简体版
原文   原文鏈接

　　小蘿蔔：師兄！過年啦！是否是很無聊啊！普通人的生活就是賺錢花錢，實在是很沒意思啊！python

　　師兄：是啊……linux

　　小蘿蔔：他們都不懂搞科研和碼代碼的樂趣呀！ios

　　師兄：可不是嘛……c++

　　小蘿蔔：因此今年過年，咱們再作一個SLAM吧！以前寫的那個太爛了啦，我都很差意思說是我作的了！git

　　師兄：嗯那可真是對不住你啊……github

　　小蘿蔔：沒事！你再寫一個好一點的，我就原諒你了！寫完再請我吃飯吧！算法

　　師兄：啊，好的……編程

　　小蘿蔔：師兄你別這麼沒精神啊！加油咯！ubuntu

 前言

　　在通過了一番激烈的思想鬥爭以後呢，師兄厭倦了年假的無聊生活，開始寫《一塊兒作RGBD SLAM》的第二季！在這一系列中，咱們會討論RGBD SLAM程序中一些更深刻的話題，從而編寫一個更快、更好用的程序。改進的地方大體以下：vim

  • 多線程的優化：在建圖算法計算時，定位算法不必等待它結束。它們能夠並行運行。
  • 更好地跟蹤：選取參考幀，並對丟失狀況進行處理；
  • 基於外觀的迴環檢測：Appearance based loop closure；
  • 八叉樹建圖：Octomap；
  • 使用更快的特徵：Orb；
  • 使用TUM數據集，並與標準軌跡進行比較；
  • 在線的Kinect demo；
  • 代碼會寫得更像c++風格，而不是像上次的c風格；

　　這麼一看，其實總體上問題仍是挺多的。在第二季中，咱們將致力於解決這些問題，同時咱們的程序也會變得相對比較複雜。鑑於不少基礎的問題咱們在第一季中已經提過，本次我就不講怎麼安裝OpenCV之類的事情啦。可是，爲了保證你們能理解博客內容，咱們和以往同樣，給出實現過程當中的全部代碼和數據。

　　代碼請參見：https://github.com/gaoxiang12/rgbd-slam-tutor2

　　TUM數據集網址：http://vision.in.tum.de/data/datasets/rgbd-dataset

　　本系列使用TUM中的一個數據：fr1_room。讀者能夠去TUM網站找，或者直接從個人百度雲裏下載： http://pan.baidu.com/s/1c1fviSS

　　TUM數據集的使用方法咱們將在後文介紹。

關於代碼

　　第二季中，咱們仍使用C++和Cmake做爲編程語言和框架。我使用的電腦是 Ubuntu 14.04 系統。讀者也能夠自行挑選其餘linux操做系統，可是我只給出在ubuntu下安裝各類工具的方式。

　　首先，請從github中下載這個系列用到的代碼：

1 git clone https://github.com/gaoxiang12/rgbd-slam-tutor2.git

 　　你會看到幾個文件夾。和第一個系列同樣，咱們把不一樣的代碼歸類放置。幾個文件夾的內容以下：

  • bin　　　　存放編譯好的可執行文件；
  • src　　　　存放源代碼；
  • include　　存放頭文件；
  • experiment　　存放一些作實驗與測試用的源文件；
  • config　　存放配置文件；
  • lib　　　　存放編譯好的庫文件；
  • Thirdparty　　一些小型的依賴庫，例如g2o，dbow2，octomap等；

　　第一講的代碼尚未那麼全。隨着講解的進行，咱們會逐步將代碼添加到各個文件夾中去。

　　咱們構建代碼的思路是這樣的。把與slam相關的代碼（include和src下）編譯成一個庫，把測試用的程序（experiment下）編譯成可執行文件，並連接到這個slam庫上。舉例來講，咱們會把orb特徵的提取和匹配代碼放到庫中，而後在experiment裏寫一個程序，讀一些具體的圖片並提取orb特徵。之後咱們也將用這個方式來編寫回環檢測等模塊。

　　至於爲什麼要放Thirdparty呢？由於像g2o這樣的庫，版本有時會發生變化。因此咱們就把它直接放到代碼目錄裏，而不是讓讀者本身去找g2o的源碼，這樣就能夠保證咱們的代碼在讀者的電腦上也能順利編譯。可是像 opencv，pcl 這些大型又較穩定的庫，咱們就交給讀者自行編譯安裝了。

　　除了Thirdparty下的庫，請讀者自行安裝這樣依賴庫：

  • OpenCV 2.4.11　　請往opencv.org下載，注意咱們沒有使用3.1版本，而opencv2系列和3系列在接口上有較大差別。若是你用ubuntu，能夠經過軟件倉庫來安裝opencv： 
    sudo apt-get install libopencv-dev
  • PCL　1.7　　　　   來自pointclouds.org。
  • Eigen3　　　　　　安裝 sudo apt-get install libeigen3-dev

　　Thirdparty下的庫，多爲cmake工程，因此按照一般的cmake編譯方式便可安裝。它們的依賴基本能夠在ubuntu的軟件倉庫中找到， 咱們會在用到時再加以介紹。

關於TUM數據集

　　本次咱們使用tum提供的數據集。tum的數據集帶有標準的軌跡和一些比較工具，更適合用來研究。同時，相比於nyud數據集，它也要更加困難一些。使用這個數據集時應當注意它的存儲格式（固然使用任何數據集都應當注意）。

　　下面咱們以fr1_room爲例來講明TUM數據集的用法。fr1_room的下載方式見上面的百度雲或者TUM官網。

　　下載咱們提供的 「rgbd_dataset_freiburg1_room.tgz」至任意目錄，解壓後像這樣：

　　rgb和depth文件夾下存放着彩色圖和深度圖。圖像的文件名是以採集時間命名的。而rgb.txt和depth.txt則存儲了全部圖像的採集時間和文件名稱，例如：

　　1305031910.765238 rgb/1305031910.765238.png

　　表示在機器時間1305031910.765238採集了一張RGB圖像，存放於rgb/1305031910.765238.png中。

　　這種存儲方式的一個特色是，沒有直接的rgb-depth一一對應關係。因爲採集時間的差別，幾乎沒有兩張圖像是同一個時刻採集的。然而，咱們在處理圖像時，須要把一個RGB和一個depth當成一對來處理。因此，咱們須要一步預處理，找到rgb和depth圖像的一一對應關係。

　　TUM爲咱們提供了一個工具來作這件事，詳細的說明請看：http://vision.in.tum.de/data/datasets/rgbd-dataset/tools 該網頁整理了一些經常使用工具，包括時間配對，ground-truth偏差比對、圖像到點雲的轉換等。對於如今預處理這一步，咱們須要的是一個 associate.py 文件，以下（你能夠直接把內容拷下來，存成本地的associate.py文件）：

#!/usr/bin/python
# Software License Agreement (BSD License)
#
# Copyright (c) 2013, Juergen Sturm, TUM
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
#  * Redistributions of source code must retain the above copyright
#    notice, this list of conditions and the following disclaimer.
#  * Redistributions in binary form must reproduce the above
#    copyright notice, this list of conditions and the following
#    disclaimer in the documentation and/or other materials provided
#    with the distribution.
#  * Neither the name of TUM nor the names of its
#    contributors may be used to endorse or promote products derived
#    from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# Requirements: 
# sudo apt-get install python-argparse

"""
The Kinect provides the color and depth images in an un-synchronized way. This means that the set of time stamps from the color images do not intersect with those of the depth images. Therefore, we need some way of associating color images to depth images.

For this purpose, you can use the ''associate.py'' script. It reads the time stamps from the rgb.txt file and the depth.txt file, and joins them by finding the best matches.
"""

import argparse
import sys
import os
import numpy


def read_file_list(filename):
    """
    Reads a trajectory from a text file. 
    
    File format:
    The file format is "stamp d1 d2 d3 ...", where stamp denotes the time stamp (to be matched)
    and "d1 d2 d3.." is arbitary data (e.g., a 3D position and 3D orientation) associated to this timestamp. 
    
    Input:
    filename -- File name
    
    Output:
    dict -- dictionary of (stamp,data) tuples
    
    """
    file = open(filename)
    data = file.read()
    lines = data.replace(","," ").replace("\t"," ").split("\n") 
    list = [[v.strip() for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"]
    list = [(float(l[0]),l[1:]) for l in list if len(l)>1]
    return dict(list)

def associate(first_list, second_list,offset,max_difference):
    """
    Associate two dictionaries of (stamp,data). As the time stamps never match exactly, we aim 
    to find the closest match for every input tuple.
    
    Input:
    first_list -- first dictionary of (stamp,data) tuples
    second_list -- second dictionary of (stamp,data) tuples
    offset -- time offset between both dictionaries (e.g., to model the delay between the sensors)
    max_difference -- search radius for candidate generation

    Output:
    matches -- list of matched tuples ((stamp1,data1),(stamp2,data2))
    
    """
    first_keys = first_list.keys()
    second_keys = second_list.keys()
    potential_matches = [(abs(a - (b + offset)), a, b) 
                         for a in first_keys 
                         for b in second_keys 
                         if abs(a - (b + offset)) < max_difference]
    potential_matches.sort()
    matches = []
    for diff, a, b in potential_matches:
        if a in first_keys and b in second_keys:
            first_keys.remove(a)
            second_keys.remove(b)
            matches.append((a, b))
    
    matches.sort()
    return matches

if __name__ == '__main__':
    
    # parse command line
    parser = argparse.ArgumentParser(description='''
    This script takes two data files with timestamps and associates them   
    ''')
    parser.add_argument('first_file', help='first text file (format: timestamp data)')
    parser.add_argument('second_file', help='second text file (format: timestamp data)')
    parser.add_argument('--first_only', help='only output associated lines from first file', action='store_true')
    parser.add_argument('--offset', help='time offset added to the timestamps of the second file (default: 0.0)',default=0.0)
    parser.add_argument('--max_difference', help='maximally allowed time difference for matching entries (default: 0.02)',default=0.02)
    args = parser.parse_args()

    first_list = read_file_list(args.first_file)
    second_list = read_file_list(args.second_file)

    matches = associate(first_list, second_list,float(args.offset),float(args.max_difference))    

    if args.first_only:
        for a,b in matches:
            print("%f %s"%(a," ".join(first_list[a])))
    else:
        for a,b in matches:
            print("%f %s %f %s"%(a," ".join(first_list[a]),b-float(args.offset)," ".join(second_list[b])))
associate.py

 　　小蘿蔔：那麼這個文件要怎麼用呢？

　　若是讀者熟悉python，就很容易看懂它的用法。實際上，只要給它兩個文件名便可，它會輸出一個匹配好的序列，像這樣：

python associate.py rgb.txt depth.txt

 　　輸出則是一行一行的數據，如：

　　1305031955.536891 rgb/1305031955.536891.png 1305031955.552015 depth/1305031955.552015.png

　　小蘿蔔：我知道！這一行就是配對好的RGB圖和深度圖了，對吧！

　　師兄：對！程序默認時間差在0.02內的就能夠當成一對圖像。爲了保存這個結果，咱們能夠把它輸出到一個文件中去，如：

python associate.py rgb.txt depth.txt > associate.txt

 　　這樣，只要有了這個associate.txt文件，咱們就能夠找到一對對的RGB和彩色圖啦！

　　小蘿蔔：配對配對什麼的，總以爲像在相親啊……

關於ground truth

　　ground truth是TUM數據集提供的標準軌跡，它是由一個外部的（很高級的）運動捕捉裝置測量的，基本上你能夠把它當成一個標準答案嘍！ground truth的記錄格式也和前面相似，像這樣：

　　1305031907.2496 -0.0730 -0.4169 1.5916 0.8772 -0.1170 0.0666 -0.4608

　　各個數據分別是：時間，位置（x,y,z），姿態四元數（qx, qy, qz, qw），對四元數不熟悉的同窗能夠看看「數學基礎」那幾篇博客。那麼這個軌跡長什麼樣呢？咱們寫個小腳原本畫個圖看看：

#!/usr/bin/env python
# coding=utf-8

import numpy as np
import matplotlib.pyplot as plt
import mpl_toolkits.mplot3d

f = open("./groundtruth.txt")
x = []
y = []
z = []
for line in f:
    if line[0] == '#':
        continue
    data = line.split()
    x.append( float(data[1] ) )
    y.append( float(data[2] ) )
    z.append( float(data[3] ) )
ax = plt.subplot( 111, projection='3d')
ax.plot(x,y,z)
plt.show()

 　　把這部分代碼複製存儲成draw_groundtruth.py存放到數據目錄中，再運行：

python draw_groundtruth.py

 　　就能看到軌跡的形狀啦：

　　第二件事，由於外部那個運動捕捉裝置的記錄頻率比較高，獲得的軌跡點也比圖像密集不少，如何查找每一個圖像的真實位置呢？

　　還記得associate.py不？咱們能夠用一樣的方式來匹配associate.txt和groundtruth.txt中的時間信息哦：

python associate.py associate.txt groundtruth.txt > associate_with_groundtruth.txt

 　　這時，咱們的新文件 associate_with_groundtruth.txt 中就含有每一個幀的位姿信息了：

　　1305031910.765238 rgb/1305031910.765238.png 1305031910.771502 depth/1305031910.771502.png 1305031910.769500 -0.8683 0.6026 1.5627 0.8219 -0.3912 0.1615 -0.3811

　　是否是很方便呢？對於TUM中其餘的序列也能夠一樣處理。

關於TUM中的相機

　　TUM數據集一共用了三個機器人，記成fr1, fr2, fr3。這三臺相機的參數在這裏： http://vision.in.tum.de/data/datasets/rgbd-dataset/file_formats#intrinsic_camera_calibration_of_the_kinect

　　數據當中，深度圖已經根據內參向RGB做了調整。因此相機內參以RGB爲主：

Camera fx fy cx cy d0 d1 d2 d3 d4
(ROS default) 525.0 525.0 319.5 239.5 0.0 0.0 0.0 0.0 0.0
Freiburg 1 RGB 517.3 516.5 318.6 255.3 0.2624 -0.9531 -0.0054 0.0026 1.1633
Freiburg 2 RGB 520.9 521.0 325.1 249.7 0.2312 -0.7849 -0.0033 -0.0001 0.9172
Freiburg 3 RGB 535.4 539.2 320.1 247.6 0 0 0 0 0

　　深度相機的scale爲5000（和kinect默認的1000是不一樣的）。也就是depth/中圖像像素值5000爲真實世界中的一米。

　　所以，你下載了哪一個序列，就要用對應的內參哦！

挑選一個IDE

　　如今讓咱們來寫第一部分代碼：讀取tum數據集並以視頻的方式顯示出來。

　　嗯，在寫代碼以前呢，師兄還有一些話要囉嗦。雖然咱們用linux的同窗以會用vim和emacs爲傲，可是寫代碼呢，仍是但願有一個IDE能夠用的。vim和emacs的編輯確實很方便，然而寫c++，你還須要在類定義/聲明裏跳轉，須要補全和提示。要讓vim和emacs來作這種事，不是不能夠，可是極其麻煩。此次師兄給你們推薦一個能夠用於c++和cmake的IDE，叫作qtcreator。

　　安裝qtcreator：

sudo apt-get install qtcreator

 　　界面大概長這樣：

　　這東西直接的好處是支持cmake。只要是cmake工程就能夠丟進去編譯。按住ctrl鍵能夠在各個類定義/變量/實現之間快速導航。若是你的cmake設置成了debug模式，它還能進行斷點調試，十分的好用！

　　此外，因爲ROS使用的catkin也是cmake的形式，因此它還能用來調試ROS程序！

　　固然，由於叫qtcreator，天然還能寫qt的程序……然而這彷佛已經不重要了……具體配置請你們自行摸索啦！

 使用qtcreator寫一個hello slam

　　這件事情其實很簡單的嘍！

　　首先，隨便找一個文件夾，做爲你代碼的根目錄。在此目錄下新建一個CMakeLists.txt，輸入這些內容：

cmake_minimum_required( VERSION 2.8 )
project( rgbd-slam-tutor2 )

# 設置用debug仍是release模式。debug容許斷點，而release更快
#set( CMAKE_BUILD_TYPE Debug )
set( CMAKE_BUILD_TYPE Release )

# 設置編譯選項
# 容許c++11標準、O3優化、多線程。match選項可避免一些cpu上的問題
set( CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -march=native -O3 -pthread" )

# 常見依賴庫：cv, eigen, pcl
find_package( OpenCV REQUIRED )
find_package( Eigen3 REQUIRED )
find_package( PCL 1.7 REQUIRED )

include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})

# 二進制文件輸出到bin
set( EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin )
# 庫輸出到lib
set( CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib )

# 頭文件目錄
include_directories(
    ${PROJECT_SOURCE_DIR}/include
    )

# 源文件目錄
add_subdirectory( ${PROJECT_SOURCE_DIR}/src/ )
add_subdirectory( ${PROJECT_SOURCE_DIR}/experiment/ )

 　　重要部分已經加上註釋。

　　而後，在src/和experiment/下也新建兩個CMakeLists.txt，暫不填寫內容：

touch src/CMakeLists.txt experiment/CMakeLists.txt

 　　下面，用qtcreator菜單中的File->Open file or project，打開剛纔寫的CMakeLists.txt，它會識別出這是個cmake工程，並提示你要在何出構建。一般咱們是新建一個build文件夾來構建的，因此此次也這麼作好了：

　　

　　這樣就設置好啦。這時，點擊左側的小錘或按下Ctrl+B，就能夠構建工程。可是因爲如今工程是空的，並無什麼能夠構建的。因此咱們加一個helloslam試試。在experiment下新建一個helloslam.cpp文件，輸入：

1 #include<iostream>
2 using namespace std;
3 
4 int main()
5 {
6     cout<<"Hello SLAM!"<<endl;
7     return 0;
8 }

 　　而後，修改experiment/CMakeLists.txt文件，告訴它咱們要編譯這個文件：

add_executable( helloslam helloslam.cpp )

 　　而後，按下Ctrl+B，完成構建。此時會出現一個小綠條，提示你構建完畢。最後，點擊左下綠色的三角按鈕，運行此程序：

　　怎麼樣，是否是很輕鬆？

　　讀者能夠嘗試按住Ctrl並點擊變量，看看qtcreator是如何跳轉的。或者人爲加一句錯誤代碼，看它會不會提示錯誤。也能夠輸入 cout. 看它會提示哪些東西。甚至能夠調成Debug模式，設置斷點，看程序是否會停在斷點上。

下期預告

　　下期咱們會講基本的IO操做，包括參數文件的讀取，TUM圖像讀取與顯示，以及程序的測速等等。

問題

　　1. draw_groundtruth.py 跑不起來？

sudo apt-get install python-matplotlib python-numpy

 　　再試試。

 　　2.爲何個人qtcreator是白的？

　　黑色只是個配色，在Tools/optoins中進行修改。其實白的也挺好看的。

相關文章
相關標籤/搜索
每日一句
    每一个你不满意的现在，都有一个你没有努力的曾经。
本站公眾號
   歡迎關注本站公眾號,獲取更多信息
聯繫我們 最近搜索 最新文章 沪ICP备13005482号-10 MyBatis教程 SQL 教程 MySQL教程 Java 教程 Thymeleaf 教程 Hibernate教程 Spring教程 Redis教程