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/python/PyDescriptor.cpp

@@ -23,6 +23,7 @@ PyDescriptor::PyDescriptor(
 		dim_(Parameters::defaultPyDescriptorDim())
 {
 	UDEBUG("");
+	PythonInterface::instance("PyDescriptor");
 	this->parseParameters(parameters);
 }
 
@@ -47,7 +48,10 @@ void PyDescriptor::parseParameters(const ParametersMap & parameters)
 	std::string previousPath = path_;
 	Parameters::parse(parameters, Parameters::kPyDescriptorPath(), path_);
 	Parameters::parse(parameters, Parameters::kPyDescriptorDim(), dim_);
-	path_ = uReplaceChar(path_, '~', UDirectory::homeDir());
+	if(!path_.empty() && path_[0] == '~' && (path_.size() == 1 || path_[1] == '/' || path_[1] == '\\'))
+	{
+		path_ = UDirectory::homeDir() + path_.substr(1);
+	}
 	UINFO("path = %s", path_.c_str());
 	UINFO("dim = %d", dim_);
 	UTimer timer;
@@ -77,15 +81,33 @@ void PyDescriptor::parseParameters(const ParametersMap & parameters)
 		{
 			return;
 		}
+		// Pre-validate the path: PyImport_Import on a non-existent script
+		// fails without setting a Python exception, after which
+		// getPythonTraceback() dereferences NULL pointers in Py_BuildValue
+		// and crashes. Mirrors the check in PyDetector::PyDetector().
+		if(!UFile::exists(path_) || UFile::getExtension(path_).compare("py") != 0)
+		{
+			UERROR("Cannot initialize Python descriptor, the path is not valid: \"%s\"=\"%s\"",
+					Parameters::kPyDescriptorPath().c_str(), path_.c_str());
+			return;
+		}
 		std::string matcherPythonDir = UDirectory::getDir(path_);
 		if(!matcherPythonDir.empty())
 		{
+			// For windows
+			matcherPythonDir = uReplaceChar(matcherPythonDir, '\\', '/');
 			PyRun_SimpleString("import sys");
 			PyRun_SimpleString(uFormat("sys.path.append(\"%s\")", matcherPythonDir.c_str()).c_str());
 		}
 
 		_import_array();
 
+		// Invalidate importlib's directory-listing caches so a script created
+		// after sys.path was first scanned in this process is still found.
+		// Without this, the second Py* instance pointing at a freshly-written
+		// script in an already-known directory fails with ModuleNotFoundError.
+		PyRun_SimpleString("import importlib; importlib.invalidate_caches()");
+
 		std::string scriptName = uSplit(UFile::getName(path_), '.').front();
 		PyObject * pName = PyUnicode_FromString(scriptName.c_str());
 		UDEBUG("PyImport_Import() beg");

corelib/src/python/PyDescriptor.h

@@ -8,12 +8,13 @@
 
 #include <rtabmap/core/GlobalDescriptorExtractor.h>
 #include "rtabmap/core/PythonInterface.h"
+#include "rtabmap/core/rtabmap_core_export.h"
 #include <Python.h>
 
 namespace rtabmap
 {
 
-class PyDescriptor : public GlobalDescriptorExtractor
+class RTABMAP_CORE_EXPORT PyDescriptor : public GlobalDescriptorExtractor
 {
 public:
   PyDescriptor(const ParametersMap & parameters = ParametersMap());

corelib/src/python/PyDetector.cpp

@@ -24,6 +24,7 @@ PyDetector::PyDetector(const ParametersMap & parameters) :
 		path_(Parameters::defaultPyDetectorPath()),
 		cuda_(Parameters::defaultPyDetectorCuda())
 {
+	PythonInterface::instance("PyDetector");
 	this->parseParameters(parameters);
 
 	UDEBUG("path = %s", path_.c_str());
@@ -39,12 +40,20 @@ PyDetector::PyDetector(const ParametersMap & parameters) :
 	std::string matcherPythonDir = UDirectory::getDir(path_);
 	if(!matcherPythonDir.empty())
 	{
+		// For Windows
+		matcherPythonDir = uReplaceChar(matcherPythonDir, '\\', '/');
 		PyRun_SimpleString("import sys");
 		PyRun_SimpleString(uFormat("sys.path.append(\"%s\")", matcherPythonDir.c_str()).c_str());
 	}
 
 	_import_array();
 
+	// Invalidate importlib's directory-listing caches so a script created
+	// after sys.path was first scanned in this process is still found. Without
+	// this, the second PyDetector instance pointing at a freshly-written
+	// script in an already-known directory fails with ModuleNotFoundError.
+	PyRun_SimpleString("import importlib; importlib.invalidate_caches()");
+
 	std::string scriptName = uSplit(UFile::getName(path_), '.').front();
 	PyObject * pName = PyUnicode_FromString(scriptName.c_str());
 	UDEBUG("PyImport_Import() beg");
@@ -81,7 +90,10 @@ void PyDetector::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kPyDetectorPath(), path_);
 	Parameters::parse(parameters, Parameters::kPyDetectorCuda(), cuda_);
 
-	path_ = uReplaceChar(path_, '~', UDirectory::homeDir());
+	if(!path_.empty() && path_[0] == '~' && (path_.size() == 1 || path_[1] == '/' || path_[1] == '\\'))
+	{
+		path_ = UDirectory::homeDir() + path_.substr(1);
+	}
 }
 
 std::vector<cv::KeyPoint> PyDetector::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask)

corelib/src/python/PyDetector.h

@@ -12,12 +12,13 @@
 #include <vector>
 
 #include "rtabmap/core/PythonInterface.h"
+#include "rtabmap/core/rtabmap_core_export.h"
 #include <Python.h>
 
 namespace rtabmap
 {
 
-class PyDetector : public Feature2D
+class RTABMAP_CORE_EXPORT PyDetector : public Feature2D
 {
 public:
 	PyDetector(const ParametersMap & parameters = ParametersMap());
@@ -25,6 +26,11 @@ public:
 
 	virtual void parseParameters(const ParametersMap & parameters);
 	virtual Feature2D::Type getType() const {return kFeaturePyDetector;}
+	// PyDetector hands off CUDA usage to the python script -- the C++
+	// wrapper can't introspect it, so we report capability is "possible"
+	// whenever the python interpreter is built in. The actual hardware
+	// presence + the script's own decision is out of our hands.
+	virtual bool isGpuAvailable() const override {return true;}
 
 private:
 	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat());

corelib/src/python/PyMatcher.cpp

@@ -30,8 +30,14 @@ PyMatcher::PyMatcher(
 				iterations_(iterations),
 				cuda_(cuda)
 {
-	path_ = uReplaceChar(pythonMatcherPath, '~', UDirectory::homeDir());
-	model_ = uReplaceChar(model, '~', UDirectory::homeDir());
+	PythonInterface::instance("PyMatcher");
+	auto expandTilde = [](const std::string & p) -> std::string {
+		if(!p.empty() && p[0] == '~' && (p.size() == 1 || p[1] == '/' || p[1] == '\\'))
+			return UDirectory::homeDir() + p.substr(1);
+		return p;
+	};
+	path_  = expandTilde(pythonMatcherPath);
+	model_ = expandTilde(model);
 	UINFO("path = %s", path_.c_str());
 	UINFO("model = %s", model_.c_str());
 
@@ -46,12 +52,20 @@ PyMatcher::PyMatcher(
 	std::string matcherPythonDir = UDirectory::getDir(path_);
 	if(!matcherPythonDir.empty())
 	{
+		// For windows:
+		matcherPythonDir = uReplaceChar(matcherPythonDir, '\\', '/');
 		PyRun_SimpleString("import sys");
 		PyRun_SimpleString(uFormat("sys.path.append(\"%s\")", matcherPythonDir.c_str()).c_str());
 	}
 
 	_import_array();
 
+	// Invalidate importlib's directory-listing caches so a script created
+	// after sys.path was first scanned in this process is still found.
+	// Without this, the second Py* instance pointing at a freshly-written
+	// script in an already-known directory fails with ModuleNotFoundError.
+	PyRun_SimpleString("import importlib; importlib.invalidate_caches()");
+
 	std::string scriptName = uSplit(UFile::getName(path_), '.').front();
 	PyObject * pName = PyUnicode_FromString(scriptName.c_str());
 	UDEBUG("PyImport_Import");

corelib/src/python/PyMatcher.h

@@ -10,13 +10,14 @@
 #include <opencv2/core/types.hpp>
 #include <opencv2/core/mat.hpp>
 #include "rtabmap/core/PythonInterface.h"
+#include "rtabmap/core/rtabmap_core_export.h"
 #include <vector>
 #include <Python.h>
 
 namespace rtabmap
 {
 
-class PyMatcher
+class RTABMAP_CORE_EXPORT PyMatcher
 {
 public:
   PyMatcher(const std::string & pythonMatcherPath,

corelib/src/python/PythonInterface.cpp

@@ -8,14 +8,39 @@
 #include <rtabmap/core/PythonInterface.h>
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UThread.h>
+#include <rtabmap/utilite/UConversion.h>
 #include <pybind11/embed.h>
 #include <filesystem>
+#include <thread>
 
 namespace rtabmap {
 
-PythonInterface::PythonInterface()
+namespace {
+// Captured when librtabmap_core is loaded. The dynamic loader runs static
+// initializers on the main thread before main(), so this records the main
+// thread id (as long as the library isn't dlopen'd from a worker thread).
+const std::thread::id g_mainThreadId = std::this_thread::get_id();
+}
+
+PythonInterface & PythonInterface::instance(const std::string & caller)
 {
-	UINFO("Initialize python interpreter");
+	// Meyers singleton: thread-safe construction in C++11, destroyed at exit.
+	// The constructor asserts it runs on the main thread; the caller tag
+	// from the first invocation is captured into the assertion message.
+	static PythonInterface inst(caller);
+	return inst;
+}
+
+PythonInterface::PythonInterface(const std::string & caller)
+{
+	UASSERT_MSG(std::this_thread::get_id() == g_mainThreadId,
+			uFormat("PythonInterface must be created on the main thread "
+					"(first construction triggered by \"%s\"). Call "
+					"PythonInterface::instance() early in main() before "
+					"any worker thread touches a Python-backed class.",
+					caller.empty()?"<unspecified>":caller.c_str()).c_str());
+	UINFO("Initialize python interpreter (triggered by \"%s\")",
+			caller.empty()?"<unspecified>":caller.c_str());
 	guard_ = new pybind11::scoped_interpreter();
 
 	// Tell Python to look in this directory for DLLs
@@ -40,12 +65,28 @@ std::string getPythonTraceback()
 {
 	// Author: https://stackoverflow.com/questions/41268061/c-c-python-exception-traceback-not-being-generated
 
+	// Early-exit when there is no active Python exception: callers commonly
+	// log this after any Python C-API failure, but some failures (notably
+	// PyImport_Import returning NULL after repeated load/unload cycles) do
+	// not set an exception. Without this guard, PyErr_Fetch returns NULL
+	// triples and Py_BuildValue("OOO", NULL, NULL, NULL) below crashes.
+	if(!PyErr_Occurred())
+	{
+		return "<no python exception set>";
+	}
+
 	PyObject* type;
 	PyObject* value;
 	PyObject* traceback;
 
 	PyErr_Fetch(&type, &value, &traceback);
 	PyErr_NormalizeException(&type, &value, &traceback);
+	// PyErr_Fetch may leave value/traceback NULL even when an exception was
+	// active. Py_BuildValue("OOO", ...) rejects NULL slots and raises
+	// SystemError, so substitute Py_None for any missing component.
+	if(!type)      { Py_INCREF(Py_None); type = Py_None; }
+	if(!value)     { Py_INCREF(Py_None); value = Py_None; }
+	if(!traceback) { Py_INCREF(Py_None); traceback = Py_None; }
 
 	std::string fcn = "";
 	fcn += "def get_pretty_traceback(exc_type, exc_value, exc_tb):\n";
@@ -60,13 +101,22 @@ std::string getPythonTraceback()
 	UASSERT(mod);
 	PyObject* method = PyObject_GetAttrString(mod, "get_pretty_traceback");
 	UASSERT(method);
-	PyObject* outStr = PyObject_CallObject(method, Py_BuildValue("OOO", type, value, traceback));
+	PyObject* args = Py_BuildValue("OOO", type, value, traceback);
+	PyObject* outStr = args ? PyObject_CallObject(method, args) : nullptr;
 	std::string pretty;
 	if(outStr)
-		pretty = PyBytes_AsString(PyUnicode_AsASCIIString(outStr));
+	{
+		PyObject* asciiStr = PyUnicode_AsASCIIString(outStr);
+		if(asciiStr)
+		{
+			pretty = PyBytes_AsString(asciiStr);
+			Py_DECREF(asciiStr);
+		}
+	}
 
+	Py_XDECREF(args);
 	Py_DECREF(method);
-	Py_DECREF(outStr);
+	Py_XDECREF(outStr);   // outStr may be NULL if PyObject_CallObject failed
 	Py_DECREF(mod);
 
 	return pretty;

corelib/src/python/rtabmap_trace_superpoint.py

@@ -1,13 +1,46 @@
+"""Trace MagicLeap's SuperPoint pretrained net to TorchScript for rtabmap.
+
+Usage:
+    python rtabmap_trace_superpoint.py [--weights superpoint_v1.pth]
+                                       [--output  superpoint_v1.pt]
+                                       [--model-dir <dir containing demo_superpoint.py>]
+
+`--model-dir` is prepended to sys.path so `from demo_superpoint import ...`
+resolves. Defaults to the directory of `--weights` (or cwd).
+"""
+
+import argparse
 import os
 import sys
-from pathlib import Path
 
 import torch
-import torchvision
-from demo_superpoint  import SuperPointNet
-model = SuperPointNet()
-model.load_state_dict(torch.load("superpoint_v1.pth"))
-model.eval()
-example = torch.rand(1, 1, 640, 480)
-traced_script_module = torch.jit.trace(model, example, check_trace=False)
-traced_script_module.save("superpoint_v1.pt")
+
+
+def main() -> int:
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--weights",   default="superpoint_v1.pth")
+    parser.add_argument("--output",    default="superpoint_v1.pt")
+    parser.add_argument("--model-dir", default=None,
+            help="directory containing demo_superpoint.py (default: dir of --weights, then cwd)")
+    args = parser.parse_args()
+
+    candidates = [args.model_dir, os.path.dirname(os.path.abspath(args.weights)), os.getcwd()]
+    for path in candidates:
+        if path and path not in sys.path:
+            sys.path.insert(0, path)
+
+    from demo_superpoint import SuperPointNet  # noqa: E402
+
+    model = SuperPointNet()
+    model.load_state_dict(torch.load(args.weights, map_location="cpu"))
+    model.eval()
+    example = torch.rand(1, 1, 640, 480)
+    traced_script_module = torch.jit.trace(model, example, check_trace=False)
+    os.makedirs(os.path.dirname(os.path.abspath(args.output)) or ".", exist_ok=True)
+    traced_script_module.save(args.output)
+    print(f"Saved TorchScript: {args.output}")
+    return 0
+
+
+if __name__ == "__main__":
+    sys.exit(main())