void My_Filter::scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan){
sensor_msgs::PointCloud2 raw_cloud_msg;
sensor_msgs::PointCloud2 local_cloud_msg;
sensor_msgs::PointCloud2 global_cloud_msg;
PointCloudPtr_t scale_cloud(new PointCloud_t());
PointCloudPtr_t local_cloud(new PointCloud_t());
PointCloudPtr_t global_cloud(new PointCloud_t());
projector_.projectLaser(*scan, raw_cloud_msg);
pcl::fromROSMsg(raw_cloud_msg, *scale_cloud);
for(int i=0; i<scale_cloud->size(); i++)
{
pcl::PointXYZ& point = scale_cloud->at(i);
point.z = std::floor(point.z);
}
this->filterGlobal(scale_cloud, global_cloud);
this->filterLocal(scale_cloud, local_cloud);
pcl::toROSMsg(*global_cloud, global_cloud_msg);
pcl::toROSMsg(*local_cloud, local_cloud_msg);
this->local_cloud_publisher_.publish(local_cloud_msg);
this->global_cloud_publisher_.publish(global_cloud_msg);
}
void CloudStitcher::CloudStitchingThread::stitchTargetClouds()
{
std::stringstream output_stream;
PointCloud::Ptr source( new PointCloud() );
PointCloud::Ptr target( new PointCloud() );
PointCloud::Ptr global_cloud( new PointCloud() );
Eigen::Matrix4f GlobalTransform = Eigen::Matrix4f::Identity();
Eigen::Matrix4f pairTransform = Eigen::Matrix4f::Identity();
for( unsigned int i = 1 ; i < file_list->size() ; i++ )
{
if( vba::openPCDFile( file_list->at( i - 1 ) , source ) == -1 )
continue;
if( vba::openPCDFile( file_list->at( i ) , target ) == -1 )
continue;
//for the algorithms to work correctly, we need all the NaN's to be removed from the clouds
std::vector< int > indices;
pcl::removeNaNFromPointCloud( *source , *source , indices );
pcl::removeNaNFromPointCloud( *target , *target , indices );
//original is 0.05
vba::voxelGridFilter( source , source , this->filter_leaf_size );
vba::voxelGridFilter( target , target , this->filter_leaf_size );
vba::statisticalOutlierFilter( source , source , 1.0 , 30 );
vba::statisticalOutlierFilter( target , target , 1.0 , 30 );
//we input the two clouds to compare and the resulting transformation that lines them up is returned in pairTransform
vba::pairAlign( source, target, pairTransform );
//We are making some changes to the resulting Transformation matrix to try and refine the accuracy of the model.
//Mostly getting rid of x and y translations as well as all rotations.
pairTransform( 0 , 0 ) = 1.0f;
pairTransform( 0 , 1 ) = 0.0f;
pairTransform( 0 , 2 ) = 0.0f;
pairTransform( 0 , 3 ) = 0.0f;
pairTransform( 1 , 0 ) = 0.0f;
pairTransform( 1 , 1 ) = 1.0f;
pairTransform( 1 , 2 ) = 0.0f;
pairTransform( 1 , 3 ) = 0.0f;
pairTransform( 2 , 0 ) = 0.0f;
pairTransform( 2 , 1 ) = 0.0f;
pairTransform( 2 , 2 ) = 1.0f;
//std::cout << "Z-transform: " << pairTransform( 2 , 3 ) << "\n";
//update the global transform with the pairTransform returned from aligning the two clouds
GlobalTransform = GlobalTransform * pairTransform;
//transform the target cloud using the accumulated transformations and the one we just got from pairAlign
pcl::transformPointCloud (*target, *target, GlobalTransform);
//add cloud to the total model. This will append the target's points to the whole cloud we are building up
*global_cloud += *target;
//send some output about the progress of stitching so far
//possible race condition here
output_stream.str( "" );
output_stream << "Registered Cloud " << ++*this->files_finished << "\n";
output_buffer->push( output_stream.str() );
//std::cout << output_stream.str();
}
this->finished_cloud = *global_cloud;
this->cloud_transform = GlobalTransform;
}
