用深度相机构图做SLAM 常需要对一个真实的环境进行测试，需要自己使用深度相机录制数据集。通常相机的硬件商或者是网上都有深度相机在ROS下的驱动程序，使得我们可以正常的通过单个topic 看到深度图像和彩色图像，但是我们经常卡在如何使用一个ROS节点接收多个topic 的数据这个步骤，解决这个问题需要使用ROS 自带的多线程创建工具（ros::AsyncSpinner）。
注：环境平台为： Ubuntu14.04+ROS indigo
1、安装kinect 驱动包
1.安装 libfreenect2 包为Kinect2提供驱动
2.下载 iai_kinect2 ROS在ROS环境下读取Kinect的彩色图像和深度图像。
这两个的步骤可以参考 hitcm 这位博主的博客，里面提供了详细的驱动安装和kinect2 图像数据读取教程。
2、同时保存Kinect彩色图和深度图
通常情况下ROS只能接受一个topic的数据，如果直接设置两个接收topic会造成第二个topic无法调用的现象。如果在一个节点上接受多个topic发出的数据，这时候你需要开启ROS自带的多线程工具（ ros::AsyncSpinner spinner(3); ），才可以正常使用。其中的参数3表示打开三个线程。
贴出了我自己使用的采集函数的代码，文件名用当前的时间戳作为文件名，你可以按照你自己的要求修改存储格式，函数中的以下变量分别表示接受topic的名称和存储路径。
topic1_name #彩色图像的topic
topic2_name #深度图像的topic
filename_rgbdata #彩色图像存储偏移路径
filename_depthdata #深度图像存储偏移路径
save_imagedata #图像存储路径根目录
如果你懒得复制代码，可以拖到文章的最后直接下载我编译好的包。
/**
*
* 函数功能：采集iaikinect2输出的彩色图和深度图数据，并以文件的形式进行存储
*
*
* 分隔符为 逗号'，'
* 时间戳单位为秒(s) 精确到小数点后６位(us)
*
* maker:crp
* 2017-5-13
*/
#include<iostream>
#include<string>
#include <stdlib.h>
#include <stdio.h>
#include <sstream>
#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 <image_transport/image_transport.h>
#include <image_transport/subscriber_filter.h>
#include <opencv2/imgproc/imgproc.hpp>
#include<opencv2/core/core.hpp>
#include<opencv2/highgui/highgui.hpp>
#include <opencv2/opencv.hpp>
#include<iostream>
#include <fstream>
#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/crp/recordData/RGBD/rgbdata.txt";
string filename_depthdata="/home/crp/recordData/RGBD/depthdata.txt";
string save_imagedata = "/home/crp/recordData/RGBD/";
bool display_IMU5211( unsigned char buf[21] ,timeval time_stamp,string &out_result);
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,50,callback_function_color);
ros::Subscriber sub2 = n.subscribe(topic2_name,50,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<<time_val.tv_sec<<"."<<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<<"."<<time_val.tv_usec<<endl;
waitKey(1);
}
if( successed_flag2 )
{
gettimeofday(&time_val,&tz);//us
ostringstream os_dep;
os_dep<<time_val.tv_sec<<"."<<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<<"."<<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指针的实例
// cout<<"the frame_id:"<<image_data->frame_id.c_str()<<endl;
// cout<<"the image heigh"<<image_data->height<<endl;
// cout<<"the image width"<<image_data->width<<endl;
// cout<<"the image step"<<image_data->step<<endl;
// cout<<"listen ...."<<endl;
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);
// cout<<"the depth heigh"<<image_data->height<<endl;
// cout<<"the depth width"<<image_data->width<<endl;
// cout<<"the depth step"<<image_data->step<<endl;
// cout<<"listen ...."<<endl;
dispDepth(temp, depth, 12000.0f);
successed_flag2=1;
// imshow("Mat depth",depth/256);
}
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);
}
3、save_rgbd_from_kinect2 包的使用
注：请确保已经正确的安装了kinect2的驱动和iai_kinect2 图像读取包。
1、在CSDN论坛这个Github 或者CSDN论坛上 去下载save_imgfrom_kinect2 包，并拷贝到你自己的ROS catkin_ws工作空间下，使用 catkin_make 命令直接编译
catkin_make save_rgbd_from_kinect2
2、确保你的存储路径下有 rgb 和 depth 两个文件夹以及 depthdata.txt 和 rgbdata.txt 两个文件
3、启动iai_kinect 的数据读取节点
roslaunch kinect2_bridge kinect2_bridge.launch
4、启动 save_rgbd_from_kinect2 节点
rosrun save_rgbd_from_kinect2 save_rgbd_from_kinect2
正常采集时窗口会输出时间戳信息：
采集完成后的效果：
其中depthdata.txt 和 rgbdata.txt 两个文件放的是图像时间戳和相对路径（仿照了TUM的格式）
欢迎拍砖,讨论 [email protected]