From 5a20ea1d80f880ff6c1ca85faab8277a4d02a633 Mon Sep 17 00:00:00 2001 From: Pepijn Hundepool Date: Thu, 28 Apr 2022 10:25:30 +0200 Subject: [PATCH 1/8] initial_commit --- ed_sensor_integration/src/kinect/code_v1.cpp | 159 +++++++++++++++++++ 1 file changed, 159 insertions(+) create mode 100644 ed_sensor_integration/src/kinect/code_v1.cpp diff --git a/ed_sensor_integration/src/kinect/code_v1.cpp b/ed_sensor_integration/src/kinect/code_v1.cpp new file mode 100644 index 00000000..e29cae99 --- /dev/null +++ b/ed_sensor_integration/src/kinect/code_v1.cpp @@ -0,0 +1,159 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +Eigen::Matrix4f ReadJson(std::string 2022-04-05-13-28-28, float *xout, float *yout, float *zout) { + + std::string json_filename = boost::filesystem::change_extension(2022-04-05-13-28-28, ".json").string(); + // read json metadata + tue::config::DataPointer meta_data; + + try + { + meta_data = tue::config::fromFile(json_filename); + } + catch (tue::config::ParseException& e) + { + std::cerr << "Could not open '" << json_filename << "'.\n\n" << e.what() << std::endl; + //return 0; + } + + tue::config::Reader r(meta_data); + // Read sensor pose + geo::Pose3D sensor_pose; + if (!ed::deserialize(r, "sensor_pose", sensor_pose)) + { + std::cerr << "No field 'sensor_pose' specified." << std::endl; + //return 0; + } + // convert from geolib coordinates to ros coordinates #TODO remove geolib coordinates for camera pose + sensor_pose.R = sensor_pose.R * geo::Mat3(1, 0, 0, 0, -1, 0, 0, 0, -1); + + float x = sensor_pose.t.x; + float y = sensor_pose.t.y; + float z = sensor_pose.t.z; + float xx = sensor_pose.R.xx; + float xy = sensor_pose.R.xy; + float xz = sensor_pose.R.xz; + float yx = sensor_pose.R.yx; + float yy = sensor_pose.R.yy; + float yz = sensor_pose.R.yz; + float zx = sensor_pose.R.zx; + float zy = sensor_pose.R.zy; + float zz = sensor_pose.R.zz; + + *xout = x; + *yout = y; + *zout = z; + + //float qx, qy, qz, qw; + + //const float n = 2.0f/(qx*qx+qy*qy+qz*qz+qw*qw); + Eigen::Matrix4f Transform = Eigen::Matrix4f::Identity();/* { + {1.0f - n*qy*qy - n*qz*qz, n*qx*qy - n*qz*qw, n*qx*qz + n*qy*qw, x}, + {n*qx*qy + n*qz*qw, 1.0f - n*qx*qx - n*qz*qz, n*qy*qz - n*qx*qw, y}, + {n*qx*qz - n*qy*qw, n*qy*qz + n*qx*qw, 1.0f - n*qx*qx - n*qy*qy, z}, + {0.0f, 0.0f, 0.0f, 1.0f}}; */ + + Transform(0,0) = xx; //1.0f - n*qy*qy - n*qz*qz; + Transform(0,1) = xy; //n*qx*qy - n*qz*qw; + Transform(0,2) = xz; //n*qx*qz + n*qy*qw; + Transform(0,3) = x; + Transform(1,0) = yx; //n*qx*qy + n*qz*qw; + Transform(1,1) = yy; //1.0f - n*qx*qx - n*qz*qz; + Transform(1,2) = yz; //n*qy*qz - n*qx*qw; + Transform(1,3) = y; + Transform(2,0) = zx; //n*qx*qz - n*qy*qw; + Transform(2,1) = zy; //n*qy*qz + n*qx*qw; + Transform(2,2) = zz; //1.0f - n*qx*qx - n*qy*qy; + Transform(2,3) = z; + + std::cout << Transform << std::endl; + return Transform; +} +} + + +int +main () +{ + pcl::PCLPointCloud2::Ptr cloud_blob (new pcl::PCLPointCloud2), cloud_filtered_blob (new pcl::PCLPointCloud2); + pcl::PointCloud::Ptr cloud_filtered (new pcl::PointCloud), cloud_p (new pcl::PointCloud), cloud_f (new pcl::PointCloud); + + // Fill in the cloud data + pcl::PCDReader reader; + reader.read ("2022-04-05-13-28-28.pcd", *cloud_blob); + + std::cerr << "PointCloud before filtering: " << cloud_blob->width * cloud_blob->height << " data points." << std::endl; + + // Create the filtering object: downsample the dataset using a leaf size of 1cm + pcl::VoxelGrid sor; + sor.setInputCloud (cloud_blob); + sor.setLeafSize (0.0025f, 0.0025f, 0.0025f); + sor.filter (*cloud_filtered_blob); + + // Convert to the templated PointCloud + pcl::fromPCLPointCloud2 (*cloud_filtered_blob, *cloud_filtered); + + std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height << " data points." << std::endl; + + // Write the downsampled version to disk + pcl::PCDWriter writer; + writer.write ("table_scene_lms400_downsampled.pcd", *cloud_filtered, false); + + 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 + seg.setModelType (pcl::SACMODEL_PLANE); + seg.setMethodType (pcl::SAC_RANSAC); + seg.setMaxIterations (1000); + seg.setDistanceThreshold (0.01); + + // Create the filtering object + pcl::ExtractIndices extract; + + int i = 0, nr_points = (int) cloud_filtered->size (); + // While 30% of the original cloud is still there + while (cloud_filtered->size () > 0.3 * nr_points) + { + // Segment the largest planar component from the remaining cloud + seg.setInputCloud (cloud_filtered); + seg.segment (*inliers, *coefficients); + if (inliers->indices.size () == 0) + { + std::cerr << "Could not estimate a planar model for the given dataset." << std::endl; + break; + } + + // Extract the inliers + extract.setInputCloud (cloud_filtered); + extract.setIndices (inliers); + extract.setNegative (false); + extract.filter (*cloud_p); + std::cerr << "PointCloud representing the planar component: " << cloud_p->width * cloud_p->height << " data points." << std::endl; + + std::stringstream ss; + ss << "table_scene_lms400_plane_" << i << ".pcd"; + writer.write (ss.str (), *cloud_p, false); + + // Create the filtering object + extract.setNegative (true); + extract.filter (*cloud_f); + cloud_filtered.swap (cloud_f); + i++; + } + + return (0); + + From 5b80acc397ab1af5383f23e3deffd0cb93e16920 Mon Sep 17 00:00:00 2001 From: Pepijn Hundepool Date: Thu, 28 Apr 2022 10:45:53 +0200 Subject: [PATCH 2/8] hoi --- ed_sensor_integration/CMakeLists.txt | 4 ++++ ed_sensor_integration/src/kinect/code_v1.cpp | 2 +- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/ed_sensor_integration/CMakeLists.txt b/ed_sensor_integration/CMakeLists.txt index 8483906c..87382260 100644 --- a/ed_sensor_integration/CMakeLists.txt +++ b/ed_sensor_integration/CMakeLists.txt @@ -6,6 +6,7 @@ find_package(catkin REQUIRED COMPONENTS ed_sensor_integration_msgs geolib2 image_geometry + pcl_ros sensor_msgs tf2 tf2_ros @@ -116,6 +117,9 @@ target_link_libraries(ed_segmenter ed_kinect) add_executable(ed_fitter tools/fitter_viz.cpp) target_link_libraries(ed_fitter ed_kinect) +add_executable(ed_table_modeler src/kinect/code_v1.cpp) +target_link_libraries(ed_table_modeler ${catkin_LIBRARIES}) + # ------------------------------------------------------------------------------------------------ # TESTS # ------------------------------------------------------------------------------------------------ diff --git a/ed_sensor_integration/src/kinect/code_v1.cpp b/ed_sensor_integration/src/kinect/code_v1.cpp index e29cae99..d329e3f9 100644 --- a/ed_sensor_integration/src/kinect/code_v1.cpp +++ b/ed_sensor_integration/src/kinect/code_v1.cpp @@ -7,7 +7,7 @@ #include #include #include -#include +#include Eigen::Matrix4f ReadJson(std::string 2022-04-05-13-28-28, float *xout, float *yout, float *zout) { From 012dba2d5ef8192f3e1e58932ad97d1681173620 Mon Sep 17 00:00:00 2001 From: Pepijn Hundepool Date: Tue, 3 May 2022 22:41:07 +0200 Subject: [PATCH 3/8] horiontal plane attempt --- ed_sensor_integration/CMakeLists.txt | 9 +++++ ed_sensor_integration/src/kinect/code_v1.cpp | 41 ++++++++++++-------- 2 files changed, 34 insertions(+), 16 deletions(-) diff --git a/ed_sensor_integration/CMakeLists.txt b/ed_sensor_integration/CMakeLists.txt index 87382260..d6e8bf22 100644 --- a/ed_sensor_integration/CMakeLists.txt +++ b/ed_sensor_integration/CMakeLists.txt @@ -2,6 +2,15 @@ cmake_minimum_required(VERSION 3.0.2) project(ed_sensor_integration) find_package(catkin REQUIRED COMPONENTS + # cv_bridge + # dynamic_reconfigure + # geometry_msgs + # actionlib_msgs + # image_transport + + # rospy + # roscpp + # std_msgs ed ed_sensor_integration_msgs geolib2 diff --git a/ed_sensor_integration/src/kinect/code_v1.cpp b/ed_sensor_integration/src/kinect/code_v1.cpp index d329e3f9..42231bda 100644 --- a/ed_sensor_integration/src/kinect/code_v1.cpp +++ b/ed_sensor_integration/src/kinect/code_v1.cpp @@ -1,17 +1,25 @@ #include #include #include + #include +#include +#include + #include #include +#include + #include + #include #include + #include -Eigen::Matrix4f ReadJson(std::string 2022-04-05-13-28-28, float *xout, float *yout, float *zout) { +Eigen::Matrix4f ReadJson(std::string pcd_filename, float *xout, float *yout, float *zout) { - std::string json_filename = boost::filesystem::change_extension(2022-04-05-13-28-28, ".json").string(); + std::string json_filename = boost::filesystem::change_extension(pcd_filename, ".json").string(); // read json metadata tue::config::DataPointer meta_data; @@ -62,24 +70,22 @@ Eigen::Matrix4f ReadJson(std::string 2022-04-05-13-28-28, float *xout, float *yo {n*qx*qz - n*qy*qw, n*qy*qz + n*qx*qw, 1.0f - n*qx*qx - n*qy*qy, z}, {0.0f, 0.0f, 0.0f, 1.0f}}; */ - Transform(0,0) = xx; //1.0f - n*qy*qy - n*qz*qz; - Transform(0,1) = xy; //n*qx*qy - n*qz*qw; - Transform(0,2) = xz; //n*qx*qz + n*qy*qw; + Transform(0,0) = xx;//1.0f - n*qy*qy - n*qz*qz; + Transform(0,1) = xy;//n*qx*qy - n*qz*qw; + Transform(0,2) = xz;//n*qx*qz + n*qy*qw; Transform(0,3) = x; - Transform(1,0) = yx; //n*qx*qy + n*qz*qw; - Transform(1,1) = yy; //1.0f - n*qx*qx - n*qz*qz; - Transform(1,2) = yz; //n*qy*qz - n*qx*qw; + Transform(1,0) = yx;//n*qx*qy + n*qz*qw; + Transform(1,1) = yy;//1.0f - n*qx*qx - n*qz*qz; + Transform(1,2) = yz;//n*qy*qz - n*qx*qw; Transform(1,3) = y; - Transform(2,0) = zx; //n*qx*qz - n*qy*qw; - Transform(2,1) = zy; //n*qy*qz + n*qx*qw; - Transform(2,2) = zz; //1.0f - n*qx*qx - n*qy*qy; + Transform(2,0) = zx;//n*qx*qz - n*qy*qw; + Transform(2,1) = zy;//n*qy*qz + n*qx*qw; + Transform(2,2) = zz;//1.0f - n*qx*qx - n*qy*qy; Transform(2,3) = z; std::cout << Transform << std::endl; return Transform; } -} - int main () @@ -112,13 +118,16 @@ main () pcl::PointIndices::Ptr inliers (new pcl::PointIndices ()); // Create the segmentation object pcl::SACSegmentation seg; + pcl::SampleConsensusModelParallelPlane model (cloud_filtered); // Optional seg.setOptimizeCoefficients (true); // Mandatory - seg.setModelType (pcl::SACMODEL_PLANE); + seg.setModelType (pcl::SACMODEL_PARALLEL_PLANE); seg.setMethodType (pcl::SAC_RANSAC); seg.setMaxIterations (1000); seg.setDistanceThreshold (0.01); + model.setAxis(Eigen::Vector3f(1,0,0)); + model.setEpsAngle(30*0.0174532925); //*0.0174532925 to radians // Create the filtering object pcl::ExtractIndices extract; @@ -130,6 +139,7 @@ main () // Segment the largest planar component from the remaining cloud seg.setInputCloud (cloud_filtered); seg.segment (*inliers, *coefficients); + if (inliers->indices.size () == 0) { std::cerr << "Could not estimate a planar model for the given dataset." << std::endl; @@ -155,5 +165,4 @@ main () } return (0); - - +} From 0dc643ba1c53817b8ed92f537e3a752bfec0d532 Mon Sep 17 00:00:00 2001 From: Pepijn Hundepool Date: Tue, 17 May 2022 09:52:24 +0200 Subject: [PATCH 4/8] Nieuwste versie --- ed_sensor_integration/src/kinect/code_v1.cpp | 68 +++++++++++++++++++- 1 file changed, 66 insertions(+), 2 deletions(-) diff --git a/ed_sensor_integration/src/kinect/code_v1.cpp b/ed_sensor_integration/src/kinect/code_v1.cpp index 42231bda..a5398a7b 100644 --- a/ed_sensor_integration/src/kinect/code_v1.cpp +++ b/ed_sensor_integration/src/kinect/code_v1.cpp @@ -14,8 +14,20 @@ #include #include +#include +#include + +#include #include +#include + +#include +#include +#include + +#include +#include Eigen::Matrix4f ReadJson(std::string pcd_filename, float *xout, float *yout, float *zout) { @@ -87,6 +99,56 @@ Eigen::Matrix4f ReadJson(std::string pcd_filename, float *xout, float *yout, flo return Transform; } +float FilterPlane (pcl::PointCloud::Ptr cloud, pcl::PointCloud::Ptr out) { + + //*out = *cloud; return(-1); //activate to bypass plane fitting and height estimation + + std::vector indices; + float threshold = 0.03; + + std::cout << "starting ransac" << std::endl; + // Create SAC model + pcl::SampleConsensusModelParallelPlane::Ptr plane (new pcl::SampleConsensusModelParallelPlane(cloud)); + plane->setAxis (Eigen::Vector3f(1,0,0)); + plane->setEpsAngle(15*0.0174532925); //*0.0174532925 to radians + std::cout << "created plane object" << std::endl; + // Create SAC method + pcl::SACSegmentation segplane; + //pcl::SampleConsensus::Ptr sac (new pcl::SampleConsensus (plane, threshold)); + std::cout << "created ransac object" << std::endl; + segplane.setMaxIterations(10000); + segplane.setProbability(0.99); + + // Fit model + //segplane.computeModel(); + + // Get inliers + std::vector inliers; + //segplane.getInliers(inliers); + + // Get the model coefficients + Eigen::VectorXf coeff; + //segplane.getModelCoefficients (coeff); + std::cout << "ransac complete" << std::endl; + + pcl::ConditionAnd::Ptr range_cond (new pcl::ConditionAnd ()); + range_cond->addComparison (pcl::FieldComparison::ConstPtr (new pcl::FieldComparison ("z", pcl::ComparisonOps::GT, (coeff[3]-0.01)))); + //range_cond->addComparison (pcl::FieldComparison::ConstPtr (new pcl::FieldComparison ("z", pcl::ComparisonOps::LT, (coeff[3]+0.01)))); + *out = *cloud; + //filter out everything below plane + pcl::ConditionalRemoval condrem; + condrem.setCondition (range_cond); + condrem.setInputCloud (out); + condrem.setKeepOrganized(true); + + condrem.filter (*out); + (*out).is_dense = false; + pcl::removeNaNFromPointCloud(*out, *out, indices); + + return(coeff[3]); + +} + int main () { @@ -126,8 +188,8 @@ main () seg.setMethodType (pcl::SAC_RANSAC); seg.setMaxIterations (1000); seg.setDistanceThreshold (0.01); - model.setAxis(Eigen::Vector3f(1,0,0)); - model.setEpsAngle(30*0.0174532925); //*0.0174532925 to radians + model.setAxis (Eigen::Vector3f(1,0,0)); + model.setEpsAngle(15*0.0174532925); //*0.0174532925 to radians // Create the filtering object pcl::ExtractIndices extract; @@ -165,4 +227,6 @@ main () } return (0); + + } From 618831cd9a25562582e4174524bf2df9443c36d2 Mon Sep 17 00:00:00 2001 From: Pepijn Hundepool Date: Mon, 23 May 2022 18:03:45 +0200 Subject: [PATCH 5/8] Filter update --- ed_sensor_integration/src/kinect/code_v1.cpp | 132 ++++++++++++++++--- 1 file changed, 113 insertions(+), 19 deletions(-) diff --git a/ed_sensor_integration/src/kinect/code_v1.cpp b/ed_sensor_integration/src/kinect/code_v1.cpp index a5398a7b..ac243602 100644 --- a/ed_sensor_integration/src/kinect/code_v1.cpp +++ b/ed_sensor_integration/src/kinect/code_v1.cpp @@ -9,18 +9,24 @@ #include #include #include +#include #include +#include + +#include #include #include #include #include +#include #include #include #include +//#include "ed_sensor_integration/sac_model_circle.h" #include #include @@ -31,7 +37,8 @@ Eigen::Matrix4f ReadJson(std::string pcd_filename, float *xout, float *yout, float *zout) { - std::string json_filename = boost::filesystem::change_extension(pcd_filename, ".json").string(); + std::string json_filename = "2022-04-22-12-04-46.json"; + //std::string json_filename = boost::filesystem::change_extension(pcd_filename, ".json").string(); // read json metadata tue::config::DataPointer meta_data; @@ -99,7 +106,7 @@ Eigen::Matrix4f ReadJson(std::string pcd_filename, float *xout, float *yout, flo return Transform; } -float FilterPlane (pcl::PointCloud::Ptr cloud, pcl::PointCloud::Ptr out) { +float FilterPlane (pcl::PointCloud::Ptr cloud, pcl::PointCloud::Ptr out) { //*out = *cloud; return(-1); //activate to bypass plane fitting and height estimation @@ -108,35 +115,35 @@ float FilterPlane (pcl::PointCloud::Ptr cloud, pcl::PointCloud std::cout << "starting ransac" << std::endl; // Create SAC model - pcl::SampleConsensusModelParallelPlane::Ptr plane (new pcl::SampleConsensusModelParallelPlane(cloud)); + pcl::SampleConsensusModelParallelPlane::Ptr plane (new pcl::SampleConsensusModelParallelPlane(cloud)); plane->setAxis (Eigen::Vector3f(1,0,0)); plane->setEpsAngle(15*0.0174532925); //*0.0174532925 to radians std::cout << "created plane object" << std::endl; // Create SAC method - pcl::SACSegmentation segplane; - //pcl::SampleConsensus::Ptr sac (new pcl::SampleConsensus (plane, threshold)); + //pcl::SACSegmentation segplane; + pcl::RandomSampleConsensus::Ptr segplane (new pcl::RandomSampleConsensus (plane, threshold)); std::cout << "created ransac object" << std::endl; - segplane.setMaxIterations(10000); - segplane.setProbability(0.99); + segplane->setMaxIterations(10000); + segplane->setProbability(0.99); // Fit model - //segplane.computeModel(); + segplane->computeModel(); // Get inliers std::vector inliers; - //segplane.getInliers(inliers); + segplane->getInliers(inliers); // Get the model coefficients Eigen::VectorXf coeff; - //segplane.getModelCoefficients (coeff); + segplane->getModelCoefficients (coeff); std::cout << "ransac complete" << std::endl; - pcl::ConditionAnd::Ptr range_cond (new pcl::ConditionAnd ()); - range_cond->addComparison (pcl::FieldComparison::ConstPtr (new pcl::FieldComparison ("z", pcl::ComparisonOps::GT, (coeff[3]-0.01)))); + pcl::ConditionAnd::Ptr range_cond (new pcl::ConditionAnd ()); + range_cond->addComparison (pcl::FieldComparison::ConstPtr (new pcl::FieldComparison ("z", pcl::ComparisonOps::GT, (coeff[3]-0.01)))); //range_cond->addComparison (pcl::FieldComparison::ConstPtr (new pcl::FieldComparison ("z", pcl::ComparisonOps::LT, (coeff[3]+0.01)))); *out = *cloud; //filter out everything below plane - pcl::ConditionalRemoval condrem; + pcl::ConditionalRemoval condrem; condrem.setCondition (range_cond); condrem.setInputCloud (out); condrem.setKeepOrganized(true); @@ -150,18 +157,22 @@ float FilterPlane (pcl::PointCloud::Ptr cloud, pcl::PointCloud } int -main () +main (int argc, char **argv) { + std::vector ::Ptr, Eigen::aligned_allocator ::Ptr > > inputs; +// float x, y, z;//used to store camera position +// pcl::transformPointCloud (*inputs[0], *inputs[0], ReadJson(argv[1], &x, &y, &z)); + pcl::PCLPointCloud2::Ptr cloud_blob (new pcl::PCLPointCloud2), cloud_filtered_blob (new pcl::PCLPointCloud2); pcl::PointCloud::Ptr cloud_filtered (new pcl::PointCloud), cloud_p (new pcl::PointCloud), cloud_f (new pcl::PointCloud); // Fill in the cloud data pcl::PCDReader reader; - reader.read ("2022-04-05-13-28-28.pcd", *cloud_blob); + reader.read ("2022-04-22-12-04-46.pcd", *cloud_blob); // std::cerr << "PointCloud before filtering: " << cloud_blob->width * cloud_blob->height << " data points." << std::endl; - // Create the filtering object: downsample the dataset using a leaf size of 1cm + // Create the filtering object: downsample the dataset using a leaf size of 0.25cm pcl::VoxelGrid sor; sor.setInputCloud (cloud_blob); sor.setLeafSize (0.0025f, 0.0025f, 0.0025f); @@ -188,8 +199,8 @@ main () seg.setMethodType (pcl::SAC_RANSAC); seg.setMaxIterations (1000); seg.setDistanceThreshold (0.01); - model.setAxis (Eigen::Vector3f(1,0,0)); - model.setEpsAngle(15*0.0174532925); //*0.0174532925 to radians + seg.setAxis (Eigen::Vector3f(1,0,0)); + seg.setEpsAngle(15*0.0174532925); //*0.0174532925 to radians // Create the filtering object pcl::ExtractIndices extract; @@ -226,7 +237,90 @@ main () i++; } - return (0); + // Creating the KdTree object for the search method of the extraction + pcl::search::KdTree::Ptr tree (new pcl::search::KdTree); + tree->setInputCloud (cloud_p); + + std::vector cluster_indices; + pcl::EuclideanClusterExtraction ec; + ec.setClusterTolerance (0.02); // 2cm + ec.setMinClusterSize (100); + ec.setMaxClusterSize (100000); + ec.setSearchMethod (tree); + ec.setInputCloud (cloud_p); + ec.extract (cluster_indices); + + int j = 0; + for (std::vector::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it) + { + pcl::PointCloud::Ptr cloud_cluster (new pcl::PointCloud); + for (const auto& idx : it->indices) + cloud_cluster->push_back ((*cloud_filtered)[idx]); //* + cloud_cluster->width = cloud_cluster->size (); + cloud_cluster->height = 1; + cloud_cluster->is_dense = true; + + std::cout << "PointCloud representing the Cluster: " << cloud_cluster->size () << " data points." << std::endl; + std::stringstream ss; + ss << "cloud_cluster_" << j << ".pcd"; + writer.write (ss.str (), *cloud_cluster, false); //* + j++; + } + + //Removing outliers using a StatisticalOutlierRemoval filter + pcl::PointCloud::Ptr cloud2 (new pcl::PointCloud); + pcl::PointCloud::Ptr cloud_filtered2 (new pcl::PointCloud); + + // Fill in the cloud data + pcl::PCDReader reader2; + // Replace the path below with the path where you saved your file + reader.read ("table_scene_lms400_plane_1.pcd", *cloud2); + + std::cerr << "Cloud before filtering: " << std::endl; + std::cerr << *cloud2 << std::endl; + + // Create the filtering object + pcl::StatisticalOutlierRemoval sor2; + sor2.setInputCloud (cloud2); + sor2.setMeanK (50); + sor2.setStddevMulThresh (1.0); + sor2.filter (*cloud_filtered2); + + std::cerr << "Cloud after filtering: " << std::endl; + std::cerr << *cloud_filtered2 << std::endl; + pcl::PCDWriter writer2; + writer2.write ("table_scene_lms400_inliers.pcd", *cloud_filtered2, false); + + sor2.setNegative (true); + sor2.filter (*cloud_filtered2); + writer2.write ("table_scene_lms400_outliers.pcd", *cloud_filtered2, false); + + FilterPlane; + +// pcl::SampleConsensusModelCircle::Ptr circle (new pcl::SampleConsensusModelCircle(cloud_ptr)); + +// // Create SAC method +// pcl::RandomSampleConsensus::Ptr saccirc (new pcl::RandomSampleConsensus (circle, threshold)); +// saccirc->setMaxIterations(10000); +// saccirc->setProbability(1); + +// // Fit model +// saccirc->computeModel(); + +// // Get inliers +// std::vector inliers3; +// saccirc->getInliers(inliers3); + +// // Get the model coefficients +// Eigen::VectorXf coeff3; +// saccirc->getModelCoefficients (coeff3); + +// float min_inliers = 0.0; +// Eigen::VectorXf output; + +// std::cout << "Circle model, " << static_cast(inliers3.size())/static_cast(cloud_p.size()) << std::endl; + + return (0); } From 52ef4df3312e939ce987314098b22e92a692ea94 Mon Sep 17 00:00:00 2001 From: Pepijn Hundepool Date: Fri, 10 Jun 2022 13:04:48 +0200 Subject: [PATCH 6/8] Code before costmap --- ed_sensor_integration/CMakeLists.txt | 1 + ed_sensor_integration/src/kinect/code_v1.cpp | 472 +++++++++++++++---- 2 files changed, 389 insertions(+), 84 deletions(-) diff --git a/ed_sensor_integration/CMakeLists.txt b/ed_sensor_integration/CMakeLists.txt index d6e8bf22..511958d8 100644 --- a/ed_sensor_integration/CMakeLists.txt +++ b/ed_sensor_integration/CMakeLists.txt @@ -11,6 +11,7 @@ find_package(catkin REQUIRED COMPONENTS # rospy # roscpp # std_msgs + costmap_2d ed ed_sensor_integration_msgs geolib2 diff --git a/ed_sensor_integration/src/kinect/code_v1.cpp b/ed_sensor_integration/src/kinect/code_v1.cpp index ac243602..11da239f 100644 --- a/ed_sensor_integration/src/kinect/code_v1.cpp +++ b/ed_sensor_integration/src/kinect/code_v1.cpp @@ -32,12 +32,70 @@ #include #include +#include +#include +//#include +#include "opencv2/imgcodecs.hpp" +#include "opencv2/highgui.hpp" +#include "opencv2/imgproc.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include // for getFilenameWithoutExtension + #include #include +//typedef pcl::PointXYZ PointType; + +//// -------------------- +//// -----Parameters----- +//// -------------------- +//float angular_resolution = 0.5f; +//float support_size = 0.2f; +//pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME; +//bool setUnseenToMaxRange = false; +//bool rotation_invariant = true; + +//// -------------- +//// -----Help----- +//// -------------- +//void +//printUsage (const char* progName) +//{ +// std::cout << "\n\nUsage: "<\n\n" +// << "Options:\n" +// << "-------------------------------------------\n" +// << "-r angular resolution in degrees (default "< coordinate frame (default "<< (int)coordinate_frame<<")\n" +// << "-m Treat all unseen points to max range\n" +// << "-s support size for the interest points (diameter of the used sphere - " +// "default "< switch rotational invariant version of the feature on/off" +// << " (default "<< (int)rotation_invariant<<")\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]); +//} + Eigen::Matrix4f ReadJson(std::string pcd_filename, float *xout, float *yout, float *zout) { - std::string json_filename = "2022-04-22-12-04-46.json"; + std::string json_filename = "2022-04-22-11-44-48.json"; //std::string json_filename = boost::filesystem::change_extension(pcd_filename, ".json").string(); // read json metadata tue::config::DataPointer meta_data; @@ -106,55 +164,78 @@ Eigen::Matrix4f ReadJson(std::string pcd_filename, float *xout, float *yout, flo return Transform; } -float FilterPlane (pcl::PointCloud::Ptr cloud, pcl::PointCloud::Ptr out) { - - //*out = *cloud; return(-1); //activate to bypass plane fitting and height estimation - - std::vector indices; - float threshold = 0.03; - - std::cout << "starting ransac" << std::endl; - // Create SAC model - pcl::SampleConsensusModelParallelPlane::Ptr plane (new pcl::SampleConsensusModelParallelPlane(cloud)); - plane->setAxis (Eigen::Vector3f(1,0,0)); - plane->setEpsAngle(15*0.0174532925); //*0.0174532925 to radians - std::cout << "created plane object" << std::endl; - // Create SAC method - //pcl::SACSegmentation segplane; - pcl::RandomSampleConsensus::Ptr segplane (new pcl::RandomSampleConsensus (plane, threshold)); - std::cout << "created ransac object" << std::endl; - segplane->setMaxIterations(10000); - segplane->setProbability(0.99); - - // Fit model - segplane->computeModel(); - - // Get inliers - std::vector inliers; - segplane->getInliers(inliers); - - // Get the model coefficients - Eigen::VectorXf coeff; - segplane->getModelCoefficients (coeff); - std::cout << "ransac complete" << std::endl; - - pcl::ConditionAnd::Ptr range_cond (new pcl::ConditionAnd ()); - range_cond->addComparison (pcl::FieldComparison::ConstPtr (new pcl::FieldComparison ("z", pcl::ComparisonOps::GT, (coeff[3]-0.01)))); - //range_cond->addComparison (pcl::FieldComparison::ConstPtr (new pcl::FieldComparison ("z", pcl::ComparisonOps::LT, (coeff[3]+0.01)))); - *out = *cloud; - //filter out everything below plane - pcl::ConditionalRemoval condrem; - condrem.setCondition (range_cond); - condrem.setInputCloud (out); - condrem.setKeepOrganized(true); - - condrem.filter (*out); - (*out).is_dense = false; - pcl::removeNaNFromPointCloud(*out, *out, indices); - - return(coeff[3]); - -} +//float FilterPlane (pcl::PointCloud::Ptr cloud, pcl::PointCloud::Ptr out) { + +// //*out = *cloud; return(-1); //activate to bypass plane fitting and height estimation + +// std::vector indices; +// float threshold = 0.03; + +// std::cout << "starting ransac" << std::endl; +// // Create SAC model +// pcl::SampleConsensusModelParallelPlane::Ptr plane (new pcl::SampleConsensusModelParallelPlane(cloud)); +// plane->setAxis (Eigen::Vector3f(1,0,0)); +// plane->setEpsAngle(15*0.0174532925); //*0.0174532925 to radians +// std::cout << "created plane object" << std::endl; +// // Create SAC method +// //pcl::SACSegmentation segplane; +// pcl::RandomSampleConsensus::Ptr segplane (new pcl::RandomSampleConsensus (plane, threshold)); +// std::cout << "created ransac object" << std::endl; +// segplane->setMaxIterations(10000); +// segplane->setProbability(0.99); + +// // Fit model +// segplane->computeModel(); + +// // Get inliers +// std::vector inliers; +// segplane->getInliers(inliers); + +// // Get the model coefficients +// Eigen::VectorXf coeff; +// segplane->getModelCoefficients (coeff); +// std::cout << "ransac complete" << std::endl; + +// pcl::ConditionAnd::Ptr range_cond (new pcl::ConditionAnd ()); +// range_cond->addComparison (pcl::FieldComparison::ConstPtr (new pcl::FieldComparison ("z", pcl::ComparisonOps::GT, (coeff[3]-0.01)))); +// //range_cond->addComparison (pcl::FieldComparison::ConstPtr (new pcl::FieldComparison ("z", pcl::ComparisonOps::LT, (coeff[3]+0.01)))); +// *out = *cloud; +// //filter out everything below plane +// pcl::ConditionalRemoval condrem; +// condrem.setCondition (range_cond); +// condrem.setInputCloud (out); +// condrem.setKeepOrganized(true); + +// condrem.filter (*out); +// (*out).is_dense = false; +// pcl::removeNaNFromPointCloud(*out, *out, indices); + +// return(coeff[3]); + +//} + +//using namespace cv; +//using namespace std; +//int thresh = 100; +//RNG rng(12345); +//void thresh_callback(int, void* ); + +//Mat src, src_gray; +//Mat dst, detected_edges; + +//int lowThreshold = 0; +//const int max_lowThreshold = 100; +//const int ratio = 3; +//const int kernel_size = 3; +//const char* window_name = "Edge Map"; +//static void CannyThreshold(int, void*) +//{ +// blur( src_gray, detected_edges, Size(3,3) ); +// Canny( detected_edges, detected_edges, lowThreshold, lowThreshold*ratio, kernel_size ); +// dst = Scalar::all(0); +// src.copyTo( dst, detected_edges); +// imshow( window_name, dst ); +//} int main (int argc, char **argv) @@ -168,7 +249,7 @@ main (int argc, char **argv) // Fill in the cloud data pcl::PCDReader reader; - reader.read ("2022-04-22-12-04-46.pcd", *cloud_blob); // + reader.read ("2022-04-22-11-44-48.pcd", *cloud_blob); // std::cerr << "PointCloud before filtering: " << cloud_blob->width * cloud_blob->height << " data points." << std::endl; @@ -237,35 +318,35 @@ main (int argc, char **argv) i++; } - // Creating the KdTree object for the search method of the extraction - pcl::search::KdTree::Ptr tree (new pcl::search::KdTree); - tree->setInputCloud (cloud_p); - - std::vector cluster_indices; - pcl::EuclideanClusterExtraction ec; - ec.setClusterTolerance (0.02); // 2cm - ec.setMinClusterSize (100); - ec.setMaxClusterSize (100000); - ec.setSearchMethod (tree); - ec.setInputCloud (cloud_p); - ec.extract (cluster_indices); - - int j = 0; - for (std::vector::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it) - { - pcl::PointCloud::Ptr cloud_cluster (new pcl::PointCloud); - for (const auto& idx : it->indices) - cloud_cluster->push_back ((*cloud_filtered)[idx]); //* - cloud_cluster->width = cloud_cluster->size (); - cloud_cluster->height = 1; - cloud_cluster->is_dense = true; - - std::cout << "PointCloud representing the Cluster: " << cloud_cluster->size () << " data points." << std::endl; - std::stringstream ss; - ss << "cloud_cluster_" << j << ".pcd"; - writer.write (ss.str (), *cloud_cluster, false); //* - j++; - } +// // Creating the KdTree object for the search method of the extraction +// pcl::search::KdTree::Ptr tree (new pcl::search::KdTree); +// tree->setInputCloud (cloud_p); + +// std::vector cluster_indices; +// pcl::EuclideanClusterExtraction ec; +// ec.setClusterTolerance (0.02); // 2cm +// ec.setMinClusterSize (100); +// ec.setMaxClusterSize (100000); +// ec.setSearchMethod (tree); +// ec.setInputCloud (cloud_p); +// ec.extract (cluster_indices); + +// int j = 0; +// for (std::vector::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it) +// { +// pcl::PointCloud::Ptr cloud_cluster (new pcl::PointCloud); +// for (const auto& idx : it->indices) +// cloud_cluster->push_back ((*cloud_filtered)[idx]); //* +// cloud_cluster->width = cloud_cluster->size (); +// cloud_cluster->height = 1; +// cloud_cluster->is_dense = true; + +// std::cout << "PointCloud representing the Cluster: " << cloud_cluster->size () << " data points." << std::endl; +// std::stringstream ss; +// ss << "cloud_cluster_" << j << ".pcd"; +// writer.write (ss.str (), *cloud_cluster, false); //* +// j++; +// } //Removing outliers using a StatisticalOutlierRemoval filter pcl::PointCloud::Ptr cloud2 (new pcl::PointCloud); @@ -282,8 +363,8 @@ main (int argc, char **argv) // Create the filtering object pcl::StatisticalOutlierRemoval sor2; sor2.setInputCloud (cloud2); - sor2.setMeanK (50); - sor2.setStddevMulThresh (1.0); + sor2.setMeanK (1); + sor2.setStddevMulThresh (0.001); sor2.filter (*cloud_filtered2); std::cerr << "Cloud after filtering: " << std::endl; @@ -296,7 +377,72 @@ main (int argc, char **argv) sor2.filter (*cloud_filtered2); writer2.write ("table_scene_lms400_outliers.pcd", *cloud_filtered2, false); - FilterPlane; + //FilterPlane; + + //Line detectors + +// CommandLineParser parser( argc, argv, "{@input | cluster.png | input image}" ); +// Mat src = imread( samples::findFile( parser.get( "@input" ) ) ); +// if( src.empty() ) +// { +// cout << "Could not open or find the image!\n" << endl; +// cout << "usage: " << argv[0] << " " << endl; +// return -1; +// } +// cvtColor( src, src_gray, COLOR_BGR2GRAY ); +// blur( src_gray, src_gray, Size(3,3) ); +// const char* source_window = "Source"; +// namedWindow( source_window ); +// imshow( source_window, src ); +// const int max_thresh = 255; +// createTrackbar( "Canny thresh:", source_window, &thresh, max_thresh, thresh_callback ); +// thresh_callback( 0, 0 ); +// waitKey(); +// return 0; +//} + +//void thresh_callback(int, void* ) +//{ +// Mat canny_output; +// Canny( src_gray, canny_output, thresh, thresh*2 ); +// vector > contours; +// findContours( canny_output, contours, RETR_TREE, CHAIN_APPROX_SIMPLE ); +// vector > contours_poly( contours.size() ); +// vector boundRect( contours.size() ); +// vectorcenters( contours.size() ); +// vectorradius( contours.size() ); +// for( size_t i = 0; i < contours.size(); i++ ) +// { +// approxPolyDP( contours[i], contours_poly[i], 3, false ); +// boundRect[i] = boundingRect( contours_poly[i] ); +// minEnclosingCircle( contours_poly[i], centers[i], radius[i] ); +// } +// Mat drawing = Mat::zeros( canny_output.size(), CV_8UC3 ); +// for( size_t i = 0; i< contours.size(); i++ ) +// { +// Scalar color = Scalar( rng.uniform(0, 256), rng.uniform(0,256), rng.uniform(0,256) ); +// drawContours( drawing, contours_poly, (int)i, color ); +// //rectangle( drawing, boundRect[i].tl(), boundRect[i].br(), color, 2 ); +// //circle( drawing, centers[i], (int)radius[i], color, 2 ); +// } +// imshow( "Contours", drawing ); +//} + + //Line detector +// CommandLineParser parser( argc, argv, "{@input | cluster.png | input image}" ); +// src = imread( samples::findFile( parser.get( "@input" ) ), IMREAD_COLOR ); // Load an image +// if( src.empty() ) +// { +// std::cout << "Could not open or find the image!\n" << std::endl; +// std::cout << "Usage: " << argv[0] << " " << std::endl; +// return -1; +// } +// dst.create( src.size(), src.type() ); +// cvtColor( src, src_gray, COLOR_BGR2GRAY ); +// namedWindow( window_name, WINDOW_AUTOSIZE ); +// createTrackbar( "Min Threshold:", window_name, &lowThreshold, max_lowThreshold, CannyThreshold ); +// CannyThreshold(0, 0); +// waitKey(0); // pcl::SampleConsensusModelCircle::Ptr circle (new pcl::SampleConsensusModelCircle(cloud_ptr)); @@ -321,6 +467,164 @@ main (int argc, char **argv) // std::cout << "Circle model, " << static_cast(inliers3.size())/static_cast(cloud_p.size()) << std::endl; - return (0); +//Costmap + +// if (pcl::console::find_argument (argc, argv, "-h") >= 0) +// { +// printUsage (argv[0]); +// return 0; +// } +// if (pcl::console::find_argument (argc, argv, "-m") >= 0) +// { +// setUnseenToMaxRange = true; +// std::cout << "Setting unseen values in range image to maximum range readings.\n"; +// } +// if (pcl::console::parse (argc, argv, "-o", rotation_invariant) >= 0) +// std::cout << "Switching rotation invariant feature version "<< (rotation_invariant ? "on" : "off")<<".\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"; +// } +// if (pcl::console::parse (argc, argv, "-s", support_size) >= 0) +// std::cout << "Setting support size to "<= 0) +// std::cout << "Setting angular resolution to "<::Ptr point_cloud_ptr (new pcl::PointCloud); +// pcl::PointCloud& point_cloud = *point_cloud_ptr; +// pcl::PointCloud far_ranges; +// 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::cerr << "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.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, pcl::deg2rad (360.0f), pcl::deg2rad (180.0f), +// scene_sensor_pose, coordinate_frame, noise_level, min_range, border_size); +// range_image.integrateFarRanges (far_ranges); +// if (setUnseenToMaxRange) +// range_image.setUnseenToMaxRange (); + +// // -------------------------------------------- +// // -----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); + +// // -------------------------------- +// // -----Extract NARF keypoints----- +// // -------------------------------- +// pcl::RangeImageBorderExtractor range_image_border_extractor; +// pcl::NarfKeypoint narf_keypoint_detector; +// narf_keypoint_detector.setRangeImageBorderExtractor (&range_image_border_extractor); +// narf_keypoint_detector.setRangeImage (&range_image); +// narf_keypoint_detector.getParameters ().support_size = support_size; + +// pcl::PointCloud keypoint_indices; +// narf_keypoint_detector.compute (keypoint_indices); +// std::cout << "Found "<::Ptr keypoints_ptr (new pcl::PointCloud); +// pcl::PointCloud& keypoints = *keypoints_ptr; +// keypoints.resize (keypoint_indices.size ()); +// for (std::size_t i=0; i keypoints_color_handler (keypoints_ptr, 0, 255, 0); +// viewer.addPointCloud (keypoints_ptr, keypoints_color_handler, "keypoints"); +// viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "keypoints"); + +// // ------------------------------------------------------ +// // -----Extract NARF descriptors for interest points----- +// // ------------------------------------------------------ +// std::vector keypoint_indices2; +// keypoint_indices2.resize (keypoint_indices.size ()); +// for (unsigned int i=0; i narf_descriptors; +// narf_descriptor.compute (narf_descriptors); +// std::cout << "Extracted "< Date: Fri, 10 Jun 2022 14:52:14 +0200 Subject: [PATCH 7/8] added empty_spot_designator files --- ed_sensor_integration/CMakeLists.txt | 3 + .../src/kinect/empty_spot_designator.cpp | 86 +++++++++++++++++++ 2 files changed, 89 insertions(+) create mode 100644 ed_sensor_integration/src/kinect/empty_spot_designator.cpp diff --git a/ed_sensor_integration/CMakeLists.txt b/ed_sensor_integration/CMakeLists.txt index 511958d8..e3f423f3 100644 --- a/ed_sensor_integration/CMakeLists.txt +++ b/ed_sensor_integration/CMakeLists.txt @@ -130,6 +130,9 @@ target_link_libraries(ed_fitter ed_kinect) add_executable(ed_table_modeler src/kinect/code_v1.cpp) target_link_libraries(ed_table_modeler ${catkin_LIBRARIES}) +add_executable(ed_empty_spot_designator src/kinect/empty_spot_designator.cpp) +target_link_libraries(ed_empty_spot_designator ${catkin_LIBRARIES}) + # ------------------------------------------------------------------------------------------------ # TESTS # ------------------------------------------------------------------------------------------------ diff --git a/ed_sensor_integration/src/kinect/empty_spot_designator.cpp b/ed_sensor_integration/src/kinect/empty_spot_designator.cpp new file mode 100644 index 00000000..061ea346 --- /dev/null +++ b/ed_sensor_integration/src/kinect/empty_spot_designator.cpp @@ -0,0 +1,86 @@ +#include +#include + +#include "opencv2/imgproc.hpp" +#include + +//pcl library +#include + + +double resolution = 0.005; +cv::Point2d canvas_center; + +void readPCD(pcl::PointCloud::Ptr cloud, std::string filename) +{ + // Fill in the cloud data + pcl::PCDReader reader; + reader.read (filename, *cloud); // +} + +cv::Point2d worldToCanvas(double x, double y) +{ + return cv::Point2d(-y / resolution, -x / resolution) + canvas_center; +} + +void createCostmap(pcl::PointCloud::Ptr cloud, cv::Mat& canvas, cv::Scalar color) +{ + canvas_center = cv::Point2d(canvas.rows / 2, canvas.cols); + + for (int nIndex = 0; nIndex < cloud->points.size (); nIndex++) + { + double x = cloud->points[nIndex].z; + double y = -cloud->points[nIndex].x; + //double z = -cloud->points[nIndex].y; + + cv::Point2d p = worldToCanvas(x, y); + if (p.x >= 0 && p.y >= 0 && p.x < canvas.cols && p.y < canvas.rows) + canvas.at(p) = cv::Vec3b(color[0], color[1], color[2]); + } +} + + +void usage() +{ + std::cout << "USAGE: empty_spot_designator [pointcloud.pcd] [table_pointcloud.pcd]" << std::endl; +} + +int main (int argc, char **argv) +{ + if (argc != 3) + { + usage(); + return 0; + } + + std::cout << "finding empty spot" << std::endl; + + std::cout << "Loading pcd file" << std::endl; + + std::string filename = argv[2]; + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + readPCD(cloud, filename); + + // read occupied data + std::string occupied_filename = argv[1]; + pcl::PointCloud::Ptr occupied_cloud (new pcl::PointCloud); + readPCD(occupied_cloud, occupied_filename); + + std::cout << "PointCloud representing the planar component: " << cloud->width * cloud->height << " data points." << std::endl; + + + std::cout << "creating costmap" << std::endl; + cv::Mat canvas(500, 500, CV_8UC3, cv::Scalar(50, 50, 50)); + cv::Scalar table_color(0, 255, 0); + cv::Scalar occupied_color(0, 0, 255); + + createCostmap(occupied_cloud, canvas, occupied_color); + //createCostmap(cloud, canvas, table_color); + + std::cout << "showing costmap" << std::endl; + cv::imshow("Laser Vizualization", canvas); + cv::waitKey(); + + + return 0; +} From 6792251c0ed66b3107478f34f48dce756103dddb Mon Sep 17 00:00:00 2001 From: Peter van Dooren Date: Fri, 10 Jun 2022 15:04:46 +0200 Subject: [PATCH 8/8] add dilation --- .../src/kinect/empty_spot_designator.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/ed_sensor_integration/src/kinect/empty_spot_designator.cpp b/ed_sensor_integration/src/kinect/empty_spot_designator.cpp index 061ea346..4babf10b 100644 --- a/ed_sensor_integration/src/kinect/empty_spot_designator.cpp +++ b/ed_sensor_integration/src/kinect/empty_spot_designator.cpp @@ -39,6 +39,13 @@ void createCostmap(pcl::PointCloud::Ptr cloud, cv::Mat& canvas, c } } +void dilateCostmap(cv::Mat& canvas) +{ + cv::Mat element = cv::getStructuringElement( cv::MORPH_RECT, + cv::Size( 11, 11), + cv::Point(5, 5) ); + cv::dilate(canvas, canvas, element ); +} void usage() { @@ -74,8 +81,10 @@ int main (int argc, char **argv) cv::Scalar table_color(0, 255, 0); cv::Scalar occupied_color(0, 0, 255); - createCostmap(occupied_cloud, canvas, occupied_color); - //createCostmap(cloud, canvas, table_color); + //createCostmap(occupied_cloud, canvas, occupied_color); + createCostmap(cloud, canvas, table_color); + + dilateCostmap(canvas); std::cout << "showing costmap" << std::endl; cv::imshow("Laser Vizualization", canvas);