i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

add xvDepthColor

Browse Source
This commit is contained in:
Borong Yuan
2024-05-28 17:08:49 +08:00
parent 3d5f1ad5c2
commit fb37b3adba
4 changed files with 72 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
corelib/include/rtabmap/core/CameraRGBD.h
View File

@@ -37,4 +37,4 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/camera/CameraRealSense2.h>
#include <rtabmap/core/camera/CameraRGBDImages.h>
#include <rtabmap/core/camera/CameraK4A.h>
#include <rtabmap/core/camera/CameraSeerSense.h>

1
corelib/include/rtabmap/core/CameraStereo.h
View File

@@ -36,4 +36,3 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/camera/CameraStereoTara.h>
#include <rtabmap/core/camera/CameraMyntEye.h>
#include <rtabmap/core/camera/CameraDepthAI.h>
#include <rtabmap/core/camera/CameraSeerSense.h>

8
corelib/include/rtabmap/core/camera/CameraSeerSense.h
View File

@@ -1,5 +1,6 @@
#pragma once
#include "rtabmap/core/CameraModel.h"
#include "rtabmap/core/Camera.h"
#include "rtabmap/core/Version.h"
@@ -29,13 +30,14 @@ public:
private:
#ifdef RTABMAP_XVSDK
Transform imuLocalTransform_;
CameraModel cameraModel_;
bool imuPublished_;
bool publishInterIMU_;
std::shared_ptr<xv::Device> device_;
std::map<double, cv::Vec3f> accBuffer_;
std::map<double, cv::Vec3f> gyroBuffer_;
std::map<double, std::pair<cv::Vec3d, cv::Vec3d>> imuBuffer_;
std::pair<double, std::pair<cv::Mat, cv::Mat>> lastData_;
UMutex imuMutex_;
UMutex dataMutex_;
#endif
};

77
corelib/src/camera/CameraSeerSense.cpp
View File

@@ -27,6 +27,14 @@ CameraSeerSense::CameraSeerSense(float imageRate, const Transform & localTransfo
CameraSeerSense::~CameraSeerSense()
{
if(device_->imuSensor())
device_->imuSensor()->stop();
if(device_->colorCamera())
device_->colorCamera()->stop();
if(device_->tofCamera())
device_->tofCamera()->stop();
}
void CameraSeerSense::setIMU(bool imuPublished, bool publishInterIMU)
@@ -43,7 +51,7 @@ bool CameraSeerSense::init(const std::string & calibrationFolder, const std::str
{
UDEBUG("");
#ifdef RTABMAP_XVSDK
auto devices = xv::getDevices(3.);
auto devices = xv::getDevices(3);
if(devices.empty())
{
UERROR("Timeout for SeerSense device detection.");
@@ -51,26 +59,73 @@ bool CameraSeerSense::init(const std::string & calibrationFolder, const std::str
}
device_ = devices.begin()->second;
if(imuPublished_ && device_->imuSensor())
{
imuLocalTransform_ = this->getLocalTransform() * imuLocalTransform_;
UINFO("IMU local transform = %s", imuLocalTransform_.prettyPrint().c_str());
device_->imuSensor()->registerCallback([this](const xv::Imu & xvImu) {
UASSERT(device_->imuSensor());
device_->imuSensor()->registerCallback([this](const xv::Imu & xvImu) {
if(imuPublished_ && xvImu.hostTimestamp > 0)
{
if(publishInterIMU_)
{
IMU imu(cv::Vec3d(xvImu.gyro[0], xvImu.gyro[1], xvImu.gyro[2]), cv::Mat::eye(3,3,CV_64FC1),
cv::Vec3d(xvImu.accel[0], xvImu.accel[1], xvImu.accel[2]), cv::Mat::eye(3,3,CV_64FC1),
imuLocalTransform_);
this->getLocalTransform());
UEventsManager::post(new IMUEvent(imu, xvImu.hostTimestamp));
}
else
{
UScopeMutex lock(imuMutex_);
accBuffer_.emplace_hint(accBuffer_.end(), xvImu.hostTimestamp, cv::Vec3d(xvImu.accel[0], xvImu.accel[1], xvImu.accel[2]));
gyroBuffer_.emplace_hint(gyroBuffer_.end(), xvImu.hostTimestamp, cv::Vec3d(xvImu.gyro[0], xvImu.gyro[1], xvImu.gyro[2]));
imuBuffer_.emplace_hint(imuBuffer_.end(), xvImu.hostTimestamp,
std::make_pair(cv::Vec3d(xvImu.gyro[0], xvImu.gyro[1], xvImu.gyro[2]), cv::Vec3d(xvImu.accel[0], xvImu.accel[1], xvImu.accel[2])));
}
});
}
}
});
auto frameRate = xv::TofCamera::Framerate::FPS_30;
if(this->getImageRate() > 25)
frameRate = xv::TofCamera::Framerate::FPS_30;
else if(this->getImageRate() > 20)
frameRate = xv::TofCamera::Framerate::FPS_25;
else if(this->getImageRate() > 15)
frameRate = xv::TofCamera::Framerate::FPS_20;
else if(this->getImageRate() > 10)
frameRate = xv::TofCamera::Framerate::FPS_15;
else if(this->getImageRate() > 5)
frameRate = xv::TofCamera::Framerate::FPS_10;
else if(this->getImageRate() > 0)
frameRate = xv::TofCamera::Framerate::FPS_5;
UASSERT(device_->colorCamera());
device_->colorCamera()->setResolution(xv::ColorCamera::Resolution::RGB_640x480);
UASSERT(device_->tofCamera());
device_->tofCamera()->setSonyTofSetting(xv::TofCamera::SonyTofLibMode::IQMIX_DF, xv::TofCamera::Resolution::QVGA, frameRate);
device_->tofCamera()->start();
UASSERT(!device_->tofCamera()->calibration().empty());
auto xvTofCalib = device_->tofCamera()->calibration()[0];
UASSERT(!xvTofCalib.pdcm.empty());
cameraModel_ = CameraModel(device_->id(), xvTofCalib.pdcm[0].fx, xvTofCalib.pdcm[0].fy, xvTofCalib.pdcm[0].u0, xvTofCalib.pdcm[0].v0,
this->getLocalTransform() * Transform(
xvTofCalib.pose.rotation()[0], xvTofCalib.pose.rotation()[1], xvTofCalib.pose.rotation()[2], xvTofCalib.pose.translation()[0],
xvTofCalib.pose.rotation()[3], xvTofCalib.pose.rotation()[4], xvTofCalib.pose.rotation()[5], xvTofCalib.pose.translation()[1],
xvTofCalib.pose.rotation()[6], xvTofCalib.pose.rotation()[7], xvTofCalib.pose.rotation()[8], xvTofCalib.pose.translation()[2]
), 0.0f, cv::Size(xvTofCalib.pdcm[0].w, xvTofCalib.pdcm[0].h)).scaled(0.5);
lastData_ = std::pair<double, std::pair<cv::Mat, cv::Mat>>();
device_->tofCamera()->registerColorDepthImageCallback([this](const xv::DepthColorImage & xvDepthColorImage) {
if(xvDepthColorImage.hostTimestamp > 0)
{
cv::Mat color = cv::Mat::zeros(cameraModel_.imageSize(), CV_8UC3);
color.forEach<cv::Vec3b>([&](cv::Vec3b& pixel, const int position[]) -> void {
const auto rgb = reinterpret_cast<std::uint8_t const *>(xvDepthColorImage.data.get() + (position[0]*cameraModel_.imageWidth()+position[1]) * 7);
pixel = cv::Vec3b(rgb[2], rgb[1], rgb[0]);
});
cv::Mat depth = cv::Mat::zeros(cameraModel_.imageSize(), CV_32FC1);
depth.forEach<float>([&](float &pixel, const int position[]) -> void {
pixel = *reinterpret_cast<float const *>(xvDepthColorImage.data.get() + (position[0]*cameraModel_.imageWidth()+position[1]) * 7 + 3);
});
UScopeMutex lock(dataMutex_);
lastData_ = std::make_pair(xvDepthColorImage.hostTimestamp, std::make_pair(color, depth));
}
});
return true;
#else