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_NORMAL_3D_H_
00041 #define PCL_FEATURES_IMPL_NORMAL_3D_H_
00042
00043 #include <pcl/features/normal_3d.h>
00044
00046 template <typename PointInT, typename PointOutT> void
00047 pcl::NormalEstimation<PointInT, PointOutT>::computeFeature (PointCloudOut &output)
00048 {
00049
00050
00051 std::vector<int> nn_indices (k_);
00052 std::vector<float> nn_dists (k_);
00053
00054 output.is_dense = true;
00055
00056 if (input_->is_dense)
00057 {
00058
00059 for (size_t idx = 0; idx < indices_->size (); ++idx)
00060 {
00061 if (this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
00062 {
00063 output.points[idx].normal[0] = output.points[idx].normal[1] = output.points[idx].normal[2] = output.points[idx].curvature = std::numeric_limits<float>::quiet_NaN ();
00064
00065 output.is_dense = false;
00066 continue;
00067 }
00068
00069 computePointNormal (*surface_, nn_indices,
00070 output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2], output.points[idx].curvature);
00071
00072 flipNormalTowardsViewpoint (input_->points[(*indices_)[idx]], vpx_, vpy_, vpz_,
00073 output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2]);
00074
00075 }
00076 }
00077 else
00078 {
00079
00080 for (size_t idx = 0; idx < indices_->size (); ++idx)
00081 {
00082 if (!isFinite ((*input_)[(*indices_)[idx]]) ||
00083 this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
00084 {
00085 output.points[idx].normal[0] = output.points[idx].normal[1] = output.points[idx].normal[2] = output.points[idx].curvature = std::numeric_limits<float>::quiet_NaN ();
00086
00087 output.is_dense = false;
00088 continue;
00089 }
00090
00091 computePointNormal (*surface_, nn_indices,
00092 output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2], output.points[idx].curvature);
00093
00094 flipNormalTowardsViewpoint (input_->points[(*indices_)[idx]], vpx_, vpy_, vpz_,
00095 output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2]);
00096
00097 }
00098 }
00099 }
00100
00102 template <typename PointInT> void
00103 pcl::NormalEstimation<PointInT, Eigen::MatrixXf>::computeFeatureEigen (pcl::PointCloud<Eigen::MatrixXf> &output)
00104 {
00105
00106 output.points.resize (indices_->size (), 4);
00107
00108
00109
00110 std::vector<int> nn_indices (k_);
00111 std::vector<float> nn_dists (k_);
00112
00113 output.is_dense = true;
00114
00115 if (input_->is_dense)
00116 {
00117
00118 for (size_t idx = 0; idx < indices_->size (); ++idx)
00119 {
00120 if (this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
00121 {
00122 output.points (idx, 0) = output.points (idx, 1) = output.points (idx, 2) = output.points (idx, 3) = std::numeric_limits<float>::quiet_NaN ();
00123 output.is_dense = false;
00124 continue;
00125 }
00126
00127 computePointNormal (*surface_, nn_indices,
00128 output.points (idx, 0), output.points (idx, 1), output.points (idx, 2), output.points (idx, 3));
00129
00130 flipNormalTowardsViewpoint (input_->points[(*indices_)[idx]], vpx_, vpy_, vpz_,
00131 output.points (idx, 0), output.points (idx, 1), output.points (idx, 2));
00132
00133 }
00134 }
00135 else
00136 {
00137
00138 for (size_t idx = 0; idx < indices_->size (); ++idx)
00139 {
00140 if (!isFinite ((*input_)[(*indices_)[idx]]) ||
00141 this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
00142 {
00143 output.points (idx, 0) = output.points (idx, 1) = output.points (idx, 2) = output.points (idx, 3) = std::numeric_limits<float>::quiet_NaN ();
00144 output.is_dense = false;
00145 continue;
00146 }
00147
00148 computePointNormal (*surface_, nn_indices,
00149 output.points (idx, 0), output.points (idx, 1), output.points (idx, 2), output.points (idx, 3));
00150
00151 flipNormalTowardsViewpoint (input_->points[(*indices_)[idx]], vpx_, vpy_, vpz_,
00152 output.points (idx, 0), output.points (idx, 1), output.points (idx, 2));
00153
00154 }
00155 }
00156 }
00157
00158 #define PCL_INSTANTIATE_NormalEstimation(T,NT) template class PCL_EXPORTS pcl::NormalEstimation<T,NT>;
00159
00160 #endif // PCL_FEATURES_IMPL_NORMAL_3D_H_