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