Merge branch 'master' of github.com:introlab/rtabmap into gtest

.github/workflows/cmake.yml

@@ -19,19 +19,26 @@ jobs:
       fail-fast: false
       matrix:
         os: [ubuntu-24.04, ubuntu-22.04]
+        include:
+        - os: ubuntu-22.04
+          extra_deps: "libunwind-dev libceres-dev"
+          extra_cmake_def: ""
+        - os: ubuntu-24.04
+          extra_deps: "libg2o-dev libceres-dev"
+          extra_cmake_def: "-DWITH_CERES=ON"
 
     steps:    
     - name: Install dependencies
       run: |
         DEBIAN_FRONTEND=noninteractive
         sudo apt-get update
-        sudo apt-get -y install libopencv-dev libpcl-dev git cmake software-properties-common libyaml-cpp-dev
+        sudo apt-get -y install libopencv-dev libpcl-dev git cmake software-properties-common libyaml-cpp-dev ${{ matrix.extra_deps }}
 
     - uses: actions/checkout@v4
 
     - name: Configure CMake
       run: |
-        cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}}
+        cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} ${{ matrix.extra_cmake_def }}
 
     - name: Build
       run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}}

CMakeLists.txt

@@ -518,21 +518,30 @@ IF(WITH_G2O)
          get_target_property(G2O_INCLUDES g2o::core INTERFACE_INCLUDE_DIRECTORIES)
          MESSAGE(STATUS "g2o include dir: ${G2O_INCLUDES}")
          FIND_FILE(G2O_FACTORY_FILE g2o/core/factory.h 
-         PATHS ${G2O_INCLUDES}
-         NO_DEFAULT_PATH)
-       FILE(READ ${G2O_FACTORY_FILE} TMPTXT)
-       STRING(FIND "${TMPTXT}" "shared_ptr" matchres)
-       IF(${matchres} EQUAL -1)
-           MESSAGE(STATUS "Old g2o factory version detected without shared ptr (factory file: ${G2O_FACTORY_FILE}).")
-           SET(G2O_CPP11 2)
-       ELSE()
-           MESSAGE(STATUS "Latest g2o factory version detected with shared ptr (factory file: ${G2O_FACTORY_FILE}).")
-           SET(G2O_CPP11 1)
-       ENDIF()
+            PATHS ${G2O_INCLUDES}
+            NO_DEFAULT_PATH)
+         FILE(READ ${G2O_FACTORY_FILE} TMPTXT)
+         STRING(FIND "${TMPTXT}" "shared_ptr" matchres)
+         IF(${matchres} EQUAL -1)
+            MESSAGE(STATUS "Old g2o factory version detected without shared ptr (factory file: ${G2O_FACTORY_FILE}).")
+            SET(G2O_CPP11 2)
+         ELSE()
+            MESSAGE(STATUS "Latest g2o factory version detected with shared ptr (factory file: ${G2O_FACTORY_FILE}).")
+            SET(G2O_CPP11 1)
+         ENDIF()
     ELSE()
         FIND_PACKAGE(G2O QUIET)
         IF(G2O_FOUND)
             MESSAGE(STATUS "Found g2o: ${G2O_INCLUDE_DIRS}")
+            FIND_FILE(G2O_FACTORY_FILE g2o/core/factory.h 
+               PATHS ${G2O_INCLUDES}
+               NO_DEFAULT_PATH)
+            FILE(READ ${G2O_FACTORY_FILE} TMPTXT)
+            STRING(FIND "${TMPTXT}" "shared_ptr" matchres)
+            IF(NOT ${matchres} EQUAL -1)
+               MESSAGE(STATUS "Latest g2o factory version detected with shared ptr (factory file: ${G2O_FACTORY_FILE}).")
+               SET(G2O_CPP11 1)
+            ENDIF()
         ENDIF(G2O_FOUND)
     ENDIF()
 ENDIF(WITH_G2O)

corelib/include/rtabmap/core/OdometryThread.h

@@ -67,8 +67,8 @@ private:
 	unsigned int _dataBufferMaxSize;
 	bool _resetOdometry;
 	Transform _resetPose;
-	double _lastImuStamp;
-	double _imuEstimatedDelay;
+	double _oldestAsyncImuStamp;
+	double _newestAsyncImuStamp;
 };
 
 } // namespace rtabmap

corelib/src/Graph.cpp

@@ -445,6 +445,10 @@ bool importPoses(
 				std::list<std::string> strList = uSplit(str);
 				if((strList.size() >=  8 && format!=11) || (strList.size() ==  9 && format==11))
 				{
+					if(!uIsNumber(strList.front())) {
+						UWARN("Skipping \"%s\"", str.c_str());
+						continue;
+					}
 					double stamp = uStr2Double(strList.front());
 					strList.pop_front();
 					if(format==11)

corelib/src/Odometry.cpp

@@ -322,7 +322,9 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 				Transform previous = this->getPose();
 				Transform newFramePose = Transform(previous.x(), previous.y(), previous.z(), imuQuat.x(), imuQuat.y(), imuQuat.z(), imuQuat.w());
 				UWARN("Updated initial pose from %s to %s with IMU orientation", previous.prettyPrint().c_str(), newFramePose.prettyPrint().c_str());
+				std::map<double, rtabmap::Transform> imus = imus_;
 				this->reset(newFramePose);
+				imus_ = imus;
 			}
 
 			imus_.insert(std::make_pair(data.stamp(), imuT));

corelib/src/OdometryThread.cpp

@@ -40,8 +40,8 @@ OdometryThread::OdometryThread(Odometry * odometry, unsigned int dataBufferMaxSi
 	_dataBufferMaxSize(dataBufferMaxSize),
 	_resetOdometry(false),
 	_resetPose(Transform::getIdentity()),
-	_lastImuStamp(0.0),
-	_imuEstimatedDelay(0.0)
+	_oldestAsyncImuStamp(0.0),
+	_newestAsyncImuStamp(0.0)
 {
 	UASSERT(_odometry != 0);
 }
@@ -110,7 +110,8 @@ void OdometryThread::mainLoop()
 		UScopeMutex lock(_dataMutex);
 		_dataBuffer.clear();
 		_imuBuffer.clear();
-		_lastImuStamp = 0.0f;
+		_oldestAsyncImuStamp = 0.0;
+		_newestAsyncImuStamp = 0.0;
 	}
 
 	SensorData data;
@@ -161,22 +162,39 @@ void OdometryThread::addData(const SensorData & data)
 			!data.laserScanCompressed().empty() || 
 			data.imu().empty())
 		{
-			_dataBuffer.push_back(data);
-			while(_dataBufferMaxSize > 0 && _dataBuffer.size() > _dataBufferMaxSize)
-			{
-				UDEBUG("Data buffer is full, the oldest data is removed to add the new one.");
-				_dataBuffer.erase(_dataBuffer.begin());
+			if(_oldestAsyncImuStamp > 0.0 && data.stamp() < _oldestAsyncImuStamp) {
+				UWARN("Received image/lidar with stamp (%f) older than oldest received imu "
+					"(%f), skipping that frame (imu buffer size=%ld). "
+					"When using async IMU, make sure IMU is published faster "
+					"than camera/lidar (assuming IMU latency is very small compared to camera/lidar).",
+					data.stamp(), _oldestAsyncImuStamp, _imuBuffer.size());
 				notify = false;
 			}
+			else if(_newestAsyncImuStamp > 0.0 && data.stamp()>=_newestAsyncImuStamp) {
+				UWARN("Received image/lidar with stamp (%f) newer than latest received imu "
+					"(%f), skipping that frame (imu buffer size=%ld). "
+					"When using async IMU, make sure IMU is published faster "
+					"than camera/lidar (assuming IMU latency is very small compared to camera/lidar).",
+					data.stamp(), _newestAsyncImuStamp, _imuBuffer.size());
+				notify = false;
+			}
+			else {
+				_dataBuffer.push_back(data);
+				while(_dataBufferMaxSize > 0 && _dataBuffer.size() > _dataBufferMaxSize)
+				{
+					UDEBUG("Data buffer is full, the oldest data is removed to add the new one.");
+					_dataBuffer.erase(_dataBuffer.begin());
+					notify = false;
+				}
+			}
 		}
 		else
 		{
 			_imuBuffer.push_back(data);
-			if(_lastImuStamp != 0.0 && data.stamp() > _lastImuStamp)
-			{
-				_imuEstimatedDelay = data.stamp() - _lastImuStamp;
+			if(_oldestAsyncImuStamp == 0) {
+				_oldestAsyncImuStamp = data.stamp();
 			}
-			_lastImuStamp = data.stamp();
+			_newestAsyncImuStamp = data.stamp();
 		}
 	}
 	_dataMutex.unlock();
@@ -195,18 +213,14 @@ bool OdometryThread::getData(SensorData & data)
 	{
 		if(!_dataBuffer.empty())
 		{
-			if(!_imuBuffer.empty())
+			// Send IMU up to stamp greater than image (OpenVINS needs this).
+			while(!_imuBuffer.empty())
 			{
-				// Send IMU up to stamp greater than image (OpenVINS needs this).
-				while(!_imuBuffer.empty())
-				{
-					_odometry->process(_imuBuffer.front());
-					double stamp = _imuBuffer.front().stamp();
-					_imuBuffer.pop_front();
-					if(stamp > _dataBuffer.front().stamp())
-					{
-						break;
-					}
+				_odometry->process(_imuBuffer.front());
+				double stamp =_imuBuffer.front().stamp();
+				_imuBuffer.pop_front();
+				if(stamp > _dataBuffer.front().stamp()) {
+					break;
 				}
 			}
 

corelib/src/optimizer/OptimizerCeres.cpp

@@ -197,7 +197,7 @@ std::map<int, Transform> OptimizerCeres::optimize(
 
 					if(angle_local_manifold == NULL)
 					{
-						angle_local_manifold = ceres::examples::AngleManfold::Create();
+						angle_local_manifold = ceres::examples::AngleManifold::Create();
 					}
 					SetCeresProblemManifold(problem, &pose_begin_iter->second.yaw_radians, angle_local_manifold);
 					SetCeresProblemManifold(problem, &pose_end_iter->second.yaw_radians, angle_local_manifold);

guilib/src/GraphViewer.cpp

@@ -577,33 +577,40 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
 	for(std::multimap<int, Link>::const_iterator iter=constraints.begin(); iter!=constraints.end(); ++iter)
 	{
 		// make the first id the smallest one
-		int idFrom = iter->first<iter->second.to()?iter->first:iter->second.to();
-		int idTo = iter->first<iter->second.to()?iter->second.to():iter->first;
+		int idFrom = iter->second.from() < iter->second.to() ? iter->second.from() : iter->second.to();
+		int idTo = iter->second.from() < iter->second.to() ? iter->second.to() : iter->second.from();
+
+		if(idFrom == idTo) {
+			continue;
+		}
 
 		std::map<int, Transform>::const_iterator jterA = poses.find(idFrom);
 		std::map<int, Transform>::const_iterator jterB = poses.find(idTo);
 		LinkItem * linkItem = 0;
 		if(jterA != poses.end() && jterB != poses.end() &&
-		   _nodeItems.contains(iter->first) && _nodeItems.contains(idTo))
+		   _nodeItems.contains(idFrom) && _nodeItems.contains(idTo))
 		{
 			const Transform & poseA = jterA->second;
 			const Transform & poseB = jterB->second;
 
 			QMultiMap<int, LinkItem*>::iterator itemIter = _linkItems.end();
+
 			if(_linkItems.contains(idFrom))
 			{
-				itemIter = _linkItems.find(iter->first);
-				while(itemIter.key() == idFrom && itemIter != _linkItems.end())
+				itemIter = _linkItems.find(idFrom);
+				bool alreadyAdded = false;
+				while(itemIter != _linkItems.end() && itemIter.key() == idFrom)
 				{
-					if(itemIter.value()->to() == idTo && itemIter.value()->type() == iter->second.type())
+					if(itemIter.value()->to() == idTo && itemIter.value()->isVisible())
 					{
-						itemIter.value()->setPoses(poseA, poseB, _viewPlane);
-						itemIter.value()->show();
-						linkItem = itemIter.value();
+						alreadyAdded = true;
 						break;
 					}
 					++itemIter;
 				}
+				if(alreadyAdded){
+					continue;
+				}
 			}
 
 			bool interSessionClosure = false;

guilib/src/ImageView.cpp

@@ -1351,7 +1351,7 @@ void ImageView::setImageDepth(const cv::Mat & imageDepth, const cv::Mat & imageD
 	_imageDepthCv = imageDepth;
 	_imageDepthConfidenceCv = imageDepthConfidence;
 	setImageDepth(
-		uCvMat2QImage(_imageDepthCv, true, getDepthColorMap(), _depthColorMapMinRange, _depthColorMapMaxRange),
+		uCvMat2QImage(_imageDepthCv, true, _imageDepthCv.type()==CV_8UC1?uCvQtDepthBlackToWhite:getDepthColorMap(), _depthColorMapMinRange, _depthColorMapMaxRange),
 		uCvMat2QImage(_imageDepthConfidenceCv, true, getDepthColorMap()));
 }
 

guilib/src/MainWindow.cpp

@@ -5878,8 +5878,11 @@ void MainWindow::startDetection()
 		}
 	}
 
-	if(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcDatabase &&
-	   camera && camera->odomProvided())
+	if((_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcDatabase ||
+	    _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcImages ||
+	    _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoImages ||
+	    _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRGBDImages) &&
+	    camera && camera->odomProvided())
 	{
 		odomSensor = camera;
 	}
@@ -8690,11 +8693,13 @@ void MainWindow::changeState(MainWindow::State newState)
 
 		if(_sensorCapture)
 		{
-			_sensorCapture->start();
 			if(_imuThread)
 			{
 				_imuThread->start();
+				// give imu thread a head start
+				uSleep(10);
 			}
+			_sensorCapture->start();
 			ULogger::setTreadIdFilter(_preferencesDialog->getGeneralLoggerThreads());
 		}
 		break;
@@ -8726,11 +8731,13 @@ void MainWindow::changeState(MainWindow::State newState)
 
 			if(_sensorCapture)
 			{
-				_sensorCapture->start();
 				if(_imuThread)
 				{
 					_imuThread->start();
+					// give imu thread a head start
+					uSleep(10);
 				}
+				_sensorCapture->start();
 				ULogger::setTreadIdFilter(_preferencesDialog->getGeneralLoggerThreads());
 			}
 		}