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