目的是：做一个更快更好用的slam的程序，比如使用更快的特征orb,用TUM数据集，TUM数据集有标准轨迹，会基于外观做会换检测，编程用C++, 而不是C语言，建图用八叉树，会比点云省空间，而且更好用。会做成一个在线的kinect_demo.会用多线程优化，不用等建图结束，就可以定位。
1.下载代码
git clone https://github.com/gaoxiang12/rgbd-slam-tutor2.git
因为opencv,pcl,Eigen3都已经安装过了，所以就不用管了。
2.下载fr1_room数据集（2）找到fr1_room,点击最右边的more info
(3) 点击download部分tgz,就会出现下载链接；
把fr1_room放在rgbd-slam-tutor2文件夹里
3.在fr1_room目录下新建一个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])))
4.用associate.py把fr1_room里的rgb.txt,depth.txt按照时间差不超过0.02的timestamp给匹配起来，把匹配好的序列输出到associate.txt
python associate.py rgb.txt depth.txt > associate.txt
associate.txt的输出格式是1305031910.935211 rgb/1305031910.935211.png 1305031910.935221 depth/1305031910.935221.png
分别是rgb和depth图像的时间戳和对应的rgb图像名称和depth图像名称
5.按照timestamp把associate.txt和groundtruth.txt给匹配起来，输入到associate_with_groundtruth.txt
groundtruth.txt里面是timestamp,tx,ty,tz,qx,qy,qz,qw的信息，这一步其实就像excel里的vlookup一样。
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
6.新建一个drew_groundtruth.py,用图来描述groundtruth.txt所描述的轨迹，代码如下
#!/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()
运行
python draw_groundtruth.py出来
7.安装qtcreator
qtcreator
是一个IDE,说白了，就是可以编c++和cmake,而且不像vim和emacs那么麻烦，虽然我还不知道它具体是怎么编译的。它也有cmake文件。
它有两种模式，一种是debug,可以断点调试，一种是Release,速度比较快。一般是在CMakeList.txt里进行设
置，set(CMAKE_BUILD_TYPE Debug)
，或者把Debug换成Release.它还可以用在ROS上，因为ROS用catkin,也是cmake形式。
安装：
sudo apt-get install qtcreator
使用qtcreator来写一个helloslam，放在experiment文件夹下，这是为了熟悉helloslam
（1）在工程目录下（和src,include等平级），新建一个CMakeLi
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/ )
1.首先选择cmake版本，一般是2.8，另外设定工程项目名称,这些一般可通用，只需要更改一下工程名就可以了
cmake_minimum_required(VERSION 2.8)
project(project_name)
2.设定编码类型，一般就两种，Debug or Release
set(CMAKE_BUILD_TYPE Debug or Release)
3.设置编译选项
set( CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -march=native -O3 -pthread" )
4.find_package( )常见依赖库，一般格式为
find_package(package_name REQUIRED)
5.一些目录问题
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
6.设定可执行程序（即二进制文件）的输出路径在bin里，库输出在lib，这些都可以通用的，但是是不是要先有bin和lib目录啊，先建倒也没事，以后先建这两个目录吧。bin里一般放的都是可执行程序，一般./就可以直接执行了，但有些现在还执行不了，估计是要满足什么条件。
7.设定头文件放在include里。
include_directories(
${PROJECT_SOURCE_DIR}/include
)
include里的文件一般都是其他cpp程序里调用的.h文件8.设定原文件目录
add_subdirectory( ${PROJECT_SOURCE_DIR}/src/ )
add_subdirectory( ${PROJECT_SOURCE_DIR}/experiment/ )
src这个目录一般用来放源代码，一般是一些cpp文件，和一个CMakeLists.txt用来编译这些cpp
experiment一般放一些做实验用的文件,同样是一个CMakeLists.txt和一些cpp文件
（2）在src和experiment下分别新建一个CMakeLists.txt文件，用touch就可以了
touch src/CMakeLists.txt experiment/CMakeLists.txt
（3）修改experiment下的CMakeLists.txt文件，加上一句
add_executable( helloslam helloslam.cpp )
