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
49 std::vector<int> indices;
52 bool temp = extract_removed_indices_;
53 extract_removed_indices_ =
true;
54 applyFilter (indices);
55 extract_removed_indices_ = temp;
58 for (
size_t rii = 0; rii < removed_indices_->size (); ++rii)
60 PointT &pt_to_remove = output.
at ((*removed_indices_)[rii]);
61 pt_to_remove.x = pt_to_remove.y = pt_to_remove.z = user_filter_value_;
62 if (!pcl_isfinite (user_filter_value_))
69 applyFilter (indices);
75 template <
typename Po
intT>
void
85 Eigen::Vector3f view = camera_pose_.block (0, 0, 3, 1);
86 Eigen::Vector3f up = camera_pose_.block (0, 1, 3, 1);
87 Eigen::Vector3f right = camera_pose_.block (0, 2, 3, 1);
88 Eigen::Vector3f T = camera_pose_.block (0, 3, 3, 1);
91 float vfov_rad = float (vfov_ * M_PI / 180);
92 float hfov_rad = float (hfov_ * M_PI / 180);
94 float np_h = float (2 * tan (vfov_rad / 2) * np_dist_);
95 float np_w = float (2 * tan (hfov_rad / 2) * np_dist_);
97 float fp_h = float (2 * tan (vfov_rad / 2) * fp_dist_);
98 float fp_w = float (2 * tan (hfov_rad / 2) * fp_dist_);
100 Eigen::Vector3f fp_c (T + view * fp_dist_);
101 Eigen::Vector3f fp_tl (fp_c + (up * fp_h / 2) - (right * fp_w / 2));
102 Eigen::Vector3f fp_tr (fp_c + (up * fp_h / 2) + (right * fp_w / 2));
103 Eigen::Vector3f fp_bl (fp_c - (up * fp_h / 2) - (right * fp_w / 2));
104 Eigen::Vector3f fp_br (fp_c - (up * fp_h / 2) + (right * fp_w / 2));
106 Eigen::Vector3f np_c (T + view * np_dist_);
108 Eigen::Vector3f np_tr (np_c + (up * np_h / 2) + (right * np_w / 2));
109 Eigen::Vector3f np_bl (np_c - (up * np_h / 2) - (right * np_w / 2));
110 Eigen::Vector3f np_br (np_c - (up * np_h / 2) + (right * np_w / 2));
112 pl_f.block (0, 0, 3, 1).matrix () = (fp_bl - fp_br).cross (fp_tr - fp_br);
113 pl_f (3) = -fp_c.dot (pl_f.block (0, 0, 3, 1));
115 pl_n.block (0, 0, 3, 1).matrix () = (np_tr - np_br).cross (np_bl - np_br);
116 pl_n (3) = -np_c.dot (pl_n.block (0, 0, 3, 1));
118 Eigen::Vector3f a (fp_bl - T);
119 Eigen::Vector3f b (fp_br - T);
120 Eigen::Vector3f c (fp_tr - T);
121 Eigen::Vector3f d (fp_tl - T);
136 pl_r.block (0, 0, 3, 1).matrix () = b.cross (c);
137 pl_l.block (0, 0, 3, 1).matrix () = d.cross (a);
138 pl_t.block (0, 0, 3, 1).matrix () = c.cross (d);
139 pl_b.block (0, 0, 3, 1).matrix () = a.cross (b);
141 pl_r (3) = -T.dot (pl_r.block (0, 0, 3, 1));
142 pl_l (3) = -T.dot (pl_l.block (0, 0, 3, 1));
143 pl_t (3) = -T.dot (pl_t.block (0, 0, 3, 1));
144 pl_b (3) = -T.dot (pl_b.block (0, 0, 3, 1));
146 if (extract_removed_indices_)
148 removed_indices_->resize (indices_->size ());
150 indices.resize (indices_->size ());
151 size_t indices_ctr = 0;
152 size_t removed_ctr = 0;
153 for (
size_t i = 0; i < indices_->size (); i++)
155 int idx = indices_->at (i);
156 Eigen::Vector4f pt (input_->points[idx].x,
157 input_->points[idx].y,
158 input_->points[idx].z,
160 bool is_in_fov = (pt.dot (pl_l) <= 0) &&
161 (pt.dot (pl_r) <= 0) &&
162 (pt.dot (pl_t) <= 0) &&
163 (pt.dot (pl_b) <= 0) &&
164 (pt.dot (pl_f) <= 0) &&
165 (pt.dot (pl_n) <= 0);
166 if (is_in_fov ^ negative_)
168 indices[indices_ctr++] = idx;
170 else if (extract_removed_indices_)
172 (*removed_indices_)[removed_ctr++] = idx;
175 indices.resize (indices_ctr);
176 removed_indices_->resize (removed_ctr);
179 #define PCL_INSTANTIATE_FrustumCulling(T) template class PCL_EXPORTS pcl::FrustumCulling<T>;
PCL_EXPORTS void copyPointCloud(const pcl::PCLPointCloud2 &cloud_in, const std::vector< int > &indices, pcl::PCLPointCloud2 &cloud_out)
Extract the indices of a given point cloud as a new point cloud.
void applyFilter(PointCloud &output)
Sample of point indices into a separate PointCloud.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
A point structure representing Euclidean xyz coordinates, and the RGB color.
const PointT & at(int column, int row) const
Obtain the point given by the (column, row) coordinates.