ORB-SLAM稠密点云地图构建（黑白+彩色）

pcl1.8.1
VTK 7.1.1
版本一定要对好，如果安装了不符的版本如我之前安的pcl1.1.3和VTK8.2 一定要卸载干净不然会一直报错
，不同版本的pcl和vtk是无法共存的，并且光把包删除是不够的，要去/usr下面使用命令行（先搜索再一起删掉）

sudorm-r /path/想删除的库

使用高翔老师的源码ORB-SLAM2-modified
运行前要先把数据集图片和深度对齐
先去官网下载associate.py文件 https://vision.in.tum.de/data/datasets/rgbd-dataset/tools
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 fileformat 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()forvin line.split(" ")if v.strip()!=""]forlinein lines if len(line)>0 and line[0]!="#"]
    list =[(float(l[0]),l[1:])forlin 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)forain first_keys 
                         forbin 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])))

然后使用命令行进行对齐

python associate.py PATH_TO_SEQUENCE/rgb.txt PATH_TO_SEQUENCE/depth.txt > associations.txt

运行命令行（注意路径问题 以及文件对应问题）

 ./Examples/RGB-D/rgbd_tum Vocabulary/ORBvoc.txt Examples/RGB-D/TUM1.yaml datasets/rgbd_dataset_freiburg1_xyz  datasets/rgbd_dataset_freiburg1_xyz/association.txt 

运行过程：

保存地图
高博的程序只能实时查看点云地图，不能保存。修改文件 ORB_SLAM2_modified/src/pointcloudmapping.cc，在其中调用 PCL 库的 pcl::io::savePCDFileBinary 函数就可以保存点云地图了。
具体修改如下：
加入头文件

#include <pcl/io/pcd_io.h>

在 void PointCloudMapping::viewer() 函数中（ 123 行附近）加入保存地图的命令，最后样式如下：

...
for( size_t i=lastKeyframeSize; i<N ; i++ ){
    PointCloud::Ptr p = generatePointCloud( keyframes[i], colorImgs[i], depthImgs[i]);
    *globalMap += *p;}
pcl::io::savePCDFileBinary("vslam.pcd", *globalMap);   // 只需要加入这一句
...

生成稠密点云地图后保存查看使用pcl_viewer

安装

sudoapt-getinstall pcl-tools

查看

pcl_viewer vslam.pcd

上述产生的点云地图为黑白的，接下来进行一些修改使其能生成实时的彩色地图。

彩色

1.在ORB_SLAM2_modified/include/Tracking.h添加

    // Current Frame
    Frame mCurrentFrame;
    cv::Mat mImRGB; //添加这行
    cv::Mat mImGray;
    cv::Mat mImDepth;

2.在ORB_SLAM2_modified/src/Tracking.cc修改2处

cv::Mat Tracking::GrabImageRGBD(const cv::Mat &imRGB,const cv::Mat &imD, const double &timestamp){
    mImRGB = imRGB;//添加这行
    mImGray = imRGB;
    mImDepth = imD;

    // insert Key Frame into point cloud viewer
    //mpPointCloudMapping->insertKeyFrame( pKF, this->mImGray, this->mImDepth );
    mpPointCloudMapping->insertKeyFrame( pKF, this->mImRGB, this->mImDepth ); //修改地方