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

@@ -673,18 +673,11 @@ int32_t lastFrameFromSegmentLength(std::vector<float> &dist,int32_t first_frame,
 }
 
 inline float rotationError(const Transform &pose_error) {
-	float a = pose_error(0,0);
-	float b = pose_error(1,1);
-	float c = pose_error(2,2);
-	float d = 0.5*(a+b+c-1.0);
-	return std::acos(std::max(std::min(d,1.0f),-1.0f));
+	return pose_error.getAngle(Transform::getIdentity());
 }
 
 inline float translationError(const Transform &pose_error) {
-	float dx = pose_error.x();
-	float dy = pose_error.y();
-	float dz = pose_error.z();
-	return sqrt(dx*dx+dy*dy+dz*dz);
+	return pose_error.getNorm();
 }
 
 void calcKittiSequenceErrors (
@@ -695,6 +688,14 @@ void calcKittiSequenceErrors (
 
 	UASSERT(poses_gt.size() == poses_result.size());
 
+	t_err = 0.0f;
+	r_err = 0.0f;
+
+	if(poses_gt.size() < 2)
+	{
+		return;
+	}
+
 	// error vector
 	std::vector<errors> err;
 
@@ -720,24 +721,35 @@ void calcKittiSequenceErrors (
 			if (last_frame==-1)
 				continue;
 
+			const Transform & gtFirst = poses_gt[first_frame];
+			const Transform & gtLast = poses_gt[last_frame];
+			const Transform & estFirst = poses_result[first_frame];
+			const Transform & estLast = poses_result[last_frame];
+			UASSERT_MSG(gtFirst.isInvertible() && gtLast.isInvertible() &&
+					estFirst.isInvertible() && estLast.isInvertible(),
+					uFormat("Non-invertible poses at frames %d and %d (segment length %f m)",
+							first_frame, last_frame, len).c_str());
+
 			// compute rotational and translational errors
-			Transform pose_delta_gt     = poses_gt[first_frame].inverse()*poses_gt[last_frame];
-			Transform pose_delta_result = poses_result[first_frame].inverse()*poses_result[last_frame];
-			Transform pose_error        = pose_delta_result.inverse()*pose_delta_gt;
-			float r_err = rotationError(pose_error);
-			float t_err = translationError(pose_error);
+			Transform pose_delta_gt     = gtFirst.inverse()*gtLast;
+			Transform pose_delta_result = estFirst.inverse()*estLast;
+			Transform pose_error = pose_delta_result.inverse()*pose_delta_gt;
+			const float rotErr = rotationError(pose_error);
+			const float transErr = translationError(pose_error);
 
 			// compute speed
 			float num_frames = (float)(last_frame-first_frame+1);
-			float speed = len/(0.1*num_frames);
+			float speed = len/(0.1f*num_frames);
 
 			// write to file
-			err.push_back(errors(first_frame,r_err/len,t_err/len,len,speed));
+			err.push_back(errors(first_frame, rotErr/len, transErr/len, len, speed));
 		}
 	}
 
-	t_err = 0;
-	r_err = 0;
+	if(err.empty())
+	{
+		return;
+	}
 
 	// for all errors do => compute sum of t_err, r_err
 	for (std::vector<errors>::iterator it=err.begin(); it!=err.end(); it++)
@@ -747,11 +759,11 @@ void calcKittiSequenceErrors (
 	}
 
 	// save errors
-	float num = err.size();
+	const float num = float(err.size());
 	t_err /= num;
 	r_err /= num;
 	t_err *= 100.0f;    // Translation error (%)
-	r_err *= 180/CV_PI; // Rotation error (deg/m)
+	r_err *= 180.0f/CV_PI; // Rotation error (deg/m)
 }
 // KITTI evaluation end
 
@@ -1471,7 +1483,7 @@ std::map<int, Transform> radiusPosesFiltering(
 
 		//pcl::IndicesPtr indicesOut(new std::vector<int>);
 		//indicesOut->insert(indicesOut->end(), indicesKept.begin(), indicesKept.end());
-		UINFO("Cloud filtered In = %d, Out = %d (radius=%f angle=%f keepLatest=%d)", cloud->size(), indicesKept.size(), radius, angle, keepLatest?1:0);
+		UINFO("Cloud filtered In = %d, Out = %d (radius=%f angle=%f keepLatest=%d)", (int)cloud->size(), (int)indicesKept.size(), radius, angle, keepLatest?1:0);
 		//pcl::io::savePCDFile("duplicateIn.pcd", *cloud);
 		//pcl::io::savePCDFile("duplicateOut.pcd", *cloud, *indicesOut);
 
@@ -1879,6 +1891,7 @@ std::list<std::pair<int, Transform> > computePath(
 						if(mapIter->second == nodeIter->first)
 						{
 							pqmap.erase(mapIter);
+							nodeIter->second.setFromId(currentNode->id());
 							nodeIter->second.setCostSoFar(newCostSoFar);
 							pqmap.insert(std::make_pair(nodeIter->second.totalCost(), nodeIter->first));
 							break;
@@ -1969,7 +1982,7 @@ std::list<int> computePath(
 					pq.push(Pair(n.id(), n.totalCost()));
 				}
 			}
-			else if(!useSameCostForAllLinks && updateNewCosts && nodeIter->second.isOpened())
+			else if(updateNewCosts && nodeIter->second.isOpened())
 			{
 				float newCostSoFar = currentNode->costSoFar() + cost;
 				if(nodeIter->second.costSoFar() > newCostSoFar)
@@ -1980,6 +1993,7 @@ std::list<int> computePath(
 						if(mapIter->second == nodeIter->first)
 						{
 							pqmap.erase(mapIter);
+							nodeIter->second.setFromId(currentNode->id());
 							nodeIter->second.setCostSoFar(newCostSoFar);
 							pqmap.insert(std::make_pair(nodeIter->second.totalCost(), nodeIter->first));
 							break;
@@ -2149,6 +2163,7 @@ std::list<std::pair<int, Transform> > computePath(
 							if(mapIter->second == nodeIter->first)
 							{
 								pqmap.erase(mapIter);
+								nodeIter->second.setFromId(currentNode->id());
 								nodeIter->second.setCostSoFar(newCostSoFar);
 								pqmap.insert(std::make_pair(nodeIter->second.totalCost(), nodeIter->first));
 								break;
@@ -2427,8 +2442,18 @@ std::list<std::map<int, Transform> > getPaths(
 			std::map<int, Transform> path;
 			for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end();)
 			{
-				std::multimap<int, Link>::const_iterator jter = findLink(links, path.rbegin()->first, iter->first);
-				if(path.size() == 0 || (jter != links.end() && (jter->second.type() == Link::kNeighbor || jter->second.type() == Link::kNeighborMerged)))
+				bool addPose = false;
+				if(path.empty())
+				{
+					addPose = true;
+				}
+				else
+				{
+					std::multimap<int, Link>::const_iterator jter = findLink(links, path.rbegin()->first, iter->first);
+					addPose = jter != links.end() &&
+							(jter->second.type() == Link::kNeighbor || jter->second.type() == Link::kNeighborMerged);
+				}
+				if(addPose)
 				{
 					path.insert(*iter);
 					poses.erase(iter++);