Point Cloud Library (PCL)  1.12.1-dev
frustum_culling.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2012-, Open Perception, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the copyright holder(s) nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  */
37 
38 #ifndef PCL_FILTERS_IMPL_FRUSTUM_CULLING_HPP_
39 #define PCL_FILTERS_IMPL_FRUSTUM_CULLING_HPP_
40 
41 #include <pcl/filters/frustum_culling.h>
42 #include <vector>
43 
44 ///////////////////////////////////////////////////////////////////////////////
45 template <typename PointT> void
47 {
48  Eigen::Vector4f pl_n; // near plane
49  Eigen::Vector4f pl_f; // far plane
50  Eigen::Vector4f pl_t; // top plane
51  Eigen::Vector4f pl_b; // bottom plane
52  Eigen::Vector4f pl_r; // right plane
53  Eigen::Vector4f pl_l; // left plane
54 
55  Eigen::Vector3f view = camera_pose_.block<3, 1> (0, 0); // view vector for the camera - first column of the rotation matrix
56  Eigen::Vector3f up = camera_pose_.block<3, 1> (0, 1); // up vector for the camera - second column of the rotation matrix
57  Eigen::Vector3f right = camera_pose_.block<3, 1> (0, 2); // right vector for the camera - third column of the rotation matrix
58  Eigen::Vector3f T = camera_pose_.block<3, 1> (0, 3); // The (X, Y, Z) position of the camera w.r.t origin
59 
60 
61  float vfov_rad = float (vfov_ * M_PI / 180); // degrees to radians
62  float hfov_rad = float (hfov_ * M_PI / 180); // degrees to radians
63 
64  float roi_xmax = roi_x_ + (roi_w_ / 2); // roi max x
65  float roi_xmin = roi_x_ - (roi_w_ / 2); // roi min x
66  float roi_ymax = roi_y_ + (roi_h_ / 2); // roi max y
67  float roi_ymin = roi_y_ - (roi_h_ / 2); // roi min y
68 
69  float np_h_u = float(2 * tan(vfov_rad / 2) * np_dist_ * (roi_ymin - 0.5) * (-1)); // near plane upper height
70  float np_h_d = float(2 * tan(vfov_rad / 2) * np_dist_ * (roi_ymax - 0.5)); // near plane lower height
71  float np_w_l = float(2 * tan(hfov_rad / 2) * np_dist_ * (roi_xmin - 0.5) * (-1)); // near plane left width
72  float np_w_r = float(2 * tan(hfov_rad / 2) * np_dist_ * (roi_xmax - 0.5)); // near plane right width
73 
74  float fp_h_u = float(2 * tan(vfov_rad / 2) * fp_dist_ * (roi_ymin - 0.5) * (-1)); // far plane upper height
75  float fp_h_d = float(2 * tan(vfov_rad / 2) * fp_dist_ * (roi_ymax - 0.5)); // far plane lower height
76  float fp_w_l = float(2 * tan(hfov_rad / 2) * fp_dist_ * (roi_xmin - 0.5) * (-1)); // far plane left width
77  float fp_w_r = float(2 * tan(hfov_rad / 2) * fp_dist_ * (roi_xmax - 0.5)); // far plane right width
78 
79  Eigen::Vector3f fp_c (T + view * fp_dist_); // far plane center
80  Eigen::Vector3f fp_tl (fp_c + (up * fp_h_u) - (right * fp_w_l)); // Top left corner of the far plane
81  Eigen::Vector3f fp_tr (fp_c + (up * fp_h_u) + (right * fp_w_r)); // Top right corner of the far plane
82  Eigen::Vector3f fp_bl (fp_c - (up * fp_h_d) - (right * fp_w_l)); // Bottom left corner of the far plane
83  Eigen::Vector3f fp_br (fp_c - (up * fp_h_d) + (right * fp_w_r)); // Bottom right corner of the far plane
84 
85  Eigen::Vector3f np_c (T + view * np_dist_); // near plane center
86  //Eigen::Vector3f np_tl = np_c + (up * np_h_u) - (right * np_w_l); // Top left corner of the near plane
87  Eigen::Vector3f np_tr (np_c + (up * np_h_u) + (right * np_w_r)); // Top right corner of the near plane
88  Eigen::Vector3f np_bl (np_c - (up * np_h_d) - (right * np_w_l)); // Bottom left corner of the near plane
89  Eigen::Vector3f np_br (np_c - (up * np_h_d) + (right * np_w_r)); // Bottom right corner of the near plane
90 
91  pl_f.head<3> () = (fp_bl - fp_br).cross (fp_tr - fp_br); // Far plane equation - cross product of the
92  pl_f (3) = -fp_c.dot (pl_f.head<3> ()); // perpendicular edges of the far plane
93 
94  pl_n.head<3> () = (np_tr - np_br).cross (np_bl - np_br); // Near plane equation - cross product of the
95  pl_n (3) = -np_c.dot (pl_n.head<3> ()); // perpendicular edges of the far plane
96 
97  Eigen::Vector3f a (fp_bl - T); // Vector connecting the camera and far plane bottom left
98  Eigen::Vector3f b (fp_br - T); // Vector connecting the camera and far plane bottom right
99  Eigen::Vector3f c (fp_tr - T); // Vector connecting the camera and far plane top right
100  Eigen::Vector3f d (fp_tl - T); // Vector connecting the camera and far plane top left
101 
102  // Frustum and the vectors a, b, c and d. T is the position of the camera
103  // _________
104  // /| . |
105  // d / | c . |
106  // / | __._____|
107  // / / . .
108  // a <---/-/ . .
109  // / / . . b
110  // / .
111  // .
112  // T
113  //
114 
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);
119 
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> ());
124 
125  if (extract_removed_indices_)
126  {
127  removed_indices_->resize (indices_->size ());
128  }
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++)
133  {
134  int idx = indices_->at (i);
135  Eigen::Vector4f pt ((*input_)[idx].x,
136  (*input_)[idx].y,
137  (*input_)[idx].z,
138  1.0f);
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_)
146  {
147  indices[indices_ctr++] = idx;
148  }
149  else if (extract_removed_indices_)
150  {
151  (*removed_indices_)[removed_ctr++] = idx;
152  }
153  }
154  indices.resize (indices_ctr);
155  removed_indices_->resize (removed_ctr);
156 }
157 
158 #define PCL_INSTANTIATE_FrustumCulling(T) template class PCL_EXPORTS pcl::FrustumCulling<T>;
159 
160 #endif
void applyFilter(Indices &indices) override
Sample of point indices.
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
#define M_PI
Definition: pcl_macros.h:201