template <typename PointNT> bool
pcl::GridProjection<PointNT>::isIntersected (const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &end_pts,
std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &vect_at_end_pts,
std::vector <int> &pt_union_indices)
{
assert (end_pts.size () == 2);
assert (vect_at_end_pts.size () == 2);
double length[2];
for (size_t i = 0; i < 2; ++i)
{
length[i] = vect_at_end_pts[i].norm ();
vect_at_end_pts[i].normalize ();
}
double dot_prod = vect_at_end_pts[0].dot (vect_at_end_pts[1]);
if (dot_prod < 0)
{
double ratio = length[0] / (length[0] + length[1]);
Eigen::Vector4f start_pt =
end_pts[0] + (end_pts[1] - end_pts[0]) * static_cast<float> (ratio);
Eigen::Vector4f intersection_pt = Eigen::Vector4f::Zero ();
findIntersection (0, end_pts, vect_at_end_pts, start_pt, pt_union_indices, intersection_pt);
Eigen::Vector3f vec;
getVectorAtPoint (intersection_pt, pt_union_indices, vec);
vec.normalize ();
double d2 = getD2AtPoint (intersection_pt, vec, pt_union_indices);
if (d2 < 0)
return (true);
}
return (false);
}
template <typename PointNT> void
pcl::GridProjection<PointNT>::getProjection (const Eigen::Vector4f &p,
std::vector <int> &pt_union_indices, Eigen::Vector4f &projection)
{
const double projection_distance = leaf_size_ * 3;
std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > end_pt (2);
std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > end_pt_vect (2);
end_pt[0] = p;
getVectorAtPoint (end_pt[0], pt_union_indices, end_pt_vect[0]);
end_pt_vect[0].normalize();
double dSecond = getD2AtPoint (end_pt[0], end_pt_vect[0], pt_union_indices);
// Find another point which is projection_distance away from the p, do a
// binary search between these two points, to find the projected point on the
// surface
if (dSecond > 0)
end_pt[1] = end_pt[0] + Eigen::Vector4f (end_pt_vect[0][0] * projection_distance,
end_pt_vect[0][1] * projection_distance,
end_pt_vect[0][2] * projection_distance, 0);
else
end_pt[1] = end_pt[0] - Eigen::Vector4f (end_pt_vect[0][0] * projection_distance,
end_pt_vect[0][1] * projection_distance,
end_pt_vect[0][2] * projection_distance, 0);
getVectorAtPoint (end_pt[1], pt_union_indices, end_pt_vect[1]);
if (end_pt_vect[1].dot (end_pt_vect[0]) < 0)
{
Eigen::Vector4f mid_pt = end_pt[0] + (end_pt[1] - end_pt[0]) * 0.5;
findIntersection (0, end_pt, end_pt_vect, mid_pt, pt_union_indices, projection);
}
else
projection = p;
}
