add xvDepthColor

corelib/include/rtabmap/core/CameraRGBD.h

@@ -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>

corelib/include/rtabmap/core/CameraStereo.h

@@ -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>

corelib/include/rtabmap/core/camera/CameraSeerSense.h

@@ -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, std::pair<cv::Vec3d, cv::Vec3d>> imuBuffer_;
-	std::map<double, cv::Vec3f> gyroBuffer_;
+	std::pair<double, std::pair<cv::Mat, cv::Mat>> lastData_;
 	UMutex imuMutex_;
+	UMutex dataMutex_;
 #endif
 };
 

corelib/src/camera/CameraSeerSense.cpp

@@ -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())
+	UASSERT(device_->imuSensor());
-	{
+	device_->imuSensor()->registerCallback([this](const xv::Imu & xvImu) {
-		imuLocalTransform_ = this->getLocalTransform() * imuLocalTransform_;
+		if(imuPublished_ && xvImu.hostTimestamp > 0)
-		UINFO("IMU local transform = %s", imuLocalTransform_.prettyPrint().c_str());
+		{
-		device_->imuSensor()->registerCallback([this](const xv::Imu & xvImu) {
 			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]));
+				imuBuffer_.emplace_hint(imuBuffer_.end(), xvImu.hostTimestamp,
-				gyroBuffer_.emplace_hint(gyroBuffer_.end(), xvImu.hostTimestamp, cv::Vec3d(xvImu.gyro[0], xvImu.gyro[1], xvImu.gyro[2]));
+					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