int main(int argc, char **argv)
{

    ros::init(argc, argv, "coverge_quantification");
    ros::NodeHandle n;
    ros::Publisher pub1 = n.advertise<sensor_msgs::PointCloud2>("originalPointCloud", 100);
    ros::Publisher pub2 = n.advertise<sensor_msgs::PointCloud2>("CoverageCloud", 100);
//   std::string topic = n.resolveName("/rtabmap/cloud_map");
//   std::string topic = n.resolveName("/iris/cloud_map");
     std::string topic = n.resolveName("/octomap_point_cloud_centers");
//    std::string topic = n.resolveName("/iris/xtion_sensor/iris/xtion_sensor_camera/depth/points");
    ros::Subscriber sub = n.subscribe<sensor_msgs::PointCloud2>(topic, 1, callback);

    pcl::PointCloud<pcl::PointXYZ>::Ptr originalCloud(new pcl::PointCloud<pcl::PointXYZ>);

    std::string path = ros::package::getPath("component_test");
    pcl::io::loadPCDFile<pcl::PointXYZ> (path+"/src/pcd/etihad_nowheels_densed.pcd", *originalCloud);


    // *****************Rviz Visualization ************

    ros::Rate loop_rate(10);
    while (ros::ok())
    {


        //***original cloud & frustum cull & occlusion cull publish***
        sensor_msgs::PointCloud2 cloud1;
        sensor_msgs::PointCloud2 cloud2;


        pcl::toROSMsg(*originalCloud, cloud1); //cloud of original (white) using original cloud
        pcl::toROSMsg(*globalCloud, cloud2); //cloud of the not occluded voxels (blue) using occlusion culling

        cloud1.header.frame_id = "map";
        cloud2.header.frame_id = "map";

        cloud1.header.stamp = ros::Time::now();
        cloud2.header.stamp = ros::Time::now();

        pub1.publish(cloud1);
        pub2.publish(cloud2);

        ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;
}
	void callback(const sensor_msgs::PointCloud2ConstPtr & cloudMsg)
	{
		ros::Time time = ros::Time::now();

		if (groundPub_.getNumSubscribers() == 0 && obstaclesPub_.getNumSubscribers() == 0)
		{
			// no one wants the results
			return;
		}

		rtabmap::Transform localTransform;
		try
		{
			if(waitForTransform_)
			{
				if(!tfListener_.waitForTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, ros::Duration(1)))
				{
					ROS_ERROR("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), cloudMsg->header.frame_id.c_str());
					return;
				}
			}
			tf::StampedTransform tmp;
			tfListener_.lookupTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, tmp);
			localTransform = rtabmap_ros::transformFromTF(tmp);
		}
		catch(tf::TransformException & ex)
		{
			ROS_ERROR("%s",ex.what());
			return;
		}

		pcl::PointCloud<pcl::PointXYZ>::Ptr originalCloud(new pcl::PointCloud<pcl::PointXYZ>);
		pcl::fromROSMsg(*cloudMsg, *originalCloud);

		//Common variables for all strategies
		pcl::IndicesPtr ground, obstacles;
		pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
		pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);

		if(originalCloud->size())
		{
			originalCloud = rtabmap::util3d::transformPointCloud(originalCloud, localTransform);
			if(maxObstaclesHeight_ > 0)
			{
				originalCloud = rtabmap::util3d::passThrough(originalCloud, "z", std::numeric_limits<int>::min(), maxObstaclesHeight_);
			}

			if(originalCloud->size())
			{
				if(!optimizeForCloseObjects_)
				{
					// This is the default strategy
					rtabmap::util3d::segmentObstaclesFromGround<pcl::PointXYZ>(
							originalCloud,
							ground,
							obstacles,
							normalEstimationRadius_,
							groundNormalAngle_,
							minClusterSize_);

					if(groundPub_.getNumSubscribers() && ground.get() && ground->size())
					{
						pcl::copyPointCloud(*originalCloud, *ground, *groundCloud);
					}

					if(obstaclesPub_.getNumSubscribers() && obstacles.get() && obstacles->size())
					{
						pcl::copyPointCloud(*originalCloud, *obstacles, *obstaclesCloud);
					}
				}
				else
				{
					// in this case optimizeForCloseObject_ is true:
					// we divide the floor point cloud into two subsections, one for all potential floor points up to 1m
					// one for potential floor points further away than 1m.
					// For the points at closer range, we use a smaller normal estimation radius and ground normal angle,
					// which allows to detect smaller objects, without increasing the number of false positive.
					// For all other points, we use a bigger normal estimation radius (* 3.) and tolerance for the
					// grond normal angle (* 2.).

					pcl::PointCloud<pcl::PointXYZ>::Ptr originalCloud_near = rtabmap::util3d::passThrough(originalCloud, "x", std::numeric_limits<int>::min(), 1.);
					pcl::PointCloud<pcl::PointXYZ>::Ptr originalCloud_far = rtabmap::util3d::passThrough(originalCloud, "x", 1., std::numeric_limits<int>::max());

					// Part 1: segment floor and obstacles near the robot
					rtabmap::util3d::segmentObstaclesFromGround<pcl::PointXYZ>(
							originalCloud_near,
							ground,
							obstacles,
							normalEstimationRadius_,
							groundNormalAngle_,
							minClusterSize_);

					if(groundPub_.getNumSubscribers() && ground.get() && ground->size())
					{
						pcl::copyPointCloud(*originalCloud_near, *ground, *groundCloud);
						ground->clear();
					}

					if(obstaclesPub_.getNumSubscribers() && obstacles.get() && obstacles->size())
					{
						pcl::copyPointCloud(*originalCloud_near, *obstacles, *obstaclesCloud);
						obstacles->clear();
					}

					// Part 2: segment floor and obstacles far from the robot
					rtabmap::util3d::segmentObstaclesFromGround<pcl::PointXYZ>(
							originalCloud_far,
							ground,
							obstacles,
							3.*normalEstimationRadius_,
							2.*groundNormalAngle_,
							minClusterSize_);

					if(groundPub_.getNumSubscribers() && ground.get() && ground->size())
					{
						pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud2 (new pcl::PointCloud<pcl::PointXYZ>);
						pcl::copyPointCloud(*originalCloud_far, *ground, *groundCloud2);
						*groundCloud += *groundCloud2;
					}


					if(obstaclesPub_.getNumSubscribers() && obstacles.get() && obstacles->size())
					{
						pcl::PointCloud<pcl::PointXYZ>::Ptr obstacles2(new pcl::PointCloud<pcl::PointXYZ>);
						pcl::copyPointCloud(*originalCloud_far, *obstacles, *obstacles2);
						*obstaclesCloud += *obstacles2;
					}
				}
			}
		}

		if(groundPub_.getNumSubscribers())
		{
			sensor_msgs::PointCloud2 rosCloud;
			pcl::toROSMsg(*groundCloud, rosCloud);
			rosCloud.header.stamp = cloudMsg->header.stamp;
			rosCloud.header.frame_id = frameId_;

			//publish the message
			groundPub_.publish(rosCloud);
		}

		if(obstaclesPub_.getNumSubscribers())
		{
			sensor_msgs::PointCloud2 rosCloud;
			pcl::toROSMsg(*obstaclesCloud, rosCloud);
			rosCloud.header.stamp = cloudMsg->header.stamp;
			rosCloud.header.frame_id = frameId_;

			//publish the message
			obstaclesPub_.publish(rosCloud);
		}

		//ROS_INFO("Obstacles segmentation time = %f s", (ros::Time::now() - time).toSec());
	}
Пример #3
0
template<typename PointInT> void
pcl::TextureMapping<PointInT>::mapMultipleTexturesToMeshUV (pcl::TextureMesh &tex_mesh, pcl::texture_mapping::CameraVector &cams)
{

  if (tex_mesh.tex_polygons.size () != cams.size () + 1)
  {
    PCL_ERROR ("The mesh should be divided into nbCamera+1 sub-meshes.\n");
    PCL_ERROR ("You provided %d cameras and a mesh containing %d sub-meshes.\n", cams.size (), tex_mesh.tex_polygons.size ());
    return;
  }

  PCL_INFO ("You provided %d  cameras and a mesh containing %d sub-meshes.\n", cams.size (), tex_mesh.tex_polygons.size ());

  pcl::PointCloud<pcl::PointXYZ>::Ptr originalCloud (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr camera_transformed_cloud (new pcl::PointCloud<pcl::PointXYZ>);

  // convert mesh's cloud to pcl format for ease
  pcl::fromPCLPointCloud2 (tex_mesh.cloud, *originalCloud);

  // texture coordinates for each mesh
  std::vector<std::vector<Eigen::Vector2f> > texture_map;

  for (size_t m = 0; m < cams.size (); ++m)
  {
    // get current camera parameters
    Camera current_cam = cams[m];

    // get camera transform
    Eigen::Affine3f cam_trans = current_cam.pose;

    // transform cloud into current camera frame
    pcl::transformPointCloud (*originalCloud, *camera_transformed_cloud, cam_trans.inverse ());

    // vector of texture coordinates for each face
    std::vector<Eigen::Vector2f> texture_map_tmp;

    // processing each face visible by this camera
    pcl::PointXYZ pt;
    size_t idx;
    for (size_t i = 0; i < tex_mesh.tex_polygons[m].size (); ++i)
    {
      Eigen::Vector2f tmp_VT;
      // for each point of this face
      for (size_t j = 0; j < tex_mesh.tex_polygons[m][i].vertices.size (); ++j)
      {
        // get point
        idx = tex_mesh.tex_polygons[m][i].vertices[j];
        pt = camera_transformed_cloud->points[idx];

        // compute UV coordinates for this point
        getPointUVCoordinates (pt, current_cam, tmp_VT);
        texture_map_tmp.push_back (tmp_VT);

      }// end points
    }// end faces

    // texture materials
    std::stringstream tex_name;
    tex_name << "material_" << m;
    tex_name >> tex_material_.tex_name;
    tex_material_.tex_file = current_cam.texture_file;
    tex_mesh.tex_materials.push_back (tex_material_);

    // texture coordinates
    tex_mesh.tex_coordinates.push_back (texture_map_tmp);
  }// end cameras

  // push on extra empty UV map (for unseen faces) so that obj writer does not crash!
  std::vector<Eigen::Vector2f> texture_map_tmp;
  for (size_t i = 0; i < tex_mesh.tex_polygons[cams.size ()].size (); ++i)
    for (size_t j = 0; j < tex_mesh.tex_polygons[cams.size ()][i].vertices.size (); ++j)
    {
      Eigen::Vector2f tmp_VT;
      tmp_VT[0] = -1;
      tmp_VT[1] = -1;
      texture_map_tmp.push_back (tmp_VT);
    }

  tex_mesh.tex_coordinates.push_back (texture_map_tmp);

  // push on an extra dummy material for the same reason
  std::stringstream tex_name;
  tex_name << "material_" << cams.size ();
  tex_name >> tex_material_.tex_name;
  tex_material_.tex_file = "occluded.jpg";
  tex_mesh.tex_materials.push_back (tex_material_);

}