ORB-SLAM稠密点云地图构建（黑白+彩色）+ pcd文件以八叉树形式表示

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

sudo rm -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 numpydef 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 nameOutput: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) tuplessecond_list -- second dictionary of (stamp,data) tuplesoffset -- time offset between both dictionaries (e.g., to model the delay between the sensors)max_difference -- search radius for candidate generationOutput: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 matchesif __name__ == '__main__':# parse command lineparser = 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

安装

sudo apt-get install pcl-tools

查看

pcl_viewer vslam.pcd

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

彩色

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

    // Current FrameFrame 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 ); //修改地方

pcd文件以八叉树表示参考这篇教程https://blog.csdn.net/weixin_47185604/article/details/123487506?spm=1001.2014.3001.5506注意命名和文件位置即可