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

@@ -48,7 +48,7 @@ namespace rtabmap
 namespace util3d
 {
 
-cv::Mat bgrFromCloud(const pcl::PointCloud<pcl::PointXYZRGBA> & cloud, bool bgrOrder)
+cv::Mat rgbFromCloud(const pcl::PointCloud<pcl::PointXYZRGBA> & cloud, bool bgrOrder)
 {
 	cv::Mat frameBGR = cv::Mat(cloud.height,cloud.width,CV_8UC3);
 
@@ -76,13 +76,9 @@ cv::Mat bgrFromCloud(const pcl::PointCloud<pcl::PointXYZRGBA> & cloud, bool bgrO
 // return float image in meter
 cv::Mat depthFromCloud(
 		const pcl::PointCloud<pcl::PointXYZRGBA> & cloud,
-		float & fx,
-		float & fy,
 		bool depth16U)
 {
 	cv::Mat frameDepth = cv::Mat(cloud.height,cloud.width,depth16U?CV_16UC1:CV_32FC1);
-	fx = 0.0f; // needed to reconstruct the cloud
-	fy = 0.0f; // needed to reconstruct the cloud
 	for(unsigned int h = 0; h < cloud.height; h++)
 	{
 		for(unsigned int w = 0; w < cloud.width; w++)
@@ -102,32 +98,6 @@ cv::Mat depthFromCloud(
 			{
 				frameDepth.at<float>(h,w) = depth;
 			}
-
-			// update constants
-			if(fx == 0.0f &&
-			   uIsFinite(cloud.at(h*cloud.width + w).x) &&
-			   uIsFinite(depth) &&
-			   w != cloud.width/2 &&
-			   depth > 0)
-			{
-				fx = cloud.at(h*cloud.width + w).x / ((float(w) - float(cloud.width)/2.0f) * depth);
-				if(depth16U)
-				{
-					fx*=1000.0f;
-				}
-			}
-			if(fy == 0.0f &&
-			   uIsFinite(cloud.at(h*cloud.width + w).y) &&
-			   uIsFinite(depth) &&
-			   h != cloud.height/2 &&
-			   depth > 0)
-			{
-				fy = cloud.at(h*cloud.width + w).y / ((float(h) - float(cloud.height)/2.0f) * depth);
-				if(depth16U)
-				{
-					fy*=1000.0f;
-				}
-			}
 		}
 	}
 	return frameDepth;
@@ -137,16 +107,12 @@ cv::Mat depthFromCloud(
 void rgbdFromCloud(const pcl::PointCloud<pcl::PointXYZRGBA> & cloud,
 		cv::Mat & frameBGR,
 		cv::Mat & frameDepth,
-		float & fx,
-		float & fy,
 		bool bgrOrder,
 		bool depth16U)
 {
 	frameDepth = cv::Mat(cloud.height,cloud.width,depth16U?CV_16UC1:CV_32FC1);
 	frameBGR = cv::Mat(cloud.height,cloud.width,CV_8UC3);
 
-	fx = 0.0f; // needed to reconstruct the cloud
-	fy = 0.0f; // needed to reconstruct the cloud
 	for(unsigned int h = 0; h < cloud.height; h++)
 	{
 		for(unsigned int w = 0; w < cloud.width; w++)
@@ -181,32 +147,6 @@ void rgbdFromCloud(const pcl::PointCloud<pcl::PointXYZRGBA> & cloud,
 			{
 				frameDepth.at<float>(h,w) = depth;
 			}
-
-			// update constants
-			if(fx == 0.0f &&
-			   uIsFinite(cloud.at(h*cloud.width + w).x) &&
-			   uIsFinite(depth) &&
-			   w != cloud.width/2 &&
-			   depth > 0)
-			{
-				fx = 1.0f/(cloud.at(h*cloud.width + w).x / ((float(w) - float(cloud.width)/2.0f) * depth));
-				if(depth16U)
-				{
-					fx/=1000.0f;
-				}
-			}
-			if(fy == 0.0f &&
-			   uIsFinite(cloud.at(h*cloud.width + w).y) &&
-			   uIsFinite(depth) &&
-			   h != cloud.height/2 &&
-			   depth > 0)
-			{
-				fy = 1.0f/(cloud.at(h*cloud.width + w).y / ((float(h) - float(cloud.height)/2.0f) * depth));
-				if(depth16U)
-				{
-					fy/=1000.0f;
-				}
-			}
 		}
 	}
 }
@@ -1464,28 +1404,25 @@ pcl::PointCloud<pcl::PointXYZ> laserScanFromDepthImages(
 		float minDepth)
 {
 	pcl::PointCloud<pcl::PointXYZ> scan;
+	UASSERT(!depthImages.empty() && !cameraModels.empty());
 	UASSERT(int((depthImages.cols/cameraModels.size())*cameraModels.size()) == depthImages.cols);
 	int subImageWidth = depthImages.cols/cameraModels.size();
 	for(int i=(int)cameraModels.size()-1; i>=0; --i)
 	{
-		if(cameraModels[i].isValidForProjection())
-		{
-			cv::Mat depth = cv::Mat(depthImages, cv::Rect(subImageWidth*i, 0, subImageWidth, depthImages.rows));
+		UASSERT(cameraModels[i].isValidForProjection());
+		UASSERT(cameraModels[i].imageWidth() == subImageWidth);
+		UASSERT(subImageWidth*(i+1) <= depthImages.cols);
+		cv::Mat depth = cv::Mat(depthImages, cv::Rect(subImageWidth*i, 0, subImageWidth, depthImages.rows));
 
-			scan += laserScanFromDepthImage(
-					depth,
-					cameraModels[i].fx(),
-					cameraModels[i].fy(),
-					cameraModels[i].cx(),
-					cameraModels[i].cy(),
-					maxDepth,
-					minDepth,
-					cameraModels[i].localTransform());
-		}
-		else
-		{
-			UERROR("Camera model %d is invalid", i);
-		}
+		scan += laserScanFromDepthImage(
+				depth,
+				cameraModels[i].fx(),
+				cameraModels[i].fy(),
+				cameraModels[i].cx(),
+				cameraModels[i].cy(),
+				maxDepth,
+				minDepth,
+				cameraModels[i].localTransform());
 	}
 	return scan;
 }
@@ -2629,11 +2566,7 @@ pcl::PointXYZRGB laserScanToPointRGB(const LaserScan & laserScan, int index, uns
 
 	if(laserScan.hasRGB())
 	{
-		int * ptrInt = (int*)ptr;
-		int indexRGB = laserScan.getRGBOffset();
-		output.b = (unsigned char)(ptrInt[indexRGB] & 0xFF);
-		output.g = (unsigned char)((ptrInt[indexRGB] >> 8) & 0xFF);
-		output.r = (unsigned char)((ptrInt[indexRGB] >> 16) & 0xFF);
+		LaserScan::unpackRGB(ptr[laserScan.getRGBOffset()], output.r, output.g, output.b);
 	}
 	else if(laserScan.hasIntensity())
 	{
@@ -2695,11 +2628,7 @@ pcl::PointXYZRGBNormal laserScanToPointRGBNormal(const LaserScan & laserScan, in
 
 	if(laserScan.hasRGB())
 	{
-		int * ptrInt = (int*)ptr;
-		int indexRGB = laserScan.getRGBOffset();
-		output.b = (unsigned char)(ptrInt[indexRGB] & 0xFF);
-		output.g = (unsigned char)((ptrInt[indexRGB] >> 8) & 0xFF);
-		output.r = (unsigned char)((ptrInt[indexRGB] >> 16) & 0xFF);
+		LaserScan::unpackRGB(ptr[laserScan.getRGBOffset()], output.r, output.g, output.b);
 	}
 	else if(laserScan.hasIntensity())
 	{
@@ -3325,7 +3254,7 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 		return pointToPixel;
 
 	std::string msg = uFormat("Computing visible points per cam (%d points, %d cams)", (int)cloud.size(), (int)cameraPoses.size());
-	UINFO(msg.c_str());
+	UINFO("%s", msg.c_str());
 	if(state && !state->callback(msg))
 	{
 		//cancelled!
@@ -3454,11 +3383,11 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 				if(count == 0)
 				{
 					registered.clear();
-					UINFO("No points projected in camera %d/%d", pter->first, camIndex);
+					UINFO("No points projected in camera %d/%d", pter->first, (int)camIndex);
 				}
 				else
 				{
-					UDEBUG("%d points projected in camera %d/%d", count, pter->first, camIndex);
+					UDEBUG("%d points projected in camera %d/%d", count, pter->first, (int)camIndex);
 				}
 				for(int u=0; u<imageSize.width; ++u)
 				{
@@ -3516,7 +3445,7 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 		}
 
 		msg = uFormat("Processed camera %d/%d", (int)cameraProcessed+1, (int)cameraPoses.size());
-		UINFO(msg.c_str());
+		UINFO("%s", msg.c_str());
 		if(state && !state->callback(msg))
 		{
 			//cancelled!
@@ -3527,7 +3456,7 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 	}
 
 	msg = uFormat("Select best camera for %d points...", (int)cloud.size());
-	UINFO(msg.c_str());
+	UINFO("%s", msg.c_str());
 	if(state && !state->callback(msg))
 	{
 		//cancelled!
@@ -3560,8 +3489,8 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 		}
 	}
 
-	msg = uFormat("Process %d points...done! (%d [%d%%] projected in cameras)", (int)cloud.size(), colorized, colorized*100/cloud.size());
-	UINFO(msg.c_str());
+	msg = uFormat("Process %d points...done! (%d [%d%%] projected in cameras)", (int)cloud.size(), colorized, (int)(colorized*100/cloud.size()));
+	UINFO("%s", msg.c_str());
 	if(state)
 	{
 		state->callback(msg);