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

line extracion from clusters

Browse Source
This commit is contained in:
caroline 2022-05-03 16:38:52 +02:00
parent 8f4ec0bf9f
commit 34b4d62856
5 changed files with 413 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
CMakeLists.txt
View File

@ -11,10 +11,13 @@ find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
sensor_msgs
genmsg
pcl_conversions
pcl_ros
dynamic_reconfigure
cv_bridge
image_transport
)
#add dynamic reconfigure api
@ -213,7 +216,7 @@ target_link_libraries(talker ${catkin_LIBRARIES})
add_dependencies(talker pcl_tutorial_generate_messages_cpp)
add_executable(initial_processing src/initial_processing.cpp)
target_link_libraries(initial_processing ${catkin_LIBRARIES})
target_link_libraries(initial_processing ${catkin_LIBRARIES}${OpenCV_LIBRARIES})
add_dependencies(initial_processing pcl_tutorial_generate_messages_cpp)
# make sure configure headers are built before any node using them

4
cfg/config.cfg
View File

@ -37,4 +37,8 @@ gen.add("green_thresh", int_t, 0, "Green threshold for color filtering", 150,
gen.add("cluster_on", bool_t, 0, "determines if Euclidean Clustering is applied", False)
gen.add("create_feature_map", bool_t, 0, "determines if feature map will be created", False)
gen.add("line_fitting_on", bool_t, 0, "determines line will be searched for clusters", False)
exit(gen.generate(PACKAGE, "pcl_tutorial", "config"))

BIN
pcl_methods.odt
View File

Binary file not shown.

246
src/initial_processing.cpp
View File

@ -1,6 +1,7 @@
#include <sstream>
#include <iostream>
#include <math.h>
#include <Eigen/Geometry>
#include "ros/ros.h"
#include "std_msgs/String.h"
@ -27,7 +28,19 @@
#include <pcl/kdtree/kdtree.h>
#include <pcl/features/normal_3d.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/range_image/range_image.h>
#include <pcl/visualization/range_image_visualizer.h>
#include <pcl/common/common_headers.h>
#include <pcl/io/pcd_io.h>
#include <pcl/console/parse.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/sample_consensus/sac_model_line.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/Image.h>
/*
TBD: get to know typedef such as stringstream
@ -37,10 +50,17 @@ TBD: get to know typedef such as stringstream
ros::Publisher pub;
ros::Publisher pub2;
ros::Publisher pub3;
ros::Publisher pub4;
ros::Publisher marker_pub;
ros::Publisher marker_pub2;
ros::Publisher visual;
ros::Publisher steer;
ros::Publisher image_pub;
ros::Publisher line_pub;
ros::Publisher pub4_1;
ros::Publisher pub4_2;
ros::Publisher pub4_3;
ros::Publisher pub4_4;
float xbpos = 5.0;
float xbneg = 0.0;
@ -49,7 +69,7 @@ float ybneg = -5.0;
float zbpos = 5.0;
float zbneg = 0.0;
float rotx = 0;
float roty = 45;
float roty = 0;
float rotz = 0;
float movex = 0.0;
float movey = 0.0;
@ -64,8 +84,21 @@ float voxel_size = 0.02;
float Pseg_dist = 0.025;
int green_thresh = 150;
bool cluster_on = true;
bool create_feature_map = false;
bool line_fitting_on = false;
typedef pcl::PointXYZRGB PointT;
typedef pcl::PointXYZ PointType;
/*
void setViewerPose (pcl::visualization::PCLVisualizer& viewer, const Eigen::Affine3f& viewer_pose)
{
Eigen::Vector3f pos_vector = viewer_pose * Eigen::Vector3f(0, 0, 0);
Eigen::Vector3f look_at_vector = viewer_pose.rotation () * Eigen::Vector3f(0, 0, 1) + pos_vector;
Eigen::Vector3f up_vector = viewer_pose.rotation () * Eigen::Vector3f(0, -1, 0);
viewer.setCameraPosition (pos_vector[0], pos_vector[1], pos_vector[2],
look_at_vector[0], look_at_vector[1], look_at_vector[2],
up_vector[0], up_vector[1], up_vector[2]);
}*/
void callback(pcl_tutorial::configConfig &config, uint32_t level)
{
@ -92,6 +125,7 @@ void callback(pcl_tutorial::configConfig &config, uint32_t level)
Pseg_dist = config.Pseg_dist;
green_thresh = config.green_thresh;
cluster_on = config.cluster_on;
line_fitting_on = config.line_fitting_on;
}
void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg){
@ -116,6 +150,20 @@ void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg){
transform.rotate(Eigen::AngleAxisf((rotz*M_PI) / 180, Eigen::Vector3f::UnitZ()));
pcl::transformPointCloud(*cloud, *cloud, transform);
/*
sensor_msgs::PointCloud2 transformed_cloud;
pcl::toROSMsg(*cloud, transformed_cloud);
transformed_cloud.header.frame_id = "base_link";
pub4.publish(transformed_cloud);
*/
/* // Getting an image from PointCloud
sensor_msgs::Image image_;
pcl::toROSMsg (*cloud, image_);
image_.header.frame_id = "base_link";
image_pub.publish(image_);
*/
// Create filtering objects for all 3 coordinates
pcl::PassThrough<PointT> passx;
passx.setInputCloud (cloud);
@ -233,7 +281,7 @@ void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg){
// apply the filter
color_filter.filter(*cloud);
if (cluster_on == true){
//if (cluster_on == true){
// Creating the KdTree object for the search method of the extraction
// https://pcl.readthedocs.io/projects/tutorials/en/master/cluster_extraction.html
pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT>);
@ -251,6 +299,7 @@ void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg){
int count = 0;
pcl::PointCloud<PointT> cluster_vector[cluster_indices.size()];
pcl::PointCloud<PointT>::Ptr cluster_vector_ptr[cluster_indices.size()];
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it){
pcl::PointCloud<PointT>::Ptr cloud_cluster (new pcl::PointCloud<PointT>);
@ -260,6 +309,7 @@ void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg){
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
cluster_vector[count] = *cloud_cluster;
cluster_vector_ptr[count] = cloud_cluster;
}
count++;
}
@ -295,7 +345,7 @@ void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg){
color_id = 0;
}
}
center_point.pose.orientation.w = 1.0;
center_point.scale.x = 0.1;
@ -308,10 +358,9 @@ void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg){
center_point.color.a = 1.0;
center_point.lifetime = ros::Duration();
center_point.header.frame_id = "zed2i_base_link";
center_point.header.frame_id = "base_link";
steer.publish (center_point);
pcl::PointCloud<PointT> clusters;
for (int j = 0; j < sizeof(cluster_vector)/sizeof(cluster_vector[0]); j++){
clusters += cluster_vector[j];
@ -319,8 +368,185 @@ void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg){
sensor_msgs::PointCloud2 clustered_cloud;
pcl::toROSMsg(clusters, clustered_cloud);
clustered_cloud.header.frame_id = "zed2i_base_link";
clustered_cloud.header.frame_id = "base_link";
pub3.publish(clustered_cloud);
//}
/*
if (create_feature_map == true){
float angularResolution_x = (float) ( 1.0f * (M_PI/180.0f)); // 1.0 degree in radians
float angularResolution_y = (float) ( 1.0f * (M_PI/180.0f));
float maxAngleWidth = (float) (360.0f * (M_PI/180.0f)); // 360.0 degree in radians
float maxAngleHeight = (float) (180.0f * (M_PI/180.0f)); // 180.0 degree in radians
Eigen::Affine3f sensorPose = Eigen::Affine3f::Identity();//(Eigen::Affine3f)Eigen::Translation3f(0.0f, 0.0f, 0.0f);
pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME;
bool live_update = false;
float noiseLevel=0.00;
float minRange = 0.0f;
int borderSize = 1;
pcl::PointCloud<PointT>::Ptr point_cloud_ptr (new pcl::PointCloud<PointT>);
pcl::PointCloud<PointT>& point_cloud = *cloud;
//pcl::RangeImage range_image;
pcl::RangeImage::Ptr range_image_ptr(new pcl::RangeImage);
//pcl::RangeImage& range_image = *range_image_ptr;
//boost::shared_ptr<pcl::RangeImage> range_image_ptr(new pcl::RangeImage);
pcl::RangeImage& range_image = *range_image_ptr;
range_image.createFromPointCloud(*cloud, angularResolution_x, maxAngleWidth, maxAngleHeight,
sensorPose, coordinate_frame, noiseLevel, minRange, borderSize);
int cols = range_image.width;
int rows = range_image.height;
//Conversion factor
float factor = 1.0f/(50-0.5);
float offset = -0.5;
std::string encoding = "mono16";
cv::Mat _rangeImage = cv::Mat::zeros(rows, cols, cv_bridge::getCvType(encoding));
for (int i=0; i<cols; ++i)
{
for (int j=0; j<rows; ++j)
{
float r = range_image_ptr->getPoint(i, j).range;
float range = (!std::isinf(r))?
std::max(0.0f, std::min(1.0f, factor * (r + offset))):
0.0;
_rangeImage.at<ushort>(j, i) = static_cast<ushort>((range) * std::numeric_limits<ushort>::max());
}
}
sensor_msgs::ImagePtr msg;
msg = cv_bridge::CvImage(std_msgs::Header(),encoding,range_image_ptr).toImageMsg();
pcl_conversions::fromPCL(range_image_ptr->header, msg->header);
pub4.publish(msg);
//pcl::visualization::PCLVisualizer viewer ("Viewer");
//viewer.setBackgroundColor (1, 1, 1);
}/*
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointWithRange> range_image_color_handler (range_image_ptr, 0, 0, 0);
viewer.addPointCloud (range_image_ptr, range_image_color_handler, "range image");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "range image");
}/*
viewer.initCameraParameters ();
//setViewerPose(viewer,range_image.getTransformationToWorldSystem ());
pcl::visualization::RangeImageVisualizer range_image_widget ("Range image");
range_image_widget.showRangeImage (range_image);
}
/*
Eigen::Affine3f scene_sensor_pose (Eigen::Affine3f::Identity ());
while (!viewer.wasStopped ())
{
range_image_widget.spinOnce ();
viewer.spinOnce ();
pcl_sleep (0.01);
if (live_update)
{
scene_sensor_pose = viewer.getViewerPose();
range_image.createFromPointCloud (cloud, angularResolution_x, angularResolution_y,
pcl::deg2rad (360.0f), pcl::deg2rad (180.0f),
scene_sensor_pose, pcl::RangeImage::LASER_FRAME, noiseLevel, minRange, borderSize);
range_image_widget.showRangeImage (range_image);
}
}
}*/
if (line_fitting_on == true){
int cluster_num = sizeof(cluster_vector)/sizeof(cluster_vector[0]);
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
if(cluster_num >= 4){
geometry_msgs::Point point;
visualization_msgs::Marker middle_line;
uint32_t shape = visualization_msgs::Marker::LINE_LIST;
middle_line.header.stamp = ros::Time::now();
middle_line.type = shape;
middle_line.action = visualization_msgs::Marker::ADD;
pcl::PointCloud<PointT> cluster_lines;
for(int i = 0; i<cluster_num; i++){
pcl::SampleConsensusModelLine<PointT>::Ptr model(new pcl::SampleConsensusModelLine<PointT> (cluster_vector_ptr[i]));
pcl::RandomSampleConsensus<PointT> ransac (model);
pcl::PointCloud<PointT>::Ptr final (new pcl::PointCloud<PointT>);
std::vector<int> inliers_;
ransac.setDistanceThreshold (.01);
ransac.computeModel();
ransac.getInliers(inliers_);
pcl::copyPointCloud (*cluster_vector_ptr[i], inliers_, *final);
cluster_lines += *final;
Eigen::VectorXf line_coefficients;
//*ransac.segment (*inliers, *line_coefficients);
//pcl::ModelCoefficients::Ptr line_coefficients_ (new pcl::ModelCoefficients);
ransac.getModelCoefficients (line_coefficients);
double pt_line_x = line_coefficients[0]; // alternatively: const auto
double pt_line_y = line_coefficients[1];
double pt_line_z = line_coefficients[2];
double pt_direction_x = line_coefficients[3];
double pt_direction_y = line_coefficients[4];
double pt_direction_z = line_coefficients[5];
geometry_msgs::Point point;
point.x = pt_line_x;
point.y = pt_line_y;
point.z = pt_line_z;
middle_line.points.push_back(point);
point.x = pt_line_x+pt_direction_x;
point.y = pt_line_y+pt_direction_y;
point.z = pt_line_z+pt_direction_z;
middle_line.points.push_back(point);
}
middle_line.pose.orientation.w = 1.0;
middle_line.scale.x = 0.02;
middle_line.scale.y = 0.02;
middle_line.scale.z = 0.02;
middle_line.color.r = 1.0;
middle_line.color.g = 1.0;
middle_line.color.b = 1.0;
middle_line.color.a = 1.0;
middle_line.lifetime = ros::Duration();
middle_line.header.frame_id = "base_link";
line_pub.publish (middle_line);
//sensor_msgs::PointCloud2 line_cloud_1,line_cloud_2,line_cloud_3,line_cloud_4;
/*pcl::toROSMsg(*final_1, line_cloud_1);
pcl::toROSMsg(*final_2, line_cloud_2);
pcl::toROSMsg(*final_3, line_cloud_3);
pcl::toROSMsg(*final_4, line_cloud_4);
line_cloud_1.header.frame_id = "base_link";
line_cloud_2.header.frame_id = "base_link";
line_cloud_3.header.frame_id = "base_link";
line_cloud_4.header.frame_id = "base_link";
//pub4_2.publish(line_cloud_2);
//pub4_3.publish(line_cloud_3);
//pub4_4.publish(line_cloud_4);*/
sensor_msgs::PointCloud2 line_cloud;
pcl::toROSMsg(cluster_lines, line_cloud);
line_cloud.header.frame_id = "base_link";
pub4_1.publish(line_cloud);
}
}
// convert to ROS data type
@ -349,9 +575,17 @@ int main(int argc, char **argv){
pub = n.advertise<sensor_msgs::PointCloud2>("unprocessed_cloud", 1);
pub2 = n.advertise<sensor_msgs::PointCloud2>("processed_cloud", 1);
pub3 = n.advertise<sensor_msgs::PointCloud2>("clustered_cloud", 1);
pub4 = n.advertise<sensor_msgs::PointCloud2>("line_cloud", 1);
marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 10);
marker_pub2 = n.advertise<visualization_msgs::Marker>("visualization_marker2", 10);
steer = n.advertise<visualization_msgs::Marker>("centroid_marker", 1);
image_pub = n.advertise<sensor_msgs::Image>("pcl_image", 30);
line_pub = n.advertise<visualization_msgs::Marker>("center_line", 10);
pub4_1 = n.advertise<sensor_msgs::PointCloud2>("line_cloud_1", 1);
pub4_2 = n.advertise<sensor_msgs::PointCloud2>("line_cloud_2", 1);
pub4_3 = n.advertise<sensor_msgs::PointCloud2>("line_cloud_3", 1);
pub4_4 = n.advertise<sensor_msgs::PointCloud2>("line_cloud_4", 1);
// create publisher for visualising marker
visual = n.advertise<visualization_msgs::Marker> ("marker", 1);

165
src/range_image.cpp Normal file
View File

@ -0,0 +1,165 @@
#include <iostream>
#include <pcl/common/common_headers.h>
#include <pcl/range_image/range_image.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/range_image_visualizer.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/parse.h>
typedef pcl::PointXYZ PointType;
// --------------------
// -----Parameters-----
// --------------------
float angular_resolution_x = 0.5f,
angular_resolution_y = angular_resolution_x;
pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME;
bool live_update = false;
// --------------
// -----Help-----
// --------------
void
printUsage (const char* progName)
{
std::cout << "\n\nUsage: "<<progName<<" [options] <scene.pcd>\n\n"
<< "Options:\n"
<< "-------------------------------------------\n"
<< "-rx <float> angular resolution in degrees (default "<<angular_resolution_x<<")\n"
<< "-ry <float> angular resolution in degrees (default "<<angular_resolution_y<<")\n"
<< "-c <int> coordinate frame (default "<< (int)coordinate_frame<<")\n"
<< "-l live update - update the range image according to the selected view in the 3D viewer.\n"
<< "-h this help\n"
<< "\n\n";
}
void
setViewerPose (pcl::visualization::PCLVisualizer& viewer, const Eigen::Affine3f& viewer_pose)
{
Eigen::Vector3f pos_vector = viewer_pose * Eigen::Vector3f(0, 0, 0);
Eigen::Vector3f look_at_vector = viewer_pose.rotation () * Eigen::Vector3f(0, 0, 1) + pos_vector;
Eigen::Vector3f up_vector = viewer_pose.rotation () * Eigen::Vector3f(0, -1, 0);
viewer.setCameraPosition (pos_vector[0], pos_vector[1], pos_vector[2],
look_at_vector[0], look_at_vector[1], look_at_vector[2],
up_vector[0], up_vector[1], up_vector[2]);
}
// --------------
// -----Main-----
// --------------
int
main (int argc, char** argv)
{
// --------------------------------------
// -----Parse Command Line Arguments-----
// --------------------------------------
if (pcl::console::find_argument (argc, argv, "-h") >= 0)
{
printUsage (argv[0]);
return 0;
}
if (pcl::console::find_argument (argc, argv, "-l") >= 0)
{
live_update = true;
std::cout << "Live update is on.\n";
}
if (pcl::console::parse (argc, argv, "-rx", angular_resolution_x) >= 0)
std::cout << "Setting angular resolution in x-direction to "<<angular_resolution_x<<"deg.\n";
if (pcl::console::parse (argc, argv, "-ry", angular_resolution_y) >= 0)
std::cout << "Setting angular resolution in y-direction to "<<angular_resolution_y<<"deg.\n";
int tmp_coordinate_frame;
if (pcl::console::parse (argc, argv, "-c", tmp_coordinate_frame) >= 0)
{
coordinate_frame = pcl::RangeImage::CoordinateFrame (tmp_coordinate_frame);
std::cout << "Using coordinate frame "<< (int)coordinate_frame<<".\n";
}
angular_resolution_x = pcl::deg2rad (angular_resolution_x);
angular_resolution_y = pcl::deg2rad (angular_resolution_y);
// ------------------------------------------------------------------
// -----Read pcd file or create example point cloud if not given-----
// ------------------------------------------------------------------
pcl::PointCloud<PointType>::Ptr point_cloud_ptr (new pcl::PointCloud<PointType>);
pcl::PointCloud<PointType>& point_cloud = *point_cloud_ptr;
Eigen::Affine3f scene_sensor_pose (Eigen::Affine3f::Identity ());
std::vector<int> pcd_filename_indices = pcl::console::parse_file_extension_argument (argc, argv, "pcd");
if (!pcd_filename_indices.empty ())
{
std::string filename = argv[pcd_filename_indices[0]];
if (pcl::io::loadPCDFile (filename, point_cloud) == -1)
{
std::cout << "Was not able to open file \""<<filename<<"\".\n";
printUsage (argv[0]);
return 0;
}
scene_sensor_pose = Eigen::Affine3f (Eigen::Translation3f (point_cloud.sensor_origin_[0],
point_cloud.sensor_origin_[1],
point_cloud.sensor_origin_[2])) *
Eigen::Affine3f (point_cloud.sensor_orientation_);
}
else
{
std::cout << "\nNo *.pcd file given => Generating example point cloud.\n\n";
for (float x=-0.5f; x<=0.5f; x+=0.01f)
{
for (float y=-0.5f; y<=0.5f; y+=0.01f)
{
PointType point; point.x = x; point.y = y; point.z = 2.0f - y;
point_cloud.points.push_back (point);
}
}
point_cloud.width = point_cloud.size (); point_cloud.height = 1;
}
// -----------------------------------------------
// -----Create RangeImage from the PointCloud-----
// -----------------------------------------------
float noise_level = 0.0;
float min_range = 0.0f;
int border_size = 1;
pcl::RangeImage::Ptr range_image_ptr(new pcl::RangeImage);
pcl::RangeImage& range_image = *range_image_ptr;
range_image.createFromPointCloud (point_cloud, angular_resolution_x, angular_resolution_y,
pcl::deg2rad (360.0f), pcl::deg2rad (180.0f),
scene_sensor_pose, coordinate_frame, noise_level, min_range, border_size);
// --------------------------------------------
// -----Open 3D viewer and add point cloud-----
// --------------------------------------------
pcl::visualization::PCLVisualizer viewer ("3D Viewer");
viewer.setBackgroundColor (1, 1, 1);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointWithRange> range_image_color_handler (range_image_ptr, 0, 0, 0);
viewer.addPointCloud (range_image_ptr, range_image_color_handler, "range image");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "range image");
//viewer.addCoordinateSystem (1.0f, "global");
//PointCloudColorHandlerCustom<PointType> point_cloud_color_handler (point_cloud_ptr, 150, 150, 150);
//viewer.addPointCloud (point_cloud_ptr, point_cloud_color_handler, "original point cloud");
viewer.initCameraParameters ();
setViewerPose(viewer, range_image.getTransformationToWorldSystem ());
// --------------------------
// -----Show range image-----
// --------------------------
pcl::visualization::RangeImageVisualizer range_image_widget ("Range image");
range_image_widget.showRangeImage (range_image);
//--------------------
// -----Main loop-----
//--------------------
while (!viewer.wasStopped ())
{
range_image_widget.spinOnce ();
viewer.spinOnce ();
pcl_sleep (0.01);
if (live_update)
{
scene_sensor_pose = viewer.getViewerPose();
range_image.createFromPointCloud (point_cloud, angular_resolution_x, angular_resolution_y,
pcl::deg2rad (360.0f), pcl::deg2rad (180.0f),
scene_sensor_pose, pcl::RangeImage::LASER_FRAME, noise_level, min_range, border_size);
range_image_widget.showRangeImage (range_image);
}
}
}