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

Added more doc/tests

Browse Source
This commit is contained in:
matlabbe
2025-05-18 20:14:33 -07:00
parent 879da2a0a7
commit 40f965d1d7
4 changed files with 632 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/util3d_filtering.h
+229 -10
View File
@@ -471,26 +471,89 @@ inline pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr uniformSampling(
}
/**
* @defgroup RandomSampling Random Sampling
* @brief Randomly samples a subset of points from the input point cloud.
*
* This function uses the PCL (Point Cloud Library) `RandomSample` filter to randomly
* select a specified number of points from the input cloud. It ensures the number
* of samples is greater than zero and returns a new point cloud containing the sampled points.
*
* @param cloud A pointer to the input point cloud from which to sample points.
* @param samples The number of points to randomly sample from the input cloud. Must be > 0.
* @return A pointer to a new point cloud containing the randomly sampled points.
*
* @throws Assertion failure if `samples <= 0`.
*/
/**
* @ingroup RandomSampling
* @brief Performs random sampling on a point cloud of type `pcl::PointXYZ`.
*/
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_CORE_EXPORT randomSampling(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
int samples);
/**
* @ingroup RandomSampling
* @brief Performs random sampling on a point cloud of type `pcl::PointNormal`.
*/
pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_CORE_EXPORT randomSampling(
const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
int samples);
/**
* @ingroup RandomSampling
* @brief Performs random sampling on a point cloud of type `pcl::PointXYZRGB`.
*/
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_CORE_EXPORT randomSampling(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
int samples);
/**
* @ingroup RandomSampling
* @brief Performs random sampling on a point cloud of type `pcl::PointXYZRGBNormal`.
*/
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_CORE_EXPORT randomSampling(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
int samples);
/**
* @ingroup RandomSampling
* @brief Performs random sampling on a point cloud of type `pcl::PointXYZI`.
*/
pcl::PointCloud<pcl::PointXYZI>::Ptr RTABMAP_CORE_EXPORT randomSampling(
const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
int samples);
/**
* @ingroup RandomSampling
* @brief Performs random sampling on a point cloud of type `pcl::PointXYZINormal`.
*/
pcl::PointCloud<pcl::PointXYZINormal>::Ptr RTABMAP_CORE_EXPORT randomSampling(
const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
int samples);
/**
* @defgroup PassThrough Pass-through Filtering
* @brief Filters points from the input point cloud using a pass-through filter along a specified axis.
*
* This function applies a PCL `PassThrough` filter to include or exclude points in the input cloud
* that fall within a specified range along a given axis (`x`, `y`, or `z`). It supports optional filtering
* using a subset of point indices and can perform either inclusive or exclusive filtering depending on the
* `negative` flag.
*
* @param cloud A pointer to the input point cloud to be filtered.
* @param indices A pointer to the indices specifying which points in the cloud to consider for filtering.
* Pass `nullptr` to apply the filter to the entire cloud.
* @param axis The axis (`"x"`, `"y"`, or `"z"`) along which the filtering will be applied.
* @param min The minimum limit of the pass-through filter (inclusive by default).
* @param max The maximum limit of the pass-through filter (inclusive by default). Must be greater than `min`.
* @param negative If `true`, the filter will exclude points within the limits; if `false`, it will include only those within.
*
* @return The indices representing the points that passed the filter if indices are
* passed to the function, or the new filtered point cloud otherwise.
*
* @throws Assertion failure if `max <= min` or if `axis` is not `"x"`, `"y"`, or `"z"`.
*/
/**
* @ingroup PassThrough
* @brief Performs pass-through filtering on a point cloud of type `pcl::PointXYZ` and returns filtered indices.
*/
pcl::IndicesPtr RTABMAP_CORE_EXPORT passThrough(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const pcl::IndicesPtr & indices,
@@ -498,6 +561,10 @@ pcl::IndicesPtr RTABMAP_CORE_EXPORT passThrough(
float min,
float max,
bool negative = false);
/**
* @ingroup PassThrough
* @brief Performs pass-through filtering on a point cloud of type `pcl::PointXYZRGB` and returns filtered indices.
*/
pcl::IndicesPtr RTABMAP_CORE_EXPORT passThrough(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices,
@@ -505,6 +572,10 @@ pcl::IndicesPtr RTABMAP_CORE_EXPORT passThrough(
float min,
float max,
bool negative = false);
/**
* @ingroup PassThrough
* @brief Performs pass-through filtering on a point cloud of type `pcl::PointXYZI` and returns filtered indices.
*/
pcl::IndicesPtr RTABMAP_CORE_EXPORT passThrough(
const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
const pcl::IndicesPtr & indices,
@@ -512,6 +583,10 @@ pcl::IndicesPtr RTABMAP_CORE_EXPORT passThrough(
float min,
float max,
bool negative = false);
/**
* @ingroup PassThrough
* @brief Performs pass-through filtering on a point cloud of type `pcl::PointNormal` and returns filtered indices.
*/
pcl::IndicesPtr RTABMAP_CORE_EXPORT passThrough(
const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
@@ -519,6 +594,10 @@ pcl::IndicesPtr RTABMAP_CORE_EXPORT passThrough(
float min,
float max,
bool negative = false);
/**
* @ingroup PassThrough
* @brief Performs pass-through filtering on a point cloud of type `pcl::PointXYZRGBNormal` and returns filtered indices.
*/
pcl::IndicesPtr RTABMAP_CORE_EXPORT passThrough(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
@@ -526,6 +605,10 @@ pcl::IndicesPtr RTABMAP_CORE_EXPORT passThrough(
float min,
float max,
bool negative = false);
/**
* @ingroup PassThrough
* @brief Performs pass-through filtering on a point cloud of type `pcl::PointXYZINormal` and returns filtered indices.
*/
pcl::IndicesPtr RTABMAP_CORE_EXPORT passThrough(
const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
@@ -533,36 +616,60 @@ pcl::IndicesPtr RTABMAP_CORE_EXPORT passThrough(
float min,
float max,
bool negative = false);
/**
* @ingroup PassThrough
* @brief Performs pass-through filtering on a point cloud of type `pcl::PointXYZ` and returns a new point cloud of the filtered points.
*/
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_CORE_EXPORT passThrough(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const std::string & axis,
float min,
float max,
bool negative = false);
/**
* @ingroup PassThrough
* @brief Performs pass-through filtering on a point cloud of type `pcl::PointXYZRGB` and returns a new point cloud of the filtered points.
*/
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_CORE_EXPORT passThrough(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const std::string & axis,
float min,
float max,
bool negative = false);
/**
* @ingroup PassThrough
* @brief Performs pass-through filtering on a point cloud of type `pcl::PointXYZI` and returns a new point cloud of the filtered points.
*/
pcl::PointCloud<pcl::PointXYZI>::Ptr RTABMAP_CORE_EXPORT passThrough(
const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
const std::string & axis,
float min,
float max,
bool negative = false);
/**
* @ingroup PassThrough
* @brief Performs pass-through filtering on a point cloud of type `pcl::PointNormal` and returns a new point cloud of the filtered points.
*/
pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_CORE_EXPORT passThrough(
const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
const std::string & axis,
float min,
float max,
bool negative = false);
/**
* @ingroup PassThrough
* @brief Performs pass-through filtering on a point cloud of type `pcl::PointXYZRGBNormal` and returns a new point cloud of the filtered points.
*/
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_CORE_EXPORT passThrough(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const std::string & axis,
float min,
float max,
bool negative = false);
/**
* @ingroup PassThrough
* @brief Performs pass-through filtering on a point cloud of type `pcl::PointXYZINormal` and returns a new point cloud of the filtered points.
*/
pcl::PointCloud<pcl::PointXYZINormal>::Ptr RTABMAP_CORE_EXPORT passThrough(
const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
const std::string & axis,
@@ -570,6 +677,31 @@ pcl::PointCloud<pcl::PointXYZINormal>::Ptr RTABMAP_CORE_EXPORT passThrough(
float max,
bool negative = false);
/**
* @defgroup CropBox Crop Box Filtering
* @brief Filters points from a point cloud using a 3D crop box.
*
* This function applies a PCL `CropBox` filter to retain or exclude points from the input point cloud
* that lie within a specified 3D axis-aligned bounding box, optionally transformed by a 3D transformation.
* The filter can be applied to the entire cloud or limited to a subset via input indices.
*
* @param cloud A pointer to the input point cloud to be filtered.
* @param indices A pointer to a vector of point indices that restricts the filtering to a subset of the cloud.
* Pass `nullptr` to apply the filter to all points in the cloud.
* @param min The minimum (corner) boundary of the crop box, as an Eigen 4D vector (x, y, z, _).
* @param max The maximum (corner) boundary of the crop box, as an Eigen 4D vector (x, y, z, _).
* @param transform A transformation to be applied to the crop box (not the cloud). If the transform is null
* or identity, no transformation is applied.
* @param negative If `true`, the filter will exclude points inside the box; if `false`, it will include only those inside.
*
* @return The indices corresponding to the points that passed the filter, or the new filtered point cloud otherwise.
*
* @throws Assertion failure if any of `min[x] >= max[x]`, `min[y] >= max[y]`, or `min[z] >= max[z]`.
*/
/**
* @ingroup CropBox
* @brief Performs crop box filtering on a point cloud of type `pcl::PCLPointCloud2` and returns filtered indices.
*/
pcl::IndicesPtr RTABMAP_CORE_EXPORT cropBox(
const pcl::PCLPointCloud2::Ptr & cloud,
const pcl::IndicesPtr & indices,
@@ -577,6 +709,10 @@ pcl::IndicesPtr RTABMAP_CORE_EXPORT cropBox(
const Eigen::Vector4f & max,
const Transform & transform = Transform::getIdentity(),
bool negative = false);
/**
* @ingroup CropBox
* @brief Performs crop box filtering on a point cloud of type `pcl::PointXYZ` and returns filtered indices.
*/
pcl::IndicesPtr RTABMAP_CORE_EXPORT cropBox(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const pcl::IndicesPtr & indices,
@@ -584,6 +720,10 @@ pcl::IndicesPtr RTABMAP_CORE_EXPORT cropBox(
const Eigen::Vector4f & max,
const Transform & transform = Transform::getIdentity(),
bool negative = false);
/**
* @ingroup CropBox
* @brief Performs crop box filtering on a point cloud of type `pcl::PointNormal` and returns filtered indices.
*/
pcl::IndicesPtr RTABMAP_CORE_EXPORT cropBox(
const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
@@ -591,6 +731,10 @@ pcl::IndicesPtr RTABMAP_CORE_EXPORT cropBox(
const Eigen::Vector4f & max,
const Transform & transform = Transform::getIdentity(),
bool negative = false);
/**
* @ingroup CropBox
* @brief Performs crop box filtering on a point cloud of type `pcl::PointXYZRGB` and returns filtered indices.
*/
pcl::IndicesPtr RTABMAP_CORE_EXPORT cropBox(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices,
@@ -598,6 +742,10 @@ pcl::IndicesPtr RTABMAP_CORE_EXPORT cropBox(
const Eigen::Vector4f & max,
const Transform & transform = Transform::getIdentity(),
bool negative = false);
/**
* @ingroup CropBox
* @brief Performs crop box filtering on a point cloud of type `pcl::PointXYZRGBNormal` and returns filtered indices.
*/
pcl::IndicesPtr RTABMAP_CORE_EXPORT cropBox(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
@@ -605,6 +753,10 @@ pcl::IndicesPtr RTABMAP_CORE_EXPORT cropBox(
const Eigen::Vector4f & max,
const Transform & transform = Transform::getIdentity(),
bool negative = false);
/**
* @ingroup CropBox
* @brief Performs crop box filtering on a point cloud of type `pcl::PointXYZI` and returns filtered indices.
*/
pcl::IndicesPtr RTABMAP_CORE_EXPORT cropBox(
const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
const pcl::IndicesPtr & indices,
@@ -612,6 +764,10 @@ pcl::IndicesPtr RTABMAP_CORE_EXPORT cropBox(
const Eigen::Vector4f & max,
const Transform & transform = Transform::getIdentity(),
bool negative = false);
/**
* @ingroup CropBox
* @brief Performs crop box filtering on a point cloud of type `pcl::PointXYZINormal` and returns filtered indices.
*/
pcl::IndicesPtr RTABMAP_CORE_EXPORT cropBox(
const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
@@ -619,36 +775,60 @@ pcl::IndicesPtr RTABMAP_CORE_EXPORT cropBox(
const Eigen::Vector4f & max,
const Transform & transform = Transform::getIdentity(),
bool negative = false);
/**
* @ingroup CropBox
* @brief Performs crop box filtering on a point cloud of type `pcl::PointXYZ` and returns a new point cloud of the filtered points.
*/
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_CORE_EXPORT cropBox(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const Eigen::Vector4f & min,
const Eigen::Vector4f & max,
const Transform & transform = Transform::getIdentity(),
bool negative = false);
/**
* @ingroup CropBox
* @brief Performs crop box filtering on a point cloud of type `pcl::PointNormal` and returns a new point cloud of the filtered points.
*/
pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_CORE_EXPORT cropBox(
const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
const Eigen::Vector4f & min,
const Eigen::Vector4f & max,
const Transform & transform = Transform::getIdentity(),
bool negative = false);
/**
* @ingroup CropBox
* @brief Performs crop box filtering on a point cloud of type `pcl::PointXYZRGB` and returns a new point cloud of the filtered points.
*/
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_CORE_EXPORT cropBox(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const Eigen::Vector4f & min,
const Eigen::Vector4f & max,
const Transform & transform = Transform::getIdentity(),
bool negative = false);
/**
* @ingroup CropBox
* @brief Performs crop box filtering on a point cloud of type `pcl::PointXYZI` and returns a new point cloud of the filtered points.
*/
pcl::PointCloud<pcl::PointXYZI>::Ptr RTABMAP_CORE_EXPORT cropBox(
const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
const Eigen::Vector4f & min,
const Eigen::Vector4f & max,
const Transform & transform = Transform::getIdentity(),
bool negative = false);
/**
* @ingroup CropBox
* @brief Performs crop box filtering on a point cloud of type `pcl::PointXYZINormal` and returns a new point cloud of the filtered points.
*/
pcl::PointCloud<pcl::PointXYZINormal>::Ptr RTABMAP_CORE_EXPORT cropBox(
const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
const Eigen::Vector4f & min,
const Eigen::Vector4f & max,
const Transform & transform = Transform::getIdentity(),
bool negative = false);
/**
* @ingroup CropBox
* @brief Performs crop box filtering on a point cloud of type `pcl::PointXYZRGBNormal` and returns a new point cloud of the filtered points.
*/
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_CORE_EXPORT cropBox(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const Eigen::Vector4f & min,
@@ -656,31 +836,70 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_CORE_EXPORT cropBox(
const Transform & transform = Transform::getIdentity(),
bool negative = false);
//Note: This assumes a coordinate system where X is forward, * Y is up, and Z is right.
/**
* @defgroup FrustumFiltering Frustum Filtering
* @brief Filters points from a point cloud based on a camera frustum.
*
* This function uses PCL's `FrustumCulling` filter to include or exclude points from a 3D point cloud
* that lie within a specified camera frustum. The frustum is defined by vertical and horizontal field of view
* angles, near and far clipping planes, and the camera pose. It can optionally apply filtering on a subset
* of points specified by indices.
*
* @note This assumes the pose includes the optical rotation of the camera (X right, Y down, Z forward).
*
* @param cloud A pointer to the input point cloud to be filtered.
* @param indices A pointer to the subset of indices in the point cloud to consider for filtering.
* Pass `nullptr` to apply the filter to the full cloud.
* @param cameraPose The transformation representing the camera pose in world coordinates.
* This defines the position and orientation of the frustum.
* @param horizontalFOV The horizontal field of view in degrees (e.g., computed as atan((image width/2)/fx) * 2).
* @param verticalFOV The vertical field of view in degrees (e.g., atan((image height/2)/fy) * 2).
* @param nearClipPlaneDistance The distance from the camera to the near clipping plane.
* @param farClipPlaneDistance The distance from the camera to the far clipping plane.
* @param negative If `true`, the filter will exclude points inside the frustum.
* If `false`, it will include only those inside.
*
* @return The indices representing the points that passed the filter, or a new filtered point cloud.
*
* @throws Assertion failure if:
* - `horizontalFOV <= 0` or `verticalFOV <= 0`,
* - `farClipPlaneDistance <= nearClipPlaneDistance`,
* - `cameraPose` is null.
*/
/**
* @ingroup FrustumFiltering
* @brief Performs frustum filtering on a point cloud of type `pcl::PointXYZ` and returns filtered indices.
*/
pcl::IndicesPtr RTABMAP_CORE_EXPORT frustumFiltering(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const pcl::IndicesPtr & indices,
const Transform & cameraPose,
float horizontalFOV, // in degrees, xfov = atan((image_width/2)/fx)*2
float verticalFOV, // in degrees, yfov = atan((image_height/2)/fy)*2
float horizontalFOV,
float verticalFOV,
float nearClipPlaneDistance,
float farClipPlaneDistance,
bool negative = false);
//Note: This assumes a coordinate system where X is forward, * Y is up, and Z is right.
/**
* @ingroup FrustumFiltering
* @brief Performs frustum filtering on a point cloud of type `pcl::PointXYZ` and returns a new point cloud of the filtered points.
*/
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_CORE_EXPORT frustumFiltering(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const Transform & cameraPose,
float horizontalFOV, // in degrees, xfov = atan((image_width/2)/fx)*2
float verticalFOV, // in degrees, yfov = atan((image_height/2)/fy)*2
float horizontalFOV,
float verticalFOV,
float nearClipPlaneDistance,
float farClipPlaneDistance,
bool negative = false);
//Note: This assumes a coordinate system where X is forward, * Y is up, and Z is right.
/**
* @ingroup FrustumFiltering
* @brief Performs frustum filtering on a point cloud of type `pcl::PointXYZRGB` and returns a new point cloud of the filtered points.
*/
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_CORE_EXPORT frustumFiltering(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const Transform & cameraPose,
float horizontalFOV, // in degrees, xfov = atan((image_width/2)/fx)*2
float verticalFOV, // in degrees, yfov = atan((image_height/2)/fy)*2
float horizontalFOV,
float verticalFOV,
float nearClipPlaneDistance,
float farClipPlaneDistance,
bool negative = false);
corelib/src/util3d_filtering.cpp
+34 -6
View File
@@ -1057,7 +1057,10 @@ pcl::IndicesPtr cropBoxImpl(
filter.setMax(max);
if(!transform.isNull() && !transform.isIdentity())
{
filter.setTransform(transform.toEigen3f());
float x,y,z,roll,pitch,yaw;
transform.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
filter.setTranslation(Eigen::Vector3f(x,y,z));
filter.setRotation(Eigen::Vector3f(roll,pitch,yaw));
}
filter.setInputCloud(cloud);
filter.setIndices(indices);
@@ -1076,7 +1079,10 @@ pcl::IndicesPtr cropBox(const pcl::PCLPointCloud2::Ptr & cloud, const pcl::Indic
filter.setMax(max);
if(!transform.isNull() && !transform.isIdentity())
{
filter.setTransform(transform.toEigen3f());
float x,y,z,roll,pitch,yaw;
transform.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
filter.setTranslation(Eigen::Vector3f(x,y,z));
filter.setRotation(Eigen::Vector3f(roll,pitch,yaw));
}
filter.setInputCloud(cloud);
filter.setIndices(indices);
@@ -1161,8 +1167,8 @@ pcl::IndicesPtr frustumFilteringImpl(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
const pcl::IndicesPtr & indices,
const Transform & cameraPose,
float horizontalFOV, // in degrees
float verticalFOV, // in degrees
float horizontalFOV, // in degrees, xfov = atan((image_width/2)/fx)*2
float verticalFOV, // in degrees, yfov = atan((image_height/2)/fy)*2
float nearClipPlaneDistance,
float farClipPlaneDistance,
bool negative)
@@ -1184,7 +1190,18 @@ pcl::IndicesPtr frustumFilteringImpl(
fc.setNearPlaneDistance (nearClipPlaneDistance);
fc.setFarPlaneDistance (farClipPlaneDistance);
fc.setCameraPose (cameraPose.toEigen4f());
//The pcl function assumes a coordinate system where:
// - X is forward (view direction),
// - Y is up,
// - Z is to the right.
// Convert from the traditional camera coordinate system (X right, Y down, Z forward):
Transform cam2robot(
0, 0, 1, 0,
0,-1, 0, 0,
1, 0, 0, 0);
Transform pose_new = cameraPose * cam2robot;
fc.setCameraPose (pose_new.toEigen4f());
fc.filter (*output);
return output;
@@ -1217,7 +1234,18 @@ typename pcl::PointCloud<PointT>::Ptr frustumFilteringImpl(
fc.setNearPlaneDistance (nearClipPlaneDistance);
fc.setFarPlaneDistance (farClipPlaneDistance);
fc.setCameraPose (cameraPose.toEigen4f());
//The pcl function assumes a coordinate system where:
// - X is forward (view direction),
// - Y is up,
// - Z is to the right.
// Convert from the traditional camera coordinate system (X right, Y down, Z forward):
Transform cam2robot(
0, 0, 1, 0,
0,-1, 0, 0,
1, 0, 0, 0);
Transform pose_new = cameraPose * cam2robot;
fc.setCameraPose (pose_new.toEigen4f());
fc.filter (*output);
return output;
corelib/test/test_util3d.cpp
+34 -10
View File
@@ -1668,51 +1668,75 @@ TEST(Util3dTest, projectCloudToCameras) {
pt3.normal_z = 0.0f;
cloud.push_back(pt3);
// Mock camera poses (using some basic transform for testing)
pcl::PointXYZRGBNormal pt4;
pt4.x = 1.0f;
pt4.y = 0.0f;
pt4.z = -0.025f; // below pt1 by 2.5 cm
pt4.normal_x = 0.0f;
pt4.normal_y = 0.0f;
pt4.normal_z = 1.0f; // Normal pointing upwards
cloud.push_back(pt4);
pcl::PointXYZRGBNormal pt5;
pt5.x = 1.0f;
pt5.y = 0.0f;
pt5.z = -0.1f; // below pt1 by 10 cm
pt5.normal_x = 0.0f;
pt5.normal_y = 0.0f;
pt5.normal_z = 1.0f; // Normal pointing upwards
cloud.push_back(pt5);
std::map<int, Transform> cameraPoses;
cameraPoses[1] = Transform(0.5f, 0.0f, 0.0f,0,0,0); // Camera looking at pt2, but closer to pt1 than camera 2
cameraPoses[2] = Transform(1.0f, 0.0f, 1.0f,0,M_PI/2,0); // Camera looking at pt1 (looking down)
cameraPoses[1] = Transform(0.5f, 0.0f, 0.0f,0,0,0); // Camera 1 looking at pt2, but closer to pt1 than camera 2
cameraPoses[2] = Transform(1.0f, 0.0f, 1.0f,0,M_PI/2,0); // Camera 2 looking at pt1 (looking down)
// Mock camera models
std::map<int, std::vector<CameraModel>> cameraModels;
CameraModel model(500, 500, 319.5f, 239.5f, CameraModel::opticalRotation(), 0, cv::Size(640,480));
cameraModels[1].push_back(model);
cameraModels[2].push_back(model);
model.setLocalTransform(Transform(0,0,0,0,0,M_PI/2)*CameraModel::opticalRotation()); // this camera is looking left (only pt3 in FOV)
model.setLocalTransform(Transform(0,0,0,0,0,M_PI/2)*CameraModel::opticalRotation()); // this view from camera 1 position is looking left (only pt3 in FOV)
cameraModels[1].push_back(model);
// Set parameters for projection
float maxDistance = 10.0f;
float maxAngle = 45.0f;
float maxAngle = 45.0f * M_PI/ 180.0f;
float maxDepthError = 0.05f; // For camera 1, it should see pt1 and pt4, but not pt5
std::vector<float> roiRatios = {0.0f, 0.0f, 0.0f, 0.0f}; // Full image ROI
cv::Mat projMask = cv::Mat::ones(480, 640, CV_8UC1); // Projection mask (all valid)
bool distanceToCamPolicy = true;
ProgressState* state = nullptr; // Not using progress state in this test
ULogger::setLevel(ULogger::kDebug);
ULogger::setType(ULogger::kTypeConsole);
// Call the function to test
auto result = util3d::projectCloudToCameras(cloud, cameraPoses, cameraModels, maxDistance, maxAngle, roiRatios, projMask, distanceToCamPolicy, state);
auto result = util3d::projectCloudToCameras(cloud, cameraPoses, cameraModels, maxDistance, maxAngle, maxDepthError, roiRatios, projMask, distanceToCamPolicy, state);
// Validate the result
ASSERT_EQ(result.size(), cloud.size()); // The result should have the same size as the input point cloud
// Check the first point's projection
EXPECT_EQ(result[0].first.first, 2); // Camera node ID
EXPECT_EQ(result[0].first.second, 0); // Camera index
EXPECT_NEAR(result[0].second.x, 0.5f, 0.1f); // UV x-coordinate, close to the center
EXPECT_NEAR(result[0].second.y, 0.5f, 0.1f); // UV y-coordinate, close to the center
// Check the second point's projection
EXPECT_EQ(result[1].first.first, 1); // Camera node ID
EXPECT_EQ(result[1].first.second, 0); // Camera index
EXPECT_NEAR(result[1].second.x, 0.5f, 0.1f); // UV x-coordinate, close to the center
EXPECT_NEAR(result[1].second.y, 0.5f, 0.1f); // UV y-coordinate, close to the center
// Check the second point's projection
EXPECT_EQ(result[2].first.first, 1); // Camera node ID
EXPECT_EQ(result[2].first.second, 1); // Camera index
EXPECT_NEAR(result[2].second.x, 0.5f, 0.1f); // UV x-coordinate, close to the center
EXPECT_NEAR(result[2].second.y, 0.5f, 0.1f); // UV y-coordinate, close to the center
EXPECT_EQ(result[3].first.first, 2); // Camera node ID
EXPECT_EQ(result[3].first.second, 0); // Camera index
EXPECT_NEAR(result[3].second.x, 0.5f, 0.1f); // UV x-coordinate, close to the center
EXPECT_NEAR(result[3].second.y, 0.5f, 0.1f); // UV y-coordinate, close to the center
EXPECT_EQ(result[4].first.first, 0); // Camera node ID (not found = 0)
}
TEST(Util3dTest, isFinite) {
corelib/test/test_util3d_filtering.cpp
+335
View File
@@ -1,5 +1,6 @@
#include "gtest/gtest.h"
#include "rtabmap/core/util3d_filtering.h"
#include "rtabmap/core/CameraModel.h"
#include "rtabmap/utilite/UException.h"
#include "rtabmap/utilite/UConversion.h"
@@ -1007,4 +1008,338 @@ TEST(Util3dFiltering, voxelizeInvalidVoxelSize) {
EXPECT_THROW(util3d::voxelize(pcl::PointCloud<pcl::PointXYZI>::Ptr(new pcl::PointCloud<pcl::PointXYZI>()), indices, 0.0f), UException);
EXPECT_THROW(util3d::voxelize(pcl::PointCloud<pcl::PointXYZINormal>::Ptr(new pcl::PointCloud<pcl::PointXYZINormal>()), 0.0f), UException);
EXPECT_THROW(util3d::voxelize(pcl::PointCloud<pcl::PointXYZINormal>::Ptr(new pcl::PointCloud<pcl::PointXYZINormal>()), indices, 0.0f), UException);
}
TEST(Util3dFiltering, randomSamplingSamplesCorrectNumberOfPoints)
{
constexpr int total_points = 100;
constexpr int sample_size = 10;
// Create a point cloud with 100 points
pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud(new pcl::PointCloud<pcl::PointXYZ>);
for (int i = 0; i < total_points; ++i) {
input_cloud->points.emplace_back(static_cast<float>(i), static_cast<float>(i), static_cast<float>(i));
}
input_cloud->width = total_points;
input_cloud->height = 1;
input_cloud->is_dense = true;
// Call the function under test
pcl::PointCloud<pcl::PointXYZ>::Ptr sampled_cloud = util3d::randomSampling(input_cloud, sample_size);
// Validate output
ASSERT_EQ(sampled_cloud->size(), sample_size);
// Check that the sampled points are from the input set
for (const auto& pt : *sampled_cloud) {
bool found = false;
for (const auto& orig_pt : *input_cloud) {
if (pt.x == orig_pt.x && pt.y == orig_pt.y && pt.z == orig_pt.z) {
found = true;
break;
}
}
EXPECT_TRUE(found) << "Sampled point not found in input cloud: (" << pt.x << ", " << pt.y << ", " << pt.z << ")";
}
}
TEST(Util3dFiltering, randomSamplingThrowsAssertionForInvalidSampleSize)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud(new pcl::PointCloud<pcl::PointXYZ>);
input_cloud->push_back(pcl::PointXYZ(1.0f, 2.0f, 3.0f));
EXPECT_THROW(util3d::randomSampling(input_cloud, 0), UException);
}
TEST(Util3dFiltering, passThroughFiltersCorrectZRange)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
// Create 10 points along z-axis from 0.0 to 9.0
for (int i = 0; i < 10; ++i)
cloud->points.emplace_back(0.0f, 0.0f, static_cast<float>(i));
cloud->width = 10;
cloud->height = 1;
pcl::IndicesPtr indices = nullptr; // Use full cloud
float min = 3.0f;
float max = 6.0f;
bool negative = false;
// Run filter
pcl::IndicesPtr output = util3d::passThrough(cloud, indices, "z", min, max, negative);
// Should include points with z = 3, 4, 5, 6
ASSERT_EQ(output->size(), 4);
for (int idx : *output) {
float z = cloud->points[idx].z;
EXPECT_GE(z, min);
EXPECT_LE(z, max);
}
}
TEST(Util3dFiltering, passThroughFiltersOutsideZRangeWithNegative)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
for (int i = 0; i < 10; ++i)
cloud->points.emplace_back(0.0f, 0.0f, static_cast<float>(i));
pcl::IndicesPtr indices = nullptr;
float min = 3.0f;
float max = 6.0f;
bool negative = true;
pcl::IndicesPtr output = util3d::passThrough(cloud, indices, "z", min, max, negative);
// Should include all except z = 3,4,5,6 => 6 points
ASSERT_EQ(output->size(), 6);
for (int idx : *output) {
float z = cloud->points[idx].z;
EXPECT_TRUE(z < min || z > max);
}
}
TEST(Util3dFiltering, passThroughFiltersWithIndicesSubset)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
for (int i = 0; i < 10; ++i)
cloud->points.emplace_back(0.0f, 0.0f, static_cast<float>(i));
// Only consider even-indexed points
pcl::IndicesPtr indices(new std::vector<int>);
for (int i = 0; i < 10; i += 2)
indices->push_back(i);
float min = 2.0f;
float max = 6.0f;
bool negative = false;
pcl::IndicesPtr output = util3d::passThrough(cloud, indices, "z", min, max, negative);
// From even indices: 2, 4, 6 match => 3 points
ASSERT_EQ(output->size(), 3);
for (int idx : *output) {
float z = cloud->points[idx].z;
EXPECT_TRUE(z == 2.0f || z == 4.0f || z == 6.0f);
}
}
TEST(Util3dFiltering, passThroughInvalidAxisTriggersAssertion)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
cloud->push_back(pcl::PointXYZ(0.0f, 0.0f, 1.0f));
pcl::IndicesPtr indices = nullptr;
EXPECT_THROW(util3d::passThrough(cloud, indices, "invalid_axis", 0.0f, 1.0f, false), UException);
}
TEST(Util3dFiltering, passThroughInvalidRangeTriggersAssertion)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
cloud->push_back(pcl::PointXYZ(0.0f, 0.0f, 1.0f));
pcl::IndicesPtr indices = nullptr;
EXPECT_THROW(util3d::passThrough(cloud, indices, "z", 5.0f, 2.0f, false), UException);
}
TEST(Util3dFiltering, cropBoxIncludesPointsInBox)
{
using PointT = pcl::PointXYZ;
pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
// Points along x-axis from 0 to 9
for (int i = 0; i < 10; ++i)
cloud->points.emplace_back(static_cast<float>(i), 0.0f, 0.0f);
Eigen::Vector4f min(3.0f, -1.0f, -1.0f, 1.0f);
Eigen::Vector4f max(6.0f, 1.0f, 1.0f, 1.0f);
pcl::IndicesPtr indices = nullptr;
Transform transform = Transform::getIdentity();
bool negative = false;
// Point included in the box
auto output = util3d::cropBox(cloud, indices, min, max, transform, negative);
ASSERT_EQ(output->size(), 4);
for (int idx : *output) {
float x = cloud->points[idx].x;
EXPECT_GE(x, 3.0f);
EXPECT_LE(x, 6.0f);
}
// Point excluded from the box
negative = true;
output = util3d::cropBox(cloud, indices, min, max, transform, negative);
ASSERT_EQ(output->size(), 6);
for (int idx : *output) {
float x = cloud->points[idx].x;
EXPECT_TRUE(x < 3.0f || x > 6.0f);
}
// Apply translation
transform = Transform(3.0f, 0.0f, 0.0f, 0,0,0); // Shift box forward by 3
negative = false;
output = util3d::cropBox(cloud, indices, min, max, transform, negative);
// Transformed box covers x = [6, 9]
ASSERT_EQ(output->size(), 4);
for (int idx : *output) {
float x = cloud->points[idx].x;
EXPECT_GE(x, 6.0f);
EXPECT_LE(x, 9.0f);
}
}
TEST(Util3dFiltering, cropBoxInvalidBoundsTriggerAssertion)
{
using PointT = pcl::PointXYZ;
pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
cloud->points.emplace_back(0.0f, 0.0f, 0.0f);
pcl::IndicesPtr indices = nullptr;
Eigen::Vector4f min(5.0f, 0.0f, 0.0f, 1.0f);
Eigen::Vector4f max(1.0f, 1.0f, 1.0f, 1.0f); // Invalid: min[0] > max[0]
Transform transform = Transform::getIdentity();
EXPECT_THROW(util3d::cropBox(cloud, indices, min, max, transform, false), UException);
}
// Main test: filtering points inside a frustum
TEST(Util3dFiltering, frustumFilteringIncludesPointsInFrustum)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
// Add points along the X axis (frustum forward direction)
pcl::IndicesPtr indicesOnXAxisOnly(new std::vector<int>{0});
pcl::IndicesPtr indicesOnYAxisOnly(new std::vector<int>);
pcl::IndicesPtr indicesOnZAxisOnly(new std::vector<int>);
pcl::IndicesPtr allIndices(new std::vector<int>{0});
for (int i = 0; i <= 10; ++i) {
cloud->points.emplace_back(static_cast<float>(i), 0.0f, 0.0f); // x goes from 0 to 10
if(i>0) {
indicesOnXAxisOnly->emplace_back(3*(i-1)+1);
allIndices->emplace_back(3*(i-1)+1);
cloud->points.emplace_back(2.0f, static_cast<float>(i)/2.0f, 0.0f); // y goes from 1 to 10 at x=2
indicesOnYAxisOnly->emplace_back(3*(i-1)+2);
allIndices->emplace_back(3*(i-1)+2);
cloud->points.emplace_back(2.0f, 0.0f, static_cast<float>(i)/2.0f); // z goes from 1 to 10 at x=2
indicesOnZAxisOnly->emplace_back(3*(i-1)+3);
allIndices->emplace_back(3*(i-1)+3);
}
}
float hFOV = 90.0f; // wide field of view
float vFOV = 70.0f;
float nearClip = 1.5f;
float farClip = 7.5f;
Transform cameraPose = Transform(-1,0,0,0,0,0) * CameraModel::opticalRotation(); // camera looking forward on x-axis, 1 meter back
pcl::IndicesPtr result = util3d::frustumFiltering(
cloud, nullptr, cameraPose, hFOV, vFOV, nearClip, farClip, false);
float halfhFOV = tan(hFOV*M_PI/180.0f/2.0f)*3; // at 3 meters from the camera
float halfvFOV = tan(vFOV*M_PI/180.0f/2.0f)*3; // at 3 meters from the camera
// Should include points with x in [1,7]
ASSERT_EQ(result->size(), 16);
for (int idx : *result) {
float x = cloud->points[idx].x;
float y = cloud->points[idx].y;
float z = cloud->points[idx].z;
EXPECT_GE(x, nearClip-1.0f);
EXPECT_LE(x, farClip-1.0f);
EXPECT_GE(y, -halfhFOV);
EXPECT_LE(y, halfhFOV);
EXPECT_GE(z, -halfvFOV);
EXPECT_LE(z, halfvFOV);
}
// Check with all indices, should give same result
result = util3d::frustumFiltering(
cloud, allIndices, cameraPose, hFOV, vFOV, nearClip, farClip, false);
// Should include points with x in [1,7]
ASSERT_EQ(result->size(), 16);
for (int idx : *result) {
float x = cloud->points[idx].x;
float y = cloud->points[idx].y;
float z = cloud->points[idx].z;
EXPECT_GE(x, nearClip-1.0f);
EXPECT_LE(x, farClip-1.0f);
EXPECT_GE(y, -halfhFOV);
EXPECT_LE(y, halfhFOV);
EXPECT_GE(z, -halfvFOV);
EXPECT_LE(z, halfvFOV);
}
// test negative flag
result = util3d::frustumFiltering(
cloud, nullptr, cameraPose, hFOV, vFOV, nearClip, farClip, true);
ASSERT_EQ(result->size(), 15);
for (int idx : *result) {
float x = cloud->points[idx].x;
float y = cloud->points[idx].y;
float z = cloud->points[idx].z;
EXPECT_TRUE(x < nearClip-1.0f || x > farClip-1.0f || y < -halfhFOV || y > halfhFOV || z < -halfvFOV || z > halfvFOV);
}
// test with sub-indices
result = util3d::frustumFiltering(
cloud, indicesOnXAxisOnly, cameraPose, hFOV, vFOV, nearClip, farClip, false);
// Should return indices in [1,4,7,10,13,16] (1m, 2m, 3m, 4m, 5m, 6m, 7m) from the indicesOnXAxisOnly list
std::vector<int> expected = {1,4,7,10,13,16};
ASSERT_EQ(result->size(), expected.size());
for (int idx : *result) {
EXPECT_NE(std::find(expected.begin(), expected.end(), idx), expected.end());
}
// Check pitch camera rotation
// Should return indices in [9,12,15,18,21,24] z=(1.5m, 2m, 2.5m, 3m, 3.5m, 4m) from the indicesOnXAxisOnly list
result = util3d::frustumFiltering(
cloud, indicesOnZAxisOnly, Transform(2,0,5,0,M_PI/2,0)*CameraModel::opticalRotation(), 1, 1, 0.75, 3.75, false);
std::vector<int> expectedZ = {9,12,15,18,21,24};
ASSERT_EQ(result->size(), expectedZ.size());
for (int idx : *result) {
EXPECT_NE(std::find(expectedZ.begin(), expectedZ.end(), idx), expectedZ.end());
}
// Check yaw camera rotation
// Should return indices in [8,11,14,17,20,23] y=(1.5m, 2m, 2.5m, 3m, 3.5m, 4m) from the indicesOnYAxisOnly list
result = util3d::frustumFiltering(
cloud, indicesOnYAxisOnly, Transform(2,5,0,0,0,-M_PI/2)*CameraModel::opticalRotation(), 1, 1, 0.75, 3.75, false);
std::vector<int> expectedY = {8,11,14,17,20,23};
ASSERT_EQ(result->size(), expectedY.size());
for (int idx : *result) {
EXPECT_NE(std::find(expectedY.begin(), expectedY.end(), idx), expectedY.end());
}
}
// Test assertion failure on invalid FOV
TEST(Util3dFiltering, frustumFilteringInvalidFOVTriggersAssertion)
{
using PointT = pcl::PointXYZ;
pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
cloud->push_back(PointT(0, 0, 0));
pcl::IndicesPtr indices = nullptr;
Transform cameraPose = Transform::getIdentity();
EXPECT_THROW(util3d::frustumFiltering(cloud, indices, cameraPose, 0.0f, 45.0f, 1.0f, 5.0f, false), UException);
EXPECT_THROW(util3d::frustumFiltering(cloud, indices, cameraPose, 45.0f, 0.0f, 1.0f, 5.0f, false), UException);
}
// Test assertion failure on invalid clip plane distances
TEST(Util3dFiltering, frustumFilteringInvalidClipPlaneTriggersAssertion)
{
using PointT = pcl::PointXYZ;
pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
cloud->push_back(PointT(0, 0, 0));
pcl::IndicesPtr indices = nullptr;
Transform cameraPose = Transform::getIdentity();
EXPECT_THROW(util3d::frustumFiltering(cloud, indices, cameraPose, 60.0f, 45.0f, 5.0f, 1.0f, false), UException);
}