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; }