38 #ifndef PCL_FILTERS_IMPL_FRUSTUM_CULLING_HPP_
39 #define PCL_FILTERS_IMPL_FRUSTUM_CULLING_HPP_
41 #include <pcl/filters/frustum_culling.h>
45 template <
typename Po
intT>
void
55 Eigen::Vector3f view = camera_pose_.block<3, 1> (0, 0);
56 Eigen::Vector3f up = camera_pose_.block<3, 1> (0, 1);
57 Eigen::Vector3f right = camera_pose_.block<3, 1> (0, 2);
58 Eigen::Vector3f T = camera_pose_.block<3, 1> (0, 3);
61 float vfov_rad = float (vfov_ *
M_PI / 180);
62 float hfov_rad = float (hfov_ *
M_PI / 180);
64 float roi_xmax = roi_x_ + (roi_w_ / 2);
65 float roi_xmin = roi_x_ - (roi_w_ / 2);
66 float roi_ymax = roi_y_ + (roi_h_ / 2);
67 float roi_ymin = roi_y_ - (roi_h_ / 2);
69 float np_h_u = float(2 * tan(vfov_rad / 2) * np_dist_ * (roi_ymin - 0.5) * (-1));
70 float np_h_d = float(2 * tan(vfov_rad / 2) * np_dist_ * (roi_ymax - 0.5));
71 float np_w_l = float(2 * tan(hfov_rad / 2) * np_dist_ * (roi_xmin - 0.5) * (-1));
72 float np_w_r = float(2 * tan(hfov_rad / 2) * np_dist_ * (roi_xmax - 0.5));
74 float fp_h_u = float(2 * tan(vfov_rad / 2) * fp_dist_ * (roi_ymin - 0.5) * (-1));
75 float fp_h_d = float(2 * tan(vfov_rad / 2) * fp_dist_ * (roi_ymax - 0.5));
76 float fp_w_l = float(2 * tan(hfov_rad / 2) * fp_dist_ * (roi_xmin - 0.5) * (-1));
77 float fp_w_r = float(2 * tan(hfov_rad / 2) * fp_dist_ * (roi_xmax - 0.5));
79 Eigen::Vector3f fp_c (T + view * fp_dist_);
80 Eigen::Vector3f fp_tl (fp_c + (up * fp_h_u) - (right * fp_w_l));
81 Eigen::Vector3f fp_tr (fp_c + (up * fp_h_u) + (right * fp_w_r));
82 Eigen::Vector3f fp_bl (fp_c - (up * fp_h_d) - (right * fp_w_l));
83 Eigen::Vector3f fp_br (fp_c - (up * fp_h_d) + (right * fp_w_r));
85 Eigen::Vector3f np_c (T + view * np_dist_);
87 Eigen::Vector3f np_tr (np_c + (up * np_h_u) + (right * np_w_r));
88 Eigen::Vector3f np_bl (np_c - (up * np_h_d) - (right * np_w_l));
89 Eigen::Vector3f np_br (np_c - (up * np_h_d) + (right * np_w_r));
91 pl_f.head<3> () = (fp_bl - fp_br).cross (fp_tr - fp_br);
92 pl_f (3) = -fp_c.dot (pl_f.head<3> ());
94 pl_n.head<3> () = (np_tr - np_br).cross (np_bl - np_br);
95 pl_n (3) = -np_c.dot (pl_n.head<3> ());
97 Eigen::Vector3f a (fp_bl - T);
98 Eigen::Vector3f b (fp_br - T);
99 Eigen::Vector3f c (fp_tr - T);
100 Eigen::Vector3f d (fp_tl - T);
115 pl_r.head<3> () = b.cross (c);
116 pl_l.head<3> () = d.cross (a);
117 pl_t.head<3> () = c.cross (d);
118 pl_b.head<3> () = a.cross (b);
120 pl_r (3) = -T.dot (pl_r.head<3> ());
121 pl_l (3) = -T.dot (pl_l.head<3> ());
122 pl_t (3) = -T.dot (pl_t.head<3> ());
123 pl_b (3) = -T.dot (pl_b.head<3> ());
125 if (extract_removed_indices_)
127 removed_indices_->resize (indices_->size ());
129 indices.resize (indices_->size ());
130 std::size_t indices_ctr = 0;
131 std::size_t removed_ctr = 0;
132 for (std::size_t i = 0; i < indices_->size (); i++)
134 int idx = indices_->at (i);
135 Eigen::Vector4f pt ((*input_)[idx].x,
139 bool is_in_fov = (pt.dot (pl_l) <= 0) &&
140 (pt.dot (pl_r) <= 0) &&
141 (pt.dot (pl_t) <= 0) &&
142 (pt.dot (pl_b) <= 0) &&
143 (pt.dot (pl_f) <= 0) &&
144 (pt.dot (pl_n) <= 0);
145 if (is_in_fov ^ negative_)
147 indices[indices_ctr++] = idx;
149 else if (extract_removed_indices_)
151 (*removed_indices_)[removed_ctr++] = idx;
154 indices.resize (indices_ctr);
155 removed_indices_->resize (removed_ctr);
158 #define PCL_INSTANTIATE_FrustumCulling(T) template class PCL_EXPORTS pcl::FrustumCulling<T>;
void applyFilter(Indices &indices) override
Sample of point indices.
IndicesAllocator<> Indices
Type used for indices in PCL.