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;
}
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>::storeVectAndSurfacePoint (int index_1d,
const Eigen::Vector3i &index_3d,
std::vector <int> &pt_union_indices,
const Leaf &cell_data)
{
// Get point on grid
Eigen::Vector4f grid_pt (cell_data.pt_on_surface.x () - leaf_size_ / 2.0,
cell_data.pt_on_surface.y () + leaf_size_ / 2.0,
cell_data.pt_on_surface.z () + leaf_size_ / 2.0, 0);
// Save the vector and the point on the surface
getVectorAtPoint (grid_pt, pt_union_indices, cell_hash_map_[index_1d].vect_at_grid_pt);
getProjection (cell_data.pt_on_surface, pt_union_indices, cell_hash_map_[index_1d].pt_on_surface);
}
template <typename PointNT> void
pcl::GridProjection<PointNT>::findIntersection (int level,
const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &end_pts,
const std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &vect_at_end_pts,
const Eigen::Vector4f &start_pt,
std::vector <int> &pt_union_indices,
Eigen::Vector4f &intersection)
{
assert (end_pts.size () == 2);
assert (vect_at_end_pts.size () == 2);
Eigen::Vector3f vec;
getVectorAtPoint (start_pt, pt_union_indices, vec);
double d1 = getD1AtPoint (start_pt, vec, pt_union_indices);
std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > new_end_pts (2);
std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > new_vect_at_end_pts (2);
if ((fabs (d1) < 10e-3) || (level == max_binary_search_level_))
{
intersection = start_pt;
return;
}
else
{
vec.normalize ();
if (vec.dot (vect_at_end_pts[0]) < 0)
{
Eigen::Vector4f new_start_pt = end_pts[0] + (start_pt - end_pts[0]) * 0.5;
new_end_pts[0] = end_pts[0];
new_end_pts[1] = start_pt;
new_vect_at_end_pts[0] = vect_at_end_pts[0];
new_vect_at_end_pts[1] = vec;
findIntersection (level + 1, new_end_pts, new_vect_at_end_pts, new_start_pt, pt_union_indices, intersection);
return;
}
if (vec.dot (vect_at_end_pts[1]) < 0)
{
Eigen::Vector4f new_start_pt = start_pt + (end_pts[1] - start_pt) * 0.5;
new_end_pts[0] = start_pt;
new_end_pts[1] = end_pts[1];
new_vect_at_end_pts[0] = vec;
new_vect_at_end_pts[1] = vect_at_end_pts[1];
findIntersection (level + 1, new_end_pts, new_vect_at_end_pts, new_start_pt, pt_union_indices, intersection);
return;
}
intersection = start_pt;
return;
}
}
