00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040 #ifndef PCL_FEATURES_IMPL_BOUNDARY_H_
00041 #define PCL_FEATURES_IMPL_BOUNDARY_H_
00042
00043 #include <pcl/features/boundary.h>
00044 #include <cfloat>
00045
00047 template <typename PointInT, typename PointNT, typename PointOutT> bool
00048 pcl::BoundaryEstimation<PointInT, PointNT, PointOutT>::isBoundaryPoint (
00049 const pcl::PointCloud<PointInT> &cloud, int q_idx,
00050 const std::vector<int> &indices,
00051 const Eigen::Vector4f &u, const Eigen::Vector4f &v,
00052 const float angle_threshold)
00053 {
00054 return (isBoundaryPoint (cloud, cloud.points[q_idx], indices, u, v, angle_threshold));
00055 }
00056
00058 template <typename PointInT, typename PointNT, typename PointOutT> bool
00059 pcl::BoundaryEstimation<PointInT, PointNT, PointOutT>::isBoundaryPoint (
00060 const pcl::PointCloud<PointInT> &cloud, const PointInT &q_point,
00061 const std::vector<int> &indices,
00062 const Eigen::Vector4f &u, const Eigen::Vector4f &v,
00063 const float angle_threshold)
00064 {
00065 if (indices.size () < 3)
00066 return (false);
00067
00068 if (!pcl_isfinite (q_point.x) || !pcl_isfinite (q_point.y) || !pcl_isfinite (q_point.z))
00069 return (false);
00070
00071
00072 std::vector<float> angles (indices.size ());
00073 float max_dif = FLT_MIN, dif;
00074 int cp = 0;
00075
00076 for (size_t i = 0; i < indices.size (); ++i)
00077 {
00078 if (!pcl_isfinite (cloud.points[indices[i]].x) ||
00079 !pcl_isfinite (cloud.points[indices[i]].y) ||
00080 !pcl_isfinite (cloud.points[indices[i]].z))
00081 continue;
00082
00083 Eigen::Vector4f delta = cloud.points[indices[i]].getVector4fMap () - q_point.getVector4fMap ();
00084
00085 angles[cp++] = atan2f (v.dot (delta), u.dot (delta));
00086 }
00087 if (cp == 0)
00088 return (false);
00089
00090 angles.resize (cp);
00091 std::sort (angles.begin (), angles.end ());
00092
00093
00094 for (size_t i = 0; i < angles.size () - 1; ++i)
00095 {
00096 dif = angles[i + 1] - angles[i];
00097 if (max_dif < dif)
00098 max_dif = dif;
00099 }
00100
00101 dif = 2 * static_cast<float> (M_PI) - angles[angles.size () - 1] + angles[0];
00102 if (max_dif < dif)
00103 max_dif = dif;
00104
00105
00106 if (max_dif > angle_threshold)
00107 return (true);
00108 else
00109 return (false);
00110 }
00111
00113 template <typename PointInT, typename PointNT, typename PointOutT> void
00114 pcl::BoundaryEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
00115 {
00116
00117
00118 std::vector<int> nn_indices (k_);
00119 std::vector<float> nn_dists (k_);
00120
00121 Eigen::Vector4f u = Eigen::Vector4f::Zero (), v = Eigen::Vector4f::Zero ();
00122
00123 output.is_dense = true;
00124
00125 if (input_->is_dense)
00126 {
00127
00128 for (size_t idx = 0; idx < indices_->size (); ++idx)
00129 {
00130 if (this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
00131 {
00132 output.points[idx].boundary_point = std::numeric_limits<uint8_t>::quiet_NaN ();
00133 output.is_dense = false;
00134 continue;
00135 }
00136
00137
00138
00139
00140 getCoordinateSystemOnPlane (normals_->points[(*indices_)[idx]], u, v);
00141
00142
00143 output.points[idx].boundary_point = isBoundaryPoint (*surface_, input_->points[(*indices_)[idx]], nn_indices, u, v, angle_threshold_);
00144 }
00145 }
00146 else
00147 {
00148
00149 for (size_t idx = 0; idx < indices_->size (); ++idx)
00150 {
00151 if (!isFinite ((*input_)[(*indices_)[idx]]) ||
00152 this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
00153 {
00154 output.points[idx].boundary_point = std::numeric_limits<uint8_t>::quiet_NaN ();
00155 output.is_dense = false;
00156 continue;
00157 }
00158
00159
00160
00161
00162 getCoordinateSystemOnPlane (normals_->points[(*indices_)[idx]], u, v);
00163
00164
00165 output.points[idx].boundary_point = isBoundaryPoint (*surface_, input_->points[(*indices_)[idx]], nn_indices, u, v, angle_threshold_);
00166 }
00167 }
00168 }
00169
00171 template <typename PointInT, typename PointNT> void
00172 pcl::BoundaryEstimation<PointInT, PointNT, Eigen::MatrixXf>::computeFeatureEigen (pcl::PointCloud<Eigen::MatrixXf> &output)
00173 {
00174
00175
00176 std::vector<int> nn_indices (k_);
00177 std::vector<float> nn_dists (k_);
00178
00179 Eigen::Vector4f u = Eigen::Vector4f::Zero (), v = Eigen::Vector4f::Zero ();
00180
00181 output.is_dense = true;
00182 output.points.resize (indices_->size (), 1);
00183
00184 if (input_->is_dense)
00185 {
00186
00187 for (size_t idx = 0; idx < indices_->size (); ++idx)
00188 {
00189 if (this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
00190 {
00191 output.points (idx, 0) = std::numeric_limits<float>::quiet_NaN ();
00192 output.is_dense = false;
00193 continue;
00194 }
00195
00196
00197
00198
00199 this->getCoordinateSystemOnPlane (normals_->points[(*indices_)[idx]], u, v);
00200
00201
00202 output.points (idx, 0) = this->isBoundaryPoint (*surface_, input_->points[(*indices_)[idx]], nn_indices, u, v, angle_threshold_);
00203 }
00204 }
00205 else
00206 {
00207
00208 for (size_t idx = 0; idx < indices_->size (); ++idx)
00209 {
00210 if (!isFinite ((*input_)[(*indices_)[idx]]) ||
00211 this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
00212 {
00213 output.points (idx, 0) = std::numeric_limits<float>::quiet_NaN ();
00214 output.is_dense = false;
00215 continue;
00216 }
00217
00218
00219
00220
00221 this->getCoordinateSystemOnPlane (normals_->points[(*indices_)[idx]], u, v);
00222
00223
00224 output.points (idx, 0) = this->isBoundaryPoint (*surface_, input_->points[(*indices_)[idx]], nn_indices, u, v, angle_threshold_);
00225 }
00226 }
00227 }
00228
00229 #define PCL_INSTANTIATE_BoundaryEstimation(PointInT,PointNT,PointOutT) template class PCL_EXPORTS pcl::BoundaryEstimation<PointInT, PointNT, PointOutT>;
00230
00231 #endif // PCL_FEATURES_IMPL_BOUNDARY_H_
00232