Adding doc and tests (#1492)

* added doc and tests for util2d.h

* updated cmake-ros ci

* Added util3d.h doc and tests

* util3d_transforms.h: Added doc and tests

* util3d_filtering.h: started doc and test

* util3d_filtering.h: more tests and doc

* Added more doc/tests

* finished util3d_filtering doc and tests

* added test for util2d::depthBleedingFiltering

* Added util3d_registration tests

* Added util3d_features.h doc/tests

* added doc/tests for util3d_correspondences.h

* added doc/gtest for util3d_mapping.h (missing hpp functions)

* finished testing util3d_mapping.hpp

* Added util3d_motion_estimation.h tests (2D->3D done)

* finished util3d_motion_estimation.h tests

* minimal util3d_surface.h

* Added Transform and VisualWord tests

* Added doc for CameraModel and StereoCameraModel

* Added more logs in ros ci

* Passing tests on fical

* improved all devcontainer

* added devcontainer kilted, fixed source setup.bash, removed ldconfig in ros-cmake workflow

* cleanup

* source ros

* Added utilite tests

* Added testing to appveyor, github actions cancellable on re-commit on same branch

* appveyor testing without all targets

* appveyor: specifying ALL_BUILD target

* Fixed Util2dTest.NMSImageBoundsRespected test

* Fixing PCL Indices error on old pcl

* Added VWDictionary tests and doc. Fixed LSH not working (fix from https://github.com/flann-lib/flann/pull/472

* fixing some appveyor CI errors, added test to check dictionary serialization against all type

* Added StereoDense, StereoBM and StereoSGBM doc and tests

* Added Stereo tests

* Added CameraModel and StereoCameraModel tests

* Added doc and test for Statistics

* Added doc/tests for Signature

* Added doc/test for SensorEvent, added doc for SensorCaptureInfo

* Added doc to SensorData

* Added SensorData tests

* Added SensorCapture and SensorCaptureThread doc and tests

* fixed sensordata test

* updated SSC test and doc

* Added doc and tests for BayesFilter class

* Enabled testing on mac, updated windows testing like on linux

* added test_link

* fixed unresolved on windows

* fixed ThreadHandle error on macos ci

* Added GPS and GeodeticCoords tests

* Added tests for compression

* Added Odometry tests (base class only)

* Added DBDriver tests

* Added coverage report

* uniformized test names

* fixing concurancy and coverage ci

* dont built tools, examples and app for coverage build

* fixed report tool rebuilt without qt compilation error

* updated coverage option

* updated coverage config

* added doc CI job

* fixing windows and mac ci errors

* Added DBDriverSqlite3 tests

* Added IMU tests

* Added Graph tests

* fixing flaky macos test

* Added IMUThread and IMUFilter tests

* Added Landmarks tests

* Added LASWriter tests

* fixing seed flaky test

* fixing flaky macos timing tests

* Added LocalGrid tests

* Added LocalGridMaker tests

* fixing ci errors

* Added GlobalMap tests

* Added doc for EnvSensor

* Added Features2D tests

* Added Registration tests

* Added RegistrationVis tests

* Added doc for Rtabmap and Memory classes

* Added Memory and Rtabmap tests

* making some tests less flaky

* lcov 1.14 support

* updated compatible tool arguments

* Added integration tests (RGB-D, Stereo, Lidar2d, Lidar3d)

* More octomap checks

* Refactored how/when python interpretor is created to simplify library usage

* Added python tests

* fixed some flaky tests

* suppressed some third party related warnings

* fixed ceres tests

* more flaky fixes

* Fixing tests without libpointmatcher

* Added RANSAC rejection filter to PCL ICP

* fixing multi platform flakiness

* Added test to detect regression

* Fixing windows pcl link error

* fixed some macos flakiness

* bigger 2D2D registration error on opencv 4.6.0

* flakiness

* fixing flaky tests on windows and mac

* flaky thread test on slow mac VM

* windows slow test

* fixing more ci erros

* fxing temp dir on windows

* Added Optimizer tests and discovered some bugs (fixed)

* fixing flaky tests in mac and windows

* Added Optimizer doc

* Added GTSAM BA, updated Ceres to use g2o ba parameters. Renamed g2o's ba related parameters to Optimizer group and used by both gtsam and ceres.

* fixing build without gtsam

* fixing home dir

* fixing python ci isssues

* Added multicam ba tests

* Added Ceres multicam BA support

* Aligned BundleAdjustment parameters with Optimizer/Strategy to avoid confusion in the code

* Added BA integration test

* Added robust graph optimization integration test

* Added loop3it test

* Added stereo20Hz test

* Added smartfactor gtsam

* Fixed bugged check and warn if python didn't return any descriptors

* Fixing gtsam version build issues

* fixing tilt on windows ci

* loosing ceres integration test for ci

* mac ci flakiness

* updating missing param in gui

* updating test bound for mac

* added appearance-based tests, set min gftt quality to quality level

* testing more stuff

* improving features2d tests

* ci flakiness

* fixing flaky ci

* ci fixes

* flaky fixes

* Added RegistrationIcp tests

* Added icp integration test with real-worl corridor like env

* intermediate nodes

* fixing enum

* Updated test to catch #1714

* Fixed 2d corridor failing on pcl

* flaky pnp test

* flaky brisk test

* Set rtabmap_integration test as long

* updating loop closure test

* flaky ci tests

* TEsting roundtrip g2o/toro save/load

* loosing test bound

* fixed cuda capable checks

* flaky tests

* Debugging test hanging

* more debugging stuff

* updating limit

* windows: disabled cuda on ci to avoid incompatible driver issue. Fixing a bad test mem allocation

* trying fixing cuda hanging issue

* fixing ci flakyness

* flaky tests

* Updated BOW flaky tests by checking min precision/recall instead of recall@100precision. Fixed signature test

* CameraModel::load() test initRectificationMap param

* test dbdriver load dictionary idsOnly

* Memory: test keepLinkedInDb param

* added dummyDictionary tests

* test intermediate nodes count

* Added MarkerDetector tests

* reverted breaking change of UMutex and USemaphore

* Features2d: fixed compiltion warnings with clang about override

* clang warnings

* fixing test build with pcl 1.8

* g2o and gtsam build errors on android

* opencv5 test fixes

* disabled testing for ios and android builds

* normalized endline characters for easier diff

* added LF CRLF rule

* bump 0.23.10. fixing doc version

* Publish rtabmap website doc from ci

* fixing MSCVC build error

* macos icp flaky test

* fixing ceres macos test bound

* ficing more flaky tests

* fixing opencv5 related test errors. Also fixed an actual bug in ENU_WGS84ToGeocentric_WGS84()

* added comment about mrpt change

* removed rosdoc2 (will add it for rtabmap_ros later)

* fixing website style

* updated download links

* locally deployable website with api

* sweep doxygen issues

* improved/revised doxygen main pages

* removed examples empty page

* Updated doxygen style

* more concise doxygen groups

* added api link on main readme

* fixing utilite test error

* fixing CommonFilteringGroundNormalsUp test

* updated precisionRecall test bounds for Freak and brief descriptors

* fixing scale check in ba tests

* disabled tests on windows cuda build (missing dlls amd runner cannot test cuda anyway)

* ceres: missing suitesparse dep in windows ci

* adjusting recall thr for fast/freak

* ficing more flaky tests

* fixing flaky tests

* disabled coverage in ros ci

* Enable integration tests for ros ci jobs

* loosing up some threshold for failing tests

* trigger cache

* fixing test data in ros ci. Updated flaky test for mac

* slaking some test limit

* Fixed rtabmap-detectMoreLoopClosures inverted output value

* loosing up sift recall on mac

* optimizer re-ordered distribution for reproducible results (mac g2o)

* macos dump test crash log

* combining all tests to save time on shared library reload. Also fixed Logs with missing arguments.

* Added ENABLE_FORMAT_ERRORS cmake option

* do test only one time

* fixed all format warnings

* format security android build errors

* less verbose tests

* updated ImuUThread test

* fixed a log

* Fixed libpointmatcher 2d normals eigen issue

* Fixing libpointmatcher conversion issues

* fixing libpointmatcher test on windows ci

* cleanup comments, relax some test thr

* disabled sequoia-intel ci build (too flaky, would need extensive testing directly on that machine)

corelib/src/Rtabmap.cpp

@@ -182,10 +182,13 @@ Rtabmap::Rtabmap() :
 	_pathStuckCount(0),
 	_pathStuckDistance(0.0f),
 	_dummyDictionary(false)
-#ifdef RTABMAP_PYTHON
-	,_python(new PythonInterface())
-#endif
 {
+#ifdef RTABMAP_PYTHON
+	// Ensure the embedded Python interpreter is up. The first call here will
+	// assert that it runs on the main thread; callers building Rtabmap on a
+	// worker thread should construct the singleton in main() beforehand.
+	PythonInterface::instance("Rtabmap");
+#endif
 }
 
 Rtabmap::~Rtabmap() {
@@ -303,7 +306,7 @@ void Rtabmap::flushStatisticLogs()
 {
 	if(_foutFloat && _bufferedLogsF.size())
 	{
-		UDEBUG("_bufferedLogsF.size=%d", _bufferedLogsF.size());
+		UDEBUG("_bufferedLogsF.size=%d", (int)_bufferedLogsF.size());
 		for(std::list<std::string>::iterator iter = _bufferedLogsF.begin(); iter!=_bufferedLogsF.end(); ++iter)
 		{
 			fprintf(_foutFloat, "%s", iter->c_str());
@@ -312,7 +315,7 @@ void Rtabmap::flushStatisticLogs()
 	}
 	if(_foutInt && _bufferedLogsI.size())
 	{
-		UDEBUG("_bufferedLogsI.size=%d", _bufferedLogsI.size());
+		UDEBUG("_bufferedLogsI.size=%d", (int)_bufferedLogsI.size());
 		for(std::list<std::string>::iterator iter = _bufferedLogsI.begin(); iter!=_bufferedLogsI.end(); ++iter)
 		{
 			fprintf(_foutInt, "%s", iter->c_str());
@@ -400,7 +403,7 @@ void Rtabmap::init(const ParametersMap & parameters, const std::string & databas
 			_lastLocalizationPose = lastPose;
 
 			UINFO("Loaded optimizedPoses=%d firstPose %d=%s lastLocalizationPose=%s",
-					_optimizedPoses.size(),
+					(int)_optimizedPoses.size(),
 					_optimizedPoses.lower_bound(1)->first,
 					_optimizedPoses.lower_bound(1)->second.prettyPrint().c_str(),
 					_lastLocalizationPose.prettyPrint().c_str());
@@ -1317,7 +1320,7 @@ bool Rtabmap::process(
 						odomPose.r21(), odomPose.r22(), odomPose.r23(), odomPose.o24(),
 						odomPose.r31(), odomPose.r32(), odomPose.r33(), odomPose.o34());
 				odomPose.normalizeRotation();
-				UASSERT_MSG(odomPose.isInvertible(), uFormat("Odometry pose is not invertible!\n"
+				UASSERT_MSG(odomPose.isInvertible(), uFormat("Odometry pose is not invertible! %s\n"
 						"[%f %f %f %f;\n"
 						" %f %f %f %f;\n"
 						" %f %f %f %f;\n"
@@ -2399,7 +2402,7 @@ bool Rtabmap::process(
 					"nbDirectNeighborsInDb=%d, "
 					"time=%fs (%fs %fs)",
 					neighborhoodSize,
-					reactivatedIds.size(),
+					(int)reactivatedIds.size(),
 					(int)nbLoadedFromDb,
 					nbDirectNeighborsInDb,
 					timeGetN.ticks(),
@@ -4439,8 +4442,13 @@ bool Rtabmap::process(
 	}
 	if(!_publishLastSignatureData)
 	{
+		// Keep the occupancy grid (compressed AND raw) on the published copy:
+		// downstream consumers rely on it to generate global occupancy grid
+		// in the same process (e.g. the raw occupancy grid used by the MainWindow 
+		// or ROS rtabmap_slam) or on an external process (the compressed occupancy
+		// grid published over ROS rtabmap_msgs/MapData).
 		lastSignatureData.sensorData().clearCompressedData(true, true, true, false);
-		lastSignatureData.sensorData().clearRawData();
+		lastSignatureData.sensorData().clearRawData(true, true, true, false);
 	}
 	if(!_rawDataKept)
 	{
@@ -4573,7 +4581,7 @@ bool Rtabmap::process(
 			if(_maxMemoryAllowed != 0 && workingMemSize > _maxMemoryAllowed)
 			{
 				ULOGGER_INFO("Removing old signatures because memory limit is reached %d > %d...",
-					workingMemSize, _maxMemoryAllowed);
+					(int)workingMemSize, _maxMemoryAllowed);
 			}
 			immunizedLocations.insert(_lastLocalizationNodeId); // keep the latest localization in working memory
 			std::list<int> transferred = _memory->forget(immunizedLocations);
@@ -5019,7 +5027,14 @@ void Rtabmap::setMemoryThreshold(int maxMemoryAllowed)
 
 void Rtabmap::setWorkingDirectory(std::string path)
 {
-	path = uReplaceChar(path, '~', UDirectory::homeDir());
+	// Expand leading "~" to the user's home directory (shell convention).
+	// We do NOT replace every "~" in the path -- Windows 8.3 short names
+	// embed "~" in the middle (e.g. C:\Users\RUNNER~1\...), and a blanket
+	// uReplaceChar would corrupt those.
+	if(!path.empty() && path[0] == '~')
+	{
+		path = UDirectory::homeDir() + path.substr(1);
+	}
 	if(!path.empty() && UDirectory::exists(path))
 	{
 		ULOGGER_DEBUG("Comparing new working directory path \"%s\" with \"%s\"", path.c_str(), _wDir.c_str());
@@ -5695,7 +5710,7 @@ std::list<std::pair<int, int> > Rtabmap::repairGraph(
 
 void Rtabmap::adjustLikelihood(std::map<int, float> & likelihood) const
 {
-	ULOGGER_DEBUG("likelihood.size()=%d", likelihood.size());
+	ULOGGER_DEBUG("likelihood.size()=%d", (int)likelihood.size());
 	UTimer timer;
 	timer.start();
 	if(likelihood.size()==0)
@@ -5714,7 +5729,7 @@ void Rtabmap::adjustLikelihood(std::map<int, float> & likelihood) const
 			values.push_back(iter->second);
 		}
 	}
-	UDEBUG("values.size=%d", values.size());
+	UDEBUG("values.size=%d", (int)values.size());
 
 	float mean = uMean(values);
 	float stdDev = std::sqrt(uVariance(values, mean));
@@ -6335,11 +6350,11 @@ int Rtabmap::detectMoreLoopClosures(
 									links.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, inf)));
 									loopClosuresAdded.push_back(Link(from, to, Link::kUserClosure, t, inf));
 									std::string msg = uFormat("Iteration %d/%d: Added loop closure %d->%d! (%d/%d)", n+1, iterations, from, to, i+1, (int)clusters.size());
-									UINFO(msg.c_str());
+									UINFO("%s", msg.c_str());
 
 									if(processState)
 									{
-										UINFO(msg.c_str());
+										UINFO("%s", msg.c_str());
 										if(!processState->callback(msg))
 										{
 											return -1;
@@ -6356,7 +6371,7 @@ int Rtabmap::detectMoreLoopClosures(
 		if(processState)
 		{
 			std::string msg = uFormat("Iteration %d/%d: Detected %d total loop closures!", n+1, iterations, (int)addedLinks.size()/2);
-			UINFO(msg.c_str());
+			UINFO("%s", msg.c_str());
 			if(!processState->callback(msg))
 			{
 				return -1;
@@ -6419,17 +6434,17 @@ bool Rtabmap::globalBundleAdjustment(
 	if(!_optimizedPoses.empty() && !_constraints.empty())
 	{
 		int iterations = Parameters::defaultOptimizerIterations();
-		float pixelVariance = Parameters::defaultg2oPixelVariance();
+		float pixelVariance = Parameters::defaultOptimizerPixelVariance();
 		ParametersMap params = _parameters;
 		Parameters::parse(params, Parameters::kOptimizerIterations(), iterations);
-		Parameters::parse(params, Parameters::kg2oPixelVariance(), pixelVariance);
+		Parameters::parse(params, Parameters::kOptimizerPixelVariance(), pixelVariance);
 		if(iterations > 0)
 		{
 			uInsert(params, ParametersPair(Parameters::kOptimizerIterations(), uNumber2Str(iterations)));
 		}
 		if(pixelVariance > 0.0f)
 		{
-			uInsert(params, ParametersPair(Parameters::kg2oPixelVariance(), uNumber2Str(pixelVariance)));
+			uInsert(params, ParametersPair(Parameters::kOptimizerPixelVariance(), uNumber2Str(pixelVariance)));
 		}
 
 		std::map<int, Signature> signatures;
@@ -6941,13 +6956,13 @@ void Rtabmap::addNodesToRepublish(const std::vector<int> & ids)
 
 void Rtabmap::setDummyDictionary(bool enabled)
 {
-	if(_memory) {
+	if(_memory && enabled) {
 		UERROR("Memory is already initialized, cannot set dummy dictionary. This "
 			"function can only be called after Rtabmap object is created, but "
 			"before init() is called.");
 	}
 	else {
-		_dummyDictionary = true;
+		_dummyDictionary = enabled;
 	}
 }
 
@@ -7041,6 +7056,33 @@ bool Rtabmap::computePath(int targetNode, bool global)
 			{
 				if(iter->first > 0)
 				{
+					// Skip intermediate nodes (weight==-1). They are not navigable
+					// waypoints and updateGoalIndex would otherwise abort the
+					// plan when it sees them. The poses of the remaining real
+					// nodes already account for cumulative transform through any
+					// intermediate chain (relative poses from graph::computePath).
+					int weight = 0;
+					const Signature * s = _memory->getSignature(iter->first);
+					if(s)
+					{
+						weight = s->getWeight();
+					}
+					else
+					{
+						// For nodes in LTM, fetch weight from the database.
+						Transform p, gt;
+						int mapId = 0;
+						std::string label;
+						double stamp = 0.0;
+						std::vector<float> vel;
+						GPS gps;
+						EnvSensors envs;
+						_memory->getNodeInfo(iter->first, p, mapId, weight, label, stamp, gt, vel, gps, envs, true);
+					}
+					if(weight == -1)
+					{
+						continue;
+					}
 					// just keep nodes in the path
 					_path[oi].first = iter->first;
 					_path[oi++].second = t * iter->second;
@@ -7314,18 +7356,14 @@ void Rtabmap::updateGoalIndex()
 	if( _memory && _path.size())
 	{
 		// remove all previous virtual links
-		bool hasIntermediateNodes = false;
 		for(unsigned int i=0; i<_pathCurrentIndex && i<_path.size(); ++i)
 		{
 			const Signature * s = _memory->getSignature(_path[i].first);
 			if(s)
 			{
+				UASSERT_MSG(s->getWeight() != -1, uFormat("path[%u] id=%d is intermediate; computePath should have filtered it", i, _path[i].first).c_str());
 				_memory->removeVirtualLinks(s->id());
 			}
-			if(s->getWeight() == -1)
-			{
-				hasIntermediateNodes = true;
-			}
 		}
 
 		// for the current index, only keep the newest virtual link
@@ -7351,51 +7389,42 @@ void Rtabmap::updateGoalIndex()
 			}
 		}
 
-		// Make sure the next signatures on the path are linked together
+		// Make sure the next signatures on the path are linked together.
+		// Intermediate nodes have been filtered out of _path by computePath, so
+		// every entry is a real node here.
 		float distanceSoFar = 0.0f;
-		for(unsigned int i=_pathCurrentIndex+1;
-			i<_path.size() && !hasIntermediateNodes;
-			++i)
+		for(unsigned int i=_pathCurrentIndex+1; i<_path.size(); ++i)
 		{
-			if(i>0)
+			if(_localRadius > 0.0f)
 			{
-				if(_localRadius > 0.0f)
+				distanceSoFar += _path[i-1].second.getDistance(_path[i].second);
+			}
+
+			if(_path[i].first != _path[i-1].first)
+			{
+				const Signature * s = _memory->getSignature(_path[i].first);
+				if(s)
 				{
-					distanceSoFar += _path[i-1].second.getDistance(_path[i].second);
-				}
-				
-				if(_path[i].first != _path[i-1].first)
-				{
-					const Signature * s = _memory->getSignature(_path[i].first);
-					if(s)
+					UASSERT_MSG(s->getWeight() != -1, uFormat("path[%u] id=%d is intermediate; computePath should have filtered it", i, _path[i].first).c_str());
+					const Signature * sPrev = _memory->getSignature(_path[i-1].first);
+					if(sPrev)
 					{
-						if(s->getWeight() == -1)
-						{
-							hasIntermediateNodes = true;
-							break;
-						}
-						if(!s->hasLink(_path[i-1].first) && _memory->getSignature(_path[i-1].first) != 0)
-						{
-							Transform virtualLoop = _path[i].second.inverse() * _path[i-1].second;
-							_memory->addLink(Link(_path[i].first, _path[i-1].first, Link::kVirtualClosure, virtualLoop, cv::Mat::eye(6,6,CV_64FC1)*0.01)); // on the optimized path
-							UINFO("Added Virtual link between %d and %d", _path[i-1].first, _path[i].first);
-						}
+						UASSERT_MSG(sPrev->getWeight() != -1, uFormat("path[%u] id=%d is intermediate; computePath should have filtered it", i-1, _path[i-1].first).c_str());
+					}
+					if(!s->hasLink(_path[i-1].first) && sPrev != 0)
+					{
+						Transform virtualLoop = _path[i].second.inverse() * _path[i-1].second;
+						_memory->addLink(Link(_path[i].first, _path[i-1].first, Link::kVirtualClosure, virtualLoop, cv::Mat::eye(6,6,CV_64FC1)*0.01)); // on the optimized path
+						UINFO("Added Virtual link between %d and %d", _path[i-1].first, _path[i].first);
 					}
 				}
-
-				if(distanceSoFar > _localRadius)
-				{
-					UDEBUG("Farthest goal=%d : %f m", _path[i].first, distanceSoFar);
-					break;
-				}
 			}
-		}
 
-		if(hasIntermediateNodes)
-		{
-			UERROR("Cannot follow a path with a map containing intermediate nodes (not supported: don't use intermediate nodes if rtabmap's planner has to be used). Aborting current plan!");
-			this->clearPath(-1);
-			return;
+			if(distanceSoFar > _localRadius)
+			{
+				UDEBUG("Farthest goal=%d : %f m", _path[i].first, distanceSoFar);
+				break;
+			}
 		}
 
 		UDEBUG("current node = %d current goal = %d", _path[_pathCurrentIndex].first, _path[_pathGoalIndex].first);