realsense D435i 实现外部时钟触发硬件同步多相机数据采集

独孤西

已于 2023-11-09 16:53:24 修改

阅读量2.6k

点赞数 10

分类专栏： SLAM 文章标签：数码相机 iot opencv 计算机视觉 ubuntu github

于 2023-11-09 16:50:17 首次发布

本文链接：https://blog.csdn.net/weixin_46190814/article/details/133894524

版权

SLAM 专栏收录该内容

17 篇文章 5 订阅

订阅专栏

前言

最近有一个调试D435i相机的工作，需要使得三个相机能够完成硬件触发的同步，具体来说，就是有一个固定频率的外部脉冲信号，使得三个相机能够根据外部脉冲信号的硬件触发完成双目图片、深度图片、彩色图片、IMU数据的实时响应采集，因为外部脉冲信号是通过一个精确的时间模块发出的，因此将采集的图片时的系统时间记录下来后，和时间模块发脉冲时间做一个时间对齐(关键是一个脉冲对应一个数据，避免丢失与错位的问题，错位问题还需要验证)，计算硬件脉冲传递时间和相机响应时间的差值(这样通过精确时钟时间的推理就能得到更靠近真实的图片采集时间)，就能够得到一个图片采集的精确时间。
总之，关键的检查点就是查看多个相机是不是每一个都是根据脉冲精确采集图片。我通过一个小实验检测程序是否好用：通过单片机发送固定脉冲数，查看是否保存相同数量图片。最后程序实现的效果就是能够从多个相机处采集硬件同步的图片数据和IMU数据。
完整代码可以从我的github下载：https://github.com/ser666/D435i_multicam_hardware_sync.git

一、背景知识

1.工作环境

我的实验环境基于ubuntu20.04，并且需要配置好C++编译环境与安装好librealsense SDK库，这方面CSDN中有较多博客分享，不再赘述。

安装librealsenseSDK可参考：Ubuntu20.04LTS下安装Intel Realsense D435i驱动与ROS包：https://blog.csdn.net/wanghq2013/article/details/123325671

2.D435i硬件同步

D435i默认模式下是根据代码设置的帧率进行图片传输，有15帧、30帧等几种模式，但我们的实验要求能够统一管理时间，通过外部硬件触发的好处是同时进行数据采集，能够使得所有传感器采集数据的时间对齐。D435i有多相机模式，支持外部硬件同步。多相机模式下有master和slave两种模式，官方主要介绍的多相机指一个主机多个从机，多个从机和主机进行同步，而我们是通过外部时钟作为“主机”时间，因此所有相机都设为从机模式。D435i硬件同步引脚情况如图所示。
硬件触发接口

二、需求实现

1.多相机硬件同步触发

(1) 多线程连接多相机

用一个程序完成任意数量相机的连接，因此使用多线程，传递设备id号和数据保存位置到线程中去，因为设置了线程休眠1秒整个程序每连接一个相机需要多一秒初始化时间。

(2) 相机配置

硬件同步需要设置slave模式，并且同样需要配置相机向电脑传输数据流的速度。（这里我的理解是相机是向电脑传输固定速率的帧数据，程序里我通过检测帧序号进行筛选，硬件触发后帧序号才会改变，因此判断传过来的这一帧是不是硬件触发造成的，所以硬件触发的时钟速率需要小于相机上传到电脑的速率）
需要注意的一点是，双目图片采集，主动结构光会导致双目图片产生小白点，主动结构光的主要作用是做纹理增强，在光线不足的场景下仍然能够得到深度图片，关闭也能得到深度图片但是就丧失了增强效果。为了得到符合光度不变假设的双目灰度图片，我把主动结构光的功能关闭，因此深度图会不够准确。

std::cout << "Device (Thread) ID: " << dev_id << std::endl;
 std::string serial = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);
 std::cout << "Camera with serial number: " << serial << std::endl;

 auto advanced_dev = dev.as<rs400::advanced_mode>();
 auto advanced_sensors = advanced_dev.query_sensors();

 bool depth_found = false;
 bool color_found = false;
 bool infrared1_found = false;
 bool infrared2_found = false;
 rs2::sensor depth_sensor;
 rs2::sensor color_sensor;
 rs2::sensor infrared1_sensor;
 rs2::sensor infrared2_sensor;


 for (auto&& sensor : advanced_sensors) {
     std::string module_name = sensor.get_info(RS2_CAMERA_INFO_NAME);
     std::cout << module_name << std::endl;

     if (module_name == "Stereo Module") {
         depth_sensor = sensor;
         depth_found = true;
     } else if (module_name == "RGB Camera") {
         color_sensor = sensor;
         color_found = true;
     }else if (module_name == "RGB Camera") {

     }
 }

 if (!(depth_found && color_found)) {
     std::cout << "Unable to find both stereo and color modules" <<
         std::endl;
 }
 
 depth_sensor.set_option(RS2_OPTION_ENABLE_AUTO_EXPOSURE, 0);
 depth_sensor.set_option(RS2_OPTION_EXPOSURE, 8500); // microseconds
 depth_sensor.set_option(RS2_OPTION_GAIN, 16);
 depth_sensor.set_option(RS2_OPTION_FRAMES_QUEUE_SIZE, 1);
 depth_sensor.set_option(RS2_OPTION_EMITTER_ENABLED, 0);//关闭主动结构光

 // RGB sync doesn't work, need to use depth as master.
     std::cout << "Setting " << dev_id << " to slave!" << std::endl;
     depth_sensor.set_option(RS2_OPTION_INTER_CAM_SYNC_MODE, 4);

 color_sensor.set_option(RS2_OPTION_ENABLE_AUTO_EXPOSURE, 0);
 color_sensor.set_option(RS2_OPTION_EXPOSURE, 100); // 1/10 ms (10)
 color_sensor.set_option(RS2_OPTION_GAIN, 64);
 color_sensor.set_option(RS2_OPTION_FRAMES_QUEUE_SIZE, 1);

 rs2::pipeline pipe;
 rs2::config cfg;
 cfg.enable_device(serial);
 cfg.enable_stream(RS2_STREAM_COLOR, 848, 480, RS2_FORMAT_BGR8, 30);
 cfg.enable_stream(RS2_STREAM_INFRARED, 1, 848, 480, RS2_FORMAT_Y8, 30);
 cfg.enable_stream(RS2_STREAM_INFRARED, 2, 848, 480, RS2_FORMAT_Y8, 30);
 cfg.enable_stream(RS2_STREAM_DEPTH, 848, 480, RS2_FORMAT_Z16, 30);
 cfg.enable_stream(RS2_STREAM_ACCEL, RS2_FORMAT_MOTION_XYZ32F);
 cfg.enable_stream(RS2_STREAM_GYRO, RS2_FORMAT_MOTION_XYZ32F);

 rs2::pipeline_profile profile = pipe.start(cfg);

(3) 帧获取与处理

主要流程包括等待帧输入，检测到帧输入后判断帧序号，这个帧序号是从深度帧中得到的，因为D435i相机只有深度图像数据是可以通过硬件外部时钟触发的，因为以深度时间为基准。如果是新的帧序号，则接着完成数据保存等操作。

rs2::frameset frames = pipe.wait_for_frames();
std::string timestamp_now = GetCurrentTimeStamp(1);
rs2_vector accel_sample;
rs2_vector gyro_sample;

// Get imu data
if (rs2::motion_frame accel_frame = frames.first_or_default(RS2_STREAM_ACCEL))
{
    accel_sample = accel_frame.get_motion_data();
}
if (rs2::motion_frame gyro_frame = frames.first_or_default(RS2_STREAM_GYRO))
{
    gyro_sample = gyro_frame.get_motion_data();
}

rs2::frame color_frame = frames.get_color_frame();
rs2::frame left_frame = frames.get_infrared_frame(1);
rs2::frame right_frame = frames.get_infrared_frame(2);
rs2::frame depth_frame = frames.get_depth_frame();
frame_counter = depth_frame.get_frame_number();
if(lastnum==frame_counter)
continue;

2.数据组织与保存

自动创建数据文件夹关键函数

//定义一个结构体，代表 linux里目录分层符号 ‘/’，再后面std：：find_if 查找符号的函数中会用到
struct pathseperator
{
    pathseperator ()
    { }

    bool
    operator () (char ch) const
    {
        return ch == '/';
    }
};

//检查路径是否已创建
int checkFolderExist(std::string const & name)
{
    if (access(name.c_str (), 0))
        return -1;
    return 0;
}

int checkandmkdir(std::string path_input)
{
    std::string sep = "/";
    //第一个参数为需要创建的全路径
   std::string target = std::string(path_input);
   
   //通过查找 / 的方式将每层路径拆开 
    std::vector<std::string> container;
    std::string::const_iterator const end = target.end ();
    std::string::const_iterator it = target.begin ();
    pathseperator pathsep = pathseperator ();
    while (it != end)
    {
        std::string::const_iterator sep = std::find_if (it, end, pathsep);
        container.push_back (std::string (it, sep));
        it = sep;
        if (it != end)
            ++it;
    }
	//将拆解的路径重新一层一层的拼接，并调用系统函数mkdir创建
    std::string path;
    for(int i=1; i<container.size(); i++) {
        path += sep;
        path += container[i];
        if(checkFolderExist(path) == 0) {
            continue;
        }
        int res = mkdir(path.c_str (), 0777);
    }
    return 0;
}

3.打印系统时间戳

获取ms级系统时间函数

//time_stamp_type - 需要获取的时间戳的级别，0表示秒级时间戳，1表示毫秒级时间戳，2表示微秒级时间戳，3表示纳秒级时间戳
std::string GetCurrentTimeStamp(int time_stamp_type = 0)
{
	std::chrono::system_clock::time_point now = std::chrono::system_clock::now();

	std::time_t now_time_t = std::chrono::system_clock::to_time_t(now);
	std::tm* now_tm = std::localtime(&now_time_t);

	char buffer[128];
	strftime(buffer, sizeof(buffer), "%F %T", now_tm);

	std::ostringstream ss;
	ss.fill('0');

	std::chrono::milliseconds ms;
	std::chrono::microseconds cs;
	std::chrono::nanoseconds ns;
	
	switch (time_stamp_type)
	{
	case 0:
		ss << buffer;
		break;
	case 1:
		ms = std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch()) % 1000;
		ss << buffer << ":" << ms.count();
		break;
	case 2:
		ms = std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch()) % 1000;
		cs = std::chrono::duration_cast<std::chrono::microseconds>(now.time_since_epoch()) % 1000000;
		ss << buffer << ":" << ms.count() << ":" << cs.count() % 1000;
		break;
	case 3:
		ms = std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch()) % 1000;
		cs = std::chrono::duration_cast<std::chrono::microseconds>(now.time_since_epoch()) % 1000000;
		ns = std::chrono::duration_cast<std::chrono::nanoseconds>(now.time_since_epoch()) % 1000000000;
		ss << buffer << ":" << ms.count() << ":" << cs.count() % 1000 << ":" << ns.count() % 1000;
		break;
	default:
		ss << buffer;
		break;
	}

	return ss.str();
}

三、实验测试

1.固定脉冲测试

(1)主机链接相机
在这里插入图片描述

(2)单片机向三个相机发送一百个10HZ脉冲，主机收到图片

(3)程序自动创建文件夹并按格式保存收到的图片

2.实车测试

略

总结

代码仍有小bug，比如IMU数据保存第一帧总是落后图片数据一帧；多个相机启动顺序不同初始化时间导致每个相机第一帧数据帧序号不一样；虽然静止初始化一段时间后再进行数据采集不影响最终数据准确性，但是总归是有一点瑕疵，以后有时间再完善一下。
完整代码可以从我的github下载：https://github.com/ser666/D435i_multicam_hardware_sync.git

独孤西

关注

10
点赞
踩
37

收藏

觉得还不错? 一键收藏
打赏
14
评论
realsense D435i 实现外部时钟触发硬件同步多相机数据采集

利用D435i相机，基于librealsenseSDK进行外部时钟硬件触发的多相机数据采集，包含D435i相机能采集到的所有数据：深度、彩色、双目灰度、加速度计、陀螺仪。
复制链接

扫一扫