void WorldDownloadManager::cropCloudWithSphere(const Eigen::Vector3f & center,const float radius,
typename pcl::PointCloud<PointT>::ConstPtr cloud,typename pcl::PointCloud<PointT>::Ptr out_cloud)
{
const uint cloud_size = cloud->size();
std::vector<bool> valid_points(cloud_size,true);
// check the points
for (uint i = 0; i < cloud_size; i++)
{
const PointT & pt = (*cloud)[i];
const Eigen::Vector3f ept(pt.x,pt.y,pt.z);
if ((ept - center).squaredNorm() > radius * radius)
valid_points[i] = false;
}
// discard invalid points
out_cloud->clear();
out_cloud->reserve(cloud_size);
uint count = 0;
for (uint i = 0; i < cloud_size; i++)
if (valid_points[i])
{
out_cloud->push_back((*cloud)[i]);
count++;
}
out_cloud->resize(count);
}
void WorldDownloadManager::cropCloud(const Eigen::Vector3f & min,const Eigen::Vector3f & max,
typename pcl::PointCloud<PointT>::ConstPtr cloud,typename pcl::PointCloud<PointT>::Ptr out_cloud)
{
const uint cloud_size = cloud->size();
std::vector<bool> valid_points(cloud_size,true);
// check the points
for (uint i = 0; i < cloud_size; i++)
{
const PointT & pt = (*cloud)[i];
if (pt.x > max.x() || pt.y > max.y() || pt.z > max.z() ||
pt.x < min.x() || pt.y < min.y() || pt.z < min.z())
valid_points[i] = false;
}
// discard invalid points
out_cloud->clear();
out_cloud->reserve(cloud_size);
uint count = 0;
for (uint i = 0; i < cloud_size; i++)
if (valid_points[i])
{
out_cloud->push_back((*cloud)[i]);
count++;
}
out_cloud->resize(count);
}
void WorldDownloadManager::cropMesh(const kinfu_msgs::KinfuCloudPoint & min,
const kinfu_msgs::KinfuCloudPoint & max,PointCloud::ConstPtr cloud,
TrianglesConstPtr triangles,PointCloud::Ptr out_cloud,TrianglesPtr out_triangles)
{
const uint triangles_size = triangles->size();
const uint cloud_size = cloud->size();
std::vector<bool> valid_points(cloud_size,true);
std::vector<uint> valid_points_remap(cloud_size,0);
std::cout << "Starting with " << cloud_size << " points and " << triangles_size << " triangles.\n";
uint offset;
// check the points
for (uint i = 0; i < cloud_size; i++)
{
const pcl::PointXYZ & pt = (*cloud)[i];
if (pt.x > max.x || pt.y > max.y || pt.z > max.z ||
pt.x < min.x || pt.y < min.y || pt.z < min.z)
valid_points[i] = false;
}
// discard invalid points
out_cloud->clear();
out_cloud->reserve(cloud_size);
offset = 0;
for (uint i = 0; i < cloud_size; i++)
if (valid_points[i])
{
out_cloud->push_back((*cloud)[i]);
// save new position for triangles remap
valid_points_remap[i] = offset;
offset++;
}
out_cloud->resize(offset);
// discard invalid triangles
out_triangles->clear();
out_triangles->reserve(triangles_size);
offset = 0;
for (uint i = 0; i < triangles_size; i++)
{
const kinfu_msgs::KinfuMeshTriangle & tri = (*triangles)[i];
bool is_valid = true;
// validate all the vertices
for (uint h = 0; h < 3; h++)
if (!valid_points[tri.vertex_id[h]])
{
is_valid = false;
break;
}
if (is_valid)
{
kinfu_msgs::KinfuMeshTriangle out_tri;
// remap the triangle
for (uint h = 0; h < 3; h++)
out_tri.vertex_id[h] = valid_points_remap[(*triangles)[i].vertex_id[h]];
out_triangles->push_back(out_tri);
offset++;
}
}
out_triangles->resize(offset);
std::cout << "Ended with " << out_cloud->size() << " points and " << out_triangles->size() << " triangles.\n";
}
int valid_main( int argc, char *argv[] )
{
char *fout;
int i, j,k,iethpo, pothie;
FILE *outfp = 0;
printf("\nPORTA - a POlyhedron Representation Transformation Algorithm\n");
printf( "Version %s\n\n", VERSION );
printf( "Written by Thomas Christof (Uni Heidelberg)\n" );
printf( "Revised by Andreas Loebel (ZIB Berlin)\n\n" );
printf( "PORTA is free software and comes with ABSOLUTELY NO WARRENTY! You are welcome\n" );
printf( "to use, modify, and redistribute it under the GNU General Public Lincese.\n\n" );
printf( "This is the program VALID from the PORTA package.\n\n" );
if( argc <= 2 )
{
fprintf( stderr,
"For more information read the manpages about porta.\n\n");
exit(-1);
}
/* 17.01.1994: include logging on file porta.log */
if( !(logfile = fopen( "porta.log", "a" )) )
fprintf( stderr, "can't open logfile porta.log\n" );
else
{
porta_log( "\n\n\nlog for " );
for( i = 0; i < argc; i++ )
porta_log( "%s ", argv[i] );
porta_log( "\n\n" );
}
initialize();
set_I_functions();
SET_MP_not_ready;
prt = stdout;
get_options(&argc,&argv);
if (option & Protocol_to_file)
{
strcat(*argv,".prt");
prt = fopen(*argv,"w");
(*argv)[strlen(*argv)-4] = '\0';
}
setbuf(prt,CP 0);
if (is_set(Vint) && !strcmp(*argv+strlen(*argv)-4,".ieq"))
{
char *cp1,*cp2;
cp1=strdup("LOWER_BOUNDS");
cp2=strdup("UPPER_BOUNDS");
if(!cp1||!cp2)
msg("allocation of new space failed", "", 0 );
points = read_input_file(*argv,outfp,&dim,&ar1,(int *)&nel_ar1,
cp1,&lowbds,cp2,&upbds,"\0",(RAT **)&i);
free(cp1);free(cp2);
sort_eqie_cvce(ar1,points,dim+2,&equa,&ineq);
valid_ints( dim, ar1, equa, dim+2, dim,
ar1+(dim+2)*equa, ineq, dim+2, *argv );
}
else
{
pothie = !strcmp(*argv+strlen(*argv)-4,".poi")
&& !strcmp(*(argv+1)+strlen(*(argv+1))-4,".ieq");
iethpo = !strcmp(*argv+strlen(*argv)-4,".ieq")
&& !strcmp(*(argv+1)+strlen(*(argv+1))-4,".poi");
if (!iethpo && !pothie)
msg("invalid format of command line", "", 0 );
if (iethpo)
{
ineq = read_input_file(*argv,outfp,&dim,&ar1,(int *)&nel_ar1,
"\0",(int **)&i,"\0",(int **)&i,"\0",(RAT **)&i );
points = read_input_file(*(argv+1),outfp,&j,&ar2,(int *)&nel_ar2,
"\0",(int **)&i,"\0",(int **)&i,"\0",(RAT **)&i );
}
else
{
ineq = read_input_file(*(argv+1),outfp,&dim,&ar1, (int *)&nel_ar1,
"\0", (int **)&i,"\0", (int **)&i,"\0", (RAT **)&i );
points = read_input_file(*argv,outfp,&j,&ar2, (int *)&nel_ar2,
"\0", (int **)&i,"\0", (int **)&i,"\0", (RAT **)&i );
}
if (j != dim)
msg("dimensions in input files are different", "", 0 );
if (is_set(Iespo))
{
sort_eqie_cvce(ar1,ineq,dim+2,&equa,&ineq);
valid_ieqs(dim,ar1,ineq+equa,&equa,&ineq,dim+2,
ar2,points,dim+1,(char **)(pothie?*argv:*(argv+1)));
}
else if (is_set(Posie) || is_set(Cfctp))
{
fout = iethpo?*argv:*(argv+1);
sort_eqie_cvce(ar2,points,dim+1,&cone,&conv);
if (is_set(Posie))
valid_points(dim,ar2,points,dim+1,
ar1,ineq,dim+2,0,(char **)fout);
else
{
char fname[100];
char command[100];
RAT *ar1p;
sort_eqie_cvce(ar1,ineq,dim+2,&equa,&ineq);
if (equa )
msg( "only inequalities are allowed", "" , 0 );
for (i = 0,ar1p = ar1; i < ineq; i++, ar1p += dim+2)
{
sprintf(fname,"%s%i",fout,i+1);
fprintf(prt,"I n e q u a l i t y %2i :\n",i+1);
fprintf(prt,"==========================\n\n");
/* 17.01.1994: include logging on file porta.log */
porta_log( "I n e q u a l i t y %2i :\n",i+1);
porta_log( "==========================\n\n");
fprintf(prt,"computing points i n v a l i d for the inequality :\n");
fprintf(prt,"=====================================================\n");
/* 17.01.1994: include logging on file porta.log */
porta_log( "computing points i n v a l i d for the inequality :\n");
porta_log( "=====================================================\n");
var[3].num = -1; var[3].den.i = 1;
scal_mul(var+3,ar1p,ar1p,dim+1);
j = valid_points(dim,ar2,points,dim+1,
ar1p,1,dim+2,1, (char **)fname);
(ar1p+dim+1)->num = 0;
fprintf(prt,"computing points satisfying inequalitiy with equation :\n");
fprintf(prt,"=======================================================\n");
/* 17.01.1994: include logging on file porta.log */
porta_log( "computing points satisfying inequalitiy with equation :\n");
porta_log( "=======================================================\n");
if (j) strcat(fname,".poi");
k = valid_points(dim,ar2,points,dim+1,
ar1p,1,dim+2,0, (char **)fname);
strcat(fname,".poi");
if (!j && k)
{
sprintf(command,"dim %s",fname);
fprintf(prt,"dimension of the points :\n");
fprintf(prt,"=========================\n");
fflush(prt);
/* 17.01.1994: include logging on file porta.log */
porta_log( "dimension of the points :\n");
porta_log( "=========================\n");
fflush( logfile );
system(command);
}
}
}
}
else
msg( "invalid format of command line", "", 0 );
}
return 0;
}
