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());
}
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_);
}