Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[filters] FrustumCulling: fix for plane corner calculation #5449

Open
wants to merge 2 commits into
base: master
Choose a base branch
from
Open

[filters] FrustumCulling: fix for plane corner calculation #5449

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
3ebf1ba
[fix] 1. np_h_u was connected with roi_ymin
keineahnung2345 Oct 3, 2022
1f7bddf
revert the unnecessary uncommenting
keineahnung2345 Oct 3, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
44 changes: 27 additions & 17 deletions filters/include/pcl/filters/impl/frustum_culling.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -73,27 +73,37 @@ pcl::FrustumCulling<PointT>::applyFilter (Indices &indices)
float roi_ymax = roi_y_ + (roi_h_ / 2); // roi max y
float roi_ymin = roi_y_ - (roi_h_ / 2); // roi min y

float np_h_u = float(2 * std::tan(fov_lower_bound_rad) * np_dist_ * (roi_ymin - 0.5)); // near plane upper height
float np_h_d = float(2 * std::tan(fov_upper_bound_rad) * np_dist_ * (roi_ymax - 0.5)); // near plane lower height
float np_w_l = float(2 * std::tan(fov_left_bound_rad) * np_dist_ * (roi_xmin - 0.5)); // near plane left width
float np_w_r = float(2 * std::tan(fov_right_bound_rad) * np_dist_ * (roi_xmax - 0.5)); // near plane right width

float fp_h_u = float(2 * std::tan(fov_lower_bound_rad) * fp_dist_ * (roi_ymin - 0.5)); // far plane upper height
float fp_h_d = float(2 * std::tan(fov_upper_bound_rad) * fp_dist_ * (roi_ymax - 0.5)); // far plane lower height
float fp_w_l = float(2 * std::tan(fov_left_bound_rad) * fp_dist_ * (roi_xmin - 0.5)); // far plane left width
float fp_w_r = float(2 * std::tan(fov_right_bound_rad) * fp_dist_ * (roi_xmax - 0.5)); // far plane right width
float np_h_u = float(std::tan(fov_upper_bound_rad) * np_dist_); // near plane upper height
float np_h_d = float(std::tan(fov_lower_bound_rad) * np_dist_); // near plane lower height
float np_w_l = float(std::tan(fov_left_bound_rad) * np_dist_); // near plane left width
float np_w_r = float(std::tan(fov_right_bound_rad) * np_dist_); // near plane right width

float np_roi_h_u = np_h_d + (np_h_u - np_h_d) * roi_ymax;
float np_roi_h_d = np_h_d + (np_h_u - np_h_d) * roi_ymin;
float np_roi_w_l = np_w_l + (np_w_r - np_w_l) * roi_xmin;
float np_roi_w_r = np_w_l + (np_w_r - np_w_l) * roi_xmax;

float fp_h_u = float(std::tan(fov_upper_bound_rad) * fp_dist_); // far plane upper height
float fp_h_d = float(std::tan(fov_lower_bound_rad) * fp_dist_); // far plane lower height
float fp_w_l = float(std::tan(fov_left_bound_rad) * fp_dist_); // far plane left width
float fp_w_r = float(std::tan(fov_right_bound_rad) * fp_dist_); // far plane right width

float fp_roi_h_u = fp_h_d + (fp_h_u - fp_h_d) * roi_ymax;
float fp_roi_h_d = fp_h_d + (fp_h_u - fp_h_d) * roi_ymin;
float fp_roi_w_l = fp_w_l + (fp_w_r - fp_w_l) * roi_xmin;
float fp_roi_w_r = fp_w_l + (fp_w_r - fp_w_l) * roi_xmax;

Eigen::Vector3f fp_c (T + view * fp_dist_); // far plane center
Eigen::Vector3f fp_tl (fp_c + (up * fp_h_u) - (right * fp_w_l)); // Top left corner of the far plane
Eigen::Vector3f fp_tr (fp_c + (up * fp_h_u) + (right * fp_w_r)); // Top right corner of the far plane
Eigen::Vector3f fp_bl (fp_c - (up * fp_h_d) - (right * fp_w_l)); // Bottom left corner of the far plane
Eigen::Vector3f fp_br (fp_c - (up * fp_h_d) + (right * fp_w_r)); // Bottom right corner of the far plane
Eigen::Vector3f fp_tl (fp_c + (up * fp_roi_h_u) + (right * fp_roi_w_l)); // Top left corner of the far plane
Eigen::Vector3f fp_tr (fp_c + (up * fp_roi_h_u) + (right * fp_roi_w_r)); // Top right corner of the far plane
Eigen::Vector3f fp_bl (fp_c + (up * fp_roi_h_d) + (right * fp_roi_w_l)); // Bottom left corner of the far plane
Eigen::Vector3f fp_br (fp_c + (up * fp_roi_h_d) + (right * fp_roi_w_r)); // Bottom right corner of the far plane

Eigen::Vector3f np_c (T + view * np_dist_); // near plane center
//Eigen::Vector3f np_tl = np_c + (up * np_h_u) - (right * np_w_l); // Top left corner of the near plane
Eigen::Vector3f np_tr (np_c + (up * np_h_u) + (right * np_w_r)); // Top right corner of the near plane
Eigen::Vector3f np_bl (np_c - (up * np_h_d) - (right * np_w_l)); // Bottom left corner of the near plane
Eigen::Vector3f np_br (np_c - (up * np_h_d) + (right * np_w_r)); // Bottom right corner of the near plane
//Eigen::Vector3f np_tl (np_c + (up * np_roi_h_u) + (right * np_roi_w_l)); // Top left corner of the near plane
Eigen::Vector3f np_tr (np_c + (up * np_roi_h_u) + (right * np_roi_w_r)); // Top right corner of the near plane
Eigen::Vector3f np_bl (np_c + (up * np_roi_h_d) + (right * np_roi_w_l)); // Bottom left corner of the near plane
Eigen::Vector3f np_br (np_c + (up * np_roi_h_d) + (right * np_roi_w_r)); // Bottom right corner of the near plane

pl_f.head<3> () = (fp_bl - fp_br).cross (fp_tr - fp_br); // Far plane equation - cross product of the
pl_f (3) = -fp_c.dot (pl_f.head<3> ()); // perpendicular edges of the far plane
Expand Down
15 changes: 8 additions & 7 deletions test/filters/test_filters.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2058,21 +2058,22 @@ TEST (FrustumCulling, Filters)
fc.setHorizontalFOV (57);
fc.setNearPlaneDistance (0);
fc.setFarPlaneDistance (0.9);
fc.setRegionOfInterest (0.44f, 0.30f, 0.16f, 0.38f);
fc.setRegionOfInterest (0.44f, 0.73f, 0.16f, 0.43f);
fc.setCameraPose (cam2robot);
fc.filter (*output);
// Should extract milk cartoon with 13541 points
EXPECT_EQ (output->size (), 13541);
// Should extract milk cartoon with 14317 points
EXPECT_EQ (output->size (), 14317);
removed = fc.getRemovedIndices ();
EXPECT_EQ (removed->size (), model->size () - output->size ());

// Cut out object based on field of view
fc.setFarPlaneDistance (0.7);
fc.setRegionOfInterest (0.5f, 0.5f, 1.0f, 1.0f); // reset ROI
fc.setVerticalFOV (-22, 6);
fc.setHorizontalFOV (-22.5, -13.5);
fc.setVerticalFOV (-6.0f, 12.0f);
fc.setHorizontalFOV (-22.5f, -13.0f);
fc.filter (*output);
// Should extract "all" laundry detergent with 10689 points
EXPECT_EQ (output->size (), 10689);
// Should extract "all" laundry detergent with 9838 points
EXPECT_EQ (output->size (), 9838);
removed = fc.getRemovedIndices ();
EXPECT_EQ (removed->size (), model->size () - output->size ());
}
Expand Down