MapBuilder::MapBuilder(std::shared_ptr<Lidar> lidar, std::shared_ptr<BasicPositionTracker> poseTracker)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr _cloud(new pcl::PointCloud<pcl::PointXYZ>);
_cloud->height = 1;
_cloud->width = 1024;
_cloud->is_dense = false;
_cloud->points.resize(_cloud->width*_cloud->height);
if(lidar.get())
{
_lidar = lidar;
connect(_lidar.get(), SIGNAL(onNewData(LidarState)), this, SLOT(onLidarData(LidarState)));
}
this->poseTracker = poseTracker;
cloud = _cloud;
}
void ClusterExtraction::processCloud(float plane_tolerance)
{
ros::Time stamp = ros::Time::now();
if(!pcl_data)
{
ROS_INFO("No xtion_camera data.");
sleep(1);
return;
}
//pcl::PointCloud<PoinT>::Ptr _cloud;// (new pcl::PointCloud<PoinT>);
sensor_msgs::PointCloud2ConstPtr _temp_cloud_ = pcl_data;
pcl::PointCloud<PoinT>::Ptr _cloud (new pcl::PointCloud<PoinT>);
pcl::fromROSMsg(*_temp_cloud_, *_cloud);
pcl::VoxelGrid<PoinT> vg_filter;
pcl::PointCloud<PoinT>::Ptr cloud_filtered (new pcl::PointCloud<PoinT>);
vg_filter.setInputCloud (_cloud);
vg_filter.setLeafSize (0.01f, 0.01f, 0.01f);
vg_filter.filter (*cloud_filtered);
_cloud = cloud_filtered;
/**********************************************
* NEW BULL
*********************************************/
//////////////////////////////////////////////////////////////////////
// Cluster Extraction
//////////////////////////////////////////////////////////////////////
// Findout the points that are more than 1.25 m away.
pcl::PointIndices::Ptr out_of_range_points (new pcl::PointIndices);
unsigned int i = 0;
BOOST_FOREACH(const PoinT& pt, _cloud->points)
{
if(sqrt( (pt.x*pt.x) + (pt.y*pt.y) + (pt.z*pt.z) ) > 1.5 || isnan (pt.x) || isnan (pt.y) || isnan (pt.z) ||
isinf (pt.x) || isinf (pt.y) || isinf (pt.z) )
out_of_range_points->indices.push_back(i);
i++;
}
pcl::ExtractIndices<PoinT> extract;
pcl::PointCloud<PoinT>::Ptr cloud (new pcl::PointCloud<PoinT>);
// Perform the extraction of these points (indices).
extract.setInputCloud(_cloud);
extract.setIndices(out_of_range_points);
extract.setNegative (true);
extract.filter (*cloud);
// Prepare plane segmentation.
pcl::SACSegmentation<PoinT> seg;
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointCloud<PoinT>::Ptr cloud_plane; // This door is opened elsewhere.
pcl::PointCloud<PoinT>::Ptr cloud_f (new pcl::PointCloud<PoinT> ());
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (1000);
seg.setDistanceThreshold (0.02);
// Remove the planes.
i = 0;
int nr_points = (int) cloud->points.size();
tf::StampedTransform base_link_to_openni;
try
{
tf_listener->waitForTransform("base_link", cloud->header.frame_id, ros::Time(0), ros::Duration(1));
//tf_listener->transformPoint("base_link", plane_normal, _plane_normal);
tf_listener->lookupTransform("base_link", cloud->header.frame_id, ros::Time(0), base_link_to_openni);
}
catch(tf::TransformException& ex)
{
ROS_INFO("COCKED UP POINT INFO! Why: %s", ex.what());
}
// We do this here so that we can put in an empty cluster, if we see no table in the first place.
doro_msgs::ClusterArray __clusters;
while (cloud->points.size () > 0.5 * nr_points)
{
seg.setInputCloud (cloud);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
//std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
break;
}
// Extract the planar inliers from the input cloud
extract.setInputCloud (cloud);
extract.setIndices (inliers);
extract.setNegative (false);
extract.filter (*cloud_f);
// Is this a parallel to ground plane? If yes, save it.
tf::Vector3 plane_normal (coefficients->values[0], coefficients->values[1], coefficients->values[2]);
tf::Vector3 _plane_normal = base_link_to_openni*plane_normal;
// What's the angle between this vector and the actual z axis? cos_inverse ( j component )...
tf::Vector3 normal (_plane_normal.x(), _plane_normal.y(), _plane_normal.z());
normal = normal.normalized();
//std::cout<<"x: "<<normal.x()<<"\t";
//std::cout<<"y: "<<normal.y()<<"\t";
//std::cout<<"z: "<<normal.z()<<"\t";
if(acos (normal.z()) < plane_tolerance)
{
cloud_plane = pcl::PointCloud<PoinT>::Ptr(new pcl::PointCloud<PoinT>);
*cloud_plane = *cloud_f;
}
extract.setNegative (true);
extract.filter (*cloud_f);
*cloud = *cloud_f;
}
if(!cloud_plane)
{
ROS_INFO("No table or table-like object could be seen. Can't extract...");
//clusters_pub.publish(__clusters);
//sleep(1);
return;
}
//ROS_INFO("Table seen.");
//table_cloud_pub.publish(cloud_plane);
//////////////////////////////////////////////////////////////////////
/**
* COMPUTE THE CENTROID OF THE PLANE AND PUBLISH IT.
*/
//////////////////////////////////////////////////////////////////////
Eigen::Vector4f plane_cen;
// REMOVE COMMENTS WITH REAL ROBOT!!!
pcl::compute3DCentroid(*cloud_plane, plane_cen);
//std::cout<< plane_cen;
tf::Vector3 plane_centroid (plane_cen[0], plane_cen[1], plane_cen[2]);
tf::Vector3 _plane_centroid = base_link_to_openni*plane_centroid;
geometry_msgs::PointStamped _plane_centroid_ROSMsg;
_plane_centroid_ROSMsg.header.frame_id = "base_link";
_plane_centroid_ROSMsg.header.stamp = stamp;
_plane_centroid_ROSMsg.point.x = _plane_centroid.x();
_plane_centroid_ROSMsg.point.y = _plane_centroid.y();
_plane_centroid_ROSMsg.point.z = _plane_centroid.z();
// Publish the centroid.
table_position_pub.publish(_plane_centroid_ROSMsg);
pcl::search::KdTree<PoinT>::Ptr tree (new pcl::search::KdTree<PoinT>);
if(cloud->points.size() == 0)
{
clusters_pub.publish(__clusters);
return;
}
tree->setInputCloud (cloud);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<PoinT> ec;
ec.setClusterTolerance (0.02); // 2cm
ec.setMinClusterSize (20);
ec.setMaxClusterSize (25000);
ec.setSearchMethod (tree);
ec.setInputCloud (cloud);
ec.extract (cluster_indices);
//ROS_INFO("WIDTH: %d, HEIGHT: %d\n", _cloud->width, _cloud->height);
//ROS_INFO("GOOD TILL HERE!");
int j = 0;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
pcl::PointCloud<PoinT>::Ptr cloud_cluster (new pcl::PointCloud<PoinT>);
cloud_cluster->header.frame_id = cloud->header.frame_id;
long int color_r, color_g, color_b;
uint8_t mean_r, mean_g, mean_b;
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
{
cloud_cluster->points.push_back (cloud->points[*pit]);
/* ***************** */
/* COLOR COMPUTATION */
/* ***************** */
color_r += cloud->points[*pit].r;
color_g += cloud->points[*pit].g;
color_b += cloud->points[*pit].b;
}
geometry_msgs::Point a, b;
std::vector <double> cluster_dims = getClusterDimensions(cloud_cluster, a, b);
mean_r = (uint8_t) (color_r / cloud_cluster->points.size ());
mean_g = (uint8_t) (color_g / cloud_cluster->points.size ());
mean_b = (uint8_t) (color_b / cloud_cluster->points.size ());
cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
cloud_cluster->header.frame_id = cloud->header.frame_id;
Eigen::Vector4f cluster_cen;
pcl::compute3DCentroid(*cloud_cluster, cluster_cen);
tf::Vector3 cluster_centroid (cluster_cen[0], cluster_cen[1], cluster_cen[2]);
tf::Vector3 _cluster_centroid = base_link_to_openni*cluster_centroid;
geometry_msgs::PointStamped _cluster_centroid_ROSMsg;
_cluster_centroid_ROSMsg.header.frame_id = "base_link";
_cluster_centroid_ROSMsg.header.stamp = stamp;
_cluster_centroid_ROSMsg.point.x = _cluster_centroid.x();
_cluster_centroid_ROSMsg.point.y = _cluster_centroid.y();
_cluster_centroid_ROSMsg.point.z = _cluster_centroid.z();
if(DIST(cluster_centroid,plane_centroid) < 0.6 && _cluster_centroid.z() > _plane_centroid.z())
{
doro_msgs::Cluster __cluster;
__cluster.centroid = _cluster_centroid_ROSMsg;
// Push cluster dimentions. Viewed width, breadth and height
__cluster.cluster_size = cluster_dims;
__cluster.a = a;
__cluster.b = b;
// Push colors
__cluster.color.push_back(mean_r);
__cluster.color.push_back(mean_g);
__cluster.color.push_back(mean_b);
__clusters.clusters.push_back (__cluster);
j++;
}
}
clusters_pub.publish(__clusters);
/////////////// RUBBISH ENDS ////////////////
//if(pcl_data)
// pcl_data.reset();
}