125 lines
3.8 KiB
C++
125 lines
3.8 KiB
C++
//
|
|
// Created by linbo on 2025/6/18.
|
|
//
|
|
|
|
#ifndef REALSENSE_CAMERA_H
|
|
#define REALSENSE_CAMERA_H
|
|
|
|
#include "camera/abstract_camera.h"
|
|
#include "common/base/ring_buffer.h"
|
|
#include "devices/camera/common/include/camera_stream_encoder.h"
|
|
#include <librealsense2/rs.hpp>
|
|
#include <librealsense2/hpp/rs_internal.hpp>
|
|
|
|
namespace cmvr::device{
|
|
enum StreamMode {RGBD_MODE, COLOR_MODE,DEPTH_MODE};
|
|
|
|
|
|
class RealsenseCamera final : public AbstractCamera {
|
|
public:
|
|
RealsenseCamera(const config::RealSenseCameraConfig& cam);
|
|
~RealsenseCamera() override;
|
|
|
|
std::string typeName() const override { return "RealSenseCamera"; }
|
|
bool init() override;
|
|
bool start() override;
|
|
bool stop() override;
|
|
void getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics) override;
|
|
void getDepthImage(cv::Mat& depth, Rs2Intrinsics& intrinsics) override;
|
|
void getRGBDImages(cv::Mat &color, cv::Mat &depth, Rs2Intrinsics& intrinsics) override;
|
|
void startRecording(const std::string &video_path) override;
|
|
void stopRecording() override;
|
|
void pauseRecording() override;
|
|
void resumeRecording() override;
|
|
|
|
void getEncodedFrame(StreamFrameData& frame_data, size_t& index) override;
|
|
bool getLatestEncodedFrame(StreamFrameData& frame_data, size_t& next_index) override;
|
|
|
|
bool startStreaming() override;
|
|
void stopStreaming() override;
|
|
|
|
|
|
Eigen::Vector3f get3DPointFromPixel(int u, int v) override;
|
|
private:
|
|
rs2::frameset get_frameset(bool align);
|
|
void streaming_worker_();
|
|
void recording_worker_();
|
|
private:
|
|
int fps_;
|
|
int width_;
|
|
int height_;
|
|
int encode_width_;
|
|
int encode_height_;
|
|
std::string serial_;
|
|
std::string align_mode_;
|
|
cv::VideoCapture cap_;
|
|
size_t buffer_size_;
|
|
std::string codec_;
|
|
bool enable_stream_timestamp_{false};
|
|
CameraMode mode_;
|
|
StreamMode stream_mode_;
|
|
|
|
|
|
int max_retry_;
|
|
bool is_sync_;
|
|
rs2::syncer sync_;
|
|
std::shared_ptr<rs2::align> align_;
|
|
rs2::pipeline pipe_;
|
|
rs2::pipeline_profile profile_;
|
|
rs2::config rs_cfg_;
|
|
rs2_intrinsics intrinsics_;
|
|
|
|
std::mutex frame_mtx_;
|
|
|
|
cv::Mat dist_coeffs_;
|
|
cv::Mat latest_rgb_;
|
|
|
|
std::shared_ptr<SPMCRingBuffer<StreamFrameData>> stream_frame_buffer_;
|
|
std::string current_video_path_;
|
|
|
|
std::mutex ctrl_mtx_{};
|
|
std::unique_ptr<cv::VideoWriter> video_writer_;
|
|
std::shared_ptr<std::thread> stream_thread_;//采集线程
|
|
std::shared_ptr<std::thread> encode_thread_;//采集线程
|
|
std::shared_ptr<std::thread> recording_thread_;
|
|
|
|
std::string output_path_;
|
|
bool is_recording_ = false;
|
|
// ffmpeg录像
|
|
AVFormatContext* format_context_ = nullptr;
|
|
AVCodecContext* codec_context_ = nullptr;
|
|
AVStream* stream_ = nullptr;
|
|
AVFrame* frame_ = nullptr;
|
|
AVPacket* packet_ = nullptr;
|
|
SwsContext* sws_context_ = nullptr;
|
|
int frame_count_ = 0;
|
|
|
|
//流模式
|
|
std::shared_ptr<FfmpegEncoderInfo> rgbEncoder_;//rgb图像编码
|
|
std::shared_ptr<FfmpegEncoderInfo> depthEncoder_;//深度图编码
|
|
size_t streamIndex_ = 0;
|
|
size_t recordingIndex_ = 0;
|
|
size_t getImageIndex_ = 0;
|
|
|
|
bool is_streaming_running = false;
|
|
bool is_recording_running = false;
|
|
int stream_count_ = 0;
|
|
|
|
cv::Mat latest_depth_;
|
|
std::mutex depth_mtx_;
|
|
|
|
rs2_intrinsics Kd_; // 深度相机内参
|
|
rs2_intrinsics Kc_; // RGB相机内参
|
|
rs2_extrinsics Ec2d_; // rgb -> 深度 的外参
|
|
rs2_extrinsics Ed2c_; // rgb <- 深度 的外参
|
|
|
|
|
|
config::RealSenseCameraConfig camera_;
|
|
};
|
|
}
|
|
|
|
|
|
|
|
|
|
#endif //REALSENSE_CAMERA_H
|