Kinect + ROS TUM数据制作

1. 环境配置

1. 安装驱动

sudo apt-get install  ros-indigo-freenect-*
rospack profile

2. 设置环境变量

echo $TURTLEBOT_3D_SENSOR
#Output: kinect

如果你看到一个3D传感器，例如asus_xtion_pro，您将需要设置环境变量的默认值，修改和重新启动终端：

echo "export TURTLEBOT_3D_SENSOR=kinect" >> .bashrc

2. 录制rosbag包

2.1 启动相机

#1. 建立一个新终端
roslaunch turtlebot_bringup minimal.launch

#2. 新建一个终端
#针对Microsoft Kinect:
roslaunch freenect_launch freenect-registered-xyzrgb.launch （新版本）--> 实验室版本
roslaunch openni_launch openni.launch (或旧版本)

#针对Asus Xtion, Xtion Pro, or Primesense 1.08/1.09 cameras:
roslaunch openni2_launch openni2.launch depth_registration:=true

2.2 测试相机

图像，在工作站打开终端执行：

# rosrun [package_name] [node_name] --基本用法
# rosrun [package_name] [node_name] __name:=[node_new_name] -- 重命名节点


# 新建终端
rosrun image_view image_view image:=/camera/rgb/image_color

# 新建终端
rosrun image_view image_view image:=/camera/depth_registered/image_raw

2.3 录制

# rosbag record -O [bag_name] [topic1] [topic2] [...]    #不加中括号
# -o xxx.bag: xxx指定文件存储位置和文件名称的,`-o` 会自动的在你的文件名称后加上当前的时间戳如：xxx_2020-06-22-21-00.bag
# -O xxx.bag: `-O` 不会添加时间戳，直接是xxx.bag

rosbag record  -o/O xxx.bag /camera/rgb/image_color /camera/depth_registered/image_raw  

3. 提取rosbag包

# 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 = '/.../rgb/'    # 已经建立好的存储rgb彩色图文件的目录
depth_path= '/.../depth/' # 已经建立好的存储深度图文件的目录
 
class ImageCreator():
 
 
    def __init__(self):
        self.bridge = CvBridge()
        with rosbag.Bag('/.../2019-05-10-09-00-55.bag', 'r') as bag:  #要读取的bag文件；
            for topic,msg,t in bag.read_messages(): 
                if topic == "/camera/rgb/image_color": #图像的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 == "/camera/depth_registered/image_raw": #图像的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

4. 生成rgb.txt 与 depth.txt

#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;
}

5. RGB-D相机标定

5.1 标定板角点提取

#include <iostream>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>
using namespace std;
using namespace cv;
int main()
{
	Mat img_1 = cv::imread("C:\\Users\\ZhaoCheng\\source\\repos\\OpenCV\\OpenCV\\rec.jpg");
	Mat img_2 = cv::imread("C:\\Users\\ZhaoCheng\\source\\repos\\OpenCV\\OpenCV\\circle.jpg");

	if (img_1.empty() || img_2.empty())
	{
		return -1;
	}
	Mat gray_1, gray_2;
	cvtColor(img_1, gray_1, COLOR_BGR2GRAY);
	cvtColor(img_2, gray_2, COLOR_BGR2GRAY);

	//定义数目
	Size board_size_1 = Size(9, 6);//方格标定板内角点数目（行、列）
	Size board_size_2 = Size(7,7);//圆形标定板圆心数目（行、列）

	//检测角点
	vector<Point2f> img_points_1, img_points_2;
	findChessboardCorners(gray_1, board_size_1, img_points_1);//计算方格标定板角点
	findCirclesGrid(gray_2, board_size_2, img_points_2);//计算圆形标定板角点

	//细化角点坐标
	find4QuadCornerSubpix(gray_1, img_points_1, Size(5, 5));//细化方格标定板角点坐标
	find4QuadCornerSubpix(gray_2, img_points_2, Size(5, 5));

	//绘制角点检测结果
	drawChessboardCorners(img_1, board_size_1, img_points_1, true);
	drawChessboardCorners(img_2, board_size_2, img_points_2, true);

	//显示结果
	imshow("方格标定", img_1);
	imshow("圆形标定", img_2);
	imwrite("方格标定.jpg",img_1);
	imwrite("圆形标定.jpg",img_2);
	waitKey(0);
	return 0;
}

5.2 相机内参

#include <vector>
#include <fstream>
#include <iostream>

#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>
using namespace std;
using namespace cv;
int main()
{
	//读取所有图像
	vector<Mat> imgs;
	string image_name;

	ifstream infile("calibdata.txt");
	if (!infile)
	{
		cout << "open error" << endl;
		return -1;
	}
	while (getline(infile,image_name))
	{
		static unsigned int num = 0;
		Mat img = imread(image_name);
		/*imshow("images" + to_string(++num), img);
		waitKey(1000);
		destroyWindow("images" + to_string(num));*/
		imgs.push_back(img);
	}

	Size board_size = Size(9, 6);//方格标定板内角点数目（行、列）
	vector<vector<Point2f>> imgs_points;

	for (int i = 0; i < imgs.size(); i++)
	{
		Mat img1 = imgs[i];
		Mat gray1;
		cvtColor(img1, gray1, COLOR_BGR2GRAY);
		vector<Point2f> img1_points;
		findChessboardCorners(gray1, board_size, img1_points);
		find4QuadCornerSubpix(gray1, img1_points, Size(5, 5));
		//绘制角点检测结果
		drawChessboardCorners(img1, board_size, img1_points, true);
		/*imshow("方格标定", img1);

		waitKey(3000);*/
		imgs_points.push_back(img1_points);
	}

	//生成棋盘格每个内角点的空间三维坐标

	Size square_size = Size(24, 24);//!棋盘格每个方格的真实尺寸
	vector<vector<Point3f>> object_points;
	
	for (int i = 0; i < imgs_points.size(); i++)
	{
		vector<Point3f> temp_points;
		for (int j = 0; j < board_size.height; j++)
		{
			for (int k = 0; k < board_size.width; k++)
			{
				Point3f real_point;

				//假设标定板为世界坐标的z平面，即z=0;
				real_point.x = j * square_size.width;
				real_point.y = k * square_size.height;

				real_point.z = 0;

				temp_points.push_back(real_point);
			}
		}
		object_points.push_back(temp_points);
	}
	
	/*初始化每幅图像中的角点数量，假定每幅图像中可以看到完整的标定板*/
	vector<int> point_number;
	for (int i = 0; i < imgs_points.size(); i++)
	{
		point_number.push_back(board_size.width * board_size.height);
	}

	//图像尺寸
	Size img_size;
	img_size.width = imgs.at(0).cols;
	img_size.height = imgs.at(0).rows;

	//相机内参矩阵
	Mat camera_matrix = Mat(3, 3, CV_32FC1, Scalar::all(0));

	//相机畸变系数 k1,k2,p1,p2,k3
	Mat dist_coeffs = Mat(1, 5, CV_32FC1, Scalar::all(0));

	vector<Mat> rvecs;//每幅图像的旋转向量
	vector<Mat> tvecs;//每幅图像的平移向量

	calibrateCamera(object_points, imgs_points, img_size, camera_matrix, dist_coeffs, rvecs, tvecs, 0);

	cout << "相机的内参矩阵：\n" << camera_matrix << endl;
	cout << "相机畸变系数：\n" << dist_coeffs << endl;
	waitKey(0);
	return 0;

}

5.3 ROS包-kinectv1 – camera_calibration

5.3.1 标定板下载

标定板下载

5.3.2 相机和标定程序

1. roslaunch freenect_launch freenect.launch #打开相机
2. rosrun camera_calibration cameracalibrator.py --size 8x6 --square 0.0245 image:=/camera/rgb/image_raw camera:=/camera/rgb 

# -- size:内角点的（行、列）
# -- --square ：小方格实际大小（单位：m）

注意：

• 8x6中间的乘是字母x
• x：表示标定板在视野中的左右位置，左右移动使绿色条变满
• y：表示标定板在视野中的前后的位置，前后移动使绿色条变满
• size：标定板在占视野的尺寸大小，也可以理解为标定板离摄像头的远近，上下移动使绿色条变满
• skew：标定板在视野中的倾斜位置，不断旋转标定板使绿色条变满

CALIBRATE被激活后，点击CALIBRATE按钮，稍等1-2分钟，SAVE按钮和COMMIT按钮被激活，点击save校准数据会被保存到/temp文件夹下；点击COMMIT将结果保存到/home/.ros/camera_info/文件夹下。

5.3.3 标定深度相机

rosrun camera_calibration cameracalibrator.py image:=/camera/ir/image_raw camera:=/camera/ir --size 8x6 --square 0.024

注意：深度相机标定的时候尽量在暗处，遮光最好，用不透光的物品把红外发射装置遮住。

# test 1:
image_width: 640
image_height: 480
camera_name: rgb_A00361D00635137A
camera_matrix:
  rows: 3
  cols: 3
  data: [519.162926187449, 0, 306.2870356586101, 0, 519.1127779687413, 266.2205037810496, 0, 0, 1]
distortion_model: plumb_bob
distortion_coefficients:
  rows: 1
  cols: 5
  data: [0.1746622752778997, -0.2169260940804337, 0.007160603616569282, -0.0121890591153448, 0]
rectification_matrix:
  rows: 3
  cols: 3
  data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
projection_matrix:
  rows: 3
  cols: 4
  data: [537.52587890625, 0, 298.1487616194645, 0, 0, 541.44287109375, 269.0468311393488, 0, 0, 0, 1, 0]

# test 2:
image_width: 640
image_height: 480
camera_name: rgb_A00361D00635137A
camera_matrix:
  rows: 3
  cols: 3
  data: [525.9082191482263, 0, 306.061301256364, 0, 524.0230229561403, 249.5250446033006, 0, 0, 1]
distortion_model: plumb_bob
distortion_coefficients:
  rows: 1
  cols: 5
  data: [0.1765549953084217, -0.3142127236680227, 0.0004540028525181106, -0.003012224131173345, 0]
rectification_matrix:
  rows: 3
  cols: 3
  data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
projection_matrix:
  rows: 3
  cols: 4
  data: [536.6383666992188, 0, 303.3809178001175, 0, 0, 536.6723022460938, 249.1436858015313, 0, 0, 0, 1, 0]

camera_matrix:
[522.536,0,306.174]
[0,521.567,257.872]
[0,      0,      1]

distortion_coefficients:
[0.1756,-0.2655,0.0038,-0.0076,0]