template <typename PointNT> double pcl::GridProjection<PointNT>::getMagAtPoint (const Eigen::Vector4f &p, const std::vector <int> &pt_union_indices) { std::vector <double> pt_union_dist (pt_union_indices.size ()); std::vector <double> pt_union_weight (pt_union_indices.size ()); double sum = 0.0; for (size_t i = 0; i < pt_union_indices.size (); ++i) { pt_union_dist[i] = (data_->points[pt_union_indices[i]].getVector4fMap () - p).norm (); sum += pow (M_E, -pow (pt_union_dist[i], 2.0) / gaussian_scale_); } return (sum); }
template <typename PointNT> void pcl::GridProjection<PointNT>::getVectorAtPoint (const Eigen::Vector4f &p, std::vector <int> &pt_union_indices, Eigen::Vector3f &vo) { std::vector <double> pt_union_dist (pt_union_indices.size ()); std::vector <double> pt_union_weight (pt_union_indices.size ()); Eigen::Vector3f out_vector (0, 0, 0); double sum = 0.0; double mag = 0.0; for (size_t i = 0; i < pt_union_indices.size (); ++i) { Eigen::Vector4f pp (data_->points[pt_union_indices[i]].x, data_->points[pt_union_indices[i]].y, data_->points[pt_union_indices[i]].z, 0); pt_union_dist[i] = (pp - p).squaredNorm (); pt_union_weight[i] = pow (M_E, -pow (pt_union_dist[i], 2.0) / gaussian_scale_); mag += pow (M_E, -pow (sqrt (pt_union_dist[i]), 2.0) / gaussian_scale_); sum += pt_union_weight[i]; } pcl::VectorAverage3f vector_average; Eigen::Vector3f v ( data_->points[pt_union_indices[0]].normal[0], data_->points[pt_union_indices[0]].normal[1], data_->points[pt_union_indices[0]].normal[2]); for (size_t i = 0; i < pt_union_weight.size (); ++i) { pt_union_weight[i] /= sum; Eigen::Vector3f vec (data_->points[pt_union_indices[i]].normal[0], data_->points[pt_union_indices[i]].normal[1], data_->points[pt_union_indices[i]].normal[2]); if (vec.dot (v) < 0) vec = -vec; vector_average.add (vec, static_cast<float> (pt_union_weight[i])); } out_vector = vector_average.getMean (); // vector_average.getEigenVector1(out_vector); out_vector.normalize (); double d1 = getD1AtPoint (p, out_vector, pt_union_indices); out_vector *= static_cast<float> (sum); vo = ((d1 > 0) ? -1 : 1) * out_vector; }