Added util3d.h doc and tests

.github/workflows/cmake.yml

@@ -25,11 +25,7 @@ jobs:
       run: |
         DEBIAN_FRONTEND=noninteractive
         sudo apt-get update
-        sudo apt-get -y install libopencv-dev libpcl-dev git cmake software-properties-common libyaml-cpp-dev libgtest-dev
+        sudo apt-get -y install libopencv-dev libpcl-dev git cmake software-properties-common libyaml-cpp-dev
-        cd /usr/src/gtest
-        sudo cmake .
-        sudo make
-        sudo cp lib/*.a /usr/lib
 
     - uses: actions/checkout@v4
 

Doxyfile

@@ -630,7 +630,7 @@ INLINE_INFO            = YES
 # name. If set to NO, the members will appear in declaration order.
 # The default value is: YES.
 
-SORT_MEMBER_DOCS       = YES
+SORT_MEMBER_DOCS       = NO
 
 # If the SORT_BRIEF_DOCS tag is set to YES then doxygen will sort the brief
 # descriptions of file, namespace and class members alphabetically by member
@@ -854,7 +854,7 @@ WARN_LOGFILE           =
 # spaces. See also FILE_PATTERNS and EXTENSION_MAPPING
 # Note: If this tag is empty the current directory is searched.
 
-INPUT                  = corelib/include guilib/include utilite/include build/corelib/src/include build/guilib/src/include build/utilite/src/include
+INPUT                  = corelib/include build/corelib/src/include
 
 # This tag can be used to specify the character encoding of the source files
 # that doxygen parses. Internally doxygen uses the UTF-8 encoding. Doxygen uses

corelib/include/rtabmap/core/LaserScan.h

@@ -34,22 +34,41 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
+/**
+ * @class LaserScan
+ * @brief Represents 2D or 3D laser scan data with support for multiple point data formats.
+ *
+ * The LaserScan class stores structured laser scan data used in SLAM, mapping, and perception.
+ * It supports various formats including point coordinates, intensity, normals, RGB colors,
+ * and timestamps. Utility methods are provided for format checking, cloning, and combining scans.
+ */
 class RTABMAP_CORE_EXPORT LaserScan
 {
 public:
-	enum Format{kUnknown=0,
+	/**
-		kXY=1,
+	 * @brief Enumeration of possible formats for laser scan data.
-		kXYI=2,
+	 *
-		kXYNormal=3,
+	 * These values represent different combinations of point attributes
-		kXYINormal=4,
+	 * that can be stored in a laser scan. The format determines how each
-		kXYZ=5,
+	 * point in the scan is structured.
-		kXYZI=6,
+	 */
-		kXYZRGB=7,
+	enum Format{
-		kXYZNormal=8,
+		kUnknown=0,			/**< Unknown format. */
-		kXYZINormal=9,
+		kXY=1,				/**< 2D points with X and Y coordinates. */
-		kXYZRGBNormal=10,
+		kXYI=2,				/**< 2D points with X, Y and intensity. */
-		kXYZIT=11};
+		kXYNormal=3,		/**< 2D points with X, Y and normal vectors. */
+		kXYINormal=4,		/**< 2D points with X, Y, intensity and normal vectors. */
+		kXYZ=5,				/**< 3D points with X, Y and Z coordinates. */
+		kXYZI=6,			/**< 3D points with X, Y, Z and intensity. */
+		kXYZRGB=7,			/**< 3D points with X, Y, Z and RGB color. */
+		kXYZNormal=8,		/**< 3D points with X, Y, Z and normal vectors. */
+		kXYZINormal=9,		/**< 3D points with X, Y, Z, intensity and normal vectors. */
+		kXYZRGBNormal=10,	/**< 3D points with X, Y, Z, RGB color and normal vectors. */
+		kXYZIT=11			/**< 3D points with X, Y, Z, intensity and time. */
+	};
 
+	/// @name Static Utility Functions
+	/// @{
 	static std::string formatName(const Format & format);
 	static int channels(const Format & format);
 	static bool isScan2d(const Format & format);
@@ -57,11 +76,21 @@ public:
 	static bool isScanHasRGB(const Format & format);
 	static bool isScanHasIntensity(const Format & format);
 	static bool isScanHasTime(const Format & format);
+	static float packRGB(unsigned char r, unsigned char g, unsigned char b);
+	static void unpackRGB(float rgb, unsigned char & r, unsigned char & g, unsigned char & b);
+
+	/**
+	 * @brief Converts legacy scan format to a LaserScan object.
+	 */
 	static LaserScan backwardCompatibility(
 			const cv::Mat & oldScanFormat,
 			int maxPoints = 0,
 			int maxRange = 0,
 			const Transform & localTransform = Transform::getIdentity());
+	
+	/**
+	 * @brief Converts legacy scan format with additional metadata to a LaserScan.
+	 */
 	static LaserScan backwardCompatibility(
 			const cv::Mat & oldScanFormat,
 			float minRange,
@@ -70,14 +99,19 @@ public:
 			float angleMax,
 			float angleInc,
 			const Transform & localTransform = Transform::getIdentity());
+	/// @}
 
 public:
+
+	/// @name Constructors
+	/// @{
 	LaserScan();
 	LaserScan(const LaserScan & data,
 			int maxPoints,
 			float maxRange,
 			const Transform & localTransform = Transform::getIdentity());
-	// Use version without \"format\" argument.
+	
+	/// @deprecated Use constructor without `format` argument.
 	RTABMAP_DEPRECATED LaserScan(const LaserScan & data,
 			int maxPoints,
 			float maxRange,
@@ -88,7 +122,8 @@ public:
 			float maxRange,
 			Format format,
 			const Transform & localTransform = Transform::getIdentity());
-	// Use version without \"format\" argument.
+	
+	/// @deprecated Use constructor without `format` argument.
 	RTABMAP_DEPRECATED LaserScan(const LaserScan & data,
 			Format format,
 			float minRange,
@@ -112,7 +147,10 @@ public:
 			float angleMax,
 			float angleIncrement,
 			const Transform & localTransform = Transform::getIdentity());
+	/// @}
 
+	/// @name Accessors
+	/// @{
 	const cv::Mat & data() const {return data_;}
 	Format format() const {return format_;}
 	std::string formatName() const {return formatName(format_);}
@@ -125,7 +163,10 @@ public:
 	float angleIncrement() const {return angleIncrement_;}
 	void setLocalTransform(const Transform & t) {localTransform_ = t;}
 	Transform localTransform() const {return localTransform_;}
+	/// @}
 
+	/// @name Status and Format Checks
+	/// @{
 	bool empty() const {return data_.empty();}
 	bool isEmpty() const {return data_.empty();}
 	int size() const {return data_.total();}
@@ -139,24 +180,38 @@ public:
 	bool isOrganized() const {return data_.rows > 1;}
 	LaserScan clone() const;
 	LaserScan densify() const;
+	/// @}
 
+	/// @name Operations
+	/// @{
 	int getIntensityOffset() const {return hasIntensity()?(is2d()?2:3):-1;}
 	int getRGBOffset() const {return hasRGB()?(is2d()?2:3):-1;}
 	int getNormalsOffset() const {return hasNormals()?(2 + (is2d()?0:1) + ((hasRGB() || hasIntensity())?1:0)):-1;}
 	int getTimeOffset() const {return hasTime()?4:-1;}
 
+	/**
+	 * @brief Access a specific field value of a point.
+	 * @param pointIndex Index of the point.
+	 * @param channelOffset Channel offset to access (e.g., 0=X, 1=Y, 2=Z if 2D, for other fields, use corresponding getter functions).
+	 * @return Reference to the field value.
+	 * @see getIntensityOffset() getRGBOffset() getNormalsOffset() getTimeOffset()
+	 */
 	float & field(unsigned int pointIndex, unsigned int channelOffset);
 
+	/**
+	 * @brief Clear the scan data.
+	 */
 	void clear() {data_ = cv::Mat();}
 
 	/**
-	 * Concatenate scan's data, localTransform is ignored.
+	 * @brief Concatenate scan's data (localTransform is ignored).
 	 */
 	LaserScan & operator+=(const LaserScan &);
 	/**
-	 * Concatenate scan's data, localTransform is ignored.
+	 * @brief Concatenate scan's data (localTransform is ignored).
 	 */
 	LaserScan operator+(const LaserScan &);
+	/// @}
 
 private:
 	void init(const cv::Mat & data,
@@ -170,17 +225,17 @@ private:
 			const Transform & localTransform = Transform::getIdentity());
 
 private:
-	cv::Mat data_;
+	cv::Mat data_;				///< The scan data matrix.
-	Format format_;
+	Format format_;				///< The scan data format.
-	int maxPoints_;
+	int maxPoints_;				///< Maximum number of points allowed.
-	float rangeMin_;
+	float rangeMin_;			///< Minimum valid range.
-	float rangeMax_;
+	float rangeMax_;			///< Maximum valid range.
-	float angleMin_;
+	float angleMin_;			///< Minimum angle (for 2D scans).
-	float angleMax_;
+	float angleMax_;			///< Maximum angle (for 2D scans).
-	float angleIncrement_;
+	float angleIncrement_;		///< Angular increment (for 2D scans).
-	Transform localTransform_;
+	Transform localTransform_;	///< Transform from base frame to scan frame.
 };
 
-}
+} // namespace rtabmap
 
 #endif /* CORELIB_INCLUDE_RTABMAP_CORE_LASERSCAN_H_ */

corelib/include/rtabmap/core/camera/CameraOpenni.h

@@ -51,7 +51,9 @@ class Grabber;
 
 namespace rtabmap
 {
-
+/**
+ * @brief OpenNI driver for cameras like Kinect for Xbox 360
+ */
 class RTABMAP_CORE_EXPORT CameraOpenni :
 	public Camera
 {

corelib/src/LaserScan.cpp

@@ -130,6 +130,22 @@ bool LaserScan::isScanHasTime(const Format & format)
 	return format==kXYZIT;
 }
 
+float LaserScan::packRGB(unsigned char r, unsigned char g, unsigned char b)
+{
+	int rgb = ((int)r) << 16 | ((int)g) << 8 | ((int)b);
+	float f;
+    std::memcpy(&f, &rgb, sizeof(float));
+	return f;
+}
+void LaserScan::unpackRGB(float rgb, unsigned char & r, unsigned char & g, unsigned char & b)
+{
+	int * ptrInt = (int*)&rgb;
+	b = (unsigned char)(*ptrInt & 0xFF);
+	g = (unsigned char)((*ptrInt >> 8) & 0xFF);
+	r = (unsigned char)((*ptrInt >> 16) & 0xFF);
+}
+
+
 LaserScan LaserScan::backwardCompatibility(
 		const cv::Mat & oldScanFormat,
 		int maxPoints,

corelib/src/util3d.cpp

@@ -49,7 +49,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);
 
@@ -77,13 +77,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++)
@@ -103,32 +99,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;
@@ -138,16 +108,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++)
@@ -182,32 +148,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;
-				}
-			}
 		}
 	}
 }
@@ -1381,28 +1321,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())
+		UASSERT(cameraModels[i].isValidForProjection());
-		{
+		UASSERT(cameraModels[i].imageWidth() == subImageWidth);
-			cv::Mat depth = cv::Mat(depthImages, cv::Rect(subImageWidth*i, 0, subImageWidth, depthImages.rows));
+		UASSERT(subImageWidth*(i+1) <= depthImages.cols);
+		cv::Mat depth = cv::Mat(depthImages, cv::Rect(subImageWidth*i, 0, subImageWidth, depthImages.rows));
 
-			scan += laserScanFromDepthImage(
+		scan += laserScanFromDepthImage(
-					depth,
+				depth,
-					cameraModels[i].fx(),
+				cameraModels[i].fx(),
-					cameraModels[i].fy(),
+				cameraModels[i].fy(),
-					cameraModels[i].cx(),
+				cameraModels[i].cx(),
-					cameraModels[i].cy(),
+				cameraModels[i].cy(),
-					maxDepth,
+				maxDepth,
-					minDepth,
+				minDepth,
-					cameraModels[i].localTransform());
+				cameraModels[i].localTransform());
-		}
-		else
-		{
-			UERROR("Camera model %d is invalid", i);
-		}
 	}
 	return scan;
 }
@@ -2497,11 +2434,7 @@ pcl::PointXYZRGB laserScanToPointRGB(const LaserScan & laserScan, int index, uns
 
 	if(laserScan.hasRGB())
 	{
-		int * ptrInt = (int*)ptr;
+		LaserScan::unpackRGB(ptr[laserScan.getRGBOffset()], output.r, output.g, output.b);
-		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);
 	}
 	else if(laserScan.hasIntensity())
 	{
@@ -2563,11 +2496,7 @@ pcl::PointXYZRGBNormal laserScanToPointRGBNormal(const LaserScan & laserScan, in
 
 	if(laserScan.hasRGB())
 	{
-		int * ptrInt = (int*)ptr;
+		LaserScan::unpackRGB(ptr[laserScan.getRGBOffset()], output.r, output.g, output.b);
-		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);
 	}
 	else if(laserScan.hasIntensity())
 	{

corelib/test/CMakeLists.txt

@@ -5,3 +5,8 @@ add_definitions("-DRTABMAP_TEST_DATA_ROOT=\"${TEST_DATA_ROOT}\"")
 add_executable(test_util2d test_util2d.cpp)
 target_link_libraries(test_util2d gtest_main rtabmap_core)
 gtest_discover_tests(test_util2d)
+
+#util3d.h
+add_executable(test_util3d test_util3d.cpp)
+target_link_libraries(test_util3d gtest_main rtabmap_core)
+gtest_discover_tests(test_util3d)

data/stereo_rect/stereo_left.yaml

@@ -1,5 +1,6 @@
 %YAML:1.0
-camera_name: stereo_20Hz_left
+---
+camera_name: stereo_left
 image_width: 640
 image_height: 480
 camera_matrix:

data/stereo_rect/stereo_right.yaml

@@ -1,5 +1,6 @@
 %YAML:1.0
-camera_name: stereo_20Hz_right
+---
+camera_name: stereo_right
 image_width: 640
 image_height: 480
 camera_matrix: