template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> bool pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::isWithinNucleusCentroid (const Eigen::Vector3f& nucleus, const Eigen::Vector3f& centroid, const Eigen::Vector3f& nc, const PointInT& point) const { Eigen::Vector3f pc = centroid - point.getVector3fMap (); Eigen::Vector3f pn = nucleus - point.getVector3fMap (); Eigen::Vector3f pc_cross_nc = pc.cross (nc); return ((pc_cross_nc.norm () <= tolerance_) && (pc.dot (nc) >= 0) && (pn.dot (nc) <= 0)); }
template <typename PointInT> double CombinedCoherence<PointInT>::computeCoherence (PointInT &source, PointInT &target) { // convert color space from RGB to HSV RGBValue source_rgb, target_rgb; source_rgb.int_value = source.rgba; target_rgb.int_value = target.rgba; float source_h, source_s, source_v, target_h, target_s, target_v; RGB2HSV (source_rgb.Red, source_rgb.Blue, source_rgb.Green, source_h, source_s, source_v); RGB2HSV (target_rgb.Red, target_rgb.Blue, target_rgb.Green, target_h, target_s, target_v); // hue value is in 0 ~ 2pi, but circulated. const float _h_diff = fabsf (source_h - target_h); // Also need to compute distance other way around circle - but need to check which is closer to 0 float _h_diff2; if (source_h < target_h) _h_diff2 = fabsf (1.0f + source_h - target_h); //Add 2pi to source, subtract target else _h_diff2 = fabsf (1.0f + target_h - source_h); //Add 2pi to target, subtract source float h_diff; //Now we need to choose the smaller distance if (_h_diff < _h_diff2) h_diff = static_cast<float> (h_weight_) * _h_diff * _h_diff; else h_diff = static_cast<float> (h_weight_) * _h_diff2 * _h_diff2; const float s_diff = static_cast<float> (s_weight_) * (source_s - target_s) * (source_s - target_s); const float v_diff = static_cast<float> (v_weight_) * (source_v - target_v) * (source_v - target_v); //const float color_diff = h_diff + s_diff + v_diff; //if (color_diff > 0.1) // return 0; Eigen::Vector4f p = source.getVector4fMap (); Eigen::Vector4f p_dash = target.getVector4fMap (); double d = (p - p_dash).norm () * dist_weight_; const float diff2 = h_diff + s_diff + v_diff + d; return (1.0 / (1.0 + weight_ * diff2)); }
template <typename PointInT, typename PointNT, typename PointOutT> bool pcl::BoundaryEstimation<PointInT, PointNT, PointOutT>::isBoundaryPoint ( const pcl::PointCloud<PointInT> &cloud, const PointInT &q_point, const std::vector<int> &indices, const Eigen::Vector4f &u, const Eigen::Vector4f &v, const float angle_threshold) { if (indices.size () < 3) return (false); if (!pcl_isfinite (q_point.x) || !pcl_isfinite (q_point.y) || !pcl_isfinite (q_point.z)) return (false); // Compute the angles between each neighboring point and the query point itself std::vector<float> angles (indices.size ()); float max_dif = FLT_MIN, dif; int cp = 0; for (size_t i = 0; i < indices.size (); ++i) { if (!pcl_isfinite (cloud.points[indices[i]].x) || !pcl_isfinite (cloud.points[indices[i]].y) || !pcl_isfinite (cloud.points[indices[i]].z)) continue; Eigen::Vector4f delta = cloud.points[indices[i]].getVector4fMap () - q_point.getVector4fMap (); if (delta == Eigen::Vector4f::Zero()) continue; angles[cp++] = atan2f (v.dot (delta), u.dot (delta)); // the angles are fine between -PI and PI too } if (cp == 0) return (false); angles.resize (cp); std::sort (angles.begin (), angles.end ()); // Compute the maximal angle difference between two consecutive angles for (size_t i = 0; i < angles.size () - 1; ++i) { dif = angles[i + 1] - angles[i]; if (max_dif < dif) max_dif = dif; } // Get the angle difference between the last and the first dif = 2 * static_cast<float> (M_PI) - angles[angles.size () - 1] + angles[0]; if (max_dif < dif) max_dif = dif; // Check results if (max_dif > angle_threshold) return (true); else return (false); }
template <typename PointInT> double pcl::tracking::NormalCoherence<PointInT>::computeCoherence (PointInT &source, PointInT &target) { Eigen::Vector4f n = source.getNormalVector4fMap (); Eigen::Vector4f n_dash = target.getNormalVector4fMap (); if ( n.norm () <= 1e-5 || n_dash.norm () <= 1e-5 ) { PCL_ERROR("norm might be ZERO!\n"); std::cout << "source: " << source << std::endl; std::cout << "target: " << target << std::endl; exit (1); return 0.0; } else { n.normalize (); n_dash.normalize (); double theta = pcl::getAngle3D (n, n_dash); if (!pcl_isnan (theta)) return 1.0 / (1.0 + weight_ * theta * theta); else return 0.0; } }
template <typename PointInT, typename PointOutT> void pcl::MovingLeastSquares<PointInT, PointOutT>::performProcessing (PointCloudOut &output) { // Compute the number of coefficients nr_coeff_ = (order_ + 1) * (order_ + 2) / 2; // Allocate enough space to hold the results of nearest neighbor searches // \note resize is irrelevant for a radiusSearch (). std::vector<int> nn_indices; std::vector<float> nn_sqr_dists; // For all points for (size_t cp = 0; cp < indices_->size (); ++cp) { // Get the initial estimates of point positions and their neighborhoods if (!searchForNeighbors (int (cp), nn_indices, nn_sqr_dists)) continue; // Check the number of nearest neighbors for normal estimation (and later // for polynomial fit as well) if (nn_indices.size () < 3) continue; PointCloudOut projected_points; NormalCloud projected_points_normals; // Get a plane approximating the local surface's tangent and project point onto it computeMLSPointNormal (int (cp), *input_, nn_indices, nn_sqr_dists, projected_points, projected_points_normals); // Append projected points to output output.insert (output.end (), projected_points.begin (), projected_points.end ()); if (compute_normals_) normals_->insert (normals_->end (), projected_points_normals.begin (), projected_points_normals.end ()); } // For the voxel grid upsampling method, generate the voxel grid and dilate it // Then, project the newly obtained points to the MLS surface if (upsample_method_ == VOXEL_GRID_DILATION) { MLSVoxelGrid voxel_grid (input_, indices_, voxel_size_); for (int iteration = 0; iteration < dilation_iteration_num_; ++iteration) voxel_grid.dilate (); BOOST_FOREACH (typename MLSVoxelGrid::HashMap::value_type voxel, voxel_grid.voxel_grid_) { // Get 3D position of point Eigen::Vector3f pos; voxel_grid.getPosition (voxel.first, pos); PointInT p; p.x = pos[0]; p.y = pos[1]; p.z = pos[2]; std::vector<int> nn_indices; std::vector<float> nn_dists; tree_->nearestKSearch (p, 1, nn_indices, nn_dists); int input_index = nn_indices.front (); // If the closest point did not have a valid MLS fitting result // OR if it is too far away from the sampled point if (mls_results_[input_index].valid == false) continue; Eigen::Vector3f add_point = p.getVector3fMap (), input_point = input_->points[input_index].getVector3fMap (); Eigen::Vector3d aux = mls_results_[input_index].u; Eigen::Vector3f u = aux.cast<float> (); aux = mls_results_[input_index].v; Eigen::Vector3f v = aux.cast<float> (); float u_disp = (add_point - input_point).dot (u), v_disp = (add_point - input_point).dot (v); PointOutT result_point; pcl::Normal result_normal; projectPointToMLSSurface (u_disp, v_disp, mls_results_[input_index].u, mls_results_[input_index].v, mls_results_[input_index].plane_normal, mls_results_[input_index].curvature, input_point, mls_results_[input_index].c_vec, mls_results_[input_index].num_neighbors, result_point, result_normal); float d_before = (pos - input_point).norm (), d_after = (result_point.getVector3fMap () - input_point). norm(); if (d_after > d_before) continue; output.push_back (result_point); if (compute_normals_) normals_->push_back (result_normal); } }
template <typename PointInT, typename PointOutT> void pcl::MovingLeastSquares<PointInT, PointOutT>::performUpsampling (PointCloudOut &output) { if (upsample_method_ == DISTINCT_CLOUD) { for (size_t dp_i = 0; dp_i < distinct_cloud_->size (); ++dp_i) // dp_i = distinct_point_i { // Distinct cloud may have nan points, skip them if (!pcl_isfinite (distinct_cloud_->points[dp_i].x)) continue; // Get 3D position of point //Eigen::Vector3f pos = distinct_cloud_->points[dp_i].getVector3fMap (); std::vector<int> nn_indices; std::vector<float> nn_dists; tree_->nearestKSearch (distinct_cloud_->points[dp_i], 1, nn_indices, nn_dists); int input_index = nn_indices.front (); // If the closest point did not have a valid MLS fitting result // OR if it is too far away from the sampled point if (mls_results_[input_index].valid == false) continue; Eigen::Vector3d add_point = distinct_cloud_->points[dp_i].getVector3fMap ().template cast<double> (); float u_disp = static_cast<float> ((add_point - mls_results_[input_index].mean).dot (mls_results_[input_index].u_axis)), v_disp = static_cast<float> ((add_point - mls_results_[input_index].mean).dot (mls_results_[input_index].v_axis)); PointOutT result_point; pcl::Normal result_normal; projectPointToMLSSurface (u_disp, v_disp, mls_results_[input_index].u_axis, mls_results_[input_index].v_axis, mls_results_[input_index].plane_normal, mls_results_[input_index].mean, mls_results_[input_index].curvature, mls_results_[input_index].c_vec, mls_results_[input_index].num_neighbors, result_point, result_normal); // Copy additional point information if available copyMissingFields (input_->points[input_index], result_point); // Store the id of the original point corresponding_input_indices_->indices.push_back (input_index); output.push_back (result_point); if (compute_normals_) normals_->push_back (result_normal); } } // For the voxel grid upsampling method, generate the voxel grid and dilate it // Then, project the newly obtained points to the MLS surface if (upsample_method_ == VOXEL_GRID_DILATION) { MLSVoxelGrid voxel_grid (input_, indices_, voxel_size_); for (int iteration = 0; iteration < dilation_iteration_num_; ++iteration) voxel_grid.dilate (); for (typename MLSVoxelGrid::HashMap::iterator m_it = voxel_grid.voxel_grid_.begin (); m_it != voxel_grid.voxel_grid_.end (); ++m_it) { // Get 3D position of point Eigen::Vector3f pos; voxel_grid.getPosition (m_it->first, pos); PointInT p; p.x = pos[0]; p.y = pos[1]; p.z = pos[2]; std::vector<int> nn_indices; std::vector<float> nn_dists; tree_->nearestKSearch (p, 1, nn_indices, nn_dists); int input_index = nn_indices.front (); // If the closest point did not have a valid MLS fitting result // OR if it is too far away from the sampled point if (mls_results_[input_index].valid == false) continue; Eigen::Vector3d add_point = p.getVector3fMap ().template cast<double> (); float u_disp = static_cast<float> ((add_point - mls_results_[input_index].mean).dot (mls_results_[input_index].u_axis)), v_disp = static_cast<float> ((add_point - mls_results_[input_index].mean).dot (mls_results_[input_index].v_axis)); PointOutT result_point; pcl::Normal result_normal; projectPointToMLSSurface (u_disp, v_disp, mls_results_[input_index].u_axis, mls_results_[input_index].v_axis, mls_results_[input_index].plane_normal, mls_results_[input_index].mean, mls_results_[input_index].curvature, mls_results_[input_index].c_vec, mls_results_[input_index].num_neighbors, result_point, result_normal); // Copy additional point information if available copyMissingFields (input_->points[input_index], result_point); // Store the id of the original point corresponding_input_indices_->indices.push_back (input_index); output.push_back (result_point); if (compute_normals_) normals_->push_back (result_normal); } } }
template <typename PointInT, typename PointOutT> void pcl::MovingLeastSquares<PointInT, PointOutT>::performProcessing (PointCloudOut &output) { // Compute the number of coefficients nr_coeff_ = (order_ + 1) * (order_ + 2) / 2; // Allocate enough space to hold the results of nearest neighbor searches // \note resize is irrelevant for a radiusSearch (). std::vector<int> nn_indices; std::vector<float> nn_sqr_dists; typedef typename pcl::traits::fieldList<typename PointCloudIn::PointType>::type FieldListInput; typedef typename pcl::traits::fieldList<typename PointCloudOut::PointType>::type FieldListOutput; // For all points for (size_t cp = 0; cp < indices_->size (); ++cp) { // Get the initial estimates of point positions and their neighborhoods if (!searchForNeighbors (int (cp), nn_indices, nn_sqr_dists)) continue; // Check the number of nearest neighbors for normal estimation (and later // for polynomial fit as well) if (nn_indices.size () < 3) continue; PointCloudOut projected_points; NormalCloud projected_points_normals; // Get a plane approximating the local surface's tangent and project point onto it computeMLSPointNormal (int (cp), *input_, nn_indices, nn_sqr_dists, projected_points, projected_points_normals); /// Copy RGB information if available float rgb_input; bool rgb_exists_input; pcl::for_each_type<FieldListInput> (pcl::CopyIfFieldExists<typename PointCloudIn::PointType, float> ( input_->points[(*indices_)[cp]], "rgb", rgb_exists_input, rgb_input)); if (rgb_exists_input) { for (size_t pp = 0; pp < projected_points.size (); ++pp) pcl::for_each_type<FieldListOutput> (pcl::SetIfFieldExists<typename PointCloudOut::PointType, float> ( projected_points.points[pp], "rgb", rgb_input)); } // Append projected points to output output.insert (output.end (), projected_points.begin (), projected_points.end ()); if (compute_normals_) normals_->insert (normals_->end (), projected_points_normals.begin (), projected_points_normals.end ()); } if (upsample_method_ == DISTINCT_CLOUD) { for (size_t dp_i = 0; dp_i < distinct_cloud_->size (); ++dp_i) // dp_i = distinct_point_i { // Distinct cloud may have nan points, skip them if (!pcl_isfinite (distinct_cloud_->points[dp_i].x)) continue; // Get 3D position of point //Eigen::Vector3f pos = distinct_cloud_->points[dp_i].getVector3fMap (); std::vector<int> nn_indices; std::vector<float> nn_dists; tree_->nearestKSearch (distinct_cloud_->points[dp_i], 1, nn_indices, nn_dists); int input_index = nn_indices.front (); // If the closest point did not have a valid MLS fitting result // OR if it is too far away from the sampled point if (mls_results_[input_index].valid == false) continue; Eigen::Vector3f add_point = distinct_cloud_->points[dp_i].getVector3fMap (), input_point = input_->points[input_index].getVector3fMap (); Eigen::Vector3d aux = mls_results_[input_index].u; Eigen::Vector3f u = aux.cast<float> (); aux = mls_results_[input_index].v; Eigen::Vector3f v = aux.cast<float> (); float u_disp = (add_point - input_point).dot (u), v_disp = (add_point - input_point).dot (v); PointOutT result_point; pcl::Normal result_normal; projectPointToMLSSurface (u_disp, v_disp, mls_results_[input_index].u, mls_results_[input_index].v, mls_results_[input_index].plane_normal, mls_results_[input_index].curvature, input_point, mls_results_[input_index].c_vec, mls_results_[input_index].num_neighbors, result_point, result_normal); /// Copy RGB information if available float rgb_input; bool rgb_exists_input; pcl::for_each_type<FieldListInput> (pcl::CopyIfFieldExists<typename PointCloudIn::PointType, float> ( input_->points[input_index], "rgb", rgb_exists_input, rgb_input)); if (rgb_exists_input) { pcl::for_each_type<FieldListOutput> (pcl::SetIfFieldExists<typename PointCloudOut::PointType, float> ( result_point, "rgb", rgb_input)); } output.push_back (result_point); if (compute_normals_) normals_->push_back (result_normal); } } // For the voxel grid upsampling method, generate the voxel grid and dilate it // Then, project the newly obtained points to the MLS surface if (upsample_method_ == VOXEL_GRID_DILATION) { MLSVoxelGrid voxel_grid (input_, indices_, voxel_size_); for (int iteration = 0; iteration < dilation_iteration_num_; ++iteration) voxel_grid.dilate (); for (typename MLSVoxelGrid::HashMap::iterator m_it = voxel_grid.voxel_grid_.begin (); m_it != voxel_grid.voxel_grid_.end (); ++m_it) { // Get 3D position of point Eigen::Vector3f pos; voxel_grid.getPosition (m_it->first, pos); PointInT p; p.x = pos[0]; p.y = pos[1]; p.z = pos[2]; std::vector<int> nn_indices; std::vector<float> nn_dists; tree_->nearestKSearch (p, 1, nn_indices, nn_dists); int input_index = nn_indices.front (); // If the closest point did not have a valid MLS fitting result // OR if it is too far away from the sampled point if (mls_results_[input_index].valid == false) continue; Eigen::Vector3f add_point = p.getVector3fMap (), input_point = input_->points[input_index].getVector3fMap (); Eigen::Vector3d aux = mls_results_[input_index].u; Eigen::Vector3f u = aux.cast<float> (); aux = mls_results_[input_index].v; Eigen::Vector3f v = aux.cast<float> (); float u_disp = (add_point - input_point).dot (u), v_disp = (add_point - input_point).dot (v); PointOutT result_point; pcl::Normal result_normal; projectPointToMLSSurface (u_disp, v_disp, mls_results_[input_index].u, mls_results_[input_index].v, mls_results_[input_index].plane_normal, mls_results_[input_index].curvature, input_point, mls_results_[input_index].c_vec, mls_results_[input_index].num_neighbors, result_point, result_normal); float d_before = (pos - input_point).norm (), d_after = (result_point.getVector3fMap () - input_point). norm(); if (d_after > d_before) continue; /// Copy RGB information if available float rgb_input; bool rgb_exists_input; pcl::for_each_type<FieldListInput> (pcl::CopyIfFieldExists<typename PointCloudIn::PointType, float> ( input_->points[input_index], "rgb", rgb_exists_input, rgb_input)); if (rgb_exists_input) { pcl::for_each_type<FieldListOutput> (pcl::SetIfFieldExists<typename PointCloudOut::PointType, float> ( result_point, "rgb", rgb_input)); } output.push_back (result_point); if (compute_normals_) normals_->push_back (result_normal); } } }
template <typename PointInT, typename PointOutT> void pcl::MovingLeastSquares<PointInT, PointOutT>::performUpsampling (PointCloudOut &output) { if (upsample_method_ == DISTINCT_CLOUD) { corresponding_input_indices_.reset (new PointIndices); for (size_t dp_i = 0; dp_i < distinct_cloud_->size (); ++dp_i) // dp_i = distinct_point_i { // Distinct cloud may have nan points, skip them if (!pcl_isfinite (distinct_cloud_->points[dp_i].x)) continue; // Get 3D position of point //Eigen::Vector3f pos = distinct_cloud_->points[dp_i].getVector3fMap (); std::vector<int> nn_indices; std::vector<float> nn_dists; tree_->nearestKSearch (distinct_cloud_->points[dp_i], 1, nn_indices, nn_dists); int input_index = nn_indices.front (); // If the closest point did not have a valid MLS fitting result // OR if it is too far away from the sampled point if (mls_results_[input_index].valid == false) continue; Eigen::Vector3d add_point = distinct_cloud_->points[dp_i].getVector3fMap ().template cast<double> (); MLSResult::MLSProjectionResults proj = mls_results_[input_index].projectPoint (add_point, projection_method_, 5 * nr_coeff_); addProjectedPointNormal (input_index, proj.point, proj.normal, mls_results_[input_index].curvature, output, *normals_, *corresponding_input_indices_); } } // For the voxel grid upsampling method, generate the voxel grid and dilate it // Then, project the newly obtained points to the MLS surface if (upsample_method_ == VOXEL_GRID_DILATION) { corresponding_input_indices_.reset (new PointIndices); MLSVoxelGrid voxel_grid (input_, indices_, voxel_size_); for (int iteration = 0; iteration < dilation_iteration_num_; ++iteration) voxel_grid.dilate (); for (typename MLSVoxelGrid::HashMap::iterator m_it = voxel_grid.voxel_grid_.begin (); m_it != voxel_grid.voxel_grid_.end (); ++m_it) { // Get 3D position of point Eigen::Vector3f pos; voxel_grid.getPosition (m_it->first, pos); PointInT p; p.x = pos[0]; p.y = pos[1]; p.z = pos[2]; std::vector<int> nn_indices; std::vector<float> nn_dists; tree_->nearestKSearch (p, 1, nn_indices, nn_dists); int input_index = nn_indices.front (); // If the closest point did not have a valid MLS fitting result // OR if it is too far away from the sampled point if (mls_results_[input_index].valid == false) continue; Eigen::Vector3d add_point = p.getVector3fMap ().template cast<double> (); MLSResult::MLSProjectionResults proj = mls_results_[input_index].projectPoint (add_point, projection_method_, 5 * nr_coeff_); addProjectedPointNormal (input_index, proj.point, proj.normal, mls_results_[input_index].curvature, output, *normals_, *corresponding_input_indices_); } } }