diff --git a/CMakeLists.txt b/CMakeLists.txt index 1574ff8..54a8596 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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 diff --git a/cfg/config.cfg b/cfg/config.cfg index 6cc9a0f..372d14b 100755 --- a/cfg/config.cfg +++ b/cfg/config.cfg @@ -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")) diff --git a/pcl_methods.odt b/pcl_methods.odt index 951ea26..8b5f863 100644 Binary files a/pcl_methods.odt and b/pcl_methods.odt differ diff --git a/src/initial_processing.cpp b/src/initial_processing.cpp index 0d6bbdc..2379b4e 100755 --- a/src/initial_processing.cpp +++ b/src/initial_processing.cpp @@ -1,6 +1,7 @@ #include #include #include +#include #include "ros/ros.h" #include "std_msgs/String.h" @@ -27,7 +28,19 @@ #include #include #include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include /* 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 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::Ptr tree (new pcl::search::KdTree); @@ -251,6 +299,7 @@ void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg){ int count = 0; pcl::PointCloud cluster_vector[cluster_indices.size()]; + pcl::PointCloud::Ptr cluster_vector_ptr[cluster_indices.size()]; for (std::vector::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it){ pcl::PointCloud::Ptr cloud_cluster (new pcl::PointCloud); @@ -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 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::Ptr point_cloud_ptr (new pcl::PointCloud); + pcl::PointCloud& 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 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; igetPoint(i, j).range; + + float range = (!std::isinf(r))? + std::max(0.0f, std::min(1.0f, factor * (r + offset))): + 0.0; + + _rangeImage.at(j, i) = static_cast((range) * std::numeric_limits::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 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 cluster_lines; + + for(int i = 0; i::Ptr model(new pcl::SampleConsensusModelLine (cluster_vector_ptr[i])); + pcl::RandomSampleConsensus ransac (model); + pcl::PointCloud::Ptr final (new pcl::PointCloud); + std::vector 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("unprocessed_cloud", 1); pub2 = n.advertise("processed_cloud", 1); pub3 = n.advertise("clustered_cloud", 1); + pub4 = n.advertise("line_cloud", 1); marker_pub = n.advertise("visualization_marker", 10); marker_pub2 = n.advertise("visualization_marker2", 10); steer = n.advertise("centroid_marker", 1); + image_pub = n.advertise("pcl_image", 30); + line_pub = n.advertise("center_line", 10); + + pub4_1 = n.advertise("line_cloud_1", 1); + pub4_2 = n.advertise("line_cloud_2", 1); + pub4_3 = n.advertise("line_cloud_3", 1); + pub4_4 = n.advertise("line_cloud_4", 1); // create publisher for visualising marker visual = n.advertise ("marker", 1); diff --git a/src/range_image.cpp b/src/range_image.cpp new file mode 100644 index 0000000..06d17af --- /dev/null +++ b/src/range_image.cpp @@ -0,0 +1,165 @@ +#include + +#include +#include +#include +#include +#include +#include + +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: "<\n\n" + << "Options:\n" + << "-------------------------------------------\n" + << "-rx angular resolution in degrees (default "< angular resolution in degrees (default "< 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 "<= 0) + std::cout << "Setting angular resolution in y-direction to "<= 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::Ptr point_cloud_ptr (new pcl::PointCloud); + pcl::PointCloud& point_cloud = *point_cloud_ptr; + Eigen::Affine3f scene_sensor_pose (Eigen::Affine3f::Identity ()); + std::vector 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 \""< 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 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 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); + } + } +} \ No newline at end of file