void compute_normals(const cv::Mat& cloud, cv::Mat& normals)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_cloud( new pcl::PointCloud<pcl::PointXYZ> );
pcl_cloud->clear();
pcl_cloud->width = cloud.cols;
pcl_cloud->height = cloud.rows;
pcl_cloud->points.resize( pcl_cloud->width * pcl_cloud->height);
for(int y = 0; y < cloud.rows; ++y)
for(int x = 0; x < cloud.cols; ++x)
{
pcl_cloud->at(x,y).x = cloud.at<cv::Point3f>(y,x).x;
pcl_cloud->at(x,y).y = cloud.at<cv::Point3f>(y,x).y;
pcl_cloud->at(x,y).z = cloud.at<cv::Point3f>(y,x).z;
}
pcl::PointCloud<pcl::Normal>::Ptr pcl_normals (new pcl::PointCloud<pcl::Normal>);
pcl_normals->clear();
pcl_normals->width = pcl_cloud->width;
pcl_normals->height = pcl_cloud->height;
pcl_normals->points.resize(pcl_cloud->width * pcl_cloud->height);
pcl::IntegralImageNormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
ne.setInputCloud( pcl_cloud );
ne.setNormalSmoothingSize( 5 );
ne.setNormalEstimationMethod(ne.COVARIANCE_MATRIX);
ne.compute( *pcl_normals );
normals.create( cloud.size(), CV_32FC3 );
for(int y = 0; y < pcl_normals->height; ++y)
for(int x = 0; x < pcl_normals->width; ++x)
{
normals.at<cv::Point3f>(y,x).x = pcl_normals->at(x,y).normal_x;
normals.at<cv::Point3f>(y,x).y = pcl_normals->at(x,y).normal_y;
normals.at<cv::Point3f>(y,x).z = pcl_normals->at(x,y).normal_z;
}
}
geometry_msgs::PoseStamped calculateNearestObject(sensor_msgs::LaserScanConstPtr scan)
{
geometry_msgs::PoseStamped observd_obj;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pcl_scan(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::Normal>::Ptr pcl_normals(new pcl::PointCloud<pcl::Normal>);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pcl_with_normals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl_scan->header = scan->header;
pcl_normals->header = scan->header;
pcl_with_normals->header = scan->header;
//reduce scan to a certain angle, see params
sensor_msgs::LaserScanPtr scan_filtered(new sensor_msgs::LaserScan);
laser_filter->update(*scan, *scan_filtered);
hbrs_msgs::LaserScanSegmentList segment_list = scan_segmentation->getSegments(scan_filtered, true);
// find nearest segment in the scan
double min_distance = DBL_MAX, distance = 0, angle = 0;
unsigned int min_segment_index = 0;
for(unsigned int i=0; i < segment_list.segments.size(); ++i)
{
Conversions::cartesian2polar2D(segment_list.segments[i].center.x, segment_list.segments[i].center.y,
distance, angle);
if(distance < min_distance)
{
min_distance = distance;
min_segment_index = i;
}
}
// calculate orientation of the segment
// transform scan to point cloud (the same points is added three times in different heights because the normal estimation is only working then correctly)
for(unsigned int i=0; i < segment_list.segments[min_segment_index].data_points.size(); ++i)
{
pcl::PointXYZRGB p;
p.x = segment_list.segments[min_segment_index].data_points[i].x;
p.y = segment_list.segments[min_segment_index].data_points[i].y;
p.z = 0.0;
pcl_scan->points.push_back(p);
p.x = segment_list.segments[min_segment_index].data_points[i].x;
p.y = segment_list.segments[min_segment_index].data_points[i].y;
p.z = 0.1;
pcl_scan->points.push_back(p);
p.x = segment_list.segments[min_segment_index].data_points[i].x;
p.y = segment_list.segments[min_segment_index].data_points[i].y;
p.z = 0.2;
pcl_scan->points.push_back(p);
}
// calculate normals of the segment points
PCLWrapper<pcl::PointXYZRGB>::computeNormals(pcl_scan, pcl_normals, 0.2);
pcl::concatenateFields (*pcl_scan, *pcl_normals, *pcl_with_normals);
//make mean normal
double normal_x = 0, normal_y = 0;
for(unsigned int j=0; j < pcl_with_normals->points.size(); ++j)
{
normal_x += pcl_with_normals->points[j].normal[0];
normal_y += pcl_with_normals->points[j].normal[1];
}
normal_x /= pcl_with_normals->points.size();
normal_y /= pcl_with_normals->points.size();
//cout << "dist: " << min_distance << " angle: " << atan(normal_y / normal_x) << endl;
observd_obj.header = scan->header;
observd_obj.pose.position = segment_list.segments[min_segment_index].center;
tf::Quaternion quat;
quat = tf::createQuaternionFromYaw(atan(normal_y / normal_x));
observd_obj.pose.orientation.x = quat[0];
observd_obj.pose.orientation.y = quat[1];
observd_obj.pose.orientation.z = quat[2];
observd_obj.pose.orientation.w = quat[3];
if(publish_marker)
{
visualization_msgs::MarkerArray marker_list;
visualization_msgs::Marker marker_shape;
visualization_msgs::Marker marker_orientation;
marker_shape.header.frame_id = scan->header.frame_id;
marker_shape.header.stamp = ros::Time::now();
marker_orientation.header = marker_shape.header;
marker_shape.ns = "nearest_object";
marker_orientation.ns = marker_shape.ns;
marker_shape.action = visualization_msgs::Marker::ADD;
marker_orientation.action = marker_shape.action;
marker_shape.id = 0;
marker_orientation.id = 1;
marker_orientation.type = visualization_msgs::Marker::ARROW;
marker_shape.type = visualization_msgs::Marker::SPHERE;
marker_shape.pose = observd_obj.pose;
marker_orientation.pose = marker_shape.pose;
marker_shape.scale.x = 0.1;
marker_shape.scale.y = 0.1;
marker_shape.scale.z = 0.1;
marker_orientation.scale.x = 0.5;
marker_orientation.scale.y = 0.5;
marker_orientation.scale.z = 0.5;
marker_shape.color.r = 1.0;
marker_shape.color.g = 0.0;
marker_shape.color.b = 0.0;
marker_shape.color.a = 0.5;
marker_orientation.color = marker_shape.color;
marker_shape.lifetime = ros::Duration(1);
marker_orientation.lifetime = marker_shape.lifetime;
marker_list.markers.push_back(marker_orientation);
marker_list.markers.push_back(marker_shape);
pub_markers.publish(marker_list);
}
return observd_obj;
}
