add XVSDK VIO

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

@@ -19,16 +19,17 @@ public:
 
 public:
 	CameraSeerSense(
-		float imageRate=0.0f,
+		bool computeOdometry = false,
+		float imageRate = 0.0f,
 		const Transform & localTransform = Transform::getIdentity()
 	);
 	virtual ~CameraSeerSense();
 
-	void setIMU(bool imuPublished, bool publishInterIMU);
-
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
 	virtual bool isCalibrated() const;
 	virtual std::string getSerial() const;
+	virtual bool odomProvided() const;
+	virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime = 0.0);
 
 protected:
 	virtual SensorData captureImage(SensorCaptureInfo * info = 0);
@@ -36,8 +37,7 @@ protected:
 private:
 #ifdef RTABMAP_XVSDK
 	CameraModel cameraModel_;
-	bool imuPublished_;
+	bool computeOdometry_;
-	bool publishInterIMU_;
 	int imuId_;
 	int tofId_;
 	std::shared_ptr<xv::Device> device_;

corelib/src/camera/CameraSeerSense.cpp

@@ -12,12 +12,11 @@ bool CameraSeerSense::available()
 #endif
 }
 
-CameraSeerSense::CameraSeerSense(float imageRate, const Transform & localTransform) :
+CameraSeerSense::CameraSeerSense(bool computeOdometry, float imageRate, const Transform & localTransform) :
 	Camera(imageRate, localTransform)
 #ifdef RTABMAP_XVSDK
 	,
-	imuPublished_(true),
+	computeOdometry_(computeOdometry),
-	publishInterIMU_(false),
 	imuId_(0),
 	tofId_(0)
 #endif
@@ -35,6 +34,9 @@ CameraSeerSense::~CameraSeerSense()
 	if(tofId_)
 		device_->tofCamera()->unregisterColorDepthImageCallback(tofId_);
 
+	if(device_->slam())
+        device_->slam()->stop();
+
     if(device_->imuSensor())
 		device_->imuSensor()->stop();
 
@@ -47,16 +49,6 @@ CameraSeerSense::~CameraSeerSense()
 	dataReady_.release();
 }
 
-void CameraSeerSense::setIMU(bool imuPublished, bool publishInterIMU)
-{
-#ifdef RTABMAP_XVSDK
-	imuPublished_ = imuPublished;
-	publishInterIMU_ = publishInterIMU;
-#else
-	UERROR("CameraSeerSense: RTAB-Map is not built with XVisio SDK support!");
-#endif
-}
-
 bool CameraSeerSense::init(const std::string & calibrationFolder, const std::string & cameraName)
 {
 	UDEBUG("");
@@ -72,14 +64,14 @@ bool CameraSeerSense::init(const std::string & calibrationFolder, const std::str
 	UASSERT(device_->imuSensor());
 	device_->imuSensor()->start();
 	imuId_ = device_->imuSensor()->registerCallback([this](const xv::Imu & xvImu) {
-		if(imuPublished_ && xvImu.hostTimestamp > 0)
+		if(xvImu.hostTimestamp > 0)
 		{
-			if(publishInterIMU_)
+			if(isInterIMUPublishing())
 			{
 				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),
 						this->getLocalTransform());
-				UEventsManager::post(new IMUEvent(imu, xvImu.hostTimestamp));
+				this->postInterIMU(imu, xvImu.hostTimestamp);
 			}
 			else
 			{
@@ -90,6 +82,12 @@ bool CameraSeerSense::init(const std::string & calibrationFolder, const std::str
 		}
 	});
 
+	if(computeOdometry_)
+	{
+		UASSERT(device_->slam());
+		device_->slam()->start(xv::Slam::Mode::Mixed);
+	}
+
 	auto frameRate = xv::TofCamera::Framerate::FPS_30;
 	if(this->getImageRate() > 25)
 		frameRate = xv::TofCamera::Framerate::FPS_30;
@@ -172,6 +170,34 @@ std::string CameraSeerSense::getSerial() const
 	return "";
 }
 
+bool CameraSeerSense::odomProvided() const
+{
+#ifdef RTABMAP_XVSDK
+	return computeOdometry_;
+#else
+	return false;
+#endif
+}
+
+bool CameraSeerSense::getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime)
+{
+#ifdef RTABMAP_XVSDK
+	xv::Pose xvPose;
+	if(computeOdometry_ && device_->slam()->getPoseAt(xvPose, stamp))
+	{
+		pose = this->getLocalTransform() *
+		Transform(
+			xvPose.transform().rotation()[0], xvPose.transform().rotation()[1], xvPose.transform().rotation()[2], xvPose.transform().translation()[0],
+			xvPose.transform().rotation()[3], xvPose.transform().rotation()[4], xvPose.transform().rotation()[5], xvPose.transform().translation()[1],
+			xvPose.transform().rotation()[6], xvPose.transform().rotation()[7], xvPose.transform().rotation()[8], xvPose.transform().translation()[2]) *
+		this->getLocalTransform().inverse();
+		covariance = cv::Mat::eye(6, 6, CV_64FC1) * 0.0005;
+		return true;
+	}
+#endif
+	return false;
+}
+
 SensorData CameraSeerSense::captureImage(SensorCaptureInfo * info)
 {
 	SensorData data;
@@ -190,7 +216,7 @@ SensorData CameraSeerSense::captureImage(SensorCaptureInfo * info)
 	lastData_ = std::pair<double, std::pair<cv::Mat, cv::Mat>>();
 	dataMutex_.unlock();
 
-	if(imuPublished_ && !publishInterIMU_)
+	if(!isInterIMUPublishing())
 	{
 		cv::Vec3d gyro, acc;
 		std::map<double, std::pair<cv::Vec3d, cv::Vec3d>>::const_iterator iterA, iterB;
@@ -223,6 +249,18 @@ SensorData CameraSeerSense::captureImage(SensorCaptureInfo * info)
 		imuMutex_.unlock();
 		data.setIMU(IMU(gyro, cv::Mat::eye(3, 3, CV_64FC1), acc, cv::Mat::eye(3, 3, CV_64FC1), this->getLocalTransform()));
 	}
+
+	xv::Pose xvPose;
+	if(computeOdometry_ && device_->slam()->getPoseAt(xvPose, data.stamp()))
+	{
+		info->odomPose = this->getLocalTransform() *
+		Transform(
+			xvPose.transform().rotation()[0], xvPose.transform().rotation()[1], xvPose.transform().rotation()[2], xvPose.transform().translation()[0],
+			xvPose.transform().rotation()[3], xvPose.transform().rotation()[4], xvPose.transform().rotation()[5], xvPose.transform().translation()[1],
+			xvPose.transform().rotation()[6], xvPose.transform().rotation()[7], xvPose.transform().rotation()[8], xvPose.transform().translation()[2]) *
+		this->getLocalTransform().inverse();
+		info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 0.0005;
+	}
 #else
 	UERROR("CameraSeerSense: RTAB-Map is not built with XVisio SDK support!");
 #endif

guilib/src/PreferencesDialog.cpp

@@ -425,6 +425,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	if (!CameraSeerSense::available())
 	{
 		_ui->comboBox_cameraRGBD->setItemData(kSrcSeerSense - kSrcRGBD, 0, Qt::UserRole - 1);
+		_ui->comboBox_odom_sensor->setItemData(3, 0, Qt::UserRole - 1);
 	}
 	_ui->openni2_exposure->setEnabled(CameraOpenNI2::exposureGainAvailable());
 	_ui->openni2_gain->setEnabled(CameraOpenNI2::exposureGainAvailable());
@@ -6240,6 +6241,11 @@ PreferencesDialog::Src PreferencesDialog::getOdomSourceDriver() const
 		//Zed SDK
 		return kSrcStereoZed;
 	}
+	else if(_ui->comboBox_odom_sensor->currentIndex() == 3)
+	{
+		//XVisio SDK
+		return kSrcSeerSense;
+	}
 	else if(_ui->comboBox_odom_sensor->currentIndex() != 0)
 	{
 		UERROR("Not implemented!");
@@ -6408,7 +6414,7 @@ Camera * PreferencesDialog::createCamera(
 		bool odomOnly,
 		bool odomSensorExtrinsicsCalib)
 {
-	if(odomOnly && !(driver == kSrcStereoRealSense2 || driver == kSrcStereoZed))
+	if(odomOnly && !(driver == kSrcStereoRealSense2 || driver == kSrcStereoZed || driver == kSrcSeerSense))
 	{
 		QMessageBox::warning(this, tr("Odometry Sensor"),
 				tr("Driver %1 cannot support odometry only mode.").arg(driver), QMessageBox::Ok);
@@ -6807,9 +6813,13 @@ Camera * PreferencesDialog::createCamera(
 	{
 		UDEBUG("SeerSense");
 		camera = new CameraSeerSense(
+			getOdomSourceDriver() == kSrcSeerSense,
 			this->getGeneralInputRate(),
 			this->getSourceLocalTransform());
-		((CameraSeerSense*)camera)->setIMU(true, _ui->checkbox_publishInterIMU->isChecked());
+		camera->setInterIMUPublishing(
+			_ui->checkbox_publishInterIMU->isChecked(),
+			_ui->checkbox_publishInterIMU->isChecked() && getIMUFilteringStrategy()>0?
+					IMUFilter::create((IMUFilter::Type)(getIMUFilteringStrategy()-1), this->getAllParameters()):0);
 	}
 	else if(driver == kSrcUsbDevice)
 	{

guilib/src/ui/preferencesDialog.ui

@@ -8144,6 +8144,11 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                              <string>ZED sdk</string>
                             </property>
                            </item>
+                           <item>
+                            <property name="text">
+                             <string>SeerSense</string>
+                            </property>
+                           </item>
                           </widget>
                          </item>
                          <item row="0" column="1">
@@ -8741,7 +8746,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                          <property name="currentIndex">
                           <number>1</number>
                          </property>
-                         <widget class="QWidget" name="page_99">
+                         <widget class="QWidget" name="page_98">
                           <layout class="QVBoxLayout" name="verticalLayout_177">
                            <item>
                             <spacer name="verticalSpacer_83">
@@ -11892,7 +11897,7 @@ see Sqlite3 doc 'PRAGMA synchronous'.</string>
                        </widget>
                       </item>
                       <item row="4" column="1">
-                       <widget class="QLabel" name="label_755">
+                       <widget class="QLabel" name="label_757">
                         <property name="text">
                          <string>Likelihood ratio for VP.</string>
                         </property>