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;
}
