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_HPP_ 00038 #define PCL_RECOGNITION_OCCLUSION_REASONING_HPP_ 00039 00040 #include <pcl/recognition/hv/occlusion_reasoning.h> 00041 00042 /////////////////////////////////////////////////////////////////////////////////////////// 00043 template<typename ModelT, typename SceneT> 00044 pcl::occlusion_reasoning::ZBuffering<ModelT, SceneT>::ZBuffering (int resx, int resy, float f) : 00045 f_ (f), cx_ (resx), cy_ (resy), depth_ (NULL) 00046 { 00047 } 00048 00049 /////////////////////////////////////////////////////////////////////////////////////////// 00050 template<typename ModelT, typename SceneT> 00051 pcl::occlusion_reasoning::ZBuffering<ModelT, SceneT>::ZBuffering () : 00052 f_ (), cx_ (), cy_ (), depth_ (NULL) 00053 { 00054 } 00055 00056 /////////////////////////////////////////////////////////////////////////////////////////// 00057 template<typename ModelT, typename SceneT> 00058 pcl::occlusion_reasoning::ZBuffering<ModelT, SceneT>::~ZBuffering () 00059 { 00060 if (depth_ != NULL) 00061 delete[] depth_; 00062 } 00063 00064 /////////////////////////////////////////////////////////////////////////////////////////// 00065 template<typename ModelT, typename SceneT> void 00066 pcl::occlusion_reasoning::ZBuffering<ModelT, SceneT>::filter (typename pcl::PointCloud<ModelT>::ConstPtr & model, 00067 typename pcl::PointCloud<ModelT>::Ptr & filtered, float thres) 00068 { 00069 std::vector<int> indices_to_keep; 00070 filter(model, indices_to_keep, thres); 00071 pcl::copyPointCloud (*model, indices_to_keep, *filtered); 00072 } 00073 00074 /////////////////////////////////////////////////////////////////////////////////////////// 00075 template<typename ModelT, typename SceneT> void 00076 pcl::occlusion_reasoning::ZBuffering<ModelT, SceneT>::filter (typename pcl::PointCloud<ModelT>::ConstPtr & model, 00077 std::vector<int> & indices_to_keep, float thres) 00078 { 00079 00080 float cx, cy; 00081 cx = static_cast<float> (cx_) / 2.f - 0.5f; 00082 cy = static_cast<float> (cy_) / 2.f - 0.5f; 00083 00084 indices_to_keep.resize (model->points.size ()); 00085 int keep = 0; 00086 for (size_t i = 0; i < model->points.size (); i++) 00087 { 00088 float x = model->points[i].x; 00089 float y = model->points[i].y; 00090 float z = model->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 if (u >= cx_ || v >= cy_ || u < 0 || v < 0) 00095 continue; 00096 00097 //Check if point depth (distance to camera) is greater than the (u,v) meaning that the point is not visible 00098 if ((z - thres) > depth_[u * cy_ + v] || !pcl_isfinite(depth_[u * cy_ + v])) 00099 continue; 00100 00101 indices_to_keep[keep] = static_cast<int> (i); 00102 keep++; 00103 } 00104 00105 indices_to_keep.resize (keep); 00106 } 00107 00108 /////////////////////////////////////////////////////////////////////////////////////////// 00109 template<typename ModelT, typename SceneT> void 00110 pcl::occlusion_reasoning::ZBuffering<ModelT, SceneT>::computeDepthMap (typename pcl::PointCloud<SceneT>::ConstPtr & scene, bool compute_focal, 00111 bool smooth, int wsize) 00112 { 00113 float cx, cy; 00114 cx = static_cast<float> (cx_) / 2.f - 0.5f; 00115 cy = static_cast<float> (cy_) / 2.f - 0.5f; 00116 00117 //compute the focal length 00118 if (compute_focal) 00119 { 00120 00121 float max_u, max_v, min_u, min_v; 00122 max_u = max_v = std::numeric_limits<float>::max () * -1; 00123 min_u = min_v = std::numeric_limits<float>::max (); 00124 00125 for (size_t i = 0; i < scene->points.size (); i++) 00126 { 00127 float b_x = scene->points[i].x / scene->points[i].z; 00128 if (b_x > max_u) 00129 max_u = b_x; 00130 if (b_x < min_u) 00131 min_u = b_x; 00132 00133 float b_y = scene->points[i].y / scene->points[i].z; 00134 if (b_y > max_v) 00135 max_v = b_y; 00136 if (b_y < min_v) 00137 min_v = b_y; 00138 } 00139 00140 float maxC = std::max (std::max (std::abs (max_u), std::abs (max_v)), std::max (std::abs (min_u), std::abs (min_v))); 00141 f_ = (cx) / maxC; 00142 } 00143 00144 depth_ = new float[cx_ * cy_]; 00145 for (int i = 0; i < (cx_ * cy_); i++) 00146 depth_[i] = std::numeric_limits<float>::quiet_NaN (); 00147 00148 for (size_t i = 0; i < scene->points.size (); i++) 00149 { 00150 float x = scene->points[i].x; 00151 float y = scene->points[i].y; 00152 float z = scene->points[i].z; 00153 int u = static_cast<int> (f_ * x / z + cx); 00154 int v = static_cast<int> (f_ * y / z + cy); 00155 00156 if (u >= cx_ || v >= cy_ || u < 0 || v < 0) 00157 continue; 00158 00159 if ((z < depth_[u * cy_ + v]) || (!pcl_isfinite(depth_[u * cy_ + v]))) 00160 depth_[u * cx_ + v] = z; 00161 } 00162 00163 if (smooth) 00164 { 00165 //Dilate and smooth the depth map 00166 int ws = wsize; 00167 int ws2 = int (std::floor (static_cast<float> (ws) / 2.f)); 00168 float * depth_smooth = new float[cx_ * cy_]; 00169 for (int i = 0; i < (cx_ * cy_); i++) 00170 depth_smooth[i] = std::numeric_limits<float>::quiet_NaN (); 00171 00172 for (int u = ws2; u < (cx_ - ws2); u++) 00173 { 00174 for (int v = ws2; v < (cy_ - ws2); v++) 00175 { 00176 float min = std::numeric_limits<float>::max (); 00177 for (int j = (u - ws2); j <= (u + ws2); j++) 00178 { 00179 for (int i = (v - ws2); i <= (v + ws2); i++) 00180 { 00181 if (pcl_isfinite(depth_[j * cx_ + i]) && (depth_[j * cx_ + i] < min)) 00182 { 00183 min = depth_[j * cx_ + i]; 00184 } 00185 } 00186 } 00187 00188 if (min < (std::numeric_limits<float>::max () - 0.1)) 00189 { 00190 depth_smooth[u * cx_ + v] = min; 00191 } 00192 } 00193 } 00194 00195 memcpy (depth_, depth_smooth, sizeof(float) * cx_ * cy_); 00196 delete[] depth_smooth; 00197 } 00198 } 00199 00200 #endif // PCL_RECOGNITION_OCCLUSION_REASONING_HPP_