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) 2011, Willow Garage, Inc. 00006 * Copyright (c) 2012-, Open Perception, Inc. 00007 * 00008 * All rights reserved. 00009 * 00010 * Redistribution and use in source and binary forms, with or without 00011 * modification, are permitted provided that the following conditions 00012 * are met: 00013 * 00014 * * Redistributions of source code must retain the above copyright 00015 * notice, this list of conditions and the following disclaimer. 00016 * * Redistributions in binary form must reproduce the above 00017 * copyright notice, this list of conditions and the following 00018 * disclaimer in the documentation and/or other materials provided 00019 * with the distribution. 00020 * * Neither the name of the copyright holder(s) nor the names of its 00021 * contributors may be used to endorse or promote products derived 00022 * from this software without specific prior written permission. 00023 * 00024 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00025 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00026 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00027 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00028 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00029 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00030 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00031 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00032 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00033 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00034 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00035 * POSSIBILITY OF SUCH DAMAGE. 00036 * 00037 */ 00038 00039 #ifndef PCL_FEATURES_LINEAR_LEAST_SQUARES_NORMAL_HPP_ 00040 #define PCL_FEATURES_LINEAR_LEAST_SQUARES_NORMAL_HPP_ 00041 #define EIGEN_II_METHOD 1 00042 00043 #include <pcl/features/linear_least_squares_normal.h> 00044 #include <pcl/common/time.h> 00045 ////////////////////////////////////////////////////////////////////////////////////////////// 00046 template <typename PointInT, typename PointOutT> 00047 pcl::LinearLeastSquaresNormalEstimation<PointInT, PointOutT>::~LinearLeastSquaresNormalEstimation () 00048 { 00049 } 00050 00051 ////////////////////////////////////////////////////////////////////////////////////////////// 00052 template <typename PointInT, typename PointOutT> void 00053 pcl::LinearLeastSquaresNormalEstimation<PointInT, PointOutT>::computePointNormal ( 00054 const int pos_x, const int pos_y, PointOutT &normal) 00055 { 00056 const float bad_point = std::numeric_limits<float>::quiet_NaN (); 00057 00058 const int width = input_->width; 00059 const int height = input_->height; 00060 00061 const int x = pos_x; 00062 const int y = pos_y; 00063 00064 const int index = y * width + x; 00065 00066 const float px = input_->points[index].x; 00067 const float py = input_->points[index].y; 00068 const float pz = input_->points[index].z; 00069 00070 if (pcl_isnan (px)) 00071 { 00072 normal.normal_x = bad_point; 00073 normal.normal_y = bad_point; 00074 normal.normal_z = bad_point; 00075 normal.curvature = bad_point; 00076 00077 return; 00078 } 00079 00080 float smoothingSize = normal_smoothing_size_; 00081 if (use_depth_dependent_smoothing_) smoothingSize = smoothingSize*(pz+0.5f); 00082 00083 const int smoothingSizeInt = static_cast<int> (smoothingSize); 00084 00085 float matA0 = 0.0f; 00086 float matA1 = 0.0f; 00087 float matA3 = 0.0f; 00088 00089 float vecb0 = 0.0f; 00090 float vecb1 = 0.0f; 00091 00092 for (int v = y - smoothingSizeInt; v <= y + smoothingSizeInt; v += smoothingSizeInt) 00093 { 00094 for (int u = x - smoothingSizeInt; u <= x + smoothingSizeInt; u += smoothingSizeInt) 00095 { 00096 if (u < 0 || u >= width || v < 0 || v >= height) continue; 00097 00098 const int index2 = v * width + u; 00099 00100 const float qx = input_->points[index2].x; 00101 const float qy = input_->points[index2].y; 00102 const float qz = input_->points[index2].z; 00103 00104 if (pcl_isnan (qx)) continue; 00105 00106 const float delta = qz - pz; 00107 const float i = qx - px; 00108 const float j = qy - py; 00109 00110 float depthChangeThreshold = pz*pz * 0.05f * max_depth_change_factor_; 00111 if (use_depth_dependent_smoothing_) depthChangeThreshold *= pz; 00112 00113 const float f = fabs (delta) > depthChangeThreshold ? 0 : 1; 00114 00115 matA0 += f * i * i; 00116 matA1 += f * i * j; 00117 matA3 += f * j * j; 00118 vecb0 += f * i * delta; 00119 vecb1 += f * j * delta; 00120 } 00121 } 00122 00123 const float det = matA0 * matA3 - matA1 * matA1; 00124 const float ddx = matA3 * vecb0 - matA1 * vecb1; 00125 const float ddy = -matA1 * vecb0 + matA0 * vecb1; 00126 00127 const float nx = ddx; 00128 const float ny = ddy; 00129 const float nz = -det * pz; 00130 00131 const float length = nx * nx + ny * ny + nz * nz; 00132 00133 if (length <= 0.01f) 00134 { 00135 normal.normal_x = bad_point; 00136 normal.normal_y = bad_point; 00137 normal.normal_z = bad_point; 00138 normal.curvature = bad_point; 00139 } 00140 else 00141 { 00142 const float normInv = 1.0f / sqrtf (length); 00143 00144 normal.normal_x = -nx * normInv; 00145 normal.normal_y = -ny * normInv; 00146 normal.normal_z = -nz * normInv; 00147 normal.curvature = bad_point; 00148 } 00149 00150 return; 00151 } 00152 00153 ////////////////////////////////////////////////////////////////////////////////////////////// 00154 template <typename PointInT, typename PointOutT> void 00155 pcl::LinearLeastSquaresNormalEstimation<PointInT, PointOutT>::computeFeature (PointCloudOut &output) 00156 { 00157 const float bad_point = std::numeric_limits<float>::quiet_NaN (); 00158 00159 const int width = input_->width; 00160 const int height = input_->height; 00161 00162 // we compute the normals as follows: 00163 // ---------------------------------- 00164 // 00165 // for the depth-gradient you can make the following first-order Taylor approximation: 00166 // D(x + dx) - D(x) = dx^T \Delta D + h.o.t. 00167 // 00168 // build linear system by stacking up equation for 8 neighbor points: 00169 // Y = X \Delta D 00170 // 00171 // => \Delta D = (X^T X)^{-1} X^T Y 00172 // => \Delta D = (A)^{-1} b 00173 00174 //const float smoothingSize = 30.0f; 00175 for (int y = 0; y < height; ++y) 00176 { 00177 for (int x = 0; x < width; ++x) 00178 { 00179 const int index = y * width + x; 00180 00181 const float px = input_->points[index].x; 00182 const float py = input_->points[index].y; 00183 const float pz = input_->points[index].z; 00184 00185 if (pcl_isnan(px)) continue; 00186 00187 //float depthDependentSmoothingSize = smoothingSize + pz / 10.0f; 00188 00189 float smoothingSize = normal_smoothing_size_; 00190 //if (use_depth_dependent_smoothing_) smoothingSize *= pz; 00191 //if (use_depth_dependent_smoothing_) smoothingSize += pz*5; 00192 //if (smoothingSize < 1.0f) smoothingSize += 1.0f; 00193 00194 const int smoothingSizeInt = static_cast<int>(smoothingSize); 00195 00196 float matA0 = 0.0f; 00197 float matA1 = 0.0f; 00198 float matA3 = 0.0f; 00199 00200 float vecb0 = 0.0f; 00201 float vecb1 = 0.0f; 00202 00203 for (int v = y - smoothingSizeInt; v <= y + smoothingSizeInt; v += smoothingSizeInt) 00204 { 00205 for (int u = x - smoothingSizeInt; u <= x + smoothingSizeInt; u += smoothingSizeInt) 00206 { 00207 if (u < 0 || u >= width || v < 0 || v >= height) continue; 00208 00209 const int index2 = v * width + u; 00210 00211 const float qx = input_->points[index2].x; 00212 const float qy = input_->points[index2].y; 00213 const float qz = input_->points[index2].z; 00214 00215 if (pcl_isnan(qx)) continue; 00216 00217 const float delta = qz - pz; 00218 const float i = qx - px; 00219 const float j = qy - py; 00220 00221 const float depthDependendDepthChange = (max_depth_change_factor_ * (fabsf (pz) + 1.0f) * 2.0f); 00222 const float f = fabs(delta) > depthDependendDepthChange ? 0 : 1; 00223 00224 //float f = fabs(delta) > (pz * 0.05f - 0.3f) ? 0 : 1; 00225 //const float f = fabs(delta) > (pz*pz * 0.05f * max_depth_change_factor_) ? 0 : 1; 00226 //float f = Math.Abs(delta) > (depth * Math.Log(depth + 1.0) * 0.02f - 0.2f) ? 0 : 1; 00227 00228 matA0 += f * i * i; 00229 matA1 += f * i * j; 00230 matA3 += f * j * j; 00231 vecb0 += f * i * delta; 00232 vecb1 += f * j * delta; 00233 } 00234 } 00235 00236 const float det = matA0 * matA3 - matA1 * matA1; 00237 const float ddx = matA3 * vecb0 - matA1 * vecb1; 00238 const float ddy = -matA1 * vecb0 + matA0 * vecb1; 00239 00240 const float nx = ddx; 00241 const float ny = ddy; 00242 const float nz = -det * pz; 00243 00244 const float length = nx * nx + ny * ny + nz * nz; 00245 00246 if (length <= 0.0f) 00247 { 00248 output.points[index].normal_x = bad_point; 00249 output.points[index].normal_y = bad_point; 00250 output.points[index].normal_z = bad_point; 00251 output.points[index].curvature = bad_point; 00252 } 00253 else 00254 { 00255 const float normInv = 1.0f / sqrtf (length); 00256 00257 output.points[index].normal_x = nx * normInv; 00258 output.points[index].normal_y = ny * normInv; 00259 output.points[index].normal_z = nz * normInv; 00260 output.points[index].curvature = bad_point; 00261 } 00262 } 00263 } 00264 } 00265 00266 #define PCL_INSTANTIATE_LinearLeastSquaresNormalEstimation(T,NT) template class PCL_EXPORTS pcl::LinearLeastSquaresNormalEstimation<T,NT>; 00267 //#define LinearLeastSquaresNormalEstimation(T,NT) template class PCL_EXPORTS pcl::LinearLeastSquaresNormalEstimation<T,NT>; 00268 00269 #endif 00270