pcl2::Neighborhood pcl2::computeFixedRadiusNeighborhood (Cloud & cloud, const MatF & query, float r) { // Convert point cloud MatF xyz = cloud["xyz"]; assert (xyz.rows () >= 1); assert (xyz.cols () == 3); pcl::PointCloud<pcl::PointXYZ>::Ptr input (new pcl::PointCloud<pcl::PointXYZ>); input->width = cloud.size (); input->height = 1; input->is_dense = false; input->points.resize (cloud.size ()); for (size_t i = 0; i < xyz.rows (); ++i) { input->points[i].x = xyz (i, 0); input->points[i].y = xyz (i, 1); input->points[i].z = xyz (i, 2); } // Convert query point assert (query.rows () == 1); assert (query.cols () == 3); pcl::PointXYZ q; q.x = query (0, 0); q.y = query (0, 1); q.z = query (0, 2); // Perform neighbor search pcl::KdTreeFLANN<pcl::PointXYZ> tree; tree.setInputCloud (input); std::vector<int> idx_vec; std::vector<float> dist_vec; size_t k = (size_t) tree.radiusSearch (q, r, idx_vec, dist_vec); assert (k == idx_vec.size ()); // Convert output EigenMat<int> neighbor_indices (k, 1); EigenMat<float> squared_distances (k, 1); for (size_t i = 0; i < k; ++i) { neighbor_indices (i, 0) = idx_vec[i]; squared_distances (i, 0) = dist_vec[i]; } //Cloud neighborhood = cloud (neighbor_indices); Neighborhood neighborhood (cloud, neighbor_indices); neighborhood.insert ("dist", squared_distances); return (neighborhood); }
pcl2::TypedMat<int> pcl2::findKNearestNeighbors (const Cloud & cloud, const MatF & query, size_t k) { // Convert point cloud MatF xyz = cloud["xyz"]; assert (xyz.rows () >= 1); assert (xyz.cols () == 3); pcl::PointCloud<pcl::PointXYZ>::Ptr input (new pcl::PointCloud<pcl::PointXYZ>); input->width = cloud.size (); input->height = 1; input->is_dense = false; input->points.resize (cloud.size ()); for (size_t i = 0; i < xyz.rows (); ++i) { input->points[i].x = xyz (i, 0); input->points[i].y = xyz (i, 1); input->points[i].z = xyz (i, 2); } // Convert query point assert (query.rows () == 1); assert (query.cols () == 3); pcl::PointXYZ q; q.x = query (0, 0); q.y = query (0, 1); q.z = query (0, 2); // Perform neighbor search pcl::KdTreeFLANN<pcl::PointXYZ> tree; tree.setInputCloud (input); std::vector<int> indices (k); std::vector<float> dists (k); k = (size_t) tree.nearestKSearch (q, k, indices, dists); assert (k == indices.size ()); // Convert output EigenMat<int> output (k, 1); for (size_t i = 0; i < indices.size (); ++i) output (i, 0) = indices[i]; return (output); }
IGL_INLINE void igl::arap_linear_block_spokes( const MatV & V, const MatF & F, const int d, Eigen::SparseMatrix<Scalar> & Kd) { using namespace std; using namespace Eigen; // simplex size (3: triangles, 4: tetrahedra) int simplex_size = F.cols(); // Number of elements int m = F.rows(); // Temporary output Matrix<int,Dynamic,2> edges; Kd.resize(V.rows(), V.rows()); vector<Triplet<Scalar> > Kd_IJV; if(simplex_size == 3) { // triangles Kd.reserve(7*V.rows()); Kd_IJV.reserve(7*V.rows()); edges.resize(3,2); edges << 1,2, 2,0, 0,1; }else if(simplex_size == 4) { // tets Kd.reserve(17*V.rows()); Kd_IJV.reserve(17*V.rows()); edges.resize(6,2); edges << 1,2, 2,0, 0,1, 3,0, 3,1, 3,2; } // gather cotangent weights Matrix<Scalar,Dynamic,Dynamic> C; cotmatrix_entries(V,F,C); // should have weights for each edge assert(C.cols() == edges.rows()); // loop over elements for(int i = 0;i<m;i++) { // loop over edges of element for(int e = 0;e<edges.rows();e++) { int source = F(i,edges(e,0)); int dest = F(i,edges(e,1)); double v = 0.5*C(i,e)*(V(source,d)-V(dest,d)); Kd_IJV.push_back(Triplet<Scalar>(source,dest,v)); Kd_IJV.push_back(Triplet<Scalar>(dest,source,-v)); Kd_IJV.push_back(Triplet<Scalar>(source,source,v)); Kd_IJV.push_back(Triplet<Scalar>(dest,dest,-v)); } } Kd.setFromTriplets(Kd_IJV.begin(),Kd_IJV.end()); Kd.makeCompressed(); }
IGL_INLINE void igl::faces_first( const MatV & V, const MatF & F, MatV & RV, MatF & RF, VecI & IM) { assert(&V != &RV); assert(&F != &RF); using namespace std; using namespace Eigen; vector<bool> in_face(V.rows()); for(int i = 0; i<F.rows(); i++) { for(int j = 0; j<F.cols(); j++) { in_face[F(i,j)] = true; } } // count number of vertices not in faces int num_in_F = 0; for(int i = 0;i<V.rows();i++) { num_in_F += (in_face[i]?1:0); } // list of unique vertices that occur in F VectorXi U(num_in_F); // list of unique vertices that do not occur in F VectorXi NU(V.rows()-num_in_F); int Ui = 0; int NUi = 0; // loop over vertices for(int i = 0;i<V.rows();i++) { if(in_face[i]) { U(Ui) = i; Ui++; }else { NU(NUi) = i; NUi++; } } IM.resize(V.rows()); // reindex vertices that occur in faces to be first for(int i = 0;i<U.size();i++) { IM(U(i)) = i; } // reindex vertices that do not occur in faces to come after those that do for(int i = 0;i<NU.size();i++) { IM(NU(i)) = i+U.size(); } RF.resize(F.rows(),F.cols()); // Reindex faces for(int i = 0; i<F.rows(); i++) { for(int j = 0; j<F.cols(); j++) { RF(i,j) = IM(F(i,j)); } } RV.resize(V.rows(),V.cols()); // Reorder vertices for(int i = 0;i<V.rows();i++) { RV.row(IM(i)) = V.row(i); } }