template <typename PointInT> void pcl::NormalEstimation<PointInT, Eigen::MatrixXf>::computeFeatureEigen (pcl::PointCloud<Eigen::MatrixXf> &output) { // Resize the output dataset output.points.resize (indices_->size (), 4); // Allocate enough space to hold the results // \note This resize is irrelevant for a radiusSearch (). std::vector<int> nn_indices (k_); std::vector<float> nn_dists (k_); output.is_dense = true; // Save a few cycles by not checking every point for NaN/Inf values if the cloud is set to dense if (input_->is_dense) { // Iterating over the entire index vector for (size_t idx = 0; idx < indices_->size (); ++idx) { if (this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0) { output.points (idx, 0) = output.points (idx, 1) = output.points (idx, 2) = output.points (idx, 3) = std::numeric_limits<float>::quiet_NaN (); output.is_dense = false; continue; } computePointNormal (*surface_, nn_indices, output.points (idx, 0), output.points (idx, 1), output.points (idx, 2), output.points (idx, 3)); flipNormalTowardsViewpoint (input_->points[(*indices_)[idx]], vpx_, vpy_, vpz_, output.points (idx, 0), output.points (idx, 1), output.points (idx, 2)); } } else { // Iterating over the entire index vector for (size_t idx = 0; idx < indices_->size (); ++idx) { if (!isFinite ((*input_)[(*indices_)[idx]]) || this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0) { output.points (idx, 0) = output.points (idx, 1) = output.points (idx, 2) = output.points (idx, 3) = std::numeric_limits<float>::quiet_NaN (); output.is_dense = false; continue; } computePointNormal (*surface_, nn_indices, output.points (idx, 0), output.points (idx, 1), output.points (idx, 2), output.points (idx, 3)); flipNormalTowardsViewpoint (input_->points[(*indices_)[idx]], vpx_, vpy_, vpz_, output.points (idx, 0), output.points (idx, 1), output.points (idx, 2)); } } }
template <typename PointInT, typename PointOutT> void pcl::NormalEstimation<PointInT, PointOutT>::computeFeature (PointCloudOut &output) { // Allocate enough space to hold the results // \note This resize is irrelevant for a radiusSearch (). std::vector<int> nn_indices (k_); std::vector<float> nn_dists (k_); // Iterating over the entire index vector for (size_t idx = 0; idx < indices_->size (); ++idx) { if (!this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists)) { 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 (); continue; } computePointNormal (*surface_, nn_indices, output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2], output.points[idx].curvature); flipNormalTowardsViewpoint (input_->points[(*indices_)[idx]], vpx_, vpy_, vpz_, output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2]); } }
void SurfaceObj::computeFaceNormal() { int i; Vec3f vector1, vector2; for(i=0;i<FaceNormal.size();i++) { vector1=Point[Face[i][1]]-Point[Face[i][0]]; vector2=Point[Face[i][2]]-Point[Face[i][0]]; FaceNormal[i] = vector1.cross(vector2); FaceNormal[i].normalize(); } computePointNormal(); }
int computeNormalsAndEigenvalues(const typename pcl::PointCloud<PointInT>::Ptr &in_cloud, const float &radius, const int &n_threads, pcl::PointCloud<PointOutT> &out_cloud) { size_t n_points = in_cloud->size(); // resize the out cloud out_cloud.resize(n_points); // build the kdtree pcl::KdTreeFLANN<PointInT> flann; flann.setInputCloud(in_cloud); // place for neighs ids and distances std::vector<int> indices; std::vector<float> distances; #ifdef USE_OPENMP #pragma omp parallel for shared(out_cloud) private(indices, distances) \ num_threads(n_threads) #endif for (int i = 0; i < n_points; ++i) { // get neighbors for this point flann.radiusSearch((*in_cloud)[i], radius, indices, distances); // estimate normal and eigenvalues computePointNormal(*in_cloud, indices, out_cloud[i].normal_x, out_cloud[i].normal_y, out_cloud[i].normal_z, out_cloud[i].lam0, out_cloud[i].lam1, out_cloud[i].lam2); flipNormal <PointInT>((*(in_cloud))[i], 0.0, 0.0, 0.0, out_cloud[i].normal_x, out_cloud[i].normal_y, out_cloud[i].normal_z); } return 1; }
template <typename PointInT, typename PointOutT> void pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::computeFeature (PointCloudOut &output) { output.sensor_origin_ = input_->sensor_origin_; output.sensor_orientation_ = input_->sensor_orientation_; float bad_point = std::numeric_limits<float>::quiet_NaN (); // compute depth-change map unsigned char * depthChangeMap = new unsigned char[input_->points.size ()]; memset (depthChangeMap, 255, input_->points.size ()); unsigned index = 0; for (unsigned int ri = 0; ri < input_->height-1; ++ri) { for (unsigned int ci = 0; ci < input_->width-1; ++ci, ++index) { index = ri * input_->width + ci; // const float depth = (*input_)(ci, ri ).z; // const float depthR = (*input_)(ci + 1, ri ).z; // const float depthD = (*input_)(ci, ri + 1).z; const float depth = input_->points [index].z; const float depthR = input_->points [index + 1].z; const float depthD = input_->points [index + input_->width].z; //const float depthDependendDepthChange = (max_depth_change_factor_ * (fabs(depth)+1.0f))/(500.0f*0.001f); const float depthDependendDepthChange = (max_depth_change_factor_ * (fabsf (depth) + 1.0f) * 2.0f); if (fabs (depth - depthR) > depthDependendDepthChange || !pcl_isfinite (depth) || !pcl_isfinite (depthR)) { depthChangeMap[index] = 0; depthChangeMap[index+1] = 0; } if (fabs (depth - depthD) > depthDependendDepthChange || !pcl_isfinite (depth) || !pcl_isfinite (depthD)) { depthChangeMap[index] = 0; depthChangeMap[index + input_->width] = 0; } } } // compute distance map //float *distanceMap = new float[input_->points.size ()]; if (distance_map_ != NULL) delete distance_map_; distance_map_ = new float[input_->points.size ()]; float *distanceMap = distance_map_; for (size_t index = 0; index < input_->points.size (); ++index) { if (depthChangeMap[index] == 0) distanceMap[index] = 0.0f; else distanceMap[index] = static_cast<float> (input_->width + input_->height); } // first pass float* previous_row = distanceMap; float* current_row = previous_row + input_->width; for (size_t ri = 1; ri < input_->height; ++ri) { for (size_t ci = 1; ci < input_->width; ++ci) { const float upLeft = previous_row [ci - 1] + 1.4f; //distanceMap[(ri-1)*input_->width + ci-1] + 1.4f; const float up = previous_row [ci] + 1.0f; //distanceMap[(ri-1)*input_->width + ci] + 1.0f; const float upRight = previous_row [ci + 1] + 1.4f; //distanceMap[(ri-1)*input_->width + ci+1] + 1.4f; const float left = current_row [ci - 1] + 1.0f; //distanceMap[ri*input_->width + ci-1] + 1.0f; const float center = current_row [ci]; //distanceMap[ri*input_->width + ci]; const float minValue = std::min (std::min (upLeft, up), std::min (left, upRight)); if (minValue < center) current_row [ci] = minValue; //distanceMap[ri * input_->width + ci] = minValue; } previous_row = current_row; current_row += input_->width; } float* next_row = distanceMap + input_->width * (input_->height - 1); current_row = next_row - input_->width; // second pass for (int ri = input_->height-2; ri >= 0; --ri) { for (int ci = input_->width-2; ci >= 0; --ci) { const float lowerLeft = next_row [ci - 1] + 1.4f; //distanceMap[(ri+1)*input_->width + ci-1] + 1.4f; const float lower = next_row [ci] + 1.0f; //distanceMap[(ri+1)*input_->width + ci] + 1.0f; const float lowerRight = next_row [ci + 1] + 1.4f; //distanceMap[(ri+1)*input_->width + ci+1] + 1.4f; const float right = current_row [ci + 1] + 1.0f; //distanceMap[ri*input_->width + ci+1] + 1.0f; const float center = current_row [ci]; //distanceMap[ri*input_->width + ci]; const float minValue = std::min (std::min (lowerLeft, lower), std::min (right, lowerRight)); if (minValue < center) current_row [ci] = minValue; //distanceMap[ri*input_->width + ci] = minValue; } next_row = current_row; current_row -= input_->width; } // Set all normals that we do not touch to NaN // top and bottom borders // That sets the output density to false! output.is_dense = false; unsigned border = int(normal_smoothing_size_); PointOutT* vec1 = &output [0]; PointOutT* vec2 = vec1 + input_->width * (input_->height - border); size_t count = border * input_->width; for (size_t idx = 0; idx < count; ++idx) { vec1 [idx].getNormalVector4fMap ().setConstant (bad_point); vec1 [idx].curvature = bad_point; vec2 [idx].getNormalVector4fMap ().setConstant (bad_point); vec2 [idx].curvature = bad_point; } // left and right borders actually columns vec1 = &output [border * input_->width]; vec2 = vec1 + input_->width - border; for (size_t ri = border; ri < input_->height - border; ++ri, vec1 += input_->width, vec2 += input_->width) { for (size_t ci = 0; ci < border; ++ci) { vec1 [ci].getNormalVector4fMap ().setConstant (bad_point); vec1 [ci].curvature = bad_point; vec2 [ci].getNormalVector4fMap ().setConstant (bad_point); vec2 [ci].curvature = bad_point; } } if (use_depth_dependent_smoothing_) { index = border + input_->width * border; unsigned skip = (border << 1); for (unsigned ri = border; ri < input_->height - border; ++ri, index += skip) { for (unsigned ci = border; ci < input_->width - border; ++ci, ++index) { index = ri * input_->width + ci; const float depth = input_->points[index].z; if (!pcl_isfinite (depth)) { output[index].getNormalVector4fMap ().setConstant (bad_point); output[index].curvature = bad_point; continue; } float smoothing = (std::min)(distanceMap[index], normal_smoothing_size_ + static_cast<float>(depth)/10.0f); if (smoothing > 2.0f) { setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing)); computePointNormal (ci, ri, index, output [index]); } else { output[index].getNormalVector4fMap ().setConstant (bad_point); output[index].curvature = bad_point; } } } } else { float smoothing_constant = normal_smoothing_size_; index = border + input_->width * border; unsigned skip = (border << 1); for (unsigned ri = border; ri < input_->height - border; ++ri, index += skip) { for (unsigned ci = border; ci < input_->width - border; ++ci, ++index) { index = ri * input_->width + ci; if (!pcl_isfinite (input_->points[index].z)) { output [index].getNormalVector4fMap ().setConstant (bad_point); output [index].curvature = bad_point; continue; } float smoothing = (std::min)(distanceMap[index], smoothing_constant); if (smoothing > 2.0f) { setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing)); computePointNormal (ci, ri, index, output [index]); } else { output [index].getNormalVector4fMap ().setConstant (bad_point); output [index].curvature = bad_point; } } } } delete[] depthChangeMap; //delete[] distanceMap; }
template <typename PointInT, typename PointOutT> void pcl::NormalEstimationOMP<PointInT, PointOutT>::computeFeature (PointCloudOut &output) { // Allocate enough space to hold the results // \note This resize is irrelevant for a radiusSearch (). std::vector<int> nn_indices (k_); std::vector<float> nn_dists (k_); output.is_dense = true; // Save a few cycles by not checking every point for NaN/Inf values if the cloud is set to dense if (input_->is_dense) { #ifdef _OPENMP #pragma omp parallel for shared (output) private (nn_indices, nn_dists) num_threads(threads_) #endif // Iterating over the entire index vector for (int idx = 0; idx < static_cast<int> (indices_->size ()); ++idx) { Eigen::Vector4f n; if (this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0 || !computePointNormal (*surface_, nn_indices, n, output.points[idx].curvature)) { 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 (); output.is_dense = false; continue; } output.points[idx].normal_x = n[0]; output.points[idx].normal_y = n[1]; output.points[idx].normal_z = n[2]; flipNormalTowardsViewpoint (input_->points[(*indices_)[idx]], vpx_, vpy_, vpz_, output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2]); } } else { #ifdef _OPENMP #pragma omp parallel for shared (output) private (nn_indices, nn_dists) num_threads(threads_) #endif // Iterating over the entire index vector for (int idx = 0; idx < static_cast<int> (indices_->size ()); ++idx) { Eigen::Vector4f n; if (!isFinite ((*input_)[(*indices_)[idx]]) || this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0 || !computePointNormal (*surface_, nn_indices, n, output.points[idx].curvature)) { 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 (); output.is_dense = false; continue; } output.points[idx].normal_x = n[0]; output.points[idx].normal_y = n[1]; output.points[idx].normal_z = n[2]; flipNormalTowardsViewpoint (input_->points[(*indices_)[idx]], vpx_, vpy_, vpz_, output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2]); } } }
template <typename PointInT, typename PointOutT> void pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::computeFeatureFull (const float *distanceMap, const float &bad_point, PointCloudOut &output) { unsigned index = 0; if (border_policy_ == BORDER_POLICY_IGNORE) { // Set all normals that we do not touch to NaN // top and bottom borders // That sets the output density to false! output.is_dense = false; unsigned border = int(normal_smoothing_size_); PointOutT* vec1 = &output [0]; PointOutT* vec2 = vec1 + input_->width * (input_->height - border); size_t count = border * input_->width; for (size_t idx = 0; idx < count; ++idx) { vec1 [idx].getNormalVector3fMap ().setConstant (bad_point); vec1 [idx].curvature = bad_point; vec2 [idx].getNormalVector3fMap ().setConstant (bad_point); vec2 [idx].curvature = bad_point; } // left and right borders actually columns vec1 = &output [border * input_->width]; vec2 = vec1 + input_->width - border; for (size_t ri = border; ri < input_->height - border; ++ri, vec1 += input_->width, vec2 += input_->width) { for (size_t ci = 0; ci < border; ++ci) { vec1 [ci].getNormalVector3fMap ().setConstant (bad_point); vec1 [ci].curvature = bad_point; vec2 [ci].getNormalVector3fMap ().setConstant (bad_point); vec2 [ci].curvature = bad_point; } } if (use_depth_dependent_smoothing_) { index = border + input_->width * border; unsigned skip = (border << 1); for (unsigned ri = border; ri < input_->height - border; ++ri, index += skip) { for (unsigned ci = border; ci < input_->width - border; ++ci, ++index) { index = ri * input_->width + ci; const float depth = input_->points[index].z; if (!pcl_isfinite (depth)) { output[index].getNormalVector3fMap ().setConstant (bad_point); output[index].curvature = bad_point; continue; } float smoothing = (std::min)(distanceMap[index], normal_smoothing_size_ + static_cast<float>(depth)/10.0f); if (smoothing > 2.0f) { setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing)); computePointNormal (ci, ri, index, output [index]); } else { output[index].getNormalVector3fMap ().setConstant (bad_point); output[index].curvature = bad_point; } } } } else { float smoothing_constant = normal_smoothing_size_; index = border + input_->width * border; unsigned skip = (border << 1); for (unsigned ri = border; ri < input_->height - border; ++ri, index += skip) { for (unsigned ci = border; ci < input_->width - border; ++ci, ++index) { index = ri * input_->width + ci; if (!pcl_isfinite (input_->points[index].z)) { output [index].getNormalVector3fMap ().setConstant (bad_point); output [index].curvature = bad_point; continue; } float smoothing = (std::min)(distanceMap[index], smoothing_constant); if (smoothing > 2.0f) { setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing)); computePointNormal (ci, ri, index, output [index]); } else { output [index].getNormalVector3fMap ().setConstant (bad_point); output [index].curvature = bad_point; } } } } } else if (border_policy_ == BORDER_POLICY_MIRROR) { output.is_dense = false; if (use_depth_dependent_smoothing_) { //index = 0; //unsigned skip = 0; //for (unsigned ri = 0; ri < input_->height; ++ri, index += skip) for (unsigned ri = 0; ri < input_->height; ++ri) { //for (unsigned ci = 0; ci < input_->width; ++ci, ++index) for (unsigned ci = 0; ci < input_->width; ++ci) { index = ri * input_->width + ci; const float depth = input_->points[index].z; if (!pcl_isfinite (depth)) { output[index].getNormalVector3fMap ().setConstant (bad_point); output[index].curvature = bad_point; continue; } float smoothing = (std::min)(distanceMap[index], normal_smoothing_size_ + static_cast<float>(depth)/10.0f); if (smoothing > 2.0f) { setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing)); computePointNormalMirror (ci, ri, index, output [index]); } else { output[index].getNormalVector3fMap ().setConstant (bad_point); output[index].curvature = bad_point; } } } } else { float smoothing_constant = normal_smoothing_size_; //index = border + input_->width * border; //unsigned skip = (border << 1); //for (unsigned ri = border; ri < input_->height - border; ++ri, index += skip) for (unsigned ri = 0; ri < input_->height; ++ri) { //for (unsigned ci = border; ci < input_->width - border; ++ci, ++index) for (unsigned ci = 0; ci < input_->width; ++ci) { index = ri * input_->width + ci; if (!pcl_isfinite (input_->points[index].z)) { output [index].getNormalVector3fMap ().setConstant (bad_point); output [index].curvature = bad_point; continue; } float smoothing = (std::min)(distanceMap[index], smoothing_constant); if (smoothing > 2.0f) { setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing)); computePointNormalMirror (ci, ri, index, output [index]); } else { output [index].getNormalVector3fMap ().setConstant (bad_point); output [index].curvature = bad_point; } } } } } }
template <typename PointInT, typename PointOutT> void pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::computeFeaturePart (const float *distanceMap, const float &bad_point, PointCloudOut &output) { if (border_policy_ == BORDER_POLICY_IGNORE) { output.is_dense = false; unsigned border = int(normal_smoothing_size_); unsigned bottom = input_->height > border ? input_->height - border : 0; unsigned right = input_->width > border ? input_->width - border : 0; if (use_depth_dependent_smoothing_) { // Iterating over the entire index vector for (std::size_t idx = 0; idx < indices_->size (); ++idx) { unsigned pt_index = (*indices_)[idx]; unsigned u = pt_index % input_->width; unsigned v = pt_index / input_->width; if (v < border || v > bottom) { output.points[idx].getNormalVector3fMap ().setConstant (bad_point); output.points[idx].curvature = bad_point; continue; } if (u < border || v > right) { output.points[idx].getNormalVector3fMap ().setConstant (bad_point); output.points[idx].curvature = bad_point; continue; } const float depth = input_->points[pt_index].z; if (!pcl_isfinite (depth)) { output.points[idx].getNormalVector3fMap ().setConstant (bad_point); output.points[idx].curvature = bad_point; continue; } float smoothing = (std::min)(distanceMap[pt_index], normal_smoothing_size_ + static_cast<float>(depth)/10.0f); if (smoothing > 2.0f) { setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing)); computePointNormal (u, v, pt_index, output [idx]); } else { output[idx].getNormalVector3fMap ().setConstant (bad_point); output[idx].curvature = bad_point; } } } else { float smoothing_constant = normal_smoothing_size_; // Iterating over the entire index vector for (std::size_t idx = 0; idx < indices_->size (); ++idx) { unsigned pt_index = (*indices_)[idx]; unsigned u = pt_index % input_->width; unsigned v = pt_index / input_->width; if (v < border || v > bottom) { output.points[idx].getNormalVector3fMap ().setConstant (bad_point); output.points[idx].curvature = bad_point; continue; } if (u < border || v > right) { output.points[idx].getNormalVector3fMap ().setConstant (bad_point); output.points[idx].curvature = bad_point; continue; } if (!pcl_isfinite (input_->points[pt_index].z)) { output [idx].getNormalVector3fMap ().setConstant (bad_point); output [idx].curvature = bad_point; continue; } float smoothing = (std::min)(distanceMap[pt_index], smoothing_constant); if (smoothing > 2.0f) { setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing)); computePointNormal (u, v, pt_index, output [idx]); } else { output [pt_index].getNormalVector3fMap ().setConstant (bad_point); output [pt_index].curvature = bad_point; } } } }// border_policy_ == BORDER_POLICY_IGNORE else if (border_policy_ == BORDER_POLICY_MIRROR) { output.is_dense = false; if (use_depth_dependent_smoothing_) { for (std::size_t idx = 0; idx < indices_->size (); ++idx) { unsigned pt_index = (*indices_)[idx]; unsigned u = pt_index % input_->width; unsigned v = pt_index / input_->width; const float depth = input_->points[pt_index].z; if (!pcl_isfinite (depth)) { output[idx].getNormalVector3fMap ().setConstant (bad_point); output[idx].curvature = bad_point; continue; } float smoothing = (std::min)(distanceMap[pt_index], normal_smoothing_size_ + static_cast<float>(depth)/10.0f); if (smoothing > 2.0f) { setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing)); computePointNormalMirror (u, v, pt_index, output [idx]); } else { output[idx].getNormalVector3fMap ().setConstant (bad_point); output[idx].curvature = bad_point; } } } else { float smoothing_constant = normal_smoothing_size_; for (size_t idx = 0; idx < indices_->size (); ++idx) { unsigned pt_index = (*indices_)[idx]; unsigned u = pt_index % input_->width; unsigned v = pt_index / input_->width; if (!pcl_isfinite (input_->points[pt_index].z)) { output [idx].getNormalVector3fMap ().setConstant (bad_point); output [idx].curvature = bad_point; continue; } float smoothing = (std::min)(distanceMap[pt_index], smoothing_constant); if (smoothing > 2.0f) { setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing)); computePointNormalMirror (u, v, pt_index, output [idx]); } else { output [idx].getNormalVector3fMap ().setConstant (bad_point); output [idx].curvature = bad_point; } } } } // border_policy_ == BORDER_POLICY_MIRROR }