void frecuency_multiplication(Image<T> &img,const char *filename, int num_kernel = 0){ int Ncols = (img.cols == (1<<(log_2(img.cols))-1))? img.cols : 1<<(log_2(img.cols)); int Nrows = (img.rows == (1<<(log_2(img.rows))-1))? img.rows : 1<<(log_2(img.rows)); Ncols = Nrows = max(Ncols,Nrows); cout<<Ncols<<" "<<Nrows<<endl; Image<cpx> ori(Nrows,Ncols); Image<cpx> kernel(Nrows,Ncols); Image<cpx> kernel_frec(Nrows,Ncols); Image<T> init_kernel(3,3); Image<cpx> dest(Nrows,Ncols); Image<cpx> final(Nrows,Ncols); Image<T> final2(Nrows,Ncols); initialize_kernel(init_kernel,num_kernel); cp_and_padding(ori,img); cp_and_padding(kernel,init_kernel); FFT_image(ori,dest); FFT_image(kernel,kernel_frec); multiply(dest,kernel_frec,final); FFT_image(final,ori,-1); for(int i = 0; i < final.rows; ++i) for(int j = 0; j < final.cols; ++j){ //final2(i,j) = ori(i,j).a / (Nrows*Ncols); // REAL final2(i,j) = sqrt(ori(i,j).modsq()) / (Nrows*Ncols); // Magnitude assert(ori(i,j).b < 1e-4); } final2.normalize(400); save_image(filename, final2); }
void My_Filter::pclCallback(const sensor_msgs::PointCloud2::ConstPtr& cloud){ pcl::PCLPointCloud2::Ptr pcl_pc(new pcl::PCLPointCloud2 ()); pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_cloud (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr final (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr final2 (new pcl::PointCloud<pcl::PointXYZ>); pcl::PCLPointCloud2::Ptr cloud_out (new pcl::PCLPointCloud2 ()); sensor_msgs::PointCloud2 pc2; std::vector<int> inliers; // Transformation into PCL type PointCloud2 pcl_conversions::toPCL((*cloud), *(pcl_pc)); // Transformation into PCL type PointCloud<pcl::PointXYZRGB> pcl::fromPCLPointCloud2(*(pcl_pc), *(pcl_cloud)); std::list<geometry_msgs::PoseStamped> tmp_list; int robot_counter = 0; for (std::list<geometry_msgs::PoseStamped>::iterator it = robots.begin() ; it != robots.end(); ++it) { if(it->pose.orientation.x > 0.001) { // Robot currently not seen //////////////////////// // PassThrough filter // //////////////////////// pcl::PassThrough<pcl::PointXYZ> pass; pass.setInputCloud (pcl_cloud); pass.setFilterFieldName ("x"); pass.setFilterLimits (it->pose.position.x-DETECTION_DISTANCE-it->pose.orientation.x*0.002, it->pose.position.x+DETECTION_DISTANCE+it->pose.orientation.x*0.002); //pass.setFilterLimitsNegative (true); pass.filter (*final2); pass.setInputCloud (final2); pass.setFilterFieldName ("y"); pass.setFilterLimits (it->pose.position.y-DETECTION_DISTANCE-it->pose.orientation.x*0.002, it->pose.position.y+DETECTION_DISTANCE+it->pose.orientation.x*0.002); //pass.setFilterLimitsNegative (true); pass.filter (*final2); } else { // Robot seen //////////////////////// // PassThrough filter // //////////////////////// pcl::PassThrough<pcl::PointXYZ> pass; pass.setInputCloud (pcl_cloud); pass.setFilterFieldName ("x"); pass.setFilterLimits (it->pose.position.x-DETECTION_DISTANCE, it->pose.position.x+DETECTION_DISTANCE); //pass.setFilterLimitsNegative (true); pass.filter (*final2); pass.setInputCloud (final2); pass.setFilterFieldName ("y"); pass.setFilterLimits (it->pose.position.y-DETECTION_DISTANCE, it->pose.position.y+DETECTION_DISTANCE); //pass.setFilterLimitsNegative (true); pass.filter (*final2); pcl::ConditionOr<pcl::PointXYZ>::Ptr range_cond (new pcl::ConditionOr<pcl::PointXYZ> ()); range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new pcl::FieldComparison<pcl::PointXYZ> ("x", pcl::ComparisonOps::GT, it->pose.position.x+DETECTION_DISTANCE))); range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new pcl::FieldComparison<pcl::PointXYZ> ("x", pcl::ComparisonOps::LT, it->pose.position.x-DETECTION_DISTANCE))); range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new pcl::FieldComparison<pcl::PointXYZ> ("y", pcl::ComparisonOps::GT, it->pose.position.y+DETECTION_DISTANCE))); range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new pcl::FieldComparison<pcl::PointXYZ> ("y", pcl::ComparisonOps::LT, it->pose.position.y-DETECTION_DISTANCE))); // build the filter pcl::ConditionalRemoval<pcl::PointXYZ> condrem (range_cond); condrem.setInputCloud (pcl_cloud); condrem.setKeepOrganized(true); // apply filter condrem.filter (*pcl_cloud); } if(final2->size() > 2) { // Something in the box double meanX = 0.0; double meanY = 0.0; for(int i = 0; i < final2->size(); i++) { meanX += final2->at(i).x; meanY += final2->at(i).y; } meanX = meanX / final2->size(); meanY = meanY / final2->size(); //robots.at(robot_counter).pose.position.x = meanX; //robots.at(robot_counter).pose.position.y = meanY; it->pose.position.x = meanX; it->pose.position.y = meanY; char numstr[50]; sprintf(numstr, "/robot_%s_link", it->header.frame_id.c_str()); t = tf::StampedTransform(tf::Transform(tf::createQuaternionFromYaw(0.0), tf::Vector3(meanX, meanY, 0.0)), ros::Time::now(), "/world", numstr); t.stamp_ = ros::Time::now(); broadcaster.sendTransform(t); it->pose.orientation.x = 0.0; tmp_list.push_front(*it); //std::cout << "size : " << final2->size() << std::endl; } else { // Nothing found in the box it->pose.orientation.x += 1.0; if(it->pose.orientation.x > 100.0) { it->pose.orientation.x = 100.0; } //std::cout << "coef : " << it->pose.orientation.x << std::endl; geometry_msgs::PoseStamped tmp = *it; //robots.push_back(*it); //robots.erase(it); //robots.push_back(tmp); tmp_list.push_back(*it); } robot_counter++; } robots = tmp_list; ///////////////////////////////// // Statistical Outlier Removal // ///////////////////////////////// /* pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor; sor.setInputCloud (pcl_cloud); sor.setMeanK (200); sor.setStddevMulThresh (1.0); sor.filter (*final); */ ////////////////////////////////// // Euclidian Cluster Extraction // ////////////////////////////////// // Creating the KdTree object for the search method of the extraction /* pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>); tree->setInputCloud (pcl_cloud); std::vector<pcl::PointIndices> cluster_indices; pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec; ec.setClusterTolerance (0.05); // 2cm ec.setMinClusterSize (5); ec.setMaxClusterSize (50); ec.setSearchMethod (tree); ec.setInputCloud (pcl_cloud); ec.extract (cluster_indices); std::cout << cluster_indices.size() << std::endl; double x = 0.0; double y = 0.0; for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it) { x = 0.0; y = 0.0; for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++) { x +=pcl_cloud->points[*pit].x; y +=pcl_cloud->points[*pit].y; } x = x / it->indices.size(); y = y / it->indices.size(); std::cout << "x : " << x << " y : " << y << " size : " << it->indices.size() << std::endl; } */ // Transformation into ROS type pcl::toPCLPointCloud2(*(pcl_cloud), *(cloud_out)); pcl_conversions::moveFromPCL(*(cloud_out), pc2); point_cloud_publisher_.publish(pc2); }
void topicCallback_input_cloud(const sensor_msgs::PointCloud2::ConstPtr& msg) { /* protected region user implementation of subscribe callback for input_cloud on begin */ pcl::PCLPointCloud2::Ptr pcl_pc(new pcl::PCLPointCloud2 ()); pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_cloud (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr final (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr final2 (new pcl::PointCloud<pcl::PointXYZ>); pcl::PCLPointCloud2::Ptr cloud_out (new pcl::PCLPointCloud2 ()); sensor_msgs::PointCloud2 pc2; std::vector<int> inliers; // Transformation into PCL type PointCloud2 pcl_conversions::toPCL((*msg), *(pcl_pc)); // Transformation into PCL type PointCloud<pcl::PointXYZRGB> pcl::fromPCLPointCloud2(*(pcl_pc), *(pcl_cloud)); std::list<geometry_msgs::PoseStamped> tmp_list; int robot_counter = 0; for (std::list<geometry_msgs::PoseStamped>::iterator it = robots.begin() ; it != robots.end(); ++it) { if(it->pose.orientation.x > 0.001) { // Robot currently not seen //////////////////////// // PassThrough filter // //////////////////////// pcl::PassThrough<pcl::PointXYZ> pass; pass.setInputCloud (pcl_cloud); pass.setFilterFieldName ("x"); pass.setFilterLimits (it->pose.position.x-localconfig.detection_distance-it->pose.orientation.x*0.002, it->pose.position.x+localconfig.detection_distance+it->pose.orientation.x*0.002); //pass.setFilterLimitsNegative (true); pass.filter (*final2); pass.setInputCloud (final2); pass.setFilterFieldName ("y"); pass.setFilterLimits (it->pose.position.y-localconfig.detection_distance-it->pose.orientation.x*0.002, it->pose.position.y+localconfig.detection_distance+it->pose.orientation.x*0.002); //pass.setFilterLimitsNegative (true); pass.filter (*final2); } else { // Robot seen //////////////////////// // PassThrough filter // //////////////////////// pcl::PassThrough<pcl::PointXYZ> pass; pass.setInputCloud (pcl_cloud); pass.setFilterFieldName ("x"); pass.setFilterLimits (it->pose.position.x-localconfig.detection_distance, it->pose.position.x+localconfig.detection_distance); //pass.setFilterLimitsNegative (true); pass.filter (*final2); pass.setInputCloud (final2); pass.setFilterFieldName ("y"); pass.setFilterLimits (it->pose.position.y-localconfig.detection_distance, it->pose.position.y+localconfig.detection_distance); //pass.setFilterLimitsNegative (true); pass.filter (*final2); pcl::ConditionOr<pcl::PointXYZ>::Ptr range_cond (new pcl::ConditionOr<pcl::PointXYZ> ()); range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new pcl::FieldComparison<pcl::PointXYZ> ("x", pcl::ComparisonOps::GT, it->pose.position.x+localconfig.detection_distance))); range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new pcl::FieldComparison<pcl::PointXYZ> ("x", pcl::ComparisonOps::LT, it->pose.position.x-localconfig.detection_distance))); range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new pcl::FieldComparison<pcl::PointXYZ> ("y", pcl::ComparisonOps::GT, it->pose.position.y+localconfig.detection_distance))); range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new pcl::FieldComparison<pcl::PointXYZ> ("y", pcl::ComparisonOps::LT, it->pose.position.y-localconfig.detection_distance))); // build the filter pcl::ConditionalRemoval<pcl::PointXYZ> condrem (range_cond); condrem.setInputCloud (pcl_cloud); condrem.setKeepOrganized(true); // apply filter condrem.filter (*pcl_cloud); } if(final2->size() > 2) { // Something in the box double meanX = 0.0; double meanY = 0.0; for(int i = 0; i < final2->size(); i++) { meanX += final2->at(i).x; meanY += final2->at(i).y; } meanX = meanX / final2->size(); meanY = meanY / final2->size(); //robots.at(robot_counter).pose.position.x = meanX; //robots.at(robot_counter).pose.position.y = meanY; it->pose.position.x = (meanX + it->pose.position.x) / 2.0; it->pose.position.y = (meanY + it->pose.position.y) / 2.0; char numstr[50]; sprintf(numstr, "/robot_%s_link", it->header.frame_id.c_str()); t = tf::StampedTransform(tf::Transform(tf::createQuaternionFromYaw(0.0), tf::Vector3(meanX, meanY, 0.0)), ros::Time::now(), "/world", numstr); t.stamp_ = ros::Time::now(); broadcaster.sendTransform(t); it->pose.orientation.x = 0.0; tmp_list.push_front(*it); if(it->header.frame_id == "0") { robot1_output_ready = true; } else { robot2_output_ready = true; } //std::cout << "size : " << final2->size() << std::endl; } else { // Nothing found in the box it->pose.orientation.x += 1.0; if(it->pose.orientation.x > 100.0) { it->pose.orientation.x = 100.0; } //std::cout << "coef : " << it->pose.orientation.x << std::endl; geometry_msgs::PoseStamped tmp = *it; //robots.push_back(*it); //robots.erase(it); //robots.push_back(tmp); tmp_list.push_back(*it); } robot_counter++; } robots = tmp_list; /* protected region user implementation of subscribe callback for input_cloud end */ }