template <typename PointInT, typename PointOutT, typename NormalT> void pcl::TrajkovicKeypoint3D<PointInT, PointOutT, NormalT>::detectKeypoints (PointCloudOut &output) { response_.reset (new pcl::PointCloud<float> (input_->width, input_->height)); const Normals &normals = *normals_; const PointCloudIn &input = *input_; pcl::PointCloud<float>& response = *response_; const int w = static_cast<int> (input_->width) - half_window_size_; const int h = static_cast<int> (input_->height) - half_window_size_; if (method_ == FOUR_CORNERS) { #ifdef _OPENMP #pragma omp parallel for num_threads (threads_) #endif for(int j = half_window_size_; j < h; ++j) { for(int i = half_window_size_; i < w; ++i) { if (!isFinite (input (i,j))) continue; const NormalT ¢er = normals (i,j); if (!isFinite (center)) continue; int count = 0; const NormalT &up = getNormalOrNull (i, j-half_window_size_, count); const NormalT &down = getNormalOrNull (i, j+half_window_size_, count); const NormalT &left = getNormalOrNull (i-half_window_size_, j, count); const NormalT &right = getNormalOrNull (i+half_window_size_, j, count); // Get rid of isolated points if (!count) continue; float sn1 = squaredNormalsDiff (up, center); float sn2 = squaredNormalsDiff (down, center); float r1 = sn1 + sn2; float r2 = squaredNormalsDiff (right, center) + squaredNormalsDiff (left, center); float d = std::min (r1, r2); if (d < first_threshold_) continue; sn1 = sqrt (sn1); sn2 = sqrt (sn2); float b1 = normalsDiff (right, up) * sn1; b1+= normalsDiff (left, down) * sn2; float b2 = normalsDiff (right, down) * sn2; b2+= normalsDiff (left, up) * sn1; float B = std::min (b1, b2); float A = r2 - r1 - 2*B; response (i,j) = ((B < 0) && ((B + A) > 0)) ? r1 - ((B*B)/A) : d; } } } else { #ifdef _OPENMP #pragma omp parallel for num_threads (threads_) #endif for(int j = half_window_size_; j < h; ++j) { for(int i = half_window_size_; i < w; ++i) { if (!isFinite (input (i,j))) continue; const NormalT ¢er = normals (i,j); if (!isFinite (center)) continue; int count = 0; const NormalT &up = getNormalOrNull (i, j-half_window_size_, count); const NormalT &down = getNormalOrNull (i, j+half_window_size_, count); const NormalT &left = getNormalOrNull (i-half_window_size_, j, count); const NormalT &right = getNormalOrNull (i+half_window_size_, j, count); const NormalT &upleft = getNormalOrNull (i-half_window_size_, j-half_window_size_, count); const NormalT &upright = getNormalOrNull (i+half_window_size_, j-half_window_size_, count); const NormalT &downleft = getNormalOrNull (i-half_window_size_, j+half_window_size_, count); const NormalT &downright = getNormalOrNull (i+half_window_size_, j+half_window_size_, count); // Get rid of isolated points if (!count) continue; std::vector<float> r (4,0); r[0] = squaredNormalsDiff (up, center); r[0]+= squaredNormalsDiff (down, center); r[1] = squaredNormalsDiff (upright, center); r[1]+= squaredNormalsDiff (downleft, center); r[2] = squaredNormalsDiff (right, center); r[2]+= squaredNormalsDiff (left, center); r[3] = squaredNormalsDiff (downright, center); r[3]+= squaredNormalsDiff (upleft, center); float d = *(std::min_element (r.begin (), r.end ())); if (d < first_threshold_) continue; std::vector<float> B (4,0); std::vector<float> A (4,0); std::vector<float> sumAB (4,0); B[0] = normalsDiff (upright, up) * normalsDiff (up, center); B[0]+= normalsDiff (downleft, down) * normalsDiff (down, center); B[1] = normalsDiff (right, upright) * normalsDiff (upright, center); B[1]+= normalsDiff (left, downleft) * normalsDiff (downleft, center); B[2] = normalsDiff (downright, right) * normalsDiff (downright, center); B[2]+= normalsDiff (upleft, left) * normalsDiff (upleft, center); B[3] = normalsDiff (down, downright) * normalsDiff (downright, center); B[3]+= normalsDiff (up, upleft) * normalsDiff (upleft, center); A[0] = r[1] - r[0] - B[0] - B[0]; A[1] = r[2] - r[1] - B[1] - B[1]; A[2] = r[3] - r[2] - B[2] - B[2]; A[3] = r[0] - r[3] - B[3] - B[3]; sumAB[0] = A[0] + B[0]; sumAB[1] = A[1] + B[1]; sumAB[2] = A[2] + B[2]; sumAB[3] = A[3] + B[3]; if ((*std::max_element (B.begin (), B.end ()) < 0) && (*std::min_element (sumAB.begin (), sumAB.end ()) > 0)) { std::vector<float> D (4,0); D[0] = B[0] * B[0] / A[0]; D[1] = B[1] * B[1] / A[1]; D[2] = B[2] * B[2] / A[2]; D[3] = B[3] * B[3] / A[3]; response (i,j) = *(std::min (D.begin (), D.end ())); } else response (i,j) = d; } } } // Non maximas suppression std::vector<int> indices = *indices_; std::sort (indices.begin (), indices.end (), boost::bind (&TrajkovicKeypoint3D::greaterCornernessAtIndices, this, _1, _2)); output.clear (); output.reserve (input_->size ()); std::vector<bool> occupency_map (indices.size (), false); const int width (input_->width); const int height (input_->height); const int occupency_map_size (indices.size ()); #ifdef _OPENMP #pragma omp parallel for shared (output) num_threads (threads_) #endif for (int i = 0; i < indices.size (); ++i) { int idx = indices[i]; if ((response_->points[idx] < second_threshold_) || occupency_map[idx]) continue; PointOutT p; p.getVector3fMap () = input_->points[idx].getVector3fMap (); p.intensity = response_->points [idx]; #ifdef _OPENMP #pragma omp critical #endif { output.push_back (p); keypoints_indices_->indices.push_back (idx); } const int x = idx % width; const int y = idx / width; const int u_end = std::min (width, x + half_window_size_); const int v_end = std::min (height, y + half_window_size_); for(int v = std::max (0, y - half_window_size_); v < v_end; ++v) for(int u = std::max (0, x - half_window_size_); u < u_end; ++u) occupency_map[v*width + u] = true; } output.height = 1; output.width = static_cast<uint32_t> (output.size()); // we don not change the denseness output.is_dense = true; }
template <typename PointInT, typename PointOutT> void pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::computePointNormalMirror ( const int pos_x, const int pos_y, const unsigned point_index, PointOutT &normal) { float bad_point = std::numeric_limits<float>::quiet_NaN (); const int width = input_->width; const int height = input_->height; // ============================================================== if (normal_estimation_method_ == COVARIANCE_MATRIX) { if (!init_covariance_matrix_) initCovarianceMatrixMethod (); const int start_x = pos_x - rect_width_2_; const int start_y = pos_y - rect_height_2_; const int end_x = start_x + rect_width_; const int end_y = start_y + rect_height_; unsigned count = 0; sumArea<unsigned>(start_x, start_y, end_x, end_y, width, height, boost::bind(&IntegralImage2D<float, 3>::getFiniteElementsCountSE, &integral_image_XYZ_, _1, _2, _3, _4), count); // no valid points within the rectangular reagion? if (count == 0) { normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point; return; } EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix; Eigen::Vector3f center; typename IntegralImage2D<float, 3>::SecondOrderType so_elements; typename IntegralImage2D<float, 3>::ElementType tmp_center; typename IntegralImage2D<float, 3>::SecondOrderType tmp_so_elements; center[0] = 0; center[1] = 0; center[2] = 0; tmp_center[0] = 0; tmp_center[1] = 0; tmp_center[2] = 0; so_elements[0] = 0; so_elements[1] = 0; so_elements[2] = 0; so_elements[3] = 0; so_elements[4] = 0; so_elements[5] = 0; sumArea<typename IntegralImage2D<float, 3>::ElementType>(start_x, start_y, end_x, end_y, width, height, boost::bind(&IntegralImage2D<float, 3>::getFirstOrderSumSE, &integral_image_XYZ_, _1, _2, _3, _4), tmp_center); sumArea<typename IntegralImage2D<float, 3>::SecondOrderType>(start_x, start_y, end_x, end_y, width, height, boost::bind(&IntegralImage2D<float, 3>::getSecondOrderSumSE, &integral_image_XYZ_, _1, _2, _3, _4), so_elements); center[0] = float (tmp_center[0]); center[1] = float (tmp_center[1]); center[2] = float (tmp_center[2]); covariance_matrix.coeffRef (0) = static_cast<float> (so_elements [0]); covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = static_cast<float> (so_elements [1]); covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = static_cast<float> (so_elements [2]); covariance_matrix.coeffRef (4) = static_cast<float> (so_elements [3]); covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = static_cast<float> (so_elements [4]); covariance_matrix.coeffRef (8) = static_cast<float> (so_elements [5]); covariance_matrix -= (center * center.transpose ()) / static_cast<float> (count); float eigen_value; Eigen::Vector3f eigen_vector; pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector); flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, eigen_vector[0], eigen_vector[1], eigen_vector[2]); normal.getNormalVector3fMap () = eigen_vector; // Compute the curvature surface change if (eigen_value > 0.0) normal.curvature = fabsf (eigen_value / (covariance_matrix.coeff (0) + covariance_matrix.coeff (4) + covariance_matrix.coeff (8))); else normal.curvature = 0; return; } // ======================================================= else if (normal_estimation_method_ == AVERAGE_3D_GRADIENT) { if (!init_average_3d_gradient_) initAverage3DGradientMethod (); const int start_x = pos_x - rect_width_2_; const int start_y = pos_y - rect_height_2_; const int end_x = start_x + rect_width_; const int end_y = start_y + rect_height_; unsigned count_x = 0; unsigned count_y = 0; sumArea<unsigned>(start_x, start_y, end_x, end_y, width, height, boost::bind(&IntegralImage2D<float, 3>::getFiniteElementsCountSE, &integral_image_DX_, _1, _2, _3, _4), count_x); sumArea<unsigned>(start_x, start_y, end_x, end_y, width, height, boost::bind(&IntegralImage2D<float, 3>::getFiniteElementsCountSE, &integral_image_DY_, _1, _2, _3, _4), count_y); if (count_x == 0 || count_y == 0) { normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point; return; } Eigen::Vector3d gradient_x (0, 0, 0); Eigen::Vector3d gradient_y (0, 0, 0); sumArea<typename IntegralImage2D<float, 3>::ElementType>(start_x, start_y, end_x, end_y, width, height, boost::bind(&IntegralImage2D<float, 3>::getFirstOrderSumSE, &integral_image_DX_, _1, _2, _3, _4), gradient_x); sumArea<typename IntegralImage2D<float, 3>::ElementType>(start_x, start_y, end_x, end_y, width, height, boost::bind(&IntegralImage2D<float, 3>::getFirstOrderSumSE, &integral_image_DY_, _1, _2, _3, _4), gradient_y); Eigen::Vector3d normal_vector = gradient_y.cross (gradient_x); double normal_length = normal_vector.squaredNorm (); if (normal_length == 0.0f) { normal.getNormalVector3fMap ().setConstant (bad_point); normal.curvature = bad_point; return; } normal_vector /= sqrt (normal_length); float nx = static_cast<float> (normal_vector [0]); float ny = static_cast<float> (normal_vector [1]); float nz = static_cast<float> (normal_vector [2]); flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, nx, ny, nz); normal.normal_x = nx; normal.normal_y = ny; normal.normal_z = nz; normal.curvature = bad_point; return; } // ====================================================== else if (normal_estimation_method_ == AVERAGE_DEPTH_CHANGE) { if (!init_depth_change_) initAverageDepthChangeMethod (); int point_index_L_x = pos_x - rect_width_4_ - 1; int point_index_L_y = pos_y; int point_index_R_x = pos_x + rect_width_4_ + 1; int point_index_R_y = pos_y; int point_index_U_x = pos_x - 1; int point_index_U_y = pos_y - rect_height_4_; int point_index_D_x = pos_x + 1; int point_index_D_y = pos_y + rect_height_4_; if (point_index_L_x < 0) point_index_L_x = -point_index_L_x; if (point_index_U_x < 0) point_index_U_x = -point_index_U_x; if (point_index_U_y < 0) point_index_U_y = -point_index_U_y; if (point_index_R_x >= width) point_index_R_x = width-(point_index_R_x-(width-1)); if (point_index_D_x >= width) point_index_D_x = width-(point_index_D_x-(width-1)); if (point_index_D_y >= height) point_index_D_y = height-(point_index_D_y-(height-1)); const int start_x_L = pos_x - rect_width_2_; const int start_y_L = pos_y - rect_height_4_; const int end_x_L = start_x_L + rect_width_2_; const int end_y_L = start_y_L + rect_height_2_; const int start_x_R = pos_x + 1; const int start_y_R = pos_y - rect_height_4_; const int end_x_R = start_x_R + rect_width_2_; const int end_y_R = start_y_R + rect_height_2_; const int start_x_U = pos_x - rect_width_4_; const int start_y_U = pos_y - rect_height_2_; const int end_x_U = start_x_U + rect_width_2_; const int end_y_U = start_y_U + rect_height_2_; const int start_x_D = pos_x - rect_width_4_; const int start_y_D = pos_y + 1; const int end_x_D = start_x_D + rect_width_2_; const int end_y_D = start_y_D + rect_height_2_; unsigned count_L_z = 0; unsigned count_R_z = 0; unsigned count_U_z = 0; unsigned count_D_z = 0; sumArea<unsigned>(start_x_L, start_y_L, end_x_L, end_y_L, width, height, boost::bind(&IntegralImage2D<float, 1>::getFiniteElementsCountSE, &integral_image_depth_, _1, _2, _3, _4), count_L_z); sumArea<unsigned>(start_x_R, start_y_R, end_x_R, end_y_R, width, height, boost::bind(&IntegralImage2D<float, 1>::getFiniteElementsCountSE, &integral_image_depth_, _1, _2, _3, _4), count_R_z); sumArea<unsigned>(start_x_U, start_y_U, end_x_U, end_y_U, width, height, boost::bind(&IntegralImage2D<float, 1>::getFiniteElementsCountSE, &integral_image_depth_, _1, _2, _3, _4), count_U_z); sumArea<unsigned>(start_x_D, start_y_D, end_x_D, end_y_D, width, height, boost::bind(&IntegralImage2D<float, 1>::getFiniteElementsCountSE, &integral_image_depth_, _1, _2, _3, _4), count_D_z); if (count_L_z == 0 || count_R_z == 0 || count_U_z == 0 || count_D_z == 0) { normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point; return; } float mean_L_z = 0; float mean_R_z = 0; float mean_U_z = 0; float mean_D_z = 0; sumArea<float>(start_x_L, start_y_L, end_x_L, end_y_L, width, height, boost::bind(&IntegralImage2D<float, 1>::getFirstOrderSumSE, &integral_image_depth_, _1, _2, _3, _4), mean_L_z); sumArea<float>(start_x_R, start_y_R, end_x_R, end_y_R, width, height, boost::bind(&IntegralImage2D<float, 1>::getFirstOrderSumSE, &integral_image_depth_, _1, _2, _3, _4), mean_R_z); sumArea<float>(start_x_U, start_y_U, end_x_U, end_y_U, width, height, boost::bind(&IntegralImage2D<float, 1>::getFirstOrderSumSE, &integral_image_depth_, _1, _2, _3, _4), mean_U_z); sumArea<float>(start_x_D, start_y_D, end_x_D, end_y_D, width, height, boost::bind(&IntegralImage2D<float, 1>::getFirstOrderSumSE, &integral_image_depth_, _1, _2, _3, _4), mean_D_z); mean_L_z /= float (count_L_z); mean_R_z /= float (count_R_z); mean_U_z /= float (count_U_z); mean_D_z /= float (count_D_z); PointInT pointL = input_->points[point_index_L_y*width + point_index_L_x]; PointInT pointR = input_->points[point_index_R_y*width + point_index_R_x]; PointInT pointU = input_->points[point_index_U_y*width + point_index_U_x]; PointInT pointD = input_->points[point_index_D_y*width + point_index_D_x]; const float mean_x_z = mean_R_z - mean_L_z; const float mean_y_z = mean_D_z - mean_U_z; const float mean_x_x = pointR.x - pointL.x; const float mean_x_y = pointR.y - pointL.y; const float mean_y_x = pointD.x - pointU.x; const float mean_y_y = pointD.y - pointU.y; float normal_x = mean_x_y * mean_y_z - mean_x_z * mean_y_y; float normal_y = mean_x_z * mean_y_x - mean_x_x * mean_y_z; float normal_z = mean_x_x * mean_y_y - mean_x_y * mean_y_x; const float normal_length = (normal_x * normal_x + normal_y * normal_y + normal_z * normal_z); if (normal_length == 0.0f) { normal.getNormalVector3fMap ().setConstant (bad_point); normal.curvature = bad_point; return; } flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, normal_x, normal_y, normal_z); const float scale = 1.0f / sqrtf (normal_length); normal.normal_x = normal_x * scale; normal.normal_y = normal_y * scale; normal.normal_z = normal_z * scale; normal.curvature = bad_point; return; } // ======================================================== else if (normal_estimation_method_ == SIMPLE_3D_GRADIENT) { PCL_THROW_EXCEPTION (PCLException, "BORDER_POLICY_MIRROR not supported for normal estimation method SIMPLE_3D_GRADIENT"); } normal.getNormalVector3fMap ().setConstant (bad_point); normal.curvature = bad_point; return; }
template <typename PointInT, typename PointOutT> void pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::computePointNormal ( const int pos_x, const int pos_y, const unsigned point_index, PointOutT &normal) { float bad_point = std::numeric_limits<float>::quiet_NaN (); if (normal_estimation_method_ == COVARIANCE_MATRIX) { if (!init_covariance_matrix_) initCovarianceMatrixMethod (); unsigned count = integral_image_XYZ_.getFiniteElementsCount (pos_x - (rect_width_2_), pos_y - (rect_height_2_), rect_width_, rect_height_); // no valid points within the rectangular reagion? if (count == 0) { normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = std::numeric_limits<float>::quiet_NaN (); return; } EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix; Eigen::Vector3f center; typename IntegralImage2D<float, 3>::SecondOrderType so_elements; center = integral_image_XYZ_.getFirstOrderSum(pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_).cast<float> (); so_elements = integral_image_XYZ_.getSecondOrderSum(pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_); covariance_matrix.coeffRef (0) = static_cast<float> (so_elements [0]); covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = static_cast<float> (so_elements [1]); covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = static_cast<float> (so_elements [2]); covariance_matrix.coeffRef (4) = static_cast<float> (so_elements [3]); covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = static_cast<float> (so_elements [4]); covariance_matrix.coeffRef (8) = static_cast<float> (so_elements [5]); covariance_matrix -= (center * center.transpose ()) / static_cast<float> (count); float eigen_value; Eigen::Vector3f eigen_vector; pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector); //pcl::flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, eigen_vector); pcl::flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, eigen_vector[0], eigen_vector[1], eigen_vector[2]); normal.getNormalVector3fMap () = eigen_vector; // Compute the curvature surface change if (eigen_value > 0.0) normal.curvature = fabsf (eigen_value / (covariance_matrix.coeff (0) + covariance_matrix.coeff (4) + covariance_matrix.coeff (8))); else normal.curvature = 0; return; } else if (normal_estimation_method_ == AVERAGE_3D_GRADIENT) { if (!init_average_3d_gradient_) initAverage3DGradientMethod (); unsigned count_x = integral_image_DX_.getFiniteElementsCount (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_); unsigned count_y = integral_image_DY_.getFiniteElementsCount (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_); if (count_x == 0 || count_y == 0) { normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = std::numeric_limits<float>::quiet_NaN (); return; } Eigen::Vector3d gradient_x = integral_image_DX_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_); Eigen::Vector3d gradient_y = integral_image_DY_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_); Eigen::Vector3d normal_vector = gradient_y.cross (gradient_x); double normal_length = normal_vector.squaredNorm (); if (normal_length == 0.0f) { normal.getNormalVector4fMap ().setConstant (bad_point); normal.curvature = bad_point; return; } normal_vector /= sqrt (normal_length); float nx = static_cast<float> (normal_vector [0]); float ny = static_cast<float> (normal_vector [1]); float nz = static_cast<float> (normal_vector [2]); //pcl::flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, normal_vector); pcl::flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, nx, ny, nz); normal.normal_x = nx; normal.normal_y = ny; normal.normal_z = nz; normal.curvature = bad_point; return; } else if (normal_estimation_method_ == AVERAGE_DEPTH_CHANGE) { if (!init_depth_change_) initAverageDepthChangeMethod (); // unsigned count = integral_image_depth_.getFiniteElementsCount (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_); // if (count == 0) // { // normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = std::numeric_limits<float>::quiet_NaN (); // return; // } // const float mean_L_z = integral_image_depth_.getFirstOrderSum (pos_x - rect_width_2_ - 1, pos_y - rect_height_2_ , rect_width_ - 1, rect_height_ - 1) / ((rect_width_-1)*(rect_height_-1)); // const float mean_R_z = integral_image_depth_.getFirstOrderSum (pos_x - rect_width_2_ + 1, pos_y - rect_height_2_ , rect_width_ - 1, rect_height_ - 1) / ((rect_width_-1)*(rect_height_-1)); // const float mean_U_z = integral_image_depth_.getFirstOrderSum (pos_x - rect_width_2_ , pos_y - rect_height_2_ - 1, rect_width_ - 1, rect_height_ - 1) / ((rect_width_-1)*(rect_height_-1)); // const float mean_D_z = integral_image_depth_.getFirstOrderSum (pos_x - rect_width_2_ , pos_y - rect_height_2_ + 1, rect_width_ - 1, rect_height_ - 1) / ((rect_width_-1)*(rect_height_-1)); // width and height are at least 3 x 3 unsigned count_L_z = integral_image_depth_.getFiniteElementsCount (pos_x - rect_width_2_, pos_y - rect_height_4_, rect_width_2_, rect_height_2_); unsigned count_R_z = integral_image_depth_.getFiniteElementsCount (pos_x + 1 , pos_y - rect_height_4_, rect_width_2_, rect_height_2_); unsigned count_U_z = integral_image_depth_.getFiniteElementsCount (pos_x - rect_width_4_, pos_y - rect_height_2_, rect_width_2_, rect_height_2_); unsigned count_D_z = integral_image_depth_.getFiniteElementsCount (pos_x - rect_width_4_, pos_y + 1 , rect_width_2_, rect_height_2_); if (count_L_z == 0 || count_R_z == 0 || count_U_z == 0 || count_D_z == 0) { normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = std::numeric_limits<float>::quiet_NaN (); return; } float mean_L_z = static_cast<float> (integral_image_depth_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_4_, rect_width_2_, rect_height_2_) / count_L_z); float mean_R_z = static_cast<float> (integral_image_depth_.getFirstOrderSum (pos_x + 1 , pos_y - rect_height_4_, rect_width_2_, rect_height_2_) / count_R_z); float mean_U_z = static_cast<float> (integral_image_depth_.getFirstOrderSum (pos_x - rect_width_4_, pos_y - rect_height_2_, rect_width_2_, rect_height_2_) / count_U_z); float mean_D_z = static_cast<float> (integral_image_depth_.getFirstOrderSum (pos_x - rect_width_4_, pos_y + 1 , rect_width_2_, rect_height_2_) / count_D_z); PointInT pointL = input_->points[point_index - rect_width_4_ - 1]; PointInT pointR = input_->points[point_index + rect_width_4_ + 1]; PointInT pointU = input_->points[point_index - rect_height_4_ * input_->width - 1]; PointInT pointD = input_->points[point_index + rect_height_4_ * input_->width + 1]; const float mean_x_z = mean_R_z - mean_L_z; const float mean_y_z = mean_D_z - mean_U_z; const float mean_x_x = pointR.x - pointL.x; const float mean_x_y = pointR.y - pointL.y; const float mean_y_x = pointD.x - pointU.x; const float mean_y_y = pointD.y - pointU.y; float normal_x = mean_x_y * mean_y_z - mean_x_z * mean_y_y; float normal_y = mean_x_z * mean_y_x - mean_x_x * mean_y_z; float normal_z = mean_x_x * mean_y_y - mean_x_y * mean_y_x; const float normal_length = (normal_x * normal_x + normal_y * normal_y + normal_z * normal_z); if (normal_length == 0.0f) { normal.getNormalVector4fMap ().setConstant (bad_point); normal.curvature = bad_point; return; } pcl::flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, normal_x, normal_y, normal_z); const float scale = 1.0f / sqrt (normal_length); normal.normal_x = normal_x * scale; normal.normal_y = normal_y * scale; normal.normal_z = normal_z * scale; normal.curvature = bad_point; return; } else if (normal_estimation_method_ == SIMPLE_3D_GRADIENT) { if (!init_simple_3d_gradient_) initSimple3DGradientMethod (); // this method does not work if lots of NaNs are in the neighborhood of the point Eigen::Vector3d gradient_x = integral_image_XYZ_.getFirstOrderSum (pos_x + rect_width_2_, pos_y - rect_height_2_, 1, rect_height_) - integral_image_XYZ_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, 1, rect_height_); Eigen::Vector3d gradient_y = integral_image_XYZ_.getFirstOrderSum (pos_x - rect_width_2_, pos_y + rect_height_2_, rect_width_, 1) - integral_image_XYZ_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, 1); Eigen::Vector3d normal_vector = gradient_y.cross (gradient_x); double normal_length = normal_vector.squaredNorm (); if (normal_length == 0.0f) { normal.getNormalVector4fMap ().setConstant (bad_point); normal.curvature = bad_point; return; } normal_vector /= sqrt (normal_length); float nx = static_cast<float> (normal_vector [0]); float ny = static_cast<float> (normal_vector [1]); float nz = static_cast<float> (normal_vector [2]); //pcl::flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, normal_vector); pcl::flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, nx, ny, nz); normal.normal_x = nx; normal.normal_y = ny; normal.normal_z = nz; normal.curvature = bad_point; return; } normal.getNormalVector4fMap ().setConstant (bad_point); normal.curvature = bad_point; return; }
template<typename PointInT, typename PointOutT, typename NormalT> void pcl::ISSKeypoint3D<PointInT, PointOutT, NormalT>::detectKeypoints (PointCloudOut &output) { // Make sure the output cloud is empty output.points.clear (); if (border_radius_ > 0.0) edge_points_ = getBoundaryPoints (*(input_->makeShared ()), border_radius_, angle_threshold_); bool* borders = new bool [input_->size()]; int index; #ifdef _OPENMP #pragma omp parallel for num_threads(threads_) #endif for (index = 0; index < int (input_->size ()); index++) { borders[index] = false; PointInT current_point = input_->points[index]; if ((border_radius_ > 0.0) && (pcl::isFinite(current_point))) { std::vector<int> nn_indices; std::vector<float> nn_distances; this->searchForNeighbors (static_cast<int> (index), border_radius_, nn_indices, nn_distances); for (size_t j = 0 ; j < nn_indices.size (); j++) { if (edge_points_[nn_indices[j]]) { borders[index] = true; break; } } } } #ifdef _OPENMP Eigen::Vector3d *omp_mem = new Eigen::Vector3d[threads_]; for (size_t i = 0; i < threads_; i++) omp_mem[i].setZero (3); #else Eigen::Vector3d *omp_mem = new Eigen::Vector3d[1]; omp_mem[0].setZero (3); #endif double *prg_local_mem = new double[input_->size () * 3]; double **prg_mem = new double * [input_->size ()]; for (size_t i = 0; i < input_->size (); i++) prg_mem[i] = prg_local_mem + 3 * i; #ifdef _OPENMP #pragma omp parallel for num_threads(threads_) #endif for (index = 0; index < static_cast<int> (input_->size ()); index++) { #ifdef _OPENMP int tid = omp_get_thread_num (); #else int tid = 0; #endif PointInT current_point = input_->points[index]; if ((!borders[index]) && pcl::isFinite(current_point)) { //if the considered point is not a border point and the point is "finite", then compute the scatter matrix Eigen::Matrix3d cov_m = Eigen::Matrix3d::Zero (); getScatterMatrix (static_cast<int> (index), cov_m); Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> solver (cov_m); const double& e1c = solver.eigenvalues ()[2]; const double& e2c = solver.eigenvalues ()[1]; const double& e3c = solver.eigenvalues ()[0]; if (!pcl_isfinite (e1c) || !pcl_isfinite (e2c) || !pcl_isfinite (e3c)) continue; if (e3c < 0) { PCL_WARN ("[pcl::%s::detectKeypoints] : The third eigenvalue is negative! Skipping the point with index %i.\n", name_.c_str (), index); continue; } omp_mem[tid][0] = e2c / e1c; omp_mem[tid][1] = e3c / e2c;; omp_mem[tid][2] = e3c; } for (int d = 0; d < omp_mem[tid].size (); d++) prg_mem[index][d] = omp_mem[tid][d]; } for (index = 0; index < int (input_->size ()); index++) { if (!borders[index]) { if ((prg_mem[index][0] < gamma_21_) && (prg_mem[index][1] < gamma_32_)) third_eigen_value_[index] = prg_mem[index][2]; } } bool* feat_max = new bool [input_->size()]; bool is_max; #ifdef _OPENMP #pragma omp parallel for private(is_max) num_threads(threads_) #endif for (index = 0; index < int (input_->size ()); index++) { feat_max [index] = false; PointInT current_point = input_->points[index]; if ((third_eigen_value_[index] > 0.0) && (pcl::isFinite(current_point))) { std::vector<int> nn_indices; std::vector<float> nn_distances; int n_neighbors; this->searchForNeighbors (static_cast<int> (index), non_max_radius_, nn_indices, nn_distances); n_neighbors = static_cast<int> (nn_indices.size ()); if (n_neighbors >= min_neighbors_) { is_max = true; for (int j = 0 ; j < n_neighbors; j++) if (third_eigen_value_[index] < third_eigen_value_[nn_indices[j]]) is_max = false; if (is_max) feat_max[index] = true; } } } #ifdef _OPENMP #pragma omp parallel for shared (output) num_threads(threads_) #endif for (index = 0; index < int (input_->size ()); index++) { if (feat_max[index]) #ifdef _OPENMP #pragma omp critical #endif { PointOutT p; p.getVector3fMap () = input_->points[index].getVector3fMap (); output.points.push_back(p); keypoints_indices_->indices.push_back (index); } } output.header = input_->header; output.width = static_cast<uint32_t> (output.points.size ()); output.height = 1; // Clear the contents of variables and arrays before the beginning of the next computation. if (border_radius_ > 0.0) normals_.reset (new pcl::PointCloud<NormalT>); delete[] borders; delete[] prg_mem; delete[] prg_local_mem; delete[] feat_max; }