读取realsense d455双目及imu

问题定义

实时读取realsense数据喂给slam系统

代码

/**
rs_d455设备
*/#include <librealsense2/rs.hpp>
#include <iostream>#include "rs_common_device.h"// opencv
#include <opencv2/opencv.hpp>class RsD455Device: public rsCmmonDevice {public:using Ptr = std::shared_ptr<RsD455Device>;static constexpr int IMU_RATE = 200;static constexpr int NUM_CAMS = 2;        // 相机数量int frame_num = 0;int imu_num = 0;RsD455Device(bool manual_exposure, int skip_frames, int webp_quality, double exposure_value = 10.0);// 开启设备void start();// 关闭设备void stop();// 设置曝光bool setExposure(double exposure);  // in milliseconds// 设置跳过的framesvoid setSkipFrames(int skip);void setWebpQuality(int quality);private:// realsense的一些配置rs2::context context;rs2::config config;rs2::pipeline pipe;rs2::sensor sensor;rs2::pipeline_profile profile;
};std::string get_date();RsD455Device::RsD455Device(bool manual_exposure, int skip_frames,int webp_quality, double exposure_value):rsCmmonDevice("rsD455"){       std::cout << "open realsense d455" << std::endl;rsCmmonDevice::manual_exposure = manual_exposure;rsCmmonDevice::skip_frames = skip_frames;rsCmmonDevice::webp_quality = webp_quality;rs2::log_to_console(RS2_LOG_SEVERITY_ERROR);pipe = rs2::pipeline(context);config.enable_stream(RS2_STREAM_ACCEL, RS2_FORMAT_MOTION_XYZ32F);// 使能加速度config.enable_stream(RS2_STREAM_GYRO, RS2_FORMAT_MOTION_XYZ32F); // 使能角速度config.enable_stream(RS2_STREAM_INFRARED, 1, 640, 480, RS2_FORMAT_Y8, 30);// left IRconfig.enable_stream(RS2_STREAM_INFRARED, 2, 640, 480, RS2_FORMAT_Y8, 30);// right IR// config.disable_stream(RS2_STREAM_DEPTH);if (context.query_devices().size() == 0) {std::cout << "Waiting for device to be connected" << std::endl;rs2::device_hub hub(context);hub.wait_for_device();}for (auto& s : context.query_devices()[0].query_sensors()) {std::cout << "Sensor " << s.get_info(RS2_CAMERA_INFO_NAME)<< ". Supported options:" << std::endl;for (const auto& o : s.get_supported_options()) {std::cout << "\t" << rs2_option_to_string(o) << std::endl;}}auto device = context.query_devices()[0];std::cout << "Device " << device.get_info(RS2_CAMERA_INFO_NAME) << " connected" << std::endl;sensor = device.query_sensors()[0];if (manual_exposure) {std::cout << "Enabling manual exposure control" << std::endl;sensor.set_option(rs2_option::RS2_OPTION_ENABLE_AUTO_EXPOSURE, 0);sensor.set_option(rs2_option::RS2_OPTION_EXPOSURE, exposure_value * 1000);}// std::cout << "finish open camera" << std::endl;
}void RsD455Device::start() {auto callback = [&](const rs2::frame& frame) {if (auto fp = frame.as<rs2::motion_frame>()) {auto motion = frame.as<rs2::motion_frame>();if (motion && motion.get_profile().stream_type() == RS2_STREAM_GYRO &&motion.get_profile().format() == RS2_FORMAT_MOTION_XYZ32F) {imu_num += 1;} else if (motion &&motion.get_profile().stream_type() == RS2_STREAM_ACCEL &&motion.get_profile().format() == RS2_FORMAT_MOTION_XYZ32F) {}} else if (auto fs = frame.as<rs2::frameset>()) {std::vector<rs2::video_frame> vfs;for (int i = 0; i < NUM_CAMS; ++i) {rs2::video_frame vf = fs[i].as<rs2::video_frame>();if (!vf) {std::cout << "Weird Frame, skipping" << std::endl;return;}vfs.push_back(vf);}for (int i = 1; i < NUM_CAMS; ++i) {if (vfs[0].get_timestamp() != vfs[i].get_timestamp()) {return;}}if (frame_counter++ % skip_frames != 0) {return;}// 读取N个图像的数据for (int i = 0; i < NUM_CAMS; i++) {const auto& vf = vfs[i];int64_t t_ns = vf.get_timestamp() * 1e6;cv::Mat img = cv::Mat(cv::Size(640, 480), CV_8UC1, (void*)vfs[i].get_data());cv::imshow("img_"+std::to_string(i), img);}// 读取ircv::waitKey(1);std::cout << "frame num = " << frame_num << " imu_num = " << imu_num << std::endl;frame_num += 1;}return;};profile = pipe.start(config, callback);
}void RsD455Device::stop() {std::cout << "stop d455" << std::endl;
}// 设置手动曝光参数
bool RsD455Device::setExposure(double exposure) {if (!manual_exposure) return false;sensor.set_option(rs2_option::RS2_OPTION_EXPOSURE, exposure * 1000);return true;
}
// 设置跳过的帧数
void RsD455Device::setSkipFrames(int skip) { skip_frames = skip; }void RsD455Device::setWebpQuality(int quality) { webp_quality = quality; }int main(void){// realsense 设备RsD455Device d455_device(false, 1, 90, 10.0);// 开启设备和加载标定参数d455_device.start();// 开启相机while(1);return 0;
}

连接

开源链接