Fixed DBReader odometry ignored

introlab · Jun 25, 2024 · 7d970ef · 7d970ef
1 parent 1ab0133
commit 7d970ef
Show file tree

Hide file tree

Showing 5 changed files with 21 additions and 5 deletions.
diff --git a/corelib/include/rtabmap/core/DBReader.h b/corelib/include/rtabmap/core/DBReader.h
@@ -82,6 +82,7 @@ class RTABMAP_CORE_EXPORT DBReader : public Camera {
 	virtual bool isCalibrated() const;
 	virtual std::string getSerial() const;
 	virtual bool odomProvided() const {return !_odometryIgnored;}
+	virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime = 0.06);
 
 	const DBDriver * driver() const {return _dbDriver;}
 

diff --git a/corelib/src/DBReader.cpp b/corelib/src/DBReader.cpp
@@ -268,6 +268,12 @@ std::string DBReader::getSerial() const
 	return "DBReader";
 }
 
+bool DBReader::getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime)
+{
+	UERROR("DBReader only provides pose when capturing data, it cannot provide asynchronous pose.");
+	return false;
+}
+
 SensorData DBReader::captureImage(SensorCaptureInfo * info)
 {
 	SensorData data = this->getNextData(info);

diff --git a/corelib/src/SensorCaptureThread.cpp b/corelib/src/SensorCaptureThread.cpp
@@ -462,7 +462,11 @@ void SensorCaptureThread::mainLoop()
 
 		Transform pose;
 		cv::Mat covariance;
-		if(_odomSensor->getPose(data.stamp()+_poseTimeOffset, pose, covariance, _poseWaitTime>0?_poseWaitTime:0))
+		if(!info.odomPose.isNull() && _lidar == 0 && _odomSensor == _camera)
+		{
+			// Do nothing, we have already the pose
+		}
+		else if(_odomSensor->getPose(data.stamp()+_poseTimeOffset, pose, covariance, _poseWaitTime>0?_poseWaitTime:0))
 		{
 			info.odomPose = pose;
 			info.odomCovariance = covariance;

diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp
@@ -5851,7 +5851,12 @@ void MainWindow::startDetection()
 		}
 	}
 
-	if(_preferencesDialog->getOdomSourceDriver() != PreferencesDialog::kSrcUndef)
+	if(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcDatabase &&
+	   camera && camera->odomProvided())
+	{
+		odomSensor = camera;
+	}
+	else if(_preferencesDialog->getOdomSourceDriver() != PreferencesDialog::kSrcUndef)
 	{
 		if(camera == 0 ||
 		   (_preferencesDialog->getOdomSourceDriver() != _preferencesDialog->getSourceDriver() &&

diff --git a/tools/Report/main.cpp b/tools/Report/main.cpp
@@ -83,9 +83,9 @@ void showUsage()
 			"    --report           Export all evaluation statistics values in report.txt \n"
 			"    --loc [#]          Show localization statistics for each \"Statistic/Id\" per\n"
 			"                       session. Optionally set number 1=min,2=max,4=mean,8=stddev,16=total,32=nonnull%%\n"
-			"                       to show cumulative results on console.\n"
-			"    --loc_delay #      Delay to split sessions for localization statistics (default 60 seconds)\n"
-			"                       (it is a mask, we can combine those numbers, e.g., 63 for all) \n"
+			"                       to show cumulative results on console (it is a mask, \n"
+			"                       we can combine those numbers, e.g., 63 for all) \n"
+			"    --loc_delay #      Delay to split sessions for localization statistics (default 60 seconds).\n"
 			"    --ignore_inter_nodes  Ignore intermediate poses and statistics.\n"
 			"    --udebug           Show debug log.\n"
 			"    --help,-h          Show usage\n\n");