Partition ClusteringGenerator::makeContinuousBalancedClustering(Graph& G, count k) { count n = G.upperNodeIdBound(); Partition clustering(n); clustering.setUpperBound(k); std::vector<count> blockSize(k, 0); // compute block sizes for (index block = 0; block < k; ++block) { blockSize[block] = n / k + (n % k > block); } // compute prefix sums of block sizes for (index block = 1; block < k; ++block) { blockSize[block] += blockSize[block-1]; } // fill clustering according to blocks node v = 0; for (index block = 0; block < k; ++block) { while (v < blockSize[block]) { clustering.addToSubset(block,v); ++v; } } return clustering; }
/** * @brief It segments a cloud using the planes and boundaries previously calculated. A point is considered to be part of a valid object if it is above the plane, * inside the limits of the planes and it is not part of any of the planes. * * @param cloud Point cloud to segment. * @param [out] clusterIndices Valid indices after the segmentation. */ void MultiplePlaneSegmentation::segment(const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &cloud, std::vector<pcl::PointIndices> &clusterIndices) { std::vector<pcl::ModelCoefficients> coefficients; getCoefficients(coefficients); std::vector<std::vector<pcl::PointXYZRGBA>> boundaries; getBoundaries(boundaries); // Cloud containing the points without the planes. pcl::PointCloud<pcl::PointXYZRGBA>::Ptr remainingCloud = pcl::PointCloud<pcl::PointXYZRGBA>::Ptr(new pcl::PointCloud<pcl::PointXYZRGBA>(*cloud)); // -1 -> part of a plane, 0 -> not part of an object, 1 -> part of an object. std::vector<char> mask = std::vector<char>(cloud->points.size(), 0); assert(coefficients.size() == boundaries.size()); for(int i = 0; i < coefficients.size(); i++) { Eigen::Vector4f planeCoef = Eigen::Vector4f(coefficients[i].values.data()); std::vector<pcl::PointXYZRGBA> planeBoundary = boundaries[i]; #pragma omp parallel for firstprivate(planeCoef, planeBoundary) shared(cloud, mask) num_threads(4) for(size_t j = 0; j < cloud->points.size(); j++) { // Calculate the distance from the point to the plane normal as the dot product // D =(P-A).N/|N| // If the x value of the pointcloud or it is marked as a point in a plane it is not needed to // make further calculations, we don't want this point. if(isnan(cloud->points[j].x) or mask[j] == -1) continue; Eigen::Vector4f pt(cloud->points[j].x, cloud->points[j].y, cloud->points[j].z, 1); float distance = planeCoef.dot(pt); if (distance >= -0.02) { if (isInlier(cloud, j , planeBoundary, planeCoef)) { if (distance <= 0.02) { // If the point is at a distance less than X, then the point is in the plane, we mark it properly. mask[j] = -1; } else { // The point is not marked as being part of an object nor plane, if it is above it we mark it as object. mask[j] = 1; } } } } } // Parse inliers. pcl::PointIndices::Ptr inliers = pcl::PointIndices::Ptr(new pcl::PointIndices()); inliers->indices.resize(cloud->points.size()); int nr_p = 0; for(int i = 0; i < mask.size(); i++) { if(mask[i] == 1) inliers->indices[nr_p++] = i; } inliers->indices.resize(nr_p); // Clustering clusterIndices = std::vector<pcl::PointIndices>(); clustering(cloud, inliers, 0.03, 200, clusterIndices); }
Partition ClusteringGenerator::makeNoncontinuousBalancedClustering(Graph &G, count k) { Partition clustering(G.upperNodeIdBound()); clustering.setUpperBound(k); count i = 0; G.forNodes([&](node u) { clustering[u] = i % k; ++i; }); return clustering; }
/** * @brief It computes de boundary of a cloud. The mask must have at least one element. * * @param cloud Cloud in which the boundary are computed. */ void PlaneSegmentation::computeBoundary(const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &cloud) { // Compute normals. pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>()); estimateNormals(cloud, normals, 0.015); // Ignore points with a different normal. pcl::PointIndices::Ptr filtIndices(new pcl::PointIndices()); filterByNormal(normals, maskIndices, coefficients, 15.0, filtIndices); // Project point cloud to a plane. pcl::ModelCoefficients::ConstPtr coefPtr = boost::make_shared<pcl::ModelCoefficients>(coefficients); pcl::PointCloud<pcl::PointXYZRGBA>::Ptr projectedCloud = pcl::PointCloud<pcl::PointXYZRGBA>::Ptr(new pcl::PointCloud<pcl::PointXYZRGBA>()); projectToPlane(cloud, coefPtr, projectedCloud); // Clustering. std::vector<pcl::PointIndices> clusterIndices; clustering(projectedCloud, filtIndices, 0.02, 5000, clusterIndices); assert(clusterIndices.size() > 0); // Find the biggest cluster. int max_size = clusterIndices[0].indices.size(); int max_pos = 0; for(int i = 0; i < clusterIndices.size(); i++) { if (clusterIndices[i].indices.size() > max_size) { max_size = clusterIndices[i].indices.size(); max_pos = i; } } pcl::PointIndices::ConstPtr clusterIndicesPtr = boost::make_shared<pcl::PointIndices>(clusterIndices[max_pos]); // Compute the convex hull of the cluster. pcl::PointIndices hullIndices = pcl::PointIndices(); findConvexHull(projectedCloud, clusterIndicesPtr, hullIndices); pcl::PointIndices::ConstPtr hullIndicesPtr = boost::make_shared<pcl::PointIndices>(hullIndices); // Simplify convex polygon. pcl::PointIndices boundaryIndices; polygonSimplification(projectedCloud, hullIndicesPtr, coefficients.values, 4, boundaryIndices); // Copy boundary points. boundary = std::vector<pcl::PointXYZRGBA>(boundaryIndices.indices.size()); for(int j = 0; j < boundary.size(); j++) { boundary[j] = cloud->points[boundaryIndices.indices[j]]; } }
int main(int argc, char *argv[]) { if (argc != 4) { std::cout << argv[0] << "input.m patchnum sampling\n"; exit(-1); } int patchnum = atoi(argv[2]); double threshold = atof(argv[3]); srand (static_cast <unsigned> (time(0))); //load seeds; std::vector<Patch*> patches; //loadSeeds("seed_sim.m", patches); //loadSeeds("seed2.m", patches); Mesh *mesh = new Mesh; mesh->readMFile(argv[1]); generateSeeds(mesh, patches, patchnum); for (int i = 0; i <= 500; ++i) { /*if (i % 100 == 0) { sprintf_s(buf, "center_%d.cm", i+1); saveCenters(buf, patches); }*/ clustering(mesh, patches); checkPatches(patches); update(patches); /*if (i % 100 == 0) { sprintf_s(buf, "out_%d.m", i+1); mesh->writeMFile(buf); }*/ } mesh->writeMFile("end.m"); std::cout << "end!\n"; traceBoundary(patches); DualGraph *dualGraph = generateGraph(patches); sampling(patches, avg_length); delete dualGraph; delete mesh; return 0; }
void LinksAspect::ShowValues(int agentId, std::vector <char *> & fields, std::vector <double> & values) { for (std::vector <char *>::size_type n = 0; n < fields.size();n++) { if (strcmp(fields[n], "Degree")==0) values.push_back((*this)[agentId]->Degree); else if (strcmp(fields[n], "Clustering")==0) values.push_back(clustering((*this)[agentId])); else if (strcmp(fields[n], "MeanShortest")==0) { checkTimeStepChange(); values.push_back((*this)[agentId]->mean_shortest); } else if (strcmp(fields[n], "Closeness")==0) { checkTimeStepChange(); values.push_back((*this)[agentId]->closeness); } else if (strcmp(fields[n], "Betweenness")==0) { checkTimeStepChange(); values.push_back((*this)[agentId]->betweenness); } else if (strcmp(fields[n], "SizeComponent")==0) { checkTimeStepChange(); values.push_back((*this)[agentId]->size_component); } else if (strcmp(fields[n], "MaxDiameter")==0) { checkTimeStepChange(); values.push_back((*this)[agentId]->max_diameter); } else values.push_back(0); } }
TEST(clustering,rand_graph_should_return_specificvalues) { int result = 0; int nodes = 0; int maxNeighbour = 0; int minNeighbour = 0; float average = 0; #define NUMBER_VERTEX_GRAPH_RAND 40 float expectedClustering[NUMBER_VERTEX_GRAPH_RAND] = { 0,1,0,1,0,0,0,0,0,1, 0,0,0,1,1,0,1,0,1,0, 1,0,0,0,0,1,1,1,0,0, 0,0,0,1,0,1,1,0,0,0 }; result = llegir_dades(FILE_UNDER_TEST_RAND, &nodes, &maxNeighbour, &minNeighbour, &average); for ( int vertex = 0; vertex < nodes ; vertex++) { int clusterVertex = clustering(vertex); EXPECT_EQ(clusterVertex, expectedClustering[vertex]); } }
Assignments get_state_clusters(const Subset &subset, const Assignments &states, ParticleStatesTable *pst, double resolution) { Vector<Ints> rotated(subset.size(), Ints(states.size(), -1)); for (unsigned int i = 0; i < states.size(); ++i) { for (unsigned int j = 0; j < states[i].size(); ++j) { rotated[j][i] = states[i][j]; } } for (unsigned int i = 0; i < rotated.size(); ++i) { std::sort(rotated[i].begin(), rotated[i].end()); rotated[i].erase(std::unique(rotated[i].begin(), rotated[i].end()), rotated[i].end()); } Vector<Ints> clustering(states.size()); for (unsigned int i = 0; i < subset.size(); ++i) { IMP::PointerMember<ParticleStates> ps = pst->get_particle_states(subset[i]); Ints c = get_state_clusters(subset[i], ps, rotated[i], resolution); clustering[i] = c; } return filter_states(subset, states, clustering); }
UserGraph::UserGraph(std::vector<User> const & graph) { importFromORM(graph); clustering(); initialized = true; }
int main ( int argc, char * argv[]) { Options options; Protein protein; Alignment * alignment = NULL; int retval; int almtctr1, almtctr2; double **score = NULL, corr, pctg_gaps; double **clustering_score = NULL; double *area, *distance; int **rank_order= NULL,**res_rank=NULL,**int_cvg=NULL ; int ** correlated = NULL, **almt2prot = NULL, **prot2almt = NULL; /* command file is required */ if ( argc < 2 ) { fprintf ( stderr, "Usage: %s <command file>.\n", argv[0]); exit (0); } retval = read_cmd_file ( argv[1], &options); if (retval) exit(retval); retval = logger (&options, INTRO, ""); if (retval) exit(retval); /*******************************************/ /* */ /* PDB input */ /* */ /*******************************************/ if ( ! options.pdbname[0]) { fprintf (stderr, "%s cannot work without structure (cmd file was %s).\n", argv[0], argv[1]); exit (1); } else { /* warn if no chain given */ if ( !options.chain) { retval = logger (&options, WARN, "No chain specified. Using the first one."); if ( retval) exit (1); } if (retval) exit(retval); /* read in the structure */ retval = read_pdb (options.pdbname, &protein, options.chain); if (retval) exit(retval); } /*******************************************/ /* */ /* alignment scoring */ /* */ /*******************************************/ if ( ! ( alignment = emalloc ( options.no_of_alignments*sizeof(Alignment)) )) return 1; if ( ! ( score = emalloc ( options.no_of_alignments*sizeof(double*)) )) return 1; if ( ! ( rank_order = emalloc ( options.no_of_alignments*sizeof(int*)) )) return 1; if ( ! ( clustering_score = emalloc ( options.no_of_alignments*sizeof(double*)) )) return 1; if ( ! ( res_rank = emalloc ( options.no_of_alignments*sizeof(int*)) )) return 1; if ( ! ( int_cvg = emalloc ( options.no_of_alignments*sizeof(int*)) )) return 1; if ( ! ( almt2prot = emalloc ( options.no_of_alignments*sizeof(int*)) )) return 1; if ( ! ( prot2almt = emalloc ( options.no_of_alignments*sizeof(int*)) )) return 1; if ( ! ( area = emalloc ( options.no_of_alignments*sizeof(double)) )) return 1; if ( ! ( distance = emalloc ( options.no_of_alignments*sizeof(double)) )) return 1; printf ( "\t%8s %20s %8s %8s %8s \n", "almt#", "name ", "<dist to qry>", "%gaps", "area"); for ( almtctr1 = 0; almtctr1 < options.no_of_alignments; almtctr1++) { /* read in the alignment */ retval = read_clustalw (options.almtname[almtctr1], alignment + almtctr1); if (retval) exit(retval); /* pairwise distances btw the seqs */ retval = seq_pw_dist (alignment+almtctr1); if ( retval) return retval; /* average dist to the query in this alignment: */ distance[almtctr1] = avg_dist_to_special (&options, alignment + almtctr1); /* percentage of gaps in the alignment: */ pctg_gaps = (double) alignment->total_gaps/ ( (alignment+almtctr1)->length*(alignment+almtctr1)->number_of_seqs); /* make the residue scoring array */ score[almtctr1] = emalloc ( alignment[almtctr1].length*sizeof(double)); /* fill in the score array */ scoring (&options, alignment+almtctr1, score[almtctr1]); /* translate the scoring into rank order */ rank_order[almtctr1] = emalloc ( alignment[almtctr1].length*sizeof(int)); score2rank (score[almtctr1], rank_order[almtctr1], alignment[almtctr1].length); /* mapping between the protein and the alignment almtctr1 */ if ( ! (almt2prot[almtctr1] = (int *) emalloc (alignment[almtctr1].length*sizeof(int))) )exit (1); if ( ! (prot2almt[almtctr1] = (int *) emalloc (protein.length*sizeof(int))) )exit (1); retval = struct_almt_mapping (&protein, alignment+almtctr1, options.query, prot2almt[almtctr1], almt2prot[almtctr1]); if (retval) exit(retval); /* find coverage info implied by the scoring array */ if ( ! (res_rank[almtctr1] = (int*) emalloc (protein.length*sizeof(int))) ) exit (1); if ( ! (int_cvg[almtctr1] = (int*) emalloc (protein.length*sizeof(int))) ) exit (1); coverage ( &protein, almt2prot[almtctr1], score[almtctr1], alignment[almtctr1].length, res_rank[almtctr1], int_cvg[almtctr1] ); /*clustering score*/ clustering_score[almtctr1] = (double*) emalloc (protein.length*sizeof(double)); if (!clustering_score[almtctr1]) exit(retval); clustering ( &protein, res_rank[almtctr1], int_cvg[almtctr1], clustering_score[almtctr1]); /* cumulative clustering score*/ area[almtctr1] = area_over_coverage (int_cvg[almtctr1], clustering_score[almtctr1], protein.length); printf ( "\t %4d %20s %8.3lf %8.3lf %8.3lf \n", almtctr1, options.almtname[almtctr1], distance[almtctr1], pctg_gaps, area[almtctr1]); } /* find the table of correlations */ if ( ! (correlated = intmatrix ( options.no_of_alignments, options.no_of_alignments) ) ) return 1; for ( almtctr1 = 0; almtctr1 < options.no_of_alignments -1; almtctr1++) { correlated[almtctr1][almtctr1] = 1; for ( almtctr2 = almtctr1+1; almtctr2 < options.no_of_alignments; almtctr2++) { if ( alignment[almtctr1].length != alignment[almtctr2].length ) { fprintf ( stderr, "Error alignments in the files %s and %s ", options.almtname[almtctr1], options.almtname[almtctr2]); fprintf ( stderr, "seem to be of unequal length: %d and %d.\n", alignment[almtctr1].length , alignment[almtctr2].length); return 1; } corr = spearman ( rank_order[almtctr1], rank_order[almtctr2], alignment[almtctr1].length ); printf ( " %3d %3d %8.4lf\n", almtctr1, almtctr2, corr); correlated[almtctr1][almtctr2] = ( corr > 0.9 ); } } /* find corelated clusters (of sequence selections)*/ { int *cluster_count_per_size; int no_of_clusters; int max_size, secnd_max_size , ** cluster; int size = options.no_of_alignments; int i,j; double dist, ar, max_area, dist_at_max_area; double min_dist_at_max_area, min_dist, max_area_at_min_dist; int almt_no, min_dist_almt; int cluster_counter (int no_of_things, int *neighbors[], int cluster_count_per_size[], int * no_of_clusters, int * max_size, int * secnd_max_size , int * cluster[]); if ( ! ( cluster_count_per_size = emalloc (size*sizeof(int)))) return 1; if ( ! (cluster = intmatrix ( size+1, size+1) ) ) return 1; retval = cluster_counter (size, correlated, cluster_count_per_size, &no_of_clusters, & max_size, &secnd_max_size , cluster); if ( retval ) return 1; printf ( "number of clusters: %d \n", no_of_clusters); for (i=0; i<=size; i++ ) { if ( ! cluster[i][0] ) continue; if ( !i ) { printf ( "\t isolated:\n"); } else { printf ("\t cluster size: %3d \n", cluster[i][0]); } for (j=1; j <= cluster[i][0]; j++ ) { printf ( "%3d ", cluster[i][j] ); } printf ( "\n"); } /* which cluster is the closest to the singled out sequence ("special") */ min_dist_at_max_area = dist_at_max_area = 10; max_area_at_min_dist = min_dist = -10; min_dist_almt = -1; for (i=0; i<=size; i++ ) { if ( ! cluster[i][0] ) continue; max_area = -100; almt_no = dist_at_max_area = -1; for (j=1; j <= cluster[i][0]; j++ ) { dist = distance[cluster[i][j]] ; ar = area[cluster[i][j]] ; if ( max_area < ar ) { max_area = ar; dist_at_max_area = dist; almt_no = cluster[i][j]; } } if ( almt_no < 0 ) { fprintf ( stderr, "Error selecting the alignment (1)\n"); exit (1); } if ( min_dist_at_max_area > dist_at_max_area ) { min_dist = dist_at_max_area; max_area_at_min_dist = max_area; min_dist_almt = almt_no; } } if ( min_dist_almt < 0 ) { fprintf ( stderr, "Error selecting the alignment (2)\n"); exit (1); } printf ( "choosing alignment %d %s (distance: %5.3f area: %6.3f)\n", min_dist_almt, options.almtname[min_dist_almt], min_dist, max_area_at_min_dist); free (cluster_count_per_size); free_matrix ( (void **) cluster); } free (score); logger ( &options, NOTE, ""); return 0; }
int main(int argc,char**argv) { //this is now an array mtt::TargetList targetList; t_config config; t_data full_data; t_flag flags; vector<t_clustersPtr> clusters; vector<t_objectPtr> object; vector<t_listPtr> list; visualization_msgs::MarkerArray markersMsg; // Initialize ROS init(argc,argv,"mtt"); NodeHandle nh("~"); Subscriber subs_points = nh.subscribe("/points", 1000, PointsHandler); Publisher target_publisher = nh.advertise<mtt::TargetList>("/targets", 1000); Publisher markers_publisher= nh.advertise<visualization_msgs::MarkerArray>("/markers", 1000); Publisher arrow_publisher= nh.advertise<visualization_msgs::MarkerArray>("/arrows", 1000); init_flags(&flags); //Inits flags values init_config(&config); //Inits configuration values cout<<"Start to spin"<<endl; Rate r(100); while(ok()) { spinOnce(); r.sleep(); if(!new_data) continue; new_data=false; //Get data from PointCloud2 to full_data PointCloud2ToData(pointData,full_data); //clustering clustering(full_data,clusters,&config,&flags); //calc_cluster_props calc_cluster_props(clusters,full_data); //clusters2objects clusters2objects(object,clusters,full_data,config); calc_object_props(object); //AssociateObjects AssociateObjects(list,object,config,flags); //MotionModelsIteration MotionModelsIteration(list,config); //cout<<"Number of targets "<<list.size()<<endl; clean_objets(object);//clean current objects //clear target list array targetList.Targets.clear(); //structure to be fed to array mtt::Target target; //build header target.header.stamp = ros::Time::now(); target.header.frame_id = pointData.header.frame_id; //trying to draw arrows visualization_msgs::MarkerArray arrow_arrray; for(uint i=0; i<list.size(); i++) { geometry_msgs::Pose pose; geometry_msgs::Twist vel; //header target.header.seq = list[i]->id; target.id = list[i]->id; //velocity vel.linear.x=list[i]->velocity.velocity_x; vel.linear.y=list[i]->velocity.velocity_y; vel.linear.z=0; target.velocity = vel; //compute orientation based on velocity. double theta = atan2(vel.linear.y,vel.linear.x); geometry_msgs::Quaternion target_orientation = tf::createQuaternionMsgFromYaw(theta); //pose pose.position.x = list[i]->position.estimated_x; pose.position.y = list[i]->position.estimated_y; pose.position.z = 0; pose.orientation = target_orientation; target.pose = pose; //feed array with current target //condition to accept as valit target (procopio change) double velocity = sqrt(pow(vel.linear.x,2)+ pow(vel.linear.y,2)); if(/*velocity > 0.5 &&*/ velocity < 3.0) targetList.Targets.push_back(target); ////////////////////////// //drawing arrow part if(list[i]->velocity.velocity_module > 0.2 && list[i]->velocity.velocity_module < 5.0) { visualization_msgs::Marker arrow_marker; arrow_marker.type = visualization_msgs::Marker::ARROW; arrow_marker.action = visualization_msgs::Marker::ADD; // Set the frame ID and timestamp. arrow_marker.header.frame_id = pointData.header.frame_id; arrow_marker.header.stamp = ros::Time::now(); arrow_marker.color.r = 0; arrow_marker.color.g = 1; arrow_marker.color.b = 0; arrow_marker.color.a = 1; // arrow_marker.scale.x = ; arrow_marker.scale.x = list[i]->velocity.velocity_module; //length arrow_marker.scale.y = 0.1; //width arrow_marker.scale.z = 0.1; //height arrow_marker.lifetime = ros::Duration(1.0); arrow_marker.id = list[i]->id; // Set the pose of the arrow_marker. This is a full 6DOF pose relative to the frame/time specified in the header arrow_marker.pose = pose; arrow_arrray.markers.push_back(arrow_marker); ////////////////////////// } } target_publisher.publish(targetList); CreateMarkers(markersMsg.markers,targetList,list); markers_publisher.publish(markersMsg); arrow_publisher.publish(arrow_arrray); flags.fi=false; } return 0; }
int main(int argc,char**argv) { mtt::TargetListPC targetList; t_config config; t_data full_data; t_flag flags; vector<t_clustersPtr> clusters; vector<t_objectPtr> object; vector<t_listPtr> list; visualization_msgs::MarkerArray markersMsg; // Initialize ROS init(argc,argv,"mtt"); NodeHandle nh("~"); Subscriber subs_points = nh.subscribe("/points", 1000, PointsHandler); Publisher target_publisher = nh.advertise<mtt::TargetList>("/targets", 1000); Publisher markers_publisher= nh.advertise<visualization_msgs::MarkerArray>("/markers", 1000); Publisher marker_publisher= nh.advertise<visualization_msgs::Marker>("/marker", 1000); init_flags(&flags); //Inits flags values init_config(&config); //Inits configuration values cout<<"Start to spin"<<endl; Rate r(100); while(ok()) { spinOnce(); r.sleep(); if(!new_data) continue; new_data=false; //Get data from PointCloud2 to full_data PointCloud2ToData(pointData,full_data); //clustering clustering(full_data,clusters,&config,&flags); //calc_cluster_props calc_cluster_props(clusters,full_data); //clusters2objects clusters2objects(object,clusters,full_data,config); calc_object_props(object); //AssociateObjects AssociateObjects(list,object,config,flags); //MotionModelsIteration MotionModelsIteration(list,config); // cout<<"Number of targets "<<list.size()<<endl; clean_objets(object);//clean current objects targetList.id.clear(); targetList.obstacle_lines.clear();//clear all lines pcl::PointCloud<pcl::PointXYZ> target_positions; pcl::PointCloud<pcl::PointXYZ> velocity; target_positions.header.frame_id = pointData.header.frame_id; velocity.header.frame_id = pointData.header.frame_id; targetList.header.stamp = ros::Time::now(); targetList.header.frame_id = pointData.header.frame_id; for(uint i=0;i<list.size();i++) { targetList.id.push_back(list[i]->id); pcl::PointXYZ position; position.x = list[i]->position.estimated_x; position.y = list[i]->position.estimated_y; position.z = 0; target_positions.points.push_back(position); pcl::PointXYZ vel; vel.x=list[i]->velocity.velocity_x; vel.y=list[i]->velocity.velocity_y; vel.z=0; velocity.points.push_back(vel); pcl::PointCloud<pcl::PointXYZ> shape; pcl::PointXYZ line_point; uint j; for(j=0;j<list[i]->shape.lines.size();j++) { line_point.x=list[i]->shape.lines[j]->xi; line_point.y=list[i]->shape.lines[j]->yi; shape.points.push_back(line_point); } line_point.x=list[i]->shape.lines[j-1]->xf; line_point.y=list[i]->shape.lines[j-1]->yf; sensor_msgs::PointCloud2 shape_cloud; pcl::toROSMsg(shape,shape_cloud); targetList.obstacle_lines.push_back(shape_cloud); } pcl::toROSMsg(target_positions, targetList.position); pcl::toROSMsg(velocity, targetList.velocity); target_publisher.publish(targetList); CreateMarkers(markersMsg.markers,targetList,list); //markersMsg.header.frame_id=targetList.header.frame_id; markers_publisher.publish(markersMsg); flags.fi=false; } return 0; }