template <typename PointSource, typename PointTarget> void pcl::PPFRegistration<PointSource, PointTarget>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess) { if (!search_method_) { PCL_ERROR("[pcl::PPFRegistration::computeTransformation] Search method not set - skipping computeTransformation!\n"); return; } if (guess != Eigen::Matrix4f::Identity ()) { PCL_ERROR("[pcl::PPFRegistration::computeTransformation] setting initial transform (guess) not implemented!\n"); } PoseWithVotesList voted_poses; std::vector <std::vector <unsigned int> > accumulator_array; accumulator_array.resize (input_->points.size ()); size_t aux_size = static_cast<size_t> (floor (2 * M_PI / search_method_->getAngleDiscretizationStep ())); for (size_t i = 0; i < input_->points.size (); ++i) { std::vector<unsigned int> aux (aux_size); accumulator_array[i] = aux; } PCL_INFO ("Accumulator array size: %u x %u.\n", accumulator_array.size (), accumulator_array.back ().size ()); // Consider every <scene_reference_point_sampling_rate>-th point as the reference point => fix s_r float f1, f2, f3, f4; for (size_t scene_reference_index = 0; scene_reference_index < target_->points.size (); scene_reference_index += scene_reference_point_sampling_rate_) { Eigen::Vector3f scene_reference_point = target_->points[scene_reference_index].getVector3fMap (), scene_reference_normal = target_->points[scene_reference_index].getNormalVector3fMap (); float rotation_angle_sg = acosf (scene_reference_normal.dot (Eigen::Vector3f::UnitX ())); bool parallel_to_x_sg = (scene_reference_normal.y() == 0.0f && scene_reference_normal.z() == 0.0f); Eigen::Vector3f rotation_axis_sg = (parallel_to_x_sg)?(Eigen::Vector3f::UnitY ()):(scene_reference_normal.cross (Eigen::Vector3f::UnitX ()). normalized()); Eigen::AngleAxisf rotation_sg (rotation_angle_sg, rotation_axis_sg); Eigen::Affine3f transform_sg (Eigen::Translation3f ( rotation_sg * ((-1) * scene_reference_point)) * rotation_sg); // For every other point in the scene => now have pair (s_r, s_i) fixed std::vector<int> indices; std::vector<float> distances; scene_search_tree_->radiusSearch (target_->points[scene_reference_index], search_method_->getModelDiameter () /2, indices, distances); for(size_t i = 0; i < indices.size (); ++i) // for(size_t i = 0; i < target_->points.size (); ++i) { //size_t scene_point_index = i; size_t scene_point_index = indices[i]; if (scene_reference_index != scene_point_index) { if (/*pcl::computePPFPairFeature*/pcl::computePairFeatures (target_->points[scene_reference_index].getVector4fMap (), target_->points[scene_reference_index].getNormalVector4fMap (), target_->points[scene_point_index].getVector4fMap (), target_->points[scene_point_index].getNormalVector4fMap (), f1, f2, f3, f4)) { std::vector<std::pair<size_t, size_t> > nearest_indices; search_method_->nearestNeighborSearch (f1, f2, f3, f4, nearest_indices); // Compute alpha_s angle Eigen::Vector3f scene_point = target_->points[scene_point_index].getVector3fMap (); Eigen::Vector3f scene_point_transformed = transform_sg * scene_point; float alpha_s = atan2f ( -scene_point_transformed(2), scene_point_transformed(1)); if (sin (alpha_s) * scene_point_transformed(2) < 0.0f) alpha_s *= (-1); alpha_s *= (-1); // Go through point pairs in the model with the same discretized feature for (std::vector<std::pair<size_t, size_t> >::iterator v_it = nearest_indices.begin (); v_it != nearest_indices.end (); ++ v_it) { size_t model_reference_index = v_it->first, model_point_index = v_it->second; // Calculate angle alpha = alpha_m - alpha_s float alpha = search_method_->alpha_m_[model_reference_index][model_point_index] - alpha_s; unsigned int alpha_discretized = static_cast<unsigned int> (floor (alpha) + floor (M_PI / search_method_->getAngleDiscretizationStep ())); accumulator_array[model_reference_index][alpha_discretized] ++; } } else PCL_ERROR ("[pcl::PPFRegistration::computeTransformation] Computing pair feature vector between points %u and %u went wrong.\n", scene_reference_index, scene_point_index); } } size_t max_votes_i = 0, max_votes_j = 0; unsigned int max_votes = 0; for (size_t i = 0; i < accumulator_array.size (); ++i) for (size_t j = 0; j < accumulator_array.back ().size (); ++j) { if (accumulator_array[i][j] > max_votes) { max_votes = accumulator_array[i][j]; max_votes_i = i; max_votes_j = j; } // Reset accumulator_array for the next set of iterations with a new scene reference point accumulator_array[i][j] = 0; } Eigen::Vector3f model_reference_point = input_->points[max_votes_i].getVector3fMap (), model_reference_normal = input_->points[max_votes_i].getNormalVector3fMap (); float rotation_angle_mg = acosf (model_reference_normal.dot (Eigen::Vector3f::UnitX ())); bool parallel_to_x_mg = (model_reference_normal.y() == 0.0f && model_reference_normal.z() == 0.0f); Eigen::Vector3f rotation_axis_mg = (parallel_to_x_mg)?(Eigen::Vector3f::UnitY ()):(model_reference_normal.cross (Eigen::Vector3f::UnitX ()). normalized()); Eigen::AngleAxisf rotation_mg (rotation_angle_mg, rotation_axis_mg); Eigen::Affine3f transform_mg (Eigen::Translation3f ( rotation_mg * ((-1) * model_reference_point)) * rotation_mg); Eigen::Affine3f max_transform = transform_sg.inverse () * Eigen::AngleAxisf ((static_cast<float> (max_votes_j) - floorf (static_cast<float> (M_PI) / search_method_->getAngleDiscretizationStep ())) * search_method_->getAngleDiscretizationStep (), Eigen::Vector3f::UnitX ()) * transform_mg; voted_poses.push_back (PoseWithVotes (max_transform, max_votes)); } PCL_DEBUG ("Done with the Hough Transform ...\n"); // Cluster poses for filtering out outliers and obtaining more precise results PoseWithVotesList results; clusterPoses (voted_poses, results); pcl::transformPointCloud (*input_, output, results.front ().pose); transformation_ = final_transformation_ = results.front ().pose.matrix (); converged_ = true; }
// Align a rigid object to a scene with clutter and occlusions int main (int argc, char **argv) { // Point clouds PointCloudT::Ptr object (new PointCloudT); PointCloudT::Ptr object_aligned (new PointCloudT); PointCloudT::Ptr scene (new PointCloudT); FeatureCloudT::Ptr object_features (new FeatureCloudT); FeatureCloudT::Ptr scene_features (new FeatureCloudT); // parameter reader readParameters param_reader; param_reader.setConfigureFile("param.cfg"); // Get input object and scene if (argc != 3) { pcl::console::print_error ("Syntax is: %s object.pcd scene.pcd\n", argv[0]); return (1); } // Load object and scene pcl::console::print_highlight ("Loading point clouds...\n"); if (pcl::io::loadPCDFile<PointNT> (argv[1], *object) < 0 || pcl::io::loadPCDFile<PointNT> (argv[2], *scene) < 0) { pcl::console::print_error ("Error loading object/scene file!\n"); return (1); } //------------------------------------------------------------ // pre translation the object model bool pre_transform_p = false; param_reader.get<bool>("pre_transform_model_p", pre_transform_p); if(pre_transform_p) { Eigen::Vector4f scene_center(0.0f, 0.0f, 0.0f, 0.0f); pcl::compute3DCentroid(*scene, scene_center); // generate the rotation matrix: // define the rotation angle and rotation axis float rotation_angle = (float)20.0/180.0*M_PI; Eigen::Vector3f rotation_axis(0.2f, 1.0f, 1.0f); Eigen::AngleAxisf rotation_mg (rotation_angle, rotation_axis); // generate the whole tranform: // translate the model to the origin first, and then do the rotation. Eigen::Vector3f tmpVec3f(scene_center(0), scene_center(1), scene_center(2)); // tmpVec3f = tmpVec3f*0.09; // translate first, then rotation Eigen::Affine3f transform_mg ( rotation_mg*Eigen::Translation3f((-1) * tmpVec3f)); // transform the model cloud pcl::transformPointCloud(*object, *object, transform_mg); } //------------------------------------------------------------ // Downsample pcl::console::print_highlight ("Downsampling...\n"); pcl::VoxelGrid<PointNT> grid; float leaf = 0.003f; param_reader.get<float>("leaf_size", leaf); grid.setLeafSize (leaf, leaf, leaf); grid.setInputCloud (object); grid.filter (*object); grid.setInputCloud (scene); grid.filter (*scene); // Estimate normals for scene pcl::console::print_highlight ("Estimating scene normals...\n"); pcl::NormalEstimationOMP<PointNT,PointNT> nest; float normal_search_radius = 0.015; param_reader.get<float>("normal_search_radius", normal_search_radius); nest.setRadiusSearch (normal_search_radius); nest.setInputCloud (scene); nest.compute (*scene); // Estimate features pcl::console::print_highlight ("Estimating features...\n"); FeatureEstimationT fest; float feature_search_radius=0.01; param_reader.get<float>("feature_search_radius", feature_search_radius); fest.setRadiusSearch (feature_search_radius); fest.setInputCloud (object); fest.setInputNormals (object); fest.compute (*object_features); fest.setInputCloud (scene); fest.setInputNormals (scene); fest.compute (*scene_features); // Perform alignment pcl::console::print_highlight ("Starting alignment...\n"); // Initialize Sample Consensus Initial Alignment (SAC-IA) pcl::SampleConsensusInitialAlignment<PointNT, PointNT, FeatureT> reg; reg.setMinSampleDistance (0.01f); reg.setMaxCorrespondenceDistance (0.01); reg.setMaximumIterations (1000); reg.setInputSource (object); reg.setInputTarget (scene); reg.setSourceFeatures (object_features); reg.setTargetFeatures (scene_features); { pcl::ScopeTime t("Alignment"); reg.align (*object_aligned); } if (reg.hasConverged ()) { // Print results printf ("\n"); Eigen::Matrix4f transformation = reg.getFinalTransformation (); pcl::console::print_info (" | %6.3f %6.3f %6.3f | \n", transformation (0,0), transformation (0,1), transformation (0,2)); pcl::console::print_info ("R = | %6.3f %6.3f %6.3f | \n", transformation (1,0), transformation (1,1), transformation (1,2)); pcl::console::print_info (" | %6.3f %6.3f %6.3f | \n", transformation (2,0), transformation (2,1), transformation (2,2)); pcl::console::print_info ("\n"); pcl::console::print_info ("t = < %0.3f, %0.3f, %0.3f >\n", transformation (0,3), transformation (1,3), transformation (2,3)); pcl::console::print_info ("\n"); // pcl::console::print_info ("Inliers: %i/%i\n", reg.getInliers ().size (), object->size ()); // Show alignment pcl::visualization::PCLVisualizer visu("Alignment"); visu.addPointCloud (scene, ColorHandlerT (scene, 0.0, 255.0, 0.0), "scene"); visu.addPointCloud (object_aligned, ColorHandlerT (object_aligned, 0.0, 0.0, 255.0), "object_aligned"); visu.spin (); } else { pcl::console::print_error ("Alignment failed!\n"); return (1); } return (0); }