$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/normals_vfh_db_estimator.h" 00014 #include <pcl/common/common.h> 00015 #include <fstream> 00016 #include <iostream> 00017 #include <pcl_ros/point_cloud.h> 00018 #include <vfh_recognizer_fs/vfh_recognition.h> 00019 #include <pcl/io/pcd_io.h> 00020 #include <boost/filesystem.hpp> 00021 #include <boost/random.hpp> 00022 #include <boost/random/normal_distribution.hpp> 00023 #include <Eigen/StdVector> 00024 00025 vfh_recognizer_fs::VFHRecognizerFS<flann::ChiSquareDistance> vfh_recognizer; 00026 00027 using namespace std; 00028 namespace ser = ros::serialization; 00029 bool USE_CLUSTER_CENTROIDS_ = true; 00030 bool NORMALIZE_VFH_BINS_ = false; 00031 bool FILL_SIZE_COMPONENT_ = true; 00032 bool USE_OLD_VFH_ = false; 00033 00034 bool 00035 create_directories_fs (std::string root) 00036 { 00037 //create views directory if does not exist 00038 boost::filesystem::path root_path = root; 00039 if (!boost::filesystem::exists (root_path)) 00040 { 00041 boost::filesystem::create_directory (root_path); 00042 } 00043 00044 boost::filesystem::path views_dir = root_path / "views"; 00045 if (!boost::filesystem::exists (views_dir)) 00046 { 00047 boost::filesystem::create_directory (views_dir); 00048 } 00049 00050 boost::filesystem::path views_vfh_map_dir = root_path / "views_vfh_map"; 00051 if (!boost::filesystem::exists (views_vfh_map_dir)) 00052 { 00053 boost::filesystem::create_directory (views_vfh_map_dir); 00054 } 00055 00056 boost::filesystem::path vfh_views_map_dir = root_path / "vfh_views_map"; 00057 if (!boost::filesystem::exists (vfh_views_map_dir)) 00058 { 00059 boost::filesystem::create_directory (vfh_views_map_dir); 00060 } 00061 00062 boost::filesystem::path vfhs_dir = root_path / "vfhs"; 00063 if (!boost::filesystem::exists (vfhs_dir)) 00064 { 00065 boost::filesystem::create_directory (vfhs_dir); 00066 } 00067 00068 boost::filesystem::path vfhs_roll_orientantion_dir = root_path / "vfhs_roll_orientation"; 00069 if (!boost::filesystem::exists (vfhs_roll_orientantion_dir)) 00070 { 00071 boost::filesystem::create_directory (vfhs_roll_orientantion_dir); 00072 } 00073 00074 boost::filesystem::path transforms_dir = root_path / "transforms"; 00075 if (!boost::filesystem::exists (transforms_dir)) 00076 { 00077 boost::filesystem::create_directory (transforms_dir); 00078 } 00079 00080 boost::filesystem::path centroids_dir = root_path / "centroids"; 00081 if (!boost::filesystem::exists (centroids_dir)) 00082 { 00083 boost::filesystem::create_directory (centroids_dir); 00084 } 00085 00086 return true; 00087 } 00088 00089 bool 00090 writePoseToFile (std::string file, geometry_msgs::Pose pose) 00091 { 00092 ofstream out (file.c_str ()); 00093 if (!out) 00094 { 00095 cout << "Cannot open file.\n"; 00096 return false; 00097 } 00098 00099 out << pose.position.x << " " << pose.position.y << " " << pose.position.z << " " << pose.orientation.x << " " 00100 << pose.orientation.y << " " << pose.orientation.z << " " << pose.orientation.w << endl; 00101 out.close (); 00102 00103 return true; 00104 } 00105 00106 bool 00107 writeViewVFHMapToFile (std::string file, std::vector<int> map) 00108 { 00109 ofstream out (file.c_str ()); 00110 if (!out) 00111 { 00112 cout << "Cannot open file.\n"; 00113 return false; 00114 } 00115 00116 std::stringstream value; 00117 for(size_t i=0; i < map.size(); i++) { 00118 value << map[i] << " "; 00119 } 00120 00121 out << value.str().substr(0,-1); 00122 out.close (); 00123 00124 return true; 00125 } 00126 00127 bool 00128 writeVFHViewMapToFile (std::string file, int view_id) 00129 { 00130 ofstream out (file.c_str ()); 00131 if (!out) 00132 { 00133 cout << "Cannot open file.\n"; 00134 return false; 00135 } 00136 00137 out << view_id; 00138 out.close (); 00139 00140 return true; 00141 } 00142 00143 bool 00144 writeCentroidToFile (std::string file, Eigen::Vector3f centroid) 00145 { 00146 ofstream out (file.c_str ()); 00147 if (!out) 00148 { 00149 cout << "Cannot open file.\n"; 00150 return false; 00151 } 00152 00153 out << centroid[0] << " " << centroid[1] << " " << centroid[2] << endl; 00154 out.close (); 00155 00156 return true; 00157 } 00158 00159 int VFH_id_ = 0; 00160 int VIEW_id_ = 0; 00161 00162 bool 00163 compute_and_store (vfh_recognizer_fs::normals_vfh_db_estimator::Request &req, 00164 vfh_recognizer_fs::normals_vfh_db_estimator::Response &res) 00165 { 00166 pcl::PointCloud<pcl::PointNormal>::Ptr cloud_normals (new pcl::PointCloud<pcl::PointNormal> ()); 00167 pcl::PointCloud<pcl::VFHSignature308>::Ptr vfh_signaturePtr (new pcl::PointCloud<pcl::VFHSignature308> ()); 00168 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ> ()); 00169 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_noise (new pcl::PointCloud<pcl::PointXYZ> ()); 00170 pcl::PointCloud<pcl::PointNormal>::Ptr cloud_normals_no_noise (new pcl::PointCloud<pcl::PointNormal> ()); 00171 pcl::PointCloud < pcl::VFHSignature308 > vfh_signature; 00172 00173 pcl::fromROSMsg (req.view, *cloud); 00174 00175 //check if cloud has any points 00176 if (cloud->points.size () == 0) 00177 { 00178 ROS_WARN ("Cloud has no points, skip view!"); 00179 return false; 00180 } 00181 00182 pcl::copyPointCloud (*cloud, *cloud_noise); 00183 00184 //Add noise to training data so we can use same parameters as for real data 00185 bool noisify = true; 00186 00187 if (noisify) 00188 { 00189 double noise_std = 0.001; //1mm 00190 00191 struct timeval start; 00192 gettimeofday (&start, NULL); 00193 00194 boost::mt19937 rng; 00195 rng.seed (start.tv_usec); 00196 boost::normal_distribution<> nd (0.0, noise_std); 00197 boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > var_nor (rng, nd); 00198 00199 // Noisify each point in the dataset 00200 for (size_t cp = 0; cp < cloud_noise->points.size (); ++cp) 00201 { 00202 cloud_noise->points[cp].z += var_nor (); 00203 } 00204 } 00205 00206 std::vector<pcl::PointCloud<pcl::VFHSignature308>,Eigen::aligned_allocator< pcl::PointCloud<pcl::VFHSignature308> > > vfh_signatures; 00207 std::vector < Eigen::Vector3f > centroids_dominant_orientations; 00208 00209 //CONFIGURATION TO USE CLUSTERS CENTROIDS OR VIEW CENTROIDS 00210 vfh_recognizer.setUseClusterCentroids (USE_CLUSTER_CENTROIDS_); 00211 00212 if (!NORMALIZE_VFH_BINS_) 00213 { 00214 std::cout << "BINS WONT BE NORMALIZED" << std::endl; 00215 vfh_recognizer.setNormalizeVFHBins (false); 00216 } 00217 00218 vfh_recognizer.setUseOldVFH (USE_OLD_VFH_); 00219 vfh_recognizer.fill_size_component_ = FILL_SIZE_COMPONENT_; 00220 00221 struct timeval start; 00222 gettimeofday (&start, NULL); 00223 00224 vfh_recognizer.view_id_being_processed_ = start.tv_usec; 00225 00226 vfh_recognizer.computeVFH (cloud_noise, cloud_normals, vfh_signatures, centroids_dominant_orientations); 00227 vfh_recognizer.computeNormals (cloud, cloud_normals_no_noise); 00228 00229 vfh_signaturePtr = vfh_signatures[0].makeShared (); 00230 00231 //Check the size of the normals and of the vfh_signature 00232 if ((cloud_normals->points.size () < 100) || (ser::serializationLength (*vfh_signaturePtr) < (sizeof(float) * 308))) 00233 { 00234 ROS_WARN ("Normals size or vfh descriptor size are wrong, skip this view - model_id:"); 00235 return false; 00236 } 00237 00238 create_directories_fs (req.root_dir.data); 00239 00240 std::vector<int> ids_for_this_view; 00241 00242 for (size_t vfh_idx = 0; vfh_idx < vfh_signatures.size (); vfh_idx++) 00243 { 00244 std::vector<float> vfh_centroid (3, 0); 00245 for (size_t i = 0; i < 3; i++) 00246 vfh_centroid[i] = centroids_dominant_orientations[vfh_idx][i]; 00247 00248 if (USE_CLUSTER_CENTROIDS_) 00249 { 00250 //Once the VFH has been estimated, we need to compute the orientation histogram 00251 //for each VFH using the cluster centroid 00252 pcl::PointCloud<pcl::VFHSignature308>::Ptr 00253 vfh_orientation_signature ( 00254 new pcl::PointCloud<pcl::VFHSignature308> ()); 00255 cv::Mat frequency_domain; 00256 vfh_recognizer.computeVFHRollOrientation (cloud_normals, vfh_orientation_signature, 00257 centroids_dominant_orientations[vfh_idx], frequency_domain); //use the centroid of the dominant clusters... 00258 00259 //SAVE vfh_signature, vfh_orientation_signature, cloud_normals and req.transform 00260 stringstream path_vfh; 00261 path_vfh << req.root_dir.data << "/vfhs/vfh_" << req.model_id.data << "_" << VFH_id_ << ".pcd"; 00262 std::cout << path_vfh.str () << std::endl; 00263 pcl::io::savePCDFileBinary (path_vfh.str (), vfh_signatures[vfh_idx]); 00264 00265 stringstream path_vfh_orientation; 00266 path_vfh_orientation << req.root_dir.data << "/vfhs_roll_orientation/vfh_orientation_" << req.model_id.data << "_" 00267 << VFH_id_ << ".pcd"; 00268 std::cout << path_vfh_orientation.str () << std::endl; 00269 pcl::io::savePCDFileBinary (path_vfh_orientation.str (), *vfh_orientation_signature); 00270 00271 stringstream path_transform; 00272 path_transform << req.root_dir.data << "/transforms/transform_" << req.model_id.data << "_" << VFH_id_ 00273 << ".txt"; 00274 std::cout << path_transform.str () << std::endl; 00275 writePoseToFile (path_transform.str (), req.transform); 00276 00277 stringstream path_centroid; 00278 path_centroid << req.root_dir.data << "/centroids/centroid_" << req.model_id.data << "_" << VFH_id_ 00279 << ".txt"; 00280 std::cout << path_centroid.str () << std::endl; 00281 writeCentroidToFile (path_centroid.str (), centroids_dominant_orientations[vfh_idx]); 00282 00283 stringstream path_vfh_view_map; 00284 path_vfh_view_map << req.root_dir.data << "/vfh_views_map/vfh_view_" << req.model_id.data << "_" << VFH_id_ 00285 << ".txt"; 00286 00287 writeVFHViewMapToFile(path_vfh_view_map.str(),VIEW_id_); 00288 00289 ids_for_this_view.push_back(VFH_id_); 00290 VFH_id_++; 00291 } 00292 } 00293 00294 stringstream path_view_vfh_map; 00295 path_view_vfh_map << req.root_dir.data << "/views_vfh_map/view_vfh_" << req.model_id.data << "_" << VIEW_id_ 00296 << ".txt"; 00297 00298 writeViewVFHMapToFile(path_view_vfh_map.str(),ids_for_this_view); 00299 00300 stringstream path_view; 00301 path_view << req.root_dir.data << "/views/view_" << req.model_id.data << "_" << VIEW_id_ << ".pcd"; 00302 std::cout << path_view.str () << std::endl; 00303 pcl::io::savePCDFileBinary (path_view.str (), *cloud_normals); 00304 VIEW_id_++; 00305 00306 /*if (!USE_CLUSTER_CENTROIDS_) 00307 { 00308 //Once the VFH has been estimated, we need to compute the orientation histogram 00309 pcl::PointCloud < pcl::PointNormal > grid; 00310 grid = *cloud_normals_no_noise; //TODO 00311 Eigen::Vector4f centroidView; 00312 pcl::compute3DCentroid (grid, centroidView); 00313 pcl::PointCloud<pcl::VFHSignature308>::Ptr vfh_orientation_signature (new pcl::PointCloud<pcl::VFHSignature308> ()); 00314 Eigen::Vector3f ci3f (centroidView[0], centroidView[1], centroidView[2]); 00315 cv::Mat frequency_domain; 00316 vfh_recognizer.computeVFHRollOrientation (cloud_normals, vfh_orientation_signature, ci3f, frequency_domain); //use the centroid of the view 00317 00318 //SAVE vfh_signature, vfh_orientation_signature, cloud_normals and req.transform 00319 stringstream path_view; 00320 path_view << req.root_dir.data << "/views/view_" << req.model_id.data << "_" << req.view_id.data << ".pcd"; 00321 std::cout << path_view.str () << std::endl; 00322 pcl::io::savePCDFileBinary (path_view.str (), *cloud_normals); 00323 00324 stringstream path_vfh; 00325 path_vfh << req.root_dir.data << "/vfhs/vfh_" << req.model_id.data << "_" << req.view_id.data << ".pcd"; 00326 std::cout << path_vfh.str () << std::endl; 00327 pcl::io::savePCDFileBinary (path_vfh.str (), vfh_signature); 00328 00329 stringstream path_vfh_orientation; 00330 path_vfh_orientation << req.root_dir.data << "/vfhs_roll_orientation/vfh_orientation_" << req.model_id.data << "_" 00331 << req.view_id.data << ".pcd"; 00332 std::cout << path_vfh_orientation.str () << std::endl; 00333 pcl::io::savePCDFileBinary (path_vfh_orientation.str (), *vfh_orientation_signature); 00334 00335 stringstream path_transform; 00336 path_transform << req.root_dir.data << "/transforms/transform_" << req.model_id.data << "_" << req.view_id.data 00337 << ".txt"; 00338 std::cout << path_transform.str () << std::endl; 00339 writePoseToFile (path_transform.str (), req.transform); 00340 00341 stringstream path_centroid; 00342 path_centroid << req.root_dir.data << "/transforms/transform_" << req.model_id.data << "_" << req.view_id.data 00343 << ".txt"; 00344 std::cout << path_centroid.str () << std::endl; 00345 writeCentroidToFile (path_centroid.str (), centroids_dominant_orientations[0]); 00346 }*/ 00347 00348 //pcl::io::savePCDFileBinary (path_vfh_orientation.str(), *vfh_orientation_signature); 00349 00350 return true; 00351 } 00352 00353 int 00354 main (int argc, char **argv) 00355 { 00356 ros::init (argc, argv, "normals_vfh_db_estimator_server"); 00357 ros::NodeHandle n; 00358 00359 ros::ServiceServer service = n.advertiseService ("normals_vfh_db_estimator", compute_and_store); 00360 ros::spin (); 00361 00362 return 0; 00363 }