void pointpickingEventOccurred (const pcl::visualization::PointPickingEvent &event, void* viewer_void)
{
int indx;
float x,y,z;
indx = event.getPointIndex();
if(indx == -1)
return;
std::cout << indx << " " << std::endl;
event.getPoint(x,y,z);
first.x = x;
first.y = y;
first.z = z;
std::cout<<"First point "<<first.x<<" , "<<first.y<<" , "<<first.z<<std::endl;
// Green region detection
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr final_cloud (new pcl::PointCloud<pcl::PointXYZRGBA>);
final_cloud->width = cloud->width;
final_cloud->height = cloud->height;
final_cloud->resize (cloud->width * cloud->height);
//
}
int CloudStitcher::stitchPCDFiles( const std::string directory_path )
{
//do some basic error handling first and make sure the user gave us an output path to work with
if( this->output_path == "" )
{
this->sendOutput( "Error: No output path has been specified yet.\n" , true );
return -1;
}
//lets open up the directory and see how many files are in there
int result = this->setPCDDirectory( directory_path );
if( result != 0 )
{
return -1;
}
unsigned int file_count = this->getNumberofFilesRead();
//if there is only one file in the given directory, then stitching is not really necessary.
if( file_count == 1 )
{
this->sendOutput( "Error: Only 1 .pcd file was found in the directory. Stitching is not necessary.\n" , true );
return -1;
}
//get all our threads setup with work they need to do
switch( this->num_threads )
{
case THREAD_1:
this->setupWorkerThreads( 1 , this->getNumberofFilesRead() );
break;
case THREAD_2:
this->setupWorkerThreads( 2 , this->getNumberofFilesRead() );
break;
case THREAD_4:
this->setupWorkerThreads( 4 , this->getNumberofFilesRead() );
break;
case THREAD_8:
this->setupWorkerThreads( 8 , this->getNumberofFilesRead() );
break;
default:
this->setupWorkerThreads( 1 , this->getNumberofFilesRead() );
break;
}
//just printing some info to the user
std::stringstream output;
output << "Read in " << this->pcd_filenames->size() << " files.\n";
this->sendOutput( output.str() , false );
output.str("");
output << "Spinning up " << this->worker_threads->size() << " threads.\n";
this->sendOutput( output.str() , false );
const clock_t begin_time = std::clock();
//spin up all the allocated threads
for( unsigned int i = 0 ; i < this->worker_threads->size() ; ++i )
{
CloudStitchingThread* temp = this->worker_threads->at( i );
temp->start();
}
unsigned int active_thread_count = this->worker_threads->size();
//We spin in this while loop until all threads have reported they have finished work.
while( active_thread_count > 0 )
{
active_thread_count = this->worker_threads->size();
for( std::vector< CloudStitchingThread* >::iterator itr = this->worker_threads->begin() ; itr != this->worker_threads->end() ; ++itr )
{
CloudStitchingThread* temp = *itr;
if( temp->isFinished() )
{
active_thread_count--;
}
}
}
this->sendOutput( "-------All threads finished--------\n" , false );
PointCloud global_cloud;
PointCloud current_cloud;
Eigen::Matrix4f current_transform = Eigen::Matrix4f::Identity();
Eigen::Matrix4f global_transform = Eigen::Matrix4f::Identity();
CloudStitchingThread* current_thread;
current_thread = this->worker_threads->at( 0 );
current_cloud = current_thread->getStitchedPointCloud();
current_transform = current_thread->getFinalCloudTransform();
global_cloud += current_cloud;
global_transform *= current_transform;
for( unsigned int i = 1; i < this->worker_threads->size() ; i++ )
{
current_thread = this->worker_threads->at( i );
current_cloud = current_thread->getStitchedPointCloud();
pcl::transformPointCloud( current_cloud , current_cloud , global_transform );
global_cloud += current_cloud;
current_transform = current_thread->getFinalCloudTransform();
global_transform *= current_transform;
}
PointCloud::Ptr final_cloud( new PointCloud( global_cloud ));
vba::voxelGridFilter( final_cloud , final_cloud , this->filter_leaf_size );
vba::statisticalOutlierFilter( final_cloud , final_cloud , 1.0 , 30 );
std::string pcd_output_path = this->output_path;
pcd_output_path.append( ".pcd" );
if( pcl::io::savePCDFile( pcd_output_path , *final_cloud , true ) == -1 )
{
this->sendOutput( "Error: Could not save final stitched cloud to specified output path.\n" , true );
return -1;
}
std::string ply_output_path = this->output_path;
ply_output_path.append( ".ply" );
if( pcl::io::savePLYFileASCII( ply_output_path , *final_cloud ) == -1 )
{
this->sendOutput( "Error: Could not save final stitched cloud to specified output path.\n" , true );
}
float elapsed_time = float( std::clock() - begin_time ) / CLOCKS_PER_SEC;
output.str("");
output << "Finished in " << elapsed_time << " seconds.\n";
this->sendOutput( output.str() , false );
return 0;
}
