ROS下获取kinectv2相机的仿照TUM数据集格式的彩色图和深度图

准备工作：　１．　ubuntu16.04上安装iai-kinect2, 2. 运行roslaunch kinect2_bridge kinect2_bridge.launch, 3. 运行　rosrun save_rgbd_from_kinect2 save_rgbd_from_kinect2,开始保存图像．

这个保存kinectV2相机的代码如下，完整的工程可以从我的github上下载　https://github.com/Serena2018/save_color_depth_from_kinect2_with_ros/tree/master/save_rgbd_from_kinect2

问题：我使用这个第一个版本的工具获取了rgb和depth图像，并使用TUM数据集提供的associate.py脚本（脚本内容在文章底部）得到彩色图和深度图的对应（在这个工作中我才意识到，深度相机获取的深度图和彩色图实时一一对应的，不信的话，你去看一下TUM数据集）．

经过上面的工作，我感觉我获取的数据集是天衣无缝的，知道今天我用我的数据集跑一下orb-slam的RGB-D接口，发现一个大问题，跟踪过程，不连贯，出现回跳的问题，就是，比如说场景中有个人，头一段时间，这个人已经走过去了，可是下一会，这个人又退回来了．

出了这样的问题，我就开始排查问题，先从获取的原始数据开始，我播放查看图像，发现图像是平滑变换的，不存在来回跳转的问题，（彩色图和深度图都没问题）

然后排查是不是associate.py脚本的问题，那么我就使用这个脚本，作用到TUM数据集，得到相应的association.txt文件，然后在orb-slam的RGBD接口测试该组数据，发现没有会跳的问题出现，那么，就不是这个脚本的问题，

我发现现在我的数据集和TUM数据集的区别是，我的保存下来的图像的时间戳的小数部分，不能保证所有的小数部分都有６位，就是说当小数部分后几位为０时，那么就直接略去了，会出现小数部分小于６位的情况，既然我可以想到的其他变量都是相同的，那么我看一下，如果我也能做到保证小数部分都是６位，那么这个问题也许就解决了，我就将原来的代码

os_rgb << time_val.tv_sec << "." <<time_val.tv_usec;
os_dep << time_val.tv_sec << "."<<time_val.tv_usec;

 改为

os_rgb << time_val.tv_sec << "." <<setiosflags(ios::fixed)<<setprecision(6)<<std::setfill('0')<<setw(6)<<time_val.tv_usec;
os_dep << time_val.tv_sec << "."<<setiosflags(ios::fixed)<<setprecision(6)<<std::setfill('0')<<setw(6) <<time_val.tv_usec;

 经过上面的修改，就可以保证图像的时间戳的小数部分都是６位．

我重新生成了association.txt文件，再次运行orb-slam2的RGB-D接口，发现之前的会跳的问题解决了．很不可思议，但这就是事实，如何解释这个问题呢，

/**
 *
 * 函数功能：采集iaikinect2输出的彩色图和深度图数据，并以文件的形式进行存储
 *
 *
 * 分隔符为　逗号'，'　　
 * 时间戳单位为秒(s)　精确到小数点后６位(us)
 *
 * maker:crp
 * 2017-5-13
 */

#include <iostream>
#include <sstream>
#include <stdio.h>
#include <stdlib.h>
#include <string>
#include <vector>

#include <ros/ros.h>
#include <ros/spinner.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/Image.h>
#include <std_msgs/String.h>

#include <cv_bridge/cv_bridge.h> //将ROS下的sensor_msgs/Image消息类型转化成cv::Mat。
#include <sensor_msgs/image_encodings.h> //头文件sensor_msgs/Image是ROS下的图像的类型，这个头文件中包含对图像进行编码的函数

#include <fstream>
#include <image_transport/image_transport.h>
#include <image_transport/subscriber_filter.h>
#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/opencv.hpp>
#include <sstream>

using namespace std;
using namespace cv;

Mat rgb, depth;
char successed_flag1 = 0, successed_flag2 = 0;

string topic1_name = "/kinect2/qhd/image_color"; // topic 名称
string topic2_name = "/kinect2/qhd/image_depth_rect";

string filename_rgbdata = "/home/yunlei/recordData/RGBD/rgbdata.txt";
string filename_depthdata = "/home/yunlei/recordData/RGBD/depthdata.txt";
string save_imagedata = "/home/yunlei/recordData/RGBD/";

void dispDepth(const cv::Mat &in, cv::Mat &out, const float maxValue);
void callback_function_color(const sensor_msgs::Image::ConstPtr image_data);
void callback_function_depth(const sensor_msgs::Image::ConstPtr image_data);
int main(int argc, char **argv) {
  string out_result;

  // namedWindow("image color",CV_WINDOW_AUTOSIZE);
  // namedWindow("image depth",CV_WINDOW_AUTOSIZE);
  ros::init(argc, argv, "kinect2_listen");
  if (!ros::ok())
    return 0;
  ros::NodeHandle n;
  ros::Subscriber sub1 = n.subscribe(topic1_name, 30, callback_function_color);
  ros::Subscriber sub2 = n.subscribe(topic2_name, 30, callback_function_depth);
  ros::AsyncSpinner spinner(3); // Use 3 threads
  spinner.start();

  string rgb_str, dep_str;

  struct timeval time_val;
  struct timezone tz;
  double time_stamp;

  ofstream fout_rgb(filename_rgbdata.c_str());
  if (!fout_rgb) {
    cerr << filename_rgbdata << " file not exist" << endl;
  }
  ofstream fout_depth(filename_depthdata.c_str());
  if (!fout_depth) {
    cerr << filename_depthdata << " file not exist" << endl;
  }

  while (ros::ok()) {

    if (successed_flag1) {
      gettimeofday(&time_val, &tz); // us
      //  time_stamp =time_val.tv_sec+ time_val.tv_usec/1000000.0;
      ostringstream os_rgb;
      // os_rgb.setf(std::ios::fixed);
      // os_rgb.precision(6);
	    
      os_rgb << time_val.tv_sec << "." <<setiosflags(ios::fixed)<<setprecision(6)<<std::setfill('0')<<setw(6)<<time_val.tv_usec;
      rgb_str = save_imagedata + "rgb/" + os_rgb.str() + ".png";
      imwrite(rgb_str, rgb);
      fout_rgb << os_rgb.str() << ",rgb/" << os_rgb.str() << ".png\n";
      successed_flag1 = 0;
      //   imshow("image color",rgb);
      cout << "rgb -- time:  " << time_val.tv_sec << "." <<setiosflags(ios::fixed)<<setprecision(6)<<std::setfill('0')<<setw(6)<< time_val.tv_usec
           << endl;
      //    waitKey(1);
    }

    if (successed_flag2) {
      gettimeofday(&time_val, &tz); // us
      ostringstream os_dep;
      // os_dep.setf(std::ios::fixed);
      // os_dep.precision(6);
	    
      os_dep << time_val.tv_sec << "."<<setiosflags(ios::fixed)<<setprecision(6)<<std::setfill('0')<<setw(6) <<time_val.tv_usec;
      dep_str =
          save_imagedata + "depth/" + os_dep.str() + ".png"; // 输出图像目录
      imwrite(dep_str, depth);
      fout_depth << os_dep.str() << ",depth/" << os_dep.str() << ".png\n";
      successed_flag2 = 0;
      //   imshow("image depth",depth);
      cout << "depth -- time:" << time_val.tv_sec << "." << setiosflags(ios::fixed)<<setprecision(6)<<std::setfill('0')<<setw(6)<<time_val.tv_usec
           << endl;
    }
  }

  ros::waitForShutdown();
  ros::shutdown();

  return 0;
}
void callback_function_color(const sensor_msgs::Image::ConstPtr image_data) {
  cv_bridge::CvImageConstPtr pCvImage; // 声明一个CvImage指针的实例

  pCvImage = cv_bridge::toCvShare(
      image_data,
      image_data
          ->encoding); //将ROS消息中的图象信息提取，生成新cv类型的图象，复制给CvImage指针
  pCvImage->image.copyTo(rgb);
  successed_flag1 = 1;
}
void callback_function_depth(const sensor_msgs::Image::ConstPtr image_data) {
  Mat temp;
  cv_bridge::CvImageConstPtr pCvImage; // 声明一个CvImage指针的实例
  pCvImage = cv_bridge::toCvShare(
      image_data,
      image_data
          ->encoding); //将ROS消息中的图象信息提取，生成新cv类型的图象，复制给CvImage指针
  pCvImage->image.copyTo(depth);

  // dispDepth(temp, depth, 12000.0f);
  successed_flag2 = 1;
  // imshow("Mat depth",depth/256);
  // cv::waitKey(1);
}
void dispDepth(const cv::Mat &in, cv::Mat &out, const float maxValue) {
  cv::Mat tmp = cv::Mat(in.rows, in.cols, CV_8U);
  const uint32_t maxInt = 255;

#pragma omp parallel for
  for (int r = 0; r < in.rows; ++r) {
    const uint16_t *itI = in.ptr<uint16_t>(r);
    uint8_t *itO = tmp.ptr<uint8_t>(r);

    for (int c = 0; c < in.cols; ++c, ++itI, ++itO) {
      *itO = (uint8_t)std::min((*itI * maxInt / maxValue), 255.0f);
    }
  }

  cv::applyColorMap(tmp, out, COLORMAP_JET);
}

 

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.015)
    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])))