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_registration.cpp

@@ -31,7 +31,20 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/util3d_filtering.h"
 #include "rtabmap/core/util3d.h"
 
+#include <pcl/registration/correspondence_rejection_sample_consensus.h>
 #include <pcl/registration/icp.h>
+
+// Explicitly instantiate pcl::RandomSampleConsensus and
+// pcl::SampleConsensusModelRegistration for pcl::PointXYZINormal. PCL itself
+// only ships precompiled symbols for the types listed in PCL_XYZ_POINT_TYPES
+// when built without PCL_ONLY_CORE_POINT_TYPES; the Windows pre-built PCL
+// (and any "core point types" build) omits PointXYZINormal, so without this
+// rtabmap_core.dll fails to link when the rejector is used with that type.
+#include <pcl/sample_consensus/impl/ransac.hpp>
+#include <pcl/sample_consensus/impl/sac_model_registration.hpp>
+template class pcl::RandomSampleConsensus<pcl::PointXYZINormal>;
+template class pcl::SampleConsensusModelRegistration<pcl::PointXYZINormal>;
+
 #include <pcl/registration/transformation_estimation_2D.h>
 #include <pcl/registration/transformation_estimation_svd.h>
 #include <pcl/sample_consensus/sac_model_registration.h>
@@ -51,6 +64,7 @@ Transform transformFromXYZCorrespondencesSVD(
 	const pcl::PointCloud<pcl::PointXYZ> & cloud1,
 	const pcl::PointCloud<pcl::PointXYZ> & cloud2)
 {
+	UASSERT(cloud1.size() == cloud2.size());
 	pcl::registration::TransformationEstimationSVD<pcl::PointXYZ, pcl::PointXYZ> svd;
 
 	// Perform the alignment
@@ -205,7 +219,7 @@ Transform transformFromXYZCorrespondences(
 				{
 					double variance =  model->computeVariance();
 					UASSERT(uIsFinite(variance));
-					*covariance *= variance;
+					*covariance *= variance + 1e-6;
 				}
 
 				// get best transformation
@@ -256,7 +270,13 @@ void computeVarianceAndCorrespondencesImpl(
 	est->setInputTarget(target);
 	est->setInputSource(source);
 	pcl::Correspondences correspondences;
-	est->determineReciprocalCorrespondences(correspondences, maxCorrespondenceDistance);
+	if(reciprocal) {
+		est->determineReciprocalCorrespondences(correspondences, maxCorrespondenceDistance);
+	}
+	else {
+		est->determineCorrespondences(correspondences, maxCorrespondenceDistance);
+	}
+
 
 	if(correspondences.size())
 	{
@@ -340,7 +360,12 @@ void computeVarianceAndCorrespondencesImpl(
 	est->setInputTarget(cloudA->size()>cloudB->size()?cloudA:cloudB);
 	est->setInputSource(cloudA->size()>cloudB->size()?cloudB:cloudA);
 	pcl::Correspondences correspondences;
-	est->determineReciprocalCorrespondences(correspondences, maxCorrespondenceDistance);
+	if(reciprocal) {
+		est->determineReciprocalCorrespondences(correspondences, maxCorrespondenceDistance);
+	}
+	else {
+		est->determineCorrespondences(correspondences, maxCorrespondenceDistance);
+	}
 
 	if(correspondences.size()>=3)
 	{
@@ -381,6 +406,28 @@ void computeVarianceAndCorrespondences(
 	computeVarianceAndCorrespondencesImpl<pcl::PointXYZI>(cloudA, cloudB, maxCorrespondenceDistance, variance, correspondencesOut, reciprocal);
 }
 
+// RANSAC-based correspondence rejector: fits a rigid transform on random
+// 3-pair subsets and discards pairs that disagree. PCL's
+// IterativeClosestPoint::setRANSACOutlierRejectionThreshold and
+// setRANSACIterations are NOT honored by ICP itself (only by NDT and
+// k-4PCS), so we install the rejector explicitly here.
+template<typename PointT>
+void addRansacRejector(
+		pcl::IterativeClosestPoint<PointT, PointT> & icp,
+		const typename pcl::PointCloud<PointT>::ConstPtr & cloud_source,
+		const typename pcl::PointCloud<PointT>::ConstPtr & cloud_target,
+		double maxCorrespondenceDistance,
+		float ransacOutlierRatio)
+{
+	typename pcl::registration::CorrespondenceRejectorSampleConsensus<PointT>::Ptr
+			rejector(new pcl::registration::CorrespondenceRejectorSampleConsensus<PointT>());
+	rejector->setInlierThreshold(maxCorrespondenceDistance * ransacOutlierRatio);
+	rejector->setMaximumIterations(50);
+	rejector->setInputSource(cloud_source);
+	rejector->setInputTarget(cloud_target);
+	icp.addCorrespondenceRejector(rejector);
+}
+
 // return transform from source to target (All points must be finite!!!)
 template<typename PointT>
 Transform icpImpl(const typename pcl::PointCloud<PointT>::ConstPtr & cloud_source,
@@ -390,7 +437,9 @@ Transform icpImpl(const typename pcl::PointCloud<PointT>::ConstPtr & cloud_sourc
 			  bool & hasConverged,
 			  pcl::PointCloud<PointT> & cloud_source_registered,
 			  float epsilon,
-			  bool icp2D)
+			  bool icp2D,
+			  float ransacOutlierRatio,
+			  int * iterationsDone)
 {
 	pcl::IterativeClosestPoint<PointT, PointT> icp;
 	// Set the input source and target
@@ -404,19 +453,25 @@ Transform icpImpl(const typename pcl::PointCloud<PointT>::ConstPtr & cloud_sourc
 		icp.setTransformationEstimation(est);
 	}
 
-	// Set the max correspondence distance to 5cm (e.g., correspondences with higher distances will be ignored)
+	// Set the max correspondence distance (e.g., correspondences with higher distances will be ignored)
 	icp.setMaxCorrespondenceDistance (maxCorrespondenceDistance);
 	// Set the maximum number of iterations (criterion 1)
 	icp.setMaximumIterations (maximumIterations);
 	// Set the transformation epsilon (criterion 2)
 	icp.setTransformationEpsilon (epsilon*epsilon);
 	// Set the euclidean distance difference epsilon (criterion 3)
-	//icp.setEuclideanFitnessEpsilon (1);
-	//icp.setRANSACOutlierRejectionThreshold(maxCorrespondenceDistance);
+	//icp.setEuclideanFitnessEpsilon (-std::numeric_limits<double>::max());
+
+	if(ransacOutlierRatio > 0.0f && ransacOutlierRatio < 1.0f)
+	{
+		addRansacRejector<PointT>(icp, cloud_source, cloud_target,
+				maxCorrespondenceDistance, ransacOutlierRatio);
+	}
 
 	// Perform the alignment
 	icp.align (cloud_source_registered);
 	hasConverged = icp.hasConverged();
+	if(iterationsDone) *iterationsDone = icp.nr_iterations_;
 	return Transform::fromEigen4f(icp.getFinalTransformation());
 }
 
@@ -428,9 +483,11 @@ Transform icp(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
 			  bool & hasConverged,
 			  pcl::PointCloud<pcl::PointXYZ> & cloud_source_registered,
 			  float epsilon,
-			  bool icp2D)
+			  bool icp2D,
+			  float ransacOutlierRatio,
+			  int * iterationsDone)
 {
-	return icpImpl(cloud_source, cloud_target, maxCorrespondenceDistance, maximumIterations, hasConverged, cloud_source_registered, epsilon, icp2D);
+	return icpImpl(cloud_source, cloud_target, maxCorrespondenceDistance, maximumIterations, hasConverged, cloud_source_registered, epsilon, icp2D, ransacOutlierRatio, iterationsDone);
 }
 
 // return transform from source to target (All points must be finite!!!)
@@ -441,9 +498,11 @@ Transform icp(const pcl::PointCloud<pcl::PointXYZI>::ConstPtr & cloud_source,
 			  bool & hasConverged,
 			  pcl::PointCloud<pcl::PointXYZI> & cloud_source_registered,
 			  float epsilon,
-			  bool icp2D)
+			  bool icp2D,
+			  float ransacOutlierRatio,
+			  int * iterationsDone)
 {
-	return icpImpl(cloud_source, cloud_target, maxCorrespondenceDistance, maximumIterations, hasConverged, cloud_source_registered, epsilon, icp2D);
+	return icpImpl(cloud_source, cloud_target, maxCorrespondenceDistance, maximumIterations, hasConverged, cloud_source_registered, epsilon, icp2D, ransacOutlierRatio, iterationsDone);
 }
 
 // return transform from source to target (All points/normals must be finite!!!)
@@ -456,7 +515,9 @@ Transform icpPointToPlaneImpl(
 		bool & hasConverged,
 		pcl::PointCloud<PointNormalT> & cloud_source_registered,
 		float epsilon,
-		bool icp2D)
+		bool icp2D,
+		float ransacOutlierRatio,
+		int * iterationsDone)
 {
 	pcl::IterativeClosestPoint<PointNormalT, PointNormalT> icp;
 	// Set the input source and target
@@ -467,7 +528,7 @@ Transform icpPointToPlaneImpl(
 	est.reset(new pcl::registration::TransformationEstimationPointToPlaneLLS<PointNormalT, PointNormalT>);
 	icp.setTransformationEstimation(est);
 
-	// Set the max correspondence distance to 5cm (e.g., correspondences with higher distances will be ignored)
+	// Set the max correspondence distance (e.g., correspondences with higher distances will be ignored)
 	icp.setMaxCorrespondenceDistance (maxCorrespondenceDistance);
 	// Set the maximum number of iterations (criterion 1)
 	icp.setMaximumIterations (maximumIterations);
@@ -475,17 +536,29 @@ Transform icpPointToPlaneImpl(
 	icp.setTransformationEpsilon (epsilon*epsilon);
 	// Set the euclidean distance difference epsilon (criterion 3)
 	//icp.setEuclideanFitnessEpsilon (1);
-	//icp.setRANSACOutlierRejectionThreshold(maxCorrespondenceDistance);
+
+	if(ransacOutlierRatio > 0.0f && ransacOutlierRatio < 1.0f)
+	{
+		addRansacRejector<PointNormalT>(icp, cloud_source, cloud_target,
+				maxCorrespondenceDistance, ransacOutlierRatio);
+	}
 
 	// Perform the alignment
 	icp.align (cloud_source_registered);
 	hasConverged = icp.hasConverged();
+	if(iterationsDone) *iterationsDone = icp.nr_iterations_;
 	Transform t = Transform::fromEigen4f(icp.getFinalTransformation());
 
 	if(icp2D)
 	{
-		// FIXME probably an estimation approach already 2D like in icp() version above exists.
+		// PCL has no 2D-aware PointToPlane estimator (only the
+		// point-to-point pcl::registration::TransformationEstimation2D),
+		// so we run the full 6DoF PointToPlaneLLS above and then snap
+		// the result to 3DoF here.  The 6DoF LLS on planar (z=0) input
+		// is numerically fragile -- callers that need 2D PointToPlane
+		// reliably should use libpointmatcher (Icp/Strategy=1).
 		t = t.to3DoF();
+		pcl::transformPointCloudWithNormals(*cloud_source, cloud_source_registered, t.toEigen4f());
 	}
 
 	return t;
@@ -500,9 +573,11 @@ Transform icpPointToPlane(
 		bool & hasConverged,
 		pcl::PointCloud<pcl::PointNormal> & cloud_source_registered,
 		float epsilon,
-		bool icp2D)
+		bool icp2D,
+		float ransacOutlierRatio,
+		int * iterationsDone)
 {
-	return icpPointToPlaneImpl(cloud_source, cloud_target, maxCorrespondenceDistance, maximumIterations, hasConverged, cloud_source_registered, epsilon, icp2D);
+	return icpPointToPlaneImpl(cloud_source, cloud_target, maxCorrespondenceDistance, maximumIterations, hasConverged, cloud_source_registered, epsilon, icp2D, ransacOutlierRatio, iterationsDone);
 }
 // return transform from source to target (All points/normals must be finite!!!)
 Transform icpPointToPlane(
@@ -513,9 +588,11 @@ Transform icpPointToPlane(
 		bool & hasConverged,
 		pcl::PointCloud<pcl::PointXYZINormal> & cloud_source_registered,
 		float epsilon,
-		bool icp2D)
+		bool icp2D,
+		float ransacOutlierRatio,
+		int * iterationsDone)
 {
-	return icpPointToPlaneImpl(cloud_source, cloud_target, maxCorrespondenceDistance, maximumIterations, hasConverged, cloud_source_registered, epsilon, icp2D);
+	return icpPointToPlaneImpl(cloud_source, cloud_target, maxCorrespondenceDistance, maximumIterations, hasConverged, cloud_source_registered, epsilon, icp2D, ransacOutlierRatio, iterationsDone);
 }
 
 }