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