Skip to content

Commit

Permalink
removed backsub and added pcl passthrough filter to remove ceiling an…
Browse files Browse the repository at this point in the history 
…d ground
  • Loading branch information
@tzven0
tzven0 committed May 10, 2018
1 parent 00365f0 commit 874cd5e
Showing 1 changed file with 110 additions and 36 deletions.
146 changes: 110 additions & 36 deletions apps/hdl_people_detection_nodelet.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,3 +1,6 @@
#define _USE_MATH_DEFINES
#include <math.h>

#include <mutex>
#include <memory>
#include <iostream>
Expand All @@ -8,6 +11,8 @@

#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/common/transforms.h>
#include <pcl/filters/passthrough.h>
#include <tf_conversions/tf_eigen.h>
#include <tf/transform_broadcaster.h>
#include <message_filters/subscriber.h>
Expand Down Expand Up @@ -46,23 +51,24 @@ class HdlPeopleDetectionNodelet : public nodelet::Nodelet {
initialize_params();

// publishers
backsub_points_pub = private_nh.advertise<sensor_msgs::PointCloud2>("backsub_points", 5);
//backsub_points_pub = private_nh.advertise<sensor_msgs::PointCloud2>("backsub_points", 5);
cluster_points_pub = private_nh.advertise<sensor_msgs::PointCloud2>("cluster_points", 5);
human_points_pub = private_nh.advertise<sensor_msgs::PointCloud2>("human_points", 5);
detection_markers_pub = private_nh.advertise<visualization_msgs::MarkerArray>("detection_markers", 5);

backsub_voxel_points_pub = private_nh.advertise<sensor_msgs::PointCloud2>("backsub_voxel_points", 1, true);
backsub_voxel_markers_pub = private_nh.advertise<visualization_msgs::Marker>("backsub_voxel_marker", 1, true);
//backsub_voxel_points_pub = private_nh.advertise<sensor_msgs::PointCloud2>("backsub_voxel_points", 1, true);
//backsub_voxel_markers_pub = private_nh.advertise<visualization_msgs::Marker>("backsub_voxel_marker", 1, true);

clusters_pub = private_nh.advertise<hdl_people_tracking::ClusterArray>("clusters", 10);

// subscribers
globalmap_sub = nh.subscribe("/globalmap", 1, &HdlPeopleDetectionNodelet::globalmap_callback, this);
//globalmap_sub = nh.subscribe("/globalmap", 1, &HdlPeopleDetectionNodelet::globalmap_callback, this);

odom_sub.reset(new message_filters::Subscriber<nav_msgs::Odometry>(mt_nh, "/odom", 20));
points_sub.reset(new message_filters::Subscriber<sensor_msgs::PointCloud2>(mt_nh, "/velodyne_points", 20));
sync.reset(new message_filters::TimeSynchronizer<nav_msgs::Odometry, sensor_msgs::PointCloud2>(*odom_sub, *points_sub, 20));
sync->registerCallback(boost::bind(&HdlPeopleDetectionNodelet::callback, this, _1, _2));
//odom_sub.reset(new message_filters::Subscriber<nav_msgs::Odometry>(mt_nh, "/odom", 20));
points_sub.reset(new message_filters::Subscriber<sensor_msgs::PointCloud2>(mt_nh, "/scan_matched_points2", 20));
cloud_sub = nh.subscribe("/scan_matched_points2", 20, &HdlPeopleDetectionNodelet::callback, this);
//sync.reset(new message_filters::TimeSynchronizer<nav_msgs::Odometry, sensor_msgs::PointCloud2>(*odom_sub, *points_sub, 20));
//sync->registerCallback(boost::bind(&HdlPeopleDetectionNodelet::callback, this, _1, _2));
}

private:
Expand All @@ -75,6 +81,9 @@ class HdlPeopleDetectionNodelet : public nodelet::Nodelet {
voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution);
downsample_filter = voxelgrid;

float resolution = 0.01f;


NODELET_INFO("create people detector");
detector.reset(new PeopleDetector(private_nh));
}
Expand All @@ -84,11 +93,12 @@ class HdlPeopleDetectionNodelet : public nodelet::Nodelet {
* @param odom_msg sensor pose
* @param points_msg point cloud
*/
void callback(const nav_msgs::OdometryConstPtr& odom_msg, const sensor_msgs::PointCloud2ConstPtr& points_msg) {
if(!globalmap) {
NODELET_ERROR("globalmap has not been received!!");
return;
}
//void callback(const nav_msgs::OdometryConstPtr& odom_msg, const sensor_msgs::PointCloud2ConstPtr& points_msg) {
void callback(const sensor_msgs::PointCloud2ConstPtr& points_msg) {
//if(!globalmap) {
// NODELET_ERROR("globalmap has not been received!!");
// return;
//}

pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>());
pcl::fromROSMsg(*points_msg, *cloud);
Expand All @@ -97,30 +107,93 @@ class HdlPeopleDetectionNodelet : public nodelet::Nodelet {
return;
}

// set global map to cloud for header
globalmap = cloud;

// use pcl passthrough filter to ignore ceilings or points higher than value
// since clustering might take too long when ceiling is clustered with tilted lidar

pcl::PointCloud<PointT>::Ptr cloud_filtered(new pcl::PointCloud<PointT>());
pcl::PassThrough<PointT> pass;
pass.setInputCloud(cloud);
pass.setFilterFieldName("z");
pass.setFilterLimits(-1.0, 1.8);
pass.filter(*cloud_filtered);

cloud = cloud_filtered;

// downsampling
/*
pcl::PointCloud<PointT>::Ptr downsampled(new pcl::PointCloud<PointT>());
downsample_filter->setInputCloud(cloud);
downsample_filter->filter(*downsampled);
downsampled->header = cloud->header;
cloud = downsampled;
*/

// ToDo az,ay,ax definition
//float taz = -M_PI;
//float tay = 0.0;
//float tax = -M_PI*0.250;

//pcl::PointCloud<PointT>::Ptr tcloud = transform_cloud(*cloud,tax,tay,taz);

// transform #cloud into the fixed frame
//const auto& position = odom_msg->pose.pose.position;
//const auto& orientation = odom_msg->pose.pose.orientation;
//tf::Quaternion
//Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
//transform.block<3, 1>(0, 3) = Eigen::Vector3f(position.x, position.y, position.z);
//transform.block<3, 3>(0, 0) = Eigen::Quaternionf(orientation.w, orientation.x, orientation.y, orientation.z).toRotationMatrix();
//pcl::transformPointCloud(*cloud, *cloud, transform);
//cloud->header.frame_id = globalmap->header.frame_id;


// transform #cloud into the globalmap space
const auto& position = odom_msg->pose.pose.position;
const auto& orientation = odom_msg->pose.pose.orientation;
Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
transform.block<3, 1>(0, 3) = Eigen::Vector3f(position.x, position.y, position.z);
transform.block<3, 3>(0, 0) = Eigen::Quaternionf(orientation.w, orientation.x, orientation.y, orientation.z).toRotationMatrix();
pcl::transformPointCloud(*cloud, *cloud, transform);
cloud->header.frame_id = globalmap->header.frame_id;
//const auto& position = odom_msg->pose.pose.position;
//const auto& orientation = odom_msg->pose.pose.orientation;
//Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
//transform.block<3, 1>(0, 3) = Eigen::Vector3f(position.x, position.y, position.z);
//transform.block<3, 3>(0, 0) = Eigen::Quaternionf(orientation.w, orientation.x, orientation.y, orientation.z).toRotationMatrix();
//pcl::transformPointCloud(*cloud, *cloud, transform);
//cloud->header.frame_id = globalmap->header.frame_id;

// background subtraction and people detection
auto filtered = backsub->filter(cloud);
//auto filtered = backsub->filter(cloud);

//auto filtered = tcloud;
auto filtered = cloud;


auto clusters = detector->detect(filtered);

publish_msgs(points_msg->header.stamp, filtered, clusters);
}

void globalmap_callback(const sensor_msgs::PointCloud2ConstPtr& points_msg) {
pcl::PointCloud<PointT>::Ptr transform_cloud(pcl::PointCloud<pcl::PointXYZI> &cloud, float ax, float ay, float az){
Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
// az = angle z; ay = angle y; ax = angle x; (in radians)

transform (0,0) = cos(az) * cos(ay);
transform (0,1) = -sin(az) * cos(ax) + cos(az) * sin(ay) * sin(ax);
transform (0,2) = sin(az) * sin(ax) + cos(az) * sin(ay) * cos(ax);

transform (1,0) = sin(az) * cos(ay);
transform (1,1) = cos(az) * cos(ax) + sin(az) * sin(ay) * sin(ax);
transform (1,2) = -cos(az) * sin(ax) + sin(az) * sin(ay) * cos(ax);

transform (2,0) = -sin(ay);
transform (2,1) = cos(ay) * sin(ax);
transform (2,2) = cos(ay) * cos(ax);

pcl::PointCloud<PointT>::Ptr transformed_cloud (new pcl::PointCloud<PointT>());

pcl::transformPointCloud (cloud, *transformed_cloud, transform);

return transformed_cloud;
}

/*void globalmap_callback(const sensor_msgs::PointCloud2ConstPtr& points_msg) {
NODELET_INFO("globalmap received!");
pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>());
pcl::fromROSMsg(*points_msg, *cloud);
Expand All @@ -137,7 +210,7 @@ class HdlPeopleDetectionNodelet : public nodelet::Nodelet {
backsub_voxel_markers_pub.publish(backsub->create_voxel_marker());
backsub_voxel_points_pub.publish(backsub->voxels());
}
}*/

private:
/**
Expand Down Expand Up @@ -176,9 +249,9 @@ class HdlPeopleDetectionNodelet : public nodelet::Nodelet {
clusters_pub.publish(clusters_msg);
}

if(backsub_points_pub.getNumSubscribers()) {
backsub_points_pub.publish(filtered);
}
//if(backsub_points_pub.getNumSubscribers()) {
// backsub_points_pub.publish(filtered);
//}

if(cluster_points_pub.getNumSubscribers()) {
pcl::PointCloud<pcl::PointXYZI>::Ptr accum(new pcl::PointCloud<pcl::PointXYZI>());
Expand Down Expand Up @@ -222,9 +295,9 @@ class HdlPeopleDetectionNodelet : public nodelet::Nodelet {
markers->markers.reserve(clusters.size());

for(int i=0; i<clusters.size(); i++) {
if(!clusters[i]->is_human) {
continue;
}
//if(!clusters[i]->is_human) {
// continue;
//}

visualization_msgs::Marker cluster_marker;
cluster_marker.header.stamp = stamp;
Expand Down Expand Up @@ -261,29 +334,30 @@ class HdlPeopleDetectionNodelet : public nodelet::Nodelet {
ros::NodeHandle private_nh;

// subscribers
std::unique_ptr<message_filters::Subscriber<nav_msgs::Odometry>> odom_sub;
//std::unique_ptr<message_filters::Subscriber<nav_msgs::Odometry>> odom_sub;
std::unique_ptr<message_filters::Subscriber<sensor_msgs::PointCloud2>> points_sub;
std::unique_ptr<message_filters::TimeSynchronizer<nav_msgs::Odometry, sensor_msgs::PointCloud2>> sync;

ros::Subscriber globalmap_sub;
//ros::Subscriber globalmap_sub;
ros::Subscriber cloud_sub;

// publishers
ros::Publisher backsub_points_pub;
ros::Publisher backsub_voxel_points_pub;
//ros::Publisher backsub_points_pub;
//ros::Publisher backsub_voxel_points_pub;

ros::Publisher cluster_points_pub;
ros::Publisher human_points_pub;

ros::Publisher detection_markers_pub;
ros::Publisher backsub_voxel_markers_pub;
//ros::Publisher backsub_voxel_markers_pub;

ros::Publisher clusters_pub;

// global map
pcl::PointCloud<PointT>::Ptr globalmap;


pcl::Filter<PointT>::Ptr downsample_filter;
std::unique_ptr<BackgroundSubtractor> backsub;
//std::unique_ptr<BackgroundSubtractor> backsub;
std::unique_ptr<PeopleDetector> detector;

};
Expand Down

0 comments on commit 874cd5e

Please sign in to comment.