Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2012-, Open Perception, Inc. 00006 * 00007 * All rights reserved. 00008 * 00009 * Redistribution and use in source and binary forms, with or without 00010 * modification, are permitted provided that the following conditions 00011 * are met: 00012 * 00013 * * Redistributions of source code must retain the above copyright 00014 * notice, this list of conditions and the following disclaimer. 00015 * * Redistributions in binary form must reproduce the above 00016 * copyright notice, this list of conditions and the following 00017 * disclaimer in the documentation and/or other materials provided 00018 * with the distribution. 00019 * * Neither the name of the copyright holder(s) nor the names of its 00020 * contributors may be used to endorse or promote products derived 00021 * from this software without specific prior written permission. 00022 * 00023 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00024 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00025 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00026 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00027 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00028 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00029 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00030 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00031 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00032 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00033 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00034 * POSSIBILITY OF SUCH DAMAGE. 00035 * 00036 */ 00037 00038 #ifndef PCL_FILTERS_IMPL_FRUSTUM_CULLING_HPP_ 00039 #define PCL_FILTERS_IMPL_FRUSTUM_CULLING_HPP_ 00040 00041 #include <pcl/filters/frustum_culling.h> 00042 #include <pcl/common/io.h> 00043 #include <vector> 00044 00045 /////////////////////////////////////////////////////////////////////////////// 00046 template <typename PointT> void 00047 pcl::FrustumCulling<PointT>::applyFilter (PointCloud& output) 00048 { 00049 std::vector<int> indices; 00050 if (keep_organized_) 00051 { 00052 bool temp = extract_removed_indices_; 00053 extract_removed_indices_ = true; 00054 applyFilter (indices); 00055 extract_removed_indices_ = temp; 00056 copyPointCloud (*input_, output); 00057 00058 for (size_t rii = 0; rii < removed_indices_->size (); ++rii) 00059 { 00060 PointT &pt_to_remove = output.at ((*removed_indices_)[rii]); 00061 pt_to_remove.x = pt_to_remove.y = pt_to_remove.z = user_filter_value_; 00062 if (!pcl_isfinite (user_filter_value_)) 00063 output.is_dense = false; 00064 } 00065 } 00066 else 00067 { 00068 output.is_dense = true; 00069 applyFilter (indices); 00070 copyPointCloud (*input_, indices, output); 00071 } 00072 } 00073 00074 /////////////////////////////////////////////////////////////////////////////// 00075 template <typename PointT> void 00076 pcl::FrustumCulling<PointT>::applyFilter (std::vector<int> &indices) 00077 { 00078 Eigen::Vector4f pl_n; // near plane 00079 Eigen::Vector4f pl_f; // far plane 00080 Eigen::Vector4f pl_t; // top plane 00081 Eigen::Vector4f pl_b; // bottom plane 00082 Eigen::Vector4f pl_r; // right plane 00083 Eigen::Vector4f pl_l; // left plane 00084 00085 Eigen::Vector3f view = camera_pose_.block (0, 0, 3, 1); // view vector for the camera - first column of the rotation matrix 00086 Eigen::Vector3f up = camera_pose_.block (0, 1, 3, 1); // up vector for the camera - second column of the rotation matix 00087 Eigen::Vector3f right = camera_pose_.block (0, 2, 3, 1); // right vector for the camera - third column of the rotation matrix 00088 Eigen::Vector3f T = camera_pose_.block (0, 3, 3, 1); // The (X, Y, Z) position of the camera w.r.t origin 00089 00090 00091 float vfov_rad = float (vfov_ * M_PI / 180); // degrees to radians 00092 float hfov_rad = float (hfov_ * M_PI / 180); // degrees to radians 00093 00094 float np_h = float (2 * tan (vfov_rad / 2) * np_dist_); // near plane height 00095 float np_w = float (2 * tan (hfov_rad / 2) * np_dist_); // near plane width 00096 00097 float fp_h = float (2 * tan (vfov_rad / 2) * fp_dist_); // far plane height 00098 float fp_w = float (2 * tan (hfov_rad / 2) * fp_dist_); // far plane width 00099 00100 Eigen::Vector3f fp_c (T + view * fp_dist_); // far plane center 00101 Eigen::Vector3f fp_tl (fp_c + (up * fp_h / 2) - (right * fp_w / 2)); // Top left corner of the far plane 00102 Eigen::Vector3f fp_tr (fp_c + (up * fp_h / 2) + (right * fp_w / 2)); // Top right corner of the far plane 00103 Eigen::Vector3f fp_bl (fp_c - (up * fp_h / 2) - (right * fp_w / 2)); // Bottom left corner of the far plane 00104 Eigen::Vector3f fp_br (fp_c - (up * fp_h / 2) + (right * fp_w / 2)); // Bottom right corner of the far plane 00105 00106 Eigen::Vector3f np_c (T + view * np_dist_); // near plane center 00107 //Eigen::Vector3f np_tl = np_c + (up * np_h/2) - (right * np_w/2); // Top left corner of the near plane 00108 Eigen::Vector3f np_tr (np_c + (up * np_h / 2) + (right * np_w / 2)); // Top right corner of the near plane 00109 Eigen::Vector3f np_bl (np_c - (up * np_h / 2) - (right * np_w / 2)); // Bottom left corner of the near plane 00110 Eigen::Vector3f np_br (np_c - (up * np_h / 2) + (right * np_w / 2)); // Bottom right corner of the near plane 00111 00112 pl_f.block (0, 0, 3, 1).matrix () = (fp_bl - fp_br).cross (fp_tr - fp_br); // Far plane equation - cross product of the 00113 pl_f (3) = -fp_c.dot (pl_f.block (0, 0, 3, 1)); // perpendicular edges of the far plane 00114 00115 pl_n.block (0, 0, 3, 1).matrix () = (np_tr - np_br).cross (np_bl - np_br); // Near plane equation - cross product of the 00116 pl_n (3) = -np_c.dot (pl_n.block (0, 0, 3, 1)); // perpendicular edges of the far plane 00117 00118 Eigen::Vector3f a (fp_bl - T); // Vector connecting the camera and far plane bottom left 00119 Eigen::Vector3f b (fp_br - T); // Vector connecting the camera and far plane bottom right 00120 Eigen::Vector3f c (fp_tr - T); // Vector connecting the camera and far plane top right 00121 Eigen::Vector3f d (fp_tl - T); // Vector connecting the camera and far plane top left 00122 00123 // Frustum and the vectors a, b, c and d. T is the position of the camera 00124 // _________ 00125 // /| . | 00126 // d / | c . | 00127 // / | __._____| 00128 // / / . . 00129 // a <---/-/ . . 00130 // / / . . b 00131 // / . 00132 // . 00133 // T 00134 // 00135 00136 pl_r.block (0, 0, 3, 1).matrix () = b.cross (c); 00137 pl_l.block (0, 0, 3, 1).matrix () = d.cross (a); 00138 pl_t.block (0, 0, 3, 1).matrix () = c.cross (d); 00139 pl_b.block (0, 0, 3, 1).matrix () = a.cross (b); 00140 00141 pl_r (3) = -T.dot (pl_r.block (0, 0, 3, 1)); 00142 pl_l (3) = -T.dot (pl_l.block (0, 0, 3, 1)); 00143 pl_t (3) = -T.dot (pl_t.block (0, 0, 3, 1)); 00144 pl_b (3) = -T.dot (pl_b.block (0, 0, 3, 1)); 00145 00146 if (extract_removed_indices_) 00147 { 00148 removed_indices_->resize (indices_->size ()); 00149 } 00150 indices.resize (indices_->size ()); 00151 size_t indices_ctr = 0; 00152 size_t removed_ctr = 0; 00153 for (size_t i = 0; i < indices_->size (); i++) 00154 { 00155 int idx = indices_->at (i); 00156 Eigen::Vector4f pt (input_->points[idx].x, 00157 input_->points[idx].y, 00158 input_->points[idx].z, 00159 1.0f); 00160 bool is_in_fov = (pt.dot (pl_l) <= 0) && 00161 (pt.dot (pl_r) <= 0) && 00162 (pt.dot (pl_t) <= 0) && 00163 (pt.dot (pl_b) <= 0) && 00164 (pt.dot (pl_f) <= 0) && 00165 (pt.dot (pl_n) <= 0); 00166 if (is_in_fov ^ negative_) 00167 { 00168 indices[indices_ctr++] = idx; 00169 } 00170 else if (extract_removed_indices_) 00171 { 00172 (*removed_indices_)[removed_ctr++] = idx; 00173 } 00174 } 00175 indices.resize (indices_ctr); 00176 removed_indices_->resize (removed_ctr); 00177 } 00178 00179 #define PCL_INSTANTIATE_FrustumCulling(T) template class PCL_EXPORTS pcl::FrustumCulling<T>; 00180 00181 #endif