i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-04 00:57:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added util3d.h doc and tests

Browse Source
This commit is contained in:
matlabbe
2025-04-26 19:58:37 -07:00
parent f14dae2683
commit bb36ce7b8b
11 changed files with 3126 additions and 259 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/LaserScan.h
+81 -26
View File
@@ -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,
kXYI=2,
kXYNormal=3,
kXYINormal=4,
kXYZ=5,
kXYZI=6,
kXYZRGB=7,
kXYZNormal=8,
kXYZINormal=9,
kXYZRGBNormal=10,
kXYZIT=11};
/**
* @brief Enumeration of possible formats for laser scan data.
*
* These values represent different combinations of point attributes
* that can be stored in a laser scan. The format determines how each
* point in the scan is structured.
*/
enum Format{
kUnknown=0, /**< Unknown format. */
kXY=1, /**< 2D points with X and Y coordinates. */
kXYI=2, /**< 2D points with X, Y and intensity. */
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_;
Format format_;
int maxPoints_;
float rangeMin_;
float rangeMax_;
float angleMin_;
float angleMax_;
float angleIncrement_;
Transform localTransform_;
cv::Mat data_; ///< The scan data matrix.
Format format_; ///< The scan data format.
int maxPoints_; ///< Maximum number of points allowed.
float rangeMin_; ///< Minimum valid range.
float rangeMax_; ///< Maximum valid range.
float angleMin_; ///< Minimum angle (for 2D scans).
float angleMax_; ///< Maximum angle (for 2D scans).
float angleIncrement_; ///< Angular increment (for 2D scans).
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
+3 -1
View File
@@ -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/include/rtabmap/core/util3d.h
+943 -134
View File
File diff suppressed because it is too large Load Diff
corelib/src/LaserScan.cpp
+16
View File
@@ -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
+17 -88
View File
@@ -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())
{
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;
}
@@ -2497,11 +2434,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())
{
@@ -2563,11 +2496,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())
{
corelib/test/CMakeLists.txt
+6 -1
View File
@@ -4,4 +4,9 @@ add_definitions("-DRTABMAP_TEST_DATA_ROOT=\"${TEST_DATA_ROOT}\"")
#util2d.h
add_executable(test_util2d test_util2d.cpp)
target_link_libraries(test_util2d gtest_main rtabmap_core)
gtest_discover_tests(test_util2d)
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)
corelib/test/test_util3d.cpp
+2053
View File
File diff suppressed because it is too large Load Diff