Hi,
I have an Oak D Lite camera and am trying to perform SLAM + 3D spatial location estimation based on output from my YOLO object detection algorithm running on my robot's processor. I have the following requirements -
- Input to my object detection model must be a small RGB image for fast processing (640x480)
- Each point in the RGB image should match the Depth image (for spatial location estimation) and both images should be time-synchronized
- RGB and Depth images should be published at 30 FPS so I can use it for RGBD-SLAM
In my attempt to meet the above goals, I have written the script shared below. However, I don't think my RGB and Depth images are aligned when I just use color-camera Preview frames at 640x480 size (each point in my depth image does not correspond to the same point on my rgb image). And when I use stereo.DepthAlign(dai::CameraBoardSocket::CAM_A) to align depth and rgb, my depth image FPS is much lesser than 30 Hz (around 5-10Hz). How can I improve my script to achieve my requirements?
void OakDLite::camera_worker_(){
// Create pipeline
dai::Pipeline pipeline;
// Define sources and outputs
auto monoLeft = pipeline.create<dai::node::MonoCamera>();
auto monoRight = pipeline.create<dai::node::MonoCamera>();
auto stereo = pipeline.create<dai::node::StereoDepth>();
auto colorCam = pipeline.create<dai::node::ColorCamera>();
auto sync = pipeline.create<dai::node::Sync>();
auto imu = pipeline.create<dai::node::IMU>();
auto xoutGrp = pipeline.create<dai::node::XLinkOut>();
auto xoutIMU = pipeline.create<dai::node::XLinkOut>();
// XLinkOut
xoutGrp->setStreamName("xout");
xoutIMU->setStreamName("imu");
// Properties
float fps = 30;
monoLeft->setResolution(dai::MonoCameraProperties::SensorResolution::THE_480_P);
monoLeft->setFps(fps);
monoLeft->setCamera("left");
monoRight->setResolution(dai::MonoCameraProperties::SensorResolution::THE_480_P);
monoRight->setFps(fps);
monoRight->setCamera("right");
// StereoDepth
stereo->setDefaultProfilePreset(dai::node::StereoDepth::PresetMode::HIGH_ACCURACY);
stereo->setRectifyEdgeFillColor(0);
stereo->initialConfig.setMedianFilter(dai::MedianFilter::KERNEL_7x7);
stereo->setLeftRightCheck(true);
stereo->setExtendedDisparity(false);
stereo->setSubpixel(true);
// ColorCamera
colorCam->setPreviewSize(640, 480);
colorCam->setBoardSocket(dai::CameraBoardSocket::CAM_A);
colorCam->setResolution(dai::ColorCameraProperties::SensorResolution::THE_1080_P);
colorCam->setFps(fps);
// IMU
imu->enableIMUSensor(dai::IMUSensor::ACCELEROMETER_RAW, 250);
imu->enableIMUSensor(dai::IMUSensor::GYROSCOPE_RAW, 250);
imu->setBatchReportThreshold(1);
imu->setMaxBatchReports(10);
//Sync
sync->setSyncThreshold(std::chrono::milliseconds(100));
// StereoDepth Configuration
auto config = stereo->initialConfig.get();
config.postProcessing.speckleFilter.enable = true;
config.postProcessing.speckleFilter.speckleRange = 10;
config.postProcessing.temporalFilter.enable = false;
config.postProcessing.spatialFilter.enable = false;
config.postProcessing.brightnessFilter.maxBrightness = 250;
config.postProcessing.decimationFilter.decimationFactor = 2; //4;
config.postProcessing.decimationFilter.decimationMode = dai::RawStereoDepthConfig::PostProcessing::DecimationFilter::DecimationMode::NON_ZERO_MEDIAN;
config.algorithmControl.depthUnit = dai::RawStereoDepthConfig::AlgorithmControl::DepthUnit::MILLIMETER;
stereo->initialConfig.set(config);
// Linking
monoLeft->out.link(stereo->left);
monoRight->out.link(stereo->right);
stereo->depth.link(sync->inputs["depth"]);
colorCam->preview.link(sync->inputs["rgb"]);
stereo->syncedRight.link(sync->inputs["right"]);
stereo->syncedLeft.link(sync->inputs["left"]);
// stereo->setDepthAlign(dai::CameraBoardSocket::CAM_A);
sync->out.link(xoutGrp->input);
imu->out.link(xoutIMU->input);
// Connect to device and start pipeline
// dai::Device device(pipeline);
auto device_infos = dai::Device::getAllAvailableDevices();
std::shared_ptr<dai::Device> device;
device = std::make_shared<dai::Device>(pipeline);
auto queue = device->getOutputQueue("xout", 4, false);
auto imuQueue = device->getOutputQueue("imu", 50, false);
bool firstTs = false;
auto baseTs = std::chrono::time_point<std::chrono::steady_clock, std::chrono::steady_clock::duration>();
while(!kill_thread_) {
rclcpp::Time current_time = now();
auto msgGrp = queue->tryGet<dai::MessageGroup>();
if(msgGrp != nullptr){
for(auto& frm : *msgGrp) {
auto imgFrm = std::dynamic_pointer_cast<dai::ImgFrame>(frm.second);
cv::Mat img = imgFrm->getCvFrame();
// More code below but omitted (gets the image and IMU frames and publishes them in ROS2).
