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) 2010-2011, Willow Garage, 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 Willow Garage, Inc. 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 #ifndef PCL_RECOGNITION_OCCLUSION_REASONING_H_ 00038 #define PCL_RECOGNITION_OCCLUSION_REASONING_H_ 00039 00040 #include <pcl/common/common.h> 00041 #include <pcl/common/transforms.h> 00042 #include <pcl/common/io.h> 00043 00044 namespace pcl 00045 { 00046 00047 namespace occlusion_reasoning 00048 { 00049 /** 00050 * \brief Class to reason about occlusions 00051 * \author Aitor Aldoma 00052 */ 00053 00054 template<typename ModelT, typename SceneT> 00055 class ZBuffering 00056 { 00057 private: 00058 float f_; 00059 int cx_, cy_; 00060 float * depth_; 00061 00062 public: 00063 00064 ZBuffering (); 00065 ZBuffering (int resx, int resy, float f); 00066 ~ZBuffering (); 00067 void 00068 computeDepthMap (typename pcl::PointCloud<SceneT>::ConstPtr & scene, bool compute_focal = false, bool smooth = false, int wsize = 3); 00069 void 00070 filter (typename pcl::PointCloud<ModelT>::ConstPtr & model, typename pcl::PointCloud<ModelT>::Ptr & filtered, float thres = 0.01); 00071 void filter (typename pcl::PointCloud<ModelT>::ConstPtr & model, std::vector<int> & indices, float thres = 0.01); 00072 }; 00073 00074 template<typename ModelT, typename SceneT> typename pcl::PointCloud<ModelT>::Ptr 00075 filter (typename pcl::PointCloud<SceneT>::ConstPtr & organized_cloud, typename pcl::PointCloud<ModelT>::ConstPtr & to_be_filtered, float f, 00076 float threshold) 00077 { 00078 float cx = (static_cast<float> (organized_cloud->width) / 2.f - 0.5f); 00079 float cy = (static_cast<float> (organized_cloud->height) / 2.f - 0.5f); 00080 typename pcl::PointCloud<ModelT>::Ptr filtered (new pcl::PointCloud<ModelT> ()); 00081 00082 std::vector<int> indices_to_keep; 00083 indices_to_keep.resize (to_be_filtered->points.size ()); 00084 00085 int keep = 0; 00086 for (size_t i = 0; i < to_be_filtered->points.size (); i++) 00087 { 00088 float x = to_be_filtered->points[i].x; 00089 float y = to_be_filtered->points[i].y; 00090 float z = to_be_filtered->points[i].z; 00091 int u = static_cast<int> (f * x / z + cx); 00092 int v = static_cast<int> (f * y / z + cy); 00093 00094 //Not out of bounds 00095 if ((u >= static_cast<int> (organized_cloud->width)) || (v >= static_cast<int> (organized_cloud->height)) || (u < 0) || (v < 0)) 00096 continue; 00097 00098 //Check for invalid depth 00099 if (!pcl_isfinite (organized_cloud->at (u, v).x) || !pcl_isfinite (organized_cloud->at (u, v).y) 00100 || !pcl_isfinite (organized_cloud->at (u, v).z)) 00101 continue; 00102 00103 float z_oc = organized_cloud->at (u, v).z; 00104 00105 //Check if point depth (distance to camera) is greater than the (u,v) 00106 if ((z - z_oc) > threshold) 00107 continue; 00108 00109 indices_to_keep[keep] = static_cast<int> (i); 00110 keep++; 00111 } 00112 00113 indices_to_keep.resize (keep); 00114 pcl::copyPointCloud (*to_be_filtered, indices_to_keep, *filtered); 00115 return filtered; 00116 } 00117 00118 template<typename ModelT, typename SceneT> typename pcl::PointCloud<ModelT>::Ptr 00119 filter (typename pcl::PointCloud<SceneT>::Ptr & organized_cloud, typename pcl::PointCloud<ModelT>::Ptr & to_be_filtered, float f, 00120 float threshold, bool check_invalid_depth = true) 00121 { 00122 float cx = (static_cast<float> (organized_cloud->width) / 2.f - 0.5f); 00123 float cy = (static_cast<float> (organized_cloud->height) / 2.f - 0.5f); 00124 typename pcl::PointCloud<ModelT>::Ptr filtered (new pcl::PointCloud<ModelT> ()); 00125 00126 std::vector<int> indices_to_keep; 00127 indices_to_keep.resize (to_be_filtered->points.size ()); 00128 00129 int keep = 0; 00130 for (size_t i = 0; i < to_be_filtered->points.size (); i++) 00131 { 00132 float x = to_be_filtered->points[i].x; 00133 float y = to_be_filtered->points[i].y; 00134 float z = to_be_filtered->points[i].z; 00135 int u = static_cast<int> (f * x / z + cx); 00136 int v = static_cast<int> (f * y / z + cy); 00137 00138 //Not out of bounds 00139 if ((u >= static_cast<int> (organized_cloud->width)) || (v >= static_cast<int> (organized_cloud->height)) || (u < 0) || (v < 0)) 00140 continue; 00141 00142 //Check for invalid depth 00143 if (check_invalid_depth) 00144 { 00145 if (!pcl_isfinite (organized_cloud->at (u, v).x) || !pcl_isfinite (organized_cloud->at (u, v).y) 00146 || !pcl_isfinite (organized_cloud->at (u, v).z)) 00147 continue; 00148 } 00149 00150 float z_oc = organized_cloud->at (u, v).z; 00151 00152 //Check if point depth (distance to camera) is greater than the (u,v) 00153 if ((z - z_oc) > threshold) 00154 continue; 00155 00156 indices_to_keep[keep] = static_cast<int> (i); 00157 keep++; 00158 } 00159 00160 indices_to_keep.resize (keep); 00161 pcl::copyPointCloud (*to_be_filtered, indices_to_keep, *filtered); 00162 return filtered; 00163 } 00164 00165 template<typename ModelT, typename SceneT> typename pcl::PointCloud<ModelT>::Ptr 00166 getOccludedCloud (typename pcl::PointCloud<SceneT>::Ptr & organized_cloud, typename pcl::PointCloud<ModelT>::Ptr & to_be_filtered, float f, 00167 float threshold, bool check_invalid_depth = true) 00168 { 00169 float cx = (static_cast<float> (organized_cloud->width) / 2.f - 0.5f); 00170 float cy = (static_cast<float> (organized_cloud->height) / 2.f - 0.5f); 00171 typename pcl::PointCloud<ModelT>::Ptr filtered (new pcl::PointCloud<ModelT> ()); 00172 00173 std::vector<int> indices_to_keep; 00174 indices_to_keep.resize (to_be_filtered->points.size ()); 00175 00176 int keep = 0; 00177 for (size_t i = 0; i < to_be_filtered->points.size (); i++) 00178 { 00179 float x = to_be_filtered->points[i].x; 00180 float y = to_be_filtered->points[i].y; 00181 float z = to_be_filtered->points[i].z; 00182 int u = static_cast<int> (f * x / z + cx); 00183 int v = static_cast<int> (f * y / z + cy); 00184 00185 //Out of bounds 00186 if ((u >= static_cast<int> (organized_cloud->width)) || (v >= static_cast<int> (organized_cloud->height)) || (u < 0) || (v < 0)) 00187 continue; 00188 00189 //Check for invalid depth 00190 if (check_invalid_depth) 00191 { 00192 if (!pcl_isfinite (organized_cloud->at (u, v).x) || !pcl_isfinite (organized_cloud->at (u, v).y) 00193 || !pcl_isfinite (organized_cloud->at (u, v).z)) 00194 continue; 00195 } 00196 00197 float z_oc = organized_cloud->at (u, v).z; 00198 00199 //Check if point depth (distance to camera) is greater than the (u,v) 00200 if ((z - z_oc) > threshold) 00201 { 00202 indices_to_keep[keep] = static_cast<int> (i); 00203 keep++; 00204 } 00205 } 00206 00207 indices_to_keep.resize (keep); 00208 pcl::copyPointCloud (*to_be_filtered, indices_to_keep, *filtered); 00209 return filtered; 00210 } 00211 } 00212 } 00213 00214 #ifdef PCL_NO_PRECOMPILE 00215 #include <pcl/recognition/impl/hv/occlusion_reasoning.hpp> 00216 #endif 00217 00218 #endif /* PCL_RECOGNITION_OCCLUSION_REASONING_H_ */