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 np_h = float (2 * tan (vfov_rad / 2) * np_dist_);
65 float np_w = float (2 * tan (hfov_rad / 2) * np_dist_);
67 float fp_h = float (2 * tan (vfov_rad / 2) * fp_dist_);
68 float fp_w = float (2 * tan (hfov_rad / 2) * fp_dist_);
70 Eigen::Vector3f fp_c (T + view * fp_dist_);
71 Eigen::Vector3f fp_tl (fp_c + (up * fp_h / 2) - (right * fp_w / 2));
72 Eigen::Vector3f fp_tr (fp_c + (up * fp_h / 2) + (right * fp_w / 2));
73 Eigen::Vector3f fp_bl (fp_c - (up * fp_h / 2) - (right * fp_w / 2));
74 Eigen::Vector3f fp_br (fp_c - (up * fp_h / 2) + (right * fp_w / 2));
76 Eigen::Vector3f np_c (T + view * np_dist_);
78 Eigen::Vector3f np_tr (np_c + (up * np_h / 2) + (right * np_w / 2));
79 Eigen::Vector3f np_bl (np_c - (up * np_h / 2) - (right * np_w / 2));
80 Eigen::Vector3f np_br (np_c - (up * np_h / 2) + (right * np_w / 2));
82 pl_f.head<3> () = (fp_bl - fp_br).cross (fp_tr - fp_br);
83 pl_f (3) = -fp_c.dot (pl_f.head<3> ());
85 pl_n.head<3> () = (np_tr - np_br).cross (np_bl - np_br);
86 pl_n (3) = -np_c.dot (pl_n.head<3> ());
88 Eigen::Vector3f a (fp_bl - T);
89 Eigen::Vector3f b (fp_br - T);
90 Eigen::Vector3f c (fp_tr - T);
91 Eigen::Vector3f d (fp_tl - T);
106 pl_r.head<3> () = b.cross (c);
107 pl_l.head<3> () = d.cross (a);
108 pl_t.head<3> () = c.cross (d);
109 pl_b.head<3> () = a.cross (b);
111 pl_r (3) = -T.dot (pl_r.head<3> ());
112 pl_l (3) = -T.dot (pl_l.head<3> ());
113 pl_t (3) = -T.dot (pl_t.head<3> ());
114 pl_b (3) = -T.dot (pl_b.head<3> ());
116 if (extract_removed_indices_)
118 removed_indices_->resize (indices_->size ());
120 indices.resize (indices_->size ());
121 std::size_t indices_ctr = 0;
122 std::size_t removed_ctr = 0;
123 for (std::size_t i = 0; i < indices_->size (); i++)
125 int idx = indices_->at (i);
126 Eigen::Vector4f pt ((*input_)[idx].x,
130 bool is_in_fov = (pt.dot (pl_l) <= 0) &&
131 (pt.dot (pl_r) <= 0) &&
132 (pt.dot (pl_t) <= 0) &&
133 (pt.dot (pl_b) <= 0) &&
134 (pt.dot (pl_f) <= 0) &&
135 (pt.dot (pl_n) <= 0);
136 if (is_in_fov ^ negative_)
138 indices[indices_ctr++] = idx;
140 else if (extract_removed_indices_)
142 (*removed_indices_)[removed_ctr++] = idx;
145 indices.resize (indices_ctr);
146 removed_indices_->resize (removed_ctr);
149 #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.