Esempio n. 1
0
my_Dvector convolution_same_orig(const my_Dvector& vec1, const my_Dvector& vec2, ThreadPool& pool)
{
    my_Dvector conv (vec1.size(),0.0);
    int init = vec2.size()/2;

    unsigned int nr_th = get_num_threads();
    my_Ivector lims (get_bounds(0,conv.size()));
    std::vector< std::future<int> > results;

    //lambda function
    auto func = [&](my_Dvector& v, int com, int ter)
    {
        for (int ii=com;ii<ter;++ii){
            for (int j=0,k=(ii+init);j<vec2.size() && k>=0;++j,--k){
                if (k<vec1.size()){ v[ii] += vec1[k]*vec2[j];}
            }
        }
        return 1;
    }; //
    for (unsigned int i=0;i<nr_th;++i) results.emplace_back(pool.enqueue(
                        func, ref(conv), lims[i], lims[i+1]));
    for(auto && result: results) result.get();

    return conv;
}
Esempio n. 2
0
static void _convolution(const my_Dvector& vec1, const my_Dvector& vec2, my_Dvector& conv, int init, ThreadPool& pool)
{
    // To do the convolution I trim the beginning and ending points if they are zero (1e-30);
    int min1 (0), max1 (vec1.size());
    _trim_vec(vec1, min1, max1);

    int min2 (0), max2 (vec2.size());
    _trim_vec(vec2, min2, max2);

    int ini = max(min1+min2-init,0);
    int fin = min(int(conv.size()), max1 + max2 - 1);

    unsigned int nr_th = get_num_threads();
    unsigned int nr_job = max((fin-ini)/100, min(int(nr_th), fin-ini));
	my_Ivector lims (get_bounds(ini, fin, nr_job));
	std::vector< std::future<int> > results;

    //lambda function
    auto func = [&](my_Dvector& v, int com, int ter)
    {
        for (int ii=com;ii<ter;++ii){
            const int delta = max(ii+init-min2-max1,0);
            for (int j=min2+delta,k=(ii+init-min2-delta);j<max2 && k>=min1;++j,--k){
                v[ii] += vec1[k]*vec2[j];
            }
        }
        return 1;
    }; //

	for (unsigned int i=0;i<nr_job;++i) results.emplace_back(pool.enqueue(
        func, ref(conv), lims[i], lims[i+1]));
    for(auto && result: results) result.get();


}
    // this function gets called every time new pcl data comes in
    void cloudcb(const sensor_msgs::PointCloud2ConstPtr &scan)
	{
	    ROS_INFO("Kinect point cloud receieved");
	    ros::Time start_time = ros::Time::now();
	    ros::Time tcur = ros::Time::now();

	    Eigen::Vector4f centroid;
	    geometry_msgs::Point point;
	    pcl::PassThrough<pcl::PointXYZ> pass;
	    Eigen::VectorXf lims(6);
	    sensor_msgs::PointCloud2::Ptr
		ros_cloud (new sensor_msgs::PointCloud2 ()),
		ros_cloud_filtered (new sensor_msgs::PointCloud2 ());    
	    pcl::PointCloud<pcl::PointXYZ>::Ptr
	    	cloud (new pcl::PointCloud<pcl::PointXYZ> ()),
	    	cloud_downsampled (new pcl::PointCloud<pcl::PointXYZ> ()),
	    	cloud_filtered (new pcl::PointCloud<pcl::PointXYZ> ());
	    
	    // set a parameter telling the world that I am tracking the robot
	    ros::param::set("/tracking_robot", true);

	    ROS_DEBUG("finished declaring vars : %f", (ros::Time::now()-tcur).toSec());
	    tcur = ros::Time::now();

	    ROS_DEBUG("About to transform cloud");
	    // New sensor message for holding the transformed data
	    sensor_msgs::PointCloud2::Ptr scan_transformed
		(new sensor_msgs::PointCloud2());
	    try{
		pcl_ros::transformPointCloud("/oriented_optimization_frame",
					     tf, *scan, *scan_transformed);
	    }
	    catch(tf::TransformException ex) {
		ROS_ERROR("%s", ex.what());
	    }
	    scan_transformed->header.frame_id = "/oriented_optimization_frame";

	    // Convert to pcl
	    ROS_DEBUG("Convert cloud to pcd from rosmsg");
	    pcl::fromROSMsg(*scan_transformed, *cloud);

	    ROS_DEBUG("cloud transformed and converted to pcl : %f",
		      (ros::Time::now()-tcur).toSec());
	    tcur = ros::Time::now();
	    
	    // set time stamp and frame id
	    ros::Time tstamp = ros::Time::now();

	    // run through pass-through filter to eliminate tarp and below robots.
	    ROS_DEBUG("Pass-through filter");
	    pass_through(cloud, cloud_filtered, robot_limits);

	    ROS_DEBUG("done with pass-through : %f", (ros::Time::now()-tcur).toSec());
	    tcur = ros::Time::now();

	    // now let's publish that filtered cloud
	    ROS_DEBUG("Converting and publishing cloud");		      
	    pcl::toROSMsg(*cloud_filtered, *ros_cloud_filtered);
	    ros_cloud_filtered->header.frame_id =
		"/oriented_optimization_frame";
	    cloud_pub[0].publish(ros_cloud_filtered);

	    ROS_DEBUG("published filtered cloud : %f", (ros::Time::now()-tcur).toSec());
	    tcur = ros::Time::now();

	    // Let's do a downsampling before doing cluster extraction
	    pcl::VoxelGrid<pcl::PointXYZ> vg;
	    vg.setInputCloud (cloud_filtered);
	    vg.setLeafSize (0.01f, 0.01f, 0.01f);
	    vg.filter (*cloud_downsampled);

	    ROS_DEBUG("finished downsampling : %f", (ros::Time::now()-tcur).toSec());
	    tcur = ros::Time::now();


	    ROS_DEBUG("Begin extraction filtering");
	    // build a KdTree object for the search method of the extraction
	    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree
	    	(new pcl::search::KdTree<pcl::PointXYZ> ());
	    tree->setInputCloud (cloud_downsampled);
	    
	    ROS_DEBUG("done with KdTree initialization : %f",
		      (ros::Time::now()-tcur).toSec());
	    tcur = ros::Time::now();


	    // create a vector for storing the indices of the clusters
	    std::vector<pcl::PointIndices> cluster_indices;

	    // setup extraction:
	    pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
	    ec.setClusterTolerance (0.02); // cm
	    ec.setMinClusterSize (50);
	    ec.setMaxClusterSize (3000);
	    ec.setSearchMethod (tree);
	    ec.setInputCloud (cloud_downsampled);

	    // now we can perform cluster extraction
	    ec.extract (cluster_indices);

	    ROS_DEBUG("finished extraction : %f", (ros::Time::now()-tcur).toSec());
	    tcur = ros::Time::now();

	    // run through the indices, create new clouds, and then publish them
	    int j=1;
	    int number_clusters=0;
	    geometry_msgs::PointStamped pt;
	    puppeteer_msgs::Robots robots;
	    std::vector<int> num;

	    for (std::vector<pcl::PointIndices>::const_iterator
	    	     it=cluster_indices.begin();
	    	 it!=cluster_indices.end (); ++it)
	    {
		number_clusters = (int) cluster_indices.size();
	    	ROS_DEBUG("Number of clusters found: %d",number_clusters);
	    	pcl::PointCloud<pcl::PointXYZ>::Ptr
	    	    cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
	    	for (std::vector<int>::const_iterator
	    		 pit = it->indices.begin ();
	    	     pit != it->indices.end (); pit++)
	    	{
	    	    cloud_cluster->points.push_back(cloud_downsampled->points[*pit]);
	    	}
	    	cloud_cluster->width = cloud_cluster->points.size ();
	    	cloud_cluster->height = 1;
	    	cloud_cluster->is_dense = true;

	    	// convert to rosmsg and publish:
	    	ROS_DEBUG("Publishing extracted cloud");
	    	pcl::toROSMsg(*cloud_cluster, *ros_cloud_filtered);
		ros_cloud_filtered->header.frame_id =
		    "/oriented_optimization_frame";
	    	if(j < MAX_CLUSTERS+1)
	    	    cloud_pub[j].publish(ros_cloud_filtered);
	    	j++;

		// compute centroid and add to Robots:
		pcl::compute3DCentroid(*cloud_cluster, centroid);
		pt.point.x = centroid(0);
		pt.point.y = centroid(1);
		pt.point.z = centroid(2);
		pt.header.stamp = tstamp;
		pt.header.frame_id = "/oriented_optimization_frame";
		robots.robots.push_back(pt);
		// add number of points in cluster to num:
		num.push_back(cloud_cluster->points.size());		
	    }

	    ROS_DEBUG("finished creating and publishing clusters : %f", 
		      (ros::Time::now()-tcur).toSec());
	    tcur = ros::Time::now();


	    if (number_clusters > number_robots)
	    {
		ROS_WARN("Number of clusters found is greater "
			 "than the number of robots");
		// pop minimum cluster count
		remove_least_likely(&robots, &num);
	    }
	    
	    robots.header.stamp = tstamp;
	    robots.header.frame_id = "/oriented_optimization_frame";
	    robots.number = number_clusters;

	    robots_pub.publish(robots);

	    ROS_DEBUG("removed extra clusters, and published : %f",
		      (ros::Time::now()-tcur).toSec());
	    tcur = ros::Time::now();

	    ros::Duration d = ros::Time::now() - start_time;
	    ROS_DEBUG("End of cloudcb; time elapsed = %f (%f Hz)",
		      d.toSec(), 1/d.toSec());
	}
    // this function gets called every time new pcl data comes in
    void cloudcb(const sensor_msgs::PointCloud2ConstPtr &scan)
	{
	    Eigen::Vector4f centroid;
	    float xpos = 0.0;
	    float ypos = 0.0;
	    float zpos = 0.0;
	    float D_sphere = 0.05; //meters
	    float R_search = 2.0*D_sphere;
	    puppeteer_msgs::PointPlus pointplus;
	    geometry_msgs::Point point;
	    pcl::PassThrough<pcl::PointXYZ> pass;
	    Eigen::VectorXf lims(6);

	    sensor_msgs::PointCloud2::Ptr
		robot_cloud (new sensor_msgs::PointCloud2 ()),
		robot_cloud_filtered (new sensor_msgs::PointCloud2 ());    

	    pcl::PointCloud<pcl::PointXYZ>::Ptr
		cloud (new pcl::PointCloud<pcl::PointXYZ> ()),
		cloud_filtered (new pcl::PointCloud<pcl::PointXYZ> ());

	    // set a parameter telling the world that I am tracking the robot
	    ros::param::set("/tracking_robot", true);

	    // New sensor message for holding the transformed data
	    sensor_msgs::PointCloud2::Ptr scan_transformed
		(new sensor_msgs::PointCloud2());
	    try{
		pcl_ros::transformPointCloud("/oriented_optimization_frame",
					     tf, *scan, *scan_transformed);
	    }
	    catch(tf::TransformException ex)
	    {
		ROS_ERROR("%s", ex.what());
	    }
	    scan_transformed->header.frame_id = "/oriented_optimization_frame";


	    // Convert to pcl
	    pcl::fromROSMsg(*scan_transformed, *cloud);

	    // set time stamp and frame id
	    pointplus.header.stamp = scan->header.stamp;
	    pointplus.header.frame_id = "/oriented_optimization_frame";

	    // do we need to find the object?
	    if (locate == true)
	    {
		// lims << XMIN, XMAX, YMIN, YMAX, ZMIN, ZMAX;
		lims << frame_limits;
	    	pass_through(cloud, cloud_filtered, lims);
		
	    	pcl::compute3DCentroid(*cloud_filtered, centroid);
	    	xpos = centroid(0); ypos = centroid(1); zpos = centroid(2);

	    	// Publish cloud:
	    	pcl::toROSMsg(*cloud_filtered, *robot_cloud_filtered);
	    	robot_cloud_filtered->header.frame_id =
		    "/oriented_optimization_frame";
	    	cloud_pub[1].publish(robot_cloud_filtered);
		
	    	// are there enough points in the point cloud?
	    	if(cloud_filtered->points.size() > POINT_THRESHOLD)
	    	{
	    	    locate = false;  // We have re-found the object!

	    	    // set values to publish
	    	    pointplus.x = xpos; pointplus.y = ypos; pointplus.z = zpos;
	    	    pointplus.error = false;
	    	    pointplus_pub.publish(pointplus);
	    	}
	    	// otherwise we should publish a blank centroid
	    	// position with an error flag
	    	else
	    	{
	    	    // set values to publish
	    	    pointplus.x = 0.0;
	    	    pointplus.y = 0.0;
	    	    pointplus.z = 0.0;
	    	    pointplus.error = true;

	    	    pointplus_pub.publish(pointplus);	    
	    	}
	    }
	    // if "else", we are just going to calculate the centroid
	    // of the input cloud
	    else
	    {
	    	lims <<
	    	    xpos_last-R_search, xpos_last+R_search,
	    	    ypos_last-R_search, ypos_last+R_search,
	    	    zpos_last-R_search, zpos_last+R_search;
	    	pass_through(cloud, cloud_filtered, lims);
		
	    	// are there enough points in the point cloud?
	    	if(cloud_filtered->points.size() < POINT_THRESHOLD)
	    	{
	    	    locate = true;
	    	    ROS_WARN("Lost Object at: x = %f  y = %f  z = %f\n",
	    		     xpos_last,ypos_last,zpos_last);
	  
	    	    pointplus.x = 0.0;
	    	    pointplus.y = 0.0;
	    	    pointplus.z = 0.0;
	    	    pointplus.error = true;

	    	    pointplus_pub.publish(pointplus);
		    	  	  
	    	    return;
	    	}
	
	    	pcl::compute3DCentroid(*cloud_filtered, centroid);
	    	xpos = centroid(0); ypos = centroid(1); zpos = centroid(2);

	    	pcl::toROSMsg(*cloud_filtered, *robot_cloud);
	    	robot_cloud_filtered->header.frame_id =
		    "/oriented_optimization_frame";
	    	cloud_pub[0].publish(robot_cloud);

	    	tf::Transform transform;
	    	transform.setOrigin(tf::Vector3(xpos, ypos, zpos));
	    	transform.setRotation(tf::Quaternion(0, 0, 0, 1));

	    	static tf::TransformBroadcaster br;
	    	br.sendTransform(tf::StampedTransform
	    			 (transform,ros::Time::now(),
	    			  "/oriented_optimization_frame",
				  "/robot_kinect_frame"));

	    	// set pointplus message values and publish
	    	pointplus.x = xpos;
	    	pointplus.y = ypos;
	    	pointplus.z = zpos;
	    	pointplus.error = false;

	    	pointplus_pub.publish(pointplus);
	    }

	    xpos_last = xpos;
	    ypos_last = ypos;
	    zpos_last = zpos;
	}