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/util3d_motion_estimation.cpp

@@ -56,6 +56,24 @@ namespace rtabmap
 namespace util3d
 {
 
+namespace {
+// When true, every newly-constructed OpenGV SAC problem inside this
+// translation unit gets its RNG reseeded with a fixed constant so that
+// RANSAC is bit-for-bit reproducible. Tests can flip this on via
+// setRansacDeterministicSeed(true); production code leaves it off.
+bool g_ransacDeterministicSeed = false;
+} // namespace
+
+void setRansacDeterministicSeed(bool enable)
+{
+	g_ransacDeterministicSeed = enable;
+}
+
+bool ransacDeterministicSeedEnabled()
+{
+	return g_ransacDeterministicSeed;
+}
+
 Transform estimateMotion3DTo2D(
 			const std::map<int, cv::Point3f> & words3A,
 			const std::map<int, cv::KeyPoint> & words2B,
@@ -224,11 +242,11 @@ Transform estimateMotion3DTo2D(
 				//divide by 4 instead of 2 to ignore very very far features (stereo)
 				double median_error_sqr_lin = 2.1981 * (double)errorSqrdDists[errorSqrdDists.size () / varianceMedianRatio];
 				UASSERT(uIsFinite(median_error_sqr_lin));
-				(*covariance)(cv::Range(0,3), cv::Range(0,3)) *= median_error_sqr_lin;
+				(*covariance)(cv::Range(0,3), cv::Range(0,3)) *= median_error_sqr_lin + 1e-6;
 				std::sort(errorSqrdAngles.begin(), errorSqrdAngles.end());
 				double median_error_sqr_ang = 2.1981 * (double)errorSqrdAngles[errorSqrdAngles.size () / varianceMedianRatio];
 				UASSERT(uIsFinite(median_error_sqr_ang));
-				(*covariance)(cv::Range(3,6), cv::Range(3,6)) *= median_error_sqr_ang;
+				(*covariance)(cv::Range(3,6), cv::Range(3,6)) *= median_error_sqr_ang + 1e-6;
 
 				if(splitLinearCovarianceComponents)
 				{
@@ -242,9 +260,9 @@ Transform estimateMotion3DTo2D(
 					UASSERT(uIsFinite(median_error_sqr_x));
 					UASSERT(uIsFinite(median_error_sqr_y));
 					UASSERT(uIsFinite(median_error_sqr_z));
-					covariance->at<double>(0,0) = median_error_sqr_x;
-					covariance->at<double>(1,1) = median_error_sqr_y;
-					covariance->at<double>(2,2) = median_error_sqr_z;
+					covariance->at<double>(0,0) = median_error_sqr_x + 1e-6;
+					covariance->at<double>(1,1) = median_error_sqr_y + 1e-6;
+					covariance->at<double>(2,2) = median_error_sqr_z + 1e-6;
 
 					median_error_sqr_lin = uMax3(median_error_sqr_x, median_error_sqr_y, median_error_sqr_z);
 				}
@@ -267,7 +285,7 @@ Transform estimateMotion3DTo2D(
 					err += uNormSquared(imagePoints.at(inliers[i]).x - imagePointsReproj.at(inliers[i]).x, imagePoints.at(inliers[i]).y - imagePointsReproj.at(inliers[i]).y);
 				}
 				UASSERT(uIsFinite(err));
-				*covariance *= std::sqrt(err/float(inliers.size()));
+				*covariance *= std::sqrt(err/float(inliers.size())) + 1e-6;
 			}
 		}
 	}
@@ -401,7 +419,7 @@ Transform estimateMotion3DTo2D(
 			int cameraIndex = int(kpt.x / subImageWidth);
 			UASSERT_MSG(cameraIndex >= 0 && cameraIndex < (int)cameraModels.size(),
 					uFormat("cameraIndex=%d, models=%d, kpt.x=%f, subImageWidth=%f (Camera model image width=%d)",
-							cameraIndex, (int)cameraModels.size(), kpt.x, subImageWidth, cameraModels[cameraIndex].imageWidth()).c_str());
+							cameraIndex, (int)cameraModels.size(), kpt.x, (double)subImageWidth, cameraModels[cameraIndex].imageWidth()).c_str());
 
 			const cv::Point3f & pt = iter->second;
 			objectPoints[oi] = pt;
@@ -420,7 +438,7 @@ Transform estimateMotion3DTo2D(
 
 	UDEBUG("words3A=%d words2B=%d matches=%d words3B=%d guess=%s reprojError=%f iterations=%d samplingPolicy=%ld",
 			(int)words3A.size(), (int)words2B.size(), (int)matches.size(), (int)words3B.size(),
-			guess.prettyPrint().c_str(), reprojError, iterations, samplingPolicy);
+			guess.prettyPrint().c_str(), reprojError, iterations, (long)samplingPolicy);
 
 	if((int)matches.size() >= minInliers)
 	{
@@ -436,7 +454,7 @@ Transform estimateMotion3DTo2D(
 
 			for (size_t i=0; i<cameraModels.size(); ++i)
 			{
-				UDEBUG("Matches in Camera %d: %d", i, cc[i]);
+				UDEBUG("Matches in Camera %d: %d", (int)i, cc[i]);
 				// opengv multi ransac needs at least 2 matches/camera
 				if (cc[i] < 2)
 				{
@@ -483,6 +501,7 @@ Transform estimateMotion3DTo2D(
 			// convert 2d-3d correspondences into bearing vectors
 			std::vector<std::shared_ptr<opengv::bearingVectors_t>> multiBearingVectors;
 			multiBearingVectors.resize(cameraModels.size());
+			std::vector<std::vector<int> > localIndexToGlobalIndex(cameraModels.size());
 			for(size_t i=0; i<cameraModels.size();++i)
 			{
 				multiPoints[i] = std::make_shared<opengv::points_t>();
@@ -497,6 +516,7 @@ Transform estimateMotion3DTo2D(
 				cameraModels[cameraIndex].project(imagePoints[i].x, imagePoints[i].y, 1, pt[0], pt[1], pt[2]);
 				pt = cv::normalize(pt);
 				multiBearingVectors[cameraIndex]->push_back(opengv::bearingVector_t(pt[0], pt[1], pt[2]));
+				localIndexToGlobalIndex[cameraIndex].push_back(i);
 			}
 
 			//create a non-central absolute multi adapter
@@ -515,6 +535,18 @@ Transform estimateMotion3DTo2D(
 			std::shared_ptr<opengv::sac_problems::absolute_pose::MultiNoncentralAbsolutePoseSacProblem> absposeproblem_ptr(
 					new opengv::sac_problems::absolute_pose::MultiNoncentralAbsolutePoseSacProblem(adapter));
 
+			// Opt-in deterministic RANSAC: OpenGV's default constructor seeds
+			// its internal mt19937 from the system clock, so without this
+			// override two calls with identical inputs can produce different
+			// inlier sets / covariances. Tests flip the toggle via
+			// setRansacDeterministicSeed(true).
+			if(g_ransacDeterministicSeed)
+			{
+				absposeproblem_ptr->rng_alg_.seed(12345u);
+				absposeproblem_ptr->rng_gen_.reset(new std::function<int()>(
+						std::bind(*absposeproblem_ptr->rng_dist_, absposeproblem_ptr->rng_alg_)));
+			}
+
 			ransac.sac_model_ = absposeproblem_ptr;
 			ransac.threshold_ = 1.0 - cos(atan(reprojError/cameraModels[0].fx()));
 			ransac.max_iterations_ = iterations;
@@ -527,9 +559,10 @@ Transform estimateMotion3DTo2D(
 
 			UDEBUG("Ransac result: %s", pnp.prettyPrint().c_str());
 			UDEBUG("Ransac iterations done: %d", ransac.iterations_);
-			for (size_t i=0; i < cameraModels.size(); ++i)
-			{
-				inliers.insert(inliers.end(), ransac.inliers_[i].begin(), ransac.inliers_[i].end());
+			for (size_t i=0; i < cameraModels.size(); ++i) {
+				for (size_t j=0; j < ransac.inliers_[i].size(); ++j) {
+					inliers.push_back(localIndexToGlobalIndex[i][ransac.inliers_[i][j]]);
+				}
 			}
 		}
 		else
@@ -569,6 +602,14 @@ Transform estimateMotion3DTo2D(
 			std::shared_ptr<opengv::sac_problems::absolute_pose::AbsolutePoseSacProblem> absposeproblem_ptr(
 					new opengv::sac_problems::absolute_pose::AbsolutePoseSacProblem(adapter, opengv::sac_problems::absolute_pose::AbsolutePoseSacProblem::GP3P));
 
+			// Opt-in deterministic RANSAC (see comment on MultiRansac above).
+			if(g_ransacDeterministicSeed)
+			{
+				absposeproblem_ptr->rng_alg_.seed(12345u);
+				absposeproblem_ptr->rng_gen_.reset(new std::function<int()>(
+						std::bind(*absposeproblem_ptr->rng_dist_, absposeproblem_ptr->rng_alg_)));
+			}
+
 			ransac.sac_model_ = absposeproblem_ptr;
 			ransac.threshold_ = 1.0 - cos(atan(reprojError/cameraModels[0].fx()));
 			ransac.max_iterations_ = iterations;
@@ -705,6 +746,7 @@ Transform estimateMotion3DTo2D(
 
 	if(matchesOut)
 	{
+		matchesOut->clear();
 		matchesOut->resize(cameraModels.size());
 		UASSERT(matches.size() == cameraIndexes.size());
 		for(size_t i=0; i<matches.size(); ++i)
@@ -715,6 +757,7 @@ Transform estimateMotion3DTo2D(
 	}
 	if(inliersOut)
 	{
+		inliersOut->clear();
 		inliersOut->resize(cameraModels.size());
 		for(unsigned int i=0; i<inliers.size(); ++i)
 		{