$search
00001 /* 00002 * normals_vfh_db_estimator_service.cpp 00003 * 00004 * Created on: Jan 25, 2011 00005 * Author: aaldoma 00006 */ 00007 00008 #include "ros/ros.h" 00009 #include <ros/message_traits.h> 00010 #include <ros/serialization.h> 00011 #include "sensor_msgs/PointCloud2.h" 00012 #include "geometry_msgs/Pose.h" 00013 #include "vfh_recognizer_fs/remove_duplicate_views.h" 00014 #include <vfh_recognizer_fs/vfh_recognition.h> 00015 #include <pcl/common/common.h> 00016 #include <fstream> 00017 #include <iostream> 00018 #include <pcl_ros/point_cloud.h> 00019 #include <pcl/registration/registration.h> 00020 #include <pcl/registration/icp.h> 00021 #include <pcl/io/io.h> 00022 #include <pcl/segmentation/segment_differences.h> 00023 #include "boost/filesystem/operations.hpp" 00024 #include "boost/filesystem/path.hpp" 00025 #include <boost/lexical_cast.hpp> 00026 #include <boost/algorithm/string/replace.hpp> 00027 #include <boost/algorithm/string.hpp> 00028 00029 using namespace std; 00030 namespace ser = ros::serialization; 00031 namespace bf = boost::filesystem; 00032 00033 void 00034 getPointCloudFromView (pcl::PointCloud<pcl::PointNormal> & cloud, std::string id, std::string root_dir) { 00035 stringstream view_file; 00036 view_file << root_dir << "/views/view_" << id << ".pcd"; 00037 std::cout << "Going to get point cloud" << view_file.str() << std::endl; 00038 pcl::io::loadPCDFile(view_file.str(), cloud); 00039 } 00040 00041 00042 bool 00043 getUniqueViews (vfh_recognizer_fs::remove_duplicate_views::Request &req, 00044 vfh_recognizer_fs::remove_duplicate_views::Response &res) 00045 { 00046 00047 vfh_recognizer_fs::VFHRecognizerFS < flann::ChiSquareDistance > vfh_recognizer; 00048 00049 //get views from filesystem given req.model_id and build views... 00050 std::string MODEL_ID = req.model_id.data; 00051 std::string ROOT_PATH = req.root_path.data; 00052 stringstream view_dir_str; 00053 view_dir_str << ROOT_PATH << "/views"; 00054 bf::path views_dir = view_dir_str.str(); 00055 00056 std::vector<std::string> views; 00057 00058 bf::directory_iterator end_itr; 00059 for (bf::directory_iterator itr (views_dir); itr != end_itr; ++itr) 00060 { 00061 if (!bf::is_directory (itr->status ())) 00062 { 00063 std::string full_file_name = itr->path ().string (); 00064 //std::cerr << "opening " << full_file_name.c_str() << std::endl; 00065 #if (!defined BOOST_FILESYSTEM_VERSION) || (BOOST_FILESYSTEM_VERSION<=2) 00066 std::string file_name = itr->path ().filename (); 00067 #else 00068 std::string file_name = itr->path ().filename ().string (); 00069 #endif 00070 std::stringstream to_search; 00071 to_search << "view_" << MODEL_ID << "_"; 00072 if (!boost::algorithm::find_first(file_name,to_search.str().c_str()).empty() ) 00073 { 00074 //extract view id from file_name 00075 std::vector<std::string> strs; 00076 boost::split (strs, file_name, boost::is_any_of ("_.")); //last is pcd and the one before, is the view_id 00077 00078 std::stringstream to_add; 00079 to_add << MODEL_ID << "_" << strs[strs.size() - 2]; 00080 00081 std::cout << to_add.str() << std::endl; 00082 views.push_back(to_add.str()); 00083 } 00084 } 00085 } 00086 00087 std::cout << "views size:" << views.size() << std::endl; 00088 00089 //build roll histograms 00090 std::vector< pcl::PointCloud<pcl::VFHSignature308>::Ptr > roll_histograms; 00091 roll_histograms.resize (views.size ()); 00092 00093 std::vector<Eigen::Vector4f,Eigen::aligned_allocator< Eigen::Vector4f> > centroids; 00094 centroids.resize (views.size ()); 00095 std::vector< pcl::PointCloud < pcl::PointNormal >::Ptr > pointclouds; 00096 pointclouds.resize(views.size()); 00097 00098 for (size_t i = 0; i < views.size (); i++) 00099 { 00100 //get point cloud from view 00101 pcl::PointCloud < pcl::PointNormal >::Ptr cloud_normals(new pcl::PointCloud<pcl::PointNormal> ()); 00102 getPointCloudFromView (*cloud_normals, views[i], ROOT_PATH); 00103 00104 //compute centroid 00105 Eigen::Vector4f centroid; 00106 pcl::compute3DCentroid (*cloud_normals, centroid); 00107 Eigen::Vector3f ci3f (centroid[0], centroid[1],centroid[2]); 00108 00109 //compute roll angle orientation 00110 pcl::PointCloud<pcl::VFHSignature308>::Ptr vfh_o_signature (new pcl::PointCloud<pcl::VFHSignature308> ()); 00111 cv::Mat frequency_domain; 00112 vfh_recognizer.computeVFHRollOrientation (cloud_normals, vfh_o_signature, ci3f,frequency_domain); 00113 00114 roll_histograms[i] = vfh_o_signature; 00115 centroids[i] = centroid; 00116 pointclouds[i] = cloud_normals; 00117 } 00118 00119 //check for duplicates 00120 std::vector<bool> unique (views.size (), true); 00121 //PCLVisualizer vis ("VFH Cluster Classifier"); 00122 //PCLVisualizer vis_diff ("Difference"); 00123 00124 for (size_t i = 0; i < views.size (); i++) 00125 { 00126 Eigen::Vector3f plane_normal; 00127 plane_normal[0] = -centroids.at(i)[0]; 00128 plane_normal[1] = -centroids.at(i)[1]; 00129 plane_normal[2] = -centroids.at(i)[2]; 00130 Eigen::Vector3f z_vector = Eigen::Vector3f::UnitZ (); 00131 plane_normal.normalize (); 00132 Eigen::Vector3f axis = plane_normal.cross (z_vector); 00133 double rotation = -asin (axis.norm ()); 00134 axis.normalize (); 00135 Eigen::Affine3f transformRM (Eigen::AngleAxisf (rotation, axis)); 00136 00137 pcl::PointCloud<pcl::VFHSignature308>::Ptr roll_i_ptr; 00138 roll_i_ptr = roll_histograms[i]; //access the correct roll histogram 00139 00140 pcl::PointCloud < pcl::PointNormal >::Ptr cloud_normals_i(new pcl::PointCloud<pcl::PointNormal> ()); 00141 cloud_normals_i = pointclouds[i]; 00142 00143 Eigen::Vector4f centroid_input (centroids.at(i)[0], centroids.at(i)[1], centroids.at(i)[2], 0); 00144 00145 pcl::PointCloud<pcl::VFHSignature308>::Ptr vfh_o_signature_trash (new pcl::PointCloud<pcl::VFHSignature308> ()); 00146 Eigen::Vector3f ci3f (centroids.at(i)[0], centroids.at(i)[1], centroids.at(i)[2]); 00147 cv::Mat frequency_domain_input; 00148 vfh_recognizer.computeVFHRollOrientation (cloud_normals_i, vfh_o_signature_trash, ci3f, frequency_domain_input); 00149 00150 for (size_t j = (i+1); j < views.size (); j++) { 00151 //compare view i with the rest of the views and mark it as unique or not unique 00152 /*vis.removePointCloud ("cloud_i"); 00153 vis.removePointCloud ("cloud_j"); 00154 vis_diff.removePointCloud ("difference");*/ 00155 00156 pcl::PointCloud<pcl::VFHSignature308>::Ptr roll_j_ptr; 00157 roll_j_ptr = roll_histograms[j]; //access the correct roll histogram 00158 00159 Eigen::Vector4f centroid_view (centroids.at(j)[0], centroids.at(j)[1], centroids.at(j)[2], 0); 00160 00161 pcl::PointCloud < pcl::PointNormal >::Ptr cloud_normals_j(new pcl::PointCloud<pcl::PointNormal> ()); 00162 cloud_normals_j = pointclouds[j]; 00163 00164 pcl::PointCloud<pcl::VFHSignature308>::Ptr vfh_o_signature_trash (new pcl::PointCloud<pcl::VFHSignature308> ()); 00165 Eigen::Vector3f ci3f (centroids.at(j)[0], centroids.at(j)[1], centroids.at(j)[2]); 00166 cv::Mat frequency_domain_view; 00167 vfh_recognizer.computeVFHRollOrientation (cloud_normals_j, vfh_o_signature_trash, ci3f, frequency_domain_view); 00168 00169 //compute roll angle! 00170 int angle = 0; 00171 angle = vfh_recognizer.computeAngleRollOrientationFrequency (frequency_domain_input, frequency_domain_view, 90, centroid_view, centroid_input , *cloud_normals_j, *cloud_normals_i, 0, *vfh_o_signature_trash, *vfh_o_signature_trash); 00172 00173 //std::cout << "Computed angle:" << angle << std::endl; 00174 plane_normal[0] = -centroids.at(j)[0]; 00175 plane_normal[1] = -centroids.at(j)[1]; 00176 plane_normal[2] = -centroids.at(j)[2]; 00177 plane_normal.normalize (); 00178 axis = plane_normal.cross (z_vector); 00179 rotation = -asin (axis.norm ()); 00180 axis.normalize (); 00181 Eigen::Affine3f transformRS (Eigen::AngleAxisf (rotation, axis)); 00182 transformRS = transformRS.inverse(); //invert transformation 00183 //roll transformation 00184 00185 Eigen::Affine3f transformRoll(Eigen::AngleAxisf((angle*M_PI/180), Eigen::Vector3f::UnitZ())); 00186 Eigen::Affine3f final_trans = transformRS * transformRoll * transformRM; 00187 00188 pcl::PointCloud < pcl::PointNormal > cloud_xyz; 00189 pcl::PointCloud < pcl::PointNormal >::Ptr gridTransformed (new pcl::PointCloud < pcl::PointNormal > ()); 00190 pcl::transformPointCloudWithNormals(*cloud_normals_j,*gridTransformed,final_trans); 00191 cloud_xyz = *gridTransformed; 00192 00193 // Demean the cloud 00194 pcl::PointCloud < pcl::PointNormal >::Ptr cloud_xyz_demean(new pcl::PointCloud<pcl::PointNormal> ()); 00195 Eigen::Vector4f diff = Eigen::Vector4f::Zero(); 00196 00197 Eigen::Vector3f centr(centroids.at(j)[0],centroids.at(j)[1],centroids.at(j)[2]); 00198 centr = final_trans * centr; 00199 00200 diff[0] = -centroids.at(i)[0] + centr[0]; 00201 diff[1] = -centroids.at(i)[1] + centr[1]; 00202 diff[2] = -centroids.at(i)[2] + centr[2]; 00203 00204 pcl::demeanPointCloud (cloud_xyz, diff, *cloud_xyz_demean); 00205 00206 //we can compare cloud_xyz_demean with cloud_normals_i 00207 pcl::IterativeClosestPoint < pcl::PointNormal, pcl::PointNormal > reg; 00208 reg.setInputCloud (cloud_xyz_demean); 00209 reg.setInputTarget (cloud_normals_i); 00210 double fscore = reg.getFitnessScore (0.005); 00211 00212 //segment difference 00213 pcl::SegmentDifferences<pcl::PointNormal> seg; 00214 typedef pcl::KdTree<pcl::PointNormal>::Ptr KdTreePtr; 00215 KdTreePtr normals_tree = boost::make_shared<pcl::KdTreeFLANN<pcl::PointNormal> > (true); 00216 //normals_tree->setInputCloud (cloud_xyz_demean); 00217 seg.setDistanceThreshold (0.01*0.01); //1cm 00218 seg.setSearchMethod(normals_tree); 00219 seg.setInputCloud(cloud_normals_i); 00220 seg.setTargetCloud(cloud_xyz_demean); 00221 00222 pcl::PointCloud < pcl::PointNormal > difference; 00223 seg.segment(difference); 00224 00225 //std::cout << "Fitness score:" << fscore << std::endl; 00226 fscore = (double)(difference.points.size()) / (double)(cloud_normals_i->points.size()); 00227 std::cout << "% of remaining points:" << fscore << " i:" << i << " j:" << j << " size diff:" << difference.points.size() << std::endl; 00228 if (fscore < 0.025) { //if less than 2.5% of the points have their nearest neighbor outside threshold, then view is not unique 00229 //std::cout << "View is not unique" << std::endl; 00230 unique[i] = false; 00231 break; //we have found at least one view after i that is the same 00232 } 00233 } 00234 } 00235 00236 //remove duplicate views and descriptors from the FS 00237 00238 int unique_views = 0; 00239 for (size_t i = 0; i < unique.size (); i++) 00240 { 00241 if (!unique.at (i)) //if its not unique, delete it from database 00242 { 00243 00244 std::string view_id = views.at(i); 00245 std::cout << "View "<< view_id << " not unique" << std::endl; 00246 00247 //delete view 00248 stringstream view_dir_str; 00249 view_dir_str << ROOT_PATH << "/views/view_" << view_id << ".pcd"; 00250 bf::path view_path = view_dir_str.str(); 00251 00252 bf::remove(view_path); 00253 00254 //get VFHid mapping from this view 00255 stringstream vfh_mappings; 00256 vfh_mappings << ROOT_PATH << "/views_vfh_map/view_vfh_" << view_id << ".txt"; 00257 std::ifstream in; 00258 in.open (vfh_mappings.str ().c_str (), std::ifstream::in); 00259 00260 char linebuf[256]; 00261 in.getline (linebuf, 256); 00262 std::string line (linebuf); 00263 00264 std::vector < std::string > strs; 00265 boost::split (strs, line, boost::is_any_of (" ")); 00266 00267 for(size_t k=0; k < strs.size(); k++) { //maybe size - 1 00268 //delete transforms,vfhs,vfhs_roll_orientation,centroids, vfh_view_map 00269 std::string vfh_id = strs[k]; 00270 00271 stringstream vfh_path; 00272 vfh_path << ROOT_PATH << "/vfhs/vfh_" << MODEL_ID << "_" << vfh_id << ".pcd"; 00273 stringstream vfh_roll_path; 00274 vfh_roll_path << ROOT_PATH << "/vfhs_roll_orientation/vfh_orientation_" << MODEL_ID << "_" << vfh_id << ".pcd"; 00275 stringstream transform_path; 00276 transform_path << ROOT_PATH << "/transforms/transform_" << MODEL_ID << "_" << vfh_id << ".txt"; 00277 stringstream centroid_path; 00278 centroid_path << ROOT_PATH << "/centroids/centroid_" << MODEL_ID << "_" << vfh_id << ".txt"; 00279 stringstream vfh_view_map_path; 00280 vfh_view_map_path << ROOT_PATH << "/vfh_views_map/vfh_view_" << MODEL_ID << "_" << vfh_id << ".txt"; 00281 00282 bf::remove(bf::path(vfh_path.str())); 00283 bf::remove(bf::path(vfh_roll_path.str())); 00284 bf::remove(bf::path(transform_path.str())); 00285 bf::remove(bf::path(centroid_path.str())); 00286 bf::remove(bf::path(vfh_view_map_path.str())); 00287 } 00288 00289 //delete view vfh map 00290 bf::remove(bf::path(vfh_mappings.str())); 00291 } else { 00292 unique_views++; 00293 } 00294 } 00295 00296 std::cout << "Number of unique views:" << unique_views << std::endl; 00297 00298 return true; 00299 } 00300 00301 int 00302 main (int argc, char **argv) 00303 { 00304 ros::init (argc, argv, "remove_duplicate_views_server"); 00305 ros::NodeHandle n; 00306 00307 ros::ServiceServer service = n.advertiseService ("remove_duplicate_views", getUniqueViews); 00308 ros::spin (); 00309 00310 return 0; 00311 }