i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-04 09:07: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/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;