Point Cloud Library (PCL) 1.14.0
Loading...
Searching...
No Matches
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///////////////////////////////////////////////////////////////////////////////
45template <typename PointT> void
47{
48 bool is_far_plane_infinite = (fp_dist_ == std::numeric_limits<float>::max());
49 if(is_far_plane_infinite) {
50 fp_dist_ = np_dist_ + 1.0f;
51 }
52
53 Eigen::Vector4f pl_n; // near plane
54 Eigen::Vector4f pl_f; // far plane
55 Eigen::Vector4f pl_t; // top plane
56 Eigen::Vector4f pl_b; // bottom plane
57 Eigen::Vector4f pl_r; // right plane
58 Eigen::Vector4f pl_l; // left plane
59
60 Eigen::Vector3f view = camera_pose_.block<3, 1> (0, 0); // view vector for the camera - first column of the rotation matrix
61 Eigen::Vector3f up = camera_pose_.block<3, 1> (0, 1); // up vector for the camera - second column of the rotation matrix
62 Eigen::Vector3f right = camera_pose_.block<3, 1> (0, 2); // right vector for the camera - third column of the rotation matrix
63 Eigen::Vector3f T = camera_pose_.block<3, 1> (0, 3); // The (X, Y, Z) position of the camera w.r.t origin
64
65
66 float fov_lower_bound_rad = static_cast<float>(fov_lower_bound_ * M_PI / 180); // degrees to radians
67 float fov_upper_bound_rad = static_cast<float>(fov_upper_bound_ * M_PI / 180); // degrees to radians
68 float fov_left_bound_rad = static_cast<float>(fov_left_bound_ * M_PI / 180); // degrees to radians
69 float fov_right_bound_rad = static_cast<float>(fov_right_bound_ * M_PI / 180); // degrees to radians
70
71 float roi_xmax = roi_x_ + (roi_w_ / 2); // roi max x
72 float roi_xmin = roi_x_ - (roi_w_ / 2); // roi min x
73 float roi_ymax = roi_y_ + (roi_h_ / 2); // roi max y
74 float roi_ymin = roi_y_ - (roi_h_ / 2); // roi min y
75
76 float np_h_u = static_cast<float>(2 * std::tan(fov_lower_bound_rad) * np_dist_ * (roi_ymin - 0.5)); // near plane upper height
77 float np_h_d = static_cast<float>(2 * std::tan(fov_upper_bound_rad) * np_dist_ * (roi_ymax - 0.5)); // near plane lower height
78 float np_w_l = static_cast<float>(2 * std::tan(fov_left_bound_rad) * np_dist_ * (roi_xmin - 0.5)); // near plane left width
79 float np_w_r = static_cast<float>(2 * std::tan(fov_right_bound_rad) * np_dist_ * (roi_xmax - 0.5)); // near plane right width
80
81 float fp_h_u = static_cast<float>(2 * std::tan(fov_lower_bound_rad) * fp_dist_ * (roi_ymin - 0.5)); // far plane upper height
82 float fp_h_d = static_cast<float>(2 * std::tan(fov_upper_bound_rad) * fp_dist_ * (roi_ymax - 0.5)); // far plane lower height
83 float fp_w_l = static_cast<float>(2 * std::tan(fov_left_bound_rad) * fp_dist_ * (roi_xmin - 0.5)); // far plane left width
84 float fp_w_r = static_cast<float>(2 * std::tan(fov_right_bound_rad) * fp_dist_ * (roi_xmax - 0.5)); // far plane right width
85
86 Eigen::Vector3f fp_c (T + view * fp_dist_); // far plane center
87 Eigen::Vector3f fp_tl (fp_c + (up * fp_h_u) - (right * fp_w_l)); // Top left corner of the far plane
88 Eigen::Vector3f fp_tr (fp_c + (up * fp_h_u) + (right * fp_w_r)); // Top right corner of the far plane
89 Eigen::Vector3f fp_bl (fp_c - (up * fp_h_d) - (right * fp_w_l)); // Bottom left corner of the far plane
90 Eigen::Vector3f fp_br (fp_c - (up * fp_h_d) + (right * fp_w_r)); // Bottom right corner of the far plane
91
92 Eigen::Vector3f np_c (T + view * np_dist_); // near plane center
93 //Eigen::Vector3f np_tl = np_c + (up * np_h_u) - (right * np_w_l); // Top left corner of the near plane
94 Eigen::Vector3f np_tr (np_c + (up * np_h_u) + (right * np_w_r)); // Top right corner of the near plane
95 Eigen::Vector3f np_bl (np_c - (up * np_h_d) - (right * np_w_l)); // Bottom left corner of the near plane
96 Eigen::Vector3f np_br (np_c - (up * np_h_d) + (right * np_w_r)); // Bottom right corner of the near plane
97
98 pl_f.head<3> () = (fp_bl - fp_br).cross (fp_tr - fp_br); // Far plane equation - cross product of the
99 pl_f (3) = -fp_c.dot (pl_f.head<3> ()); // perpendicular edges of the far plane
100
101 if(is_far_plane_infinite) {
102 pl_f.setZero();
103 fp_dist_ = std::numeric_limits<float>::max();
104 }
105
106 pl_n.head<3> () = (np_tr - np_br).cross (np_bl - np_br); // Near plane equation - cross product of the
107 pl_n (3) = -np_c.dot (pl_n.head<3> ()); // perpendicular edges of the near plane
108
109 Eigen::Vector3f a (fp_bl - T); // Vector connecting the camera and far plane bottom left
110 Eigen::Vector3f b (fp_br - T); // Vector connecting the camera and far plane bottom right
111 Eigen::Vector3f c (fp_tr - T); // Vector connecting the camera and far plane top right
112 Eigen::Vector3f d (fp_tl - T); // Vector connecting the camera and far plane top left
113
114 // Frustum and the vectors a, b, c and d. T is the position of the camera
115 // _________
116 // /| . |
117 // d / | c . |
118 // / | __._____|
119 // / / . .
120 // a <---/-/ . .
121 // / / . . b
122 // / .
123 // .
124 // T
125 //
126
127 pl_r.head<3> () = b.cross (c);
128 pl_l.head<3> () = d.cross (a);
129 pl_t.head<3> () = c.cross (d);
130 pl_b.head<3> () = a.cross (b);
131
132 pl_r (3) = -T.dot (pl_r.head<3> ());
133 pl_l (3) = -T.dot (pl_l.head<3> ());
134 pl_t (3) = -T.dot (pl_t.head<3> ());
135 pl_b (3) = -T.dot (pl_b.head<3> ());
136
137 if (extract_removed_indices_)
138 {
139 removed_indices_->resize (indices_->size ());
140 }
141 indices.resize (indices_->size ());
142 std::size_t indices_ctr = 0;
143 std::size_t removed_ctr = 0;
144 for (std::size_t i = 0; i < indices_->size (); i++)
145 {
146 int idx = indices_->at (i);
147 Eigen::Vector4f pt ((*input_)[idx].x,
148 (*input_)[idx].y,
149 (*input_)[idx].z,
150 1.0f);
151 bool is_in_fov = (pt.dot (pl_l) <= 0) &&
152 (pt.dot (pl_r) <= 0) &&
153 (pt.dot (pl_t) <= 0) &&
154 (pt.dot (pl_b) <= 0) &&
155 (pt.dot (pl_f) <= 0) &&
156 (pt.dot (pl_n) <= 0);
157 if (is_in_fov ^ negative_)
158 {
159 indices[indices_ctr++] = idx;
160 }
161 else if (extract_removed_indices_)
162 {
163 (*removed_indices_)[removed_ctr++] = idx;
164 }
165 }
166 indices.resize (indices_ctr);
167 removed_indices_->resize (removed_ctr);
168}
169
170#define PCL_INSTANTIATE_FrustumCulling(T) template class PCL_EXPORTS pcl::FrustumCulling<T>;
171
172#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