Point Cloud Library (PCL)
1.3.1
|
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 * $Id: shot_common.hpp 3275 2011-11-30 16:59:04Z mdixon $ 00035 */ 00036 00037 #ifndef PCL_FEATURES_IMPL_SHOT_COMMON_H_ 00038 #define PCL_FEATURES_IMPL_SHOT_COMMON_H_ 00039 00040 #include <utility> 00041 00042 // Useful constants. 00043 #define PST_PI 3.1415926535897932384626433832795 00044 #define PST_RAD_45 0.78539816339744830961566084581988 00045 #define PST_RAD_90 1.5707963267948966192313216916398 00046 #define PST_RAD_135 2.3561944901923449288469825374596 00047 #define PST_RAD_180 PST_PI 00048 #define PST_RAD_360 6.283185307179586476925286766558 00049 #define PST_RAD_PI_7_8 2.7488935718910690836548129603691 00050 00051 const double zeroDoubleEps15 = 1E-15; 00052 const float zeroFloatEps8 = 1E-8f; 00053 00055 // Compute a local Reference Frame for a 3D feature; the output is stored in the "rf" vector 00056 template <typename PointInT> float 00057 pcl::getLocalRF (const pcl::PointCloud<PointInT> &cloud, 00058 const double search_radius, 00059 const Eigen::Vector4f & central_point, 00060 const std::vector<int> &indices, 00061 const std::vector<float> &dists, 00062 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &rf) 00063 { 00064 if (rf.size () != 3) 00065 rf.resize (3); 00066 00067 // Allocate enough space 00068 Eigen::Vector4d *vij = new Eigen::Vector4d[indices.size ()]; 00069 00070 Eigen::Matrix3d cov_m = Eigen::Matrix3d::Zero (); 00071 00072 double distance = 0.0; 00073 double sum = 0.0; 00074 00075 int valid_nn_points = 0; 00076 00077 for (size_t i_idx = 0; i_idx < indices.size (); ++i_idx) 00078 { 00079 /*if (indices[i_idx] == index) 00080 continue;*/ 00081 00082 Eigen::Vector4f pt = cloud.points[indices[i_idx]].getVector4fMap (); 00083 pt[3] = 0; 00084 00085 if (pt == central_point) 00086 continue; 00087 00088 // Difference between current point and origin 00089 vij[valid_nn_points] = (pt - central_point).cast<double> (); 00090 vij[valid_nn_points][3] = 0; 00091 00092 distance = search_radius - sqrt (dists[i_idx]); 00093 00094 // Multiply vij * vij' 00095 cov_m += distance * (vij[valid_nn_points].head<3> () * vij[valid_nn_points].head<3> ().transpose ()); 00096 00097 sum += distance; 00098 valid_nn_points++; 00099 } 00100 00101 if (valid_nn_points < 5) 00102 { 00103 PCL_ERROR ("[pcl::%s::getSHOTLocalRF] Warning! Neighborhood has less than 5 vertexes. Aborting Local RF computation of feature point (%lf, %lf, %lf)\n", "SHOT", central_point[0], central_point[1], central_point[2]); 00104 rf[0].setZero (); 00105 rf[1].setZero (); 00106 rf[2].setZero (); 00107 00108 rf[0][0] = 1; 00109 rf[1][1] = 1; 00110 rf[2][2] = 1; 00111 00112 delete [] vij; 00113 00114 return (std::numeric_limits<float>::max ()); 00115 } 00116 00117 cov_m /= sum; 00118 00119 Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> solver (cov_m); 00120 00121 // Disambiguation 00122 00123 Eigen::Vector3d v1c = solver.eigenvectors ().col (0); 00124 Eigen::Vector3d v2c = solver.eigenvectors ().col (1); 00125 Eigen::Vector3d v3c = solver.eigenvectors ().col (2); 00126 00127 double e1c = solver.eigenvalues ()[0]; 00128 double e2c = solver.eigenvalues ()[1]; 00129 double e3c = solver.eigenvalues ()[2]; 00130 00131 Eigen::Vector4d v1 = Eigen::Vector4d::Zero (); 00132 Eigen::Vector4d v3 = Eigen::Vector4d::Zero (); 00133 00134 if (e1c > e2c) 00135 { 00136 if (e1c > e3c) // v1c > max(v2c,v3c) 00137 { 00138 v1.head<3> () = v1c; 00139 00140 if (e2c > e3c) // v1c > v2c > v3c 00141 v3.head<3> () = v3c; 00142 else // v1c > v3c > v2c 00143 v3.head<3> () = v2c; 00144 } 00145 else // v3c > v1c > v2c 00146 { 00147 v1.head<3> () = v3c; 00148 v3.head<3> () = v2c; 00149 } 00150 } 00151 else 00152 { 00153 if (e2c > e3c) // v2c > max(v1c,v3c) 00154 { 00155 v1.head<3> () = v2c; 00156 00157 if (e1c > e3c) // v2c > v1c > v3c 00158 v3.head<3> () = v3c; 00159 else // v2c > v3c > v1c 00160 v3.head<3> () = v1c; 00161 } 00162 else // v3c > v2c > v1c 00163 { 00164 v1.head<3> () = v3c; 00165 v3.head<3> () = v1c; 00166 } 00167 } 00168 00169 int plusNormal = 0, plusTangentDirection1=0; 00170 for (int ne = 0; ne < valid_nn_points; ne++) 00171 { 00172 double dp = vij[ne].dot (v1); 00173 if (dp >= 0) 00174 plusTangentDirection1++; 00175 00176 dp = vij[ne].dot (v3); 00177 if (dp >= 0) 00178 plusNormal++; 00179 } 00180 00181 //TANGENT 00182 if( abs ( plusTangentDirection1 - valid_nn_points + plusTangentDirection1 ) > 0 ) 00183 { 00184 if (plusTangentDirection1 < valid_nn_points - plusTangentDirection1) 00185 v1 *= - 1; 00186 } 00187 else 00188 { 00189 plusTangentDirection1=0; 00190 int points = 5; 00191 int medianIndex = valid_nn_points/2; 00192 00193 for (int i = -points/2; i <= points/2; i++) 00194 if ( vij[medianIndex- i ].dot (v1) > 0) 00195 plusTangentDirection1 ++; 00196 00197 if (plusTangentDirection1 < points/2+1) 00198 v1 *= - 1; 00199 } 00200 00201 //Normal 00202 if( abs ( plusNormal - valid_nn_points + plusNormal ) > 0 ) 00203 { 00204 if (plusNormal < valid_nn_points - plusNormal) 00205 v3 *= - 1; 00206 } 00207 else 00208 { 00209 plusNormal = 0; 00210 int points = 5; //std::min(valid_nn_points*2/2+1, 11); 00211 //std::cout << points << std::endl; 00212 int medianIndex = valid_nn_points/2; 00213 00214 for (int i = -points/2; i <= points/2; i++) 00215 if ( vij[medianIndex- i ].dot (v3) > 0) 00216 plusNormal ++; 00217 00218 if (plusNormal < points/2+1) 00219 v3 *= - 1; 00220 } 00221 00222 00223 00224 rf[0] = v1.cast<float>(); 00225 rf[2] = v3.cast<float>(); 00226 rf[1] = rf[2].cross3 (rf[0]); 00227 rf[0][3] = 0; rf[1][3] = 0; rf[2][3] = 0; 00228 00229 delete [] vij; 00230 00231 return (0.0f); 00232 } 00233 00234 #endif // PCL_FEATURES_IMPL_SHOT_COMMON_H_ 00235