Ubuntu20.04配置Kinect 2.0驱动安装和ROS环境下配置以及录制bag包和制作ORB-SLAM数据集

[1. 安装libfreenect2](#1. 安装libfreenect2)
- [1.1 下载官方文件](#1.1 下载官方文件)
- [1.2 安装build工具](#1.2 安装build工具)
- [1.3 安装libusb](#1.3 安装libusb)
- [1.4 安装urboJPEG](#1.4 安装urboJPEG)
- [1.5 安装OpenGL](#1.5 安装OpenGL)
- [1.6 安装OpenCL](#1.6 安装OpenCL)
- [1.7 安装OpenNI](#1.7 安装OpenNI)
- [1.8 进入libfreenect2 文件夹，编译安装](#1.8 进入libfreenect2 文件夹，编译安装)
- [1.9 设定udev rules](#1.9 设定udev rules)
- [1.10 测试](#1.10 测试)
[2. 配置ROS环境](#2. 配置ROS环境)
- [2.1 下载iai_kinect2包并安装](#2.1 下载iai_kinect2包并安装)
- [2.2 相机上电，测试](#2.2 相机上电，测试)
[3 录制bag包和制作ORB-SLAM2数据集](#3 录制bag包和制作ORB-SLAM2数据集)
- [3.1 启动roscore，并启动相机](#3.1 启动roscore，并启动相机)
- [3.2 录制](#3.2 录制)
- [3.3 提取录制的rosbag包内的rgb和depth](#3.3 提取录制的rosbag包内的rgb和depth)
- [3.4. 生成rgb.txt和depth.txt](#3.4. 生成rgb.txt和depth.txt)
- [3.5 associate.py将rgb.txt和depth.txt进行匹配](#3.5 associate.py将rgb.txt和depth.txt进行匹配)

1. 安装libfreenect2

1.1 下载官方文件

bash 复制代码

git clone https://github.com/OpenKinect/libfreenect2.git
cd libfreenect2

1.2 安装build工具

bash 复制代码

sudo apt-get install build-essential cmake pkg-config

1.3 安装libusb

bash 复制代码

sudo apt-get install libusb-1.0-0-dev

1.4 安装urboJPEG

bash 复制代码

sudo apt-get install libturbojpeg0-dev

1.5 安装OpenGL

bash 复制代码

sudo apt-get install libglfw3-dev

1.6 安装OpenCL

bash 复制代码

sudo apt-get install beignet-dev

1.7 安装OpenNI

bash 复制代码

sudo apt-get install libopenni2-dev

1.8 进入libfreenect2 文件夹，编译安装

bash 复制代码

cd libfreenect2
mkdir build && cd build
cmake ..
make
make install

注：在对libfreenect2进行make时报错：

bash 复制代码

fatal error: helper_math.h: No such file or directory

错误原因：

11.6版本之前的CUDA安装时会附带安装CUDA Samples，helper_math.h文件在/xxxx/NVIDIA_CUDA-10.1_Samples/common/inc/helper_math.h位置；11.6版本之后不再附带Samples，从而找不到该文件。
解决方法：

手动下载Samples

1.1. 从github下载Samples库，选择与那cuda同版本的sample库

https://github.com/NVIDIA/cuda-samples

1.2. 在cmake之前添加相应路径到CPATH
cd <sample_dir>
make

1.3 将help_math.h复制到那报错的路径下
cp /home/kunyuwan/cuda-samples/Common/helper_math.h [Where the error occurs]

1.9 设定udev rules

bash 复制代码

sudo cp /home/kunyuwan/libfreenect2/platform/linux/udev/90-kinect2.rules /etc/udev/rules.d/

1.10 测试

bash 复制代码

./bin/Protonect

不出意外，看到以下界面

错误内容：在运行./bin/Protonect可能遇到的问题

解决方法

修改~./bashrc

添加一行在文件里面：export MESA_GL_VERSION_OVERRIDE=3.3

之后执行 soure ~/.bashrc即可

2. 配置ROS环境

2.1 下载iai_kinect2包并安装

bash 复制代码

cd ~/catkin_ws/src/
git clone https://github.com/code-iai/iai_kinect2.git
cd iai_kinect2
rosdep install -r --from-paths .
cd ~/catkin_ws
catkin_make

编译过程可能出现以下的问题
（1）CXX14 error：

解决方法

在kinect2_viewer文件夹下的CMakeLists.txt中，

在开头，把CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)

改成CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14)

后面的一句也改为SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14")
（2）OpenCV各种宏定义找不到：

如果按照的是OpenCV4的版本，可能会出现各种宏定义找不到的问题
解决方法：把所有include了opencv的cpp文件都添加了同样的库文件

kinect2_bridge/src/kinect2_bridge.cpp

kinect2_calibration/src/kinect2_calibration.cpp

kinect2_viewer/src/viewer.cpp

在这三个文件中添加以下头文件
#include <opencv2/imgproc/types_c.h>
#include <opencv2/imgproc/imgproc_c.h>
#include <opencv2/imgcodecs/legacy/constants_c.h>
#include <opencv2/highgui/highgui.hpp>
(3)/home/mh/catkin_ws/devel/lib//libkinect2_bridge_nodelet.so: undefined symbol: _ZN2cv3MatC1Ev

报错全部内容为：

bash 复制代码

/home/mh/catkin_ws/devel/lib//libkinect2_bridge_nodelet.so: undefined symbol: _ZN2cv3MatC1Ev
[FATAL] [1712663692.831371988]: Failed to load nodelet '/kinect2_points_xyzrgb_sd` of type `depth_image_proc/point_cloud_xyzrgb` to manager `kinect2'
[FATAL] [1712663692.831424141]: Failed to load nodelet '/kinect2_bridge` of type `kinect2_bridge/kinect2_bridge_nodelet` to manager `kinect2'
[kinect2-1] process has died [pid 7561, exit code 127, cmd /opt/ros/melodic/lib/nodelet/nodelet manager __name:=kinect2 __log:=/home/mh/.ros/log/1b531282-f666-11ee-bc63-088fc3ed4c91/kinect2-1.log].
log file: /home/mh/.ros/log/1b531282-f666-11ee-bc63-088fc3ed4c91/kinect2-1*.log
[kinect2_bridge-2] process has died [pid 7562, exit code 255, cmd /opt/ros/melodic/lib/nodelet/nodelet load kinect2_bridge/kinect2_bridge_nodelet kinect2 __name:=kinect2_bridge __log:=/home/mh/.ros/log/1b531282-f666-11ee-bc63-088fc3ed4c91/kinect2_bridge-2.log].
log file: /home/mh/.ros/log/1b531282-f666-11ee-bc63-088fc3ed4c91/kinect2_bridge-2*.log
[kinect2_points_xyzrgb_sd-3] process has died [pid 7563, exit code 255, cmd /opt/ros/melodic/lib/nodelet/nodelet load depth_image_proc/point_cloud_xyzrgb kinect2 rgb/camera_info:=kinect2/sd/camera_info rgb/image_rect_color:=kinect2/sd/image_color_rect depth_registered/image_rect:=kinect2/sd/image_depth_rect depth_registered/points:=kinect2/sd/points __name:=kinect2_points_xyzrgb_sd __log:=/home/mh/.ros/log/1b531282-f666-11ee-bc63-088fc3ed4c91/kinect2_points_xyzrgb_sd-3.log].
log file: /home/mh/.ros/log/1b531282-f666-11ee-bc63-088fc3ed4c91/kinect2_points_xyzrgb_sd-3*.log

解决方法：

导致该问题的原因可能是电脑中存在多个OpenCV的版本，使用find无法找到OpenCV的路径，可以在iai_kinect2文件里面的所以cmakelist里面指定OpenCV的绝对路径，从而可以直接找到OpenCV

2.2 相机上电，测试

打开一个新的终端测试话题订阅

bash 复制代码

cd ~/catkin_ws
source devel/setup.bash
roslaunch kinect2_bridge kinect2_bridge.launch

打开一个新的终端查看实时画面。

bash 复制代码

rqt

输入rostopic list可以查看订阅的对应话题

此时再打开一个新的终端，运行以下命令：

bash 复制代码

cd ~/catkin_ws
source devel/setup.bash
rosrun kinect2_viewer kinect2_viewer kinect2 sd cloud

显示点云相机窗口，则代表启动成功!!!

3 录制bag包和制作ORB-SLAM2数据集

3.1 启动roscore，并启动相机

打开两个终端，分别执行以下命令

bash 复制代码

roscore

bash 复制代码

cd ~/catkin_ws
source devel/setup.bash
roslaunch kinect2_bridge kinect2_bridge.launch

3.2 录制

在打开一个终端

bash 复制代码

rosbag record  -o xxx.bag /kinect2/qhd/image_color_rect /kinect2/qhd/image_depth_rect

3.3 提取录制的rosbag包内的rgb和depth

注意修改路径和订阅的话题！！！

python 复制代码

# coding:utf-8
#!/usr/bin/python
 
# Extract images from a bag file.
 
#PKG = 'beginner_tutorials'
import roslib;   #roslib.load_manifest(PKG)
import rosbag
import rospy
import cv2
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
from cv_bridge import CvBridgeError
 
# Reading bag filename from command line or roslaunch parameter.
#import os
#import sys
 
rgb_path = '/home/kunyuwan/test_kinectv2_3/rgb/'    # 已经建立好的存储rgb彩色图文件的目录（最好改成绝对路径）
depth_path= '/home/kunyuwan/test_kinectv2_3/depth/' # 已经建立好的存储深度图文件的目录（最好改成绝对路径）
 
class ImageCreator():
 
 
    def __init__(self):
        self.bridge = CvBridge()
        with rosbag.Bag('/home/kunyuwan/test_kinectv2_3/4_2024-04-03-21-40-12.bag', 'r') as bag:  #要读取的bag文件（最好改成绝对路径）；
            for topic,msg,t in bag.read_messages(): 
                if topic == "/kinect2/qhd/image_color_rect": #图像的topic 注意不要错了；
                        try:
                            cv_image = self.bridge.imgmsg_to_cv2(msg,"bgr8")
                        except CvBridgeError as e:
                            print(e)
                        timestr = "%.6f" %  msg.header.stamp.to_sec()
                        #%.6f表示小数点后带有6位，可根据精确度需要修改；
                        image_name = timestr+ ".png" #图像命名：时间戳.png
                        cv2.imwrite(rgb_path + image_name, cv_image)  #保存；

                elif topic == "/kinect2/qhd/image_depth_rect": #图像的topic；
                        try:
                            cv_image = self.bridge.imgmsg_to_cv2(msg,"16UC1")
                        except CvBridgeError as e:
                            print(e)
                        timestr = "%.6f" %  msg.header.stamp.to_sec()
                        #%.6f表示小数点后带有6位，可根据精确度需要修改；
                        image_name = timestr+ ".png" #图像命名：时间戳.png
                        cv2.imwrite(depth_path + image_name, cv_image)  #保存；
 
if __name__ == '__main__':
 
    #rospy.init_node(PKG)
 
    try:
        image_creator = ImageCreator()
    except rospy.ROSInterruptException:
        pass

3.4. 生成rgb.txt和depth.txt

cpp 复制代码

#include <iostream>
#include <fmt/os.h>
#include <fmt/core.h>
#include <fmt/format.h>
#include <string>
#include <vector>
#include <algorithm>
extern "C"
{
#include <sys/types.h>
#include <sys/dir.h>
#include <dirent.h>
}
using namespace std;

void getNamesToText(const char *filepath,const char *filetype)
{
    DIR *streamp = nullptr;
    dirent *dep = nullptr;

    string filename(filepath);
    auto out = fmt::output_file("./"+string(filetype));

    vector<string> names;
    streamp = opendir(filepath);
    errno = 0;
    while ((dep = readdir(streamp)) != NULL)
    {
        if ((dep->d_name == ".") || (dep->d_name == ".."))
        {
            continue;
        }
        names.push_back(dep->d_name);
    }

    if (errno != 0)
    {
        fmt::print("error");
    }
    closedir(streamp);
    for (vector<string>::iterator ite = names.begin(); ite != names.end();)
    {
        if ((*ite == ".") || (*ite == ".."))
        {
            ite = names.erase(ite); 
        }
        else
        {
            ++ite;
        }
    }

    sort(names.begin(), names.end());
    for (auto &item : names)
    {
        auto pos = item.find_last_of('.');
        string filenum;
        for (int i = 0; i < pos; i++)
        {
            filenum += item[i];
        }
        if(strcmp(filetype,"rgb.txt")==0)
        {
            out.print("{} rgb/{}\n",filenum,item);
        }
        else if(strcmp(filetype,"depth.txt")==0)
        {
            out.print("{} depth/{}\n",filenum,item);
        }
        
        
       
    }
    fmt::print("{} finished\n",filetype);
}

int main(int argc, char *argv[])
{
    if(argc<3)
    {
        fmt::print("Usage: [executable] [rgb/depth path] [type]");
        return 0;
    }
    const char *filepath =argv[1];
    const char *filetype =argv[2];
    getNamesToText(filepath,filetype);

    
    return 0;
}

注意安装fmt

bash 复制代码

git clone  https://github.com/fmtlib/fmt.git
cd fmt
mkdir build
cd build
cmake ..
make
sudo make install

运行命令

bash 复制代码

g++ 文件名.cpp -lfmt

bash 复制代码

./a.out rgb rgb.txt # 文件夹路径和生成文件路径
./a.out depth depth.txt # 文件夹路径和生成文件路径

3.5 associate.py将rgb.txt和depth.txt进行匹配

python 复制代码

#!/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 = list(first_list)
    second_keys = list(second_list)
    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])))

运行指令

bash 复制代码

chmod 777 associate.py
./associate.py rgb.txt depth.txt > associate.txt

最终得到的文件夹内部的结构如图即可：