diff --git a/cfg/config.cfg b/cfg/config.cfg index 39c120c..a6c6309 100755 --- a/cfg/config.cfg +++ b/cfg/config.cfg @@ -14,6 +14,25 @@ gen.add("ybneg", double_t, 0, "Min bound of Y - Axis cropping", -5, -5, 5) gen.add("zbpos", double_t, 0, "Max bound of Z - Axis cropping", 5, -5, 5) gen.add("zbneg", double_t, 0, "Min bound of Z - Axis cropping", 0, -5, 5) +gen.add("rotx", double_t, 0, "Rotation of cloud around X - Axis", 0, 0, 180) +gen.add("roty", double_t, 0, "Rotation of cloud around Y - Axis", 45, 0, 180) +gen.add("rotz", double_t, 0, "Rotation of cloud around Z - Axis", 0, 0, 180) +gen.add("movex", double_t, 0, "Move along X - Axis", 0, -10, 10) +gen.add("movey", double_t, 0, "Move along Y - Axis", 0, -10, 10) +gen.add("movez", double_t, 0, "Move along Z - Axis", 0, -10, 10) + +gen.add("passx_min", double_t, 0, "Start value for Passthrough Filtering for X - Axis", 0.0, -5.0 , 5.0) +gen.add("passx_max", double_t, 0, "End value for Passthrough Filtering for X - Axis", 5.0, -5.0 , 5.0) +gen.add("passy_min", double_t, 0, "Start value for Passthrough Filtering for Y - Axis", -5.0, -5.0 , 5.0) +gen.add("passy_max", double_t, 0, "End value for Passthrough Filtering for Y - Axis", 5.0, -5.0 , 5.0) +gen.add("passz_min", double_t, 0, "Start value for Passthrough Filtering for Z - Axis", 0.0, -5.0 , 5.0) +gen.add("passz_max", double_t, 0, "End value for Passthrough Filtering for Z - Axis", 5.0, -5.0 , 5.0) + +gen.add("voxel_size", double_t, 0, "Size of the Leafs for the Voxel Filter", 0.02, 0.0 , 0.1) + +gen.add("Pseg_dist", double_t, 0, "Point distance for planar segmentation", 0.025, 0.0 , 0.05) + +gen.add("green_thresh", int_t, 0, "Green threshold for color filtering", 150, 0 , 255) exit(gen.generate(PACKAGE, "pcl_tutorial", "config")) diff --git a/pcl_methods.odt b/pcl_methods.odt new file mode 100644 index 0000000..b00a985 Binary files /dev/null and b/pcl_methods.odt differ diff --git a/src/initial_processing.cpp b/src/initial_processing.cpp index 9dec29e..57e0306 100755 --- a/src/initial_processing.cpp +++ b/src/initial_processing.cpp @@ -1,23 +1,33 @@ +#include +#include +#include + #include "ros/ros.h" #include "std_msgs/String.h" #include "std_msgs/Bool.h" #include "std_msgs/Float32.h" #include +#include +#include +#include + #include - - -#include -#include -#include - -#include +#include +#include // used for passthrough filtering #include #include #include #include -#include -#include -#include +#include +#include // for plane segmentation +#include // for plane segmentation +#include +#include +#include // for color filtering +#include // to use the kdtree search in pcl +#include + + /* TBD: get to know typedef such as stringstream @@ -25,7 +35,9 @@ TBD: get to know typedef such as stringstream // define publisher ros::Publisher pub; -ros::Publisher pub_2; +ros::Publisher pub2; +ros::Publisher marker_pub; +ros::Publisher marker_pub2; ros::Publisher visual; float xbpos = 5.0; @@ -34,6 +46,21 @@ float ybpos = 5.0; float ybneg = -5.0; float zbpos = 5.0; float zbneg = 0.0; +float rotx = 0; +float roty = 45; +float rotz = 0; +float movex = 0.0; +float movey = 0.0; +float movez = 0.0; +float passx_min = 0.0; +float passx_max = 5.0; +float passy_min = -5.0; +float passy_max = 5.0; +float passz_min = 0.0; +float passz_max = 5.0; +float voxel_size = 0.02; +float Pseg_dist = 0.025; +int green_thresh = 150; typedef pcl::PointXYZRGB PointT; @@ -44,49 +71,224 @@ void callback(pcl_tutorial::configConfig &config, uint32_t level) ybpos = config.ybpos; ybneg = config.ybneg; zbpos = config.zbpos; - zbneg = config.zbneg; + zbneg = config.zbneg; + rotx = config.rotx; + roty = config.roty; + rotz = config.rotz; + movex = config.movex; + movey = config.movey; + movez = config.movez; + passx_min = config.passx_min; + passx_max = config.passx_max; + passy_min = config.passy_min; + passy_max = config.passy_max; + passz_min = config.passz_min; + passz_max = config.passz_max; + voxel_size = config.voxel_size; + Pseg_dist = config.Pseg_dist; + green_thresh = config.green_thresh; } void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg){ - - // create pointer to Point Cloud in pcl + // create pointer to Point Clouds in pcl pcl::PointCloud::Ptr cloud(new pcl::PointCloud); - pcl::PointCloud::Ptr cloud_filtered (new pcl::PointCloud); + //pcl::PointCloud::Ptr cloud_filtered (new pcl::PointCloud); + + // publish unprocessed point cloud + sensor_msgs::PointCloud2 input; + input = *cloud_msg; + pub.publish(input); // convert ROS message data to point cloud in pcl pcl::fromROSMsg(*cloud_msg, *cloud); - - // Create filtering object + //Perform a translation and rotation on the object cloud using rqt: dynamic reconfigure + Eigen::Affine3f transform = Eigen::Affine3f::Identity(); + transform.translation() << movex, movey, movez; + transform.rotate(Eigen::AngleAxisf((rotx*M_PI) / 180, Eigen::Vector3f::UnitX())); + transform.rotate(Eigen::AngleAxisf((roty*M_PI) / 180, Eigen::Vector3f::UnitY())); + transform.rotate(Eigen::AngleAxisf((rotz*M_PI) / 180, Eigen::Vector3f::UnitZ())); + pcl::transformPointCloud(*cloud, *cloud, transform); + + // Create filtering objects for all 3 coordinates pcl::PassThrough passx; passx.setInputCloud (cloud); passx.setFilterFieldName ("x"); - passx.setFilterLimits (0.0, 5.0); + passx.setFilterLimits (passx_min, passx_max); passx.filter (*cloud); pcl::PassThrough passy; passy.setInputCloud (cloud); passy.setFilterFieldName ("y"); - passy.setFilterLimits (-5.0, 5.0); + passy.setFilterLimits (passy_min, passy_max); passy.filter (*cloud); pcl::PassThrough passz; passz.setInputCloud (cloud); passz.setFilterFieldName ("z"); - passz.setFilterLimits (0.0, 5.0); - passz.filter (*cloud); + passz.setFilterLimits (passz_min, passz_max); + passz.filter (*cloud); +/* // Visualizing the bound of the passthrough filter in RVIZ + visualization_msgs::Marker bounding_box; + bounding_box.id = 0; + bounding_box.type = visualization_msgs::Marker::CUBE; + bounding_box.color.g = 1.0; + bounding_box.color.a = 0.5; + bounding_box.header.frame_id = "zed_base_link"; + bounding_box.action = visualization_msgs::Marker::ADD; + bounding_box.pose.orientation.w = 1.0; + bounding_box.pose.position.x = (passx_max+passx_min)/2; + bounding_box.pose.position.y = (passy_max+passy_min)/2; + bounding_box.pose.position.z = (passz_max+passz_min)/2; + bounding_box.scale.x = abs(passx_max-passx_min); + bounding_box.scale.y = abs(passy_max-passy_min); + bounding_box.scale.z = abs(passz_max-passz_min); + marker_pub.publish(bounding_box); */ -/* pcl::VoxelGrid vox; // Create the object for performing voxelgrid filtering //Maybe this has to be changes to RGB as well + visualization_msgs::Marker line_list; + line_list.id = 1; + line_list.type = visualization_msgs::Marker::LINE_LIST; + line_list.scale.x = 0.05; + line_list.color.g = 1.0; + line_list.color.a = 1.0; + line_list.header.frame_id = "zed_base_link"; + line_list.action = visualization_msgs::Marker::ADD; + line_list.pose.orientation.w = 1.0; + geometry_msgs::Point p1; geometry_msgs::Point p2; geometry_msgs::Point p3; geometry_msgs::Point p4; + geometry_msgs::Point p5; geometry_msgs::Point p6; geometry_msgs::Point p7; geometry_msgs::Point p8; + p1.x = passx_min; p1.y = passy_min; p1.z = passz_min; + p2.x = passx_min; p2.y = passy_max; p2.z = passz_min; + p3.x = passx_max; p3.y = passy_min; p3.z = passz_min; + p4.x = passx_max; p4.y = passy_max; p4.z = passz_min; + p5.x = passx_min; p5.y = passy_min; p5.z = passz_max; + p6.x = passx_min; p6.y = passy_max; p6.z = passz_max; + p7.x = passx_max; p7.y = passy_min; p7.z = passz_max; + p8.x = passx_max; p8.y = passy_max; p8.z = passz_max; + line_list.points.push_back(p1); line_list.points.push_back(p2); + line_list.points.push_back(p1); line_list.points.push_back(p3); + line_list.points.push_back(p4); line_list.points.push_back(p2); + line_list.points.push_back(p4); line_list.points.push_back(p3); + line_list.points.push_back(p1); line_list.points.push_back(p5); + line_list.points.push_back(p2); line_list.points.push_back(p6); + line_list.points.push_back(p3); line_list.points.push_back(p7); + line_list.points.push_back(p4); line_list.points.push_back(p8); + line_list.points.push_back(p5); line_list.points.push_back(p6); + line_list.points.push_back(p5); line_list.points.push_back(p7); + line_list.points.push_back(p8); line_list.points.push_back(p6); + line_list.points.push_back(p8); line_list.points.push_back(p7); + marker_pub2.publish(line_list); + + pcl::VoxelGrid vox; // Create the object for performing voxelgrid filtering //Maybe this has to be changes to RGB as well vox.setInputCloud (cloud); // Set input cloud - vox.setLeafSize (0.02, 0.02, 0.02); // Set the size of the voxel - vox.filter (*cloud); */ + vox.setLeafSize (voxel_size, voxel_size, voxel_size); // Set the size of the voxel [m] + vox.filter (*cloud); + + // Planar segmentation + // coefficients: used to show the contents of inliers set together with estimated plane parameters + pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); + pcl::PointIndices::Ptr inliers(new pcl::PointIndices); + // Create the segmentation object + pcl::SACSegmentation seg; + // Optional + seg.setOptimizeCoefficients (true); + // Mandatory: set model and method type + // distance threshold defines how close a point has to be to the model to be considered an inlier + seg.setModelType (pcl::SACMODEL_PLANE); + seg.setMethodType (pcl::SAC_RANSAC); // ransac is a simple and robust method + seg.setDistanceThreshold (Pseg_dist); + seg.setInputCloud (cloud); + seg.segment (*inliers, *coefficients); + if (inliers->indices.size () == 0){ + PCL_ERROR ("Could not estimate a planar model for the given dataset."); + std::cout << "Could not estimate a planar model"; + //return (-1); + } + //Extraction step of the inliers + pcl::ExtractIndices extract; + extract.setInputCloud (cloud); + extract.setIndices (inliers); + extract.setNegative (true); + //extract.filter(*cloud); + pcl::PointCloud cloudF; + extract.filter(cloudF); + cloud->swap(cloudF); + + // color filtering + // Building the condition for points to remain in cloud + pcl::ConditionAnd::Ptr color_condition (new pcl::ConditionAnd ()); + pcl::PackedRGBComparison::Ptr green_condition(new pcl::PackedRGBComparison("g", pcl::ComparisonOps::GT, green_thresh)); + color_condition->addComparison(green_condition); + // Building the filter + pcl::ConditionalRemoval color_filter; + color_filter.setCondition (color_condition); + color_filter.setInputCloud(cloud); + // apply the filter + color_filter.filter(*cloud); + + + /* // Creating the KdTree object for the search method of the extraction + pcl::search::KdTree::Ptr tree (new pcl::search::KdTree); + tree->setInputCloud (cloud); + + std::vector cluster_indices; + pcl::EuclideanClusterExtraction ec; + ec.setClusterTolerance (0.2); // 2cm + ec.setMinClusterSize (50); + ec.setMaxClusterSize (25000); + ec.setSearchMethod (tree); + ec.setInputCloud (cloud); + ec.extract (cluster_indices); + ROS_INFO_STREAM(cluster_indices.size()<<" clusters extracted "); + + Eigen::Vector4f centroidL; + Eigen::Vector4f minL; + Eigen::Vector4f maxL; + Eigen::Vector4f centroidR; + Eigen::Vector4f minR; + Eigen::Vector4f maxR; + bool first_Lcentroid_detected = false; + bool first_Rcentroid_detected = false; + Eigen::Vector4f centroidCENT; + Eigen::Vector4f minCENT; + Eigen::Vector4f maxCENT; + bool first_centroid_detected = false; + + for (std::vector::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it){ + pcl::PointCloud::Ptr cloud_clusterL (new pcl::PointCloud); + for (const auto& idx : it->indices){ + cloud_clusterL->push_back ((*cloud)[idx]); //* + cloud_clusterL->width = cloud_clusterL->size (); + cloud_clusterL->height = 1; + cloud_clusterL->is_dense = true; + Eigen::Vector4f centroid; + Eigen::Vector4f min; + Eigen::Vector4f max; + pcl::compute3DCentroid (*cloud_clusterL, centroid); + pcl::getMinMax3D (*cloud_clusterL, min, max); + } + + if (first_centroid_detected){ + if (abs(centroid[1]) < abs(centroidCENT[1])){ + centroidCENT = centroid; + minCENT = min; + maxCENT = max; + } + } + else{ + centroidCENT = centroid; + minCENT = min; + maxCENT = max; + first_centroid_detected = true; + } + } */ // convert to ROS data type sensor_msgs::PointCloud2 output; pcl::toROSMsg(*cloud, output); - pub.publish(output); + pub2.publish(output); + } @@ -104,11 +306,13 @@ int main(int argc, char **argv){ server.setCallback(f); // create subscriber to receive point cloud - ros::Subscriber sub = n.subscribe("/zed/zed_node/point_cloud/cloud_registered", 1, cloud_cb); + ros::Subscriber sub = n.subscribe("/zed2i/zed_node/point_cloud/cloud_registered", 1, cloud_cb); // create publisher for output processed point cloud - pub = n.advertise("processed_cloud", 1); - pub_2 = n.advertise("unprocessed_cloud", 1); + pub = n.advertise("unprocessed_cloud", 1); + pub2 = n.advertise("processed_cloud", 1); + marker_pub = n.advertise("visualization_marker", 10); + marker_pub2 = n.advertise("visualization_marker2", 10); // create publisher for visualising marker visual = n.advertise ("marker", 1);