Роботы и другие сложные машины обычно оснащены самыми разнообразными датчиками. Часто при выполнении таких задач, как автономное вождение или сбор данных для автономной обработки, полезно синхронизировать различные датчики. Например, в моей работе с 3D-машинным обучением мне нужно было собрать набор данных, состоящий из облаков точек LiDAR, а также кадров камеры, чтобы служить контекстом.
Почему важно синхронизировать эти данные? Есть разные причины. Во-первых, разные датчики собирают данные с разной скоростью. Например, мой LiDAR собирает данные с частотой 10 Гц (10 выборок в секунду), а моя камера делает снимки с частотой 3 Гц. Для каждого изображения у нас есть около 3 облаков точек. Другая причина связана с тем, насколько быстро движется ваша машина. Если бы вы собирали данные с датчиков, установленных на автомобиле, разница между облаком точек, собранным в данный момент времени, и соответствующим контекстным изображением, полученным примерно через секунду, была бы большой, если автомобиль движется очень быстро. На изображении могут быть видны вещи, которые LiDAR не мог уловить ранее (закрытые объекты и т. д.).
Синхронизация данных датчиков имеет два очевидных преимущества. Во-первых, это позволяет нам отбрасывать ненужные данные с более быстрых датчиков, тем самым экономя место и усложняя последующую обработку. Во-вторых, убедившись, что данные собираются примерно в одно и то же время, мы уверены, что все данные представляют одно и то же состояние мира, воспринимаемое сенсорами в это время. В оставшейся части этой статьи я опишу, как я синхронизировал данные с LiDAR и камеры с помощью операционной системы роботов (ROS). Такой же подход можно использовать для произвольного количества и типа источников.
Во-первых, что такое ROS и почему я ее использовал? ROS — это промежуточное программное обеспечение для робототехники с открытым исходным кодом. Его основная цель — предоставлять услуги поверх операционной системы (в моем случае — Ubuntu), чтобы облегчить разработку приложений для робототехники. Две основные причины использования ROS заключаются в том, что он обеспечивает широко используемые функции (в нашем случае синхронизацию данных) и передачу сообщений между процессами (в моем случае SDK LiDAR и камера отправляют сообщения). Сбор и синхронизация данных без такого инструмента, как ROS, потребует большего объема кода и дополнительной сложности. Если вы работаете с роботами, стоит изучить ROS.
Итак, на компьютере моего робота работает узел ROS, предоставленный производителем моего LiDAR, который публикует сообщения типа PointCloud2 с частотой 10 Гц, а другой узел, предоставленный производителем моей камеры, публикует данные изображения с частотой 3 Гц. Узел ROS подписывается на обе темы (механизм обмена сообщениями ROS) для получения данных. Пакет ROS под названием message_filters используется для синхронизации тем с помощью синхронизатора политики ApproximateTime. Эта политика полезна, когда данные от разных датчиков не поступают в точное время, но могут быть собраны в пределах достаточно небольшой разницы во времени.
Некоторые из наиболее важных аспектов кода включают создание подписчиков на разделы LiDAR и данных камеры, создание синхронизатора с политикой ApproximateTime и регистрацию обратного вызова для синхронизатора, как показано в фрагменте кода ниже.
// Create the subscribers message_filters::Subscriber<Image> image_sub(nh, "/zed/rgb/image_rect_color", 1); message_filters::Subscriber<CameraInfo> camera_info_sub(nh, "/zed/rgb/camera_info", 1); message_filters::Subscriber<PointCloud2> point_cloud_sub(nh, "/rslidar_points", 1); // Create the synchronizer typedef sync_policies::ApproximateTime<Image, CameraInfo, PointCloud2> MySyncPolicy; // ApproximateTime takes a queue size as its constructor argument, hence MySyncPolicy(10) Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), image_sub, camera_info_sub, point_cloud_sub); sync.registerCallback(boost::bind(&callback, _1, _2, _3));
Зарегистрированный обратный вызов будет запускаться всякий раз, когда данные LiDAR и камеры поступают почти одновременно. Любые другие данные будут игнорироваться. В рамках обратного вызова можно сделать несколько вещей, но в моем случае я просто сохраняю все синхронизированные пары облако точек-изображение для автономной обработки.
// Synchronizer Callback, it will be called when camera and LiDAR data are in sync.
void callback(const ImageConstPtr& image, const CameraInfoConstPtr& camara_info, const PointCloud2ConstPtr& point_cloud)
{
printf("%s\n", "All data in sync!" );
// Process data
}
Основная причина сбора таких данных заключается в том, что я могу, например, использовать контекстное изображение, чтобы аннотировать облако точек для обнаружения объектов. Гораздо проще узнать, что находится перед роботом, из контекстного изображения, чем из облака точек. Наконец, как описано в предыдущем посте, в облаке точек можно выполнить некоторую фильтрацию, чтобы сохранить только точки, непосредственно видимые камерой, как показано на изображении ниже.
Полный код представлен ниже. Обратите внимание, что для запуска кода вам необходимо установить ROS, а также несколько других библиотек. Инструкции по установке всех этих библиотек выходят за рамки этой статьи.
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/PointCloud2.h>
#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <iostream>
#include <fstream>
#include <sstream>
#include <boost/filesystem.hpp>
#include <ros/package.h>
static const std::string OPENCV_WINDOW = "Image window";
using namespace sensor_msgs;
using namespace message_filters;
using namespace std;
std::string root_output_dir, lidar_output_dir, camera_output_dir;
bool debug;
// Synchronizer Callback, it will be called when camera and LiDAR data are in sync.
void callback(const ImageConstPtr& image, const CameraInfoConstPtr& camara_info, const PointCloud2ConstPtr& point_cloud)
{
printf("%s\n", "All data in sync!" );
// Convert ROS PointCloud2 to PCL point cloud
pcl::PointCloud<pcl::PointXYZI> cloud;
pcl::fromROSMsg(*point_cloud, cloud);
// Create a date string from the point cloud's timestamp to use in the file name of the saved data
const int output_size = 100;
char output[output_size];
std::time_t raw_time = static_cast<time_t>(point_cloud->header.stamp.sec);
struct tm* timeinfo = localtime(&raw_time);
std::strftime(output, output_size, "lidar_%Y_%m_%d_%H_%M_%S", timeinfo);
// Creates a string containing the millisencods to be added to the previously created date string
std::stringstream ss;
ss << std::setw(9) << std::setfill('0') << point_cloud->header.stamp.nsec;
const size_t fractional_second_digits = 4;
// Combine all of the pieces to get the output file name
std::string output_file = lidar_output_dir + "/" + std::string(output) + "." + ss.str().substr(0, fractional_second_digits)+".pcd";
// Save the point cloud as a PCD file
pcl::io::savePCDFileASCII (output_file, cloud);
printf("%s\n", output_file.c_str() );
// Convert the ROS image to an OpenCV image
cv_bridge::CvImagePtr cv_ptr;
try{
cv_ptr = cv_bridge::toCvCopy(image, sensor_msgs::image_encodings::BGR8);
}catch (cv_bridge::Exception& e){
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
// Update GUI Window
if (debug){
cv::imshow(OPENCV_WINDOW, cv_ptr->image);
cv::waitKey(3);
}
// Create the filename for the image
output_file = camera_output_dir + "/" + std::string(output) + "." + ss.str().substr(0, fractional_second_digits)+".jpg";
// Save the image
cv::imwrite(output_file, cv_ptr->image);
}
int main(int argc, char** argv)
{
// Initialize the ROS node
ros::init(argc, argv, "lidar_and_cam_synchronizer");
ros::NodeHandle nh("~");
// Get the paremeters or use default values
nh.param("root_output_dir", root_output_dir, ros::package::getPath("shl_robot")+"/data/lidar");
nh.param("debug", debug, false);
// Create the subscribers
message_filters::Subscriber<Image> image_sub(nh, "/zed/rgb/image_rect_color", 1);
message_filters::Subscriber<CameraInfo> camera_info_sub(nh, "/zed/rgb/camera_info", 1);
message_filters::Subscriber<PointCloud2> point_cloud_sub(nh, "/rslidar_points", 1);
// Create the synchronizer
typedef sync_policies::ApproximateTime<Image, CameraInfo, PointCloud2> MySyncPolicy;
// ApproximateTime takes a queue size as its constructor argument, hence MySyncPolicy(10)
Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), image_sub, camera_info_sub, point_cloud_sub);
sync.registerCallback(boost::bind(&callback, _1, _2, _3));
// Create the folder for the current run. A new folder will be created each time the node runs
const int output_size = 100;
char output[output_size];
std::time_t raw_time = static_cast<time_t>(ros::Time::now().sec);
struct tm* timeinfo = localtime(&raw_time);
std::strftime(output, output_size, "run_%Y_%m_%d_%H_%M_%S", timeinfo);
// Combine all of the pieces to get the output folders for this run
lidar_output_dir = root_output_dir + "/" + std::string(output)+"/pcd";
camera_output_dir = root_output_dir + "/" + std::string(output)+"/jpg";
boost::filesystem::create_directories(lidar_output_dir);
boost::filesystem::create_directories(camera_output_dir);
std::cout << lidar_output_dir << std::endl;
// Call spin() to let the node run until stopped.
ros::spin();
return 0;
}
