38 #ifndef PCL_FILTERS_IMPL_FRUSTUM_CULLING_HPP_
39 #define PCL_FILTERS_IMPL_FRUSTUM_CULLING_HPP_
41 #include <pcl/filters/frustum_culling.h>
42 #include <pcl/common/io.h>
46 template <
typename Po
intT>
void
56 Eigen::Vector3f view = camera_pose_.block<3, 1> (0, 0);
57 Eigen::Vector3f up = camera_pose_.block<3, 1> (0, 1);
58 Eigen::Vector3f right = camera_pose_.block<3, 1> (0, 2);
59 Eigen::Vector3f T = camera_pose_.block<3, 1> (0, 3);
62 float vfov_rad = float (vfov_ * M_PI / 180);
63 float hfov_rad = float (hfov_ * M_PI / 180);
65 float np_h = float (2 * tan (vfov_rad / 2) * np_dist_);
66 float np_w = float (2 * tan (hfov_rad / 2) * np_dist_);
68 float fp_h = float (2 * tan (vfov_rad / 2) * fp_dist_);
69 float fp_w = float (2 * tan (hfov_rad / 2) * fp_dist_);
71 Eigen::Vector3f fp_c (T + view * fp_dist_);
72 Eigen::Vector3f fp_tl (fp_c + (up * fp_h / 2) - (right * fp_w / 2));
73 Eigen::Vector3f fp_tr (fp_c + (up * fp_h / 2) + (right * fp_w / 2));
74 Eigen::Vector3f fp_bl (fp_c - (up * fp_h / 2) - (right * fp_w / 2));
75 Eigen::Vector3f fp_br (fp_c - (up * fp_h / 2) + (right * fp_w / 2));
77 Eigen::Vector3f np_c (T + view * np_dist_);
79 Eigen::Vector3f np_tr (np_c + (up * np_h / 2) + (right * np_w / 2));
80 Eigen::Vector3f np_bl (np_c - (up * np_h / 2) - (right * np_w / 2));
81 Eigen::Vector3f np_br (np_c - (up * np_h / 2) + (right * np_w / 2));
83 pl_f.head<3> () = (fp_bl - fp_br).cross (fp_tr - fp_br);
84 pl_f (3) = -fp_c.dot (pl_f.head<3> ());
86 pl_n.head<3> () = (np_tr - np_br).cross (np_bl - np_br);
87 pl_n (3) = -np_c.dot (pl_n.head<3> ());
89 Eigen::Vector3f a (fp_bl - T);
90 Eigen::Vector3f b (fp_br - T);
91 Eigen::Vector3f c (fp_tr - T);
92 Eigen::Vector3f d (fp_tl - T);
107 pl_r.head<3> () = b.cross (c);
108 pl_l.head<3> () = d.cross (a);
109 pl_t.head<3> () = c.cross (d);
110 pl_b.head<3> () = a.cross (b);
112 pl_r (3) = -T.dot (pl_r.head<3> ());
113 pl_l (3) = -T.dot (pl_l.head<3> ());
114 pl_t (3) = -T.dot (pl_t.head<3> ());
115 pl_b (3) = -T.dot (pl_b.head<3> ());
117 if (extract_removed_indices_)
119 removed_indices_->resize (indices_->size ());
121 indices.resize (indices_->size ());
122 std::size_t indices_ctr = 0;
123 std::size_t removed_ctr = 0;
124 for (std::size_t i = 0; i < indices_->size (); i++)
126 int idx = indices_->at (i);
127 Eigen::Vector4f pt (input_->points[idx].x,
128 input_->points[idx].y,
129 input_->points[idx].z,
131 bool is_in_fov = (pt.dot (pl_l) <= 0) &&
132 (pt.dot (pl_r) <= 0) &&
133 (pt.dot (pl_t) <= 0) &&
134 (pt.dot (pl_b) <= 0) &&
135 (pt.dot (pl_f) <= 0) &&
136 (pt.dot (pl_n) <= 0);
137 if (is_in_fov ^ negative_)
139 indices[indices_ctr++] = idx;
141 else if (extract_removed_indices_)
143 (*removed_indices_)[removed_ctr++] = idx;
146 indices.resize (indices_ctr);
147 removed_indices_->resize (removed_ctr);
150 #define PCL_INSTANTIATE_FrustumCulling(T) template class PCL_EXPORTS pcl::FrustumCulling<T>;
void applyFilter(std::vector< int > &indices) override
Sample of point indices.