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