vfh_recognizer_fs: vfh_recognizer_fs: remove_duplicate_views

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 }
remove_duplicate_views_service.cpp
