int main(int argc, char **argv, char **envp) {
int ** matriz_adj = NULL;
int N;
int i;
if (argc != 2) {
printUsage();
exit(1);
}
if (strlen(argv[1]) < 4 || strcmp(argv[1] + strlen(argv[1]) - 4, ".adj") != 0) {
printUsage();
exit(1);
}
if (readMatrixFromFile(argv[1], &matriz_adj, &N)) {
printf("Erro na leitura da matriz do ficheiro!!!\n");
exit(3);
}
printf("Densidade do grafo: %.4f\n", getGraphDensity(matriz_adj, N));
printf("Grau maximo do grafo: %d\n", getGraphHighRank(matriz_adj, N));
writeMatrixToFile(argv[1], matriz_adj, N);
for (i=0;i<N;i++)
free(matriz_adj[i]);
free(matriz_adj);
return 0;
}
void reconfig(scene_processing::pcmergerConfig & config, uint32_t level) {
conf = config;
boost::recursive_mutex::scoped_lock lock(global_mutex);
// pcl::PointCloud<PointT>::Ptr prev_cloud_ptr(new pcl::PointCloud<PointT > ());
// pcl::PointCloud<PointT>::Ptr new_cloud_ptr(new pcl::PointCloud<PointT > ());
if(conf.add_pc) {
//conf.add_pc = false;
doUpdate = true;
*cloud_new_ptr = *cloud_mod_ptr;
*cloud_merged_backup_ptr=*cloud_merged_ptr;
if(Merged)
{
*cloud_merged_ptr += *cloud_new_ptr;
Matrix4f globalTrans=computeTransformXYZYPR(config.x, config.y, config.z, config.yaw/180.0*PI, config.pitch/180.0*PI, config.roll/180.0*PI);
writeMatrixToFile(globalTrans);
}
else
*cloud_merged_ptr = *cloud_new_ptr;
//*cloud_merged_ptr =*cloud_new_ptr;
viewer.removePointCloud("new");
if(Merged)
viewer.removePointCloud("merged");
Merged = true;
pcl::toROSMsg(*cloud_merged_ptr, cloud_blobc_merged);
color_handler_merged.reset(new pcl_visualization::PointCloudColorHandlerRGBField<sensor_msgs::PointCloud2 > (cloud_blobc_merged));
viewer.addPointCloud(*cloud_merged_ptr, color_handler_merged, "merged", viewportOrig);
ROS_INFO("displaying mergered pointcloud");
}
if(conf.setIT) {
std::string transformsFileName=fn+".transforms.txt";
transformFile.open(transformsFileName.data());
conf.setIT = false;
doUpdate = true;
InitialTransformConfig = conf;
Matrix4f globalTrans=computeTransformXYZYPR(InitialTransformConfig.x, InitialTransformConfig.y, InitialTransformConfig.z, InitialTransformConfig.yaw/180.0*PI, InitialTransformConfig.pitch/180.0*PI, InitialTransformConfig.roll/180.0*PI);
writeMatrixToFile(globalTrans);
*cloud_new_ptr = *cloud_mod_ptr;
ITpresent = true;
}
if(config.undo)
{
if(noMoreUndo)
{
conf.undo=false;
doUpdate=true;
return;
}
noMoreUndo=true;
transformFile<<"endo"<<endl;
*cloud_merged_ptr=*cloud_merged_backup_ptr;
if(Merged)
viewer.removePointCloud("merged");
Merged = true;
pcl::toROSMsg(*cloud_merged_ptr, cloud_blobc_merged);
color_handler_merged.reset(new pcl_visualization::PointCloudColorHandlerRGBField<sensor_msgs::PointCloud2 > (cloud_blobc_merged));
viewer.addPointCloud(*cloud_merged_ptr, color_handler_merged, "merged", viewportOrig);
ROS_INFO("undo:displaying mergered pointcloud");
conf.undo=false;
doUpdate=true;
if (pcl::io::loadPCDFile (std::string("tempAppend.pcd"), cloud_blobc_new) == -1)
{
ROS_ERROR ("Couldn't read file ");
return ;
}
// ROS_INFO ("Loaded %d data points from %s with the following fields: %s", (int)(cloud_blob.width * cloud_blob.height), argv[1] ,pcl::getFieldsList (cloud_blob).c_str ());
// Convert to the templated message type
pcl::fromROSMsg (cloud_blobc_new, *cloud_new_ptr);
// pcl::PointCloud<PointT>::Ptr cloud_ptr (new pcl::PointCloud<PointT> (cloud));
if(ITpresent){
cout<<"inside IT"<<endl;
transformXYZYPR<PointT>(*cloud_new_ptr, *cloud_mod_ptr, InitialTransformConfig.x, InitialTransformConfig.y, InitialTransformConfig.z, InitialTransformConfig.yaw/180.0*PI, InitialTransformConfig.pitch/180.0*PI, InitialTransformConfig.roll/180.0*PI);
*cloud_new_ptr = *cloud_mod_ptr;
conf.pitch=0;
conf.yaw=0;
conf.roll=0;
}
//ROS_INFO("PointCloud with %d data points and frame %s (%f) received.", (int) cloud_new_ptr->points.size(), cloud_new_ptr->header.frame_id.c_str(), cloud_new_ptr->header.stamp.toSec());
viewer.removePointCloud("new");
// pcl::toROSMsg<PointT>(*cloud_new_ptr, cloud_blobc_new);
color_handler_new.reset(new pcl_visualization::PointCloudColorHandlerRGBField<sensor_msgs::PointCloud2 > (cloud_blobc_new));
viewer.addPointCloud(*cloud_new_ptr, color_handler_new, "new", viewportOrig);
ROS_INFO("undo:displaying new point cloud");
conf.x=0;
conf.y=0;
conf.z=0;
conf.yaw=0;
conf.pitch=0;
conf.roll=0;
}
if(conf.skip_pc || conf.add_pc)
{
if (conf.skip_pc)
{
conf.x = -1;
conf.y = -0.3;
conf.z = 0.78;
conf.yaw = 0;
conf.pitch = 30;
conf.roll = 0;
}
else
{
conf.x = -0.1;
conf.y = -0.8;
conf.z = 0.019;
conf.yaw = 175;
conf.pitch = 0;
conf.roll = 0;
}
noMoreUndo=false;
conf.skip_pc = false;
conf.add_pc =false;
doUpdate = true;
int count = 0;
cloud_blob_prev = cloud_blob_new;
cloud_blob_new = reader.getNextCloud();
cout<<"header"<<cloud_blob_new->header<<endl;
// cloud_blob_new->
ros::M_string::iterator iter;
//for(iter=cloud_blob_new->__connection_header->begin ();iter!=cloud_blob_new->__connection_header->end ();iter++)
// cout<<iter->first<<","<<iter->second<<endl;
while(count < skipNum && cloud_blob_prev != cloud_blob_new)
{
cloud_blob_prev = cloud_blob_new;
cloud_blob_new = reader.getNextCloud();
count ++;
}
if (cloud_blob_prev != cloud_blob_new) {
pcl::fromROSMsg(*cloud_blob_new, *cloud_temp_ptr);
cloud_temp_ptr->header;
cout<<"numPoints="<<cloud_temp_ptr->size ()<<endl;
appendCamIndexAndDistance (cloud_temp_ptr,cloud_new_ptr,globalFrameCount,VectorG(0,0,0));
globalFrameCount++;
cloud_new_ptr->width=1;
cloud_new_ptr->height=cloud_new_ptr->size();
writer.write (std::string("tempAppend.pcd"),*cloud_new_ptr,true);
if (pcl::io::loadPCDFile (std::string("tempAppend.pcd"), cloud_blobc_new) == -1)
{
ROS_ERROR ("Couldn't read temp file ");
return ;
}
// ROS_INFO ("Loaded %d data points from %s with the following fields: %s", (int)(cloud_blob.width * cloud_blob.height), argv[1] ,pcl::getFieldsList (cloud_blob).c_str ());
// Convert to the templated message type
pcl::fromROSMsg (cloud_blobc_new, *cloud_new_ptr);
// pcl::PointCloud<PointT>::Ptr cloud_ptr (new pcl::PointCloud<PointT> (cloud));
if(ITpresent){
cout<<"inside IT"<<endl;
transformXYZYPR<PointT>(*cloud_new_ptr, *cloud_mod_ptr, InitialTransformConfig.x, InitialTransformConfig.y, InitialTransformConfig.z, InitialTransformConfig.yaw/180.0*PI, InitialTransformConfig.pitch/180.0*PI, InitialTransformConfig.roll/180.0*PI);
*cloud_new_ptr = *cloud_mod_ptr;
// conf.pitch=0;
// conf.yaw=0;
// conf.roll=0;
}
//ROS_INFO("PointCloud with %d data points and frame %s (%f) received.", (int) cloud_new_ptr->points.size(), cloud_new_ptr->header.frame_id.c_str(), cloud_new_ptr->header.stamp.toSec());
viewer.removePointCloud("new");
// pcl::toROSMsg<PointT>(*cloud_new_ptr, cloud_blobc_new);
color_handler_new.reset(new pcl_visualization::PointCloudColorHandlerRGBField<sensor_msgs::PointCloud2 > (cloud_blobc_new));
viewer.addPointCloud(*cloud_new_ptr, color_handler_new, "new", viewportOrig);
ROS_INFO("displaying new point cloud");
/* if(Merged){
viewer.removePointCloud("merged");
pcl::toROSMsg(*cloud_merged_ptr, cloud_blobc_merged);
color_handler_merged.reset(new pcl_visualization::PointCloudColorHandlerRGBField<sensor_msgs::PointCloud2 > (cloud_blobc_merged));
viewer.addPointCloud(*cloud_merged_ptr, color_handler_merged, "merged", viewportOrig);
}*/
}else {
ROS_INFO("Finised reading all pointclouds! Select done to save.");
}
}
if(conf.set_skip){
conf.set_skip = false;
doUpdate = true;
skipNum = (conf.skipNum);
}
if(conf.update)
{
conf.update = false;
doUpdate = true;
updateUI();
}
}
