Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2009, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of Willow Garage, Inc. nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 * 00034 * Author : Christian Potthast 00035 * Email : potthast@usc.edu 00036 * 00037 */ 00038 00039 #ifndef PCL_FILTERS_IMPL_VOXEL_GRID_OCCLUSION_ESTIMATION_H_ 00040 #define PCL_FILTERS_IMPL_VOXEL_GRID_OCCLUSION_ESTIMATION_H_ 00041 00042 #include <pcl/common/common.h> 00043 #include <pcl/filters/voxel_grid_occlusion_estimation.h> 00044 00045 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00046 template <typename PointT> void 00047 pcl::VoxelGridOcclusionEstimation<PointT>::initializeVoxelGrid () 00048 { 00049 // initialization set to true 00050 initialized_ = true; 00051 00052 // create the voxel grid and store the output cloud 00053 this->filter (filtered_cloud_); 00054 00055 // Get the minimum and maximum bounding box dimensions 00056 b_min_[0] = (static_cast<float> ( min_b_[0]) * leaf_size_[0]); 00057 b_min_[1] = (static_cast<float> ( min_b_[1]) * leaf_size_[1]); 00058 b_min_[2] = (static_cast<float> ( min_b_[2]) * leaf_size_[2]); 00059 b_max_[0] = (static_cast<float> ( (max_b_[0]) + 1) * leaf_size_[0]); 00060 b_max_[1] = (static_cast<float> ( (max_b_[1]) + 1) * leaf_size_[1]); 00061 b_max_[2] = (static_cast<float> ( (max_b_[2]) + 1) * leaf_size_[2]); 00062 00063 // set the sensor origin and sensor orientation 00064 sensor_origin_ = filtered_cloud_.sensor_origin_; 00065 sensor_orientation_ = filtered_cloud_.sensor_orientation_; 00066 } 00067 00068 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00069 template <typename PointT> int 00070 pcl::VoxelGridOcclusionEstimation<PointT>::occlusionEstimation (int& out_state, 00071 const Eigen::Vector3i& in_target_voxel) 00072 { 00073 if (!initialized_) 00074 { 00075 PCL_ERROR ("Voxel grid not initialized; call initializeVoxelGrid () first! \n"); 00076 return -1; 00077 } 00078 00079 // estimate direction to target voxel 00080 Eigen::Vector4f p = getCentroidCoordinate (in_target_voxel); 00081 Eigen::Vector4f direction = p - sensor_origin_; 00082 direction.normalize (); 00083 00084 // estimate entry point into the voxel grid 00085 float tmin = rayBoxIntersection (sensor_origin_, direction); 00086 00087 if (tmin == -1) 00088 { 00089 PCL_ERROR ("The ray does not intersect with the bounding box \n"); 00090 return -1; 00091 } 00092 00093 // ray traversal 00094 out_state = rayTraversal (in_target_voxel, sensor_origin_, direction, tmin); 00095 00096 return 0; 00097 } 00098 00099 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00100 template <typename PointT> int 00101 pcl::VoxelGridOcclusionEstimation<PointT>::occlusionEstimation (int& out_state, 00102 std::vector<Eigen::Vector3i>& out_ray, 00103 const Eigen::Vector3i& in_target_voxel) 00104 { 00105 if (!initialized_) 00106 { 00107 PCL_ERROR ("Voxel grid not initialized; call initializeVoxelGrid () first! \n"); 00108 return -1; 00109 } 00110 00111 // estimate direction to target voxel 00112 Eigen::Vector4f p = getCentroidCoordinate (in_target_voxel); 00113 Eigen::Vector4f direction = p - sensor_origin_; 00114 direction.normalize (); 00115 00116 // estimate entry point into the voxel grid 00117 float tmin = rayBoxIntersection (sensor_origin_, direction); 00118 00119 if (tmin == -1) 00120 { 00121 PCL_ERROR ("The ray does not intersect with the bounding box \n"); 00122 return -1; 00123 } 00124 00125 // ray traversal 00126 out_state = rayTraversal (out_ray, in_target_voxel, sensor_origin_, direction, tmin); 00127 00128 return 0; 00129 } 00130 00131 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00132 template <typename PointT> int 00133 pcl::VoxelGridOcclusionEstimation<PointT>::occlusionEstimationAll (std::vector<Eigen::Vector3i>& occluded_voxels) 00134 { 00135 if (!initialized_) 00136 { 00137 PCL_ERROR ("Voxel grid not initialized; call initializeVoxelGrid () first! \n"); 00138 return -1; 00139 } 00140 00141 // reserve space for the ray vector 00142 int reserve_size = div_b_[0] * div_b_[1] * div_b_[2]; 00143 occluded_voxels.reserve (reserve_size); 00144 00145 // iterate over the entire voxel grid 00146 for (int kk = min_b_.z (); kk <= max_b_.z (); ++kk) 00147 for (int jj = min_b_.y (); jj <= max_b_.y (); ++jj) 00148 for (int ii = min_b_.x (); ii <= max_b_.x (); ++ii) 00149 { 00150 Eigen::Vector3i ijk (ii, jj, kk); 00151 // process all free voxels 00152 int index = this->getCentroidIndexAt (ijk); 00153 if (index == -1) 00154 { 00155 // estimate direction to target voxel 00156 Eigen::Vector4f p = getCentroidCoordinate (ijk); 00157 Eigen::Vector4f direction = p - sensor_origin_; 00158 direction.normalize (); 00159 00160 // estimate entry point into the voxel grid 00161 float tmin = rayBoxIntersection (sensor_origin_, direction); 00162 00163 // ray traversal 00164 int state = rayTraversal (ijk, sensor_origin_, direction, tmin); 00165 00166 // if voxel is occluded 00167 if (state == 1) 00168 occluded_voxels.push_back (ijk); 00169 } 00170 } 00171 return 0; 00172 } 00173 00174 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00175 template <typename PointT> float 00176 pcl::VoxelGridOcclusionEstimation<PointT>::rayBoxIntersection (const Eigen::Vector4f& origin, 00177 const Eigen::Vector4f& direction) 00178 { 00179 float tmin, tmax, tymin, tymax, tzmin, tzmax; 00180 00181 if (direction[0] >= 0) 00182 { 00183 tmin = (b_min_[0] - origin[0]) / direction[0]; 00184 tmax = (b_max_[0] - origin[0]) / direction[0]; 00185 } 00186 else 00187 { 00188 tmin = (b_max_[0] - origin[0]) / direction[0]; 00189 tmax = (b_min_[0] - origin[0]) / direction[0]; 00190 } 00191 00192 if (direction[1] >= 0) 00193 { 00194 tymin = (b_min_[1] - origin[1]) / direction[1]; 00195 tymax = (b_max_[1] - origin[1]) / direction[1]; 00196 } 00197 else 00198 { 00199 tymin = (b_max_[1] - origin[1]) / direction[1]; 00200 tymax = (b_min_[1] - origin[1]) / direction[1]; 00201 } 00202 00203 if ((tmin > tymax) || (tymin > tmax)) 00204 { 00205 PCL_ERROR ("no intersection with the bounding box \n"); 00206 tmin = -1.0f; 00207 return tmin; 00208 } 00209 00210 if (tymin > tmin) 00211 tmin = tymin; 00212 if (tymax < tmax) 00213 tmax = tymax; 00214 00215 if (direction[2] >= 0) 00216 { 00217 tzmin = (b_min_[2] - origin[2]) / direction[2]; 00218 tzmax = (b_max_[2] - origin[2]) / direction[2]; 00219 } 00220 else 00221 { 00222 tzmin = (b_max_[2] - origin[2]) / direction[2]; 00223 tzmax = (b_min_[2] - origin[2]) / direction[2]; 00224 } 00225 00226 if ((tmin > tzmax) || (tzmin > tmax)) 00227 { 00228 PCL_ERROR ("no intersection with the bounding box \n"); 00229 tmin = -1.0f; 00230 return tmin; 00231 } 00232 00233 if (tzmin > tmin) 00234 tmin = tzmin; 00235 if (tzmax < tmax) 00236 tmax = tzmax; 00237 00238 return tmin; 00239 } 00240 00241 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00242 template <typename PointT> int 00243 pcl::VoxelGridOcclusionEstimation<PointT>::rayTraversal (const Eigen::Vector3i& target_voxel, 00244 const Eigen::Vector4f& origin, 00245 const Eigen::Vector4f& direction, 00246 const float t_min) 00247 { 00248 // coordinate of the boundary of the voxel grid 00249 Eigen::Vector4f start = origin + t_min * direction; 00250 00251 // i,j,k coordinate of the voxel were the ray enters the voxel grid 00252 Eigen::Vector3i ijk = getGridCoordinatesRound (start[0], start[1], start[2]); 00253 00254 // steps in which direction we have to travel in the voxel grid 00255 int step_x, step_y, step_z; 00256 00257 // centroid coordinate of the entry voxel 00258 Eigen::Vector4f voxel_max = getCentroidCoordinate (ijk); 00259 00260 if (direction[0] >= 0) 00261 { 00262 voxel_max[0] += leaf_size_[0] * 0.5f; 00263 step_x = 1; 00264 } 00265 else 00266 { 00267 voxel_max[0] -= leaf_size_[0] * 0.5f; 00268 step_x = -1; 00269 } 00270 if (direction[1] >= 0) 00271 { 00272 voxel_max[1] += leaf_size_[1] * 0.5f; 00273 step_y = 1; 00274 } 00275 else 00276 { 00277 voxel_max[1] -= leaf_size_[1] * 0.5f; 00278 step_y = -1; 00279 } 00280 if (direction[2] >= 0) 00281 { 00282 voxel_max[2] += leaf_size_[2] * 0.5f; 00283 step_z = 1; 00284 } 00285 else 00286 { 00287 voxel_max[2] -= leaf_size_[2] * 0.5f; 00288 step_z = -1; 00289 } 00290 00291 float t_max_x = t_min + (voxel_max[0] - start[0]) / direction[0]; 00292 float t_max_y = t_min + (voxel_max[1] - start[1]) / direction[1]; 00293 float t_max_z = t_min + (voxel_max[2] - start[2]) / direction[2]; 00294 00295 float t_delta_x = leaf_size_[0] / static_cast<float> (fabs (direction[0])); 00296 float t_delta_y = leaf_size_[1] / static_cast<float> (fabs (direction[1])); 00297 float t_delta_z = leaf_size_[2] / static_cast<float> (fabs (direction[2])); 00298 00299 // index of the point in the point cloud 00300 int index; 00301 00302 while ( (ijk[0] < max_b_[0]+1) && (ijk[0] >= min_b_[0]) && 00303 (ijk[1] < max_b_[1]+1) && (ijk[1] >= min_b_[1]) && 00304 (ijk[2] < max_b_[2]+1) && (ijk[2] >= min_b_[2]) ) 00305 { 00306 // check if we reached target voxel 00307 if (ijk[0] == target_voxel[0] && ijk[1] == target_voxel[1] && ijk[2] == target_voxel[2]) 00308 return 0; 00309 00310 // check if voxel is occupied, if yes return 1 for occluded 00311 index = this->getCentroidIndexAt (ijk); 00312 if (index != -1) 00313 return 1; 00314 00315 // estimate next voxel 00316 if(t_max_x <= t_max_y && t_max_x <= t_max_z) 00317 { 00318 t_max_x += t_delta_x; 00319 ijk[0] += step_x; 00320 } 00321 else if(t_max_y <= t_max_z && t_max_y <= t_max_x) 00322 { 00323 t_max_y += t_delta_y; 00324 ijk[1] += step_y; 00325 } 00326 else 00327 { 00328 t_max_z += t_delta_z; 00329 ijk[2] += step_z; 00330 } 00331 } 00332 return 0; 00333 } 00334 00335 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00336 template <typename PointT> int 00337 pcl::VoxelGridOcclusionEstimation<PointT>::rayTraversal (std::vector <Eigen::Vector3i>& out_ray, 00338 const Eigen::Vector3i& target_voxel, 00339 const Eigen::Vector4f& origin, 00340 const Eigen::Vector4f& direction, 00341 const float t_min) 00342 { 00343 // reserve space for the ray vector 00344 int reserve_size = div_b_.maxCoeff () * div_b_.maxCoeff (); 00345 out_ray.reserve (reserve_size); 00346 00347 // coordinate of the boundary of the voxel grid 00348 Eigen::Vector4f start = origin + t_min * direction; 00349 00350 // i,j,k coordinate of the voxel were the ray enters the voxel grid 00351 Eigen::Vector3i ijk = getGridCoordinatesRound (start[0], start[1], start[2]); 00352 //Eigen::Vector3i ijk = this->getGridCoordinates (start_x, start_y, start_z); 00353 00354 // steps in which direction we have to travel in the voxel grid 00355 int step_x, step_y, step_z; 00356 00357 // centroid coordinate of the entry voxel 00358 Eigen::Vector4f voxel_max = getCentroidCoordinate (ijk); 00359 00360 if (direction[0] >= 0) 00361 { 00362 voxel_max[0] += leaf_size_[0] * 0.5f; 00363 step_x = 1; 00364 } 00365 else 00366 { 00367 voxel_max[0] -= leaf_size_[0] * 0.5f; 00368 step_x = -1; 00369 } 00370 if (direction[1] >= 0) 00371 { 00372 voxel_max[1] += leaf_size_[1] * 0.5f; 00373 step_y = 1; 00374 } 00375 else 00376 { 00377 voxel_max[1] -= leaf_size_[1] * 0.5f; 00378 step_y = -1; 00379 } 00380 if (direction[2] >= 0) 00381 { 00382 voxel_max[2] += leaf_size_[2] * 0.5f; 00383 step_z = 1; 00384 } 00385 else 00386 { 00387 voxel_max[2] -= leaf_size_[2] * 0.5f; 00388 step_z = -1; 00389 } 00390 00391 float t_max_x = t_min + (voxel_max[0] - start[0]) / direction[0]; 00392 float t_max_y = t_min + (voxel_max[1] - start[1]) / direction[1]; 00393 float t_max_z = t_min + (voxel_max[2] - start[2]) / direction[2]; 00394 00395 float t_delta_x = leaf_size_[0] / static_cast<float> (fabs (direction[0])); 00396 float t_delta_y = leaf_size_[1] / static_cast<float> (fabs (direction[1])); 00397 float t_delta_z = leaf_size_[2] / static_cast<float> (fabs (direction[2])); 00398 00399 // the index of the cloud (-1 if empty) 00400 int index = -1; 00401 int result = 0; 00402 00403 while ( (ijk[0] < max_b_[0]+1) && (ijk[0] >= min_b_[0]) && 00404 (ijk[1] < max_b_[1]+1) && (ijk[1] >= min_b_[1]) && 00405 (ijk[2] < max_b_[2]+1) && (ijk[2] >= min_b_[2]) ) 00406 { 00407 // add voxel to ray 00408 out_ray.push_back (ijk); 00409 00410 // check if we reached target voxel 00411 if (ijk[0] == target_voxel[0] && ijk[1] == target_voxel[1] && ijk[2] == target_voxel[2]) 00412 break; 00413 00414 // check if voxel is occupied 00415 index = this->getCentroidIndexAt (ijk); 00416 if (index != -1) 00417 result = 1; 00418 00419 // estimate next voxel 00420 if(t_max_x <= t_max_y && t_max_x <= t_max_z) 00421 { 00422 t_max_x += t_delta_x; 00423 ijk[0] += step_x; 00424 } 00425 else if(t_max_y <= t_max_z && t_max_y <= t_max_x) 00426 { 00427 t_max_y += t_delta_y; 00428 ijk[1] += step_y; 00429 } 00430 else 00431 { 00432 t_max_z += t_delta_z; 00433 ijk[2] += step_z; 00434 } 00435 } 00436 return result; 00437 } 00438 00439 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00440 #define PCL_INSTANTIATE_VoxelGridOcclusionEstimation(T) template class PCL_EXPORTS pcl::VoxelGridOcclusionEstimation<T>; 00441 00442 #endif // PCL_FILTERS_IMPL_VOXEL_GRID_OCCLUSION_ESTIMATION_H_