int sac_ia_alignment(pcl::PointCloud<pcl::PointXYZ>::Ptr prev_cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in) { pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud(new pcl::PointCloud<pcl::PointXYZ>(*prev_cloud)); std::cout << "Performing Sample Consensus Initial Alignment.. " << std::endl; FeatureCloud targetCloud; FeatureCloud templateCloud; targetCloud.setInputCloud(temp_cloud); templateCloud.setInputCloud(cloud_in); TemplateAlignment templateAlign; templateAlign.addTemplateCloud(templateCloud); templateAlign.setTargetCloud(targetCloud); Result bestAlignment; std::cout << "entering alignment" << std::endl; templateAlign.findBestAlignment(bestAlignment); std::cout << "exiting alignment" << std::endl; printf("Fitness Score: %f \n", bestAlignment.fitness_score); Eigen::Matrix3f rotation = bestAlignment.final_transformation.block<3,3>(0, 0); Eigen::Vector3f translation = bestAlignment.final_transformation.block<3,1>(0, 3); printf("\n"); printf(" | %6.3f %6.3f %6.3f | \n", rotation(0, 0), rotation(0, 1), rotation(0, 2)); printf("R = | %6.3f %6.3f %6.3f | \n", rotation(1, 0), rotation(1, 1), rotation(1, 2)); printf(" | %6.3f %6.3f %6.3f | \n", rotation(2, 0), rotation(2, 1), rotation(2, 2)); printf("\n"); printf("t = < %0.3f, %0.3f, %0.3f >\n", translation(0), translation(1), translation(2)); pcl::transformPointCloud(*templateCloud.getPointCloud(), *cloud_in, bestAlignment.final_transformation); std::cout << "***Initial alignment complete***" << std::endl; }
// Align a collection of object templates to a sample point cloud void cloud_cb( const sensor_msgs::PointCloud2ConstPtr& input ) { if( controllerState != 1 ) return; //--- Convert Incoming Cloud --- // pcl::PointCloud<pcl::PointXYZ>::Ptr cloud( new pcl::PointCloud<pcl::PointXYZ> ); pcl::fromROSMsg( *input, *cloud ); // --- Z-Filter And Downsample Cloud --- // // Preprocess the cloud by removing distant points pcl::PassThrough<pcl::PointXYZ> pass_z; pass_z.setInputCloud( cloud ); pass_z.setFilterFieldName( "z" ); pass_z.setFilterLimits( 0, 1.75 ); pass_z.filter( *cloud ); pcl::PassThrough<pcl::PointXYZ> pass_y; pass_y.setInputCloud( cloud ); pass_y.setFilterFieldName("y"); pass_y.setFilterLimits( -0.5, 0.2 ); pass_y.filter( *cloud ); pcl::PassThrough<pcl::PointXYZ> pass_x; pass_x.setInputCloud( cloud ); pass_x.setFilterFieldName("x"); pass_x.setFilterLimits( -0.5, 0.5 ); pass_x.filter( *cloud ); // It is possible to not have any points after z-filtering (ex. if we are looking up). // Just bail if there is nothing left. if( cloud->points.size() == 0 ) return; //visualize( cloud, visualizer_o_Ptr ); // --- Calculate Scene Normals --- // pcl::PointCloud<pcl::Normal>::Ptr pSceneNormals( new pcl::PointCloud<pcl::Normal>() ); pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> normEst; normEst.setKSearch(10); normEst.setInputCloud( cloud ); normEst.compute( *pSceneNormals ); // --- Get Rid Of Floor --- // pcl::PointIndices::Ptr inliers_plane( new pcl::PointIndices ); pcl::ModelCoefficients::Ptr coefficients_plane( new pcl::ModelCoefficients ); pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> seg1; seg1.setOptimizeCoefficients( true ); seg1.setModelType( pcl::SACMODEL_NORMAL_PLANE ); seg1.setNormalDistanceWeight( 0.05 ); seg1.setMethodType( pcl::SAC_RANSAC ); seg1.setMaxIterations( 100 ); seg1.setDistanceThreshold( 0.075 ); seg1.setInputCloud( cloud ); seg1.setInputNormals( pSceneNormals ); // Obtain the plane inliers and coefficients seg1.segment( *inliers_plane, *coefficients_plane ); // Extract the planar inliers from the input cloud pcl::ExtractIndices<pcl::PointXYZ> extract; extract.setInputCloud( cloud ); extract.setIndices( inliers_plane ); extract.setNegative( false ); // Write the planar inliers to disk pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane( new pcl::PointCloud<pcl::PointXYZ> ); extract.filter( *cloud_plane ); // Remove the planar inliers, extract the rest pcl::PointCloud<pcl::PointXYZ>::Ptr filteredScene( new pcl::PointCloud<pcl::PointXYZ> ); extract.setNegative( true ); extract.filter( *filteredScene ); pcl::ExtractIndices<pcl::Normal> extract_normals; pcl::PointCloud<pcl::Normal>::Ptr filteredSceneNormals( new pcl::PointCloud<pcl::Normal> ); extract_normals.setNegative( true ); extract_normals.setInputCloud( pSceneNormals ); extract_normals.setIndices( inliers_plane ); extract_normals.filter( *filteredSceneNormals ); if( filteredScene->points.size() == 0 ) return; // --- Set Our Target Cloud --- // // Assign to the target FeatureCloud FeatureCloud target_cloud; target_cloud.setInputCloud( filteredScene ); // --- Visualize the Filtered Cloud --- // //visualize( filteredScene, visualizer_o_Ptr ); // --- Set Input Cloud For Alignment --- // template_align.setTargetCloud( target_cloud ); // --- Align Templates --- // std::cout << "Searching for best fit" << std::endl; // Find the best template alignment TemplateAlignment::Result best_alignment; int best_index = template_align.findBestAlignment( best_alignment ); std::cerr << "Best alignment index: " << best_index << std::endl; const FeatureCloud &best_template = object_templates[best_index]; // --- Report Best Match --- // // Print the alignment fitness score (values less than 0.00002 are good) std::cerr << "Best fitness score: " << best_alignment.fitness_score << std::endl; // Print the rotation matrix and translation vector Eigen::Matrix3f rotation = best_alignment.final_transformation.block<3,3>(0, 0); Eigen::Vector3f translation = best_alignment.final_transformation.block<3,1>(0, 3); std::cerr << std::setprecision(3); std::cerr << std::endl; std::cerr << " | " << rotation(0,0) << " " << rotation(0,1) << " " << rotation(0,2) << " | " << std::endl; std::cerr << "R = | " << rotation(1,0) << " " << rotation(1,1) << " " << rotation(1,2) << " | " << std::endl; std::cerr << " | " << rotation(2,0) << " " << rotation(2,1) << " " << rotation(2,2) << " | " << std::endl; std::cerr << std::endl; std::cerr << "t = < " << translation(0) << ", " << translation(1) << ", " << translation(2) << " >" << std::endl << std::endl; // pcl::PointCloud<pcl::PointXYZ> transformedCloud; // pcl::transformPointCloud( *best_template.getPointCloud(), transformedCloud, best_alignment.final_transformation); // visualize( filteredScene, transformedCloud.makeShared(), visualizer_o_Ptr ); // --- Publish --- // // TODO: Clean up this part. geometry_msgs::Pose pose; tf::Matrix3x3 rot( rotation(0,0), rotation(0,1), rotation(0,2), rotation(1,0), rotation(1,1), rotation(1,2), rotation(2,0), rotation(2,1), rotation(2,2) ); tf::Quaternion q; rot.getRotation(q); pose.orientation.w = q.getW(); pose.orientation.x = q.getX(); pose.orientation.y = q.getY(); pose.orientation.z = q.getZ(); tf::Vector3 t( translation(0), translation(1), translation(2) ); pose.position.x = t.getX(); pose.position.y = t.getY(); pose.position.z = t.getZ(); std::cerr << "Publishing" << std::endl; pub.publish(pose); sensor_msgs::PointCloud2 toPub; pcl::toROSMsg( *filteredScene, toPub ); cloud_pub.publish(toPub); }