initial commit

.github/workflows/cmake-ros.yml

@@ -3,7 +3,7 @@ name: CMake-ROS
 on:
   push:
     branches: 
-      - humble-devel
+      - master
   pull_request:
     branches: 
       - '**'
@@ -23,7 +23,7 @@ jobs:
     strategy:
       fail-fast: false
       matrix:
-        ros_distribution: [ humble ]
+        ros_distribution: [ humble, jazzy, kilted, rolling ]
         include:
         - ros_distribution: 'humble'
           os: ubuntu-22.04

CMakeLists.txt

@@ -1410,11 +1410,11 @@ MESSAGE(STATUS "  With ORB OcTree           = NO (WITH_ORB_OCTREE=OFF)")
 ENDIF()
 
 IF(TORCH_FOUND)
-MESSAGE(STATUS "  With SupertPoint          = YES (License: GPLv3) libtorch=${Torch_VERSION}")
+MESSAGE(STATUS "  With SuperPoint          = YES (License: GPLv3) libtorch=${Torch_VERSION}")
 ELSEIF(NOT WITH_TORCH)
-MESSAGE(STATUS "  With SupertPoint          = NO (WITH_TORCH=OFF)")
+MESSAGE(STATUS "  With SuperPoint          = NO (WITH_TORCH=OFF)")
 ELSE()
-MESSAGE(STATUS "  With SupertPoint          = NO (libtorch not found)")
+MESSAGE(STATUS "  With SuperPoint          = NO (libtorch not found)")
 ENDIF()
 
 IF(WITH_PYTHON AND Python3_FOUND)

corelib/include/rtabmap/core/Rtabmap.h

@@ -50,6 +50,7 @@ class BayesFilter;
 class Signature;
 class Optimizer;
 class PythonInterface;
+class DBDriver;
 
 class RTABMAP_CORE_EXPORT Rtabmap
 {
@@ -371,6 +372,7 @@ private:
 	Transform _lastLocalizationPose; // Corrected odometry pose. In mapping mode, this corresponds to last pose return by getLocalOptimizedPoses().
 	int _lastLocalizationNodeId; // for localization mode
 	cv::Mat _localizationCovariance;
+	DBDriver * _externalLocalizationDbDriver;
 	std::map<int, std::pair<cv::Point3d, Transform> > _gpsGeocentricCache;
 	bool _currentSessionHasGPS;
 	LaserScan _globalScanMap;

corelib/src/Features2d.cpp

@@ -688,7 +688,7 @@ Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parame
 #ifndef RTABMAP_TORCH
 	if(type == Feature2D::kFeatureSuperPointTorch)
 	{
-		UWARN("SupertPoint Torch feature cannot be used as RTAB-Map is not built with the option enabled. GFTT/ORB is used instead.");
+		UWARN("SuperPoint Torch feature cannot be used as RTAB-Map is not built with the option enabled. GFTT/ORB is used instead.");
 		type = Feature2D::kFeatureGfttOrb;
 	}
 #endif

corelib/src/Rtabmap.cpp

@@ -423,6 +423,32 @@ void Rtabmap::init(const ParametersMap & parameters, const std::string & databas
 
 			if(_createGlobalScanMap)
 				createGlobalScanMap();
+
+			if(true)
+			{
+				std::string externalLocalizationDataPath = _wDir +"/rtabmap_log.db"; // use parameter to know prefix, then generate timestamp
+				_externalLocalizationDbDriver = DBDriver::create(allParameters);
+				if(!_externalLocalizationDbDriver->openConnection(externalLocalizationDataPath)) {
+					UERROR("Failed to create database \"%s\" to save external localization data!");
+				}
+				else
+				{
+					float x, y, resolution;
+					cv::Mat map = _memory->load2DMap(x, y, resolution);
+					if(!map.empty()) {
+						_externalLocalizationDbDriver->save2DMap(map, x, y, resolution);
+					}
+					if(!_optimizedPoses.empty()) {
+						_externalLocalizationDbDriver->saveOptimizedPoses(_optimizedPoses, _lastLocalizationPose);
+					}
+					if(!allParameters.empty())
+					{
+						ParametersMap params = allParameters;
+						params.erase(Parameters::kRtabmapWorkingDirectory()); // don't save working directory as it is machine dependent
+						_externalLocalizationDbDriver->addInfoAfterRun(0, 0, 0, 0, 0, params);
+					}
+				}
+			}
 		}
 		else
 		{
@@ -519,6 +545,9 @@ void Rtabmap::close(bool databaseSaved, const std::string & ouputDatabasePath)
 					_optimizedPoses.erase(iter->first);
 				}
 			}
+			if(!_memory->isIncremental() && _externalLocalizationDbDriver) {
+				_externalLocalizationDbDriver->saveOptimizedPoses(_optimizedPoses, _lastLocalizationPose);
+			}
 			_memory->saveOptimizedPoses(_optimizedPoses, _lastLocalizationPose);
 		}
 		_memory->close(databaseSaved, true, ouputDatabasePath);
@@ -528,6 +557,12 @@ void Rtabmap::close(bool databaseSaved, const std::string & ouputDatabasePath)
 	_optimizedPoses.clear();
 	_lastLocalizationPose.setNull();
 
+	if(_externalLocalizationDbDriver) {
+		_externalLocalizationDbDriver->closeConnection();
+		delete _externalLocalizationDbDriver;
+		_externalLocalizationDbDriver = 0;
+	}
+
 	if(_bayesFilter)
 	{
 		delete _bayesFilter;
@@ -4265,9 +4300,19 @@ bool Rtabmap::process(
 
 	// Localization mode and saving localization data: save odometry covariance in a prior link
 	// so that DBReader can republish the covariance of localization data
-	if(!_memory->isIncremental() && _memory->isLocalizationDataSaved() && !odomCovariance.empty())
+	if(!_memory->isIncremental())
 	{
-		_memory->addLink(Link(signature->id(), signature->id(), Link::kPosePrior, odomPose, odomCovariance.inv()));
+		if(_externalLocalizationDbDriver)
+		{
+			Signature * cpy = new Signature();
+			*cpy = *signature;
+			_externalLocalizationDbDriver->asyncSave(cpy);
+		}
+
+		if(_memory->isLocalizationDataSaved() && !odomCovariance.empty())
+		{
+			_memory->addLink(Link(signature->id(), signature->id(), Link::kPosePrior, odomPose, odomCovariance.inv()));
+		}
 	}
 
 	// remove last signature if the memory is not incremental or is a bad signature (if bad signatures are ignored)
@@ -4690,6 +4735,11 @@ bool Rtabmap::process(
 		_memory->saveStatistics(statistics_, _saveWMState);
 	}
 	
+	if(_externalLocalizationDbDriver) // external db log
+	{
+		_externalLocalizationDbDriver->addStatistics(statistics_, _saveWMState);
+	}
+
 	//Start trashing
 	UDEBUG("Empty trash...");
 	_memory->emptyTrash();

docker/focal/deps/Dockerfile

@@ -11,7 +11,7 @@ ENV DEBIAN_FRONTEND=noninteractive
 WORKDIR /root/
 
 # issue: https://github.com/introlab/rtabmap/issues/1523
-RUN rm /etc/apt/sources.list.d/ros1-latest.list && \
+RUN rm /etc/apt/sources.list.d/ros1-latest.list || true && \
     apt-get update && apt-get install -y curl && \
     sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' && \
     curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add - && \
@@ -155,7 +155,7 @@ RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then git clone https://github.com/
     cd AliceVision && \
     git checkout 0f6115b6af6183c524aa7fcf26141337c1cf3872 && \
     git submodule update -i && \
-    wget https://gist.githubusercontent.com/matlabbe/1df724465106c056ca4cc195c81d8cf0/raw/b3ed4cb8f9b270833a40d57d870a259eabfa4415/alicevision_0f6115b.patch && \
+    wget https://gist.githubusercontent.com/matlabbe/1df724465106c056ca4cc195c81d8cf0/raw/5e3437cf6229c8d534bbaee475ed9bcb92eb84a1/alicevision_0f6115b.patch && \
     git apply alicevision_0f6115b.patch && \
     mkdir build && \
     cd build && \

guilib/include/rtabmap/gui/GraphViewer.h

@@ -80,6 +80,7 @@ public:
 	void updateLocalPath(const std::vector<int> & localPath);
 	void setGlobalPath(const std::vector<std::pair<int, Transform> > & globalPath);
 	void setCurrentGoalID(int id, const Transform & pose = Transform());
+	void setNodeInfo(int id, const QString & info);
 	void setLocalRadius(float radius);
 	void highlightNode(int nodeId, int highlightIndex);
 	void clearGraph();

guilib/src/GraphViewer.cpp

@@ -109,6 +109,10 @@ public:
 		_value = value;
 	}
 
+	void setToolTipInfo(const QString & info) {
+		_info = info;
+	}
+
 	void setRadius(float radius)
 	{
 		float r,p,yaw;
@@ -160,6 +164,10 @@ protected:
 		{
 			msg += QString("\n%1=%2").arg(_valueName).arg(_value);
 		}
+		if(!_info.isEmpty())
+		{
+			msg += QString("\n%1").arg(_info);
+		}
 
 		this->setToolTip(msg);
 		
@@ -181,6 +189,7 @@ private:
 	QGraphicsLineItem * _line;
 	QString _valueName;
 	float _value;
+	QString _info;
 };
 
 class NodeGPSItem: public NodeItem
@@ -522,6 +531,7 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
 		}
 		iter.value()->hide();
 		iter.value()->setColor(color); // reset color
+		iter.value()->setToolTipInfo(QString());
 		iter.value()->setZValue(iter.key()<0?21:20);
 	}
 	for(QMultiMap<int, LinkItem*>::iterator iter = _linkItems.begin(); iter!=_linkItems.end(); ++iter)
@@ -1166,6 +1176,19 @@ void GraphViewer::setCurrentGoalID(int id, const Transform & pose)
 	}
 }
 
+void GraphViewer::setNodeInfo(int id, const QString & info)
+{
+	NodeItem * node = _nodeItems.value(id, 0);
+	if(node)
+	{
+		node->setToolTipInfo(info);
+	}
+	else
+	{
+		UWARN("Current goal %d not found in the graph", id);
+	}
+}
+
 void GraphViewer::setLocalRadius(float radius)
 {
 	_localRadius->setRect(-radius*100, -radius*100, radius*200, radius*200);

guilib/src/ui/preferencesDialog.ui

@@ -27222,13 +27222,13 @@ Lower the ratio -&gt; higher the precision.</string>
                  <item>
                   <widget class="QGroupBox" name="groupBox_detector_superpoint_torch2">
                    <property name="title">
-                    <string>SupertPoint Torch</string>
+                    <string>SuperPoint Torch</string>
                    </property>
                    <layout class="QVBoxLayout" name="verticalLayout_147">
                     <item>
                      <widget class="QLabel" name="label_575">
                       <property name="text">
-                       <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;SupertPoint c++ implementation from &lt;a href=&quot;https://github.com/KinglittleQ/SuperPoint_SLAM&quot;&gt;&lt;span style=&quot; text-decoration: underline; color:#0000ff;&quot;&gt;SuperPoint_SLAM&lt;/span&gt;&lt;/a&gt; project based on pytorch. A &lt;span style=&quot; font-weight:600;&quot;&gt;superpoint.pt&lt;/span&gt; weights file can be downloaded from its Git. It should be the same weights file &lt;a href=&quot;https://pytorch.org/tutorials/advanced/cpp_export.html&quot;&gt;&lt;span style=&quot; text-decoration: underline; color:#0000ff;&quot;&gt;converted&lt;/span&gt;&lt;/a&gt; from &lt;span style=&quot; font-weight:600;&quot;&gt;superpoint.pth&lt;/span&gt; of the &lt;a href=&quot;https://github.com/magicleap/SuperPointPretrainedNetwork&quot;&gt;&lt;span style=&quot; text-decoration: underline; color:#0000ff;&quot;&gt;SuperPointPretrainedNetwork&lt;/span&gt;&lt;/a&gt; project.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+                       <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;SuperPoint c++ implementation from &lt;a href=&quot;https://github.com/KinglittleQ/SuperPoint_SLAM&quot;&gt;&lt;span style=&quot; text-decoration: underline; color:#0000ff;&quot;&gt;SuperPoint_SLAM&lt;/span&gt;&lt;/a&gt; project based on pytorch. A &lt;span style=&quot; font-weight:600;&quot;&gt;superpoint.pt&lt;/span&gt; weights file can be downloaded from its Git. It should be the same weights file &lt;a href=&quot;https://pytorch.org/tutorials/advanced/cpp_export.html&quot;&gt;&lt;span style=&quot; text-decoration: underline; color:#0000ff;&quot;&gt;converted&lt;/span&gt;&lt;/a&gt; from &lt;span style=&quot; font-weight:600;&quot;&gt;superpoint.pth&lt;/span&gt; of the &lt;a href=&quot;https://github.com/magicleap/SuperPointPretrainedNetwork&quot;&gt;&lt;span style=&quot; text-decoration: underline; color:#0000ff;&quot;&gt;SuperPointPretrainedNetwork&lt;/span&gt;&lt;/a&gt; project.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
                       </property>
                       <property name="wordWrap">
                        <bool>true</bool>