DataCollection

KITTI采集数据

此前我们已经安装了相关的相机驱动，且我们的海康工业相机采用USB3.0接口，但是直接用opencv读取不了数据，但是又由于购买的时候没有ROS驱动。因此通过官网所提供的示例，修改为通过ROS发布图像数据话题。分为下面三个步骤:

1. 编写工业相机代码发布话题
2. 同步采集激光雷达和图像数据
3. pcd文件转bin文件

一、编写代码发布图像话题

因为要同时采集激光雷达数据和图像数据，本文利用ROS的同步机制而没有采用相机的触发机制。而KITTI上，有这么一句话说明图像采集方法:
The cameras are triggered at 10 frames per second by the laser scanner (when facing forward) with shutter time adjusted dynamically (maximum shutter time: 2 ms).

根据官方所提供的案例:
我们改写下列代码并放入ROS工作空间中:

#!/usr/bin/env python
#coding:utf-8

import sys

import threading
import os
import termios

from ctypes import *


from MvImport.MvCameraControl_class import *
import cv2
import numpy as np
g_bExit = False
import rospy
from cv_bridge import CvBridge,CvBridgeError
from sensor_msgs.msg import Image
from std_msgs.msg import Header


bridge = CvBridge()
pub_img = rospy.Publisher('/camera/image_raw', Image, queue_size=1)
header = Header()
header.frame_id = '/camera'

# 为线程定义一个函数
def work_thread(cam=0, pData=0, nDataSize=0):
	stFrameInfo = MV_FRAME_OUT_INFO_EX()
	memset(byref(stFrameInfo), 0, sizeof(stFrameInfo))
	while True:
		ret = cam.MV_CC_GetOneFrameTimeout(pData, nDataSize, stFrameInfo, 1000)
		if ret == 0:
			image = np.asarray(pData._obj)
			#image = image.reshape((stFrameInfo.nHeight, stFrameInfo.nWidth))    # 灰度图
			image = image.reshape((stFrameInfo.nHeight, stFrameInfo.nWidth, 3))  # RGB
			image = cv2.cvtColor(image , cv2.COLOR_BGR2RGB)
			cv_image = bridge.cv2_to_imgmsg(image,"bgr8")
			header.stamp = rospy.Time.now()
			cv_image.header = header
			pub_img.publish(cv_image)
			#cv2.imshow("", image)
			#cv2.waitKey(1)
			print ("get one frame: Width[%d], Height[%d], PixelType[0x%x], nFrameNum[%d]"  % (stFrameInfo.nWidth, stFrameInfo.nHeight, stFrameInfo.enPixelType,stFrameInfo.nFrameNum))
		else:
			print ("no data[0x%x]" % ret)
		if g_bExit == True:
				break

def press_any_key_exit():
	fd = sys.stdin.fileno()
	old_ttyinfo = termios.tcgetattr(fd)
	new_ttyinfo = old_ttyinfo[:]
	new_ttyinfo[3] &= ~termios.ICANON
	new_ttyinfo[3] &= ~termios.ECHO
	#sys.stdout.write(msg)
	#sys.stdout.flush()
	termios.tcsetattr(fd, termios.TCSANOW, new_ttyinfo)
	try:
		os.read(fd, 7)
	except:
		pass
	finally:
		termios.tcsetattr(fd, termios.TCSANOW, old_ttyinfo)
	
if __name__ == "__main__":
	rospy.init_node('CAM_LIDAR',anonymous = True)
	SDKVersion = MvCamera.MV_CC_GetSDKVersion()
	print ("SDKVersion[0x%x]" % SDKVersion)

	deviceList = MV_CC_DEVICE_INFO_LIST()
	tlayerType = MV_GIGE_DEVICE | MV_USB_DEVICE
	
	# ch:枚举设备 | en:Enum device
	ret = MvCamera.MV_CC_EnumDevices(tlayerType, deviceList)
	if ret != 0:
		print ("enum devices fail! ret[0x%x]" % ret)
		sys.exit()

	if deviceList.nDeviceNum == 0:
		print ("find no device!")
		sys.exit()

	print ("Find %d devices!" % deviceList.nDeviceNum)

	for i in range(0, deviceList.nDeviceNum):
		mvcc_dev_info = cast(deviceList.pDeviceInfo[i], POINTER(MV_CC_DEVICE_INFO)).contents
		if mvcc_dev_info.nTLayerType == MV_GIGE_DEVICE:
			print ("\ngige device: [%d]" % i)
			strModeName = ""
			for per in mvcc_dev_info.SpecialInfo.stGigEInfo.chModelName:
				strModeName = strModeName + chr(per)
			print ("device model name: %s" % strModeName)

			nip1 = ((mvcc_dev_info.SpecialInfo.stGigEInfo.nCurrentIp & 0xff000000) >> 24)
			nip2 = ((mvcc_dev_info.SpecialInfo.stGigEInfo.nCurrentIp & 0x00ff0000) >> 16)
			nip3 = ((mvcc_dev_info.SpecialInfo.stGigEInfo.nCurrentIp & 0x0000ff00) >> 8)
			nip4 = (mvcc_dev_info.SpecialInfo.stGigEInfo.nCurrentIp & 0x000000ff)
			print ("current ip: %d.%d.%d.%d\n" % (nip1, nip2, nip3, nip4))
		elif mvcc_dev_info.nTLayerType == MV_USB_DEVICE:
			print ("\nu3v device: [%d]" % i)
			strModeName = ""
			for per in mvcc_dev_info.SpecialInfo.stUsb3VInfo.chModelName:
				if per == 0:
					break
				strModeName = strModeName + chr(per)
			print ("device model name: %s" % strModeName)

			strSerialNumber = ""
			for per in mvcc_dev_info.SpecialInfo.stUsb3VInfo.chSerialNumber:
				if per == 0:
					break
				strSerialNumber = strSerialNumber + chr(per)
			print ("user serial number: %s" % strSerialNumber)

	nConnectionNum = input("please input the number of the device to connect:")

	if int(nConnectionNum) >= deviceList.nDeviceNum:
		print ("intput error!")
		sys.exit()

	# ch:创建相机实例 | en:Creat Camera Object
	cam = MvCamera()
	
	# ch:选择设备并创建句柄| en:Select device and create handle
	stDeviceList = cast(deviceList.pDeviceInfo[int(nConnectionNum)], POINTER(MV_CC_DEVICE_INFO)).contents

	ret = cam.MV_CC_CreateHandle(stDeviceList)
	if ret != 0:
		print ("create handle fail! ret[0x%x]" % ret)
		sys.exit()

	# ch:打开设备 | en:Open device
	ret = cam.MV_CC_OpenDevice(MV_ACCESS_Exclusive, 0)
	if ret != 0:
		print ("open device fail! ret[0x%x]" % ret)
		sys.exit()
	
	# ch:探测网络最佳包大小(只对GigE相机有效) | en:Detection network optimal package size(It only works for the GigE camera)
	if stDeviceList.nTLayerType == MV_GIGE_DEVICE:
		nPacketSize = cam.MV_CC_GetOptimalPacketSize()
		if int(nPacketSize) > 0:
			ret = cam.MV_CC_SetIntValue("GevSCPSPacketSize",nPacketSize)
			if ret != 0:
				print ("Warning: Set Packet Size fail! ret[0x%x]" % ret)
		else:
			print ("Warning: Get Packet Size fail! ret[0x%x]" % nPacketSize)

	# ch:设置触发模式为off | en:Set trigger mode as off
	ret = cam.MV_CC_SetEnumValue("TriggerMode", MV_TRIGGER_MODE_OFF)
	if ret != 0:
		print ("set trigger mode fail! ret[0x%x]" % ret)
		sys.exit()

	# ch:获取数据包大小 | en:Get payload size
	stParam =  MVCC_INTVALUE()
	memset(byref(stParam), 0, sizeof(MVCC_INTVALUE))
	
	ret = cam.MV_CC_GetIntValue("PayloadSize", stParam)
	if ret != 0:
		print ("get payload size fail! ret[0x%x]" % ret)
		sys.exit()
	nPayloadSize = stParam.nCurValue

	# ch:开始取流 | en:Start grab image
	ret = cam.MV_CC_StartGrabbing()
	if ret != 0:
		print ("start grabbing fail! ret[0x%x]" % ret)
		sys.exit()

	data_buf = (c_ubyte * nPayloadSize)()

	try:
		hThreadHandle = threading.Thread(target=work_thread, args=(cam, byref(data_buf), nPayloadSize))
		hThreadHandle.start()
	except:
		print ("error: unable to start thread")
		
	print ("press a key to stop grabbing.")
	press_any_key_exit()

	g_bExit = True
	hThreadHandle.join()
	# ch:停止取流 | en:Stop grab image

	ret = cam.MV_CC_StopGrabbing()
	if ret != 0:
		print ("stop grabbing fail! ret[0x%x]" % ret)
		del data_buf
		sys.exit()

	# ch:关闭设备 | Close device
	ret = cam.MV_CC_CloseDevice()
	if ret != 0:
		print ("close deivce fail! ret[0x%x]" % ret)
		del data_buf
		sys.exit()

	# ch:销毁句柄 | Destroy handle
	ret = cam.MV_CC_DestroyHandle()
	if ret != 0:
		print ("destroy handle fail! ret[0x%x]" % ret)
		del data_buf
		sys.exit()

	del data_buf

二、同步采集激光雷达数据和图像数据

对于velodyne的部分，本文不进行赘述。本文采用先录ROS包的方法，对激光雷达数据和图像数据分别作保存。保存的格式根据KITTI为.pcd和.png。具体代码如下:

#include <ros/ros.h>
#include <iostream>
#include <string>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
// PCL specific includes
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Image.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <opencv/cv.hpp>
#include <cv_bridge/cv_bridge.h>


int count_num = 0;
int n_zero = 6; //设置补0位数


void callback(const sensor_msgs::PointCloud2ConstPtr& velodyneMsg, const sensor_msgs::ImageConstPtr& ImageMsg)
{


	// 声明存储原始数据与滤波后的数据的点云的 格式
	pcl::PCLPointCloud2::Ptr cloud (new pcl::PCLPointCloud2 ());
	
	// 转化为PCL中的点云的数据格式
	pcl_conversions::toPCL(*velodyneMsg, *cloud);
        
	std::string new_string = std::string(n_zero - std::to_string(count_num).length(), '0') + std::to_string(count_num);
	std::string velodyneName = "/home/.../catkin_ws/src/cam_lidar/src/CamLidarData/pcd/"  + new_string +  ".pcd";
	std::string imageName = "/home/.../catkin_ws/src/cam_lidar/src/CamLidarData/image/front/"  + new_string +  ".png";
	count_num += 1;

	pcl::io::savePCDFile(velodyneName, *cloud);  //保存.pcd 

	cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(ImageMsg, sensor_msgs::image_encodings::BGR8);
	cv::Mat img = cv_ptr -> image;
	//cv::Mat resizedImage;
	//cv::resize(img, resizedImage, cv::Size(1280, 370));
	cv::imwrite(imageName, img);

	std::cout<< "capture done:" << count_num << std::endl;
  

}

int main (int argc, char** argv)
{
	/*dynamic reconfigure*/
	ros::init (argc, argv, "bag2pcd");


	// Initialize ROS
	
	ros::NodeHandle nh;
  	message_filters::Subscriber<sensor_msgs::PointCloud2> velodyne_sub(nh,"/points_raw" , 1);//订阅激光雷达Topic
  	message_filters::Subscriber<sensor_msgs::Image> camera_sub(nh,"/camera/image_raw" , 1);//订阅相机Topic
	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, sensor_msgs::Image> MySyncPolicy;
	// ApproximateTime takes a queue size as its constructor argument, hence MySyncPolicy(10)
	message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(2),
													velodyne_sub,
													camera_sub
													);
	std::cout<< "************************ start to capture ****************" << std::endl;
	sync.registerCallback(boost::bind(&callback, _1, _2));
	
	// Spin
	ros::spin ();
}

三、 pcd转成KITTI的bin格式

目前，我们就拥有了pcd点云文件和相对应的png图像数据。但是KITTI数据格式中，不使用pcd作为输入，而是输入bin。因此我们需要进行转换,所采用的代码来自leofansq:

/*  This code is to transform the .pcd files to .bin files.
*
*   The point cloud files decoded from the .bag file are usually in .pcd format. 
* In order to facilitate the experiment of 3D detection, the format of the KITTI 
* data set needs to be unified, that is, converted into the .bin format.In the 
* .bin file, each point corresponds to four data, which are xyz and intensity.
* 
*   Input file: a .pcd file containing xyz and intensity
*   Output file: a .bin file in KITTI format
*   Parameters to be adjusted: file path in the Main function
*
*   Code by FSQ 2018.11.5
**************************************************************************************/
#include <ros/ros.h>
#include <iostream>           
#include <pcl/io/pcd_io.h>      
#include <pcl/point_types.h>     
using namespace std;

//Transform PCD 2 BIN
void pcd2bin (string &in_file, string& out_file)
{ 
   //Create a PointCloud value
  pcl::PointCloud<pcl::PointXYZI>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZI>);
  
  //Open the PCD file
  if (pcl::io::loadPCDFile<pcl::PointXYZI> (in_file, *cloud) == -1) 
  {
    PCL_ERROR ("Couldn't read in_file\n");
  }
  //Create & write .bin file
  ofstream bin_file(out_file.c_str(),ios::out|ios::binary|ios::app);
  if(!bin_file.good()) cout<<"Couldn't open "<<out_file<<endl;  

  //PCD 2 BIN 
  cout << "Converting "
            << in_file <<"  to  "<< out_file
            << endl;
  for (size_t i = 0; i < cloud->points.size (); ++i)
  {
  	bin_file.write((char*)&cloud->points[i].x,3*sizeof(float)); 
    bin_file.write((char*)&cloud->points[i].intensity,sizeof(float));
    //cout<< 	cloud->points[i]<<endl;
  }
  	
  bin_file.close();
}
static std::vector<std::string> file_lists;

//Read the file lists
void read_filelists(const std::string& dir_path,std::vector<std::string>& out_filelsits,std::string type)
{
    struct dirent *ptr;
    DIR *dir;
    dir = opendir(dir_path.c_str());
    out_filelsits.clear();
    while ((ptr = readdir(dir)) != NULL){
        std::string tmp_file = ptr->d_name;
        if (tmp_file[0] == '.')continue;
        if (type.size() <= 0){
            out_filelsits.push_back(ptr->d_name);
        }else{
            if (tmp_file.size() < type.size())continue;
            std::string tmp_cut_type = tmp_file.substr(tmp_file.size() - type.size(),type.size());
            if (tmp_cut_type == type){
                out_filelsits.push_back(ptr->d_name);
            }
        }
    }
}

bool computePairNum(std::string pair1,std::string pair2)
{
    return pair1 < pair2;
}

//Sort the file list
void sort_filelists(std::vector<std::string>& filists,std::string type)
{
    if (filists.empty())return;

    std::sort(filists.begin(),filists.end(),computePairNum);
}

int main(int argc, char **argv)
{
	ros::init (argc, argv, "pcd2bin");
    //Set the file path
    std::string bin_path = "/home/.../catkin_ws/src/cam_lidar/src/CamLidarData/bin/";
    std::string pcd_path = "/home/.../catkin_ws/src/cam_lidar/src/CamLidarData/pcd/";
    //Read file lists of specific type
    read_filelists( pcd_path, file_lists, "pcd" );
    sort_filelists( file_lists, "pcd" );
    //PCD2BIN one by one
    for (int i = 0; i < file_lists.size(); ++i)
    {
        std::string pcd_file = pcd_path + file_lists[i];
        std::string tmp_str = file_lists[i].substr(0, file_lists[i].length() - 4) + ".bin";
        std::string bin_file = bin_path + tmp_str;
        pcd2bin( pcd_file, bin_file);
    }

    return 0;
}

目前数据采集已经完成，下一步就是数据的标注。

Reference:

1. https://www.hikrobotics.com/machinevision/service/download?module=0
2. https://github.com/leofansq/Tools_RosBag2KITTI
3. https://blog.csdn.net/zhaoyin214/article/details/110251486
4. https://blog.csdn.net/qq_23107577/article/details/113984935