andrew/pcl_tutorial
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

kdtree needs to be altered

Browse Source
This commit is contained in:
caroline 2022-04-25 16:34:21 +02:00
parent 5d509e4e85
commit ff7823c83f
3 changed files with 251 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
cfg/config.cfg
View File

@ -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"))

BIN
pcl_methods.odt Normal file
View File

Binary file not shown.

260
src/initial_processing.cpp
View File

@ -1,23 +1,33 @@
#include <sstream>
#include <iostream>
#include <math.h>
#include "ros/ros.h"
#include "std_msgs/String.h"
#include "std_msgs/Bool.h"
#include "std_msgs/Float32.h"
#include <dynamic_reconfigure/server.h>
#include <sensor_msgs/PointCloud2.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <pcl_tutorial/configConfig.h>
#include <sstream>
#include <iostream>
#include <math.h>
#include <pcl/filters/passthrough.h>
#include <pcl/point_types.h>
#include <pcl/filters/passthrough.h> // used for passthrough filtering
#include <pcl/conversions.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/voxel_grid.h>
#include <sensor_msgs/PointCloud2.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <pcl/common/transforms.h>
#include <pcl/ModelCoefficients.h> // for plane segmentation
#include <pcl/segmentation/sac_segmentation.h> // for plane segmentation
#include <pcl/filters/extract_indices.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/filters/conditional_removal.h> // for color filtering
#include <pcl/kdtree/kdtree_flann.h> // to use the kdtree search in pcl
#include <pcl/kdtree/kdtree.h>
/*
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<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
pcl::PointCloud<PointT>::Ptr cloud_filtered (new pcl::PointCloud<PointT>);
//pcl::PointCloud<PointT>::Ptr cloud_filtered (new pcl::PointCloud<PointT>);
// 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<PointT> passx;
passx.setInputCloud (cloud);
passx.setFilterFieldName ("x");
passx.setFilterLimits (0.0, 5.0);
passx.setFilterLimits (passx_min, passx_max);
passx.filter (*cloud);
pcl::PassThrough<PointT> passy;
passy.setInputCloud (cloud);
passy.setFilterFieldName ("y");
passy.setFilterLimits (-5.0, 5.0);
passy.setFilterLimits (passy_min, passy_max);
passy.filter (*cloud);
pcl::PassThrough<PointT> 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<PointT> 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<PointT> 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<PointT> 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<PointT> extract;
extract.setInputCloud (cloud);
extract.setIndices (inliers);
extract.setNegative (true);
//extract.filter(*cloud);
pcl::PointCloud<PointT> cloudF;
extract.filter(cloudF);
cloud->swap(cloudF);
// color filtering
// Building the condition for points to remain in cloud
pcl::ConditionAnd<PointT>::Ptr color_condition (new pcl::ConditionAnd<PointT> ());
pcl::PackedRGBComparison<PointT>::Ptr green_condition(new pcl::PackedRGBComparison<pcl::PointXYZRGB>("g", pcl::ComparisonOps::GT, green_thresh));
color_condition->addComparison(green_condition);
// Building the filter
pcl::ConditionalRemoval<PointT> 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<PointT>::Ptr tree (new pcl::search::KdTree<PointT>);
tree->setInputCloud (cloud);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<PointT> 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<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it){
pcl::PointCloud<PointT>::Ptr cloud_clusterL (new pcl::PointCloud<PointT>);
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<sensor_msgs::PointCloud2>("/zed/zed_node/point_cloud/cloud_registered", 1, cloud_cb);
ros::Subscriber sub = n.subscribe<sensor_msgs::PointCloud2>("/zed2i/zed_node/point_cloud/cloud_registered", 1, cloud_cb);
// create publisher for output processed point cloud
pub = n.advertise<sensor_msgs::PointCloud2>("processed_cloud", 1);
pub_2 = n.advertise<sensor_msgs::PointCloud2>("unprocessed_cloud", 1);
pub = n.advertise<sensor_msgs::PointCloud2>("unprocessed_cloud", 1);
pub2 = n.advertise<sensor_msgs::PointCloud2>("processed_cloud", 1);
marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 10);
marker_pub2 = n.advertise<visualization_msgs::Marker>("visualization_marker2", 10);
// create publisher for visualising marker
visual = n.advertise<visualization_msgs::Marker> ("marker", 1);