From 945d0a802749b721ceba798cf8b20d0b038a810c Mon Sep 17 00:00:00 2001 From: Marco Camurri Date: Tue, 8 Dec 2020 11:11:24 +0000 Subject: [PATCH 01/58] fix catkin, separate plane_seg_ros app and lib --- plane_seg/CMakeLists.txt | 7 +- plane_seg_ros/CMakeLists.txt | 47 +++++-- plane_seg_ros/include/plane_seg_ros/Pass.hpp | 46 +++++++ .../include/plane_seg_ros/geometry_utils.hpp | 12 ++ plane_seg_ros/src/geometry_utils.cpp | 41 ++++++ plane_seg_ros/src/plane_seg_ros.cpp | 125 +----------------- plane_seg_ros/src/plane_seg_ros_node.cpp | 45 +++++++ 7 files changed, 194 insertions(+), 129 deletions(-) create mode 100644 plane_seg_ros/include/plane_seg_ros/Pass.hpp create mode 100644 plane_seg_ros/include/plane_seg_ros/geometry_utils.hpp create mode 100644 plane_seg_ros/src/geometry_utils.cpp create mode 100644 plane_seg_ros/src/plane_seg_ros_node.cpp diff --git a/plane_seg/CMakeLists.txt b/plane_seg/CMakeLists.txt index b0488c2..9a66f36 100644 --- a/plane_seg/CMakeLists.txt +++ b/plane_seg/CMakeLists.txt @@ -63,5 +63,8 @@ install(TARGETS plane_seg LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) -# Mark header files for installation -install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) + +install(TARGETS block_fitter plane_seg_test plane_seg_test2 + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) diff --git a/plane_seg_ros/CMakeLists.txt b/plane_seg_ros/CMakeLists.txt index d152b27..a6f719c 100644 --- a/plane_seg_ros/CMakeLists.txt +++ b/plane_seg_ros/CMakeLists.txt @@ -18,19 +18,50 @@ find_package(catkin REQUIRED COMPONENTS find_package(OpenCV 3.0 QUIET) -catkin_package( - INCLUDE_DIRS - #include - CATKIN_DEPENDS eigen_conversions pcl_conversions tf_conversions pcl_ros plane_seg -) +catkin_package(INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} + CATKIN_DEPENDS eigen_conversions + pcl_conversions + tf_conversions + pcl_ros + plane_seg + grid_map_msgs) + + include_directories( -# include + include ${catkin_INCLUDE_DIRS} ) +add_library(${PROJECT_NAME} SHARED src/${PROJECT_NAME}.cpp + src/geometry_utils.cpp) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) -set(APP_NAME plane_seg_ros) + +set(APP_NAME ${PROJECT_NAME}_node) add_executable(${APP_NAME} src/${APP_NAME}.cpp) -target_link_libraries(${APP_NAME} boost_system ${catkin_LIBRARIES}) +target_link_libraries(${APP_NAME} ${PROJECT_NAME} ${catkin_LIBRARIES}) + +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +install(TARGETS ${APP_NAME} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) + +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch) + +install(DIRECTORY data/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/data) + +install(DIRECTORY config/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config) diff --git a/plane_seg_ros/include/plane_seg_ros/Pass.hpp b/plane_seg_ros/include/plane_seg_ros/Pass.hpp new file mode 100644 index 0000000..5633a81 --- /dev/null +++ b/plane_seg_ros/include/plane_seg_ros/Pass.hpp @@ -0,0 +1,46 @@ +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace planeseg { + +class Pass{ + public: + Pass(ros::NodeHandle node_); + + ~Pass(){ + } + + void elevationMapCallback(const grid_map_msgs::GridMap& msg); + void pointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr &msg); + void robotPoseCallBack(const geometry_msgs::PoseWithCovarianceStampedConstPtr &msg); + + void processCloud(planeseg::LabeledCloud::Ptr& inCloud, Eigen::Vector3f origin, Eigen::Vector3f lookDir); + void processFromFile(int test_example); + + void publishHullsAsCloud(std::vector::Ptr> cloud_ptrs, + int secs, int nsecs); + + void publishHullsAsMarkers(std::vector< pcl::PointCloud::Ptr > cloud_ptrs, + int secs, int nsecs); + void printResultAsJson(); + void publishResult(); + + private: + ros::NodeHandle node_; + std::vector colors_; + + ros::Subscriber point_cloud_sub_, grid_map_sub_, pose_sub_; + ros::Publisher received_cloud_pub_, hull_cloud_pub_, hull_markers_pub_, look_pose_pub_; + + Eigen::Isometry3d last_robot_pose_; + planeseg::BlockFitter::Result result_; +}; +} diff --git a/plane_seg_ros/include/plane_seg_ros/geometry_utils.hpp b/plane_seg_ros/include/plane_seg_ros/geometry_utils.hpp new file mode 100644 index 0000000..18fafd5 --- /dev/null +++ b/plane_seg_ros/include/plane_seg_ros/geometry_utils.hpp @@ -0,0 +1,12 @@ +#pragma once +#include +#include + +namespace planeseg{ + +std::string vecToStr(const Eigen::Vector3f& iVec); +std::string rotToStr(const Eigen::Matrix3f& iRot); +void quat_to_euler(const Eigen::Quaterniond& q, double& roll, double& pitch, double& yaw); +Eigen::Vector3f convertRobotPoseToSensorLookDir(Eigen::Isometry3d robot_pose); + +} diff --git a/plane_seg_ros/src/geometry_utils.cpp b/plane_seg_ros/src/geometry_utils.cpp new file mode 100644 index 0000000..09c4da7 --- /dev/null +++ b/plane_seg_ros/src/geometry_utils.cpp @@ -0,0 +1,41 @@ +#include "plane_seg_ros/geometry_utils.hpp" +namespace planeseg { +// convenience methods +std::string vecToStr(const Eigen::Vector3f& iVec) { + std::ostringstream oss; + oss << iVec[0] << ", " << iVec[1] << ", " << iVec[2]; + return oss.str(); +} + +std::string rotToStr(const Eigen::Matrix3f& iRot) { + std::ostringstream oss; + Eigen::Quaternionf q(iRot); + oss << q.w() << ", " << q.x() << ", " << q.y() << ", " << q.z(); + return oss.str(); +} + +void quat_to_euler(const Eigen::Quaterniond& q, double& roll, double& pitch, double& yaw) { + const double q0 = q.w(); + const double q1 = q.x(); + const double q2 = q.y(); + const double q3 = q.z(); + roll = atan2(2.0*(q0*q1+q2*q3), 1.0-2.0*(q1*q1+q2*q2)); + pitch = asin(2.0*(q0*q2-q3*q1)); + yaw = atan2(2.0*(q0*q3+q1*q2), 1.0-2.0*(q2*q2+q3*q3)); +} + +Eigen::Vector3f convertRobotPoseToSensorLookDir(Eigen::Isometry3d robot_pose){ + + Eigen::Quaterniond quat = Eigen::Quaterniond( robot_pose.rotation() ); + double r,p,y; + quat_to_euler(quat, r, p, y); + //std::cout << r*180/M_PI << ", " << p*180/M_PI << ", " << y*180/M_PI << " rpy in Degrees\n"; + + double yaw = y; + double pitch = -p; + double xDir = cos(yaw)*cos(pitch); + double yDir = sin(yaw)*cos(pitch); + double zDir = sin(pitch); + return Eigen::Vector3f(xDir, yDir, zDir); +} +} diff --git a/plane_seg_ros/src/plane_seg_ros.cpp b/plane_seg_ros/src/plane_seg_ros.cpp index 8890974..51b01e0 100644 --- a/plane_seg_ros/src/plane_seg_ros.cpp +++ b/plane_seg_ros/src/plane_seg_ros.cpp @@ -1,72 +1,23 @@ +#include "plane_seg_ros/Pass.hpp" +#include "plane_seg_ros/geometry_utils.hpp" + #include #include -#include -#include +#include #include #include #include #include -#include -#include #include -#include -#include #include #include +#include -#include "plane_seg/BlockFitter.hpp" - - -// convenience methods -auto vecToStr = [](const Eigen::Vector3f& iVec) { - std::ostringstream oss; - oss << iVec[0] << ", " << iVec[1] << ", " << iVec[2]; - return oss.str(); -}; -auto rotToStr = [](const Eigen::Matrix3f& iRot) { - std::ostringstream oss; - Eigen::Quaternionf q(iRot); - oss << q.w() << ", " << q.x() << ", " << q.y() << ", " << q.z(); - return oss.str(); -}; - - -class Pass{ - public: - Pass(ros::NodeHandle node_); - - ~Pass(){ - } - - void elevationMapCallback(const grid_map_msgs::GridMap& msg); - void pointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr &msg); - void robotPoseCallBack(const geometry_msgs::PoseWithCovarianceStampedConstPtr &msg); - - void processCloud(planeseg::LabeledCloud::Ptr& inCloud, Eigen::Vector3f origin, Eigen::Vector3f lookDir); - void processFromFile(int test_example); - - void publishHullsAsCloud(std::vector::Ptr> cloud_ptrs, - int secs, int nsecs); - - void publishHullsAsMarkers(std::vector< pcl::PointCloud::Ptr > cloud_ptrs, - int secs, int nsecs); - void printResultAsJson(); - void publishResult(); - - private: - ros::NodeHandle node_; - std::vector colors_; - - ros::Subscriber point_cloud_sub_, grid_map_sub_, pose_sub_; - ros::Publisher received_cloud_pub_, hull_cloud_pub_, hull_markers_pub_, look_pose_pub_; - - Eigen::Isometry3d last_robot_pose_; - planeseg::BlockFitter::Result result_; -}; +using namespace planeseg; Pass::Pass(ros::NodeHandle node_): node_(node_){ @@ -125,33 +76,6 @@ void Pass::robotPoseCallBack(const geometry_msgs::PoseWithCovarianceStampedConst tf::poseMsgToEigen(msg->pose.pose, last_robot_pose_); } - -void quat_to_euler(const Eigen::Quaterniond& q, double& roll, double& pitch, double& yaw) { - const double q0 = q.w(); - const double q1 = q.x(); - const double q2 = q.y(); - const double q3 = q.z(); - roll = atan2(2.0*(q0*q1+q2*q3), 1.0-2.0*(q1*q1+q2*q2)); - pitch = asin(2.0*(q0*q2-q3*q1)); - yaw = atan2(2.0*(q0*q3+q1*q2), 1.0-2.0*(q2*q2+q3*q3)); -} - -Eigen::Vector3f convertRobotPoseToSensorLookDir(Eigen::Isometry3d robot_pose){ - - Eigen::Quaterniond quat = Eigen::Quaterniond( robot_pose.rotation() ); - double r,p,y; - quat_to_euler(quat, r, p, y); - //std::cout << r*180/M_PI << ", " << p*180/M_PI << ", " << y*180/M_PI << " rpy in Degrees\n"; - - double yaw = y; - double pitch = -p; - double xDir = cos(yaw)*cos(pitch); - double yDir = sin(yaw)*cos(pitch); - double zDir = sin(pitch); - return Eigen::Vector3f(xDir, yDir, zDir); -} - - void Pass::elevationMapCallback(const grid_map_msgs::GridMap& msg){ //std::cout << "got grid map / ev map\n"; @@ -463,41 +387,4 @@ void Pass::publishHullsAsMarkers(std::vector< pcl::PointCloud::Pt } -int main( int argc, char** argv ){ - // Turn off warning message about labels - // TODO: look into how labels are used - pcl::console::setVerbosityLevel(pcl::console::L_ALWAYS); - - ros::init(argc, argv, "plane_seg"); - ros::NodeHandle nh("~"); - std::unique_ptr app = std::make_unique(nh); - - ROS_INFO_STREAM("plane_seg ros ready"); - ROS_INFO_STREAM("============================="); - - bool run_test_program = false; - nh.param("/plane_seg/run_test_program", run_test_program, false); - std::cout << "run_test_program: " << run_test_program << "\n"; - - - // Enable this to run the test programs - if (run_test_program){ - std::cout << "Running test examples\n"; - app->processFromFile(0); - app->processFromFile(1); - app->processFromFile(2); - app->processFromFile(3); - // RACE examples don't work well - //app->processFromFile(4); - //app->processFromFile(5); - - std::cout << "Finished!\n"; - exit(-1); - } - - ROS_INFO_STREAM("Waiting for ROS messages"); - ros::spin(); - - return 1; -} diff --git a/plane_seg_ros/src/plane_seg_ros_node.cpp b/plane_seg_ros/src/plane_seg_ros_node.cpp new file mode 100644 index 0000000..3f41568 --- /dev/null +++ b/plane_seg_ros/src/plane_seg_ros_node.cpp @@ -0,0 +1,45 @@ +#include +#include "plane_seg_ros/Pass.hpp" +#include +#include + +using namespace planeseg; + +int main( int argc, char** argv ){ + // Turn off warning message about labels + // TODO: look into how labels are used + pcl::console::setVerbosityLevel(pcl::console::L_ALWAYS); + + + ros::init(argc, argv, "plane_seg"); + ros::NodeHandle nh("~"); + std::unique_ptr app = std::make_unique(nh); + + ROS_INFO_STREAM("plane_seg ros ready"); + ROS_INFO_STREAM("============================="); + + bool run_test_program = false; + nh.param("/plane_seg/run_test_program", run_test_program, false); + std::cout << "run_test_program: " << run_test_program << "\n"; + + + // Enable this to run the test programs + if (run_test_program){ + std::cout << "Running test examples\n"; + app->processFromFile(0); + app->processFromFile(1); + app->processFromFile(2); + app->processFromFile(3); + // RACE examples don't work well + //app->processFromFile(4); + //app->processFromFile(5); + + std::cout << "Finished!\n"; + exit(-1); + } + + ROS_INFO_STREAM("Waiting for ROS messages"); + ros::spin(); + + return 0; +} From a21b69752c5d3d48df9ee032833e92d835ac88f7 Mon Sep 17 00:00:00 2001 From: Christos Ioannou Date: Mon, 14 Dec 2020 18:15:33 +0000 Subject: [PATCH 02/58] putting code from rosbag_pass into plane_seg_ros --- plane_seg_ros/include/plane_seg_ros/Pass.hpp | 6 +- plane_seg_ros/src/plane_seg_ros.cpp | 65 ++++++++++++++++++-- plane_seg_ros/src/plane_seg_ros_node.cpp | 12 ++++ 3 files changed, 75 insertions(+), 8 deletions(-) diff --git a/plane_seg_ros/include/plane_seg_ros/Pass.hpp b/plane_seg_ros/include/plane_seg_ros/Pass.hpp index 5633a81..41b518e 100644 --- a/plane_seg_ros/include/plane_seg_ros/Pass.hpp +++ b/plane_seg_ros/include/plane_seg_ros/Pass.hpp @@ -8,6 +8,8 @@ #include #include #include +#include +#include namespace planeseg { @@ -24,7 +26,7 @@ class Pass{ void processCloud(planeseg::LabeledCloud::Ptr& inCloud, Eigen::Vector3f origin, Eigen::Vector3f lookDir); void processFromFile(int test_example); - + void stepThroughFile(); void publishHullsAsCloud(std::vector::Ptr> cloud_ptrs, int secs, int nsecs); @@ -38,7 +40,7 @@ class Pass{ std::vector colors_; ros::Subscriber point_cloud_sub_, grid_map_sub_, pose_sub_; - ros::Publisher received_cloud_pub_, hull_cloud_pub_, hull_markers_pub_, look_pose_pub_; + ros::Publisher received_cloud_pub_, hull_cloud_pub_, hull_markers_pub_, look_pose_pub_, elev_map_pub_, pose_pub_; Eigen::Isometry3d last_robot_pose_; planeseg::BlockFitter::Result result_; diff --git a/plane_seg_ros/src/plane_seg_ros.cpp b/plane_seg_ros/src/plane_seg_ros.cpp index 51b01e0..9a0873a 100644 --- a/plane_seg_ros/src/plane_seg_ros.cpp +++ b/plane_seg_ros/src/plane_seg_ros.cpp @@ -3,8 +3,10 @@ #include #include - +#include #include +#include +#include #include #include @@ -16,6 +18,13 @@ #include #include #include +#include +#include + +#include +#include +#define foreach BOOST_FOREACH + using namespace planeseg; @@ -24,18 +33,22 @@ Pass::Pass(ros::NodeHandle node_): std::string input_body_pose_topic; node_.getParam("input_body_pose_topic", input_body_pose_topic); - +// std::string filename; +// node_.getParm("/rosbag_pass/filename", filename); +/* grid_map_sub_ = node_.subscribe("/elevation_mapping/elevation_map", 100, &Pass::elevationMapCallback, this); point_cloud_sub_ = node_.subscribe("/plane_seg/point_cloud_in", 100, &Pass::pointCloudCallback, this); pose_sub_ = node_.subscribe("/state_estimator/pose_in_odom", 100, &Pass::robotPoseCallBack, this); - +*/ received_cloud_pub_ = node_.advertise("/plane_seg/received_cloud", 10); hull_cloud_pub_ = node_.advertise("/plane_seg/hull_cloud", 10); hull_markers_pub_ = node_.advertise("/plane_seg/hull_markers", 10); look_pose_pub_ = node_.advertise("/plane_seg/look_pose", 10); + elev_map_pub_ = node_.advertise("/rooster_elevation_mapping/elevation_map", 1); + pose_pub_ = node_.advertise("/vilens/pose", 1); last_robot_pose_ = Eigen::Isometry3d::Identity(); @@ -77,15 +90,21 @@ void Pass::robotPoseCallBack(const geometry_msgs::PoseWithCovarianceStampedConst } void Pass::elevationMapCallback(const grid_map_msgs::GridMap& msg){ - //std::cout << "got grid map / ev map\n"; + std::cout << "got grid map / ev map\n"; // convert message to GridMap, to PointCloud to LabeledCloud grid_map::GridMap map; + std::cout << "created gmap object map\n"; grid_map::GridMapRosConverter::fromMessage(msg, map); + std::cout << "converted gmap_msg to gmap map\n"; sensor_msgs::PointCloud2 pointCloud; + std::cout << "created pointcloud object pointCloud\n"; grid_map::GridMapRosConverter::toPointCloud(map, "elevation", pointCloud); + std::cout << "converted gmap map to pointcloud pointCloud\n"; planeseg::LabeledCloud::Ptr inCloud(new planeseg::LabeledCloud()); - pcl::fromROSMsg(pointCloud,*inCloud); + std::cout << "created object inCloud\n"; + pcl::fromROSMsg(pointCloud, *inCloud); + std::cout << "called fromROSMsg\n"; Eigen::Vector3f origin, lookDir; origin << last_robot_pose_.translation().cast(); @@ -167,6 +186,40 @@ void Pass::processFromFile(int test_example){ processCloud(inCloud, origin, lookDir); } +void Pass::stepThroughFile(){ + +// std::cout << filename < topics; + topics.push_back(std::string("/rooster_elevation_mapping/elevation_map")); + topics.push_back(std::string("/vilens/pose")); + + rosbag::View view(bag, rosbag::TopicQuery(topics)); + + foreach(rosbag::MessageInstance const m, view){ + grid_map_msgs::GridMap::ConstPtr s = m.instantiate(); + + if (s != NULL){ + std::cout << "received gridmap at time " << m.getTime().toNSec() << " with resolution:" << s->info.resolution << std::endl; + elevationMapCallback(*s); + elev_map_pub_.publish(*s); + std::cout << "Press [Enter] to continue to next gridmap message" << std::endl; + std::cin.get(); + } + + geometry_msgs::PoseWithCovarianceStamped::ConstPtr i = m.instantiate(); + if (i !=NULL){ + std::cout << "position (x, y, z): " << i->pose.pose.position.x << ", " << i->pose.pose.position.y << ", " << i->pose.pose.position.z << std::endl; + robotPoseCallBack(i); + pose_pub_.publish(*i); + } + } + + bag.close(); +} + void Pass::processCloud(planeseg::LabeledCloud::Ptr& inCloud, Eigen::Vector3f origin, Eigen::Vector3f lookDir){ @@ -267,7 +320,7 @@ void Pass::publishResult(){ publishHullsAsMarkers(cloud_ptrs, 0, 0); //pcl::PCDWriter pcd_writer_; - //pcd_writer_.write ("/home/mfallon/out.pcd", cloud, false); + //cd_writer_.write ("/home/mfallon/out.pcd", cloud, false); //std::cout << "blocks: " << result_.mBlocks.size() << " blocks\n"; //std::cout << "cloud: " << cloud.points.size() << " pts\n"; } diff --git a/plane_seg_ros/src/plane_seg_ros_node.cpp b/plane_seg_ros/src/plane_seg_ros_node.cpp index 3f41568..0e3fcd1 100644 --- a/plane_seg_ros/src/plane_seg_ros_node.cpp +++ b/plane_seg_ros/src/plane_seg_ros_node.cpp @@ -38,6 +38,18 @@ int main( int argc, char** argv ){ exit(-1); } + bool run_sequential_test = false; + nh.param("/plane_seg/run_sequential_test", run_sequential_test, false); + std::cout << "run_sequential_test: " << run_sequential_test << "\n"; + + //Enable this to run the sequential test + if (run_sequential_test){ + std::cout << "Running sequential test\n"; + app->stepThroughFile(); + std::cout << "Finished!\n"; + exit(-1); + } + ROS_INFO_STREAM("Waiting for ROS messages"); ros::spin(); From 77ec3266198b43a562ecc7060dee1212a794cc0c Mon Sep 17 00:00:00 2001 From: Christos Ioannou Date: Mon, 14 Dec 2020 19:01:02 +0000 Subject: [PATCH 03/58] make function stepThroughFile accept filename as argument --- plane_seg_ros/include/plane_seg_ros/Pass.hpp | 2 +- plane_seg_ros/src/plane_seg_ros.cpp | 6 +++--- plane_seg_ros/src/plane_seg_ros_node.cpp | 4 +++- 3 files changed, 7 insertions(+), 5 deletions(-) diff --git a/plane_seg_ros/include/plane_seg_ros/Pass.hpp b/plane_seg_ros/include/plane_seg_ros/Pass.hpp index 41b518e..18e8411 100644 --- a/plane_seg_ros/include/plane_seg_ros/Pass.hpp +++ b/plane_seg_ros/include/plane_seg_ros/Pass.hpp @@ -26,7 +26,7 @@ class Pass{ void processCloud(planeseg::LabeledCloud::Ptr& inCloud, Eigen::Vector3f origin, Eigen::Vector3f lookDir); void processFromFile(int test_example); - void stepThroughFile(); + void stepThroughFile(std::string filename); void publishHullsAsCloud(std::vector::Ptr> cloud_ptrs, int secs, int nsecs); diff --git a/plane_seg_ros/src/plane_seg_ros.cpp b/plane_seg_ros/src/plane_seg_ros.cpp index 9a0873a..34d67cb 100644 --- a/plane_seg_ros/src/plane_seg_ros.cpp +++ b/plane_seg_ros/src/plane_seg_ros.cpp @@ -186,12 +186,12 @@ void Pass::processFromFile(int test_example){ processCloud(inCloud, origin, lookDir); } -void Pass::stepThroughFile(){ +void Pass::stepThroughFile(std::string filename){ -// std::cout << filename < topics; topics.push_back(std::string("/rooster_elevation_mapping/elevation_map")); topics.push_back(std::string("/vilens/pose")); diff --git a/plane_seg_ros/src/plane_seg_ros_node.cpp b/plane_seg_ros/src/plane_seg_ros_node.cpp index 0e3fcd1..2699454 100644 --- a/plane_seg_ros/src/plane_seg_ros_node.cpp +++ b/plane_seg_ros/src/plane_seg_ros_node.cpp @@ -13,6 +13,8 @@ int main( int argc, char** argv ){ ros::init(argc, argv, "plane_seg"); ros::NodeHandle nh("~"); + std::string filename_; + nh.getParam("/rosbag_pass/filename", filename_); std::unique_ptr app = std::make_unique(nh); ROS_INFO_STREAM("plane_seg ros ready"); @@ -45,7 +47,7 @@ int main( int argc, char** argv ){ //Enable this to run the sequential test if (run_sequential_test){ std::cout << "Running sequential test\n"; - app->stepThroughFile(); + app->stepThroughFile(filename_); std::cout << "Finished!\n"; exit(-1); } From fdb58abb2da7fb745551cb3dc4415350c167e5a6 Mon Sep 17 00:00:00 2001 From: Marco Camurri Date: Mon, 14 Dec 2020 19:07:57 +0000 Subject: [PATCH 04/58] add config launch --- plane_seg_ros/config/rosbag_sequential.yaml | 4 ++++ plane_seg_ros/launch/rosbag_sequential.launch | 8 ++++++++ 2 files changed, 12 insertions(+) create mode 100644 plane_seg_ros/config/rosbag_sequential.yaml create mode 100644 plane_seg_ros/launch/rosbag_sequential.launch diff --git a/plane_seg_ros/config/rosbag_sequential.yaml b/plane_seg_ros/config/rosbag_sequential.yaml new file mode 100644 index 0000000..f7dd18a --- /dev/null +++ b/plane_seg_ros/config/rosbag_sequential.yaml @@ -0,0 +1,4 @@ +plane_seg: + run_test_program: false + run_sequential_test: true + diff --git a/plane_seg_ros/launch/rosbag_sequential.launch b/plane_seg_ros/launch/rosbag_sequential.launch new file mode 100644 index 0000000..8c0d0ae --- /dev/null +++ b/plane_seg_ros/launch/rosbag_sequential.launch @@ -0,0 +1,8 @@ + + + + + + + + From c093831a21bbd3ed655768586ab1e264b1cb0a04 Mon Sep 17 00:00:00 2001 From: Marco Camurri Date: Mon, 14 Dec 2020 19:31:05 +0000 Subject: [PATCH 05/58] change the param name to rosbag_path. read it when needed --- plane_seg_ros/src/plane_seg_ros_node.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/plane_seg_ros/src/plane_seg_ros_node.cpp b/plane_seg_ros/src/plane_seg_ros_node.cpp index 2699454..a3fe2ea 100644 --- a/plane_seg_ros/src/plane_seg_ros_node.cpp +++ b/plane_seg_ros/src/plane_seg_ros_node.cpp @@ -13,8 +13,7 @@ int main( int argc, char** argv ){ ros::init(argc, argv, "plane_seg"); ros::NodeHandle nh("~"); - std::string filename_; - nh.getParam("/rosbag_pass/filename", filename_); + std::unique_ptr app = std::make_unique(nh); ROS_INFO_STREAM("plane_seg ros ready"); @@ -47,6 +46,10 @@ int main( int argc, char** argv ){ //Enable this to run the sequential test if (run_sequential_test){ std::cout << "Running sequential test\n"; + + std::string filename_; + nh.getParam("rosbag_path", filename_); + app->stepThroughFile(filename_); std::cout << "Finished!\n"; exit(-1); From 07f6b8e1ff531a0fea22742bd403ab985718b080 Mon Sep 17 00:00:00 2001 From: Christos Ioannou Date: Mon, 14 Dec 2020 20:39:29 +0000 Subject: [PATCH 06/58] rviz node to view results --- plane_seg_ros/launch/rosbag_sequential.launch | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/plane_seg_ros/launch/rosbag_sequential.launch b/plane_seg_ros/launch/rosbag_sequential.launch index 8c0d0ae..72b4a37 100644 --- a/plane_seg_ros/launch/rosbag_sequential.launch +++ b/plane_seg_ros/launch/rosbag_sequential.launch @@ -1,8 +1,12 @@ - + + +> + + From 86f1bb8ef5d4b43e4525ba48b3d9ae0a84b9f109 Mon Sep 17 00:00:00 2001 From: Christos Ioannou Date: Wed, 6 Jan 2021 17:44:53 +0000 Subject: [PATCH 07/58] printing centroids --- plane_seg/CMakeLists.txt | 1 + plane_seg/src/tracker.cpp | 0 plane_seg_ros/launch/rosbag_sequential.launch | 2 +- plane_seg_ros/src/plane_seg_ros.cpp | 57 +++++++++++++++++-- 4 files changed, 53 insertions(+), 7 deletions(-) create mode 100644 plane_seg/src/tracker.cpp diff --git a/plane_seg/CMakeLists.txt b/plane_seg/CMakeLists.txt index 9a66f36..67aa163 100644 --- a/plane_seg/CMakeLists.txt +++ b/plane_seg/CMakeLists.txt @@ -34,6 +34,7 @@ add_library(${LIB_NAME} SHARED src/PlaneSegmenter.cpp src/RectangleFitter.cpp src/BlockFitter.cpp + src/Tracker.cpp ) add_dependencies(plane_seg ${catkin_EXPORTED_TARGETS}) target_link_libraries(plane_seg ${catkin_LIBRARIES}) diff --git a/plane_seg/src/tracker.cpp b/plane_seg/src/tracker.cpp new file mode 100644 index 0000000..e69de29 diff --git a/plane_seg_ros/launch/rosbag_sequential.launch b/plane_seg_ros/launch/rosbag_sequential.launch index 72b4a37..83b25a5 100644 --- a/plane_seg_ros/launch/rosbag_sequential.launch +++ b/plane_seg_ros/launch/rosbag_sequential.launch @@ -2,7 +2,7 @@ - + diff --git a/plane_seg_ros/src/plane_seg_ros.cpp b/plane_seg_ros/src/plane_seg_ros.cpp index 34d67cb..bb99bfa 100644 --- a/plane_seg_ros/src/plane_seg_ros.cpp +++ b/plane_seg_ros/src/plane_seg_ros.cpp @@ -12,6 +12,7 @@ #include #include #include +#include #include @@ -89,22 +90,17 @@ void Pass::robotPoseCallBack(const geometry_msgs::PoseWithCovarianceStampedConst tf::poseMsgToEigen(msg->pose.pose, last_robot_pose_); } + void Pass::elevationMapCallback(const grid_map_msgs::GridMap& msg){ std::cout << "got grid map / ev map\n"; // convert message to GridMap, to PointCloud to LabeledCloud grid_map::GridMap map; - std::cout << "created gmap object map\n"; grid_map::GridMapRosConverter::fromMessage(msg, map); - std::cout << "converted gmap_msg to gmap map\n"; sensor_msgs::PointCloud2 pointCloud; - std::cout << "created pointcloud object pointCloud\n"; grid_map::GridMapRosConverter::toPointCloud(map, "elevation", pointCloud); - std::cout << "converted gmap map to pointcloud pointCloud\n"; planeseg::LabeledCloud::Ptr inCloud(new planeseg::LabeledCloud()); - std::cout << "created object inCloud\n"; pcl::fromROSMsg(pointCloud, *inCloud); - std::cout << "called fromROSMsg\n"; Eigen::Vector3f origin, lookDir; origin << last_robot_pose_.translation().cast(); @@ -203,6 +199,8 @@ void Pass::stepThroughFile(std::string filename){ if (s != NULL){ std::cout << "received gridmap at time " << m.getTime().toNSec() << " with resolution:" << s->info.resolution << std::endl; + int i = 0; + if (i ) elevationMapCallback(*s); elev_map_pub_.publish(*s); std::cout << "Press [Enter] to continue to next gridmap message" << std::endl; @@ -299,6 +297,19 @@ void Pass::printResultAsJson(){ void Pass::publishResult(){ // convert result to a vector of point clouds std::vector< pcl::PointCloud::Ptr > cloud_ptrs; + std::vector centroid_list; + std::vector old_centroid_list; + + if (old_centroid_list.empty()){ + Eigen::Vector4f old_centroid_list_init; + old_centroid_list_init << 0, + 0, + 0, + 1; + old_centroid_list.push_back(old_centroid_list_init); + } +// printCentroidList(old_centroid_list); + for (size_t i=0; i cloud; const auto& block = result_.mBlocks[i]; @@ -311,14 +322,24 @@ void Pass::publishResult(){ } cloud.height = cloud.points.size(); cloud.width = 1; + Eigen::Vector4f centroid; + pcl::compute3DCentroid(cloud, centroid); + centroid_list.push_back(centroid); pcl::PointCloud::Ptr cloud_ptr; cloud_ptr = cloud.makeShared(); cloud_ptrs.push_back(cloud_ptr); } + std::cout << "Centroid list: " << std::endl; + printCentroidList(centroid_list); + compareCentroidLists(old_centroid_list, centroid_list); +// std::cout << centroid_list[0] << std::endl; + publishHullsAsCloud(cloud_ptrs, 0, 0); publishHullsAsMarkers(cloud_ptrs, 0, 0); + old_centroid_list = centroid_list; + //pcl::PCDWriter pcd_writer_; //cd_writer_.write ("/home/mfallon/out.pcd", cloud, false); //std::cout << "blocks: " << result_.mBlocks.size() << " blocks\n"; @@ -440,4 +461,28 @@ void Pass::publishHullsAsMarkers(std::vector< pcl::PointCloud::Pt } +void Pass::printCentroidList(std::vector centroid_list_){ + for (unsigned i=0; i < centroid_list_.size(); i++){ + for (unsigned j=0; j < centroid_list_[i].size(); j++){ + std::cout << centroid_list_[i][j] << std::endl; + } + } +} + +void Pass::compareCentroidLists(std::vector old_centroid_list_, std::vector centroid_list_){ + +// double threshold = 1; + std::cout << "Entered compareCentroidLists" << std::endl; + for (unsigned i = 0; i < old_centroid_list_.size(); i++){ + + std::cout << "Entered first for loop" << std::endl; + for (unsigned j = 0; j < centroid_list_.size(); j++){ +// double z_dist = fabs(old_centroid_list_[i][2] - centroid_list_[j][2]); +// if (z_dist < threshold){ +// std::complex dist = (old_centroid_list_[i] - centroid_list_[j]).norm(); + std::cout << "The " << i << "'th old centroid is " << (old_centroid_list_[i] - centroid_list_[j]).norm() << " far away from the " << j << "'th new centroid." << std::endl; +// } + } + } +} From 728429db217599307c037dc1c163cf954d63f6bf Mon Sep 17 00:00:00 2001 From: Christos Ioannou Date: Sun, 10 Jan 2021 23:12:00 +0000 Subject: [PATCH 08/58] moving tracking to Tracker.cpp (1) --- plane_seg/include/plane_seg/Types.hpp | 4 ++-- plane_seg_ros/include/plane_seg_ros/Pass.hpp | 4 ++++ plane_seg_ros/src/plane_seg_ros.cpp | 25 ++++++++++++-------- plane_seg_ros/src/plane_seg_ros_node.cpp | 1 + 4 files changed, 22 insertions(+), 12 deletions(-) diff --git a/plane_seg/include/plane_seg/Types.hpp b/plane_seg/include/plane_seg/Types.hpp index 02078ad..da32837 100644 --- a/plane_seg/include/plane_seg/Types.hpp +++ b/plane_seg/include/plane_seg/Types.hpp @@ -8,7 +8,7 @@ namespace planeseg { - /* +/* struct Point { PCL_ADD_POINT4D; float delta; @@ -23,7 +23,7 @@ POINT_CLOUD_REGISTER_POINT_STRUCT(Point, (float, delta, delta) (int, label, label) ) - */ +*/ typedef pcl::PointXYZL Point; typedef pcl::PointCloud NormalCloud; diff --git a/plane_seg_ros/include/plane_seg_ros/Pass.hpp b/plane_seg_ros/include/plane_seg_ros/Pass.hpp index 18e8411..cb05469 100644 --- a/plane_seg_ros/include/plane_seg_ros/Pass.hpp +++ b/plane_seg_ros/include/plane_seg_ros/Pass.hpp @@ -10,6 +10,7 @@ #include #include #include +#include namespace planeseg { @@ -34,6 +35,8 @@ class Pass{ int secs, int nsecs); void printResultAsJson(); void publishResult(); + void printCentroidList(std::vector centroid_list); + void compareCentroidLists(std::vector old_centroid_list, std::vector centroid_list); private: ros::NodeHandle node_; @@ -44,5 +47,6 @@ class Pass{ Eigen::Isometry3d last_robot_pose_; planeseg::BlockFitter::Result result_; + planeseg::Tracker tracking_; }; } diff --git a/plane_seg_ros/src/plane_seg_ros.cpp b/plane_seg_ros/src/plane_seg_ros.cpp index bb99bfa..d387399 100644 --- a/plane_seg_ros/src/plane_seg_ros.cpp +++ b/plane_seg_ros/src/plane_seg_ros.cpp @@ -24,6 +24,8 @@ #include #include + +#include "plane_seg/Tracker.hpp" #define foreach BOOST_FOREACH @@ -53,6 +55,8 @@ Pass::Pass(ros::NodeHandle node_): last_robot_pose_ = Eigen::Isometry3d::Identity(); + tracking_ = new planeseg::Tracker(); + colors_ = { 51/255.0, 160/255.0, 44/255.0, //0 166/255.0, 206/255.0, 227/255.0, @@ -232,7 +236,8 @@ void Pass::processCloud(planeseg::LabeledCloud::Ptr& inCloud, Eigen::Vector3f or fitter.setMaxAngleOfPlaneSegmenter(10); result_ = fitter.go(); - + tracking_.reset(); + tracking_.test(result_); Eigen::Vector3f rz = lookDir; Eigen::Vector3f rx = rz.cross(Eigen::Vector3f::UnitZ()); @@ -297,7 +302,7 @@ void Pass::printResultAsJson(){ void Pass::publishResult(){ // convert result to a vector of point clouds std::vector< pcl::PointCloud::Ptr > cloud_ptrs; - std::vector centroid_list; +/* std::vector centroid_list; std::vector old_centroid_list; if (old_centroid_list.empty()){ @@ -309,7 +314,7 @@ void Pass::publishResult(){ old_centroid_list.push_back(old_centroid_list_init); } // printCentroidList(old_centroid_list); - +*/ for (size_t i=0; i cloud; const auto& block = result_.mBlocks[i]; @@ -322,23 +327,23 @@ void Pass::publishResult(){ } cloud.height = cloud.points.size(); cloud.width = 1; - Eigen::Vector4f centroid; - pcl::compute3DCentroid(cloud, centroid); - centroid_list.push_back(centroid); +// Eigen::Vector4f centroid; +// pcl::compute3DCentroid(cloud, centroid); +// centroid_list.push_back(centroid); pcl::PointCloud::Ptr cloud_ptr; cloud_ptr = cloud.makeShared(); cloud_ptrs.push_back(cloud_ptr); } - std::cout << "Centroid list: " << std::endl; +/* std::cout << "Centroid list: " << std::endl; printCentroidList(centroid_list); compareCentroidLists(old_centroid_list, centroid_list); -// std::cout << centroid_list[0] << std::endl; - + std::cout << centroid_list[0] << std::endl; +*/ publishHullsAsCloud(cloud_ptrs, 0, 0); publishHullsAsMarkers(cloud_ptrs, 0, 0); - old_centroid_list = centroid_list; +// old_centroid_list = centroid_list; //pcl::PCDWriter pcd_writer_; //cd_writer_.write ("/home/mfallon/out.pcd", cloud, false); diff --git a/plane_seg_ros/src/plane_seg_ros_node.cpp b/plane_seg_ros/src/plane_seg_ros_node.cpp index a3fe2ea..36af384 100644 --- a/plane_seg_ros/src/plane_seg_ros_node.cpp +++ b/plane_seg_ros/src/plane_seg_ros_node.cpp @@ -2,6 +2,7 @@ #include "plane_seg_ros/Pass.hpp" #include #include +#include using namespace planeseg; From 0113ffefa7a0d5be87b17859c20dfd56b8744d4b Mon Sep 17 00:00:00 2001 From: Christos Ioannou Date: Sun, 10 Jan 2021 23:14:21 +0000 Subject: [PATCH 09/58] moving tracker into Tracker.cpp (2) --- plane_seg/include/plane_seg/Tracker.hpp | 45 ++++++++ plane_seg/src/Tracker.cpp | 134 ++++++++++++++++++++++++ 2 files changed, 179 insertions(+) create mode 100644 plane_seg/include/plane_seg/Tracker.hpp create mode 100644 plane_seg/src/Tracker.cpp diff --git a/plane_seg/include/plane_seg/Tracker.hpp b/plane_seg/include/plane_seg/Tracker.hpp new file mode 100644 index 0000000..3669215 --- /dev/null +++ b/plane_seg/include/plane_seg/Tracker.hpp @@ -0,0 +1,45 @@ +#ifndef _planeseg_Tracker_hpp_ +#define _planeseg_Tracker_hpp_ + +#include +#include +#include +#include "plane_seg/BlockFitter.hpp" + +namespace planeseg { + +struct plane { + pcl::PointCloud cloud; + int id; + pcl::PointXYZ centroid; +}; + +// a vector to hold the ids of oldStairs and a flag for whether the id has been assigned +struct IdAssigned{ + bool taken; + int id; +}; + +class Tracker { +public: + Tracker(); + ~Tracker(); + + int get_new_centroid_id(plane plane_); + pcl::PointXYZ find_centroid(pcl::PointCloud::Ptr cloud ); + void reset(); + std::vector::Ptr> convertResult(planeseg::BlockFitter::Result result); + void test(planeseg::BlockFitter::Result result); + void printStairs(std::vector centroid_list_); + +private: + std::vector newStairs; + std::vector oldStairs; + std::vector idAssigned; + std::vector::Ptr> cloud_ptrs; + std::vector icentroid_list_init; + int totalIds; +}; +} + +#endif diff --git a/plane_seg/src/Tracker.cpp b/plane_seg/src/Tracker.cpp new file mode 100644 index 0000000..21b7fef --- /dev/null +++ b/plane_seg/src/Tracker.cpp @@ -0,0 +1,134 @@ +#include "plane_seg/Tracker.hpp" +#include +#include > +#include +#include +#include "plane_seg/BlockFitter.hpp" +#include "plane_seg/Types.hpp" +#include "pcl/common/impl/centroid.hpp" +#include + +using namespace planeseg; + +Tracker::Tracker(){ + totalIds = 1; +} + +Tracker::~Tracker(){} + +pcl::PointXYZ Tracker::find_centroid(pcl::PointCloud::Ptr cloud ){ + std::cout << "Computing centroid" << std::endl; + pcl::PointXYZ centroid; + pcl::compute3DCentroid(cloud, centroid); + return centroid; +} + +std::vector::Ptr> Tracker::convertResult(planeseg::BlockFitter::Result result_){ + for (size_t i=0; i cloud; + const auto& block = result_.mBlocks[i]; + for (size_t j =0; j < block.mHull.size(); ++j){ + pcl::PointXYZ pt; + pt.x =block.mHull[j](0); + pt.y =block.mHull[j](1); + pt.z =block.mHull[j](2); + cloud.points.push_back(pt); + } + cloud.height = cloud.points.size(); + cloud.width = 1; + pcl::PointCloud::Ptr cloud_ptr; + cloud_ptr = cloud.makeShared(); + cloud_ptrs.push_back(cloud_ptr); + } + return cloud_ptrs; +} + +// First test is to just calculate and print centroids +void Tracker::test(planeseg::BlockFitter::Result result_){ + reset(); + std::vector::Ptr> cloud_ptrs_; + cloud_ptrs = convertResult(result_); + for (int i =0 ; i < cloud_ptrs_.size ; i++){ + newStairs[i] = find_centroid(cloud_ptrs_[i]); + } + printStairs(newStairs); +}; + +// Create a vector newStairs containing the centroids from result +std::vector Tracker::newStairs{ + +}; + +int Tracker::get_new_centroid_id(plane plane){ + + bool test = true; + int id; + + if(oldStairs.empty()){ + id = totalIds; + ++totalIds; + } + + else{ + pcl::PointXYZ centroid; + double threshold = 1; + double distance; + double distz; + int closest = -1; + double closestDist = 0; + + std::cout << "Entered get_new_centroid_id" << std::endl; + + // go through each plane in oldStair and compare it to the new plane + for (int i = 0; i < oldStairs.size(); i++){ + + centroid = oldStairs[i].centroid; + + if (!idAssigned[i].taken && test){ + distz = fabs(centroid.z = plane.centroid.z); + if (distz < threshold) { + distance = (oldStairs[i] - plane.centroid).norm(); + + if (closest == -1 || distance < closestDist){ + closest = i; + closestDist = distance; + } + } + } + } + + if (closest != -1){ + id = oldStairs[closest].id; + idAssigned[closest].taken = true; + } + else{ + id = totalIds; + ++totalIds; + } + } + plane.id = id; + newStairs.push_back(plane); + return id; +} + +void Tracker::reset(){ + oldStairs = newStairs; + newStairs = clear(); + idAssigned.clear(); + idAssigned temp; + temp.taken = false; + for (int i=0; i < oldStairs.size(); i++){ + temp.id = oldStairs[i].id; + idAssigned.push_back(temp); + } +} + + +void printStairs(std::vector stairs){ + for (unsigned i=0; i < stairs.size(); i++){ + for (unsigned j=0; j < stairs[i].size(); j++){ + std::cout << stairs[i][j] << std::endl; + } + } +} + From 1cabb63547cd3a26b1893b8340b0c52f39d8d759 Mon Sep 17 00:00:00 2001 From: Christos Ioannou Date: Sun, 10 Jan 2021 23:21:50 +0000 Subject: [PATCH 10/58] Moving tracker to Tracker.cpp (3) --- plane_seg/src/Tracker.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/plane_seg/src/Tracker.cpp b/plane_seg/src/Tracker.cpp index 21b7fef..2b487a3 100644 --- a/plane_seg/src/Tracker.cpp +++ b/plane_seg/src/Tracker.cpp @@ -54,10 +54,6 @@ void Tracker::test(planeseg::BlockFitter::Result result_){ printStairs(newStairs); }; -// Create a vector newStairs containing the centroids from result -std::vector Tracker::newStairs{ - -}; int Tracker::get_new_centroid_id(plane plane){ From 0f939fcdaa21c90f7b2cfebbbae1313bdd6b76d2 Mon Sep 17 00:00:00 2001 From: Christos Ioannou Date: Tue, 12 Jan 2021 04:00:05 +0000 Subject: [PATCH 11/58] the internal algorithms of a Visualiser --- plane_seg/CMakeLists.txt.user | 311 ++++++++++++++++++ plane_seg/include/plane_seg/Tracker.hpp | 2 +- plane_seg/src/Tracker.cpp | 6 +- plane_seg_ros/CMakeLists.txt | 3 +- plane_seg_ros/CMakeLists.txt.user | 199 +++++++++++ .../include/plane_seg_ros/Visualizer.hpp | 30 ++ plane_seg_ros/src/Visualizer.cpp | 186 +++++++++++ 7 files changed, 732 insertions(+), 5 deletions(-) create mode 100644 plane_seg/CMakeLists.txt.user create mode 100644 plane_seg_ros/CMakeLists.txt.user create mode 100644 plane_seg_ros/include/plane_seg_ros/Visualizer.hpp create mode 100644 plane_seg_ros/src/Visualizer.cpp diff --git a/plane_seg/CMakeLists.txt.user b/plane_seg/CMakeLists.txt.user new file mode 100644 index 0000000..a2e7fb1 --- /dev/null +++ b/plane_seg/CMakeLists.txt.user @@ -0,0 +1,311 @@ + + + + + + EnvironmentId + {7ac00433-0568-4ce0-bd62-1a22dfacb6f0} + + + ProjectExplorer.Project.ActiveTarget + 0 + + + ProjectExplorer.Project.EditorSettings + + true + false + true + + Cpp + + CppGlobal + + + + QmlJS + + QmlJSGlobal + + + 2 + UTF-8 + false + 4 + false + 80 + true + true + 1 + true + false + 0 + true + true + 1 + 8 + true + 1 + true + true + true + false + + + + ProjectExplorer.Project.PluginSettings + + + + ProjectExplorer.Project.Target.0 + + Desktop + Desktop + {37551fef-a8d4-4681-9dee-ef96a26fa7a7} + 0 + 0 + 0 + + + CMAKE_CXX_COMPILER:FILEPATH=/usr/bin/c++ + CMAKE_C_COMPILER:FILEPATH=/usr/bin/cc + + /home/christos/catkin_ws/src/plane_seg/plane_seg/build + + + + + all + + true + CMake Build + + CMakeProjectManager.MakeStep + + 1 + Build + + ProjectExplorer.BuildSteps.Build + + + + + + clean + + true + CMake Build + + CMakeProjectManager.MakeStep + + 1 + Clean + + ProjectExplorer.BuildSteps.Clean + + 2 + false + + Default + Default + CMakeProjectManager.CMakeBuildConfiguration + + 1 + + + 0 + Deploy + + ProjectExplorer.BuildSteps.Deploy + + 1 + Deploy Configuration + + ProjectExplorer.DefaultDeployConfiguration + + 1 + + + false + false + 1000 + + true + + false + false + false + false + true + 0.01 + 10 + true + 1 + 25 + + 1 + true + false + true + valgrind + + 0 + 1 + 2 + 3 + 4 + 5 + 6 + 7 + 8 + 9 + 10 + 11 + 12 + 13 + 14 + + block_fitter + + + /home/christos/catkin_ws/src/plane_seg/plane_seg/build/devel/lib/plane_seg + 2 + + block_fitter + + CMakeProjectManager.CMakeRunConfiguration.block_fitter + 3768 + false + true + false + false + true + + + false + false + 1000 + + true + + false + false + false + false + true + 0.01 + 10 + true + 1 + 25 + + 1 + true + false + true + valgrind + + 0 + 1 + 2 + 3 + 4 + 5 + 6 + 7 + 8 + 9 + 10 + 11 + 12 + 13 + 14 + + plane_seg_test + + + /home/christos/catkin_ws/src/plane_seg/plane_seg/build/devel/lib/plane_seg + -1 + + plane_seg_test + + CMakeProjectManager.CMakeRunConfiguration.plane_seg_test + 3768 + false + true + false + false + true + + + false + false + 1000 + + true + + false + false + false + false + true + 0.01 + 10 + true + 1 + 25 + + 1 + true + false + true + valgrind + + 0 + 1 + 2 + 3 + 4 + 5 + 6 + 7 + 8 + 9 + 10 + 11 + 12 + 13 + 14 + + plane_seg_test2 + + + /home/christos/catkin_ws/src/plane_seg/plane_seg/build/devel/lib/plane_seg + -1 + + plane_seg_test2 + + CMakeProjectManager.CMakeRunConfiguration.plane_seg_test2 + 3768 + false + true + false + false + true + + 3 + + + + ProjectExplorer.Project.TargetCount + 1 + + + ProjectExplorer.Project.Updater.FileVersion + 18 + + + Version + 18 + + diff --git a/plane_seg/include/plane_seg/Tracker.hpp b/plane_seg/include/plane_seg/Tracker.hpp index 3669215..01ed632 100644 --- a/plane_seg/include/plane_seg/Tracker.hpp +++ b/plane_seg/include/plane_seg/Tracker.hpp @@ -25,7 +25,7 @@ class Tracker { Tracker(); ~Tracker(); - int get_new_centroid_id(plane plane_); + int get_centroid_id(plane plane_); pcl::PointXYZ find_centroid(pcl::PointCloud::Ptr cloud ); void reset(); std::vector::Ptr> convertResult(planeseg::BlockFitter::Result result); diff --git a/plane_seg/src/Tracker.cpp b/plane_seg/src/Tracker.cpp index 2b487a3..fdca777 100644 --- a/plane_seg/src/Tracker.cpp +++ b/plane_seg/src/Tracker.cpp @@ -55,7 +55,7 @@ void Tracker::test(planeseg::BlockFitter::Result result_){ }; -int Tracker::get_new_centroid_id(plane plane){ +int Tracker::get_centroid_id(planeseg::plane plane){ bool test = true; int id; @@ -73,9 +73,9 @@ int Tracker::get_new_centroid_id(plane plane){ int closest = -1; double closestDist = 0; - std::cout << "Entered get_new_centroid_id" << std::endl; + std::cout << "Entered get_centroid_id" << std::endl; - // go through each plane in oldStair and compare it to the new plane + // go through each plane in oldStairs and compare it to the new plane for (int i = 0; i < oldStairs.size(); i++){ centroid = oldStairs[i].centroid; diff --git a/plane_seg_ros/CMakeLists.txt b/plane_seg_ros/CMakeLists.txt index a6f719c..2367f0e 100644 --- a/plane_seg_ros/CMakeLists.txt +++ b/plane_seg_ros/CMakeLists.txt @@ -36,7 +36,8 @@ include_directories( ) add_library(${PROJECT_NAME} SHARED src/${PROJECT_NAME}.cpp - src/geometry_utils.cpp) + src/geometry_utils.cpp + src/Visualizer.cpp) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) diff --git a/plane_seg_ros/CMakeLists.txt.user b/plane_seg_ros/CMakeLists.txt.user new file mode 100644 index 0000000..8637735 --- /dev/null +++ b/plane_seg_ros/CMakeLists.txt.user @@ -0,0 +1,199 @@ + + + + + + EnvironmentId + {7ac00433-0568-4ce0-bd62-1a22dfacb6f0} + + + ProjectExplorer.Project.ActiveTarget + 0 + + + ProjectExplorer.Project.EditorSettings + + true + false + true + + Cpp + + CppGlobal + + + + QmlJS + + QmlJSGlobal + + + 2 + UTF-8 + false + 4 + false + 80 + true + true + 1 + true + false + 0 + true + true + 1 + 8 + true + 1 + true + true + true + false + + + + ProjectExplorer.Project.PluginSettings + + + + ProjectExplorer.Project.Target.0 + + Desktop + Desktop + {37551fef-a8d4-4681-9dee-ef96a26fa7a7} + 0 + 0 + 0 + + + CMAKE_CXX_COMPILER:FILEPATH=/usr/bin/c++ + CMAKE_C_COMPILER:FILEPATH=/usr/bin/cc + + /home/christos/catkin_ws/src/plane_seg/plane_seg_ros/build + + + + + all + + true + CMake Build + + CMakeProjectManager.MakeStep + + 1 + Build + + ProjectExplorer.BuildSteps.Build + + + + + + all + + true + CMake Build + + CMakeProjectManager.MakeStep + + 1 + Clean + + ProjectExplorer.BuildSteps.Clean + + 2 + false + + Default + Default + CMakeProjectManager.CMakeBuildConfiguration + + 1 + + + 0 + Deploy + + ProjectExplorer.BuildSteps.Deploy + + 1 + Deploy Configuration + + ProjectExplorer.DefaultDeployConfiguration + + 1 + + + false + false + 1000 + + true + + false + false + false + false + true + 0.01 + 10 + true + 1 + 25 + + 1 + true + false + true + valgrind + + 0 + 1 + 2 + 3 + 4 + 5 + 6 + 7 + 8 + 9 + 10 + 11 + 12 + 13 + 14 + + plane_seg_ros_node + + + /home/christos/catkin_ws/src/plane_seg/plane_seg_ros/build/devel/lib/plane_seg_ros + 2 + + plane_seg_ros_node + + CMakeProjectManager.CMakeRunConfiguration.plane_seg_ros_node + 3768 + false + true + false + false + true + + 1 + + + + ProjectExplorer.Project.TargetCount + 1 + + + ProjectExplorer.Project.Updater.FileVersion + 18 + + + Version + 18 + + diff --git a/plane_seg_ros/include/plane_seg_ros/Visualizer.hpp b/plane_seg_ros/include/plane_seg_ros/Visualizer.hpp new file mode 100644 index 0000000..ca206ef --- /dev/null +++ b/plane_seg_ros/include/plane_seg_ros/Visualizer.hpp @@ -0,0 +1,30 @@ +#ifndef _planeseg_Visualizer_hpp_ +#define _planeseg_Visualizer_hpp_ + +#include +#include +#include +#include +#include "plane_seg/Tracker.hpp" + +struct line_strip { + visualization_msgs::Marker centroidsMarker; + int id; +} + +class Visualizer{ +public: + Visualizer(); + explicit Visualizer(ros::Publisher centroids_pub, ros::Publisher linestrip_pub); + ~Visualizer(); + +private: + sensor_msgs::PointCloud2 displayCentroids(std::vector &planes); + visualization_msgs::Marker displayString(int id, std::string string_, geometry_msgs::Point point_); + visualization_msgs::Marker displayLineStrip(int id, pcl::PointXYZ newCentroid); + std::vector colors_; + std::vector lineStrips; + unsigned getR(int id); + unsigned getG(int id); + unsigned getB(int id); +} diff --git a/plane_seg_ros/src/Visualizer.cpp b/plane_seg_ros/src/Visualizer.cpp new file mode 100644 index 0000000..502945d --- /dev/null +++ b/plane_seg_ros/src/Visualizer.cpp @@ -0,0 +1,186 @@ +#include +#include +#include "plane_seg_ros/Visualizer.hpp" +#include +#include +#include +#include +#include "plane_seg/Tracker.hpp" + +#include +#include + +Visualizer::Visualizer(ros::Publisher centroids_pub, ros:: Publisher linestrip_pub): + centroids_pub_(centroids_pub), + linestrip_pub_(linestrip_pub) +{ + colors_ = { + 51/255.0, 160/255.0, 44/255.0, //0 + 166/255.0, 206/255.0, 227/255.0, + 178/255.0, 223/255.0, 138/255.0,//6 + 31/255.0, 120/255.0, 180/255.0, + 251/255.0, 154/255.0, 153/255.0,// 12 + 227/255.0, 26/255.0, 28/255.0, + 253/255.0, 191/255.0, 111/255.0,// 18 + 106/255.0, 61/255.0, 154/255.0, + 255/255.0, 127/255.0, 0/255.0, // 24 + 202/255.0, 178/255.0, 214/255.0, + 1.0, 0.0, 0.0, // red // 30 + 0.0, 1.0, 0.0, // green + 0.0, 0.0, 1.0, // blue// 36 + 1.0, 1.0, 0.0, + 1.0, 0.0, 1.0, // 42 + 0.0, 1.0, 1.0, + 0.5, 1.0, 0.0, + 1.0, 0.5, 0.0, + 0.5, 0.0, 1.0, + 1.0, 0.0, 0.5, + 0.0, 0.5, 1.0, + 0.0, 1.0, 0.5, + 1.0, 0.5, 0.5, + 0.5, 1.0, 0.5, + 0.5, 0.5, 1.0, + 0.5, 0.5, 1.0, + 0.5, 1.0, 0.5, + 0.5, 0.5, 1.0}; + +} + +/* NOT FINISHED YET +void Visualizer::publish(std::vector &planes){ + + // publish coloured centroids + sensor_msgs::PointCloud2 displayCentroidsMsg(displayCentroids(planes)); + centroids_pub_.publish(displayCentroidsMsg); + + // publish line segments + visualization_msgs::Marker displayLineStripMsg(displayCentroidsMsg); +} +*/ + +sensor_msgs::PointCloud2 Visualizer::displayCentroids(std::vector &planes){ + int id; + pcl::PointXYZRGB centroid; + pcl::PointCloud display_centroids; + + for (unsigned i = 0; i < planes.size(); i++){ + id = planes[i].id; + centroid.r = getR(id); + centroid.g = getG(id); + centroid.b = getB(id); + centroid.x = planes[i].centroid.x; + centroid.y = planes[i].centroid.y; + centroid.z = planes[i].centroid.z; + display_centroids.push_back(centroid); + } + sensor_msgs::PointCloud2 displayCentroidsMsg; + pcl::toROSMsg(display_centroids, displayCentroidsMsg); + return displayCentroidsMsg; +} + +visualization_msgs::Marker Visualizer::displayLineStrip(int id, pcl::PointXYZ newCentroid){ + + bool point_added; + point_added = false; + + // create visualization markers + visualization_msgs::Marker centroidMarker; + visualization_msgs::Marker lineStripMarker; + visualization_msgs::Marker newPlaneMarker; + + // create string of centroid_ID + std::ostringstream s; + s << "centroid_" << id; + std::string centroid_id; + centroid_id = s.str(); + + // first we must find out if the newCentroid should be added to an already existing linestrip + for (unsigned i = 0; i < lineStrips.size() && point_added == false; i++){ + if (id == lineStrips[i].id){ + lineStrips[i].centroidsMarker.points.push_back(newCentroid); + centroidMarker = lineStrips[i].centroidsMarker; + centroidMarker.header.frame_id = "odom"; + centroidMarker.header.stamp = ros::Time::now(); + centroidMarker.ns = "linestrips"; + centroidMarker.id = centroid_id; + + // declare outgoing marker as the linestrip with matching ID + lineStripMarker = centroidMarker; + point_added = true; + } + } + // if it is the centroid of a new plane, we must create a new linestrip for it + if (point_added == false){ + + // publish a point for the first marker of the new ID + newPlaneMarker.type = visualization_msgs::Marker::POINTS; + newPlaneMarker.header.frame_id = "odom"; + newPlaneMarker.header.stamp = ros::Time::now(); + newPlaneMarker.ns = "linestrips"; + newPlaneMarker.id = centroid_id; + newPlaneMarker.scale.x = 0.1; + newPlaneMarker.scale.y = 0.1; + newPlaneMarker.color.r = getR(id); + newPlaneMarker.color.g = getG(id); + newPlaneMarker.color.b = getB(id); + newPlaneMarker.color.a = 1; + newPlaneMarker.pose.orientation.w = 1.0; + newPlaneMarker.points.push_back(newCentroid); + + // create a new linestrip for the new ID + centroidMarker.type = visualization_msgs::Marker::LINE_STRIP; + centroidMarker.scale.x = 0.1; + centroidMarker.color.r = getR(id); + centroidMarker.color.g = getG(id); + centroidMarker.color.b = getB(id); + centroidMarker.color.a = 1; + centroidMarker.pose.orientation.w = 1.0; + centroidMarker.points.push_back(newCentroid); + line_strip newLineStrip; + newLineStrip.centroidsMarker = centroidMarker; + newLineStrip.id = id; + lineStrips.push_back(newLineStrip); + + // declare the outgoing marker as the new marker + lineStripMarker = newPlaneMarker; + } + + return lineStripMarker; +} + +visualization_msgs::Marker Visualizer::displayString(int id, std::string string, geometry_msgs::Point point){ + visualization_msgs::Marker stringMarker; + stringMarker.header.frame_id = "odom"; + stringMarker.header.stamp = ros::Time(); + stringMarker.ns = "strings"; + stringMarker.id = id; + stringMarker.type = visualization_msgs::Marker::TEXT_VIEW_FACING; + stringMarker.action = visualization_msgs::Marker::ADD; + stringMarker.pose.position.x = point.x; + stringMarker.pose.position.y = point.y; + stringMarker.pose.position.z = point.z; + stringMarker.scale.x = 1; + stringMarker.color.a = 1; + stringMarker.color.r = 1; + stringMarker.color.g = 1; + stringMarker.color.b = 1; + stringMarker.text = string; +} + +unsigned Visualizer::getR(int id){ + unsigned j; + j = id % (colors_.size()/3); + return colors_[3*j]*255; +} + +unsigned Visualizer::getG(int id){ + unsigned j; + j = id % (colors_.size()/3); + return colors_[3*j+1]*255; +} + +unsigned Visualizer::getB(int id){ + unsigned j; + j = id % (colors_.size()/3); + return colors_[3*j+2]*255; +} From d97043ea914a83803258333ff5931450be6a8a2e Mon Sep 17 00:00:00 2001 From: Marco Camurri Date: Tue, 12 Jan 2021 11:54:52 +0000 Subject: [PATCH 12/58] remove qtcreator project files from versioning --- plane_seg/CMakeLists.txt.user | 311 ------------------------------ plane_seg_ros/CMakeLists.txt.user | 199 ------------------- 2 files changed, 510 deletions(-) delete mode 100644 plane_seg/CMakeLists.txt.user delete mode 100644 plane_seg_ros/CMakeLists.txt.user diff --git a/plane_seg/CMakeLists.txt.user b/plane_seg/CMakeLists.txt.user deleted file mode 100644 index a2e7fb1..0000000 --- a/plane_seg/CMakeLists.txt.user +++ /dev/null @@ -1,311 +0,0 @@ - - - - - - EnvironmentId - {7ac00433-0568-4ce0-bd62-1a22dfacb6f0} - - - ProjectExplorer.Project.ActiveTarget - 0 - - - ProjectExplorer.Project.EditorSettings - - true - false - true - - Cpp - - CppGlobal - - - - QmlJS - - QmlJSGlobal - - - 2 - UTF-8 - false - 4 - false - 80 - true - true - 1 - true - false - 0 - true - true - 1 - 8 - true - 1 - true - true - true - false - - - - ProjectExplorer.Project.PluginSettings - - - - ProjectExplorer.Project.Target.0 - - Desktop - Desktop - {37551fef-a8d4-4681-9dee-ef96a26fa7a7} - 0 - 0 - 0 - - - CMAKE_CXX_COMPILER:FILEPATH=/usr/bin/c++ - CMAKE_C_COMPILER:FILEPATH=/usr/bin/cc - - /home/christos/catkin_ws/src/plane_seg/plane_seg/build - - - - - all - - true - CMake Build - - CMakeProjectManager.MakeStep - - 1 - Build - - ProjectExplorer.BuildSteps.Build - - - - - - clean - - true - CMake Build - - CMakeProjectManager.MakeStep - - 1 - Clean - - ProjectExplorer.BuildSteps.Clean - - 2 - false - - Default - Default - CMakeProjectManager.CMakeBuildConfiguration - - 1 - - - 0 - Deploy - - ProjectExplorer.BuildSteps.Deploy - - 1 - Deploy Configuration - - ProjectExplorer.DefaultDeployConfiguration - - 1 - - - false - false - 1000 - - true - - false - false - false - false - true - 0.01 - 10 - true - 1 - 25 - - 1 - true - false - true - valgrind - - 0 - 1 - 2 - 3 - 4 - 5 - 6 - 7 - 8 - 9 - 10 - 11 - 12 - 13 - 14 - - block_fitter - - - /home/christos/catkin_ws/src/plane_seg/plane_seg/build/devel/lib/plane_seg - 2 - - block_fitter - - CMakeProjectManager.CMakeRunConfiguration.block_fitter - 3768 - false - true - false - false - true - - - false - false - 1000 - - true - - false - false - false - false - true - 0.01 - 10 - true - 1 - 25 - - 1 - true - false - true - valgrind - - 0 - 1 - 2 - 3 - 4 - 5 - 6 - 7 - 8 - 9 - 10 - 11 - 12 - 13 - 14 - - plane_seg_test - - - /home/christos/catkin_ws/src/plane_seg/plane_seg/build/devel/lib/plane_seg - -1 - - plane_seg_test - - CMakeProjectManager.CMakeRunConfiguration.plane_seg_test - 3768 - false - true - false - false - true - - - false - false - 1000 - - true - - false - false - false - false - true - 0.01 - 10 - true - 1 - 25 - - 1 - true - false - true - valgrind - - 0 - 1 - 2 - 3 - 4 - 5 - 6 - 7 - 8 - 9 - 10 - 11 - 12 - 13 - 14 - - plane_seg_test2 - - - /home/christos/catkin_ws/src/plane_seg/plane_seg/build/devel/lib/plane_seg - -1 - - plane_seg_test2 - - CMakeProjectManager.CMakeRunConfiguration.plane_seg_test2 - 3768 - false - true - false - false - true - - 3 - - - - ProjectExplorer.Project.TargetCount - 1 - - - ProjectExplorer.Project.Updater.FileVersion - 18 - - - Version - 18 - - diff --git a/plane_seg_ros/CMakeLists.txt.user b/plane_seg_ros/CMakeLists.txt.user deleted file mode 100644 index 8637735..0000000 --- a/plane_seg_ros/CMakeLists.txt.user +++ /dev/null @@ -1,199 +0,0 @@ - - - - - - EnvironmentId - {7ac00433-0568-4ce0-bd62-1a22dfacb6f0} - - - ProjectExplorer.Project.ActiveTarget - 0 - - - ProjectExplorer.Project.EditorSettings - - true - false - true - - Cpp - - CppGlobal - - - - QmlJS - - QmlJSGlobal - - - 2 - UTF-8 - false - 4 - false - 80 - true - true - 1 - true - false - 0 - true - true - 1 - 8 - true - 1 - true - true - true - false - - - - ProjectExplorer.Project.PluginSettings - - - - ProjectExplorer.Project.Target.0 - - Desktop - Desktop - {37551fef-a8d4-4681-9dee-ef96a26fa7a7} - 0 - 0 - 0 - - - CMAKE_CXX_COMPILER:FILEPATH=/usr/bin/c++ - CMAKE_C_COMPILER:FILEPATH=/usr/bin/cc - - /home/christos/catkin_ws/src/plane_seg/plane_seg_ros/build - - - - - all - - true - CMake Build - - CMakeProjectManager.MakeStep - - 1 - Build - - ProjectExplorer.BuildSteps.Build - - - - - - all - - true - CMake Build - - CMakeProjectManager.MakeStep - - 1 - Clean - - ProjectExplorer.BuildSteps.Clean - - 2 - false - - Default - Default - CMakeProjectManager.CMakeBuildConfiguration - - 1 - - - 0 - Deploy - - ProjectExplorer.BuildSteps.Deploy - - 1 - Deploy Configuration - - ProjectExplorer.DefaultDeployConfiguration - - 1 - - - false - false - 1000 - - true - - false - false - false - false - true - 0.01 - 10 - true - 1 - 25 - - 1 - true - false - true - valgrind - - 0 - 1 - 2 - 3 - 4 - 5 - 6 - 7 - 8 - 9 - 10 - 11 - 12 - 13 - 14 - - plane_seg_ros_node - - - /home/christos/catkin_ws/src/plane_seg/plane_seg_ros/build/devel/lib/plane_seg_ros - 2 - - plane_seg_ros_node - - CMakeProjectManager.CMakeRunConfiguration.plane_seg_ros_node - 3768 - false - true - false - false - true - - 1 - - - - ProjectExplorer.Project.TargetCount - 1 - - - ProjectExplorer.Project.Updater.FileVersion - 18 - - - Version - 18 - - From 7f0c69718fb53341b3a5cab782de3ee9d264568a Mon Sep 17 00:00:00 2001 From: Marco Camurri Date: Tue, 12 Jan 2021 11:55:09 +0000 Subject: [PATCH 13/58] ignore qtcreator project files for the future --- .gitignore | 1 + 1 file changed, 1 insertion(+) create mode 100644 .gitignore diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..01e00f3 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +CMakeLists.txt.user From 857d6a64bd56f9836a3e8122d68ad17ea3aec727 Mon Sep 17 00:00:00 2001 From: Marco Camurri Date: Tue, 12 Jan 2021 12:32:17 +0000 Subject: [PATCH 14/58] fix compilation issues (see comments for details) --- plane_seg/include/plane_seg/Tracker.hpp | 2 +- plane_seg/src/Tracker.cpp | 37 +++++++++++++++---------- 2 files changed, 24 insertions(+), 15 deletions(-) diff --git a/plane_seg/include/plane_seg/Tracker.hpp b/plane_seg/include/plane_seg/Tracker.hpp index 01ed632..576e0c0 100644 --- a/plane_seg/include/plane_seg/Tracker.hpp +++ b/plane_seg/include/plane_seg/Tracker.hpp @@ -30,7 +30,7 @@ class Tracker { void reset(); std::vector::Ptr> convertResult(planeseg::BlockFitter::Result result); void test(planeseg::BlockFitter::Result result); - void printStairs(std::vector centroid_list_); + void printStairs(const std::vector& centroid_list_); private: std::vector newStairs; diff --git a/plane_seg/src/Tracker.cpp b/plane_seg/src/Tracker.cpp index fdca777..18ad8f8 100644 --- a/plane_seg/src/Tracker.cpp +++ b/plane_seg/src/Tracker.cpp @@ -1,12 +1,13 @@ #include "plane_seg/Tracker.hpp" #include -#include > +#include #include #include #include "plane_seg/BlockFitter.hpp" #include "plane_seg/Types.hpp" #include "pcl/common/impl/centroid.hpp" #include +#include using namespace planeseg; @@ -16,10 +17,16 @@ Tracker::Tracker(){ Tracker::~Tracker(){} -pcl::PointXYZ Tracker::find_centroid(pcl::PointCloud::Ptr cloud ){ +pcl::PointXYZ Tracker::find_centroid(pcl::PointCloud::Ptr cloud ){ std::cout << "Computing centroid" << std::endl; + Eigen::Vector4f centroid_eigen; + pcl::compute3DCentroid(*cloud, centroid_eigen); pcl::PointXYZ centroid; - pcl::compute3DCentroid(cloud, centroid); + // NOTE: assuming the first three values of the centroid are the Eucledian + // coordinates of the centroid. The fourth value is discarded. + centroid.x = centroid_eigen(0); + centroid.y = centroid_eigen(1); + centroid.z = centroid_eigen(2); return centroid; } @@ -48,11 +55,11 @@ void Tracker::test(planeseg::BlockFitter::Result result_){ reset(); std::vector::Ptr> cloud_ptrs_; cloud_ptrs = convertResult(result_); - for (int i =0 ; i < cloud_ptrs_.size ; i++){ - newStairs[i] = find_centroid(cloud_ptrs_[i]); + for (size_t i =0 ; i < cloud_ptrs_.size(); i++){ + newStairs[i].centroid = find_centroid(cloud_ptrs_[i]); } printStairs(newStairs); -}; +} int Tracker::get_centroid_id(planeseg::plane plane){ @@ -76,14 +83,14 @@ int Tracker::get_centroid_id(planeseg::plane plane){ std::cout << "Entered get_centroid_id" << std::endl; // go through each plane in oldStairs and compare it to the new plane - for (int i = 0; i < oldStairs.size(); i++){ + for (size_t i = 0; i < oldStairs.size(); i++){ centroid = oldStairs[i].centroid; if (!idAssigned[i].taken && test){ distz = fabs(centroid.z = plane.centroid.z); if (distz < threshold) { - distance = (oldStairs[i] - plane.centroid).norm(); + distance = pcl::euclideanDistance(oldStairs[i].centroid, plane.centroid); if (closest == -1 || distance < closestDist){ closest = i; @@ -109,9 +116,9 @@ int Tracker::get_centroid_id(planeseg::plane plane){ void Tracker::reset(){ oldStairs = newStairs; - newStairs = clear(); + newStairs.clear(); idAssigned.clear(); - idAssigned temp; + IdAssigned temp; temp.taken = false; for (int i=0; i < oldStairs.size(); i++){ temp.id = oldStairs[i].id; @@ -120,11 +127,13 @@ void Tracker::reset(){ } -void printStairs(std::vector stairs){ +void Tracker::printStairs(const std::vector& stairs){ + std::cout << "Centroids of " << stairs.size() << " steps: " << std::endl; for (unsigned i=0; i < stairs.size(); i++){ - for (unsigned j=0; j < stairs[i].size(); j++){ - std::cout << stairs[i][j] << std::endl; - } + std::cout << "Step " << i << " (x,y,z): "; + std::cout << stairs[i].centroid.x << ", "; + std::cout << stairs[i].centroid.y << ", "; + std::cout << stairs[i].centroid.z << std::endl; } } From 493b50da44d7bd59044de2969ef3aebdd0db9d94 Mon Sep 17 00:00:00 2001 From: Christos Ioannou Date: Tue, 12 Jan 2021 15:55:01 +0000 Subject: [PATCH 15/58] Tracker fixes --- plane_seg/src/Tracker.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/plane_seg/src/Tracker.cpp b/plane_seg/src/Tracker.cpp index 18ad8f8..0c99f41 100644 --- a/plane_seg/src/Tracker.cpp +++ b/plane_seg/src/Tracker.cpp @@ -83,6 +83,7 @@ int Tracker::get_centroid_id(planeseg::plane plane){ std::cout << "Entered get_centroid_id" << std::endl; // go through each plane in oldStairs and compare it to the new plane + for (size_t i = 0; i < oldStairs.size(); i++){ centroid = oldStairs[i].centroid; @@ -90,6 +91,7 @@ int Tracker::get_centroid_id(planeseg::plane plane){ if (!idAssigned[i].taken && test){ distz = fabs(centroid.z = plane.centroid.z); if (distz < threshold) { + distance = pcl::euclideanDistance(oldStairs[i].centroid, plane.centroid); if (closest == -1 || distance < closestDist){ From 06ad14604c1bf68575a775d87ce82bd7e543845a Mon Sep 17 00:00:00 2001 From: Christos Ioannou Date: Wed, 13 Jan 2021 16:53:54 +0000 Subject: [PATCH 16/58] completing functions for tracker --- plane_seg/include/plane_seg/Tracker.hpp | 10 +++-- plane_seg/src/Tracker.cpp | 42 ++++++++++++++----- .../include/plane_seg_ros/Visualizer.hpp | 5 ++- plane_seg_ros/src/Visualizer.cpp | 25 +++++++---- plane_seg_ros/src/plane_seg_ros.cpp | 8 +++- 5 files changed, 65 insertions(+), 25 deletions(-) diff --git a/plane_seg/include/plane_seg/Tracker.hpp b/plane_seg/include/plane_seg/Tracker.hpp index 576e0c0..2fabf71 100644 --- a/plane_seg/include/plane_seg/Tracker.hpp +++ b/plane_seg/include/plane_seg/Tracker.hpp @@ -25,18 +25,22 @@ class Tracker { Tracker(); ~Tracker(); - int get_centroid_id(plane plane_); + int get_centroid_id(planeseg::plane plane_); pcl::PointXYZ find_centroid(pcl::PointCloud::Ptr cloud ); void reset(); - std::vector::Ptr> convertResult(planeseg::BlockFitter::Result result); + std::vector convertResult(planeseg::BlockFitter::Result result); + std::vector planesToIds(); void test(planeseg::BlockFitter::Result result); - void printStairs(const std::vector& centroid_list_); + void printStairs(std::vector centroid_list_); + void printIds(); private: std::vector newStairs; std::vector oldStairs; std::vector idAssigned; std::vector::Ptr> cloud_ptrs; + std::vector vector_of_planes; + std::vector vector_of_ids; std::vector icentroid_list_init; int totalIds; }; diff --git a/plane_seg/src/Tracker.cpp b/plane_seg/src/Tracker.cpp index 0c99f41..8d17fa9 100644 --- a/plane_seg/src/Tracker.cpp +++ b/plane_seg/src/Tracker.cpp @@ -30,7 +30,7 @@ pcl::PointXYZ Tracker::find_centroid(pcl::PointCloud::Ptr cloud ) return centroid; } -std::vector::Ptr> Tracker::convertResult(planeseg::BlockFitter::Result result_){ +std::vector Tracker::convertResult(planeseg::BlockFitter::Result result_){ for (size_t i=0; i cloud; const auto& block = result_.mBlocks[i]; @@ -43,19 +43,35 @@ std::vector::Ptr> Tracker::convertResult(planeseg } cloud.height = cloud.points.size(); cloud.width = 1; - pcl::PointCloud::Ptr cloud_ptr; - cloud_ptr = cloud.makeShared(); - cloud_ptrs.push_back(cloud_ptr); + // pcl::PointCloud::Ptr cloud_ptr; + // cloud_ptr = cloud.makeShared(); + // cloud_ptrs.push_back(cloud_ptr); + planeseg::plane plane_no_id; + plane_no_id.cloud.push_back(cloud); + vector_of_planes.push_back(plane_no_id); + + } + + return vector_of_planes; +} + +// *** NEXT STEP: create std::vector = planesToIds(std::vector) which goes through each plane in the vector and calls get_centroid_id and assigns it to the plane +std::vector Tracker::planesToIds(){ + for (size_t i = 0; i < vector_of_planes.size(); ++i){ + int current_id; + current_id = Tracker::get_centroid_id(vector_of_planes[i]); + vector_of_ids.push_back(current_id); } - return cloud_ptrs; + return vector_of_ids; } + // First test is to just calculate and print centroids void Tracker::test(planeseg::BlockFitter::Result result_){ reset(); std::vector::Ptr> cloud_ptrs_; cloud_ptrs = convertResult(result_); - for (size_t i =0 ; i < cloud_ptrs_.size(); i++){ + for (size_t i =0 ; i < cloud_ptrs_.size(); ++i){ newStairs[i].centroid = find_centroid(cloud_ptrs_[i]); } printStairs(newStairs); @@ -84,7 +100,7 @@ int Tracker::get_centroid_id(planeseg::plane plane){ // go through each plane in oldStairs and compare it to the new plane - for (size_t i = 0; i < oldStairs.size(); i++){ + for (size_t i = 0; i < oldStairs.size(); ++i){ centroid = oldStairs[i].centroid; @@ -122,16 +138,16 @@ void Tracker::reset(){ idAssigned.clear(); IdAssigned temp; temp.taken = false; - for (int i=0; i < oldStairs.size(); i++){ + for (int i=0; i < oldStairs.size(); ++i){ temp.id = oldStairs[i].id; idAssigned.push_back(temp); } } -void Tracker::printStairs(const std::vector& stairs){ +void Tracker::printStairs(std::vector stairs){ std::cout << "Centroids of " << stairs.size() << " steps: " << std::endl; - for (unsigned i=0; i < stairs.size(); i++){ + for (unsigned i = 0; i < stairs.size(); ++i){ std::cout << "Step " << i << " (x,y,z): "; std::cout << stairs[i].centroid.x << ", "; std::cout << stairs[i].centroid.y << ", "; @@ -139,3 +155,9 @@ void Tracker::printStairs(const std::vector& stairs){ } } +void Tracker::printIds(){ + std::cout << "Total number of ids assigned: " << vector_of_ids_.size() << std::endl; + for (size_t i = 0; i < vector_of_ids.size(); ++i){ + std::cout << vector_of_ids[i] << std::endl; + } +} diff --git a/plane_seg_ros/include/plane_seg_ros/Visualizer.hpp b/plane_seg_ros/include/plane_seg_ros/Visualizer.hpp index ca206ef..b1a7c4e 100644 --- a/plane_seg_ros/include/plane_seg_ros/Visualizer.hpp +++ b/plane_seg_ros/include/plane_seg_ros/Visualizer.hpp @@ -1,5 +1,6 @@ #ifndef _planeseg_Visualizer_hpp_ #define _planeseg_Visualizer_hpp_ +#endif #include #include @@ -10,7 +11,7 @@ struct line_strip { visualization_msgs::Marker centroidsMarker; int id; -} +}; class Visualizer{ public: @@ -27,4 +28,4 @@ class Visualizer{ unsigned getR(int id); unsigned getG(int id); unsigned getB(int id); -} +}; diff --git a/plane_seg_ros/src/Visualizer.cpp b/plane_seg_ros/src/Visualizer.cpp index 502945d..42f6841 100644 --- a/plane_seg_ros/src/Visualizer.cpp +++ b/plane_seg_ros/src/Visualizer.cpp @@ -9,10 +9,11 @@ #include #include +#include -Visualizer::Visualizer(ros::Publisher centroids_pub, ros:: Publisher linestrip_pub): - centroids_pub_(centroids_pub), - linestrip_pub_(linestrip_pub) +Visualizer::Visualizer(ros::Publisher centroids_pub, ros:: Publisher linestrip_pub)//: + // centroids_pub_(centroids_pub), + // linestrip_pub_(linestrip_pub) { colors_ = { 51/255.0, 160/255.0, 44/255.0, //0 @@ -83,6 +84,12 @@ visualization_msgs::Marker Visualizer::displayLineStrip(int id, pcl::PointXYZ ne bool point_added; point_added = false; + // convert newCentroid to geometry_msgs/Point + geometry_msgs::Point newCentroidgm; + newCentroidgm.x = newCentroid.x; + newCentroidgm.y = newCentroid.y; + newCentroidgm.z = newCentroid.z; + // create visualization markers visualization_msgs::Marker centroidMarker; visualization_msgs::Marker lineStripMarker; @@ -97,12 +104,12 @@ visualization_msgs::Marker Visualizer::displayLineStrip(int id, pcl::PointXYZ ne // first we must find out if the newCentroid should be added to an already existing linestrip for (unsigned i = 0; i < lineStrips.size() && point_added == false; i++){ if (id == lineStrips[i].id){ - lineStrips[i].centroidsMarker.points.push_back(newCentroid); + lineStrips[i].centroidsMarker.points.push_back(newCentroidgm); centroidMarker = lineStrips[i].centroidsMarker; centroidMarker.header.frame_id = "odom"; centroidMarker.header.stamp = ros::Time::now(); centroidMarker.ns = "linestrips"; - centroidMarker.id = centroid_id; + centroidMarker.id = id; // declare outgoing marker as the linestrip with matching ID lineStripMarker = centroidMarker; @@ -117,7 +124,7 @@ visualization_msgs::Marker Visualizer::displayLineStrip(int id, pcl::PointXYZ ne newPlaneMarker.header.frame_id = "odom"; newPlaneMarker.header.stamp = ros::Time::now(); newPlaneMarker.ns = "linestrips"; - newPlaneMarker.id = centroid_id; + newPlaneMarker.id = id; newPlaneMarker.scale.x = 0.1; newPlaneMarker.scale.y = 0.1; newPlaneMarker.color.r = getR(id); @@ -125,7 +132,7 @@ visualization_msgs::Marker Visualizer::displayLineStrip(int id, pcl::PointXYZ ne newPlaneMarker.color.b = getB(id); newPlaneMarker.color.a = 1; newPlaneMarker.pose.orientation.w = 1.0; - newPlaneMarker.points.push_back(newCentroid); + newPlaneMarker.points.push_back(newCentroidgm); // create a new linestrip for the new ID centroidMarker.type = visualization_msgs::Marker::LINE_STRIP; @@ -135,7 +142,7 @@ visualization_msgs::Marker Visualizer::displayLineStrip(int id, pcl::PointXYZ ne centroidMarker.color.b = getB(id); centroidMarker.color.a = 1; centroidMarker.pose.orientation.w = 1.0; - centroidMarker.points.push_back(newCentroid); + centroidMarker.points.push_back(newCentroidgm); line_strip newLineStrip; newLineStrip.centroidsMarker = centroidMarker; newLineStrip.id = id; @@ -165,6 +172,8 @@ visualization_msgs::Marker Visualizer::displayString(int id, std::string string, stringMarker.color.g = 1; stringMarker.color.b = 1; stringMarker.text = string; + + return stringMarker; } unsigned Visualizer::getR(int id){ diff --git a/plane_seg_ros/src/plane_seg_ros.cpp b/plane_seg_ros/src/plane_seg_ros.cpp index d387399..c53e8f0 100644 --- a/plane_seg_ros/src/plane_seg_ros.cpp +++ b/plane_seg_ros/src/plane_seg_ros.cpp @@ -55,7 +55,7 @@ Pass::Pass(ros::NodeHandle node_): last_robot_pose_ = Eigen::Isometry3d::Identity(); - tracking_ = new planeseg::Tracker(); + tracking_ = planeseg::Tracker(); colors_ = { 51/255.0, 160/255.0, 44/255.0, //0 @@ -237,7 +237,11 @@ void Pass::processCloud(planeseg::LabeledCloud::Ptr& inCloud, Eigen::Vector3f or result_ = fitter.go(); tracking_.reset(); - tracking_.test(result_); + std::vector vector_of_planes_; + vector_of_planes_ = tracking_.convertResult(result_); + std::vector vector_of_ids_; + vector_of_ids_ = tracking_.planesToIds(); + tracking_.printIds(); Eigen::Vector3f rz = lookDir; Eigen::Vector3f rx = rz.cross(Eigen::Vector3f::UnitZ()); From c6491b7cf66ee8f3c93e377d746858bbf7390817 Mon Sep 17 00:00:00 2001 From: Christos Ioannou Date: Wed, 13 Jan 2021 17:38:11 +0000 Subject: [PATCH 17/58] displayString to change id to string within function --- plane_seg_ros/include/plane_seg_ros/Visualizer.hpp | 2 +- plane_seg_ros/src/Visualizer.cpp | 6 ++++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/plane_seg_ros/include/plane_seg_ros/Visualizer.hpp b/plane_seg_ros/include/plane_seg_ros/Visualizer.hpp index b1a7c4e..5e3f741 100644 --- a/plane_seg_ros/include/plane_seg_ros/Visualizer.hpp +++ b/plane_seg_ros/include/plane_seg_ros/Visualizer.hpp @@ -21,7 +21,7 @@ class Visualizer{ private: sensor_msgs::PointCloud2 displayCentroids(std::vector &planes); - visualization_msgs::Marker displayString(int id, std::string string_, geometry_msgs::Point point_); + visualization_msgs::Marker displayString(int id, geometry_msgs::Point point_); visualization_msgs::Marker displayLineStrip(int id, pcl::PointXYZ newCentroid); std::vector colors_; std::vector lineStrips; diff --git a/plane_seg_ros/src/Visualizer.cpp b/plane_seg_ros/src/Visualizer.cpp index 42f6841..267b1db 100644 --- a/plane_seg_ros/src/Visualizer.cpp +++ b/plane_seg_ros/src/Visualizer.cpp @@ -155,7 +155,7 @@ visualization_msgs::Marker Visualizer::displayLineStrip(int id, pcl::PointXYZ ne return lineStripMarker; } -visualization_msgs::Marker Visualizer::displayString(int id, std::string string, geometry_msgs::Point point){ +visualization_msgs::Marker Visualizer::displayString(int id, geometry_msgs::Point point){ visualization_msgs::Marker stringMarker; stringMarker.header.frame_id = "odom"; stringMarker.header.stamp = ros::Time(); @@ -171,7 +171,9 @@ visualization_msgs::Marker Visualizer::displayString(int id, std::string string, stringMarker.color.r = 1; stringMarker.color.g = 1; stringMarker.color.b = 1; - stringMarker.text = string; + + std::string id_string = std::to_string(id); + stringMarker.text = id_string; return stringMarker; } From 80d93c62275003ae33cbc0831796c678ca4446d3 Mon Sep 17 00:00:00 2001 From: Christos Ioannou Date: Wed, 13 Jan 2021 18:06:04 +0000 Subject: [PATCH 18/58] fixed printIds --- plane_seg/src/Tracker.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/plane_seg/src/Tracker.cpp b/plane_seg/src/Tracker.cpp index 8d17fa9..a408963 100644 --- a/plane_seg/src/Tracker.cpp +++ b/plane_seg/src/Tracker.cpp @@ -156,7 +156,7 @@ void Tracker::printStairs(std::vector stairs){ } void Tracker::printIds(){ - std::cout << "Total number of ids assigned: " << vector_of_ids_.size() << std::endl; + std::cout << "Total number of ids assigned: " << vector_of_ids.size() << std::endl; for (size_t i = 0; i < vector_of_ids.size(); ++i){ std::cout << vector_of_ids[i] << std::endl; } From bdc0d4bce1d8cfa5eb97e30598e3a1a3424e1da3 Mon Sep 17 00:00:00 2001 From: Christos Ioannou Date: Wed, 13 Jan 2021 18:27:58 +0000 Subject: [PATCH 19/58] remove test() from Tracker --- plane_seg/src/Tracker.cpp | 12 ------------ 1 file changed, 12 deletions(-) diff --git a/plane_seg/src/Tracker.cpp b/plane_seg/src/Tracker.cpp index a408963..903f79b 100644 --- a/plane_seg/src/Tracker.cpp +++ b/plane_seg/src/Tracker.cpp @@ -66,18 +66,6 @@ std::vector Tracker::planesToIds(){ } -// First test is to just calculate and print centroids -void Tracker::test(planeseg::BlockFitter::Result result_){ - reset(); - std::vector::Ptr> cloud_ptrs_; - cloud_ptrs = convertResult(result_); - for (size_t i =0 ; i < cloud_ptrs_.size(); ++i){ - newStairs[i].centroid = find_centroid(cloud_ptrs_[i]); - } - printStairs(newStairs); -} - - int Tracker::get_centroid_id(planeseg::plane plane){ bool test = true; From ccfcac962be41ee410308814219d423de01867a3 Mon Sep 17 00:00:00 2001 From: Christos Ioannou Date: Wed, 13 Jan 2021 20:19:52 +0000 Subject: [PATCH 20/58] print entrance lines to trace code --- plane_seg/src/Tracker.cpp | 16 +++++++--- plane_seg_ros/src/plane_seg_ros.cpp | 40 ++++++------------------ plane_seg_ros/src/plane_seg_ros_node.cpp | 2 +- 3 files changed, 22 insertions(+), 36 deletions(-) diff --git a/plane_seg/src/Tracker.cpp b/plane_seg/src/Tracker.cpp index 903f79b..2701b1f 100644 --- a/plane_seg/src/Tracker.cpp +++ b/plane_seg/src/Tracker.cpp @@ -9,7 +9,7 @@ #include #include -using namespace planeseg; +namespace planeseg { Tracker::Tracker(){ totalIds = 1; @@ -31,6 +31,7 @@ pcl::PointXYZ Tracker::find_centroid(pcl::PointCloud::Ptr cloud ) } std::vector Tracker::convertResult(planeseg::BlockFitter::Result result_){ + std::cout << "entered convertResult" << std::endl; for (size_t i=0; i cloud; const auto& block = result_.mBlocks[i]; @@ -47,7 +48,7 @@ std::vector Tracker::convertResult(planeseg::BlockFitter::Resul // cloud_ptr = cloud.makeShared(); // cloud_ptrs.push_back(cloud_ptr); planeseg::plane plane_no_id; - plane_no_id.cloud.push_back(cloud); + plane_no_id.cloud = cloud; vector_of_planes.push_back(plane_no_id); } @@ -56,7 +57,9 @@ std::vector Tracker::convertResult(planeseg::BlockFitter::Resul } // *** NEXT STEP: create std::vector = planesToIds(std::vector) which goes through each plane in the vector and calls get_centroid_id and assigns it to the plane + std::vector Tracker::planesToIds(){ + std::cout << "entered planesToIds" << std::endl; for (size_t i = 0; i < vector_of_planes.size(); ++i){ int current_id; current_id = Tracker::get_centroid_id(vector_of_planes[i]); @@ -68,6 +71,7 @@ std::vector Tracker::planesToIds(){ int Tracker::get_centroid_id(planeseg::plane plane){ + std::cout << "entered get_centroid_id" << std::endl; bool test = true; int id; @@ -84,8 +88,6 @@ int Tracker::get_centroid_id(planeseg::plane plane){ int closest = -1; double closestDist = 0; - std::cout << "Entered get_centroid_id" << std::endl; - // go through each plane in oldStairs and compare it to the new plane for (size_t i = 0; i < oldStairs.size(); ++i){ @@ -121,12 +123,13 @@ int Tracker::get_centroid_id(planeseg::plane plane){ } void Tracker::reset(){ + std::cout << "entered Tracker reset" << std::endl; oldStairs = newStairs; newStairs.clear(); idAssigned.clear(); IdAssigned temp; temp.taken = false; - for (int i=0; i < oldStairs.size(); ++i){ + for (size_t i=0; i < oldStairs.size(); ++i){ temp.id = oldStairs[i].id; idAssigned.push_back(temp); } @@ -144,8 +147,11 @@ void Tracker::printStairs(std::vector stairs){ } void Tracker::printIds(){ + std::cout << "entered printIds" << std::endl; std::cout << "Total number of ids assigned: " << vector_of_ids.size() << std::endl; for (size_t i = 0; i < vector_of_ids.size(); ++i){ std::cout << vector_of_ids[i] << std::endl; } } + +} // namespace plane_seg diff --git a/plane_seg_ros/src/plane_seg_ros.cpp b/plane_seg_ros/src/plane_seg_ros.cpp index c53e8f0..ba973b2 100644 --- a/plane_seg_ros/src/plane_seg_ros.cpp +++ b/plane_seg_ros/src/plane_seg_ros.cpp @@ -25,6 +25,7 @@ #include #include +#include "plane_seg/BlockFitter.hpp" #include "plane_seg/Tracker.hpp" #define foreach BOOST_FOREACH @@ -213,7 +214,7 @@ void Pass::stepThroughFile(std::string filename){ geometry_msgs::PoseWithCovarianceStamped::ConstPtr i = m.instantiate(); if (i !=NULL){ - std::cout << "position (x, y, z): " << i->pose.pose.position.x << ", " << i->pose.pose.position.y << ", " << i->pose.pose.position.z << std::endl; + // std::cout << "position (x, y, z): " << i->pose.pose.position.x << ", " << i->pose.pose.position.y << ", " << i->pose.pose.position.z << std::endl; robotPoseCallBack(i); pose_pub_.publish(*i); } @@ -225,6 +226,7 @@ void Pass::stepThroughFile(std::string filename){ void Pass::processCloud(planeseg::LabeledCloud::Ptr& inCloud, Eigen::Vector3f origin, Eigen::Vector3f lookDir){ + std::cout << "entered processCloud" << std::endl; planeseg::BlockFitter fitter; fitter.setSensorPose(origin, lookDir); fitter.setCloud(inCloud); @@ -236,11 +238,13 @@ void Pass::processCloud(planeseg::LabeledCloud::Ptr& inCloud, Eigen::Vector3f or fitter.setMaxAngleOfPlaneSegmenter(10); result_ = fitter.go(); + std::cout << "got result" << std::endl; tracking_.reset(); - std::vector vector_of_planes_; - vector_of_planes_ = tracking_.convertResult(result_); - std::vector vector_of_ids_; - vector_of_ids_ = tracking_.planesToIds(); + std::cout << "reset tracker" << std::endl; + tracking_.convertResult(result_); + std::cout << "converted result to planes" << std::endl; + tracking_.planesToIds(); + std::cout << "converted planes to ids" << std::endl; tracking_.printIds(); Eigen::Vector3f rz = lookDir; @@ -306,19 +310,6 @@ void Pass::printResultAsJson(){ void Pass::publishResult(){ // convert result to a vector of point clouds std::vector< pcl::PointCloud::Ptr > cloud_ptrs; -/* std::vector centroid_list; - std::vector old_centroid_list; - - if (old_centroid_list.empty()){ - Eigen::Vector4f old_centroid_list_init; - old_centroid_list_init << 0, - 0, - 0, - 1; - old_centroid_list.push_back(old_centroid_list_init); - } -// printCentroidList(old_centroid_list); -*/ for (size_t i=0; i cloud; const auto& block = result_.mBlocks[i]; @@ -331,31 +322,20 @@ void Pass::publishResult(){ } cloud.height = cloud.points.size(); cloud.width = 1; -// Eigen::Vector4f centroid; -// pcl::compute3DCentroid(cloud, centroid); -// centroid_list.push_back(centroid); pcl::PointCloud::Ptr cloud_ptr; cloud_ptr = cloud.makeShared(); cloud_ptrs.push_back(cloud_ptr); } -/* std::cout << "Centroid list: " << std::endl; - printCentroidList(centroid_list); - compareCentroidLists(old_centroid_list, centroid_list); - std::cout << centroid_list[0] << std::endl; -*/ publishHullsAsCloud(cloud_ptrs, 0, 0); publishHullsAsMarkers(cloud_ptrs, 0, 0); -// old_centroid_list = centroid_list; - //pcl::PCDWriter pcd_writer_; - //cd_writer_.write ("/home/mfallon/out.pcd", cloud, false); + //pcd_writer_.write ("/home/mfallon/out.pcd", cloud, false); //std::cout << "blocks: " << result_.mBlocks.size() << " blocks\n"; //std::cout << "cloud: " << cloud.points.size() << " pts\n"; } - // combine the individual clouds into one, with a different each void Pass::publishHullsAsCloud(std::vector< pcl::PointCloud::Ptr > cloud_ptrs, int secs, int nsecs){ diff --git a/plane_seg_ros/src/plane_seg_ros_node.cpp b/plane_seg_ros/src/plane_seg_ros_node.cpp index 36af384..0a11d26 100644 --- a/plane_seg_ros/src/plane_seg_ros_node.cpp +++ b/plane_seg_ros/src/plane_seg_ros_node.cpp @@ -9,7 +9,7 @@ using namespace planeseg; int main( int argc, char** argv ){ // Turn off warning message about labels // TODO: look into how labels are used - pcl::console::setVerbosityLevel(pcl::console::L_ALWAYS); + //pcl::console::setVerbosityLevel(pcl::console::L_ALWAYS); ros::init(argc, argv, "plane_seg"); From d7a7a45dcff2fc24aa8ed3623cb65b434f2bc383 Mon Sep 17 00:00:00 2001 From: Christos Ioannou Date: Thu, 14 Jan 2021 16:36:38 +0000 Subject: [PATCH 21/58] debugging --- plane_seg/include/plane_seg/Tracker.hpp | 4 ++-- plane_seg/src/Tracker.cpp | 20 ++++++++++++-------- plane_seg_ros/CMakeLists.txt | 3 ++- plane_seg_ros/include/plane_seg_ros/Pass.hpp | 2 +- plane_seg_ros/src/Visualizer.cpp | 1 + plane_seg_ros/src/plane_seg_ros.cpp | 10 +++++----- plane_seg_ros/src/plane_seg_ros_node.cpp | 2 +- 7 files changed, 24 insertions(+), 18 deletions(-) diff --git a/plane_seg/include/plane_seg/Tracker.hpp b/plane_seg/include/plane_seg/Tracker.hpp index 2fabf71..4ade478 100644 --- a/plane_seg/include/plane_seg/Tracker.hpp +++ b/plane_seg/include/plane_seg/Tracker.hpp @@ -25,8 +25,8 @@ class Tracker { Tracker(); ~Tracker(); - int get_centroid_id(planeseg::plane plane_); - pcl::PointXYZ find_centroid(pcl::PointCloud::Ptr cloud ); + int get_plane_id(planeseg::plane plane_); + pcl::PointXYZ find_centroid(pcl::PointCloud cloud ); void reset(); std::vector convertResult(planeseg::BlockFitter::Result result); std::vector planesToIds(); diff --git a/plane_seg/src/Tracker.cpp b/plane_seg/src/Tracker.cpp index 2701b1f..1f7091b 100644 --- a/plane_seg/src/Tracker.cpp +++ b/plane_seg/src/Tracker.cpp @@ -17,10 +17,10 @@ Tracker::Tracker(){ Tracker::~Tracker(){} -pcl::PointXYZ Tracker::find_centroid(pcl::PointCloud::Ptr cloud ){ +pcl::PointXYZ Tracker::find_centroid(pcl::PointCloud cloud ){ std::cout << "Computing centroid" << std::endl; Eigen::Vector4f centroid_eigen; - pcl::compute3DCentroid(*cloud, centroid_eigen); + pcl::compute3DCentroid(cloud, centroid_eigen); pcl::PointXYZ centroid; // NOTE: assuming the first three values of the centroid are the Eucledian // coordinates of the centroid. The fourth value is discarded. @@ -44,34 +44,38 @@ std::vector Tracker::convertResult(planeseg::BlockFitter::Resul } cloud.height = cloud.points.size(); cloud.width = 1; + + pcl::PointXYZ cloud_centroid; + cloud_centroid = find_centroid(cloud); + // pcl::PointCloud::Ptr cloud_ptr; // cloud_ptr = cloud.makeShared(); // cloud_ptrs.push_back(cloud_ptr); planeseg::plane plane_no_id; plane_no_id.cloud = cloud; - vector_of_planes.push_back(plane_no_id); + plane_no_id.centroid = cloud_centroid; + vector_of_planes.push_back(plane_no_id); } return vector_of_planes; } -// *** NEXT STEP: create std::vector = planesToIds(std::vector) which goes through each plane in the vector and calls get_centroid_id and assigns it to the plane +// *** NEXT STEP: create std::vector = planesToIds(std::vector) which goes through each plane in the vector and calls get_plane_id and assigns it to the plane std::vector Tracker::planesToIds(){ std::cout << "entered planesToIds" << std::endl; for (size_t i = 0; i < vector_of_planes.size(); ++i){ int current_id; - current_id = Tracker::get_centroid_id(vector_of_planes[i]); + current_id = Tracker::get_plane_id(vector_of_planes[i]); vector_of_ids.push_back(current_id); } return vector_of_ids; } +int Tracker::get_plane_id(planeseg::plane plane){ -int Tracker::get_centroid_id(planeseg::plane plane){ - - std::cout << "entered get_centroid_id" << std::endl; + std::cout << "entered get_plane_id" << std::endl; bool test = true; int id; diff --git a/plane_seg_ros/CMakeLists.txt b/plane_seg_ros/CMakeLists.txt index 2367f0e..3391bcd 100644 --- a/plane_seg_ros/CMakeLists.txt +++ b/plane_seg_ros/CMakeLists.txt @@ -37,7 +37,8 @@ include_directories( add_library(${PROJECT_NAME} SHARED src/${PROJECT_NAME}.cpp src/geometry_utils.cpp - src/Visualizer.cpp) + src/Visualizer.cpp + ) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) diff --git a/plane_seg_ros/include/plane_seg_ros/Pass.hpp b/plane_seg_ros/include/plane_seg_ros/Pass.hpp index cb05469..66528d4 100644 --- a/plane_seg_ros/include/plane_seg_ros/Pass.hpp +++ b/plane_seg_ros/include/plane_seg_ros/Pass.hpp @@ -10,7 +10,7 @@ #include #include #include -#include +//#include namespace planeseg { diff --git a/plane_seg_ros/src/Visualizer.cpp b/plane_seg_ros/src/Visualizer.cpp index 267b1db..439c371 100644 --- a/plane_seg_ros/src/Visualizer.cpp +++ b/plane_seg_ros/src/Visualizer.cpp @@ -11,6 +11,7 @@ #include #include + Visualizer::Visualizer(ros::Publisher centroids_pub, ros:: Publisher linestrip_pub)//: // centroids_pub_(centroids_pub), // linestrip_pub_(linestrip_pub) diff --git a/plane_seg_ros/src/plane_seg_ros.cpp b/plane_seg_ros/src/plane_seg_ros.cpp index ba973b2..3258e40 100644 --- a/plane_seg_ros/src/plane_seg_ros.cpp +++ b/plane_seg_ros/src/plane_seg_ros.cpp @@ -56,7 +56,7 @@ Pass::Pass(ros::NodeHandle node_): last_robot_pose_ = Eigen::Isometry3d::Identity(); - tracking_ = planeseg::Tracker(); +// tracking_ = planeseg::Tracker(); colors_ = { 51/255.0, 160/255.0, 44/255.0, //0 @@ -239,13 +239,13 @@ void Pass::processCloud(planeseg::LabeledCloud::Ptr& inCloud, Eigen::Vector3f or result_ = fitter.go(); std::cout << "got result" << std::endl; - tracking_.reset(); +// tracking_.reset(); std::cout << "reset tracker" << std::endl; - tracking_.convertResult(result_); +// tracking_.convertResult(result_); std::cout << "converted result to planes" << std::endl; - tracking_.planesToIds(); +// tracking_.planesToIds(); std::cout << "converted planes to ids" << std::endl; - tracking_.printIds(); +// tracking_.printIds(); Eigen::Vector3f rz = lookDir; Eigen::Vector3f rx = rz.cross(Eigen::Vector3f::UnitZ()); diff --git a/plane_seg_ros/src/plane_seg_ros_node.cpp b/plane_seg_ros/src/plane_seg_ros_node.cpp index 0a11d26..36af384 100644 --- a/plane_seg_ros/src/plane_seg_ros_node.cpp +++ b/plane_seg_ros/src/plane_seg_ros_node.cpp @@ -9,7 +9,7 @@ using namespace planeseg; int main( int argc, char** argv ){ // Turn off warning message about labels // TODO: look into how labels are used - //pcl::console::setVerbosityLevel(pcl::console::L_ALWAYS); + pcl::console::setVerbosityLevel(pcl::console::L_ALWAYS); ros::init(argc, argv, "plane_seg"); From f049778b93e8ea683e2cb7c96d224e8422dab0a3 Mon Sep 17 00:00:00 2001 From: Christos Ioannou Date: Thu, 14 Jan 2021 23:19:49 +0000 Subject: [PATCH 22/58] Tracker working - show ids as numbers --- plane_seg/include/plane_seg/Tracker.hpp | 7 +- plane_seg/src/Tracker.cpp | 68 +++++++++++-------- plane_seg_ros/include/plane_seg_ros/Pass.hpp | 7 +- .../include/plane_seg_ros/Visualizer.hpp | 13 ++-- plane_seg_ros/src/Visualizer.cpp | 35 ++++++---- plane_seg_ros/src/plane_seg_ros.cpp | 51 ++++++-------- 6 files changed, 102 insertions(+), 79 deletions(-) diff --git a/plane_seg/include/plane_seg/Tracker.hpp b/plane_seg/include/plane_seg/Tracker.hpp index 4ade478..fa27bde 100644 --- a/plane_seg/include/plane_seg/Tracker.hpp +++ b/plane_seg/include/plane_seg/Tracker.hpp @@ -28,19 +28,22 @@ class Tracker { int get_plane_id(planeseg::plane plane_); pcl::PointXYZ find_centroid(pcl::PointCloud cloud ); void reset(); - std::vector convertResult(planeseg::BlockFitter::Result result); + void convertResult(planeseg::BlockFitter::Result result); std::vector planesToIds(); void test(planeseg::BlockFitter::Result result); void printStairs(std::vector centroid_list_); void printIds(); + void printidAssigned(); + std::vector newStairs; private: - std::vector newStairs; + std::vector oldStairs; std::vector idAssigned; std::vector::Ptr> cloud_ptrs; std::vector vector_of_planes; std::vector vector_of_ids; + std::vector vec_planes_no_ids; std::vector icentroid_list_init; int totalIds; }; diff --git a/plane_seg/src/Tracker.cpp b/plane_seg/src/Tracker.cpp index 1f7091b..bea944b 100644 --- a/plane_seg/src/Tracker.cpp +++ b/plane_seg/src/Tracker.cpp @@ -30,7 +30,8 @@ pcl::PointXYZ Tracker::find_centroid(pcl::PointCloud cloud ){ return centroid; } -std::vector Tracker::convertResult(planeseg::BlockFitter::Result result_){ +// converts the result from the BlockFitter to a vector of planes complete with pointclouds, centroids and ids, and saves it as newStairs +void Tracker::convertResult(planeseg::BlockFitter::Result result_){ std::cout << "entered convertResult" << std::endl; for (size_t i=0; i cloud; @@ -51,14 +52,16 @@ std::vector Tracker::convertResult(planeseg::BlockFitter::Resul // pcl::PointCloud::Ptr cloud_ptr; // cloud_ptr = cloud.makeShared(); // cloud_ptrs.push_back(cloud_ptr); - planeseg::plane plane_no_id; - plane_no_id.cloud = cloud; - plane_no_id.centroid = cloud_centroid; - - vector_of_planes.push_back(plane_no_id); + planeseg::plane newPlane; + newPlane.cloud = cloud; + newPlane.centroid = cloud_centroid; + newPlane.id = get_plane_id(newPlane); + +// std::vector vec_planes_no_ids.push_back(plane_no_id); +// vector_of_planes.push_back(plane_no_id); + newStairs.push_back(newPlane); } - return vector_of_planes; } // *** NEXT STEP: create std::vector = planesToIds(std::vector) which goes through each plane in the vector and calls get_plane_id and assigns it to the plane @@ -75,7 +78,7 @@ std::vector Tracker::planesToIds(){ int Tracker::get_plane_id(planeseg::plane plane){ - std::cout << "entered get_plane_id" << std::endl; +// std::cout << "entered get_plane_id" << std::endl; bool test = true; int id; @@ -85,44 +88,46 @@ int Tracker::get_plane_id(planeseg::plane plane){ } else{ - pcl::PointXYZ centroid; - double threshold = 1; + pcl::PointXYZ oldCentroid; + double threshold = 0.1; double distance; double distz; int closest = -1; - double closestDist = 0; + // start with closestDist being far larger than anything that would ever happen + double closestDist = 1000000000; // go through each plane in oldStairs and compare it to the new plane for (size_t i = 0; i < oldStairs.size(); ++i){ - centroid = oldStairs[i].centroid; - - if (!idAssigned[i].taken && test){ - distz = fabs(centroid.z = plane.centroid.z); - if (distz < threshold) { + // if the id of the ith plane in oldStair exists + // if (!idAssigned[i].taken && test){ + distz = fabs(oldStairs[i].centroid.z - plane.centroid.z); + std::cout << "distz = " << distz << std::endl; + if(distz < threshold){ + // distance = pcl::euclideanDistance(oldStairs[i].centroid, plane.centroid); - distance = pcl::euclideanDistance(oldStairs[i].centroid, plane.centroid); - - if (closest == -1 || distance < closestDist){ + if (distz < closestDist){ closest = i; - closestDist = distance; + closestDist = distz; } } - } + // } } + std::cout << "closestDist = " << closestDist << std::endl; + if (closest != -1){ id = oldStairs[closest].id; - idAssigned[closest].taken = true; +// idAssigned[closest].taken = true; } else{ id = totalIds; ++totalIds; } } - plane.id = id; - newStairs.push_back(plane); +// plane.id = id; +// newStairs.push_back(plane); return id; } @@ -131,6 +136,8 @@ void Tracker::reset(){ oldStairs = newStairs; newStairs.clear(); idAssigned.clear(); + + // set the ids in the vector IdAssigned to be the same as the ids in the planes of oldStairs IdAssigned temp; temp.taken = false; for (size_t i=0; i < oldStairs.size(); ++i){ @@ -152,9 +159,16 @@ void Tracker::printStairs(std::vector stairs){ void Tracker::printIds(){ std::cout << "entered printIds" << std::endl; - std::cout << "Total number of ids assigned: " << vector_of_ids.size() << std::endl; - for (size_t i = 0; i < vector_of_ids.size(); ++i){ - std::cout << vector_of_ids[i] << std::endl; + std::cout << "Total number of ids assigned: " << newStairs.size() << std::endl; + for (size_t i = 0; i < newStairs.size(); ++i){ + std::cout << newStairs[i].id << std::endl; + } +} + +void Tracker::printidAssigned(){ + std::cout << "idAssigned: " << std::endl; + for (size_t i = 0; i < idAssigned.size(); ++i){ + std::cout << idAssigned[i].id << std::endl; } } diff --git a/plane_seg_ros/include/plane_seg_ros/Pass.hpp b/plane_seg_ros/include/plane_seg_ros/Pass.hpp index 66528d4..3a014d8 100644 --- a/plane_seg_ros/include/plane_seg_ros/Pass.hpp +++ b/plane_seg_ros/include/plane_seg_ros/Pass.hpp @@ -10,7 +10,8 @@ #include #include #include -//#include +#include +#include namespace planeseg { @@ -37,16 +38,18 @@ class Pass{ void publishResult(); void printCentroidList(std::vector centroid_list); void compareCentroidLists(std::vector old_centroid_list, std::vector centroid_list); + void publishIdsAsStrings(); private: ros::NodeHandle node_; std::vector colors_; ros::Subscriber point_cloud_sub_, grid_map_sub_, pose_sub_; - ros::Publisher received_cloud_pub_, hull_cloud_pub_, hull_markers_pub_, look_pose_pub_, elev_map_pub_, pose_pub_; + ros::Publisher received_cloud_pub_, hull_cloud_pub_, hull_markers_pub_, look_pose_pub_, elev_map_pub_, pose_pub_, id_strings_pub_; Eigen::Isometry3d last_robot_pose_; planeseg::BlockFitter::Result result_; planeseg::Tracker tracking_; + planeseg::Visualizer visualizer_; }; } diff --git a/plane_seg_ros/include/plane_seg_ros/Visualizer.hpp b/plane_seg_ros/include/plane_seg_ros/Visualizer.hpp index 5e3f741..c52c427 100644 --- a/plane_seg_ros/include/plane_seg_ros/Visualizer.hpp +++ b/plane_seg_ros/include/plane_seg_ros/Visualizer.hpp @@ -8,24 +8,29 @@ #include #include "plane_seg/Tracker.hpp" +namespace planeseg{ + struct line_strip { visualization_msgs::Marker centroidsMarker; int id; }; -class Visualizer{ +class Visualizer { public: Visualizer(); - explicit Visualizer(ros::Publisher centroids_pub, ros::Publisher linestrip_pub); ~Visualizer(); -private: sensor_msgs::PointCloud2 displayCentroids(std::vector &planes); - visualization_msgs::Marker displayString(int id, geometry_msgs::Point point_); + visualization_msgs::Marker displayString(int id, pcl::PointXYZ point_); visualization_msgs::Marker displayLineStrip(int id, pcl::PointXYZ newCentroid); + +private: + std::vector colors_; std::vector lineStrips; unsigned getR(int id); unsigned getG(int id); unsigned getB(int id); }; + +} // namespace planeseg diff --git a/plane_seg_ros/src/Visualizer.cpp b/plane_seg_ros/src/Visualizer.cpp index 439c371..9e5c14c 100644 --- a/plane_seg_ros/src/Visualizer.cpp +++ b/plane_seg_ros/src/Visualizer.cpp @@ -6,16 +6,15 @@ #include #include #include "plane_seg/Tracker.hpp" +#include #include #include #include +namespace planeseg { -Visualizer::Visualizer(ros::Publisher centroids_pub, ros:: Publisher linestrip_pub)//: - // centroids_pub_(centroids_pub), - // linestrip_pub_(linestrip_pub) -{ +Visualizer::Visualizer(){ colors_ = { 51/255.0, 160/255.0, 44/255.0, //0 166/255.0, 206/255.0, 227/255.0, @@ -48,6 +47,8 @@ Visualizer::Visualizer(ros::Publisher centroids_pub, ros:: Publisher linestrip_p } +Visualizer::~Visualizer(){} + /* NOT FINISHED YET void Visualizer::publish(std::vector &planes){ @@ -156,7 +157,14 @@ visualization_msgs::Marker Visualizer::displayLineStrip(int id, pcl::PointXYZ ne return lineStripMarker; } -visualization_msgs::Marker Visualizer::displayString(int id, geometry_msgs::Point point){ +visualization_msgs::Marker Visualizer::displayString(int id, pcl::PointXYZ point){ + + // convert pcl::PointXYZ into geometry_msgs::Point + geometry_msgs::Point pointGM; + pointGM.x = point.x; + pointGM.y = point.y; + pointGM.z = point.z; + visualization_msgs::Marker stringMarker; stringMarker.header.frame_id = "odom"; stringMarker.header.stamp = ros::Time(); @@ -164,14 +172,16 @@ visualization_msgs::Marker Visualizer::displayString(int id, geometry_msgs::Poin stringMarker.id = id; stringMarker.type = visualization_msgs::Marker::TEXT_VIEW_FACING; stringMarker.action = visualization_msgs::Marker::ADD; - stringMarker.pose.position.x = point.x; - stringMarker.pose.position.y = point.y; - stringMarker.pose.position.z = point.z; - stringMarker.scale.x = 1; + stringMarker.pose.position.x = pointGM.x; + stringMarker.pose.position.y = pointGM.y; + stringMarker.pose.position.z = pointGM.z; + stringMarker.scale.x = 0.2; + stringMarker.scale.y = 0.2; + stringMarker.scale.z = 0.2; stringMarker.color.a = 1; - stringMarker.color.r = 1; - stringMarker.color.g = 1; - stringMarker.color.b = 1; + stringMarker.color.r = 0.5; + stringMarker.color.g = 0.5; + stringMarker.color.b = 0.5; std::string id_string = std::to_string(id); stringMarker.text = id_string; @@ -196,3 +206,4 @@ unsigned Visualizer::getB(int id){ j = id % (colors_.size()/3); return colors_[3*j+2]*255; } +} // namespace planeseg diff --git a/plane_seg_ros/src/plane_seg_ros.cpp b/plane_seg_ros/src/plane_seg_ros.cpp index 3258e40..c5e13c9 100644 --- a/plane_seg_ros/src/plane_seg_ros.cpp +++ b/plane_seg_ros/src/plane_seg_ros.cpp @@ -15,6 +15,7 @@ #include #include +#include #include #include @@ -53,10 +54,12 @@ Pass::Pass(ros::NodeHandle node_): look_pose_pub_ = node_.advertise("/plane_seg/look_pose", 10); elev_map_pub_ = node_.advertise("/rooster_elevation_mapping/elevation_map", 1); pose_pub_ = node_.advertise("/vilens/pose", 1); + id_strings_pub_ = node_.advertise("/plane_seg/id_strings", 10); last_robot_pose_ = Eigen::Isometry3d::Identity(); -// tracking_ = planeseg::Tracker(); + tracking_ = planeseg::Tracker(); + visualizer_ = planeseg::Visualizer(); colors_ = { 51/255.0, 160/255.0, 44/255.0, //0 @@ -204,8 +207,6 @@ void Pass::stepThroughFile(std::string filename){ if (s != NULL){ std::cout << "received gridmap at time " << m.getTime().toNSec() << " with resolution:" << s->info.resolution << std::endl; - int i = 0; - if (i ) elevationMapCallback(*s); elev_map_pub_.publish(*s); std::cout << "Press [Enter] to continue to next gridmap message" << std::endl; @@ -238,14 +239,12 @@ void Pass::processCloud(planeseg::LabeledCloud::Ptr& inCloud, Eigen::Vector3f or fitter.setMaxAngleOfPlaneSegmenter(10); result_ = fitter.go(); - std::cout << "got result" << std::endl; -// tracking_.reset(); - std::cout << "reset tracker" << std::endl; -// tracking_.convertResult(result_); - std::cout << "converted result to planes" << std::endl; + tracking_.reset(); + tracking_.convertResult(result_); // tracking_.planesToIds(); - std::cout << "converted planes to ids" << std::endl; -// tracking_.printIds(); + tracking_.printIds(); +// tracking_.printidAssigned(); + publishIdsAsStrings(); Eigen::Vector3f rz = lookDir; Eigen::Vector3f rx = rz.cross(Eigen::Vector3f::UnitZ()); @@ -450,28 +449,16 @@ void Pass::publishHullsAsMarkers(std::vector< pcl::PointCloud::Pt } -void Pass::printCentroidList(std::vector centroid_list_){ - for (unsigned i=0; i < centroid_list_.size(); i++){ - for (unsigned j=0; j < centroid_list_[i].size(); j++){ - std::cout << centroid_list_[i][j] << std::endl; - } +void Pass::publishIdsAsStrings(){ + visualization_msgs::MarkerArray strings_array; + for (size_t i = 0; i < tracking_.newStairs.size(); ++i){ + visualization_msgs::Marker id_marker; + int id = tracking_.newStairs[i].id; + pcl::PointXYZ centroid = tracking_.newStairs[i].centroid; + id_marker = visualizer_.displayString(id, centroid); + id_marker.frame_locked = true; + strings_array.markers.push_back(id_marker); } -} - -void Pass::compareCentroidLists(std::vector old_centroid_list_, std::vector centroid_list_){ - -// double threshold = 1; - std::cout << "Entered compareCentroidLists" << std::endl; - for (unsigned i = 0; i < old_centroid_list_.size(); i++){ - std::cout << "Entered first for loop" << std::endl; - for (unsigned j = 0; j < centroid_list_.size(); j++){ -// double z_dist = fabs(old_centroid_list_[i][2] - centroid_list_[j][2]); - -// if (z_dist < threshold){ -// std::complex dist = (old_centroid_list_[i] - centroid_list_[j]).norm(); - std::cout << "The " << i << "'th old centroid is " << (old_centroid_list_[i] - centroid_list_[j]).norm() << " far away from the " << j << "'th new centroid." << std::endl; -// } - } - } + id_strings_pub_.publish(strings_array); } From 569b51f68fe30027d0823b1a0981e81a43c2328b Mon Sep 17 00:00:00 2001 From: Christos Ioannou Date: Fri, 15 Jan 2021 00:17:37 +0000 Subject: [PATCH 23/58] display markers at centroids --- plane_seg_ros/include/plane_seg_ros/Pass.hpp | 5 +- .../include/plane_seg_ros/Visualizer.hpp | 5 +- plane_seg_ros/src/Visualizer.cpp | 53 ++++++++++++------- plane_seg_ros/src/plane_seg_ros.cpp | 19 +++++-- 4 files changed, 53 insertions(+), 29 deletions(-) diff --git a/plane_seg_ros/include/plane_seg_ros/Pass.hpp b/plane_seg_ros/include/plane_seg_ros/Pass.hpp index 3a014d8..1cc061b 100644 --- a/plane_seg_ros/include/plane_seg_ros/Pass.hpp +++ b/plane_seg_ros/include/plane_seg_ros/Pass.hpp @@ -36,16 +36,15 @@ class Pass{ int secs, int nsecs); void printResultAsJson(); void publishResult(); - void printCentroidList(std::vector centroid_list); - void compareCentroidLists(std::vector old_centroid_list, std::vector centroid_list); void publishIdsAsStrings(); + void publishCentroidsAsSpheres(); private: ros::NodeHandle node_; std::vector colors_; ros::Subscriber point_cloud_sub_, grid_map_sub_, pose_sub_; - ros::Publisher received_cloud_pub_, hull_cloud_pub_, hull_markers_pub_, look_pose_pub_, elev_map_pub_, pose_pub_, id_strings_pub_; + ros::Publisher received_cloud_pub_, hull_cloud_pub_, hull_markers_pub_, look_pose_pub_, elev_map_pub_, pose_pub_, id_strings_pub_, centroids_pub_; Eigen::Isometry3d last_robot_pose_; planeseg::BlockFitter::Result result_; diff --git a/plane_seg_ros/include/plane_seg_ros/Visualizer.hpp b/plane_seg_ros/include/plane_seg_ros/Visualizer.hpp index c52c427..67154d3 100644 --- a/plane_seg_ros/include/plane_seg_ros/Visualizer.hpp +++ b/plane_seg_ros/include/plane_seg_ros/Visualizer.hpp @@ -20,8 +20,9 @@ class Visualizer { Visualizer(); ~Visualizer(); - sensor_msgs::PointCloud2 displayCentroids(std::vector &planes); - visualization_msgs::Marker displayString(int id, pcl::PointXYZ point_); +// sensor_msgs::PointCloud2 displayCentroids(std::vector &planes); + visualization_msgs::Marker displayCentroid(planeseg::plane plane); + visualization_msgs::Marker displayString(planeseg::plane); visualization_msgs::Marker displayLineStrip(int id, pcl::PointXYZ newCentroid); private: diff --git a/plane_seg_ros/src/Visualizer.cpp b/plane_seg_ros/src/Visualizer.cpp index 9e5c14c..58b8d4d 100644 --- a/plane_seg_ros/src/Visualizer.cpp +++ b/plane_seg_ros/src/Visualizer.cpp @@ -61,24 +61,36 @@ void Visualizer::publish(std::vector &planes){ } */ -sensor_msgs::PointCloud2 Visualizer::displayCentroids(std::vector &planes){ - int id; - pcl::PointXYZRGB centroid; - pcl::PointCloud display_centroids; - - for (unsigned i = 0; i < planes.size(); i++){ - id = planes[i].id; - centroid.r = getR(id); - centroid.g = getG(id); - centroid.b = getB(id); - centroid.x = planes[i].centroid.x; - centroid.y = planes[i].centroid.y; - centroid.z = planes[i].centroid.z; - display_centroids.push_back(centroid); - } - sensor_msgs::PointCloud2 displayCentroidsMsg; - pcl::toROSMsg(display_centroids, displayCentroidsMsg); - return displayCentroidsMsg; +visualization_msgs::Marker Visualizer::displayCentroid(planeseg::plane plane){ + + int id = plane.id; + pcl::PointXYZ centroid = plane.centroid; + + // convert centroid to geometry_msgs/Point + geometry_msgs::Point centroidGM; + centroidGM.x = centroid.x; + centroidGM.y = centroid.y; + centroidGM.z = centroid.z; + + visualization_msgs::Marker centroidMarker; + centroidMarker.type = visualization_msgs::Marker::SPHERE; + centroidMarker.header.frame_id = "odom"; + centroidMarker.header.stamp = ros::Time(); + centroidMarker.ns = "centroids"; + centroidMarker.id = id; + centroidMarker.scale.x = 0.05; + centroidMarker.scale.y = 0.05; + centroidMarker.scale.z = 0.05; + centroidMarker.color.r = getR(id); + centroidMarker.color.g = getG(id); + centroidMarker.color.b = getB(id); + centroidMarker.color.a = 1; + centroidMarker.pose.orientation.w = 1.0; + centroidMarker.pose.position.x = centroidGM.x; + centroidMarker.pose.position.y = centroidGM.y; + centroidMarker.pose.position.z = centroidGM.z; + + return centroidMarker; } visualization_msgs::Marker Visualizer::displayLineStrip(int id, pcl::PointXYZ newCentroid){ @@ -157,7 +169,10 @@ visualization_msgs::Marker Visualizer::displayLineStrip(int id, pcl::PointXYZ ne return lineStripMarker; } -visualization_msgs::Marker Visualizer::displayString(int id, pcl::PointXYZ point){ +visualization_msgs::Marker Visualizer::displayString(planeseg::plane plane){ + + int id = plane.id; + pcl::PointXYZ point = plane.centroid; // convert pcl::PointXYZ into geometry_msgs::Point geometry_msgs::Point pointGM; diff --git a/plane_seg_ros/src/plane_seg_ros.cpp b/plane_seg_ros/src/plane_seg_ros.cpp index c5e13c9..9c80011 100644 --- a/plane_seg_ros/src/plane_seg_ros.cpp +++ b/plane_seg_ros/src/plane_seg_ros.cpp @@ -55,6 +55,7 @@ Pass::Pass(ros::NodeHandle node_): elev_map_pub_ = node_.advertise("/rooster_elevation_mapping/elevation_map", 1); pose_pub_ = node_.advertise("/vilens/pose", 1); id_strings_pub_ = node_.advertise("/plane_seg/id_strings", 10); + centroids_pub_ = node_.advertise("/plane_seg/centroids", 10); last_robot_pose_ = Eigen::Isometry3d::Identity(); @@ -245,6 +246,7 @@ void Pass::processCloud(planeseg::LabeledCloud::Ptr& inCloud, Eigen::Vector3f or tracking_.printIds(); // tracking_.printidAssigned(); publishIdsAsStrings(); + publishCentroidsAsSpheres(); Eigen::Vector3f rz = lookDir; Eigen::Vector3f rx = rz.cross(Eigen::Vector3f::UnitZ()); @@ -448,17 +450,24 @@ void Pass::publishHullsAsMarkers(std::vector< pcl::PointCloud::Pt hull_markers_pub_.publish(marker); } - void Pass::publishIdsAsStrings(){ visualization_msgs::MarkerArray strings_array; for (size_t i = 0; i < tracking_.newStairs.size(); ++i){ visualization_msgs::Marker id_marker; - int id = tracking_.newStairs[i].id; - pcl::PointXYZ centroid = tracking_.newStairs[i].centroid; - id_marker = visualizer_.displayString(id, centroid); + id_marker = visualizer_.displayString(tracking_.newStairs[i]); id_marker.frame_locked = true; strings_array.markers.push_back(id_marker); } - id_strings_pub_.publish(strings_array); } + +void Pass::publishCentroidsAsSpheres(){ + visualization_msgs::MarkerArray centroids_array; + for (size_t i = 0; i < tracking_.newStairs.size(); ++i){ + visualization_msgs::Marker centroid_marker; + centroid_marker = visualizer_.displayCentroid(tracking_.newStairs[i]); + centroid_marker.frame_locked = true; + centroids_array.markers.push_back(centroid_marker); + } + centroids_pub_.publish(centroids_array); +} From 90492df922f935a19df32c649de24f408e7ecc42 Mon Sep 17 00:00:00 2001 From: Christos Ioannou Date: Fri, 15 Jan 2021 18:47:59 +0000 Subject: [PATCH 24/58] tracker and visualiser working --- plane_seg/src/Tracker.cpp | 8 +- plane_seg_ros/include/plane_seg_ros/Pass.hpp | 7 +- .../include/plane_seg_ros/Visualizer.hpp | 10 +- plane_seg_ros/src/Visualizer.cpp | 116 +++++++----- plane_seg_ros/src/plane_seg_ros.cpp | 168 ++++++++++++++---- 5 files changed, 217 insertions(+), 92 deletions(-) diff --git a/plane_seg/src/Tracker.cpp b/plane_seg/src/Tracker.cpp index bea944b..e150bb2 100644 --- a/plane_seg/src/Tracker.cpp +++ b/plane_seg/src/Tracker.cpp @@ -18,7 +18,7 @@ Tracker::Tracker(){ Tracker::~Tracker(){} pcl::PointXYZ Tracker::find_centroid(pcl::PointCloud cloud ){ - std::cout << "Computing centroid" << std::endl; +// std::cout << "Computing centroid" << std::endl; Eigen::Vector4f centroid_eigen; pcl::compute3DCentroid(cloud, centroid_eigen); pcl::PointXYZ centroid; @@ -103,7 +103,7 @@ int Tracker::get_plane_id(planeseg::plane plane){ // if the id of the ith plane in oldStair exists // if (!idAssigned[i].taken && test){ distz = fabs(oldStairs[i].centroid.z - plane.centroid.z); - std::cout << "distz = " << distz << std::endl; + // std::cout << "distz = " << distz << std::endl; if(distz < threshold){ // distance = pcl::euclideanDistance(oldStairs[i].centroid, plane.centroid); @@ -115,7 +115,7 @@ int Tracker::get_plane_id(planeseg::plane plane){ // } } - std::cout << "closestDist = " << closestDist << std::endl; + // std::cout << "closestDist = " << closestDist << std::endl; if (closest != -1){ id = oldStairs[closest].id; @@ -158,7 +158,7 @@ void Tracker::printStairs(std::vector stairs){ } void Tracker::printIds(){ - std::cout << "entered printIds" << std::endl; +// std::cout << "entered printIds" << std::endl; std::cout << "Total number of ids assigned: " << newStairs.size() << std::endl; for (size_t i = 0; i < newStairs.size(); ++i){ std::cout << newStairs[i].id << std::endl; diff --git a/plane_seg_ros/include/plane_seg_ros/Pass.hpp b/plane_seg_ros/include/plane_seg_ros/Pass.hpp index 1cc061b..773a34d 100644 --- a/plane_seg_ros/include/plane_seg_ros/Pass.hpp +++ b/plane_seg_ros/include/plane_seg_ros/Pass.hpp @@ -32,19 +32,22 @@ class Pass{ void publishHullsAsCloud(std::vector::Ptr> cloud_ptrs, int secs, int nsecs); - void publishHullsAsMarkers(std::vector< pcl::PointCloud::Ptr > cloud_ptrs, + void publishHullsAsMarkersOLD(std::vector< pcl::PointCloud::Ptr > cloud_ptrs, int secs, int nsecs); void printResultAsJson(); void publishResult(); void publishIdsAsStrings(); void publishCentroidsAsSpheres(); + void publishHullsAsMarkers(); private: ros::NodeHandle node_; std::vector colors_; + std::vector colors_2; + std::vector colors_3; ros::Subscriber point_cloud_sub_, grid_map_sub_, pose_sub_; - ros::Publisher received_cloud_pub_, hull_cloud_pub_, hull_markers_pub_, look_pose_pub_, elev_map_pub_, pose_pub_, id_strings_pub_, centroids_pub_; + ros::Publisher received_cloud_pub_, hull_cloud_pub_, hull_markers_pub_, look_pose_pub_, elev_map_pub_, pose_pub_, id_strings_pub_, centroids_pub_, hulls_pub_; Eigen::Isometry3d last_robot_pose_; planeseg::BlockFitter::Result result_; diff --git a/plane_seg_ros/include/plane_seg_ros/Visualizer.hpp b/plane_seg_ros/include/plane_seg_ros/Visualizer.hpp index 67154d3..d8dbaa9 100644 --- a/plane_seg_ros/include/plane_seg_ros/Visualizer.hpp +++ b/plane_seg_ros/include/plane_seg_ros/Visualizer.hpp @@ -22,16 +22,18 @@ class Visualizer { // sensor_msgs::PointCloud2 displayCentroids(std::vector &planes); visualization_msgs::Marker displayCentroid(planeseg::plane plane); - visualization_msgs::Marker displayString(planeseg::plane); + visualization_msgs::Marker displayString(planeseg::plane plane); visualization_msgs::Marker displayLineStrip(int id, pcl::PointXYZ newCentroid); + visualization_msgs::Marker displayHull(planeseg::plane plane); + double getR(int id); + double getG(int id); + double getB(int id); private: std::vector colors_; std::vector lineStrips; - unsigned getR(int id); - unsigned getG(int id); - unsigned getB(int id); + }; } // namespace planeseg diff --git a/plane_seg_ros/src/Visualizer.cpp b/plane_seg_ros/src/Visualizer.cpp index 58b8d4d..db26416 100644 --- a/plane_seg_ros/src/Visualizer.cpp +++ b/plane_seg_ros/src/Visualizer.cpp @@ -16,50 +16,38 @@ namespace planeseg { Visualizer::Visualizer(){ colors_ = { - 51/255.0, 160/255.0, 44/255.0, //0 - 166/255.0, 206/255.0, 227/255.0, - 178/255.0, 223/255.0, 138/255.0,//6 - 31/255.0, 120/255.0, 180/255.0, - 251/255.0, 154/255.0, 153/255.0,// 12 - 227/255.0, 26/255.0, 28/255.0, - 253/255.0, 191/255.0, 111/255.0,// 18 - 106/255.0, 61/255.0, 154/255.0, - 255/255.0, 127/255.0, 0/255.0, // 24 - 202/255.0, 178/255.0, 214/255.0, - 1.0, 0.0, 0.0, // red // 30 - 0.0, 1.0, 0.0, // green - 0.0, 0.0, 1.0, // blue// 36 - 1.0, 1.0, 0.0, - 1.0, 0.0, 1.0, // 42 - 0.0, 1.0, 1.0, - 0.5, 1.0, 0.0, - 1.0, 0.5, 0.0, - 0.5, 0.0, 1.0, - 1.0, 0.0, 0.5, - 0.0, 0.5, 1.0, - 0.0, 1.0, 0.5, - 1.0, 0.5, 0.5, - 0.5, 1.0, 0.5, - 0.5, 0.5, 1.0, - 0.5, 0.5, 1.0, - 0.5, 1.0, 0.5, - 0.5, 0.5, 1.0}; + 1, 1, 1, // 42 + 255, 255, 120, + 1, 120, 1, + 1, 225, 1, + 120, 255, 1, + 1, 255, 255, + 120, 1, 1, + 255, 120, 255, + 120, 1, 255, + 1, 1, 120, + 255, 255, 255, + 120, 120, 1, + 120, 120, 120, + 1, 1, 255, + 255, 1, 255, + 120, 120, 255, + 120, 255, 120, + 1, 120, 120, + 1, 1, 255, + 255, 1, 1, + 155, 1, 120, + 120, 1, 120, + 255, 120, 1, + 1, 120, 255, + 255, 120, 120, + 1, 255, 120, + 255, 255, 1}; } Visualizer::~Visualizer(){} -/* NOT FINISHED YET -void Visualizer::publish(std::vector &planes){ - - // publish coloured centroids - sensor_msgs::PointCloud2 displayCentroidsMsg(displayCentroids(planes)); - centroids_pub_.publish(displayCentroidsMsg); - - // publish line segments - visualization_msgs::Marker displayLineStripMsg(displayCentroidsMsg); -} -*/ visualization_msgs::Marker Visualizer::displayCentroid(planeseg::plane plane){ @@ -194,9 +182,9 @@ visualization_msgs::Marker Visualizer::displayString(planeseg::plane plane){ stringMarker.scale.y = 0.2; stringMarker.scale.z = 0.2; stringMarker.color.a = 1; - stringMarker.color.r = 0.5; - stringMarker.color.g = 0.5; - stringMarker.color.b = 0.5; + stringMarker.color.r = 1; + stringMarker.color.g = 1; + stringMarker.color.b = 1; std::string id_string = std::to_string(id); stringMarker.text = id_string; @@ -204,21 +192,51 @@ visualization_msgs::Marker Visualizer::displayString(planeseg::plane plane){ return stringMarker; } -unsigned Visualizer::getR(int id){ - unsigned j; +visualization_msgs::Marker Visualizer::displayHull(planeseg::plane plane){ + + geometry_msgs::Point pointGM; + std_msgs::ColorRGBA point_color; + visualization_msgs::Marker hullMarker; + std::string FrameID; + + hullMarker.header.frame_id = "odom"; + hullMarker.header.stamp = ros::Time(); + hullMarker.ns = "hull lines"; + hullMarker.id = plane.id; + hullMarker.type = visualization_msgs::Marker::LINE_STRIP; + hullMarker.action = visualization_msgs::Marker::ADD; + hullMarker.pose.orientation.w = 1.0; + hullMarker.scale.x = 0.03; + hullMarker.color.r = getR(plane.id); + hullMarker.color.g = getG(plane.id); + hullMarker.color.b = getB(plane.id); + hullMarker.color.a = 1; + + for (size_t i = 0; i < plane.cloud.size(); ++i){ + pointGM.x = plane.cloud[i].x; + pointGM.y = plane.cloud[i].y; + pointGM.z = plane.cloud[i].z; + hullMarker.points.push_back(pointGM); + } + + return hullMarker; +} + +double Visualizer::getR(int id){ + double j; j = id % (colors_.size()/3); - return colors_[3*j]*255; + return colors_[3*j]; } -unsigned Visualizer::getG(int id){ +double Visualizer::getG(int id){ unsigned j; j = id % (colors_.size()/3); - return colors_[3*j+1]*255; + return colors_[3*j+1]; } -unsigned Visualizer::getB(int id){ +double Visualizer::getB(int id){ unsigned j; j = id % (colors_.size()/3); - return colors_[3*j+2]*255; + return colors_[3*j+2]; } } // namespace planeseg diff --git a/plane_seg_ros/src/plane_seg_ros.cpp b/plane_seg_ros/src/plane_seg_ros.cpp index 9c80011..922b079 100644 --- a/plane_seg_ros/src/plane_seg_ros.cpp +++ b/plane_seg_ros/src/plane_seg_ros.cpp @@ -56,6 +56,7 @@ Pass::Pass(ros::NodeHandle node_): pose_pub_ = node_.advertise("/vilens/pose", 1); id_strings_pub_ = node_.advertise("/plane_seg/id_strings", 10); centroids_pub_ = node_.advertise("/plane_seg/centroids", 10); + hulls_pub_ = node_.advertise("/plane_seg/hulls", 10); last_robot_pose_ = Eigen::Isometry3d::Identity(); @@ -63,34 +64,33 @@ Pass::Pass(ros::NodeHandle node_): visualizer_ = planeseg::Visualizer(); colors_ = { - 51/255.0, 160/255.0, 44/255.0, //0 - 166/255.0, 206/255.0, 227/255.0, - 178/255.0, 223/255.0, 138/255.0,//6 - 31/255.0, 120/255.0, 180/255.0, - 251/255.0, 154/255.0, 153/255.0,// 12 - 227/255.0, 26/255.0, 28/255.0, - 253/255.0, 191/255.0, 111/255.0,// 18 - 106/255.0, 61/255.0, 154/255.0, - 255/255.0, 127/255.0, 0/255.0, // 24 - 202/255.0, 178/255.0, 214/255.0, - 1.0, 0.0, 0.0, // red // 30 - 0.0, 1.0, 0.0, // green - 0.0, 0.0, 1.0, // blue// 36 - 1.0, 1.0, 0.0, - 1.0, 0.0, 1.0, // 42 - 0.0, 1.0, 1.0, - 0.5, 1.0, 0.0, - 1.0, 0.5, 0.0, - 0.5, 0.0, 1.0, - 1.0, 0.0, 0.5, - 0.0, 0.5, 1.0, - 0.0, 1.0, 0.5, - 1.0, 0.5, 0.5, - 0.5, 1.0, 0.5, - 0.5, 0.5, 1.0, - 0.5, 0.5, 1.0, - 0.5, 1.0, 0.5, - 0.5, 0.5, 1.0}; + 1, 1, 1, // 42 + 255, 255, 120, + 1, 120, 1, + 225, 120, 1, + 1, 255, 1, + 1, 255, 255, + 120, 1, 1, + 255, 120, 255, + 120, 1, 255, + 1, 1, 120, + 255, 255, 255, + 120, 120, 1, + 120, 120, 120, + 1, 1, 255, + 255, 1, 255, + 120, 120, 255, + 120, 255, 120, + 1, 120, 120, + 120, 255, 255, + 255, 1, 1, + 155, 1, 120, + 120, 1, 120, + 255, 120, 1, + 1, 120, 255, + 255, 120, 120, + 1, 255, 120, + 255, 255, 1}; } @@ -245,8 +245,6 @@ void Pass::processCloud(planeseg::LabeledCloud::Ptr& inCloud, Eigen::Vector3f or // tracking_.planesToIds(); tracking_.printIds(); // tracking_.printidAssigned(); - publishIdsAsStrings(); - publishCentroidsAsSpheres(); Eigen::Vector3f rz = lookDir; Eigen::Vector3f rx = rz.cross(Eigen::Vector3f::UnitZ()); @@ -328,8 +326,12 @@ void Pass::publishResult(){ cloud_ptrs.push_back(cloud_ptr); } + std::cout << "colors_.size() = " << colors_.size() << std::endl; + publishIdsAsStrings(); + publishCentroidsAsSpheres(); + publishHullsAsMarkers(); publishHullsAsCloud(cloud_ptrs, 0, 0); - publishHullsAsMarkers(cloud_ptrs, 0, 0); +// publishHullsAsMarkers(cloud_ptrs, 0, 0); //pcl::PCDWriter pcd_writer_; //pcd_writer_.write ("/home/mfallon/out.pcd", cloud, false); @@ -369,7 +371,7 @@ void Pass::publishHullsAsCloud(std::vector< pcl::PointCloud::Ptr } -void Pass::publishHullsAsMarkers(std::vector< pcl::PointCloud::Ptr > cloud_ptrs, +void Pass::publishHullsAsMarkersOLD(std::vector< pcl::PointCloud::Ptr > cloud_ptrs, int secs, int nsecs){ geometry_msgs::Point point; std_msgs::ColorRGBA point_color; @@ -447,7 +449,7 @@ void Pass::publishHullsAsMarkers(std::vector< pcl::PointCloud::Pt marker.points.push_back(point); } marker.frame_locked = true; - hull_markers_pub_.publish(marker); +// hull_markers_pub_.publish(marker); } void Pass::publishIdsAsStrings(){ @@ -471,3 +473,103 @@ void Pass::publishCentroidsAsSpheres(){ } centroids_pub_.publish(centroids_array); } + +void Pass::publishHullsAsMarkers(){ + + std::vector> clouds; + for (size_t k = 0; k < tracking_.newStairs.size(); ++k){ + clouds.push_back(tracking_.newStairs[k].cloud); + } + + std::vector ids; + for (size_t m = 0; m < tracking_.newStairs.size(); ++m){ + ids.push_back(tracking_.newStairs[m].id); + } + + geometry_msgs::Point point; + std_msgs::ColorRGBA point_color; + visualization_msgs::Marker hullMarker; + std::string frameID; + + // define markers + hullMarker.header.frame_id = "odom"; + hullMarker.header.stamp = ros::Time(0, 0); + hullMarker.ns = "hull lines"; + hullMarker.id = 0; + hullMarker.type = visualization_msgs::Marker::LINE_LIST; //visualization_msgs::Marker::POINTS; + hullMarker.action = visualization_msgs::Marker::ADD; + hullMarker.pose.position.x = 0; + hullMarker.pose.position.y = 0; + hullMarker.pose.position.z = 0; + hullMarker.pose.orientation.x = 0.0; + hullMarker.pose.orientation.y = 0.0; + hullMarker.pose.orientation.z = 0.0; + hullMarker.pose.orientation.w = 1.0; + hullMarker.scale.x = 0.03; + hullMarker.scale.y = 0.03; + hullMarker.scale.z = 0.03; + hullMarker.color.a = 1.0; + + for (size_t i = 0; i < clouds.size (); i++){ + +/* int nColor = ids[i] % (colors_.size()/3); +// std::cout << "nColor for id " << ids[i] << " = " << nColor << std::endl; + double r = colors_[nColor*3]; +// std::cout << "r for id " << i << " = " << r << std::endl; + double g = colors_[nColor*3+1]; +// std::cout << "g for id " << ids[i] << " = " << g << std::endl; + double b = colors_[nColor*3+2]; +// std::cout << "b for id " << ids[i] << " = " << b << std::endl; +*/ + + double r = visualizer_.getR(ids[i]); + double g = visualizer_.getG(ids[i]); + double b = visualizer_.getB(ids[i]); + + for (size_t j = 1; j < clouds[i].points.size (); j++){ + point.x = clouds[i].points[j-1].x; + point.y = clouds[i].points[j-1].y; + point.z = clouds[i].points[j-1].z; + point_color.r = r; + point_color.g = g; + point_color.b = b; + point_color.a = 1.0; + hullMarker.colors.push_back(point_color); + hullMarker.points.push_back(point); + + // + point.x = clouds[i].points[j].x; + point.y = clouds[i].points[j].y; + point.z = clouds[i].points[j].z; + point_color.r = r; + point_color.g = g; + point_color.b = b; + point_color.a = 1.0; + hullMarker.colors.push_back(point_color); + hullMarker.points.push_back(point); + } + + // start to end line: + point.x = clouds[i].points[0].x; + point.y = clouds[i].points[0].y; + point.z = clouds[i].points[0].z; + point_color.r = r; + point_color.g = g; + point_color.b = b; + point_color.a = 1.0; + hullMarker.colors.push_back(point_color); + hullMarker.points.push_back(point); + + point.x = clouds[i].points[ clouds[i].points.size()-1 ].x; + point.y = clouds[i].points[ clouds[i].points.size()-1 ].y; + point.z = clouds[i].points[ clouds[i].points.size()-1 ].z; + point_color.r = r; + point_color.g = g; + point_color.b = b; + point_color.a = 1.0; + hullMarker.colors.push_back(point_color); + hullMarker.points.push_back(point); + } + hullMarker.frame_locked = true; + hull_markers_pub_.publish(hullMarker); +} From e2860ec14ce459469a4b3d14f0c7e11910386f57 Mon Sep 17 00:00:00 2001 From: Christos Ioannou Date: Sat, 16 Jan 2021 02:01:44 +0000 Subject: [PATCH 25/58] tracker and visualizer cleanup --- plane_seg/include/plane_seg/Tracker.hpp | 8 ---- plane_seg/src/Tracker.cpp | 59 ++----------------------- plane_seg_ros/src/plane_seg_ros.cpp | 13 ------ 3 files changed, 3 insertions(+), 77 deletions(-) diff --git a/plane_seg/include/plane_seg/Tracker.hpp b/plane_seg/include/plane_seg/Tracker.hpp index fa27bde..bb5f9f2 100644 --- a/plane_seg/include/plane_seg/Tracker.hpp +++ b/plane_seg/include/plane_seg/Tracker.hpp @@ -29,22 +29,14 @@ class Tracker { pcl::PointXYZ find_centroid(pcl::PointCloud cloud ); void reset(); void convertResult(planeseg::BlockFitter::Result result); - std::vector planesToIds(); void test(planeseg::BlockFitter::Result result); - void printStairs(std::vector centroid_list_); void printIds(); - void printidAssigned(); std::vector newStairs; private: std::vector oldStairs; - std::vector idAssigned; std::vector::Ptr> cloud_ptrs; - std::vector vector_of_planes; - std::vector vector_of_ids; - std::vector vec_planes_no_ids; - std::vector icentroid_list_init; int totalIds; }; } diff --git a/plane_seg/src/Tracker.cpp b/plane_seg/src/Tracker.cpp index e150bb2..d341b66 100644 --- a/plane_seg/src/Tracker.cpp +++ b/plane_seg/src/Tracker.cpp @@ -12,7 +12,7 @@ namespace planeseg { Tracker::Tracker(){ - totalIds = 1; + totalIds = 0; } Tracker::~Tracker(){} @@ -49,37 +49,19 @@ void Tracker::convertResult(planeseg::BlockFitter::Result result_){ pcl::PointXYZ cloud_centroid; cloud_centroid = find_centroid(cloud); - // pcl::PointCloud::Ptr cloud_ptr; - // cloud_ptr = cloud.makeShared(); - // cloud_ptrs.push_back(cloud_ptr); planeseg::plane newPlane; newPlane.cloud = cloud; newPlane.centroid = cloud_centroid; newPlane.id = get_plane_id(newPlane); -// std::vector vec_planes_no_ids.push_back(plane_no_id); -// vector_of_planes.push_back(plane_no_id); newStairs.push_back(newPlane); } } -// *** NEXT STEP: create std::vector = planesToIds(std::vector) which goes through each plane in the vector and calls get_plane_id and assigns it to the plane - -std::vector Tracker::planesToIds(){ - std::cout << "entered planesToIds" << std::endl; - for (size_t i = 0; i < vector_of_planes.size(); ++i){ - int current_id; - current_id = Tracker::get_plane_id(vector_of_planes[i]); - vector_of_ids.push_back(current_id); - } - return vector_of_ids; -} - int Tracker::get_plane_id(planeseg::plane plane){ // std::cout << "entered get_plane_id" << std::endl; - bool test = true; int id; if(oldStairs.empty()){ @@ -90,7 +72,6 @@ int Tracker::get_plane_id(planeseg::plane plane){ else{ pcl::PointXYZ oldCentroid; double threshold = 0.1; - double distance; double distz; int closest = -1; // start with closestDist being far larger than anything that would ever happen @@ -100,10 +81,8 @@ int Tracker::get_plane_id(planeseg::plane plane){ for (size_t i = 0; i < oldStairs.size(); ++i){ - // if the id of the ith plane in oldStair exists - // if (!idAssigned[i].taken && test){ distz = fabs(oldStairs[i].centroid.z - plane.centroid.z); - // std::cout << "distz = " << distz << std::endl; + if(distz < threshold){ // distance = pcl::euclideanDistance(oldStairs[i].centroid, plane.centroid); @@ -112,22 +91,17 @@ int Tracker::get_plane_id(planeseg::plane plane){ closestDist = distz; } } - // } } - // std::cout << "closestDist = " << closestDist << std::endl; - if (closest != -1){ id = oldStairs[closest].id; -// idAssigned[closest].taken = true; } else{ id = totalIds; ++totalIds; } } -// plane.id = id; -// newStairs.push_back(plane); + return id; } @@ -135,27 +109,7 @@ void Tracker::reset(){ std::cout << "entered Tracker reset" << std::endl; oldStairs = newStairs; newStairs.clear(); - idAssigned.clear(); - - // set the ids in the vector IdAssigned to be the same as the ids in the planes of oldStairs - IdAssigned temp; - temp.taken = false; - for (size_t i=0; i < oldStairs.size(); ++i){ - temp.id = oldStairs[i].id; - idAssigned.push_back(temp); } -} - - -void Tracker::printStairs(std::vector stairs){ - std::cout << "Centroids of " << stairs.size() << " steps: " << std::endl; - for (unsigned i = 0; i < stairs.size(); ++i){ - std::cout << "Step " << i << " (x,y,z): "; - std::cout << stairs[i].centroid.x << ", "; - std::cout << stairs[i].centroid.y << ", "; - std::cout << stairs[i].centroid.z << std::endl; - } -} void Tracker::printIds(){ // std::cout << "entered printIds" << std::endl; @@ -165,11 +119,4 @@ void Tracker::printIds(){ } } -void Tracker::printidAssigned(){ - std::cout << "idAssigned: " << std::endl; - for (size_t i = 0; i < idAssigned.size(); ++i){ - std::cout << idAssigned[i].id << std::endl; - } -} - } // namespace plane_seg diff --git a/plane_seg_ros/src/plane_seg_ros.cpp b/plane_seg_ros/src/plane_seg_ros.cpp index 922b079..44911dd 100644 --- a/plane_seg_ros/src/plane_seg_ros.cpp +++ b/plane_seg_ros/src/plane_seg_ros.cpp @@ -242,9 +242,7 @@ void Pass::processCloud(planeseg::LabeledCloud::Ptr& inCloud, Eigen::Vector3f or result_ = fitter.go(); tracking_.reset(); tracking_.convertResult(result_); -// tracking_.planesToIds(); tracking_.printIds(); -// tracking_.printidAssigned(); Eigen::Vector3f rz = lookDir; Eigen::Vector3f rx = rz.cross(Eigen::Vector3f::UnitZ()); @@ -331,7 +329,6 @@ void Pass::publishResult(){ publishCentroidsAsSpheres(); publishHullsAsMarkers(); publishHullsAsCloud(cloud_ptrs, 0, 0); -// publishHullsAsMarkers(cloud_ptrs, 0, 0); //pcl::PCDWriter pcd_writer_; //pcd_writer_.write ("/home/mfallon/out.pcd", cloud, false); @@ -512,16 +509,6 @@ void Pass::publishHullsAsMarkers(){ for (size_t i = 0; i < clouds.size (); i++){ -/* int nColor = ids[i] % (colors_.size()/3); -// std::cout << "nColor for id " << ids[i] << " = " << nColor << std::endl; - double r = colors_[nColor*3]; -// std::cout << "r for id " << i << " = " << r << std::endl; - double g = colors_[nColor*3+1]; -// std::cout << "g for id " << ids[i] << " = " << g << std::endl; - double b = colors_[nColor*3+2]; -// std::cout << "b for id " << ids[i] << " = " << b << std::endl; -*/ - double r = visualizer_.getR(ids[i]); double g = visualizer_.getG(ids[i]); double b = visualizer_.getB(ids[i]); From 7fa893066328d0325743926068e74a4e1f7ff34d Mon Sep 17 00:00:00 2001 From: Christos Ioannou Date: Sat, 16 Jan 2021 18:00:24 +0000 Subject: [PATCH 26/58] trace centroids through time --- plane_seg_ros/include/plane_seg_ros/Pass.hpp | 3 ++- plane_seg_ros/include/plane_seg_ros/Visualizer.hpp | 2 +- plane_seg_ros/src/Visualizer.cpp | 11 +++++++---- plane_seg_ros/src/plane_seg_ros.cpp | 14 ++++++++++++++ 4 files changed, 24 insertions(+), 6 deletions(-) diff --git a/plane_seg_ros/include/plane_seg_ros/Pass.hpp b/plane_seg_ros/include/plane_seg_ros/Pass.hpp index 773a34d..6ec54df 100644 --- a/plane_seg_ros/include/plane_seg_ros/Pass.hpp +++ b/plane_seg_ros/include/plane_seg_ros/Pass.hpp @@ -39,6 +39,7 @@ class Pass{ void publishIdsAsStrings(); void publishCentroidsAsSpheres(); void publishHullsAsMarkers(); + void publishLineStrips(); private: ros::NodeHandle node_; @@ -47,7 +48,7 @@ class Pass{ std::vector colors_3; ros::Subscriber point_cloud_sub_, grid_map_sub_, pose_sub_; - ros::Publisher received_cloud_pub_, hull_cloud_pub_, hull_markers_pub_, look_pose_pub_, elev_map_pub_, pose_pub_, id_strings_pub_, centroids_pub_, hulls_pub_; + ros::Publisher received_cloud_pub_, hull_cloud_pub_, hull_markers_pub_, look_pose_pub_, elev_map_pub_, pose_pub_, id_strings_pub_, centroids_pub_, hulls_pub_, linestrips_pub_; Eigen::Isometry3d last_robot_pose_; planeseg::BlockFitter::Result result_; diff --git a/plane_seg_ros/include/plane_seg_ros/Visualizer.hpp b/plane_seg_ros/include/plane_seg_ros/Visualizer.hpp index d8dbaa9..4d9f372 100644 --- a/plane_seg_ros/include/plane_seg_ros/Visualizer.hpp +++ b/plane_seg_ros/include/plane_seg_ros/Visualizer.hpp @@ -23,7 +23,7 @@ class Visualizer { // sensor_msgs::PointCloud2 displayCentroids(std::vector &planes); visualization_msgs::Marker displayCentroid(planeseg::plane plane); visualization_msgs::Marker displayString(planeseg::plane plane); - visualization_msgs::Marker displayLineStrip(int id, pcl::PointXYZ newCentroid); + visualization_msgs::Marker displayLineStrip(planeseg::plane plane); visualization_msgs::Marker displayHull(planeseg::plane plane); double getR(int id); double getG(int id); diff --git a/plane_seg_ros/src/Visualizer.cpp b/plane_seg_ros/src/Visualizer.cpp index db26416..320acc0 100644 --- a/plane_seg_ros/src/Visualizer.cpp +++ b/plane_seg_ros/src/Visualizer.cpp @@ -81,7 +81,10 @@ visualization_msgs::Marker Visualizer::displayCentroid(planeseg::plane plane){ return centroidMarker; } -visualization_msgs::Marker Visualizer::displayLineStrip(int id, pcl::PointXYZ newCentroid){ +visualization_msgs::Marker Visualizer::displayLineStrip(planeseg::plane plane){ + + int id = plane.id; + pcl::PointXYZ newCentroid = plane.centroid; bool point_added; point_added = false; @@ -127,8 +130,8 @@ visualization_msgs::Marker Visualizer::displayLineStrip(int id, pcl::PointXYZ ne newPlaneMarker.header.stamp = ros::Time::now(); newPlaneMarker.ns = "linestrips"; newPlaneMarker.id = id; - newPlaneMarker.scale.x = 0.1; - newPlaneMarker.scale.y = 0.1; + newPlaneMarker.scale.x = 0.01; + newPlaneMarker.scale.y = 0.01; newPlaneMarker.color.r = getR(id); newPlaneMarker.color.g = getG(id); newPlaneMarker.color.b = getB(id); @@ -138,7 +141,7 @@ visualization_msgs::Marker Visualizer::displayLineStrip(int id, pcl::PointXYZ ne // create a new linestrip for the new ID centroidMarker.type = visualization_msgs::Marker::LINE_STRIP; - centroidMarker.scale.x = 0.1; + centroidMarker.scale.x = 0.01; centroidMarker.color.r = getR(id); centroidMarker.color.g = getG(id); centroidMarker.color.b = getB(id); diff --git a/plane_seg_ros/src/plane_seg_ros.cpp b/plane_seg_ros/src/plane_seg_ros.cpp index 44911dd..1b881ea 100644 --- a/plane_seg_ros/src/plane_seg_ros.cpp +++ b/plane_seg_ros/src/plane_seg_ros.cpp @@ -57,6 +57,7 @@ Pass::Pass(ros::NodeHandle node_): id_strings_pub_ = node_.advertise("/plane_seg/id_strings", 10); centroids_pub_ = node_.advertise("/plane_seg/centroids", 10); hulls_pub_ = node_.advertise("/plane_seg/hulls", 10); + linestrips_pub_ = node_.advertise("/plane_seg/linestrips", 10); last_robot_pose_ = Eigen::Isometry3d::Identity(); @@ -328,6 +329,7 @@ void Pass::publishResult(){ publishIdsAsStrings(); publishCentroidsAsSpheres(); publishHullsAsMarkers(); + publishLineStrips(); publishHullsAsCloud(cloud_ptrs, 0, 0); //pcl::PCDWriter pcd_writer_; @@ -560,3 +562,15 @@ void Pass::publishHullsAsMarkers(){ hullMarker.frame_locked = true; hull_markers_pub_.publish(hullMarker); } + + +void Pass::publishLineStrips(){ + visualization_msgs::MarkerArray linestrips_array; + for (size_t i = 0; i < tracking_.newStairs.size(); ++i){ + visualization_msgs::Marker linestrip_marker; + linestrip_marker = visualizer_.displayLineStrip(tracking_.newStairs[i]); + linestrip_marker.frame_locked = true; + linestrips_array.markers.push_back(linestrip_marker); + } + linestrips_pub_.publish(linestrips_array); +} From 8ee2d6388e06eb2853846219a8fab0cab8f7d103 Mon Sep 17 00:00:00 2001 From: Christos Ioannou Date: Wed, 20 Jan 2021 00:22:32 +0000 Subject: [PATCH 27/58] functionality to process nth cloud --- plane_seg_ros/config/nth_cloud.yaml | 4 ++ plane_seg_ros/config/rosbag_sequential.yaml | 1 + plane_seg_ros/include/plane_seg_ros/Pass.hpp | 1 + plane_seg_ros/launch/nth_cloud.launch | 14 +++++ plane_seg_ros/src/plane_seg_ros.cpp | 57 +++++++++++++++++--- plane_seg_ros/src/plane_seg_ros_node.cpp | 16 ++++++ 6 files changed, 87 insertions(+), 6 deletions(-) create mode 100644 plane_seg_ros/config/nth_cloud.yaml create mode 100644 plane_seg_ros/launch/nth_cloud.launch diff --git a/plane_seg_ros/config/nth_cloud.yaml b/plane_seg_ros/config/nth_cloud.yaml new file mode 100644 index 0000000..861b385 --- /dev/null +++ b/plane_seg_ros/config/nth_cloud.yaml @@ -0,0 +1,4 @@ +plane_seg: + run_test_program: false + run_sequential_test: false + run_nth_cloud: true diff --git a/plane_seg_ros/config/rosbag_sequential.yaml b/plane_seg_ros/config/rosbag_sequential.yaml index f7dd18a..0902492 100644 --- a/plane_seg_ros/config/rosbag_sequential.yaml +++ b/plane_seg_ros/config/rosbag_sequential.yaml @@ -1,4 +1,5 @@ plane_seg: run_test_program: false run_sequential_test: true + run_nth_cloud: false diff --git a/plane_seg_ros/include/plane_seg_ros/Pass.hpp b/plane_seg_ros/include/plane_seg_ros/Pass.hpp index 6ec54df..088aead 100644 --- a/plane_seg_ros/include/plane_seg_ros/Pass.hpp +++ b/plane_seg_ros/include/plane_seg_ros/Pass.hpp @@ -40,6 +40,7 @@ class Pass{ void publishCentroidsAsSpheres(); void publishHullsAsMarkers(); void publishLineStrips(); + void extractNthCloud(std::string filename, int n); private: ros::NodeHandle node_; diff --git a/plane_seg_ros/launch/nth_cloud.launch b/plane_seg_ros/launch/nth_cloud.launch new file mode 100644 index 0000000..1cadef9 --- /dev/null +++ b/plane_seg_ros/launch/nth_cloud.launch @@ -0,0 +1,14 @@ + + + + + + + + + + +> + + + diff --git a/plane_seg_ros/src/plane_seg_ros.cpp b/plane_seg_ros/src/plane_seg_ros.cpp index 1b881ea..1ba8146 100644 --- a/plane_seg_ros/src/plane_seg_ros.cpp +++ b/plane_seg_ros/src/plane_seg_ros.cpp @@ -5,6 +5,7 @@ #include #include #include +#include #include #include @@ -195,7 +196,7 @@ void Pass::processFromFile(int test_example){ void Pass::stepThroughFile(std::string filename){ std::cout << filename < topics; @@ -208,11 +209,15 @@ void Pass::stepThroughFile(std::string filename){ grid_map_msgs::GridMap::ConstPtr s = m.instantiate(); if (s != NULL){ - std::cout << "received gridmap at time " << m.getTime().toNSec() << " with resolution:" << s->info.resolution << std::endl; + std::cin.get(); + ++frame; + + std::cout << "frames = " << frame << std::endl; +// std::cout << "received gridmap at time " << m.getTime().toNSec() << " with resolution:" << s->info.resolution << "and time: " << s->info.header.stamp << std::endl; + std::cout << "rosbag time: " << s->info.header.stamp << std::endl; elevationMapCallback(*s); elev_map_pub_.publish(*s); std::cout << "Press [Enter] to continue to next gridmap message" << std::endl; - std::cin.get(); } geometry_msgs::PoseWithCovarianceStamped::ConstPtr i = m.instantiate(); @@ -220,7 +225,7 @@ void Pass::stepThroughFile(std::string filename){ // std::cout << "position (x, y, z): " << i->pose.pose.position.x << ", " << i->pose.pose.position.y << ", " << i->pose.pose.position.z << std::endl; robotPoseCallBack(i); pose_pub_.publish(*i); - } + } } bag.close(); @@ -243,7 +248,7 @@ void Pass::processCloud(planeseg::LabeledCloud::Ptr& inCloud, Eigen::Vector3f or result_ = fitter.go(); tracking_.reset(); tracking_.convertResult(result_); - tracking_.printIds(); +// tracking_.printIds(); Eigen::Vector3f rz = lookDir; Eigen::Vector3f rx = rz.cross(Eigen::Vector3f::UnitZ()); @@ -325,7 +330,6 @@ void Pass::publishResult(){ cloud_ptrs.push_back(cloud_ptr); } - std::cout << "colors_.size() = " << colors_.size() << std::endl; publishIdsAsStrings(); publishCentroidsAsSpheres(); publishHullsAsMarkers(); @@ -574,3 +578,44 @@ void Pass::publishLineStrips(){ } linestrips_pub_.publish(linestrips_array); } + + +void Pass::extractNthCloud(std::string filename, int n){ + + std::cout << filename < topics; + topics.push_back(std::string("/rooster_elevation_mapping/elevation_map")); + topics.push_back(std::string("/vilens/pose")); + + rosbag::View view(bag, rosbag::TopicQuery(topics)); + + foreach(rosbag::MessageInstance const m, view){ + grid_map_msgs::GridMap::ConstPtr s = m.instantiate(); + + // std::cout << " frame = " << frame << std::endl; + + if (s != NULL){ + ++frame; + if (frame == n){ + std::cin.get(); + elevationMapCallback(*s); + elev_map_pub_.publish(*s); + std::cout << "Press [Enter] to continue to next gridmap message" << std::endl; + } + } + + geometry_msgs::PoseWithCovarianceStamped::ConstPtr i = m.instantiate(); + if (i !=NULL){ + if (frame == n){ + robotPoseCallBack(i); + pose_pub_.publish(*i); + } + } + } + + +bag.close(); +} diff --git a/plane_seg_ros/src/plane_seg_ros_node.cpp b/plane_seg_ros/src/plane_seg_ros_node.cpp index 36af384..11e88b0 100644 --- a/plane_seg_ros/src/plane_seg_ros_node.cpp +++ b/plane_seg_ros/src/plane_seg_ros_node.cpp @@ -56,6 +56,22 @@ int main( int argc, char** argv ){ exit(-1); } + bool run_nth_cloud = false; + nh.param("/plane_seg/run_nth_cloud", run_nth_cloud, false); + std::cout << "run_nth_cloud: " << run_nth_cloud << "\n"; + + // Enable this to run nth coud + if (run_nth_cloud){ + std::cout << "Running nth cloud\n"; + std::string filename_; + nh.getParam("rosbag_path", filename_); + int n_; + nh.getParam("n", n_); + app->extractNthCloud(filename_, n_); + std::cout << "Finshed!\n"; + exit(-1); + } + ROS_INFO_STREAM("Waiting for ROS messages"); ros::spin(); From efafa6b82f60cd947cfd9de9f4cd93f9452f25e2 Mon Sep 17 00:00:00 2001 From: Christos Ioannou Date: Thu, 21 Jan 2021 18:58:42 +0000 Subject: [PATCH 28/58] display gridmap as image and provide option to save and erode --- plane_seg_ros/CMakeLists.txt | 5 +- plane_seg_ros/include/plane_seg_ros/Pass.hpp | 6 ++ plane_seg_ros/src/plane_seg_ros.cpp | 79 +++++++++++++++++++- 3 files changed, 88 insertions(+), 2 deletions(-) diff --git a/plane_seg_ros/CMakeLists.txt b/plane_seg_ros/CMakeLists.txt index 3391bcd..49fe27b 100644 --- a/plane_seg_ros/CMakeLists.txt +++ b/plane_seg_ros/CMakeLists.txt @@ -16,7 +16,8 @@ find_package(catkin REQUIRED COMPONENTS plane_seg ) -find_package(OpenCV 3.0 QUIET) + +find_package(OpenCV 3.0 REQUIRED COMPONENTS core highgui imgproc imgcodecs) catkin_package(INCLUDE_DIRS include LIBRARIES ${PROJECT_NAME} @@ -40,11 +41,13 @@ add_library(${PROJECT_NAME} SHARED src/${PROJECT_NAME}.cpp src/Visualizer.cpp ) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) +target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBRARIES}) set(APP_NAME ${PROJECT_NAME}_node) add_executable(${APP_NAME} src/${APP_NAME}.cpp) target_link_libraries(${APP_NAME} ${PROJECT_NAME} ${catkin_LIBRARIES}) +target_link_libraries(${APP_NAME} ${PROJECT_NAME} ${OpenCV_LIBRARIES}) install(TARGETS ${PROJECT_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} diff --git a/plane_seg_ros/include/plane_seg_ros/Pass.hpp b/plane_seg_ros/include/plane_seg_ros/Pass.hpp index 088aead..5aad7b9 100644 --- a/plane_seg_ros/include/plane_seg_ros/Pass.hpp +++ b/plane_seg_ros/include/plane_seg_ros/Pass.hpp @@ -6,6 +6,7 @@ #include #include #include +#include #include #include #include @@ -41,6 +42,11 @@ class Pass{ void publishHullsAsMarkers(); void publishLineStrips(); void extractNthCloud(std::string filename, int n); + cv_bridge::CvImage convertToImg(const grid_map_msgs::GridMap &msg); + void displayImage(cv_bridge::CvImage image); + void displayProcessedImage(cv_bridge::CvImage image, std::string process); + cv_bridge::CvImage erodeImage(cv_bridge::CvImage originalImage); + void saveImage(cv_bridge::CvImage image); private: ros::NodeHandle node_; diff --git a/plane_seg_ros/src/plane_seg_ros.cpp b/plane_seg_ros/src/plane_seg_ros.cpp index 1ba8146..3d6f048 100644 --- a/plane_seg_ros/src/plane_seg_ros.cpp +++ b/plane_seg_ros/src/plane_seg_ros.cpp @@ -20,13 +20,20 @@ #include #include +#include #include #include #include +#include #include #include +#include +#include +#include +#include + #include "plane_seg/BlockFitter.hpp" #include "plane_seg/Tracker.hpp" #define foreach BOOST_FOREACH @@ -603,7 +610,29 @@ void Pass::extractNthCloud(std::string filename, int n){ std::cin.get(); elevationMapCallback(*s); elev_map_pub_.publish(*s); - std::cout << "Press [Enter] to continue to next gridmap message" << std::endl; + std::cout << 0 << std::endl; + cv_bridge::CvImage cv_img, erode_img; + std::cout << 1 << std::endl; + cv_img = convertToImg(*s); +// cv_bridge::CvImage img_rgb; +// cv::applyColorMap(image.image, img_rgb.image, cv::COLORMAP_JET); + displayImage(cv_img); + std::cout << 2 << std::endl; + std::cout << "Press 's' to save, 'e' to erode, anything else to close" << std::endl; + int l = cv::waitKey(0); + if (l == 's'){ + saveImage(cv_img); + } + else if (l == 'e'){ + erode_img = erodeImage(cv_img); + displayProcessedImage(erode_img, "erode"); + std::cout << "Press 's' to save both images, original then eroded" << std::endl; + int k = cv::waitKey(0); + if (k == 's'){ + saveImage(cv_img); + saveImage(erode_img); + } + } } } @@ -619,3 +648,51 @@ void Pass::extractNthCloud(std::string filename, int n){ bag.close(); } + +cv_bridge::CvImage Pass::convertToImg(const grid_map_msgs::GridMap &msg){ + grid_map::GridMap gridmap; + grid_map::GridMapRosConverter::fromMessage(msg, gridmap); + std::string layer; + cv_bridge::CvImage cv_img; + grid_map::GridMapRosConverter::toCvImage(gridmap, "elevation", sensor_msgs::image_encodings::MONO8, cv_img); + return cv_img; +} + + +void Pass::displayImage(cv_bridge::CvImage image_){ + cv::namedWindow("image", cv::WINDOW_AUTOSIZE); + cv::imshow("image", image_.image); +/* std::cout << "Press 's' to save image, anything else to close" << std::endl; + int k = cv::waitKey(0); + + if(k == 's') + { + saveImage(img_rgb); + }*/ +} + +void Pass::displayProcessedImage(cv_bridge::CvImage image, std::string process){ + cv::namedWindow(process, cv::WINDOW_AUTOSIZE); + cv::imshow(process, image.image); +} + + +void Pass::saveImage(cv_bridge::CvImage image_){ + std::string imagename; + std::cout << "Enter filename to save image (don't forget .png!): " << std::endl; + std::cin >> imagename; + cv::imwrite("/home/christos/rosbags/" + imagename, image_.image); +} + + +cv_bridge::CvImage Pass::erodeImage(cv_bridge::CvImage originalImage){ + cv_bridge::CvImage erodeImage; + int erode_size = 10; + cv::Mat element = cv::getStructuringElement(cv::MORPH_ELLIPSE, + cv::Size(2*erode_size + 1, 2*erode_size + 1), + cv::Point(erode_size, erode_size) + ); + cv::erode(originalImage.image, erodeImage.image, element); + return erodeImage; +} + From 12d8292520c58c0f6bbd8f31e1c47bd63c579480 Mon Sep 17 00:00:00 2001 From: Christos Ioannou Date: Fri, 22 Jan 2021 15:15:35 +0000 Subject: [PATCH 29/58] image processor class --- plane_seg/CMakeLists.txt | 4 +- .../include/plane_seg/ImageProcessor.hpp | 31 ++++++++++++ plane_seg/src/ImageProcessor.cpp | 41 ++++++++++++++++ plane_seg_ros/include/plane_seg_ros/Pass.hpp | 3 ++ plane_seg_ros/src/plane_seg_ros.cpp | 49 ++++++++++--------- 5 files changed, 104 insertions(+), 24 deletions(-) create mode 100644 plane_seg/include/plane_seg/ImageProcessor.hpp create mode 100644 plane_seg/src/ImageProcessor.cpp diff --git a/plane_seg/CMakeLists.txt b/plane_seg/CMakeLists.txt index 67aa163..fd9c04f 100644 --- a/plane_seg/CMakeLists.txt +++ b/plane_seg/CMakeLists.txt @@ -9,7 +9,7 @@ find_package(catkin REQUIRED COMPONENTS pcl_ros ) -find_package(OpenCV REQUIRED) +find_package(OpenCV REQUIRED COMPONENTS core highgui imgproc imgcodecs) catkin_package( INCLUDE_DIRS @@ -35,9 +35,11 @@ add_library(${LIB_NAME} SHARED src/RectangleFitter.cpp src/BlockFitter.cpp src/Tracker.cpp + src/ImageProcessor.cpp ) add_dependencies(plane_seg ${catkin_EXPORTED_TARGETS}) target_link_libraries(plane_seg ${catkin_LIBRARIES}) +target_link_libraries(plane_seg ${OpenCV_LIBRARIES}) # standalone lcm-based block fitter diff --git a/plane_seg/include/plane_seg/ImageProcessor.hpp b/plane_seg/include/plane_seg/ImageProcessor.hpp new file mode 100644 index 0000000..efe88a6 --- /dev/null +++ b/plane_seg/include/plane_seg/ImageProcessor.hpp @@ -0,0 +1,31 @@ +#ifndef _planeseg_ImageProcessor_hpp_ +#define _planeseg_ImageProcessor_hpp_ + +#include +#include +#include +#include +#include +#include + +namespace planeseg { + +class ImageProcessor { +public: + ImageProcessor(); + ~ImageProcessor(); + + void convertToImg(const grid_map_msgs::GridMap &msg); + void displayImage(cv_bridge::CvImage image, std::string process); + void saveImage(cv_bridge::CvImage image); + void erodeImage(cv_bridge::CvImage originalImage); + cv_bridge::CvImage original_img_; + cv_bridge::CvImage erode_img_; + +private: + +}; + +} //namespace planeseg + +#endif diff --git a/plane_seg/src/ImageProcessor.cpp b/plane_seg/src/ImageProcessor.cpp new file mode 100644 index 0000000..8a4095f --- /dev/null +++ b/plane_seg/src/ImageProcessor.cpp @@ -0,0 +1,41 @@ +#include +#include +#include + +namespace planeseg { + +ImageProcessor::ImageProcessor(){ +} + +ImageProcessor::~ImageProcessor(){} + +void ImageProcessor::convertToImg(const grid_map_msgs::GridMap &msg){ + grid_map::GridMap gridmap; + grid_map::GridMapRosConverter::fromMessage(msg, gridmap); + std::string layer; + grid_map::GridMapRosConverter::toCvImage(gridmap, "elevation", sensor_msgs::image_encodings::MONO8, original_img_); +} + +void ImageProcessor::displayImage(cv_bridge::CvImage image, std::string process){ + cv::namedWindow(process, cv::WINDOW_AUTOSIZE); + cv::imshow(process, image.image); +} + +void ImageProcessor::saveImage(cv_bridge::CvImage image_){ + std::string imagename; + std::cout << "Enter filename to save image (don't forget .png!): " << std::endl; + std::cin >> imagename; + cv::imwrite("/home/christos/rosbags/" + imagename, image_.image); +} + +void ImageProcessor::erodeImage(cv_bridge::CvImage originalImage){ + + int erode_size = 10; + cv::Mat element = cv::getStructuringElement(cv::MORPH_ELLIPSE, + cv::Size(2*erode_size + 1, 2*erode_size + 1), + cv::Point(erode_size, erode_size) + ); + cv::erode(originalImage.image, erode_img_.image, element); +} + +} //namespace planeseg diff --git a/plane_seg_ros/include/plane_seg_ros/Pass.hpp b/plane_seg_ros/include/plane_seg_ros/Pass.hpp index 5aad7b9..5e8b8ab 100644 --- a/plane_seg_ros/include/plane_seg_ros/Pass.hpp +++ b/plane_seg_ros/include/plane_seg_ros/Pass.hpp @@ -13,6 +13,7 @@ #include #include #include +#include namespace planeseg { @@ -43,6 +44,7 @@ class Pass{ void publishLineStrips(); void extractNthCloud(std::string filename, int n); cv_bridge::CvImage convertToImg(const grid_map_msgs::GridMap &msg); + void imageProcessingCallback(const grid_map_msgs::GridMap &msg); void displayImage(cv_bridge::CvImage image); void displayProcessedImage(cv_bridge::CvImage image, std::string process); cv_bridge::CvImage erodeImage(cv_bridge::CvImage originalImage); @@ -61,5 +63,6 @@ class Pass{ planeseg::BlockFitter::Result result_; planeseg::Tracker tracking_; planeseg::Visualizer visualizer_; + planeseg::ImageProcessor imgprocessor_; }; } diff --git a/plane_seg_ros/src/plane_seg_ros.cpp b/plane_seg_ros/src/plane_seg_ros.cpp index 3d6f048..22ca630 100644 --- a/plane_seg_ros/src/plane_seg_ros.cpp +++ b/plane_seg_ros/src/plane_seg_ros.cpp @@ -36,6 +36,8 @@ #include "plane_seg/BlockFitter.hpp" #include "plane_seg/Tracker.hpp" +#include "plane_seg/ImageProcessor.hpp" + #define foreach BOOST_FOREACH @@ -71,6 +73,7 @@ Pass::Pass(ros::NodeHandle node_): tracking_ = planeseg::Tracker(); visualizer_ = planeseg::Visualizer(); + imgprocessor_ = planeseg::ImageProcessor(); colors_ = { 1, 1, 1, // 42 @@ -610,29 +613,7 @@ void Pass::extractNthCloud(std::string filename, int n){ std::cin.get(); elevationMapCallback(*s); elev_map_pub_.publish(*s); - std::cout << 0 << std::endl; - cv_bridge::CvImage cv_img, erode_img; - std::cout << 1 << std::endl; - cv_img = convertToImg(*s); -// cv_bridge::CvImage img_rgb; -// cv::applyColorMap(image.image, img_rgb.image, cv::COLORMAP_JET); - displayImage(cv_img); - std::cout << 2 << std::endl; - std::cout << "Press 's' to save, 'e' to erode, anything else to close" << std::endl; - int l = cv::waitKey(0); - if (l == 's'){ - saveImage(cv_img); - } - else if (l == 'e'){ - erode_img = erodeImage(cv_img); - displayProcessedImage(erode_img, "erode"); - std::cout << "Press 's' to save both images, original then eroded" << std::endl; - int k = cv::waitKey(0); - if (k == 's'){ - saveImage(cv_img); - saveImage(erode_img); - } - } + imageProcessingCallback(*s); } } @@ -649,6 +630,28 @@ void Pass::extractNthCloud(std::string filename, int n){ bag.close(); } +void Pass::imageProcessingCallback(const grid_map_msgs::GridMap &msg){ + imgprocessor_.convertToImg(msg); +// cv_bridge::CvImage img_rgb; +// cv::applyColorMap(image.image, img_rgb.image, cv::COLORMAP_JET); + imgprocessor_.displayImage(imgprocessor_.original_img_, "original"); + std::cout << "Press 's' to save, 'e' to erode, anything else to close" << std::endl; + int l = cv::waitKey(0); + if (l == 's'){ + imgprocessor_.saveImage(imgprocessor_.original_img_); + } + else if (l == 'e'){ + imgprocessor_.erodeImage(imgprocessor_.original_img_); + imgprocessor_.displayImage(imgprocessor_.erode_img_, "erode"); + std::cout << "Press 's' to save both images, original then eroded" << std::endl; + int k = cv::waitKey(0); + if (k == 's'){ + imgprocessor_.saveImage(imgprocessor_.original_img_); + imgprocessor_.saveImage(imgprocessor_.erode_img_); + } + } +} + cv_bridge::CvImage Pass::convertToImg(const grid_map_msgs::GridMap &msg){ grid_map::GridMap gridmap; grid_map::GridMapRosConverter::fromMessage(msg, gridmap); From 6651c6a4d759cb4ac9c55a334b90a212e2b0758c Mon Sep 17 00:00:00 2001 From: Christos Ioannou Date: Fri, 22 Jan 2021 16:24:11 +0000 Subject: [PATCH 30/58] cleaning up includes --- .../include/plane_seg/ImageProcessor.hpp | 6 +++- plane_seg_ros/include/plane_seg_ros/Pass.hpp | 13 +++---- plane_seg_ros/src/plane_seg_ros.cpp | 34 +++++++------------ 3 files changed, 24 insertions(+), 29 deletions(-) diff --git a/plane_seg/include/plane_seg/ImageProcessor.hpp b/plane_seg/include/plane_seg/ImageProcessor.hpp index efe88a6..a457c13 100644 --- a/plane_seg/include/plane_seg/ImageProcessor.hpp +++ b/plane_seg/include/plane_seg/ImageProcessor.hpp @@ -1,12 +1,16 @@ #ifndef _planeseg_ImageProcessor_hpp_ #define _planeseg_ImageProcessor_hpp_ + +#include +#include +#include #include #include #include #include #include -#include +#include namespace planeseg { diff --git a/plane_seg_ros/include/plane_seg_ros/Pass.hpp b/plane_seg_ros/include/plane_seg_ros/Pass.hpp index 5e8b8ab..d5e8a42 100644 --- a/plane_seg_ros/include/plane_seg_ros/Pass.hpp +++ b/plane_seg_ros/include/plane_seg_ros/Pass.hpp @@ -27,6 +27,7 @@ class Pass{ void elevationMapCallback(const grid_map_msgs::GridMap& msg); void pointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr &msg); void robotPoseCallBack(const geometry_msgs::PoseWithCovarianceStampedConstPtr &msg); + void imageProcessingCallback(const grid_map_msgs::GridMap &msg); void processCloud(planeseg::LabeledCloud::Ptr& inCloud, Eigen::Vector3f origin, Eigen::Vector3f lookDir); void processFromFile(int test_example); @@ -43,12 +44,12 @@ class Pass{ void publishHullsAsMarkers(); void publishLineStrips(); void extractNthCloud(std::string filename, int n); - cv_bridge::CvImage convertToImg(const grid_map_msgs::GridMap &msg); - void imageProcessingCallback(const grid_map_msgs::GridMap &msg); - void displayImage(cv_bridge::CvImage image); - void displayProcessedImage(cv_bridge::CvImage image, std::string process); - cv_bridge::CvImage erodeImage(cv_bridge::CvImage originalImage); - void saveImage(cv_bridge::CvImage image); +// cv_bridge::CvImage convertToImg(const grid_map_msgs::GridMap &msg); +// void imageProcessingCallback(const grid_map_msgs::GridMap &msg); +// void displayImage(cv_bridge::CvImage image); +// void displayProcessedImage(cv_bridge::CvImage image, std::string process); +// cv_bridge::CvImage erodeImage(cv_bridge::CvImage originalImage); +// void saveImage(cv_bridge::CvImage image); private: ros::NodeHandle node_; diff --git a/plane_seg_ros/src/plane_seg_ros.cpp b/plane_seg_ros/src/plane_seg_ros.cpp index 22ca630..0a19554 100644 --- a/plane_seg_ros/src/plane_seg_ros.cpp +++ b/plane_seg_ros/src/plane_seg_ros.cpp @@ -1,6 +1,3 @@ -#include "plane_seg_ros/Pass.hpp" -#include "plane_seg_ros/geometry_utils.hpp" - #include #include #include @@ -9,6 +6,10 @@ #include #include +#include "plane_seg/ImageProcessor.hpp" +#include "plane_seg/BlockFitter.hpp" +#include "plane_seg/Tracker.hpp" + #include #include #include @@ -24,19 +25,13 @@ #include #include #include -#include #include #include -#include -#include -#include -#include +#include "plane_seg_ros/Pass.hpp" +#include "plane_seg_ros/geometry_utils.hpp" -#include "plane_seg/BlockFitter.hpp" -#include "plane_seg/Tracker.hpp" -#include "plane_seg/ImageProcessor.hpp" #define foreach BOOST_FOREACH @@ -652,6 +647,7 @@ void Pass::imageProcessingCallback(const grid_map_msgs::GridMap &msg){ } } +/* cv_bridge::CvImage Pass::convertToImg(const grid_map_msgs::GridMap &msg){ grid_map::GridMap gridmap; grid_map::GridMapRosConverter::fromMessage(msg, gridmap); @@ -660,20 +656,14 @@ cv_bridge::CvImage Pass::convertToImg(const grid_map_msgs::GridMap &msg){ grid_map::GridMapRosConverter::toCvImage(gridmap, "elevation", sensor_msgs::image_encodings::MONO8, cv_img); return cv_img; } - - +*/ +/* void Pass::displayImage(cv_bridge::CvImage image_){ cv::namedWindow("image", cv::WINDOW_AUTOSIZE); cv::imshow("image", image_.image); -/* std::cout << "Press 's' to save image, anything else to close" << std::endl; - int k = cv::waitKey(0); - - if(k == 's') - { - saveImage(img_rgb); - }*/ } - +*/ +/* void Pass::displayProcessedImage(cv_bridge::CvImage image, std::string process){ cv::namedWindow(process, cv::WINDOW_AUTOSIZE); cv::imshow(process, image.image); @@ -698,4 +688,4 @@ cv_bridge::CvImage Pass::erodeImage(cv_bridge::CvImage originalImage){ cv::erode(originalImage.image, erodeImage.image, element); return erodeImage; } - +*/ From fe50fcaa4e38c0518f1ed595613da66c49bc9cd3 Mon Sep 17 00:00:00 2001 From: Marco Camurri Date: Fri, 22 Jan 2021 20:45:21 +0000 Subject: [PATCH 31/58] cleanup includes so it compiles --- plane_seg/CMakeLists.txt | 2 +- plane_seg/include/plane_seg/ImageProcessor.hpp | 11 +---------- plane_seg/src/ImageProcessor.cpp | 5 +++-- plane_seg_ros/src/plane_seg_ros.cpp | 2 ++ 4 files changed, 7 insertions(+), 13 deletions(-) diff --git a/plane_seg/CMakeLists.txt b/plane_seg/CMakeLists.txt index fd9c04f..3cbde39 100644 --- a/plane_seg/CMakeLists.txt +++ b/plane_seg/CMakeLists.txt @@ -6,7 +6,7 @@ add_compile_options(-std=c++1y -Wall) #add_definitions(-std=c++1y -Wall) find_package(catkin REQUIRED COMPONENTS - pcl_ros + pcl_ros grid_map_ros ) find_package(OpenCV REQUIRED COMPONENTS core highgui imgproc imgcodecs) diff --git a/plane_seg/include/plane_seg/ImageProcessor.hpp b/plane_seg/include/plane_seg/ImageProcessor.hpp index a457c13..22d1e09 100644 --- a/plane_seg/include/plane_seg/ImageProcessor.hpp +++ b/plane_seg/include/plane_seg/ImageProcessor.hpp @@ -1,16 +1,7 @@ #ifndef _planeseg_ImageProcessor_hpp_ #define _planeseg_ImageProcessor_hpp_ - - -#include -#include -#include #include -#include -#include -#include -#include -#include +#include namespace planeseg { diff --git a/plane_seg/src/ImageProcessor.cpp b/plane_seg/src/ImageProcessor.cpp index 8a4095f..432bfd9 100644 --- a/plane_seg/src/ImageProcessor.cpp +++ b/plane_seg/src/ImageProcessor.cpp @@ -1,6 +1,7 @@ -#include +#include "plane_seg/ImageProcessor.hpp" +#include #include -#include +#include namespace planeseg { diff --git a/plane_seg_ros/src/plane_seg_ros.cpp b/plane_seg_ros/src/plane_seg_ros.cpp index 0a19554..3742118 100644 --- a/plane_seg_ros/src/plane_seg_ros.cpp +++ b/plane_seg_ros/src/plane_seg_ros.cpp @@ -26,6 +26,8 @@ #include #include +#include + #include #include From 7d10e5bef4b47194508bb5399b8487aa994972ca Mon Sep 17 00:00:00 2001 From: Christos Ioannou Date: Sat, 23 Jan 2021 16:59:25 +0000 Subject: [PATCH 32/58] removed plane_seg dependency on ros and full include cleanup --- plane_seg/CMakeLists.txt | 2 +- .../include/plane_seg/ImageProcessor.hpp | 7 +-- plane_seg/include/plane_seg/Tracker.hpp | 8 +-- plane_seg/src/ImageProcessor.cpp | 25 +++++--- plane_seg_ros/include/plane_seg_ros/Pass.hpp | 9 --- .../include/plane_seg_ros/Visualizer.hpp | 7 +-- plane_seg_ros/src/plane_seg_ros.cpp | 61 ++----------------- 7 files changed, 28 insertions(+), 91 deletions(-) diff --git a/plane_seg/CMakeLists.txt b/plane_seg/CMakeLists.txt index 3cbde39..fd9c04f 100644 --- a/plane_seg/CMakeLists.txt +++ b/plane_seg/CMakeLists.txt @@ -6,7 +6,7 @@ add_compile_options(-std=c++1y -Wall) #add_definitions(-std=c++1y -Wall) find_package(catkin REQUIRED COMPONENTS - pcl_ros grid_map_ros + pcl_ros ) find_package(OpenCV REQUIRED COMPONENTS core highgui imgproc imgcodecs) diff --git a/plane_seg/include/plane_seg/ImageProcessor.hpp b/plane_seg/include/plane_seg/ImageProcessor.hpp index 22d1e09..4df28e1 100644 --- a/plane_seg/include/plane_seg/ImageProcessor.hpp +++ b/plane_seg/include/plane_seg/ImageProcessor.hpp @@ -1,6 +1,4 @@ -#ifndef _planeseg_ImageProcessor_hpp_ -#define _planeseg_ImageProcessor_hpp_ -#include +#pragma once #include namespace planeseg { @@ -10,7 +8,7 @@ class ImageProcessor { ImageProcessor(); ~ImageProcessor(); - void convertToImg(const grid_map_msgs::GridMap &msg); + void process(); void displayImage(cv_bridge::CvImage image, std::string process); void saveImage(cv_bridge::CvImage image); void erodeImage(cv_bridge::CvImage originalImage); @@ -23,4 +21,3 @@ class ImageProcessor { } //namespace planeseg -#endif diff --git a/plane_seg/include/plane_seg/Tracker.hpp b/plane_seg/include/plane_seg/Tracker.hpp index bb5f9f2..bc31f58 100644 --- a/plane_seg/include/plane_seg/Tracker.hpp +++ b/plane_seg/include/plane_seg/Tracker.hpp @@ -1,9 +1,6 @@ -#ifndef _planeseg_Tracker_hpp_ -#define _planeseg_Tracker_hpp_ - +#pragma once #include #include -#include #include "plane_seg/BlockFitter.hpp" namespace planeseg { @@ -39,6 +36,5 @@ class Tracker { std::vector::Ptr> cloud_ptrs; int totalIds; }; -} -#endif +} // namespace planeseg diff --git a/plane_seg/src/ImageProcessor.cpp b/plane_seg/src/ImageProcessor.cpp index 432bfd9..341a32b 100644 --- a/plane_seg/src/ImageProcessor.cpp +++ b/plane_seg/src/ImageProcessor.cpp @@ -1,6 +1,4 @@ #include "plane_seg/ImageProcessor.hpp" -#include -#include #include namespace planeseg { @@ -10,13 +8,26 @@ ImageProcessor::ImageProcessor(){ ImageProcessor::~ImageProcessor(){} -void ImageProcessor::convertToImg(const grid_map_msgs::GridMap &msg){ - grid_map::GridMap gridmap; - grid_map::GridMapRosConverter::fromMessage(msg, gridmap); - std::string layer; - grid_map::GridMapRosConverter::toCvImage(gridmap, "elevation", sensor_msgs::image_encodings::MONO8, original_img_); +void ImageProcessor::process(){ + displayImage(original_img_, "original"); + std::cout << "Press 's' to save, 'e' to erode, anything else to close" << std::endl; + int l = cv::waitKey(0); + if (l == 's'){ + saveImage(original_img_); + } + else if (l == 'e'){ + erodeImage(original_img_); + displayImage(erode_img_, "erode"); + std::cout << "Press 's' to save both images, original then eroded" << std::endl; + int k = cv::waitKey(0); + if (k == 's'){ + saveImage(original_img_); + saveImage(erode_img_); + } + } } + void ImageProcessor::displayImage(cv_bridge::CvImage image, std::string process){ cv::namedWindow(process, cv::WINDOW_AUTOSIZE); cv::imshow(process, image.image); diff --git a/plane_seg_ros/include/plane_seg_ros/Pass.hpp b/plane_seg_ros/include/plane_seg_ros/Pass.hpp index d5e8a42..aaad608 100644 --- a/plane_seg_ros/include/plane_seg_ros/Pass.hpp +++ b/plane_seg_ros/include/plane_seg_ros/Pass.hpp @@ -6,11 +6,8 @@ #include #include #include -#include #include #include -#include -#include #include #include #include @@ -44,12 +41,6 @@ class Pass{ void publishHullsAsMarkers(); void publishLineStrips(); void extractNthCloud(std::string filename, int n); -// cv_bridge::CvImage convertToImg(const grid_map_msgs::GridMap &msg); -// void imageProcessingCallback(const grid_map_msgs::GridMap &msg); -// void displayImage(cv_bridge::CvImage image); -// void displayProcessedImage(cv_bridge::CvImage image, std::string process); -// cv_bridge::CvImage erodeImage(cv_bridge::CvImage originalImage); -// void saveImage(cv_bridge::CvImage image); private: ros::NodeHandle node_; diff --git a/plane_seg_ros/include/plane_seg_ros/Visualizer.hpp b/plane_seg_ros/include/plane_seg_ros/Visualizer.hpp index 4d9f372..21f2905 100644 --- a/plane_seg_ros/include/plane_seg_ros/Visualizer.hpp +++ b/plane_seg_ros/include/plane_seg_ros/Visualizer.hpp @@ -1,11 +1,6 @@ -#ifndef _planeseg_Visualizer_hpp_ -#define _planeseg_Visualizer_hpp_ -#endif - +#pragma once #include -#include #include -#include #include "plane_seg/Tracker.hpp" namespace planeseg{ diff --git a/plane_seg_ros/src/plane_seg_ros.cpp b/plane_seg_ros/src/plane_seg_ros.cpp index 3742118..b0d634f 100644 --- a/plane_seg_ros/src/plane_seg_ros.cpp +++ b/plane_seg_ros/src/plane_seg_ros.cpp @@ -628,66 +628,13 @@ bag.close(); } void Pass::imageProcessingCallback(const grid_map_msgs::GridMap &msg){ - imgprocessor_.convertToImg(msg); -// cv_bridge::CvImage img_rgb; -// cv::applyColorMap(image.image, img_rgb.image, cv::COLORMAP_JET); - imgprocessor_.displayImage(imgprocessor_.original_img_, "original"); - std::cout << "Press 's' to save, 'e' to erode, anything else to close" << std::endl; - int l = cv::waitKey(0); - if (l == 's'){ - imgprocessor_.saveImage(imgprocessor_.original_img_); - } - else if (l == 'e'){ - imgprocessor_.erodeImage(imgprocessor_.original_img_); - imgprocessor_.displayImage(imgprocessor_.erode_img_, "erode"); - std::cout << "Press 's' to save both images, original then eroded" << std::endl; - int k = cv::waitKey(0); - if (k == 's'){ - imgprocessor_.saveImage(imgprocessor_.original_img_); - imgprocessor_.saveImage(imgprocessor_.erode_img_); - } - } -} - -/* -cv_bridge::CvImage Pass::convertToImg(const grid_map_msgs::GridMap &msg){ grid_map::GridMap gridmap; grid_map::GridMapRosConverter::fromMessage(msg, gridmap); - std::string layer; - cv_bridge::CvImage cv_img; - grid_map::GridMapRosConverter::toCvImage(gridmap, "elevation", sensor_msgs::image_encodings::MONO8, cv_img); - return cv_img; -} -*/ -/* -void Pass::displayImage(cv_bridge::CvImage image_){ - cv::namedWindow("image", cv::WINDOW_AUTOSIZE); - cv::imshow("image", image_.image); -} -*/ -/* -void Pass::displayProcessedImage(cv_bridge::CvImage image, std::string process){ - cv::namedWindow(process, cv::WINDOW_AUTOSIZE); - cv::imshow(process, image.image); -} + grid_map::GridMapRosConverter::toCvImage(gridmap, "elevation", sensor_msgs::image_encodings::MONO8, imgprocessor_.original_img_); +// cv_bridge::CvImage img_rgb; +// cv::applyColorMap(image.image, img_rgb.image, cv::COLORMAP_JET); -void Pass::saveImage(cv_bridge::CvImage image_){ - std::string imagename; - std::cout << "Enter filename to save image (don't forget .png!): " << std::endl; - std::cin >> imagename; - cv::imwrite("/home/christos/rosbags/" + imagename, image_.image); -} - + imgprocessor_.process(); -cv_bridge::CvImage Pass::erodeImage(cv_bridge::CvImage originalImage){ - cv_bridge::CvImage erodeImage; - int erode_size = 10; - cv::Mat element = cv::getStructuringElement(cv::MORPH_ELLIPSE, - cv::Size(2*erode_size + 1, 2*erode_size + 1), - cv::Point(erode_size, erode_size) - ); - cv::erode(originalImage.image, erodeImage.image, element); - return erodeImage; } -*/ From e39b3d5ca248f88dadc55dea6dfa59ac5acf4910 Mon Sep 17 00:00:00 2001 From: Christos Ioannou Date: Mon, 25 Jan 2021 23:35:40 +0000 Subject: [PATCH 33/58] integration of filter chain --- plane_seg_ros/config/nth_cloud.yaml | 130 ++++++++++++++++++ plane_seg_ros/include/plane_seg_ros/Pass.hpp | 20 ++- plane_seg_ros/launch/nth_cloud.launch | 10 +- plane_seg_ros/src/plane_seg_ros.cpp | 136 ++++++++++++++++++- 4 files changed, 290 insertions(+), 6 deletions(-) diff --git a/plane_seg_ros/config/nth_cloud.yaml b/plane_seg_ros/config/nth_cloud.yaml index 861b385..fc1ee07 100644 --- a/plane_seg_ros/config/nth_cloud.yaml +++ b/plane_seg_ros/config/nth_cloud.yaml @@ -2,3 +2,133 @@ plane_seg: run_test_program: false run_sequential_test: false run_nth_cloud: true + +grid_map_filters: + + - name: buffer_normalizer + type: gridMapFilters/BufferNormalizerFilter + + # # Duplicate layer. + # - name: duplicate + # type: gridMapFilters/DuplicationFilter + # params: + # input_layer: ... + # output_layer: ... + + # Delete color layer. + - name: delete_original_layers + type: gridMapFilters/DeletionFilter + params: + layers: [color] # List of layers. + + # Fill holes in the map with inpainting. + - name: inpaint + type: gridMapCv/InpaintFilter + params: + input_layer: elevation + output_layer: elevation_inpainted + radius: 0.05 + + # Reduce noise with a radial blurring filter. + - name: mean_in_radius + type: gridMapFilters/MeanInRadiusFilter + params: + input_layer: elevation_inpainted + output_layer: elevation_smooth + radius: 0.06 + + # Boxblur as an alternative to the inpaint and radial blurring filter above. + # - name: boxblur + # type: gridMapFilters/SlidingWindowMathExpressionFilter + # params: + # input_layer: elevation + # output_layer: elevation_smooth + # expression: meanOfFinites(elevation) + # compute_empty_cells: true + # edge_handling: crop # options: inside, crop, empty, mean + # window_size: 5 # optional + + # Compute surface normals. + - name: surface_normals + type: gridMapFilters/NormalVectorsFilter + params: + input_layer: elevation_inpainted + output_layers_prefix: normal_vectors_ + radius: 0.05 + normal_vector_positive_axis: z + + # Add a color layer for visualization based on the surface normal. + - name: normal_color_map + type: gridMapFilters/NormalColorMapFilter + params: + input_layers_prefix: normal_vectors_ + output_layer: normal_color + + # Compute slope from surface normal. + - name: slope + type: gridMapFilters/MathExpressionFilter + params: + output_layer: slope + expression: acos(normal_vectors_z) + + # Compute roughness as absolute difference from map to smoothened map. + - name: roughness + type: gridMapFilters/MathExpressionFilter + params: + output_layer: roughness + expression: abs(elevation_inpainted - elevation_smooth) + + # Edge detection by computing the standard deviation from slope. + - name: edge_detection + type: gridMapFilters/SlidingWindowMathExpressionFilter + params: + input_layer: slope + output_layer: edges + expression: sqrt(sumOfFinites(square(slope - meanOfFinites(slope))) ./ numberOfFinites(slope)) # Standard deviation + compute_empty_cells: false + edge_handling: crop # options: inside, crop, empty, mean + window_length: 0.05 + + # # Edge detection on elevation layer with convolution filter as alternative to filter above. + # - name: edge_detection + # type: gridMapFilters/SlidingWindowMathExpressionFilter + # params: + # input_layer: elevation_inpainted + # output_layer: edges + # expression: 'sumOfFinites([0,1,0;1,-4,1;0,1,0].*elevation_inpainted)' # Edge detection. + # # expression: 'sumOfFinites([0,-1,0;-1,5,-1;0,-1,0].*elevation_inpainted)' # Sharpen. + # compute_empty_cells: false + # edge_handling: mean # options: inside, crop, empty, mean + # window_size: 3 # Make sure to make this compatible with the kernel matrix. + + # Compute traversability as normalized weighted sum of slope and roughness. + - name: traversability + type: gridMapFilters/MathExpressionFilter + params: + output_layer: traversability + expression: 0.5 * (1.0 - (slope / 0.6)) + 0.5 * (1.0 - (roughness / 0.1)) + + # Set lower threshold on traversability. + - name: traversability_lower_threshold + type: gridMapFilters/ThresholdFilter + params: + condition_layer: traversability + output_layer: traversability + lower_threshold: 0.0 + set_to: 0.0 + + # Set upper threshold on traversability. + - name: traversability_upper_threshold + type: gridMapFilters/ThresholdFilter + params: + condition_layer: traversability + output_layer: traversability + upper_threshold: 1.0 + set_to: 1.0 # Other uses: .nan, .inf + + # Duplicate layer. + - name: traversability_clean + type: gridMapFilters/DuplicationFilter + params: + input_layer: traversability + output_layer: traversability_clean diff --git a/plane_seg_ros/include/plane_seg_ros/Pass.hpp b/plane_seg_ros/include/plane_seg_ros/Pass.hpp index aaad608..d10b785 100644 --- a/plane_seg_ros/include/plane_seg_ros/Pass.hpp +++ b/plane_seg_ros/include/plane_seg_ros/Pass.hpp @@ -11,6 +11,11 @@ #include #include #include +#include +#include +#include +#include +#include namespace planeseg { @@ -21,6 +26,7 @@ class Pass{ ~Pass(){ } + bool loadParameters(); void elevationMapCallback(const grid_map_msgs::GridMap& msg); void pointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr &msg); void robotPoseCallBack(const geometry_msgs::PoseWithCovarianceStampedConstPtr &msg); @@ -41,6 +47,9 @@ class Pass{ void publishHullsAsMarkers(); void publishLineStrips(); void extractNthCloud(std::string filename, int n); + void tic(); + std::chrono::duration toc(); + void gridMapCallback(const grid_map_msgs::GridMap& msg); private: ros::NodeHandle node_; @@ -49,12 +58,21 @@ class Pass{ std::vector colors_3; ros::Subscriber point_cloud_sub_, grid_map_sub_, pose_sub_; - ros::Publisher received_cloud_pub_, hull_cloud_pub_, hull_markers_pub_, look_pose_pub_, elev_map_pub_, pose_pub_, id_strings_pub_, centroids_pub_, hulls_pub_, linestrips_pub_; + ros::Publisher received_cloud_pub_, hull_cloud_pub_, hull_markers_pub_, look_pose_pub_, elev_map_pub_, pose_pub_, id_strings_pub_, centroids_pub_, hulls_pub_, linestrips_pub_, filtered_map_pub_; Eigen::Isometry3d last_robot_pose_; planeseg::BlockFitter::Result result_; planeseg::Tracker tracking_; planeseg::Visualizer visualizer_; planeseg::ImageProcessor imgprocessor_; + filters::FilterChain filter_chain_; + tf::TransformListener* listener_; + std::string elevation_map_topic_; + std::string filter_chain_parameters_name_; + std::string filtered_map_topic_; + double erode_radius_; + double traversability_threshold_; + bool verbose_timer_; + std::chrono::high_resolution_clock::time_point last_time_; }; } diff --git a/plane_seg_ros/launch/nth_cloud.launch b/plane_seg_ros/launch/nth_cloud.launch index 1cadef9..4af265d 100644 --- a/plane_seg_ros/launch/nth_cloud.launch +++ b/plane_seg_ros/launch/nth_cloud.launch @@ -6,9 +6,15 @@ - -> + + + + + + + +> diff --git a/plane_seg_ros/src/plane_seg_ros.cpp b/plane_seg_ros/src/plane_seg_ros.cpp index b0d634f..f64fc01 100644 --- a/plane_seg_ros/src/plane_seg_ros.cpp +++ b/plane_seg_ros/src/plane_seg_ros.cpp @@ -10,6 +10,8 @@ #include "plane_seg/BlockFitter.hpp" #include "plane_seg/Tracker.hpp" +#include + #include #include #include @@ -41,7 +43,9 @@ using namespace planeseg; Pass::Pass(ros::NodeHandle node_): - node_(node_){ + node_(node_), + filter_chain_("grid_map::GridMap") +{ std::string input_body_pose_topic; node_.getParam("input_body_pose_topic", input_body_pose_topic); @@ -55,6 +59,7 @@ Pass::Pass(ros::NodeHandle node_): pose_sub_ = node_.subscribe("/state_estimator/pose_in_odom", 100, &Pass::robotPoseCallBack, this); */ + received_cloud_pub_ = node_.advertise("/plane_seg/received_cloud", 10); hull_cloud_pub_ = node_.advertise("/plane_seg/hull_cloud", 10); hull_markers_pub_ = node_.advertise("/plane_seg/hull_markers", 10); @@ -65,12 +70,27 @@ Pass::Pass(ros::NodeHandle node_): centroids_pub_ = node_.advertise("/plane_seg/centroids", 10); hulls_pub_ = node_.advertise("/plane_seg/hulls", 10); linestrips_pub_ = node_.advertise("/plane_seg/linestrips", 10); + filtered_map_pub_ = node_.advertise("/rooster_elevation_mapping/filtered_map", 1, true); last_robot_pose_ = Eigen::Isometry3d::Identity(); tracking_ = planeseg::Tracker(); visualizer_ = planeseg::Visualizer(); imgprocessor_ = planeseg::ImageProcessor(); + listener_ = new tf::TransformListener(); + + node_.param("input_topic", elevation_map_topic_, std::string("/rooster_elevation_mapping/elevation_map")); + node_.param("erode_radius", erode_radius_, 0.2); + ROS_INFO("Erode Radius [%f]", erode_radius_); + node_.param("traversability_threshold", traversability_threshold_, 0.8); + ROS_INFO("traversability_threshold [%f]", traversability_threshold_); + node_.param("verbose_timer", verbose_timer_, true); + + // Setup filter chain + if (!filter_chain_.configure("grid_map_filters", node_)){ + std::cout << "couldn't configure filter chain" << std::endl; + return; + } colors_ = { 1, 1, 1, // 42 @@ -608,9 +628,11 @@ void Pass::extractNthCloud(std::string filename, int n){ ++frame; if (frame == n){ std::cin.get(); + grid_map_msgs::GridMap n; + gridMapCallback(*s); elevationMapCallback(*s); - elev_map_pub_.publish(*s); - imageProcessingCallback(*s); +// elev_map_pub_.publish(*s); +// imageProcessingCallback(*s); } } @@ -638,3 +660,111 @@ void Pass::imageProcessingCallback(const grid_map_msgs::GridMap &msg){ imgprocessor_.process(); } + +void Pass::tic(){ + last_time_ = std::chrono::high_resolution_clock::now(); +} + +std::chrono::duration Pass::toc(){ + auto now_time = std::chrono::high_resolution_clock::now(); + std::chrono::duration elapsed_time = std::chrono::duration_cast(now_time - last_time_); + last_time_ = now_time; + // std::cout << elapsedTime.count() << "ms elapsed" << std::endl; + return elapsed_time; +} + +void Pass::gridMapCallback(const grid_map_msgs::GridMap& msg){ + tic(); + + // Convert message to map. + grid_map::GridMap input_map; + grid_map::GridMapRosConverter::fromMessage(msg, input_map); + + // Apply filter chain. + grid_map::GridMap output_map; + if (!filter_chain_.update(input_map, output_map)) { + std::cout << "couldn't update the grid map filter chain" << std::endl; +// grid_map_msgs::GridMap failmessage; +// grid_map::GridMapRosConverter::toMessage(input_map, failmessage); + return; + } + + if (verbose_timer_) { + std::cout << toc().count() << " ms: filter chain\n"; + } + + tic(); + + Eigen::Isometry3d pose_robot = Eigen::Isometry3d::Identity(); + tf::StampedTransform transform; + try { + listener_->waitForTransform("/odom", "/base", ros::Time(0), ros::Duration(10.0) ); + listener_->lookupTransform("/odom", "/base", ros::Time(0), transform); + } catch (tf::TransformException ex) { + ROS_ERROR("%s",ex.what()); + } + + tf::transformTFToEigen(transform, pose_robot); + + // Threshold traversability conservatively + grid_map::Matrix& data = output_map["traversability_clean"]; + for (grid_map::GridMapIterator iterator(output_map); !iterator.isPastEnd(); ++iterator){ + const grid_map::Index index(*iterator); + + // make cells very near robot traversable + grid_map::Position pos_cell; + output_map.getPosition(index, pos_cell); + grid_map::Position pos_robot(pose_robot.translation().head(2)); + double dist = (pos_robot - pos_cell).norm(); + if (dist <1.0){ + data(index(0), index(1)) = 1.0; + } + + if (data(index(0), index(1)) < traversability_threshold_){ + data(index(0), index(1)) =0.0; + }else{ + data(index(0), index(1)) =1.0; + } + } + + // Erode the traversable image + cv::Mat original_image, erode_image; + grid_map::GridMapCvConverter::toImage(output_map, "traversability_clean", CV_16UC1, 0.0, 0.3, original_image); + //std::cout << "write original\n"; + //cv::imwrite( "original_image.png", original_image); + + int erode_size = int(floor(erode_radius_ / msg.info.resolution)); // was 25 for conservative + cv::Mat element = cv::getStructuringElement(cv::MORPH_ELLIPSE, + cv::Size(2*erode_size + 1, 2*erode_size + 1), + cv::Point(erode_size, erode_size)); + /// Apply the dilation operation + cv::erode(original_image, erode_image, element); + //std::cout << "write eroded\n"; + //cv::imwrite( "erode_image.png", erode_image); + grid_map::GridMapCvConverter::addLayerFromImage(erode_image, "traversability_clean_eroded", output_map, 0.0, 0.3); + + if (verbose_timer_) { + std::cout << toc().count() << " ms: traversability edition (i.e. erosion)\n"; + } + + // Publish filtered output grid map. + grid_map_msgs::GridMap output_msg; + grid_map::GridMapRosConverter::toMessage(output_map, output_msg); + filtered_map_pub_.publish(output_msg); + + std::vector outlayers, inlayers; + outlayers = output_map.getLayers(); + inlayers = input_map.getLayers(); + std::cout << "Input map layers: " << std::endl; + for (size_t i=0; i Date: Mon, 25 Jan 2021 23:39:10 +0000 Subject: [PATCH 34/58] pass filtered grid_map to elevationMapCallback --- plane_seg_ros/include/plane_seg_ros/Pass.hpp | 2 +- plane_seg_ros/src/plane_seg_ros.cpp | 14 +++++++------- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/plane_seg_ros/include/plane_seg_ros/Pass.hpp b/plane_seg_ros/include/plane_seg_ros/Pass.hpp index d10b785..b7859f2 100644 --- a/plane_seg_ros/include/plane_seg_ros/Pass.hpp +++ b/plane_seg_ros/include/plane_seg_ros/Pass.hpp @@ -49,7 +49,7 @@ class Pass{ void extractNthCloud(std::string filename, int n); void tic(); std::chrono::duration toc(); - void gridMapCallback(const grid_map_msgs::GridMap& msg); + grid_map_msgs::GridMap gridMapCallback(const grid_map_msgs::GridMap& msg); private: ros::NodeHandle node_; diff --git a/plane_seg_ros/src/plane_seg_ros.cpp b/plane_seg_ros/src/plane_seg_ros.cpp index f64fc01..5b283ae 100644 --- a/plane_seg_ros/src/plane_seg_ros.cpp +++ b/plane_seg_ros/src/plane_seg_ros.cpp @@ -629,8 +629,8 @@ void Pass::extractNthCloud(std::string filename, int n){ if (frame == n){ std::cin.get(); grid_map_msgs::GridMap n; - gridMapCallback(*s); - elevationMapCallback(*s); + n = gridMapCallback(*s); + elevationMapCallback(n); // elev_map_pub_.publish(*s); // imageProcessingCallback(*s); } @@ -673,7 +673,7 @@ std::chrono::duration Pass::toc(){ return elapsed_time; } -void Pass::gridMapCallback(const grid_map_msgs::GridMap& msg){ +grid_map_msgs::GridMap Pass::gridMapCallback(const grid_map_msgs::GridMap& msg){ tic(); // Convert message to map. @@ -684,9 +684,9 @@ void Pass::gridMapCallback(const grid_map_msgs::GridMap& msg){ grid_map::GridMap output_map; if (!filter_chain_.update(input_map, output_map)) { std::cout << "couldn't update the grid map filter chain" << std::endl; -// grid_map_msgs::GridMap failmessage; -// grid_map::GridMapRosConverter::toMessage(input_map, failmessage); - return; + grid_map_msgs::GridMap failmessage; + grid_map::GridMapRosConverter::toMessage(input_map, failmessage); + return failmessage; } if (verbose_timer_) { @@ -765,6 +765,6 @@ void Pass::gridMapCallback(const grid_map_msgs::GridMap& msg){ } - return; + return output_msg; } From d4a8e9953ce272373da2c3e42eb5a937e0d178de Mon Sep 17 00:00:00 2001 From: Christos Ioannou Date: Tue, 26 Jan 2021 13:48:23 +0000 Subject: [PATCH 35/58] move rviz config for filters to plane_seg_ros --- .../config/elevation_map_filters_config.rviz | 281 ++++++++++++++++++ plane_seg_ros/launch/nth_cloud.launch | 2 +- 2 files changed, 282 insertions(+), 1 deletion(-) create mode 100644 plane_seg_ros/config/elevation_map_filters_config.rviz diff --git a/plane_seg_ros/config/elevation_map_filters_config.rviz b/plane_seg_ros/config/elevation_map_filters_config.rviz new file mode 100644 index 0000000..636bf22 --- /dev/null +++ b/plane_seg_ros/config/elevation_map_filters_config.rviz @@ -0,0 +1,281 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /PointCloud21 + - /Marker1 + - /GridMap1 + - /PoseWithCovariance1 + - /GridMap2 + - /GridMap3 + Splitter Ratio: 0.5058823823928833 + Tree Height: 728 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /plane_seg/hull_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /plane_seg/hull_markers + Name: Marker + Namespaces: + hull lines: true + Queue Size: 100 + Value: true + - Alpha: 0.20000000298023224 + Autocompute Intensity Bounds: true + Class: grid_map_rviz_plugin/GridMap + Color: 200; 200; 200 + Color Layer: elevation + Color Transformer: GridMapLayer + Enabled: false + Height Layer: elevation + Height Transformer: GridMapLayer + History Length: 1 + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 10 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: GridMap + Show Grid Lines: true + Topic: /rooster_elevation_mapping/elevation_map + Unreliable: false + Use Rainbow: true + Value: false + - Alpha: 1 + Axes Length: 0.5 + Axes Radius: 0.05000000074505806 + Class: rviz/PoseWithCovariance + Color: 255; 25; 0 + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Head Length: 0.20000000298023224 + Head Radius: 0.10000000149011612 + Name: PoseWithCovariance + Shaft Length: 0.5 + Shaft Radius: 0.05000000074505806 + Shape: Axes + Topic: /vilens/pose + Unreliable: false + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /plane_seg/id_strings + Name: MarkerArray + Namespaces: + strings: true + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: false + Marker Topic: /plane_seg/centroids + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /plane_seg/linestrips + Name: MarkerArray + Namespaces: + linestrips: true + Queue Size: 100 + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Class: grid_map_rviz_plugin/GridMap + Color: 200; 200; 200 + Color Layer: slope + Color Transformer: GridMapLayer + Enabled: false + Height Layer: elevation + Height Transformer: GridMapLayer + History Length: 1 + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 10 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: GridMap + Show Grid Lines: true + Topic: /lidar_elevation_map_filter/filtered_map + Unreliable: false + Use Rainbow: true + Value: false + - Alpha: 0.20000000298023224 + Autocompute Intensity Bounds: true + Class: grid_map_rviz_plugin/GridMap + Color: 200; 200; 200 + Color Layer: slope + Color Transformer: GridMapLayer + Enabled: true + Height Layer: elevation + Height Transformer: GridMapLayer + History Length: 1 + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 10 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: GridMap + Show Grid Lines: true + Topic: /rooster_elevation_mapping/filtered_map + Unreliable: false + Use Rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: odom + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 4.560603141784668 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 2.4227819442749023 + Y: -0.27603957056999207 + Z: 0.34015417098999023 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5852032899856567 + Target Frame: + Value: Orbit (rviz) + Yaw: 4.28046178817749 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1025 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd00000004000000000000015600000363fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000363000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000363000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005e10000036300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1853 + X: 67 + Y: 27 diff --git a/plane_seg_ros/launch/nth_cloud.launch b/plane_seg_ros/launch/nth_cloud.launch index 4af265d..aec6f8c 100644 --- a/plane_seg_ros/launch/nth_cloud.launch +++ b/plane_seg_ros/launch/nth_cloud.launch @@ -14,7 +14,7 @@ -> +> From 1c0a0efe015ed4091396250815d0ac1e89f57d4c Mon Sep 17 00:00:00 2001 From: Marco Camurri Date: Tue, 26 Jan 2021 11:30:35 +0000 Subject: [PATCH 36/58] find and create the folder for image processor if it does not exist --- plane_seg/src/ImageProcessor.cpp | 17 ++++++++++++++++- 1 file changed, 16 insertions(+), 1 deletion(-) diff --git a/plane_seg/src/ImageProcessor.cpp b/plane_seg/src/ImageProcessor.cpp index 341a32b..c3e7347 100644 --- a/plane_seg/src/ImageProcessor.cpp +++ b/plane_seg/src/ImageProcessor.cpp @@ -1,5 +1,6 @@ #include "plane_seg/ImageProcessor.hpp" #include +#include namespace planeseg { @@ -37,7 +38,21 @@ void ImageProcessor::saveImage(cv_bridge::CvImage image_){ std::string imagename; std::cout << "Enter filename to save image (don't forget .png!): " << std::endl; std::cin >> imagename; - cv::imwrite("/home/christos/rosbags/" + imagename, image_.image); + std::string homedir = getenv("HOME"); + if(!homedir.empty()){ + boost::filesystem::path output_path(homedir + "/rosbags/"); + if(!boost::filesystem::is_directory(output_path)) + if(boost::filesystem::create_directory(output_path)){ + std::cout << "Creating directory " << output_path.string() << " ... " << std::endl; + } else { + std::cerr << "ERROR: directory " << output_path.string() << " does not exists and couln't be created." << std::endl; + return; + } + cv::imwrite(output_path.string() + imagename, image_.image); + } else { + std::cerr << "ERROR: the $HOME variable is empty!" << std::endl; + return; + } } void ImageProcessor::erodeImage(cv_bridge::CvImage originalImage){ From 134ff49e16cc85c89b0b7f047d5c9d011c7ab016 Mon Sep 17 00:00:00 2001 From: Marco Camurri Date: Tue, 26 Jan 2021 11:30:44 +0000 Subject: [PATCH 37/58] fix typo in launch --- plane_seg_ros/launch/nth_cloud.launch | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/plane_seg_ros/launch/nth_cloud.launch b/plane_seg_ros/launch/nth_cloud.launch index aec6f8c..1fcdd59 100644 --- a/plane_seg_ros/launch/nth_cloud.launch +++ b/plane_seg_ros/launch/nth_cloud.launch @@ -14,7 +14,6 @@ -> - + From 774ef51b70c15efd51e85834e3d8cc1bbbc9e053 Mon Sep 17 00:00:00 2001 From: Marco Camurri Date: Tue, 26 Jan 2021 11:30:57 +0000 Subject: [PATCH 38/58] improve initial prinitng --- plane_seg_ros/src/plane_seg_ros_node.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/plane_seg_ros/src/plane_seg_ros_node.cpp b/plane_seg_ros/src/plane_seg_ros_node.cpp index 11e88b0..9d95a74 100644 --- a/plane_seg_ros/src/plane_seg_ros_node.cpp +++ b/plane_seg_ros/src/plane_seg_ros_node.cpp @@ -22,7 +22,7 @@ int main( int argc, char** argv ){ bool run_test_program = false; nh.param("/plane_seg/run_test_program", run_test_program, false); - std::cout << "run_test_program: " << run_test_program << "\n"; + std::cout << "run_test_program: " << std::boolalpha << run_test_program << "\n"; // Enable this to run the test programs @@ -42,7 +42,7 @@ int main( int argc, char** argv ){ bool run_sequential_test = false; nh.param("/plane_seg/run_sequential_test", run_sequential_test, false); - std::cout << "run_sequential_test: " << run_sequential_test << "\n"; + std::cout << "run_sequential_test: " << std::boolalpha << run_sequential_test << "\n"; //Enable this to run the sequential test if (run_sequential_test){ @@ -58,15 +58,16 @@ int main( int argc, char** argv ){ bool run_nth_cloud = false; nh.param("/plane_seg/run_nth_cloud", run_nth_cloud, false); - std::cout << "run_nth_cloud: " << run_nth_cloud << "\n"; + std::cout << "run_nth_cloud: " << std::boolalpha << run_nth_cloud << "\n"; // Enable this to run nth coud if (run_nth_cloud){ - std::cout << "Running nth cloud\n"; + int n_ = 1; + nh.getParam("n", n_); + std::cout << "Running " << n_ << "-th cloud\n"; std::string filename_; nh.getParam("rosbag_path", filename_); - int n_; - nh.getParam("n", n_); + app->extractNthCloud(filename_, n_); std::cout << "Finshed!\n"; exit(-1); From 1a2cdda3f6b1f8e71a419a5e016f4d65f2c7c076 Mon Sep 17 00:00:00 2001 From: Marco Camurri Date: Tue, 26 Jan 2021 11:31:31 +0000 Subject: [PATCH 39/58] use a copy of the other rviz config as placeholder for the real one --- .../config/elevation_map_filters_config.rviz | 148 +++++++++++++++++- 1 file changed, 140 insertions(+), 8 deletions(-) diff --git a/plane_seg_ros/config/elevation_map_filters_config.rviz b/plane_seg_ros/config/elevation_map_filters_config.rviz index 636bf22..940bf48 100644 --- a/plane_seg_ros/config/elevation_map_filters_config.rviz +++ b/plane_seg_ros/config/elevation_map_filters_config.rviz @@ -6,13 +6,9 @@ Panels: Expanded: - /Global Options1 - /Status1 + - /hull markers1/Namespaces1 - /PointCloud21 - - /Marker1 - - /GridMap1 - - /PoseWithCovariance1 - - /GridMap2 - - /GridMap3 - Splitter Ratio: 0.5058823823928833 + Splitter Ratio: 0.3891509473323822 Tree Height: 728 - Class: rviz/Selection Name: Selection @@ -32,7 +28,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: PointCloud2 + SyncSource: "" Preferences: PromptSaveOnExit: true Toolbars: @@ -76,7 +72,7 @@ Visualization Manager: Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 - Name: PointCloud2 + Name: hull cloud Position Transformer: XYZ Queue Size: 10 Selectable: true @@ -91,18 +87,105 @@ Visualization Manager: - Class: rviz/Marker Enabled: true Marker Topic: /plane_seg/hull_markers +<<<<<<< HEAD Name: Marker Namespaces: hull lines: true Queue Size: 100 Value: true +======= + Name: hull markers + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 0.5 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: input cloud + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.0020000000949949026 + Style: Flat Squares + Topic: /plane_seg/received_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: look pose + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Axes + Topic: /plane_seg/look_pose + Unreliable: false + Value: true + - Alpha: 0.4000000059604645 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 252; 175; 62 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.003000000026077032 + Style: Flat Squares + Topic: /plane_seg/point_cloud_in + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true +>>>>>>> 17fd460... use a copy of the other rviz config as placeholder for the real one - Alpha: 0.20000000298023224 Autocompute Intensity Bounds: true Class: grid_map_rviz_plugin/GridMap Color: 200; 200; 200 Color Layer: elevation Color Transformer: GridMapLayer +<<<<<<< HEAD Enabled: false +======= + Enabled: true +>>>>>>> 17fd460... use a copy of the other rviz config as placeholder for the real one Height Layer: elevation Height Transformer: GridMapLayer History Length: 1 @@ -113,6 +196,7 @@ Visualization Manager: Min Intensity: 0 Name: GridMap Show Grid Lines: true +<<<<<<< HEAD Topic: /rooster_elevation_mapping/elevation_map Unreliable: false Use Rainbow: true @@ -120,6 +204,15 @@ Visualization Manager: - Alpha: 1 Axes Length: 0.5 Axes Radius: 0.05000000074505806 +======= + Topic: /elevation_mapping/elevation_map + Unreliable: false + Use Rainbow: true + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 +>>>>>>> 17fd460... use a copy of the other rviz config as placeholder for the real one Class: rviz/PoseWithCovariance Color: 255; 25; 0 Covariance: @@ -138,6 +231,7 @@ Visualization Manager: Value: true Value: true Enabled: true +<<<<<<< HEAD Head Length: 0.20000000298023224 Head Radius: 0.10000000149011612 Name: PoseWithCovariance @@ -213,6 +307,17 @@ Visualization Manager: Unreliable: false Use Rainbow: true Value: true +======= + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: robot pose + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Axes + Topic: /state_estimator/pose_in_odom + Unreliable: false + Value: true +>>>>>>> 17fd460... use a copy of the other rviz config as placeholder for the real one Enabled: true Global Options: Background Color: 48; 48; 48 @@ -241,25 +346,42 @@ Visualization Manager: Views: Current: Class: rviz/Orbit +<<<<<<< HEAD Distance: 4.560603141784668 +======= + Distance: 5.569014072418213 +>>>>>>> 17fd460... use a copy of the other rviz config as placeholder for the real one Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: +<<<<<<< HEAD X: 2.4227819442749023 Y: -0.27603957056999207 Z: 0.34015417098999023 +======= + X: 2.669053792953491 + Y: 0.7634352445602417 + Z: 0.9945032000541687 +>>>>>>> 17fd460... use a copy of the other rviz config as placeholder for the real one Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 +<<<<<<< HEAD Pitch: 0.5852032899856567 Target Frame: Value: Orbit (rviz) Yaw: 4.28046178817749 +======= + Pitch: 0.3297964334487915 + Target Frame: + Value: Orbit (rviz) + Yaw: 4.404192924499512 +>>>>>>> 17fd460... use a copy of the other rviz config as placeholder for the real one Saved: ~ Window Geometry: Displays: @@ -267,7 +389,11 @@ Window Geometry: Height: 1025 Hide Left Dock: false Hide Right Dock: true +<<<<<<< HEAD QMainWindow State: 000000ff00000000fd00000004000000000000015600000363fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000363000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000363000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005e10000036300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 +======= + QMainWindow State: 000000ff00000000fd0000000400000000000001aa00000363fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000363000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065000000030b000000950000000000000000fb0000000a0049006d00610067006501000001e1000000b50000000000000000000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000363000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004800000003efc0100000002fb0000000800540069006d0065010000000000000480000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002d00000036300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 +>>>>>>> 17fd460... use a copy of the other rviz config as placeholder for the real one Selection: collapsed: false Time: @@ -276,6 +402,12 @@ Window Geometry: collapsed: false Views: collapsed: true +<<<<<<< HEAD Width: 1853 X: 67 Y: 27 +======= + Width: 1152 + X: 1069 + Y: 168 +>>>>>>> 17fd460... use a copy of the other rviz config as placeholder for the real one From 891419a5469a59d4ee6144fa07c49684c72678c0 Mon Sep 17 00:00:00 2001 From: Christos Ioannou Date: Tue, 26 Jan 2021 13:58:58 +0000 Subject: [PATCH 40/58] move filters config to plane_seg_ros --- .../config/elevation_map_filters_config.rviz | 52 +++---------------- 1 file changed, 8 insertions(+), 44 deletions(-) diff --git a/plane_seg_ros/config/elevation_map_filters_config.rviz b/plane_seg_ros/config/elevation_map_filters_config.rviz index 940bf48..d45cee7 100644 --- a/plane_seg_ros/config/elevation_map_filters_config.rviz +++ b/plane_seg_ros/config/elevation_map_filters_config.rviz @@ -6,9 +6,13 @@ Panels: Expanded: - /Global Options1 - /Status1 - - /hull markers1/Namespaces1 - /PointCloud21 - Splitter Ratio: 0.3891509473323822 + - /Marker1 + - /GridMap1 + - /PoseWithCovariance1 + - /GridMap2 + - /GridMap3 + Splitter Ratio: 0.5058823823928833 Tree Height: 728 - Class: rviz/Selection Name: Selection @@ -28,7 +32,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: "" + SyncSource: PointCloud2 Preferences: PromptSaveOnExit: true Toolbars: @@ -72,7 +76,7 @@ Visualization Manager: Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 - Name: hull cloud + Name: PointCloud2 Position Transformer: XYZ Queue Size: 10 Selectable: true @@ -87,13 +91,11 @@ Visualization Manager: - Class: rviz/Marker Enabled: true Marker Topic: /plane_seg/hull_markers -<<<<<<< HEAD Name: Marker Namespaces: hull lines: true Queue Size: 100 Value: true -======= Name: hull markers Namespaces: {} @@ -174,18 +176,13 @@ Visualization Manager: Use Fixed Frame: true Use rainbow: true Value: true ->>>>>>> 17fd460... use a copy of the other rviz config as placeholder for the real one - Alpha: 0.20000000298023224 Autocompute Intensity Bounds: true Class: grid_map_rviz_plugin/GridMap Color: 200; 200; 200 Color Layer: elevation Color Transformer: GridMapLayer -<<<<<<< HEAD Enabled: false -======= - Enabled: true ->>>>>>> 17fd460... use a copy of the other rviz config as placeholder for the real one Height Layer: elevation Height Transformer: GridMapLayer History Length: 1 @@ -196,7 +193,6 @@ Visualization Manager: Min Intensity: 0 Name: GridMap Show Grid Lines: true -<<<<<<< HEAD Topic: /rooster_elevation_mapping/elevation_map Unreliable: false Use Rainbow: true @@ -204,7 +200,6 @@ Visualization Manager: - Alpha: 1 Axes Length: 0.5 Axes Radius: 0.05000000074505806 -======= Topic: /elevation_mapping/elevation_map Unreliable: false Use Rainbow: true @@ -212,7 +207,6 @@ Visualization Manager: - Alpha: 1 Axes Length: 1 Axes Radius: 0.10000000149011612 ->>>>>>> 17fd460... use a copy of the other rviz config as placeholder for the real one Class: rviz/PoseWithCovariance Color: 255; 25; 0 Covariance: @@ -231,7 +225,6 @@ Visualization Manager: Value: true Value: true Enabled: true -<<<<<<< HEAD Head Length: 0.20000000298023224 Head Radius: 0.10000000149011612 Name: PoseWithCovariance @@ -307,7 +300,6 @@ Visualization Manager: Unreliable: false Use Rainbow: true Value: true -======= Head Length: 0.30000001192092896 Head Radius: 0.10000000149011612 Name: robot pose @@ -317,7 +309,6 @@ Visualization Manager: Topic: /state_estimator/pose_in_odom Unreliable: false Value: true ->>>>>>> 17fd460... use a copy of the other rviz config as placeholder for the real one Enabled: true Global Options: Background Color: 48; 48; 48 @@ -346,42 +337,25 @@ Visualization Manager: Views: Current: Class: rviz/Orbit -<<<<<<< HEAD - Distance: 4.560603141784668 -======= Distance: 5.569014072418213 ->>>>>>> 17fd460... use a copy of the other rviz config as placeholder for the real one Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: -<<<<<<< HEAD X: 2.4227819442749023 Y: -0.27603957056999207 Z: 0.34015417098999023 -======= - X: 2.669053792953491 - Y: 0.7634352445602417 - Z: 0.9945032000541687 ->>>>>>> 17fd460... use a copy of the other rviz config as placeholder for the real one Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 -<<<<<<< HEAD Pitch: 0.5852032899856567 Target Frame: Value: Orbit (rviz) Yaw: 4.28046178817749 -======= - Pitch: 0.3297964334487915 - Target Frame: - Value: Orbit (rviz) - Yaw: 4.404192924499512 ->>>>>>> 17fd460... use a copy of the other rviz config as placeholder for the real one Saved: ~ Window Geometry: Displays: @@ -389,11 +363,7 @@ Window Geometry: Height: 1025 Hide Left Dock: false Hide Right Dock: true -<<<<<<< HEAD QMainWindow State: 000000ff00000000fd00000004000000000000015600000363fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000363000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000363000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005e10000036300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 -======= - QMainWindow State: 000000ff00000000fd0000000400000000000001aa00000363fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000363000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065000000030b000000950000000000000000fb0000000a0049006d00610067006501000001e1000000b50000000000000000000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000363000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004800000003efc0100000002fb0000000800540069006d0065010000000000000480000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002d00000036300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 ->>>>>>> 17fd460... use a copy of the other rviz config as placeholder for the real one Selection: collapsed: false Time: @@ -402,12 +372,6 @@ Window Geometry: collapsed: false Views: collapsed: true -<<<<<<< HEAD - Width: 1853 - X: 67 - Y: 27 -======= Width: 1152 X: 1069 Y: 168 ->>>>>>> 17fd460... use a copy of the other rviz config as placeholder for the real one From 9d5c20f972a7fa01eb1d99a12ce29f3e32d35616 Mon Sep 17 00:00:00 2001 From: Marco Camurri Date: Tue, 26 Jan 2021 15:13:44 +0000 Subject: [PATCH 41/58] get a reference of the node instead of copying it --- plane_seg_ros/include/plane_seg_ros/Pass.hpp | 4 ++-- plane_seg_ros/src/plane_seg_ros.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/plane_seg_ros/include/plane_seg_ros/Pass.hpp b/plane_seg_ros/include/plane_seg_ros/Pass.hpp index b7859f2..031b828 100644 --- a/plane_seg_ros/include/plane_seg_ros/Pass.hpp +++ b/plane_seg_ros/include/plane_seg_ros/Pass.hpp @@ -21,7 +21,7 @@ namespace planeseg { class Pass{ public: - Pass(ros::NodeHandle node_); + Pass(ros::NodeHandle& node); ~Pass(){ } @@ -52,7 +52,7 @@ class Pass{ grid_map_msgs::GridMap gridMapCallback(const grid_map_msgs::GridMap& msg); private: - ros::NodeHandle node_; + ros::NodeHandle& node_; std::vector colors_; std::vector colors_2; std::vector colors_3; diff --git a/plane_seg_ros/src/plane_seg_ros.cpp b/plane_seg_ros/src/plane_seg_ros.cpp index 5b283ae..aff9ad3 100644 --- a/plane_seg_ros/src/plane_seg_ros.cpp +++ b/plane_seg_ros/src/plane_seg_ros.cpp @@ -42,8 +42,8 @@ using namespace planeseg; -Pass::Pass(ros::NodeHandle node_): - node_(node_), +Pass::Pass(ros::NodeHandle& node): + node_(node), filter_chain_("grid_map::GridMap") { From 86429703ba04f07a5fccff5076424fab9b6c4837 Mon Sep 17 00:00:00 2001 From: Marco Camurri Date: Tue, 26 Jan 2021 15:14:39 +0000 Subject: [PATCH 42/58] no need to use a pointer to the tf listener --- plane_seg_ros/include/plane_seg_ros/Pass.hpp | 2 +- plane_seg_ros/src/plane_seg_ros.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/plane_seg_ros/include/plane_seg_ros/Pass.hpp b/plane_seg_ros/include/plane_seg_ros/Pass.hpp index 031b828..4eb505b 100644 --- a/plane_seg_ros/include/plane_seg_ros/Pass.hpp +++ b/plane_seg_ros/include/plane_seg_ros/Pass.hpp @@ -66,7 +66,7 @@ class Pass{ planeseg::Visualizer visualizer_; planeseg::ImageProcessor imgprocessor_; filters::FilterChain filter_chain_; - tf::TransformListener* listener_; + tf::TransformListener listener_; std::string elevation_map_topic_; std::string filter_chain_parameters_name_; std::string filtered_map_topic_; diff --git a/plane_seg_ros/src/plane_seg_ros.cpp b/plane_seg_ros/src/plane_seg_ros.cpp index aff9ad3..97027bc 100644 --- a/plane_seg_ros/src/plane_seg_ros.cpp +++ b/plane_seg_ros/src/plane_seg_ros.cpp @@ -698,8 +698,8 @@ grid_map_msgs::GridMap Pass::gridMapCallback(const grid_map_msgs::GridMap& msg){ Eigen::Isometry3d pose_robot = Eigen::Isometry3d::Identity(); tf::StampedTransform transform; try { - listener_->waitForTransform("/odom", "/base", ros::Time(0), ros::Duration(10.0) ); - listener_->lookupTransform("/odom", "/base", ros::Time(0), transform); + listener_.waitForTransform("odom", "base", ros::Time(0), ros::Duration(10.0) ); + listener_.lookupTransform("odom", "base", ros::Time(0), transform); } catch (tf::TransformException ex) { ROS_ERROR("%s",ex.what()); } From d21245c5d5864175d0565e1fd6ebc2174c84b3d7 Mon Sep 17 00:00:00 2001 From: Marco Camurri Date: Tue, 26 Jan 2021 15:15:05 +0000 Subject: [PATCH 43/58] print more checks and put the filters under the same namespace --- plane_seg_ros/config/nth_cloud.yaml | 152 ++++++++++++++-------------- plane_seg_ros/src/plane_seg_ros.cpp | 41 ++++++-- 2 files changed, 110 insertions(+), 83 deletions(-) diff --git a/plane_seg_ros/config/nth_cloud.yaml b/plane_seg_ros/config/nth_cloud.yaml index fc1ee07..3e150df 100644 --- a/plane_seg_ros/config/nth_cloud.yaml +++ b/plane_seg_ros/config/nth_cloud.yaml @@ -3,10 +3,10 @@ plane_seg: run_sequential_test: false run_nth_cloud: true -grid_map_filters: + grid_map_filters: - - name: buffer_normalizer - type: gridMapFilters/BufferNormalizerFilter + - name: buffer_normalizer + type: gridMapFilters/BufferNormalizerFilter # # Duplicate layer. # - name: duplicate @@ -16,26 +16,26 @@ grid_map_filters: # output_layer: ... # Delete color layer. - - name: delete_original_layers - type: gridMapFilters/DeletionFilter - params: - layers: [color] # List of layers. + - name: delete_original_layers + type: gridMapFilters/DeletionFilter + params: + layers: [color] # List of layers. # Fill holes in the map with inpainting. - - name: inpaint - type: gridMapCv/InpaintFilter - params: - input_layer: elevation - output_layer: elevation_inpainted - radius: 0.05 + - name: inpaint + type: gridMapCv/InpaintFilter + params: + input_layer: elevation + output_layer: elevation_inpainted + radius: 0.05 # Reduce noise with a radial blurring filter. - - name: mean_in_radius - type: gridMapFilters/MeanInRadiusFilter - params: - input_layer: elevation_inpainted - output_layer: elevation_smooth - radius: 0.06 + - name: mean_in_radius + type: gridMapFilters/MeanInRadiusFilter + params: + input_layer: elevation_inpainted + output_layer: elevation_smooth + radius: 0.06 # Boxblur as an alternative to the inpaint and radial blurring filter above. # - name: boxblur @@ -49,45 +49,45 @@ grid_map_filters: # window_size: 5 # optional # Compute surface normals. - - name: surface_normals - type: gridMapFilters/NormalVectorsFilter - params: - input_layer: elevation_inpainted - output_layers_prefix: normal_vectors_ - radius: 0.05 - normal_vector_positive_axis: z + - name: surface_normals + type: gridMapFilters/NormalVectorsFilter + params: + input_layer: elevation_inpainted + output_layers_prefix: normal_vectors_ + radius: 0.05 + normal_vector_positive_axis: z # Add a color layer for visualization based on the surface normal. - - name: normal_color_map - type: gridMapFilters/NormalColorMapFilter - params: - input_layers_prefix: normal_vectors_ - output_layer: normal_color + - name: normal_color_map + type: gridMapFilters/NormalColorMapFilter + params: + input_layers_prefix: normal_vectors_ + output_layer: normal_color # Compute slope from surface normal. - - name: slope - type: gridMapFilters/MathExpressionFilter - params: - output_layer: slope - expression: acos(normal_vectors_z) + - name: slope + type: gridMapFilters/MathExpressionFilter + params: + output_layer: slope + expression: acos(normal_vectors_z) # Compute roughness as absolute difference from map to smoothened map. - - name: roughness - type: gridMapFilters/MathExpressionFilter - params: - output_layer: roughness - expression: abs(elevation_inpainted - elevation_smooth) - - # Edge detection by computing the standard deviation from slope. - - name: edge_detection - type: gridMapFilters/SlidingWindowMathExpressionFilter - params: - input_layer: slope - output_layer: edges - expression: sqrt(sumOfFinites(square(slope - meanOfFinites(slope))) ./ numberOfFinites(slope)) # Standard deviation - compute_empty_cells: false - edge_handling: crop # options: inside, crop, empty, mean - window_length: 0.05 + - name: roughness + type: gridMapFilters/MathExpressionFilter + params: + output_layer: roughness + expression: abs(elevation_inpainted - elevation_smooth) + + # Edge detection by computing the standard deviation from slope. + - name: edge_detection + type: gridMapFilters/SlidingWindowMathExpressionFilter + params: + input_layer: slope + output_layer: edges + expression: sqrt(sumOfFinites(square(slope - meanOfFinites(slope))) ./ numberOfFinites(slope)) # Standard deviation + compute_empty_cells: false + edge_handling: crop # options: inside, crop, empty, mean + window_length: 0.05 # # Edge detection on elevation layer with convolution filter as alternative to filter above. # - name: edge_detection @@ -102,33 +102,33 @@ grid_map_filters: # window_size: 3 # Make sure to make this compatible with the kernel matrix. # Compute traversability as normalized weighted sum of slope and roughness. - - name: traversability - type: gridMapFilters/MathExpressionFilter - params: - output_layer: traversability - expression: 0.5 * (1.0 - (slope / 0.6)) + 0.5 * (1.0 - (roughness / 0.1)) + - name: traversability + type: gridMapFilters/MathExpressionFilter + params: + output_layer: traversability + expression: 0.5 * (1.0 - (slope / 0.6)) + 0.5 * (1.0 - (roughness / 0.1)) # Set lower threshold on traversability. - - name: traversability_lower_threshold - type: gridMapFilters/ThresholdFilter - params: - condition_layer: traversability - output_layer: traversability - lower_threshold: 0.0 - set_to: 0.0 + - name: traversability_lower_threshold + type: gridMapFilters/ThresholdFilter + params: + condition_layer: traversability + output_layer: traversability + lower_threshold: 0.0 + set_to: 0.0 # Set upper threshold on traversability. - - name: traversability_upper_threshold - type: gridMapFilters/ThresholdFilter - params: - condition_layer: traversability - output_layer: traversability - upper_threshold: 1.0 - set_to: 1.0 # Other uses: .nan, .inf + - name: traversability_upper_threshold + type: gridMapFilters/ThresholdFilter + params: + condition_layer: traversability + output_layer: traversability + upper_threshold: 1.0 + set_to: 1.0 # Other uses: .nan, .inf # Duplicate layer. - - name: traversability_clean - type: gridMapFilters/DuplicationFilter - params: - input_layer: traversability - output_layer: traversability_clean + - name: traversability_clean + type: gridMapFilters/DuplicationFilter + params: + input_layer: traversability + output_layer: traversability_clean diff --git a/plane_seg_ros/src/plane_seg_ros.cpp b/plane_seg_ros/src/plane_seg_ros.cpp index 97027bc..d1a31c6 100644 --- a/plane_seg_ros/src/plane_seg_ros.cpp +++ b/plane_seg_ros/src/plane_seg_ros.cpp @@ -48,7 +48,9 @@ Pass::Pass(ros::NodeHandle& node): { std::string input_body_pose_topic; - node_.getParam("input_body_pose_topic", input_body_pose_topic); + if(!node_.getParam("input_body_pose_topic", input_body_pose_topic)){ + ROS_WARN_STREAM("Couldn't get parameter: input_body_pose_topic"); + } // std::string filename; // node_.getParm("/rosbag_pass/filename", filename); /* @@ -77,17 +79,30 @@ Pass::Pass(ros::NodeHandle& node): tracking_ = planeseg::Tracker(); visualizer_ = planeseg::Visualizer(); imgprocessor_ = planeseg::ImageProcessor(); - listener_ = new tf::TransformListener(); - node_.param("input_topic", elevation_map_topic_, std::string("/rooster_elevation_mapping/elevation_map")); - node_.param("erode_radius", erode_radius_, 0.2); + if(!node_.param("input_topic", elevation_map_topic_, std::string("/rooster_elevation_mapping/elevation_map"))){ + ROS_WARN_STREAM("Couldn't get parameter: input_topic"); + } + if(!node_.param("erode_radius", erode_radius_, 0.2)){ + ROS_WARN_STREAM("Couldn't get parameter: erode_radius"); + } ROS_INFO("Erode Radius [%f]", erode_radius_); - node_.param("traversability_threshold", traversability_threshold_, 0.8); + if(!node_.param("traversability_threshold", traversability_threshold_, 0.8)){ + ROS_WARN_STREAM("Couldn't get parameter: traversability_threshold"); + } ROS_INFO("traversability_threshold [%f]", traversability_threshold_); - node_.param("verbose_timer", verbose_timer_, true); + if(!node_.param("verbose_timer", verbose_timer_, true)){ + ROS_WARN_STREAM("Couldn't get parameter: verbose_timer"); + } + std::string param_name = "grid_map_filters"; + XmlRpc::XmlRpcValue config; + if(!node_.getParam(param_name, config)) { + ROS_ERROR("Could not load the filter chain configuration from parameter %s, are you sure it was pushed to the parameter server? Assuming that you meant to leave it empty.", param_name.c_str()); + return; + } // Setup filter chain - if (!filter_chain_.configure("grid_map_filters", node_)){ + if (!filter_chain_.configure(param_name, node_)){ std::cout << "couldn't configure filter chain" << std::endl; return; } @@ -689,6 +704,18 @@ grid_map_msgs::GridMap Pass::gridMapCallback(const grid_map_msgs::GridMap& msg){ return failmessage; } +/* DEBUG PRINT INPUT AND OUTPUT LAYERS + std::vector in_layers = input_map.getLayers(); + for(const auto& it : in_layers){ + std::cerr << "||||||||||| IN LAYER: " << it << std::endl; + } + + std::vector layers = output_map.getLayers(); + for(const auto& it : layers){ + std::cerr << "||||||||||| OUT LAYER: " << it << std::endl; + } +*/ + if (verbose_timer_) { std::cout << toc().count() << " ms: filter chain\n"; } From 8f031288bed942c499c6effe9c915254bd22869a Mon Sep 17 00:00:00 2001 From: Marco Camurri Date: Tue, 26 Jan 2021 15:32:26 +0000 Subject: [PATCH 44/58] visualize point clouds and exit cleanly from the program --- .../config/elevation_map_filters_config.rviz | 30 +++++++++---------- plane_seg_ros/src/plane_seg_ros_node.cpp | 2 +- 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/plane_seg_ros/config/elevation_map_filters_config.rviz b/plane_seg_ros/config/elevation_map_filters_config.rviz index d45cee7..2f906d8 100644 --- a/plane_seg_ros/config/elevation_map_filters_config.rviz +++ b/plane_seg_ros/config/elevation_map_filters_config.rviz @@ -182,7 +182,7 @@ Visualization Manager: Color: 200; 200; 200 Color Layer: elevation Color Transformer: GridMapLayer - Enabled: false + Enabled: true Height Layer: elevation Height Transformer: GridMapLayer History Length: 1 @@ -196,7 +196,7 @@ Visualization Manager: Topic: /rooster_elevation_mapping/elevation_map Unreliable: false Use Rainbow: true - Value: false + Value: true - Alpha: 1 Axes Length: 0.5 Axes Radius: 0.05000000074505806 @@ -264,7 +264,7 @@ Visualization Manager: Color: 200; 200; 200 Color Layer: slope Color Transformer: GridMapLayer - Enabled: false + Enabled: true Height Layer: elevation Height Transformer: GridMapLayer History Length: 1 @@ -275,10 +275,10 @@ Visualization Manager: Min Intensity: 0 Name: GridMap Show Grid Lines: true - Topic: /lidar_elevation_map_filter/filtered_map + Topic: /rooster_elevation_mapping/filtered_map Unreliable: false Use Rainbow: true - Value: false + Value: true - Alpha: 0.20000000298023224 Autocompute Intensity Bounds: true Class: grid_map_rviz_plugin/GridMap @@ -337,25 +337,25 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 5.569014072418213 + Distance: 5.306222438812256 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 2.4227819442749023 - Y: -0.27603957056999207 - Z: 0.34015417098999023 + X: 2.500084638595581 + Y: 0.5421060919761658 + Z: -0.48014968633651733 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.5852032899856567 + Pitch: 0.7852033972740173 Target Frame: Value: Orbit (rviz) - Yaw: 4.28046178817749 + Yaw: 4.4954609870910645 Saved: ~ Window Geometry: Displays: @@ -363,7 +363,7 @@ Window Geometry: Height: 1025 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd00000004000000000000015600000363fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000363000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000363000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005e10000036300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000002db00000363fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000363000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000363000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000045c0000036300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -372,6 +372,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1152 - X: 1069 - Y: 168 + Width: 1853 + X: 67 + Y: 32 diff --git a/plane_seg_ros/src/plane_seg_ros_node.cpp b/plane_seg_ros/src/plane_seg_ros_node.cpp index 9d95a74..78ceca7 100644 --- a/plane_seg_ros/src/plane_seg_ros_node.cpp +++ b/plane_seg_ros/src/plane_seg_ros_node.cpp @@ -70,7 +70,7 @@ int main( int argc, char** argv ){ app->extractNthCloud(filename_, n_); std::cout << "Finshed!\n"; - exit(-1); + return 0; } ROS_INFO_STREAM("Waiting for ROS messages"); From 835f9d63b0f43c26e29061b793f619c7a043556a Mon Sep 17 00:00:00 2001 From: Marco Camurri Date: Tue, 26 Jan 2021 16:21:41 +0000 Subject: [PATCH 45/58] relative param, return 0 when the program ends --- plane_seg_ros/src/plane_seg_ros_node.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/plane_seg_ros/src/plane_seg_ros_node.cpp b/plane_seg_ros/src/plane_seg_ros_node.cpp index 78ceca7..3c14dfb 100644 --- a/plane_seg_ros/src/plane_seg_ros_node.cpp +++ b/plane_seg_ros/src/plane_seg_ros_node.cpp @@ -21,10 +21,11 @@ int main( int argc, char** argv ){ ROS_INFO_STREAM("============================="); bool run_test_program = false; - nh.param("/plane_seg/run_test_program", run_test_program, false); + nh.param("run_test_program", run_test_program, false); std::cout << "run_test_program: " << std::boolalpha << run_test_program << "\n"; + // Enable this to run the test programs if (run_test_program){ std::cout << "Running test examples\n"; @@ -37,11 +38,11 @@ int main( int argc, char** argv ){ //app->processFromFile(5); std::cout << "Finished!\n"; - exit(-1); + return 0; } bool run_sequential_test = false; - nh.param("/plane_seg/run_sequential_test", run_sequential_test, false); + nh.param("run_sequential_test", run_sequential_test, false); std::cout << "run_sequential_test: " << std::boolalpha << run_sequential_test << "\n"; //Enable this to run the sequential test @@ -53,11 +54,11 @@ int main( int argc, char** argv ){ app->stepThroughFile(filename_); std::cout << "Finished!\n"; - exit(-1); + return 0; } bool run_nth_cloud = false; - nh.param("/plane_seg/run_nth_cloud", run_nth_cloud, false); + nh.param("run_nth_cloud", run_nth_cloud, false); std::cout << "run_nth_cloud: " << std::boolalpha << run_nth_cloud << "\n"; // Enable this to run nth coud From 49502f599855408f8dafee54928366fa3951e3dc Mon Sep 17 00:00:00 2001 From: Christos Ioannou Date: Mon, 1 Feb 2021 23:23:15 +0000 Subject: [PATCH 46/58] morphological filtering of gridmap image and then remove small blobs --- plane_seg/CMakeLists.txt | 2 +- .../include/plane_seg/ImageProcessor.hpp | 14 +- plane_seg/src/ImageProcessor.cpp | 155 +++++++++++- .../config/elevation_map_filters_config.rviz | 36 +-- plane_seg_ros/config/nth_cloud.yaml | 77 +----- .../config/plane_seg_sequential_config.rviz | 237 ++++++++++++++++++ plane_seg_ros/config/rosbag_sequential.yaml | 68 +++++ plane_seg_ros/include/plane_seg_ros/Pass.hpp | 2 + plane_seg_ros/launch/rosbag_sequential.launch | 2 +- plane_seg_ros/src/plane_seg_ros.cpp | 111 +++----- 10 files changed, 514 insertions(+), 190 deletions(-) create mode 100644 plane_seg_ros/config/plane_seg_sequential_config.rviz diff --git a/plane_seg/CMakeLists.txt b/plane_seg/CMakeLists.txt index fd9c04f..4a59c1c 100644 --- a/plane_seg/CMakeLists.txt +++ b/plane_seg/CMakeLists.txt @@ -9,7 +9,7 @@ find_package(catkin REQUIRED COMPONENTS pcl_ros ) -find_package(OpenCV REQUIRED COMPONENTS core highgui imgproc imgcodecs) +find_package(OpenCV REQUIRED COMPONENTS core highgui imgproc imgcodecs features2d) catkin_package( INCLUDE_DIRS diff --git a/plane_seg/include/plane_seg/ImageProcessor.hpp b/plane_seg/include/plane_seg/ImageProcessor.hpp index 4df28e1..ca0f4d5 100644 --- a/plane_seg/include/plane_seg/ImageProcessor.hpp +++ b/plane_seg/include/plane_seg/ImageProcessor.hpp @@ -11,9 +11,21 @@ class ImageProcessor { void process(); void displayImage(cv_bridge::CvImage image, std::string process); void saveImage(cv_bridge::CvImage image); - void erodeImage(cv_bridge::CvImage originalImage); + void erodeImage(cv_bridge::CvImage originalImage, int erode_size); + void thresholdImage(cv_bridge::CvImage image); + void dilateImage(cv_bridge::CvImage originalImage, int dilate_size); + void blobDetector(cv_bridge::CvImage image); + void removeBlobs(cv_bridge::CvImage image); cv_bridge::CvImage original_img_; + cv_bridge::CvImage bit32_img_; cv_bridge::CvImage erode_img_; + cv_bridge::CvImage threshold_img_; + cv_bridge::CvImage dilate_img_; + cv_bridge::CvImage adap_threshold_img_; + cv_bridge::CvImage detect_img_; + cv_bridge::CvImage large_contours_; + cv_bridge::CvImage small_contours_; + std::vector> contours1; private: diff --git a/plane_seg/src/ImageProcessor.cpp b/plane_seg/src/ImageProcessor.cpp index c3e7347..98ba500 100644 --- a/plane_seg/src/ImageProcessor.cpp +++ b/plane_seg/src/ImageProcessor.cpp @@ -1,5 +1,7 @@ #include "plane_seg/ImageProcessor.hpp" #include +#include +#include #include namespace planeseg { @@ -11,21 +13,67 @@ ImageProcessor::~ImageProcessor(){} void ImageProcessor::process(){ displayImage(original_img_, "original"); - std::cout << "Press 's' to save, 'e' to erode, anything else to close" << std::endl; +/* + std::cout << "Press 's' to save, 't' to threshold, anything else to close" << std::endl; +// displayImage(original_img_, "white NaN"); int l = cv::waitKey(0); if (l == 's'){ saveImage(original_img_); } - else if (l == 'e'){ - erodeImage(original_img_); - displayImage(erode_img_, "erode"); - std::cout << "Press 's' to save both images, original then eroded" << std::endl; + else if (l == 't'){ + thresholdImage(original_img_); + displayImage(threshold_img_, "threshold"); + std::cout << "Press 's' to save both images, original then thresholded, or 'd' to dilate!" << std::endl; int k = cv::waitKey(0); if (k == 's'){ saveImage(original_img_); - saveImage(erode_img_); + saveImage(threshold_img_); + } else if (k == 'd'){ + dilateImage(threshold_img_); + displayImage(dilate_img_, "dilated"); + std::cout <<"Press 's' to save all three images, original, threshold, then dilated; 'e' to erode" << std::endl; + int j = cv::waitKey(0); + if (j == 's'){ + saveImage(original_img_); + saveImage(threshold_img_); + saveImage(dilate_img_); + } else if (j == 'e'){ + erodeImage(dilate_img_); + displayImage(erode_img_, "eroded"); + std::cout <<"Press 's' to save all four images, original, threshold, dilated then eroded" << std::endl; + int h = cv::waitKey(0); + if (h == 's'){ + saveImage(original_img_); + saveImage(threshold_img_); + saveImage(dilate_img_); + saveImage(erode_img_); + } + } } } + */ + thresholdImage(original_img_); + erodeImage(threshold_img_, 1); + dilateImage(erode_img_, 1); + dilateImage(dilate_img_, 2); + erodeImage(dilate_img_, 2); + removeBlobs(erode_img_); + +// blobDetector(erode_img_); +// erodeImage(erode_img_); +// dilateImage(erode_img_); + + displayImage(threshold_img_, "threshold"); + displayImage(dilate_img_, "dilated"); + displayImage(erode_img_, "eroded"); + displayImage(large_contours_, "large contours"); + displayImage(small_contours_, "small contours"); + + + int p = cv::waitKey(0); + if (p == 's'){ + saveImage(dilate_img_); + } } @@ -40,14 +88,15 @@ void ImageProcessor::saveImage(cv_bridge::CvImage image_){ std::cin >> imagename; std::string homedir = getenv("HOME"); if(!homedir.empty()){ - boost::filesystem::path output_path(homedir + "/rosbags/"); - if(!boost::filesystem::is_directory(output_path)) + boost::filesystem::path output_path(homedir + "/rosbags/image_processing"); + if(!boost::filesystem::is_directory(output_path)){ if(boost::filesystem::create_directory(output_path)){ std::cout << "Creating directory " << output_path.string() << " ... " << std::endl; } else { std::cerr << "ERROR: directory " << output_path.string() << " does not exists and couln't be created." << std::endl; return; } + } cv::imwrite(output_path.string() + imagename, image_.image); } else { std::cerr << "ERROR: the $HOME variable is empty!" << std::endl; @@ -55,14 +104,98 @@ void ImageProcessor::saveImage(cv_bridge::CvImage image_){ } } -void ImageProcessor::erodeImage(cv_bridge::CvImage originalImage){ +void ImageProcessor::erodeImage(cv_bridge::CvImage originalImage, int erode_size){ - int erode_size = 10; - cv::Mat element = cv::getStructuringElement(cv::MORPH_ELLIPSE, + cv::Mat element = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(2*erode_size + 1, 2*erode_size + 1), cv::Point(erode_size, erode_size) ); cv::erode(originalImage.image, erode_img_.image, element); } +void ImageProcessor::dilateImage(cv_bridge::CvImage originalImage, int dilate_size){ + + cv::Mat element = cv::getStructuringElement(cv::MORPH_RECT, + cv::Size(2*dilate_size+1, 2*dilate_size +1), + cv::Point(dilate_size, dilate_size) + ); + cv::dilate(originalImage.image, dilate_img_.image, element); +} + +void ImageProcessor::thresholdImage(cv_bridge::CvImage image){ + + int threshold_value = 70; + int threshold_type = cv::ThresholdTypes::THRESH_BINARY_INV; + double maxval = 255; + + cv::threshold(image.image, threshold_img_.image, threshold_value, maxval, threshold_type); + +} + + +void ImageProcessor::blobDetector(cv_bridge::CvImage image){ + + cv::SimpleBlobDetector::Params params; + + params.minThreshold = 10; + params.maxThreshold = 100; + params.thresholdStep = 10; + + params.filterByColor = true; + params.blobColor = 255; + + params.filterByArea = true; + params.minArea = 1; + params.maxArea = 200; + + params.filterByCircularity = false; + params.maxCircularity = 0.1; + + params.filterByConvexity = false; + params.minConvexity = 0.87; + + params.filterByInertia = false; + params.maxInertiaRatio = 0.1; + + cv::Ptr detector = cv::SimpleBlobDetector::create(params); + + std::vector keypoints; + detector->detect(image.image, keypoints); + + cv::drawKeypoints(image.image, keypoints, detect_img_.image, cv::Scalar(0,0,255), cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS); +} + + +void ImageProcessor::removeBlobs(cv_bridge::CvImage image){ + + large_contours_.image = cv::Mat::zeros(image.image.size(), CV_8U); + small_contours_.image = cv::Mat::zeros(image.image.size(), CV_8U); + std::vector> contours; + std::vector> large_contours; + std::vector> small_contours; + + cv::findContours(image.image, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE); + + + for(size_t k = 0; k < contours.size(); ++k){ + double area = cv::contourArea(contours[k]); + + if (area >= 200){ + large_contours.push_back(contours[k]); + } + else{ + small_contours.push_back(contours[k]); + } + + for(size_t i = 0; i < large_contours.size(); ++i){ + cv::drawContours(large_contours_.image, large_contours, i, cv::Scalar(255), cv::FILLED); + } + + for(size_t j = 0; j < small_contours.size(); ++j){ + cv::drawContours(small_contours_.image, small_contours, j, cv::Scalar(255), cv::FILLED); + } + } +} + + } //namespace planeseg diff --git a/plane_seg_ros/config/elevation_map_filters_config.rviz b/plane_seg_ros/config/elevation_map_filters_config.rviz index 2f906d8..080a9bc 100644 --- a/plane_seg_ros/config/elevation_map_filters_config.rviz +++ b/plane_seg_ros/config/elevation_map_filters_config.rviz @@ -11,9 +11,8 @@ Panels: - /GridMap1 - /PoseWithCovariance1 - /GridMap2 - - /GridMap3 Splitter Ratio: 0.5058823823928833 - Tree Height: 728 + Tree Height: 726 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -258,27 +257,6 @@ Visualization Manager: linestrips: true Queue Size: 100 Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Class: grid_map_rviz_plugin/GridMap - Color: 200; 200; 200 - Color Layer: slope - Color Transformer: GridMapLayer - Enabled: true - Height Layer: elevation - Height Transformer: GridMapLayer - History Length: 1 - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 10 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: GridMap - Show Grid Lines: true - Topic: /rooster_elevation_mapping/filtered_map - Unreliable: false - Use Rainbow: true - Value: true - Alpha: 0.20000000298023224 Autocompute Intensity Bounds: true Class: grid_map_rviz_plugin/GridMap @@ -337,7 +315,7 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 5.306222438812256 + Distance: 7.008275508880615 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -352,7 +330,7 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.7852033972740173 + Pitch: 0.7902033925056458 Target Frame: Value: Orbit (rviz) Yaw: 4.4954609870910645 @@ -360,10 +338,10 @@ Visualization Manager: Window Geometry: Displays: collapsed: false - Height: 1025 + Height: 1023 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000002db00000363fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000363000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000363000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000045c0000036300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000019500000361fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000361000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000363000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005a60000003efc0100000002fb0000000800540069006d00650100000000000005a6000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000040b0000036100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -372,6 +350,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1853 + Width: 1446 X: 67 - Y: 32 + Y: 27 diff --git a/plane_seg_ros/config/nth_cloud.yaml b/plane_seg_ros/config/nth_cloud.yaml index 3e150df..073bdaa 100644 --- a/plane_seg_ros/config/nth_cloud.yaml +++ b/plane_seg_ros/config/nth_cloud.yaml @@ -22,18 +22,18 @@ plane_seg: layers: [color] # List of layers. # Fill holes in the map with inpainting. - - name: inpaint - type: gridMapCv/InpaintFilter - params: - input_layer: elevation - output_layer: elevation_inpainted - radius: 0.05 + # - name: inpaint + # type: gridMapCv/InpaintFilter + # params: + # input_layer: elevation + # output_layer: elevation_inpainted + # radius: 0.05 # Reduce noise with a radial blurring filter. - name: mean_in_radius type: gridMapFilters/MeanInRadiusFilter params: - input_layer: elevation_inpainted + input_layer: elevation output_layer: elevation_smooth radius: 0.06 @@ -52,7 +52,7 @@ plane_seg: - name: surface_normals type: gridMapFilters/NormalVectorsFilter params: - input_layer: elevation_inpainted + input_layer: elevation_smooth output_layers_prefix: normal_vectors_ radius: 0.05 normal_vector_positive_axis: z @@ -71,64 +71,3 @@ plane_seg: output_layer: slope expression: acos(normal_vectors_z) - # Compute roughness as absolute difference from map to smoothened map. - - name: roughness - type: gridMapFilters/MathExpressionFilter - params: - output_layer: roughness - expression: abs(elevation_inpainted - elevation_smooth) - - # Edge detection by computing the standard deviation from slope. - - name: edge_detection - type: gridMapFilters/SlidingWindowMathExpressionFilter - params: - input_layer: slope - output_layer: edges - expression: sqrt(sumOfFinites(square(slope - meanOfFinites(slope))) ./ numberOfFinites(slope)) # Standard deviation - compute_empty_cells: false - edge_handling: crop # options: inside, crop, empty, mean - window_length: 0.05 - - # # Edge detection on elevation layer with convolution filter as alternative to filter above. - # - name: edge_detection - # type: gridMapFilters/SlidingWindowMathExpressionFilter - # params: - # input_layer: elevation_inpainted - # output_layer: edges - # expression: 'sumOfFinites([0,1,0;1,-4,1;0,1,0].*elevation_inpainted)' # Edge detection. - # # expression: 'sumOfFinites([0,-1,0;-1,5,-1;0,-1,0].*elevation_inpainted)' # Sharpen. - # compute_empty_cells: false - # edge_handling: mean # options: inside, crop, empty, mean - # window_size: 3 # Make sure to make this compatible with the kernel matrix. - - # Compute traversability as normalized weighted sum of slope and roughness. - - name: traversability - type: gridMapFilters/MathExpressionFilter - params: - output_layer: traversability - expression: 0.5 * (1.0 - (slope / 0.6)) + 0.5 * (1.0 - (roughness / 0.1)) - - # Set lower threshold on traversability. - - name: traversability_lower_threshold - type: gridMapFilters/ThresholdFilter - params: - condition_layer: traversability - output_layer: traversability - lower_threshold: 0.0 - set_to: 0.0 - - # Set upper threshold on traversability. - - name: traversability_upper_threshold - type: gridMapFilters/ThresholdFilter - params: - condition_layer: traversability - output_layer: traversability - upper_threshold: 1.0 - set_to: 1.0 # Other uses: .nan, .inf - - # Duplicate layer. - - name: traversability_clean - type: gridMapFilters/DuplicationFilter - params: - input_layer: traversability - output_layer: traversability_clean diff --git a/plane_seg_ros/config/plane_seg_sequential_config.rviz b/plane_seg_ros/config/plane_seg_sequential_config.rviz new file mode 100644 index 0000000..869b08c --- /dev/null +++ b/plane_seg_ros/config/plane_seg_sequential_config.rviz @@ -0,0 +1,237 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /PointCloud21 + - /Marker1 + - /GridMap1 + - /PoseWithCovariance1 + Splitter Ratio: 0.5058823823928833 + Tree Height: 726 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /plane_seg/hull_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /plane_seg/hull_markers + Name: Marker + Namespaces: + hull lines: true + Queue Size: 100 + Value: true + - Alpha: 0.20000000298023224 + Autocompute Intensity Bounds: true + Class: grid_map_rviz_plugin/GridMap + Color: 200; 200; 200 + Color Layer: elevation + Color Transformer: GridMapLayer + Enabled: true + Height Layer: elevation + Height Transformer: GridMapLayer + History Length: 1 + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 10 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: GridMap + Show Grid Lines: true + Topic: /rooster_elevation_mapping/elevation_map + Unreliable: false + Use Rainbow: true + Value: true + - Alpha: 1 + Axes Length: 0.5 + Axes Radius: 0.05000000074505806 + Class: rviz/PoseWithCovariance + Color: 255; 25; 0 + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Head Length: 0.20000000298023224 + Head Radius: 0.10000000149011612 + Name: PoseWithCovariance + Shaft Length: 0.5 + Shaft Radius: 0.05000000074505806 + Shape: Axes + Topic: /vilens/pose + Unreliable: false + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /plane_seg/id_strings + Name: MarkerArray + Namespaces: + strings: true + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: false + Marker Topic: /plane_seg/centroids + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /plane_seg/linestrips + Name: MarkerArray + Namespaces: + linestrips: true + Queue Size: 100 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: odom + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 6.1330952644348145 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 1.9186757802963257 + Y: 0.04618240147829056 + Z: -0.26492729783058167 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.8402029275894165 + Target Frame: + Value: Orbit (rviz) + Yaw: 4.375433921813965 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1023 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd00000004000000000000015600000361fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000361000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000363000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004400000003efc0100000002fb0000000800540069006d0065010000000000000440000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002e40000036100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1088 + X: 67 + Y: 27 diff --git a/plane_seg_ros/config/rosbag_sequential.yaml b/plane_seg_ros/config/rosbag_sequential.yaml index 0902492..7a9650b 100644 --- a/plane_seg_ros/config/rosbag_sequential.yaml +++ b/plane_seg_ros/config/rosbag_sequential.yaml @@ -3,3 +3,71 @@ plane_seg: run_sequential_test: true run_nth_cloud: false + grid_map_filters: + + - name: buffer_normalizer + type: gridMapFilters/BufferNormalizerFilter + + # # Duplicate layer. + # - name: duplicate + # type: gridMapFilters/DuplicationFilter + # params: + # input_layer: ... + # output_layer: ... + + # Delete color layer. + - name: delete_original_layers + type: gridMapFilters/DeletionFilter + params: + layers: [color] # List of layers. + + # Fill holes in the map with inpainting. + # - name: inpaint + # type: gridMapCv/InpaintFilter + # params: + # input_layer: elevation + # output_layer: elevation_inpainted + # radius: 0.05 + + # Reduce noise with a radial blurring filter. + # - name: mean_in_radius + # type: gridMapFilters/MeanInRadiusFilter + # params: + # input_layer: elevation_inpainted + # output_layer: elevation_smooth + # radius: 0.06 + + # Boxblur as an alternative to the inpaint and radial blurring filter above. + # - name: boxblur + # type: gridMapFilters/SlidingWindowMathExpressionFilter + # params: + # input_layer: elevation + # output_layer: elevation_smooth + # expression: meanOfFinites(elevation) + # compute_empty_cells: true + # edge_handling: crop # options: inside, crop, empty, mean + # window_size: 5 # optional + + # Compute surface normals. + - name: surface_normals + type: gridMapFilters/NormalVectorsFilter + params: + input_layer: elevation + output_layers_prefix: normal_vectors_ + radius: 0.05 + normal_vector_positive_axis: z + + # Add a color layer for visualization based on the surface normal. + - name: normal_color_map + type: gridMapFilters/NormalColorMapFilter + params: + input_layers_prefix: normal_vectors_ + output_layer: normal_color + + # Compute slope from surface normal. + - name: slope + type: gridMapFilters/MathExpressionFilter + params: + output_layer: slope + expression: acos(normal_vectors_z) + diff --git a/plane_seg_ros/include/plane_seg_ros/Pass.hpp b/plane_seg_ros/include/plane_seg_ros/Pass.hpp index 4eb505b..c91f5bf 100644 --- a/plane_seg_ros/include/plane_seg_ros/Pass.hpp +++ b/plane_seg_ros/include/plane_seg_ros/Pass.hpp @@ -50,6 +50,8 @@ class Pass{ void tic(); std::chrono::duration toc(); grid_map_msgs::GridMap gridMapCallback(const grid_map_msgs::GridMap& msg); + void saveGridMapMsgAsPCD(const grid_map_msgs::GridMap& msg, int frame); + void replaceNan(grid_map::GridMap::Matrix& m, const double newValue); private: ros::NodeHandle& node_; diff --git a/plane_seg_ros/launch/rosbag_sequential.launch b/plane_seg_ros/launch/rosbag_sequential.launch index 83b25a5..fc40ac6 100644 --- a/plane_seg_ros/launch/rosbag_sequential.launch +++ b/plane_seg_ros/launch/rosbag_sequential.launch @@ -6,7 +6,7 @@ -> +> diff --git a/plane_seg_ros/src/plane_seg_ros.cpp b/plane_seg_ros/src/plane_seg_ros.cpp index d1a31c6..6522b78 100644 --- a/plane_seg_ros/src/plane_seg_ros.cpp +++ b/plane_seg_ros/src/plane_seg_ros.cpp @@ -259,6 +259,7 @@ void Pass::stepThroughFile(std::string filename){ std::cout << "rosbag time: " << s->info.header.stamp << std::endl; elevationMapCallback(*s); elev_map_pub_.publish(*s); +// saveGridMapMsgAsPCD(*s, frame); std::cout << "Press [Enter] to continue to next gridmap message" << std::endl; } @@ -647,7 +648,7 @@ void Pass::extractNthCloud(std::string filename, int n){ n = gridMapCallback(*s); elevationMapCallback(n); // elev_map_pub_.publish(*s); -// imageProcessingCallback(*s); + imageProcessingCallback(n); } } @@ -667,7 +668,9 @@ bag.close(); void Pass::imageProcessingCallback(const grid_map_msgs::GridMap &msg){ grid_map::GridMap gridmap; grid_map::GridMapRosConverter::fromMessage(msg, gridmap); - grid_map::GridMapRosConverter::toCvImage(gridmap, "elevation", sensor_msgs::image_encodings::MONO8, imgprocessor_.original_img_); + const float nanValue = 1; + replaceNan(gridmap.get("slope"), nanValue); + grid_map::GridMapRosConverter::toCvImage(gridmap, "slope", sensor_msgs::image_encodings::MONO8, imgprocessor_.original_img_); // cv_bridge::CvImage img_rgb; // cv::applyColorMap(image.image, img_rgb.image, cv::COLORMAP_JET); @@ -704,94 +707,46 @@ grid_map_msgs::GridMap Pass::gridMapCallback(const grid_map_msgs::GridMap& msg){ return failmessage; } -/* DEBUG PRINT INPUT AND OUTPUT LAYERS - std::vector in_layers = input_map.getLayers(); - for(const auto& it : in_layers){ - std::cerr << "||||||||||| IN LAYER: " << it << std::endl; - } - - std::vector layers = output_map.getLayers(); - for(const auto& it : layers){ - std::cerr << "||||||||||| OUT LAYER: " << it << std::endl; - } -*/ - if (verbose_timer_) { std::cout << toc().count() << " ms: filter chain\n"; } tic(); - Eigen::Isometry3d pose_robot = Eigen::Isometry3d::Identity(); - tf::StampedTransform transform; - try { - listener_.waitForTransform("odom", "base", ros::Time(0), ros::Duration(10.0) ); - listener_.lookupTransform("odom", "base", ros::Time(0), transform); - } catch (tf::TransformException ex) { - ROS_ERROR("%s",ex.what()); - } - - tf::transformTFToEigen(transform, pose_robot); - - // Threshold traversability conservatively - grid_map::Matrix& data = output_map["traversability_clean"]; - for (grid_map::GridMapIterator iterator(output_map); !iterator.isPastEnd(); ++iterator){ - const grid_map::Index index(*iterator); - - // make cells very near robot traversable - grid_map::Position pos_cell; - output_map.getPosition(index, pos_cell); - grid_map::Position pos_robot(pose_robot.translation().head(2)); - double dist = (pos_robot - pos_cell).norm(); - if (dist <1.0){ - data(index(0), index(1)) = 1.0; - } - - if (data(index(0), index(1)) < traversability_threshold_){ - data(index(0), index(1)) =0.0; - }else{ - data(index(0), index(1)) =1.0; - } - } - - // Erode the traversable image - cv::Mat original_image, erode_image; - grid_map::GridMapCvConverter::toImage(output_map, "traversability_clean", CV_16UC1, 0.0, 0.3, original_image); - //std::cout << "write original\n"; - //cv::imwrite( "original_image.png", original_image); - - int erode_size = int(floor(erode_radius_ / msg.info.resolution)); // was 25 for conservative - cv::Mat element = cv::getStructuringElement(cv::MORPH_ELLIPSE, - cv::Size(2*erode_size + 1, 2*erode_size + 1), - cv::Point(erode_size, erode_size)); - /// Apply the dilation operation - cv::erode(original_image, erode_image, element); - //std::cout << "write eroded\n"; - //cv::imwrite( "erode_image.png", erode_image); - grid_map::GridMapCvConverter::addLayerFromImage(erode_image, "traversability_clean_eroded", output_map, 0.0, 0.3); - - if (verbose_timer_) { - std::cout << toc().count() << " ms: traversability edition (i.e. erosion)\n"; - } - // Publish filtered output grid map. grid_map_msgs::GridMap output_msg; grid_map::GridMapRosConverter::toMessage(output_map, output_msg); filtered_map_pub_.publish(output_msg); - std::vector outlayers, inlayers; - outlayers = output_map.getLayers(); - inlayers = input_map.getLayers(); - std::cout << "Input map layers: " << std::endl; - for (size_t i=0; i point_cloud; + pcl::fromROSMsg(pointCloud_sensor_msg, point_cloud); - return output_msg; + std::string pcd_filename; + pcd_filename = "/home/christos/rosbags/pcd_by_frame/pcd_frame_" + std::to_string(frame) + ".pcd"; + + pcl::io::savePCDFile(pcd_filename, point_cloud); + std::cout << "Saved " << point_cloud.size () << " data points to " << pcd_filename << std::endl; +} +void Pass::replaceNan(grid_map::GridMap::Matrix& m, const double newValue) +{ + for(int r = 0; r < m.rows(); r++) + { + for(int c = 0; c < m.cols(); c++) + { + if (std::isnan(m(r,c))) + { + m(r,c) = newValue; + } + } + } } From dcc58d739f4d9f6e86bc5792323c858582d10be5 Mon Sep 17 00:00:00 2001 From: Christos Ioannou Date: Mon, 8 Feb 2021 01:47:08 +0000 Subject: [PATCH 47/58] created new layer with processed gridmap and used to mask elevation layer --- .../include/plane_seg/ImageProcessor.hpp | 47 ++- plane_seg/src/ImageProcessor.cpp | 273 ++++++++++++------ plane_seg/src/PlaneSegmenter.cpp | 2 +- .../config/elevation_map_filters_config.rviz | 20 +- plane_seg_ros/config/nth_cloud.yaml | 11 + plane_seg_ros/include/plane_seg_ros/Pass.hpp | 5 +- .../launch/plane_seg_sequential_config.rviz | 237 +++++++++++++++ plane_seg_ros/src/plane_seg_ros.cpp | 78 ++++- 8 files changed, 553 insertions(+), 120 deletions(-) create mode 100644 plane_seg_ros/launch/plane_seg_sequential_config.rviz diff --git a/plane_seg/include/plane_seg/ImageProcessor.hpp b/plane_seg/include/plane_seg/ImageProcessor.hpp index ca0f4d5..9a3f94f 100644 --- a/plane_seg/include/plane_seg/ImageProcessor.hpp +++ b/plane_seg/include/plane_seg/ImageProcessor.hpp @@ -3,29 +3,58 @@ namespace planeseg { +struct contours{ + std::vector> contours_; + void filterSmallContours(); + void filterMinConvexity(int min_convexity); + void filterMinElongation(int min_elongation); + void approxAsPoly(); +}; + + class ImageProcessor { public: ImageProcessor(); ~ImageProcessor(); void process(); - void displayImage(cv_bridge::CvImage image, std::string process); + void copyOrigToProc(); + void displayImage(std::string process); void saveImage(cv_bridge::CvImage image); - void erodeImage(cv_bridge::CvImage originalImage, int erode_size); - void thresholdImage(cv_bridge::CvImage image); - void dilateImage(cv_bridge::CvImage originalImage, int dilate_size); + void erodeImage(int erode_size); + void thresholdImage(int threshold_value); + void dilateImage(int dilate_size); void blobDetector(cv_bridge::CvImage image); void removeBlobs(cv_bridge::CvImage image); + void extractContours(); + void splitContours(); + void mergeContours(); + void drawContours(contours contour, std::string process); + void displayResult(); + void fourierTransform(cv_bridge::CvImage image); cv_bridge::CvImage original_img_; + cv_bridge::CvImage processed_img_; + cv_bridge::CvImage final_img_; + +/* + cv_bridge::CvImage filled_img_; cv_bridge::CvImage bit32_img_; - cv_bridge::CvImage erode_img_; - cv_bridge::CvImage threshold_img_; - cv_bridge::CvImage dilate_img_; - cv_bridge::CvImage adap_threshold_img_; cv_bridge::CvImage detect_img_; cv_bridge::CvImage large_contours_; + cv_bridge::CvImage medium_contours_; cv_bridge::CvImage small_contours_; - std::vector> contours1; + cv_bridge::CvImage poly_img_; + cv_bridge::CvImage convex_img_; + cv_bridge::CvImage elongation_img_; + cv_bridge::CvImage step_img_; + + */ + cv_bridge::CvImage fourier_img1_; + cv_bridge::CvImage fourier_img2_; + cv_bridge::CvImage magI; + contours all_contours_; + contours large_contours_; + contours med_contours_; private: diff --git a/plane_seg/src/ImageProcessor.cpp b/plane_seg/src/ImageProcessor.cpp index 98ba500..e25ca45 100644 --- a/plane_seg/src/ImageProcessor.cpp +++ b/plane_seg/src/ImageProcessor.cpp @@ -6,80 +6,123 @@ namespace planeseg { -ImageProcessor::ImageProcessor(){ -} +void contours::filterMinConvexity(int min_convexity){ -ImageProcessor::~ImageProcessor(){} + std::vector> temp; + temp = contours_; + contours_.clear(); -void ImageProcessor::process(){ - displayImage(original_img_, "original"); -/* - std::cout << "Press 's' to save, 't' to threshold, anything else to close" << std::endl; -// displayImage(original_img_, "white NaN"); - int l = cv::waitKey(0); - if (l == 's'){ - saveImage(original_img_); + for (size_t p = 0; p < temp.size(); ++p){ + std::vector hull; + double hull_perimeter; + double contour_perimeter; + double convexity; + + cv::convexHull(temp[p], hull); + hull_perimeter = cv::arcLength(hull, true); + contour_perimeter = cv::arcLength(temp[p], true); + convexity = hull_perimeter / contour_perimeter; + + if (convexity >= min_convexity){ + contours_.push_back(temp[p]); + } } - else if (l == 't'){ - thresholdImage(original_img_); - displayImage(threshold_img_, "threshold"); - std::cout << "Press 's' to save both images, original then thresholded, or 'd' to dilate!" << std::endl; - int k = cv::waitKey(0); - if (k == 's'){ - saveImage(original_img_); - saveImage(threshold_img_); - } else if (k == 'd'){ - dilateImage(threshold_img_); - displayImage(dilate_img_, "dilated"); - std::cout <<"Press 's' to save all three images, original, threshold, then dilated; 'e' to erode" << std::endl; - int j = cv::waitKey(0); - if (j == 's'){ - saveImage(original_img_); - saveImage(threshold_img_); - saveImage(dilate_img_); - } else if (j == 'e'){ - erodeImage(dilate_img_); - displayImage(erode_img_, "eroded"); - std::cout <<"Press 's' to save all four images, original, threshold, dilated then eroded" << std::endl; - int h = cv::waitKey(0); - if (h == 's'){ - saveImage(original_img_); - saveImage(threshold_img_); - saveImage(dilate_img_); - saveImage(erode_img_); - } - } +} + +void contours::filterMinElongation(int min_elongation){ + + std::vector> temp; + temp = contours_; + contours_.clear(); + + for (size_t r = 0; r < temp.size(); ++r){ + double elongation; + cv::RotatedRect minarea_rect; + minarea_rect = cv::minAreaRect(temp[r]); + double major_axis = std::max(minarea_rect.size.height, minarea_rect.size.width); + double minor_axis = std::min(minarea_rect.size.height, minarea_rect.size.width); + elongation = major_axis / minor_axis; + + if (elongation >= min_elongation){ + contours_.push_back(temp[r]); } } - */ - thresholdImage(original_img_); - erodeImage(threshold_img_, 1); - dilateImage(erode_img_, 1); - dilateImage(dilate_img_, 2); - erodeImage(dilate_img_, 2); - removeBlobs(erode_img_); +} + +void contours::approxAsPoly(){ + + std::vector> temp; + temp = contours_; + contours_.clear(); + + for(size_t l = 0; l < temp.size(); ++l){ + double epsilon; + epsilon = 3; + std::vector approx; + cv::approxPolyDP(temp[l], approx, epsilon, true); + contours_.push_back(approx); + } +} -// blobDetector(erode_img_); -// erodeImage(erode_img_); -// dilateImage(erode_img_); +ImageProcessor::ImageProcessor(){ +} - displayImage(threshold_img_, "threshold"); - displayImage(dilate_img_, "dilated"); - displayImage(erode_img_, "eroded"); - displayImage(large_contours_, "large contours"); - displayImage(small_contours_, "small contours"); +ImageProcessor::~ImageProcessor(){} +void ImageProcessor::process(){ + + copyOrigToProc(); + displayImage("original"); + + thresholdImage(70); + displayImage("threshold"); + erodeImage(1); + dilateImage(1); + dilateImage(2); + erodeImage(2); + displayImage("erode/dilate"); + + extractContours(); + splitContours(); + drawContours(med_contours_, "med_contours"); + drawContours(large_contours_, "large contours"); + med_contours_.filterMinConvexity(0.8); + drawContours(med_contours_, "filtered by convexity"); + med_contours_.filterMinElongation(2.5); + drawContours(med_contours_, "filtered by elongation"); + mergeContours(); + + displayResult(); + final_img_ = processed_img_; int p = cv::waitKey(0); if (p == 's'){ - saveImage(dilate_img_); + saveImage(original_img_); } } +void ImageProcessor::copyOrigToProc(){ + processed_img_ = original_img_; +} -void ImageProcessor::displayImage(cv_bridge::CvImage image, std::string process){ +void ImageProcessor::displayImage(std::string process){ + + cv_bridge::CvImage temp; + temp = processed_img_; cv::namedWindow(process, cv::WINDOW_AUTOSIZE); - cv::imshow(process, image.image); + cv::imshow(process, temp.image); +} + +void ImageProcessor::displayResult(){ + + final_img_.image = cv::Mat::zeros(original_img_.image.size(), CV_8U); + + for(size_t i = 0; i < all_contours_.contours_.size(); ++i){ + cv::drawContours(final_img_.image, all_contours_.contours_, i, cv::Scalar(255), cv::FILLED); + } + + cv::namedWindow("final", cv::WINDOW_AUTOSIZE); + cv::imshow("final", final_img_.image); } void ImageProcessor::saveImage(cv_bridge::CvImage image_){ @@ -104,35 +147,35 @@ void ImageProcessor::saveImage(cv_bridge::CvImage image_){ } } -void ImageProcessor::erodeImage(cv_bridge::CvImage originalImage, int erode_size){ +void ImageProcessor::erodeImage(int erode_size){ cv::Mat element = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(2*erode_size + 1, 2*erode_size + 1), cv::Point(erode_size, erode_size) ); - cv::erode(originalImage.image, erode_img_.image, element); + + cv::erode(processed_img_.image, processed_img_.image, element); } -void ImageProcessor::dilateImage(cv_bridge::CvImage originalImage, int dilate_size){ +void ImageProcessor::dilateImage(int dilate_size){ cv::Mat element = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(2*dilate_size+1, 2*dilate_size +1), cv::Point(dilate_size, dilate_size) ); - cv::dilate(originalImage.image, dilate_img_.image, element); + cv::dilate(processed_img_.image, processed_img_.image, element); } -void ImageProcessor::thresholdImage(cv_bridge::CvImage image){ +void ImageProcessor::thresholdImage(int threshold_value){ - int threshold_value = 70; int threshold_type = cv::ThresholdTypes::THRESH_BINARY_INV; double maxval = 255; - cv::threshold(image.image, threshold_img_.image, threshold_value, maxval, threshold_type); + cv::threshold(processed_img_.image, processed_img_.image, threshold_value, maxval, threshold_type); } - +/* void ImageProcessor::blobDetector(cv_bridge::CvImage image){ cv::SimpleBlobDetector::Params params; @@ -164,38 +207,90 @@ void ImageProcessor::blobDetector(cv_bridge::CvImage image){ cv::drawKeypoints(image.image, keypoints, detect_img_.image, cv::Scalar(0,0,255), cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS); } +*/ +void ImageProcessor::extractContours(){ + cv::findContours(processed_img_.image, all_contours_.contours_, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE); +} -void ImageProcessor::removeBlobs(cv_bridge::CvImage image){ - - large_contours_.image = cv::Mat::zeros(image.image.size(), CV_8U); - small_contours_.image = cv::Mat::zeros(image.image.size(), CV_8U); - std::vector> contours; - std::vector> large_contours; - std::vector> small_contours; - - cv::findContours(image.image, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE); - +void ImageProcessor::splitContours(){ - for(size_t k = 0; k < contours.size(); ++k){ - double area = cv::contourArea(contours[k]); + double area; + for(size_t k = 0; k < all_contours_.contours_.size(); ++k){ + area = fabs(cv::contourArea(all_contours_.contours_[k])); - if (area >= 200){ - large_contours.push_back(contours[k]); - } - else{ - small_contours.push_back(contours[k]); + if (area >= 1000){ + large_contours_.contours_.push_back(all_contours_.contours_[k]); + } else if (area >=200 && area <= 1000){ + med_contours_.contours_.push_back(all_contours_.contours_[k]); } + } +} + +void ImageProcessor::mergeContours(){ + all_contours_.contours_.clear(); + for (size_t v = 0; v < large_contours_.contours_.size(); ++v){ + all_contours_.contours_.push_back(large_contours_.contours_[v]); + } + for (size_t w = 0; w < med_contours_.contours_.size(); ++w){ + all_contours_.contours_.push_back(med_contours_.contours_[w]); + } +} - for(size_t i = 0; i < large_contours.size(); ++i){ - cv::drawContours(large_contours_.image, large_contours, i, cv::Scalar(255), cv::FILLED); - } +void ImageProcessor::drawContours(contours contour, std::string process){ + cv_bridge::CvImage temp; + temp.image = cv::Mat::zeros(original_img_.image.size(), CV_8U); - for(size_t j = 0; j < small_contours.size(); ++j){ - cv::drawContours(small_contours_.image, small_contours, j, cv::Scalar(255), cv::FILLED); - } + for(size_t i = 0; i < contour.contours_.size(); ++i){ + cv::drawContours(temp.image, contour.contours_, i, cv::Scalar(255), cv::FILLED); } + + cv::namedWindow(process, cv::WINDOW_AUTOSIZE); + cv::imshow(process, temp.image); } +void ImageProcessor::fourierTransform(cv_bridge::CvImage image){ + cv::Mat padded; + int m = cv::getOptimalDFTSize(image.image.rows); + int n = cv::getOptimalDFTSize(image.image.cols); + cv::copyMakeBorder(image.image, padded, m - image.image.rows, 0, n - image.image.cols, 0, cv::BORDER_CONSTANT, cv::Scalar::all(0)); + + cv::Mat planes[] = {cv::Mat_(padded), cv::Mat::zeros(padded.size(), CV_32F)}; + cv::Mat complexI; + cv::merge(planes, 2, complexI); + cv::dft(complexI, complexI); + + // compute the magnitude and switch to logarithmic scale + // => log(1 + sqrt(Re(DFT(I))^2 + Im(DFT(I))^2)) + cv::split(complexI, planes); + cv::magnitude(planes[0], planes[1], planes[0]); + magI.image = planes[0]; + + magI.image += cv::Scalar::all(1); + cv::log(magI.image, magI.image); + + // crop the spectrum, if it has an odd number of rows or columns + magI.image = magI.image(cv::Rect(0, 0, magI.image.cols & -2, magI.image.rows & -2)); + + // rearrange the quadrants of Fourier image so that the origin is at the image centre + int cx = magI.image.cols/2; + int cy = magI.image.cols/2; + + cv::Mat q0(magI.image, cv::Rect(0, 0, cx, cy)); // Top-left + cv::Mat q1(magI.image, cv::Rect(cx, 0, cx, cy)); // Top-Right + cv::Mat q2(magI.image, cv::Rect(0, cy, cx, cy)); // Bottom-Left + cv::Mat q3(magI.image, cv::Rect(cx, cy, cx, cy)); // Bottom-Right + + cv::Mat tmp; + q0.copyTo(tmp); + q3.copyTo(q0); + tmp.copyTo(q3); + q1.copyTo(tmp); + q2.copyTo(q1); + tmp.copyTo(q2); + + cv::normalize(magI.image, magI.image, 0, 1, CV_MINMAX); + +} } //namespace planeseg diff --git a/plane_seg/src/PlaneSegmenter.cpp b/plane_seg/src/PlaneSegmenter.cpp index 3c2310c..6bebdcc 100644 --- a/plane_seg/src/PlaneSegmenter.cpp +++ b/plane_seg/src/PlaneSegmenter.cpp @@ -9,7 +9,7 @@ using namespace planeseg; PlaneSegmenter:: PlaneSegmenter() { - setMaxError(0.02); + setMaxError(0.02); //0.02 setMaxAngle(30); setSearchRadius(0.03); setMinPoints(500); diff --git a/plane_seg_ros/config/elevation_map_filters_config.rviz b/plane_seg_ros/config/elevation_map_filters_config.rviz index 080a9bc..d6734c5 100644 --- a/plane_seg_ros/config/elevation_map_filters_config.rviz +++ b/plane_seg_ros/config/elevation_map_filters_config.rviz @@ -181,7 +181,7 @@ Visualization Manager: Color: 200; 200; 200 Color Layer: elevation Color Transformer: GridMapLayer - Enabled: true + Enabled: false Height Layer: elevation Height Transformer: GridMapLayer History Length: 1 @@ -195,7 +195,7 @@ Visualization Manager: Topic: /rooster_elevation_mapping/elevation_map Unreliable: false Use Rainbow: true - Value: true + Value: false - Alpha: 1 Axes Length: 0.5 Axes Radius: 0.05000000074505806 @@ -261,10 +261,10 @@ Visualization Manager: Autocompute Intensity Bounds: true Class: grid_map_rviz_plugin/GridMap Color: 200; 200; 200 - Color Layer: slope + Color Layer: product Color Transformer: GridMapLayer Enabled: true - Height Layer: elevation + Height Layer: product Height Transformer: GridMapLayer History Length: 1 Invert Rainbow: false @@ -315,25 +315,25 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 7.008275508880615 + Distance: 4.408163070678711 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 2.500084638595581 - Y: 0.5421060919761658 - Z: -0.48014968633651733 + X: 2.565027952194214 + Y: 2.0226247310638428 + Z: 1.0681487321853638 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.7902033925056458 + Pitch: 0.5247978568077087 Target Frame: Value: Orbit (rviz) - Yaw: 4.4954609870910645 + Yaw: 0.05730084329843521 Saved: ~ Window Geometry: Displays: diff --git a/plane_seg_ros/config/nth_cloud.yaml b/plane_seg_ros/config/nth_cloud.yaml index 073bdaa..e70ac65 100644 --- a/plane_seg_ros/config/nth_cloud.yaml +++ b/plane_seg_ros/config/nth_cloud.yaml @@ -71,3 +71,14 @@ plane_seg: output_layer: slope expression: acos(normal_vectors_z) +# grid_map_mask_filter: +# +# - name: buffer_normalizer +# type: gridMapFilters/BufferNormalizerFilter + + # Use output from image processor as mask over elevation +# - name: masking +# type: gridMapFilters/MathExpressionFilter +# params: +# output_layer: masked +# expression: elevation * slope diff --git a/plane_seg_ros/include/plane_seg_ros/Pass.hpp b/plane_seg_ros/include/plane_seg_ros/Pass.hpp index c91f5bf..8f4204e 100644 --- a/plane_seg_ros/include/plane_seg_ros/Pass.hpp +++ b/plane_seg_ros/include/plane_seg_ros/Pass.hpp @@ -30,7 +30,7 @@ class Pass{ void elevationMapCallback(const grid_map_msgs::GridMap& msg); void pointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr &msg); void robotPoseCallBack(const geometry_msgs::PoseWithCovarianceStampedConstPtr &msg); - void imageProcessingCallback(const grid_map_msgs::GridMap &msg); + grid_map_msgs::GridMap imageProcessingCallback(const grid_map_msgs::GridMap &msg); void processCloud(planeseg::LabeledCloud::Ptr& inCloud, Eigen::Vector3f origin, Eigen::Vector3f lookDir); void processFromFile(int test_example); @@ -52,6 +52,8 @@ class Pass{ grid_map_msgs::GridMap gridMapCallback(const grid_map_msgs::GridMap& msg); void saveGridMapMsgAsPCD(const grid_map_msgs::GridMap& msg, int frame); void replaceNan(grid_map::GridMap::Matrix& m, const double newValue); + void replaceZeroToNan(grid_map::GridMap::Matrix& m); + void multiplyLayers(grid_map::GridMap::Matrix& factor1, grid_map::GridMap::Matrix& factor2, grid_map::GridMap::Matrix& result); private: ros::NodeHandle& node_; @@ -68,6 +70,7 @@ class Pass{ planeseg::Visualizer visualizer_; planeseg::ImageProcessor imgprocessor_; filters::FilterChain filter_chain_; +// filters::FilterChain mask_filter_; tf::TransformListener listener_; std::string elevation_map_topic_; std::string filter_chain_parameters_name_; diff --git a/plane_seg_ros/launch/plane_seg_sequential_config.rviz b/plane_seg_ros/launch/plane_seg_sequential_config.rviz new file mode 100644 index 0000000..869b08c --- /dev/null +++ b/plane_seg_ros/launch/plane_seg_sequential_config.rviz @@ -0,0 +1,237 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /PointCloud21 + - /Marker1 + - /GridMap1 + - /PoseWithCovariance1 + Splitter Ratio: 0.5058823823928833 + Tree Height: 726 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /plane_seg/hull_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /plane_seg/hull_markers + Name: Marker + Namespaces: + hull lines: true + Queue Size: 100 + Value: true + - Alpha: 0.20000000298023224 + Autocompute Intensity Bounds: true + Class: grid_map_rviz_plugin/GridMap + Color: 200; 200; 200 + Color Layer: elevation + Color Transformer: GridMapLayer + Enabled: true + Height Layer: elevation + Height Transformer: GridMapLayer + History Length: 1 + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 10 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: GridMap + Show Grid Lines: true + Topic: /rooster_elevation_mapping/elevation_map + Unreliable: false + Use Rainbow: true + Value: true + - Alpha: 1 + Axes Length: 0.5 + Axes Radius: 0.05000000074505806 + Class: rviz/PoseWithCovariance + Color: 255; 25; 0 + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Head Length: 0.20000000298023224 + Head Radius: 0.10000000149011612 + Name: PoseWithCovariance + Shaft Length: 0.5 + Shaft Radius: 0.05000000074505806 + Shape: Axes + Topic: /vilens/pose + Unreliable: false + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /plane_seg/id_strings + Name: MarkerArray + Namespaces: + strings: true + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: false + Marker Topic: /plane_seg/centroids + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /plane_seg/linestrips + Name: MarkerArray + Namespaces: + linestrips: true + Queue Size: 100 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: odom + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 6.1330952644348145 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 1.9186757802963257 + Y: 0.04618240147829056 + Z: -0.26492729783058167 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.8402029275894165 + Target Frame: + Value: Orbit (rviz) + Yaw: 4.375433921813965 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1023 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd00000004000000000000015600000361fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000361000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000363000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004400000003efc0100000002fb0000000800540069006d0065010000000000000440000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002e40000036100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1088 + X: 67 + Y: 27 diff --git a/plane_seg_ros/src/plane_seg_ros.cpp b/plane_seg_ros/src/plane_seg_ros.cpp index 6522b78..686689c 100644 --- a/plane_seg_ros/src/plane_seg_ros.cpp +++ b/plane_seg_ros/src/plane_seg_ros.cpp @@ -11,6 +11,7 @@ #include "plane_seg/Tracker.hpp" #include +#include #include #include @@ -106,6 +107,19 @@ Pass::Pass(ros::NodeHandle& node): std::cout << "couldn't configure filter chain" << std::endl; return; } +/* + std::string mask_name = "grid_map_mask_filter"; + XmlRpc::XmlRpcValue config2; + if(!node_.getParam(mask_name, config2)) { + ROS_ERROR("Could not load mask filter configuration from parameter %s, are you sure it was pushed to the parameter server? Assuming that you meant to leave it empty.", mask_name.c_str()); + return; + } + + // Setup mask filter + if (!mask_filter_.configure(mask_name, node_)){ + std::cout << "couldn't configure mask filter" << std::endl; + return; + }*/ colors_ = { 1, 1, 1, // 42 @@ -151,7 +165,7 @@ void Pass::elevationMapCallback(const grid_map_msgs::GridMap& msg){ grid_map::GridMap map; grid_map::GridMapRosConverter::fromMessage(msg, map); sensor_msgs::PointCloud2 pointCloud; - grid_map::GridMapRosConverter::toPointCloud(map, "elevation", pointCloud); + grid_map::GridMapRosConverter::toPointCloud(map, "product", pointCloud); // used to take layer "elevation" planeseg::LabeledCloud::Ptr inCloud(new planeseg::LabeledCloud()); pcl::fromROSMsg(pointCloud, *inCloud); @@ -644,11 +658,12 @@ void Pass::extractNthCloud(std::string filename, int n){ ++frame; if (frame == n){ std::cin.get(); - grid_map_msgs::GridMap n; + grid_map_msgs::GridMap n, p; n = gridMapCallback(*s); - elevationMapCallback(n); // elev_map_pub_.publish(*s); - imageProcessingCallback(n); + p = imageProcessingCallback(n); + + elevationMapCallback(p); } } @@ -665,18 +680,36 @@ void Pass::extractNthCloud(std::string filename, int n){ bag.close(); } -void Pass::imageProcessingCallback(const grid_map_msgs::GridMap &msg){ +grid_map_msgs::GridMap Pass::imageProcessingCallback(const grid_map_msgs::GridMap &msg){ grid_map::GridMap gridmap; grid_map::GridMapRosConverter::fromMessage(msg, gridmap); const float nanValue = 1; replaceNan(gridmap.get("slope"), nanValue); grid_map::GridMapRosConverter::toCvImage(gridmap, "slope", sensor_msgs::image_encodings::MONO8, imgprocessor_.original_img_); -// cv_bridge::CvImage img_rgb; -// cv::applyColorMap(image.image, img_rgb.image, cv::COLORMAP_JET); - imgprocessor_.process(); + imgprocessor_.displayResult(); + sensor_msgs::ImagePtr mask_layer; + mask_layer = imgprocessor_.final_img_.toImageMsg(); + + grid_map::GridMapRosConverter::addLayerFromImage(*mask_layer, "mask", gridmap, 0.0, 1.0); + // need to fix the actual masking of mask over elevation map + // maybe multiply as 0/1 binary image first then replace zeros with NANs? + gridmap.add("product"); + +// gridmap["sum"] = gridmap["mask"] gridmap["elevation"]; + multiplyLayers(gridmap.get("elevation"), gridmap.get("mask"), gridmap.get("product")); + + replaceZeroToNan(gridmap.get("product")); + + + // Publish updated grid map. + grid_map_msgs::GridMap output_msg; + grid_map::GridMapRosConverter::toMessage(gridmap, output_msg); + filtered_map_pub_.publish(output_msg); + + return output_msg; } void Pass::tic(){ @@ -737,8 +770,8 @@ void Pass::saveGridMapMsgAsPCD(const grid_map_msgs::GridMap& msg, int frame){ std::cout << "Saved " << point_cloud.size () << " data points to " << pcd_filename << std::endl; } -void Pass::replaceNan(grid_map::GridMap::Matrix& m, const double newValue) -{ +void Pass::replaceNan(grid_map::GridMap::Matrix& m, const double newValue){ + for(int r = 0; r < m.rows(); r++) { for(int c = 0; c < m.cols(); c++) @@ -750,3 +783,28 @@ void Pass::replaceNan(grid_map::GridMap::Matrix& m, const double newValue) } } } + +void Pass::replaceZeroToNan(grid_map::GridMap::Matrix& m){ + + for(int r = 0; r < m.rows(); r++) + { + for(int c = 0; c < m.cols(); c++) + { + if (m(r,c) == 0) + { + m(r,c) = NAN; + } + } + } +} + +void Pass::multiplyLayers(grid_map::GridMap::Matrix& factor1, grid_map::GridMap::Matrix& factor2, grid_map::GridMap::Matrix& result){ + + for(int r = 0; r < result.rows(); r++) + { + for(int c = 0; c < result.cols(); c++) + { + result(r,c) = factor1(r,c) * factor2(r,c); + } + } +} From 15c2ac045f07a24466b352da0e14eee86255e662 Mon Sep 17 00:00:00 2001 From: Christos Ioannou Date: Tue, 9 Feb 2021 02:26:36 +0000 Subject: [PATCH 48/58] added rectangle fitting function to image processor --- .../include/plane_seg/ImageProcessor.hpp | 17 +--- plane_seg/src/ImageProcessor.cpp | 98 ++++++++++--------- .../config/plane_seg_sequential_config.rviz | 30 +++++- plane_seg_ros/launch/rosbag_sequential.launch | 6 ++ plane_seg_ros/src/plane_seg_ros.cpp | 12 ++- 5 files changed, 98 insertions(+), 65 deletions(-) diff --git a/plane_seg/include/plane_seg/ImageProcessor.hpp b/plane_seg/include/plane_seg/ImageProcessor.hpp index 9a3f94f..689e843 100644 --- a/plane_seg/include/plane_seg/ImageProcessor.hpp +++ b/plane_seg/include/plane_seg/ImageProcessor.hpp @@ -8,6 +8,7 @@ struct contours{ void filterSmallContours(); void filterMinConvexity(int min_convexity); void filterMinElongation(int min_elongation); + void fitMinAreaRect(); void approxAsPoly(); }; @@ -29,26 +30,14 @@ class ImageProcessor { void extractContours(); void splitContours(); void mergeContours(); - void drawContours(contours contour, std::string process); + void drawContoursIP(contours contour, std::string process); void displayResult(); + void reset(); void fourierTransform(cv_bridge::CvImage image); cv_bridge::CvImage original_img_; cv_bridge::CvImage processed_img_; cv_bridge::CvImage final_img_; -/* - cv_bridge::CvImage filled_img_; - cv_bridge::CvImage bit32_img_; - cv_bridge::CvImage detect_img_; - cv_bridge::CvImage large_contours_; - cv_bridge::CvImage medium_contours_; - cv_bridge::CvImage small_contours_; - cv_bridge::CvImage poly_img_; - cv_bridge::CvImage convex_img_; - cv_bridge::CvImage elongation_img_; - cv_bridge::CvImage step_img_; - - */ cv_bridge::CvImage fourier_img1_; cv_bridge::CvImage fourier_img2_; cv_bridge::CvImage magI; diff --git a/plane_seg/src/ImageProcessor.cpp b/plane_seg/src/ImageProcessor.cpp index e25ca45..11036e9 100644 --- a/plane_seg/src/ImageProcessor.cpp +++ b/plane_seg/src/ImageProcessor.cpp @@ -2,7 +2,10 @@ #include #include #include +#include #include +#include +#include namespace planeseg { @@ -64,6 +67,31 @@ void contours::approxAsPoly(){ } } +void contours::fitMinAreaRect(){ + + std::vector> temp; + temp = contours_; + contours_.clear(); + + for (size_t r = 0; r < temp.size(); ++r){ + cv::RotatedRect minarea_rect; + cv::Point2f point2f_box[4]; + minarea_rect = cv::minAreaRect(temp[r]); + minarea_rect.points(point2f_box); + std::vector point_box; + for (int i = 0; i < 4; ++i){ + cv::Point pnt; + pnt.x = point2f_box[i].x; + pnt.y = point2f_box[i].y; + point_box.push_back(pnt); + } + + contours_.push_back(point_box); + + } + +} + ImageProcessor::ImageProcessor(){ } @@ -84,20 +112,21 @@ void ImageProcessor::process(){ extractContours(); splitContours(); - drawContours(med_contours_, "med_contours"); - drawContours(large_contours_, "large contours"); - med_contours_.filterMinConvexity(0.8); - drawContours(med_contours_, "filtered by convexity"); + drawContoursIP(med_contours_, "med_contours"); + drawContoursIP(large_contours_, "large contours"); + med_contours_.filterMinConvexity(0.9); // have another look at the threshold for convexity + drawContoursIP(med_contours_, "filtered by convexity"); med_contours_.filterMinElongation(2.5); - drawContours(med_contours_, "filtered by elongation"); + drawContoursIP(med_contours_, "filtered by elongation"); + med_contours_.fitMinAreaRect(); mergeContours(); displayResult(); - final_img_ = processed_img_; +// final_img_ = processed_img_; int p = cv::waitKey(0); if (p == 's'){ - saveImage(original_img_); + saveImage(final_img_); } } @@ -115,12 +144,13 @@ void ImageProcessor::displayImage(std::string process){ void ImageProcessor::displayResult(){ - final_img_.image = cv::Mat::zeros(original_img_.image.size(), CV_8U); + processed_img_.image = cv::Mat::zeros(original_img_.image.size(), CV_8U); for(size_t i = 0; i < all_contours_.contours_.size(); ++i){ - cv::drawContours(final_img_.image, all_contours_.contours_, i, cv::Scalar(255), cv::FILLED); + cv::drawContours(processed_img_.image, all_contours_.contours_, i, cv::Scalar(255), cv::FILLED); } + final_img_ = processed_img_; cv::namedWindow("final", cv::WINDOW_AUTOSIZE); cv::imshow("final", final_img_.image); } @@ -131,7 +161,7 @@ void ImageProcessor::saveImage(cv_bridge::CvImage image_){ std::cin >> imagename; std::string homedir = getenv("HOME"); if(!homedir.empty()){ - boost::filesystem::path output_path(homedir + "/rosbags/image_processing"); + boost::filesystem::path output_path(homedir + "/rosbags/image_processing/"); if(!boost::filesystem::is_directory(output_path)){ if(boost::filesystem::create_directory(output_path)){ std::cout << "Creating directory " << output_path.string() << " ... " << std::endl; @@ -175,40 +205,6 @@ void ImageProcessor::thresholdImage(int threshold_value){ } -/* -void ImageProcessor::blobDetector(cv_bridge::CvImage image){ - - cv::SimpleBlobDetector::Params params; - - params.minThreshold = 10; - params.maxThreshold = 100; - params.thresholdStep = 10; - - params.filterByColor = true; - params.blobColor = 255; - - params.filterByArea = true; - params.minArea = 1; - params.maxArea = 200; - - params.filterByCircularity = false; - params.maxCircularity = 0.1; - - params.filterByConvexity = false; - params.minConvexity = 0.87; - - params.filterByInertia = false; - params.maxInertiaRatio = 0.1; - - cv::Ptr detector = cv::SimpleBlobDetector::create(params); - - std::vector keypoints; - detector->detect(image.image, keypoints); - - cv::drawKeypoints(image.image, keypoints, detect_img_.image, cv::Scalar(0,0,255), cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS); -} -*/ - void ImageProcessor::extractContours(){ cv::findContours(processed_img_.image, all_contours_.contours_, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE); } @@ -237,7 +233,7 @@ void ImageProcessor::mergeContours(){ } } -void ImageProcessor::drawContours(contours contour, std::string process){ +void ImageProcessor::drawContoursIP(contours contour, std::string process){ cv_bridge::CvImage temp; temp.image = cv::Mat::zeros(original_img_.image.size(), CV_8U); @@ -245,10 +241,22 @@ void ImageProcessor::drawContours(contours contour, std::string process){ cv::drawContours(temp.image, contour.contours_, i, cv::Scalar(255), cv::FILLED); } +// processed_img_ = temp; +// displayImage(process); cv::namedWindow(process, cv::WINDOW_AUTOSIZE); cv::imshow(process, temp.image); } +/* +void ImageProcessor::reset(){ + original_img_.image = cv::Mat(); + processed_img_.image = cv::Mat(); + final_img_.image = cv::Mat(); + all_contours_.contours_.clear(); + large_contours_.contours_.clear(); + med_contours_.contours_.clear(); +}*/ + void ImageProcessor::fourierTransform(cv_bridge::CvImage image){ cv::Mat padded; int m = cv::getOptimalDFTSize(image.image.rows); diff --git a/plane_seg_ros/config/plane_seg_sequential_config.rviz b/plane_seg_ros/config/plane_seg_sequential_config.rviz index 869b08c..4a04499 100644 --- a/plane_seg_ros/config/plane_seg_sequential_config.rviz +++ b/plane_seg_ros/config/plane_seg_sequential_config.rviz @@ -10,6 +10,7 @@ Panels: - /Marker1 - /GridMap1 - /PoseWithCovariance1 + - /GridMap2 Splitter Ratio: 0.5058823823928833 Tree Height: 726 - Class: rviz/Selection @@ -100,7 +101,7 @@ Visualization Manager: Color: 200; 200; 200 Color Layer: elevation Color Transformer: GridMapLayer - Enabled: true + Enabled: false Height Layer: elevation Height Transformer: GridMapLayer History Length: 1 @@ -114,7 +115,7 @@ Visualization Manager: Topic: /rooster_elevation_mapping/elevation_map Unreliable: false Use Rainbow: true - Value: true + Value: false - Alpha: 1 Axes Length: 0.5 Axes Radius: 0.05000000074505806 @@ -169,6 +170,27 @@ Visualization Manager: linestrips: true Queue Size: 100 Value: true + - Alpha: 0.20000000298023224 + Autocompute Intensity Bounds: true + Class: grid_map_rviz_plugin/GridMap + Color: 200; 200; 200 + Color Layer: elevation + Color Transformer: GridMapLayer + Enabled: true + Height Layer: elevation + Height Transformer: GridMapLayer + History Length: 1 + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 10 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: GridMap + Show Grid Lines: true + Topic: /rooster_elevation_mapping/filtered_map + Unreliable: false + Use Rainbow: true + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -223,7 +245,7 @@ Window Geometry: Height: 1023 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd00000004000000000000015600000361fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000361000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000363000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004400000003efc0100000002fb0000000800540069006d0065010000000000000440000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002e40000036100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000021800000361fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000361000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000363000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000052b0000003efc0100000002fb0000000800540069006d006501000000000000052b000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000030d0000036100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -232,6 +254,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1088 + Width: 1323 X: 67 Y: 27 diff --git a/plane_seg_ros/launch/rosbag_sequential.launch b/plane_seg_ros/launch/rosbag_sequential.launch index fc40ac6..10b5ec9 100644 --- a/plane_seg_ros/launch/rosbag_sequential.launch +++ b/plane_seg_ros/launch/rosbag_sequential.launch @@ -4,6 +4,12 @@ + + + + + + > diff --git a/plane_seg_ros/src/plane_seg_ros.cpp b/plane_seg_ros/src/plane_seg_ros.cpp index 686689c..fd42680 100644 --- a/plane_seg_ros/src/plane_seg_ros.cpp +++ b/plane_seg_ros/src/plane_seg_ros.cpp @@ -271,9 +271,15 @@ void Pass::stepThroughFile(std::string filename){ std::cout << "frames = " << frame << std::endl; // std::cout << "received gridmap at time " << m.getTime().toNSec() << " with resolution:" << s->info.resolution << "and time: " << s->info.header.stamp << std::endl; std::cout << "rosbag time: " << s->info.header.stamp << std::endl; - elevationMapCallback(*s); - elev_map_pub_.publish(*s); +// elevationMapCallback(*s); +// elev_map_pub_.publish(*s); // saveGridMapMsgAsPCD(*s, frame); + grid_map_msgs::GridMap n, p; + n = gridMapCallback(*s); +// elev_map_pub_.publish(*s); + p = imageProcessingCallback(n); + + elevationMapCallback(p); std::cout << "Press [Enter] to continue to next gridmap message" << std::endl; } @@ -681,6 +687,7 @@ bag.close(); } grid_map_msgs::GridMap Pass::imageProcessingCallback(const grid_map_msgs::GridMap &msg){ + grid_map::GridMap gridmap; grid_map::GridMapRosConverter::fromMessage(msg, gridmap); const float nanValue = 1; @@ -691,6 +698,7 @@ grid_map_msgs::GridMap Pass::imageProcessingCallback(const grid_map_msgs::GridMa imgprocessor_.displayResult(); sensor_msgs::ImagePtr mask_layer; mask_layer = imgprocessor_.final_img_.toImageMsg(); +// imgprocessor_.reset(); grid_map::GridMapRosConverter::addLayerFromImage(*mask_layer, "mask", gridmap, 0.0, 1.0); // need to fix the actual masking of mask over elevation map From 910fdcfe53b569b73ef594e4bf573e5f46cda400 Mon Sep 17 00:00:00 2001 From: Christos Ioannou Date: Thu, 11 Feb 2021 18:50:26 +0000 Subject: [PATCH 49/58] convert gridmap to floating point cvimage --- .../include/plane_seg/ImageProcessor.hpp | 29 ++- plane_seg/src/ImageProcessor.cpp | 237 +++++++++++++----- plane_seg_ros/include/plane_seg_ros/Pass.hpp | 1 + plane_seg_ros/launch/nth_cloud.launch | 2 +- plane_seg_ros/src/plane_seg_ros.cpp | 21 +- 5 files changed, 210 insertions(+), 80 deletions(-) diff --git a/plane_seg/include/plane_seg/ImageProcessor.hpp b/plane_seg/include/plane_seg/ImageProcessor.hpp index 689e843..84e374b 100644 --- a/plane_seg/include/plane_seg/ImageProcessor.hpp +++ b/plane_seg/include/plane_seg/ImageProcessor.hpp @@ -3,16 +3,31 @@ namespace planeseg { +//struct colour{ +// double r; +// double g; +// double b; +//}; + struct contours{ std::vector> contours_; + std::vector> contours_rect_; void filterSmallContours(); void filterMinConvexity(int min_convexity); void filterMinElongation(int min_elongation); void fitMinAreaRect(); void approxAsPoly(); +// void setColors(); +// void assignColors(); +// void assignIDs(); +// double getR(int id); +// double getG(int id); +// double getB(int id); +// std::vector contour_colours_; +// std::vector ids; +// std::vector colors_; }; - class ImageProcessor { public: ImageProcessor(); @@ -20,7 +35,8 @@ class ImageProcessor { void process(); void copyOrigToProc(); - void displayImage(std::string process); + void convertImgType(cv::Mat img, int type); + void displayImage(std::string process, cv::Mat img, int n = 0); void saveImage(cv_bridge::CvImage image); void erodeImage(int erode_size); void thresholdImage(int threshold_value); @@ -30,17 +46,16 @@ class ImageProcessor { void extractContours(); void splitContours(); void mergeContours(); - void drawContoursIP(contours contour, std::string process); + void drawContoursIP(contours contour, std::string process, int n = 0); void displayResult(); void reset(); - void fourierTransform(cv_bridge::CvImage image); + void histogram(cv_bridge::CvImage img); + cv::Mat createMask(cv_bridge::CvImage img); cv_bridge::CvImage original_img_; cv_bridge::CvImage processed_img_; cv_bridge::CvImage final_img_; + cv_bridge::CvImage colour_img_; - cv_bridge::CvImage fourier_img1_; - cv_bridge::CvImage fourier_img2_; - cv_bridge::CvImage magI; contours all_contours_; contours large_contours_; contours med_contours_; diff --git a/plane_seg/src/ImageProcessor.cpp b/plane_seg/src/ImageProcessor.cpp index 11036e9..8004990 100644 --- a/plane_seg/src/ImageProcessor.cpp +++ b/plane_seg/src/ImageProcessor.cpp @@ -71,7 +71,7 @@ void contours::fitMinAreaRect(){ std::vector> temp; temp = contours_; - contours_.clear(); + contours_rect_.clear(); for (size_t r = 0; r < temp.size(); ++r){ cv::RotatedRect minarea_rect; @@ -86,12 +86,74 @@ void contours::fitMinAreaRect(){ point_box.push_back(pnt); } - contours_.push_back(point_box); + contours_rect_.push_back(point_box); } +} +/* +void contours::setColors(){ + colors_ = { + 1, 1, 1, // 42 + 255, 255, 120, + 1, 120, 1, + 1, 225, 1, + 120, 255, 1, + 1, 255, 255, + 120, 1, 1, + 255, 120, 255, + 120, 1, 255, + 1, 1, 120, + 255, 255, 255, + 120, 120, 1, + 120, 120, 120, + 1, 1, 255, + 255, 1, 255, + 120, 120, 255, + 120, 255, 120, + 1, 120, 120, + 1, 1, 255, + 255, 1, 1, + 155, 1, 120, + 120, 1, 120, + 255, 120, 1, + 1, 120, 255, + 255, 120, 120, + 1, 255, 120, + 255, 255, 1}; +} +void contours::assignIDs(){ + for (size_t i = 0; i < contours_.size(); ++i){ + ids[i] = i; + } } +void contours::assignColors(){ + for (size_t i = 0; i < contours_.size(); ++i){ + contour_colours_[i].r = getR(ids[i]); + contour_colours_[i].g = getG(ids[i]); + contour_colours_[i].b = getB(ids[i]); + } +} + +double contours::getR(int id){ + double j; + j = id % (colors_.size()/3); + return colors_[3*j]; +} + +double contours::getG(int id){ + unsigned j; + j = id % (colors_.size()/3); + return colors_[3*j+1]; +} + +double contours::getB(int id){ + unsigned j; + j = id % (colors_.size()/3); + return colors_[3*j+2]; +} +*/ ImageProcessor::ImageProcessor(){ } @@ -99,27 +161,35 @@ ImageProcessor::~ImageProcessor(){} void ImageProcessor::process(){ - copyOrigToProc(); - displayImage("original"); - - thresholdImage(70); - displayImage("threshold"); +// copyOrigToProc(); + displayImage("original", original_img_.image, 0); +// double min, max; +// cv::minMaxLoc(original_img_.image, &min, &max); +// std::cout << "min = " << min << ", max = " << max << std::endl; + histogram(original_img_); + thresholdImage(1); + convertImgType(processed_img_.image, CV_8U); + std::cout << "processed_img_ has type " << processed_img_.image.type() << std::endl; + displayImage("threshold", processed_img_.image, 1); erodeImage(1); - dilateImage(1); dilateImage(2); - erodeImage(2); - displayImage("erode/dilate"); + erodeImage(1); +// erodeImage(2); +// dilateImage(2); + displayImage("erode/dilate", processed_img_.image, 2); extractContours(); splitContours(); - drawContoursIP(med_contours_, "med_contours"); - drawContoursIP(large_contours_, "large contours"); + drawContoursIP(med_contours_, "med_contours", 3); + drawContoursIP(large_contours_, "large contours", 4); med_contours_.filterMinConvexity(0.9); // have another look at the threshold for convexity - drawContoursIP(med_contours_, "filtered by convexity"); + drawContoursIP(med_contours_, "filtered by convexity", 5); med_contours_.filterMinElongation(2.5); - drawContoursIP(med_contours_, "filtered by elongation"); + drawContoursIP(med_contours_, "filtered by elongation", 6); med_contours_.fitMinAreaRect(); mergeContours(); +// all_contours_.assignIDs(); +// all_contours_.assignColors(); displayResult(); // final_img_ = processed_img_; @@ -134,25 +204,38 @@ void ImageProcessor::copyOrigToProc(){ processed_img_ = original_img_; } -void ImageProcessor::displayImage(std::string process){ +void ImageProcessor::convertImgType(cv::Mat img, int type){ + img.convertTo(img, type); +} - cv_bridge::CvImage temp; - temp = processed_img_; +void ImageProcessor::displayImage(std::string process, cv::Mat img, int n) { + + std::cout << "Entered displayImage()" << std::endl; cv::namedWindow(process, cv::WINDOW_AUTOSIZE); - cv::imshow(process, temp.image); + std::cout << 3 << std::endl; + cv::moveWindow(process, 100 + img.cols * n, 50); + std::cout << 4 << std::endl; + cv::imshow(process, img); + std::cout << 5 << std::endl; } void ImageProcessor::displayResult(){ + colour_img_.image = cv::Mat::zeros(original_img_.image.size(), CV_8UC3); processed_img_.image = cv::Mat::zeros(original_img_.image.size(), CV_8U); for(size_t i = 0; i < all_contours_.contours_.size(); ++i){ + cv::RNG rng; + cv::Scalar color(rand()&255, rand()&255, rand()&255); + cv::drawContours(colour_img_.image, all_contours_.contours_, i, color, cv::FILLED); cv::drawContours(processed_img_.image, all_contours_.contours_, i, cv::Scalar(255), cv::FILLED); } + for(size_t j = 0; j < all_contours_.contours_rect_.size(); ++j){ + cv::drawContours(colour_img_.image, all_contours_.contours_rect_, j, cv::Scalar(0, 0, 255), 1, cv::LINE_AA); + } final_img_ = processed_img_; - cv::namedWindow("final", cv::WINDOW_AUTOSIZE); - cv::imshow("final", final_img_.image); + displayImage("final", colour_img_.image, 7); } void ImageProcessor::saveImage(cv_bridge::CvImage image_){ @@ -201,7 +284,7 @@ void ImageProcessor::thresholdImage(int threshold_value){ int threshold_type = cv::ThresholdTypes::THRESH_BINARY_INV; double maxval = 255; - cv::threshold(processed_img_.image, processed_img_.image, threshold_value, maxval, threshold_type); + cv::threshold(original_img_.image, processed_img_.image, threshold_value, maxval, threshold_type); } @@ -225,15 +308,19 @@ void ImageProcessor::splitContours(){ void ImageProcessor::mergeContours(){ all_contours_.contours_.clear(); + all_contours_.contours_rect_.clear(); + for (size_t v = 0; v < large_contours_.contours_.size(); ++v){ all_contours_.contours_.push_back(large_contours_.contours_[v]); } for (size_t w = 0; w < med_contours_.contours_.size(); ++w){ all_contours_.contours_.push_back(med_contours_.contours_[w]); } + + all_contours_.contours_rect_ = med_contours_.contours_rect_; } -void ImageProcessor::drawContoursIP(contours contour, std::string process){ +void ImageProcessor::drawContoursIP(contours contour, std::string process, int n){ cv_bridge::CvImage temp; temp.image = cv::Mat::zeros(original_img_.image.size(), CV_8U); @@ -242,12 +329,39 @@ void ImageProcessor::drawContoursIP(contours contour, std::string process){ } // processed_img_ = temp; -// displayImage(process); - cv::namedWindow(process, cv::WINDOW_AUTOSIZE); - cv::imshow(process, temp.image); + displayImage(process, temp.image, n); +// cv::namedWindow(process, cv::WINDOW_AUTOSIZE); +// cv::imshow(process, temp.image); +} + +void ImageProcessor::histogram(cv_bridge::CvImage img){ + int bins = 30; + int histSize = bins; + float range[] = {0, 1}; + const float* histRange = { range }; + bool uniform = true, accumulate = false; + cv::Mat hist, mask; + mask = createMask(img); + std::cout << "Exited createMask()" << std::endl; +// displayImage("mask", mask, 1); + std::cout << "Exited displayImage()" << std::endl; +// convertImgType(mask, CV_8UC1); + cv::calcHist(&img.image, 1, 0, cv::Mat(), hist, 1, &histSize, &histRange, uniform, accumulate); + + int hist_w = 512, hist_h = 400; + int bin_w = cvRound( (double) hist_w/bins ); + + cv::Mat histImage( hist_h, hist_w, CV_8UC3, cv::Scalar(0,0,0) ); + cv::normalize(hist, hist, 0, histImage.rows, cv::NORM_MINMAX, -1, cv::Mat() ); + + for (int i = 1; i < histSize; i++){ + cv::line(histImage, cv::Point(bin_w*(i-1), hist_h - cvRound(hist.at(i-1))), cv::Point(bin_w*(i), hist_h - cvRound(hist.at(i))), cv::Scalar(255, 0, 0), 2, 8, 0); + } + + cv::imshow("original_img_ histogram", histImage); + cv::waitKey(0); } -/* void ImageProcessor::reset(){ original_img_.image = cv::Mat(); processed_img_.image = cv::Mat(); @@ -255,50 +369,35 @@ void ImageProcessor::reset(){ all_contours_.contours_.clear(); large_contours_.contours_.clear(); med_contours_.contours_.clear(); -}*/ - -void ImageProcessor::fourierTransform(cv_bridge::CvImage image){ - cv::Mat padded; - int m = cv::getOptimalDFTSize(image.image.rows); - int n = cv::getOptimalDFTSize(image.image.cols); - cv::copyMakeBorder(image.image, padded, m - image.image.rows, 0, n - image.image.cols, 0, cv::BORDER_CONSTANT, cv::Scalar::all(0)); - - cv::Mat planes[] = {cv::Mat_(padded), cv::Mat::zeros(padded.size(), CV_32F)}; - cv::Mat complexI; - cv::merge(planes, 2, complexI); - cv::dft(complexI, complexI); - - // compute the magnitude and switch to logarithmic scale - // => log(1 + sqrt(Re(DFT(I))^2 + Im(DFT(I))^2)) - cv::split(complexI, planes); - cv::magnitude(planes[0], planes[1], planes[0]); - magI.image = planes[0]; - - magI.image += cv::Scalar::all(1); - cv::log(magI.image, magI.image); - - // crop the spectrum, if it has an odd number of rows or columns - magI.image = magI.image(cv::Rect(0, 0, magI.image.cols & -2, magI.image.rows & -2)); - - // rearrange the quadrants of Fourier image so that the origin is at the image centre - int cx = magI.image.cols/2; - int cy = magI.image.cols/2; - - cv::Mat q0(magI.image, cv::Rect(0, 0, cx, cy)); // Top-left - cv::Mat q1(magI.image, cv::Rect(cx, 0, cx, cy)); // Top-Right - cv::Mat q2(magI.image, cv::Rect(0, cy, cx, cy)); // Bottom-Left - cv::Mat q3(magI.image, cv::Rect(cx, cy, cx, cy)); // Bottom-Right - - cv::Mat tmp; - q0.copyTo(tmp); - q3.copyTo(q0); - tmp.copyTo(q3); - q1.copyTo(tmp); - q2.copyTo(q1); - tmp.copyTo(q2); - - cv::normalize(magI.image, magI.image, 0, 1, CV_MINMAX); +} + + +cv::Mat ImageProcessor::createMask(cv_bridge::CvImage img){ +// std::cout<< img.image << std::endl; + std::cout << "Entered createMask()" << std::endl; + cv::Mat mask_; + mask_ = cv::Mat::zeros(img.image.size(), CV_8UC1); + +// std::cout << "Image size: " << img.image.size() << std::endl; +// std::cout << "Mask size: " << mask.size() << std::endl; + for(int r = 0; r < img.image.rows; r++) + { + for(int c = 0; c < img.image.cols; c++) + { + if (img.image.at(r,c) > 0.64085822 && img.image.at(r,c) < 0.64085824) + { + mask_.at(r,c) = 255; + } + } + } + + std::cout << "Image type: " << img.image.type() << std::endl; + std::cout << "Mask size: " << mask_.type() << std::endl; + +// displayImage("mask", mask, 8); +// cv::waitKey(0); + return mask_; } } //namespace planeseg diff --git a/plane_seg_ros/include/plane_seg_ros/Pass.hpp b/plane_seg_ros/include/plane_seg_ros/Pass.hpp index 8f4204e..29fed4d 100644 --- a/plane_seg_ros/include/plane_seg_ros/Pass.hpp +++ b/plane_seg_ros/include/plane_seg_ros/Pass.hpp @@ -54,6 +54,7 @@ class Pass{ void replaceNan(grid_map::GridMap::Matrix& m, const double newValue); void replaceZeroToNan(grid_map::GridMap::Matrix& m); void multiplyLayers(grid_map::GridMap::Matrix& factor1, grid_map::GridMap::Matrix& factor2, grid_map::GridMap::Matrix& result); + bool convertGridmapToFloatImage(const grid_map::GridMap& gridMap, const std::string& layer, cv_bridge::CvImage& cvImage); private: ros::NodeHandle& node_; diff --git a/plane_seg_ros/launch/nth_cloud.launch b/plane_seg_ros/launch/nth_cloud.launch index 1fcdd59..281344a 100644 --- a/plane_seg_ros/launch/nth_cloud.launch +++ b/plane_seg_ros/launch/nth_cloud.launch @@ -3,7 +3,7 @@ - + diff --git a/plane_seg_ros/src/plane_seg_ros.cpp b/plane_seg_ros/src/plane_seg_ros.cpp index fd42680..015efdb 100644 --- a/plane_seg_ros/src/plane_seg_ros.cpp +++ b/plane_seg_ros/src/plane_seg_ros.cpp @@ -265,7 +265,7 @@ void Pass::stepThroughFile(std::string filename){ grid_map_msgs::GridMap::ConstPtr s = m.instantiate(); if (s != NULL){ - std::cin.get(); +// std::cin.get(); ++frame; std::cout << "frames = " << frame << std::endl; @@ -692,13 +692,16 @@ grid_map_msgs::GridMap Pass::imageProcessingCallback(const grid_map_msgs::GridMa grid_map::GridMapRosConverter::fromMessage(msg, gridmap); const float nanValue = 1; replaceNan(gridmap.get("slope"), nanValue); - grid_map::GridMapRosConverter::toCvImage(gridmap, "slope", sensor_msgs::image_encodings::MONO8, imgprocessor_.original_img_); +// grid_map::GridMapRosConverter::toCvImage(gridmap, "slope", sensor_msgs::image_encodings::TYPE_32FC1, imgprocessor_.original_img_); + convertGridmapToFloatImage(gridmap, "slope", imgprocessor_.original_img_); + std::cout << imgprocessor_.original_img_.image.type() << std::endl; + imgprocessor_.process(); imgprocessor_.displayResult(); sensor_msgs::ImagePtr mask_layer; mask_layer = imgprocessor_.final_img_.toImageMsg(); -// imgprocessor_.reset(); + imgprocessor_.reset(); grid_map::GridMapRosConverter::addLayerFromImage(*mask_layer, "mask", gridmap, 0.0, 1.0); // need to fix the actual masking of mask over elevation map @@ -816,3 +819,15 @@ void Pass::multiplyLayers(grid_map::GridMap::Matrix& factor1, grid_map::GridMap: } } } + +bool Pass::convertGridmapToFloatImage(const grid_map::GridMap& gridMap, const std::string& layer, cv_bridge::CvImage& cvImage){ + + cvImage.header.stamp.fromNSec(gridMap.getTimestamp()); + cvImage.header.frame_id = gridMap.getFrameId(); + cvImage.encoding = CV_32F; + + const float minValue = gridMap.get(layer).minCoeffOfFinites(); + const float maxValue = gridMap.get(layer).maxCoeffOfFinites(); + + return grid_map::GridMapCvConverter::toImage(gridMap, layer, CV_32F, minValue, maxValue, cvImage.image); +} From 5268a7e8b91bc6c900b46cbd633760729772cc9f Mon Sep 17 00:00:00 2001 From: Christos Ioannou Date: Tue, 16 Feb 2021 17:16:05 +0000 Subject: [PATCH 50/58] StepCreator to recreate steps in rviz from cv image --- plane_seg/CMakeLists.txt | 1 + .../include/plane_seg/ImageProcessor.hpp | 43 +++--- plane_seg/include/plane_seg/StepCreator.hpp | 34 ++++ plane_seg/src/ImageProcessor.cpp | 146 +++++++++++------- plane_seg/src/StepCreator.cpp | 87 +++++++++++ .../config/elevation_map_filters_config.rviz | 24 +-- .../config/plane_seg_sequential_config.rviz | 32 ++-- plane_seg_ros/include/plane_seg_ros/Pass.hpp | 5 +- .../include/plane_seg_ros/Visualizer.hpp | 3 + plane_seg_ros/src/Visualizer.cpp | 25 +++ plane_seg_ros/src/plane_seg_ros.cpp | 113 +++----------- 11 files changed, 327 insertions(+), 186 deletions(-) create mode 100644 plane_seg/include/plane_seg/StepCreator.hpp create mode 100644 plane_seg/src/StepCreator.cpp diff --git a/plane_seg/CMakeLists.txt b/plane_seg/CMakeLists.txt index 4a59c1c..0957774 100644 --- a/plane_seg/CMakeLists.txt +++ b/plane_seg/CMakeLists.txt @@ -36,6 +36,7 @@ add_library(${LIB_NAME} SHARED src/BlockFitter.cpp src/Tracker.cpp src/ImageProcessor.cpp + src/StepCreator.cpp ) add_dependencies(plane_seg ${catkin_EXPORTED_TARGETS}) target_link_libraries(plane_seg ${catkin_LIBRARIES}) diff --git a/plane_seg/include/plane_seg/ImageProcessor.hpp b/plane_seg/include/plane_seg/ImageProcessor.hpp index 84e374b..e92eddd 100644 --- a/plane_seg/include/plane_seg/ImageProcessor.hpp +++ b/plane_seg/include/plane_seg/ImageProcessor.hpp @@ -2,30 +2,36 @@ #include namespace planeseg { - -//struct colour{ -// double r; -// double g; -// double b; -//}; - +/* +struct colour{ + double r; + double g; + double b; +}; +*/ struct contours{ std::vector> contours_; std::vector> contours_rect_; void filterSmallContours(); void filterMinConvexity(int min_convexity); void filterMinElongation(int min_elongation); + void filterMinRectangularity(int min_rectangularity); void fitMinAreaRect(); void approxAsPoly(); -// void setColors(); -// void assignColors(); -// void assignIDs(); -// double getR(int id); -// double getG(int id); -// double getB(int id); -// std::vector contour_colours_; -// std::vector ids; -// std::vector colors_; + void fitSquare(); + bool isSquare(std::vector contour_); + +/* + void setColors(); + void assignColors(); + void assignIDs(); + double getR(int id); + double getG(int id); + double getB(int id); + std::vector contour_colours_; + std::vector ids; + std::vector colors_; +*/ }; class ImageProcessor { @@ -35,15 +41,15 @@ class ImageProcessor { void process(); void copyOrigToProc(); - void convertImgType(cv::Mat img, int type); void displayImage(std::string process, cv::Mat img, int n = 0); void saveImage(cv_bridge::CvImage image); void erodeImage(int erode_size); - void thresholdImage(int threshold_value); + void thresholdImage(float threshold_value); void dilateImage(int dilate_size); void blobDetector(cv_bridge::CvImage image); void removeBlobs(cv_bridge::CvImage image); void extractContours(); + void fillContours(); void splitContours(); void mergeContours(); void drawContoursIP(contours contour, std::string process, int n = 0); @@ -53,6 +59,7 @@ class ImageProcessor { cv::Mat createMask(cv_bridge::CvImage img); cv_bridge::CvImage original_img_; cv_bridge::CvImage processed_img_; + cv_bridge::CvImage rect_img_; cv_bridge::CvImage final_img_; cv_bridge::CvImage colour_img_; diff --git a/plane_seg/include/plane_seg/StepCreator.hpp b/plane_seg/include/plane_seg/StepCreator.hpp new file mode 100644 index 0000000..ddf1b13 --- /dev/null +++ b/plane_seg/include/plane_seg/StepCreator.hpp @@ -0,0 +1,34 @@ +#pragma once +#include + +namespace planeseg { + +struct contour{ + std::vector points_; + cv::Scalar elevation_; +}; + + +class StepCreator{ + +public: + StepCreator(); + ~StepCreator(); + + void go(); + void setImages(cv_bridge::CvImage img1, cv_bridge::CvImage img2); + void extractContours(); + void displayImage(std::string process, cv::Mat img, int n); + void maskElevation(cv::Mat mask_); + void reset(); + cv::Mat createMask(contour cntr); + cv_bridge::CvImage processed_; + cv_bridge::CvImage elevation_; + cv_bridge::CvImage elevation_masked_; + std::vector rectangles_; + +private: + +}; + +} // namespace planeseg diff --git a/plane_seg/src/ImageProcessor.cpp b/plane_seg/src/ImageProcessor.cpp index 8004990..bd737f6 100644 --- a/plane_seg/src/ImageProcessor.cpp +++ b/plane_seg/src/ImageProcessor.cpp @@ -6,6 +6,7 @@ #include #include #include +#include namespace planeseg { @@ -52,6 +53,25 @@ void contours::filterMinElongation(int min_elongation){ } } +void contours::filterMinRectangularity(int min_rectangularity){ + + std::vector> temp; + temp = contours_; + contours_.clear(); + + for (size_t r = 0; r < temp.size(); ++r){ + double rectangularity, contour_area; + cv::RotatedRect minarea_rect; + minarea_rect = cv::minAreaRect(temp[r]); + contour_area = cv::contourArea(temp[r]); + rectangularity = contour_area / minarea_rect.size.area(); + + if (rectangularity >= min_rectangularity){ + contours_.push_back(temp[r]); + } + } +} + void contours::approxAsPoly(){ std::vector> temp; @@ -90,6 +110,37 @@ void contours::fitMinAreaRect(){ } } + +bool contours::isSquare(std::vector contour_){ + double elongation, rectangularity, contour_area; + cv::RotatedRect minarea_rect; + minarea_rect = cv::minAreaRect(contour_); + double major_axis = std::max(minarea_rect.size.height, minarea_rect.size.width); + double minor_axis = std::min(minarea_rect.size.height, minarea_rect.size.width); + elongation = major_axis / minor_axis; + + contour_area = cv::contourArea(contour_); + rectangularity = contour_area / minarea_rect.size.area(); + + if(elongation > 0.5 && elongation < 1.5 && rectangularity > 0.75){ + return true; + } else { + return false; + } +} + +void contours::fitSquare(){ + + contours_rect_.clear(); + for(size_t i = 0; i < contours_.size(); ++i){ + if (isSquare(contours_[i])){ + fitMinAreaRect(); + } else { + contours_rect_.push_back(contours_[i]); + } + } +} + /* void contours::setColors(){ colors_ = { @@ -129,6 +180,7 @@ void contours::assignIDs(){ } void contours::assignColors(){ + setColors(); for (size_t i = 0; i < contours_.size(); ++i){ contour_colours_[i].r = getR(ids[i]); contour_colours_[i].g = getG(ids[i]); @@ -161,38 +213,35 @@ ImageProcessor::~ImageProcessor(){} void ImageProcessor::process(){ -// copyOrigToProc(); displayImage("original", original_img_.image, 0); -// double min, max; -// cv::minMaxLoc(original_img_.image, &min, &max); -// std::cout << "min = " << min << ", max = " << max << std::endl; - histogram(original_img_); - thresholdImage(1); - convertImgType(processed_img_.image, CV_8U); - std::cout << "processed_img_ has type " << processed_img_.image.type() << std::endl; - displayImage("threshold", processed_img_.image, 1); +// histogram(original_img_); + thresholdImage(0.2); + + processed_img_.image.convertTo(processed_img_.image, CV_8U); +// displayImage("threshold", processed_img_.image, 1); + erodeImage(1); dilateImage(2); erodeImage(1); -// erodeImage(2); -// dilateImage(2); - displayImage("erode/dilate", processed_img_.image, 2); +// displayImage("erode/dilate", processed_img_.image, 2); extractContours(); splitContours(); - drawContoursIP(med_contours_, "med_contours", 3); - drawContoursIP(large_contours_, "large contours", 4); + med_contours_.filterMinConvexity(0.9); // have another look at the threshold for convexity - drawContoursIP(med_contours_, "filtered by convexity", 5); - med_contours_.filterMinElongation(2.5); - drawContoursIP(med_contours_, "filtered by elongation", 6); +// drawContoursIP(med_contours_, "filtered by convexity", 6); + med_contours_.filterMinElongation(3); +// drawContoursIP(med_contours_, "filtered by elongation", 7); + med_contours_.filterMinRectangularity(0.6); +// drawContoursIP(med_contours_, "filtered by rectangularity", 8); med_contours_.fitMinAreaRect(); + large_contours_.filterMinRectangularity(0.6); + large_contours_.fitSquare(); mergeContours(); // all_contours_.assignIDs(); // all_contours_.assignColors(); displayResult(); -// final_img_ = processed_img_; int p = cv::waitKey(0); if (p == 's'){ @@ -204,38 +253,31 @@ void ImageProcessor::copyOrigToProc(){ processed_img_ = original_img_; } -void ImageProcessor::convertImgType(cv::Mat img, int type){ - img.convertTo(img, type); -} - void ImageProcessor::displayImage(std::string process, cv::Mat img, int n) { - std::cout << "Entered displayImage()" << std::endl; cv::namedWindow(process, cv::WINDOW_AUTOSIZE); - std::cout << 3 << std::endl; cv::moveWindow(process, 100 + img.cols * n, 50); - std::cout << 4 << std::endl; cv::imshow(process, img); - std::cout << 5 << std::endl; } void ImageProcessor::displayResult(){ colour_img_.image = cv::Mat::zeros(original_img_.image.size(), CV_8UC3); - processed_img_.image = cv::Mat::zeros(original_img_.image.size(), CV_8U); + rect_img_.image = cv::Mat::zeros(original_img_.image.size(), CV_8UC1); for(size_t i = 0; i < all_contours_.contours_.size(); ++i){ cv::RNG rng; cv::Scalar color(rand()&255, rand()&255, rand()&255); cv::drawContours(colour_img_.image, all_contours_.contours_, i, color, cv::FILLED); - cv::drawContours(processed_img_.image, all_contours_.contours_, i, cv::Scalar(255), cv::FILLED); +// cv::drawContours(processed_img_.image, all_contours_.contours_, i, cv::Scalar(255), cv::FILLED); } for(size_t j = 0; j < all_contours_.contours_rect_.size(); ++j){ cv::drawContours(colour_img_.image, all_contours_.contours_rect_, j, cv::Scalar(0, 0, 255), 1, cv::LINE_AA); + cv::drawContours(rect_img_.image, all_contours_.contours_rect_, j, cv::Scalar(255), cv::FILLED); } - final_img_ = processed_img_; - displayImage("final", colour_img_.image, 7); + final_img_ = rect_img_; + displayImage("final", colour_img_.image, 9); } void ImageProcessor::saveImage(cv_bridge::CvImage image_){ @@ -249,7 +291,7 @@ void ImageProcessor::saveImage(cv_bridge::CvImage image_){ if(boost::filesystem::create_directory(output_path)){ std::cout << "Creating directory " << output_path.string() << " ... " << std::endl; } else { - std::cerr << "ERROR: directory " << output_path.string() << " does not exists and couln't be created." << std::endl; + std::cerr << "ERROR: directory " << output_path.string() << " does not exists and couldn't be created." << std::endl; return; } } @@ -279,7 +321,7 @@ void ImageProcessor::dilateImage(int dilate_size){ cv::dilate(processed_img_.image, processed_img_.image, element); } -void ImageProcessor::thresholdImage(int threshold_value){ +void ImageProcessor::thresholdImage(float threshold_value){ int threshold_type = cv::ThresholdTypes::THRESH_BINARY_INV; double maxval = 255; @@ -288,6 +330,10 @@ void ImageProcessor::thresholdImage(int threshold_value){ } +void ImageProcessor::fillContours(){ + drawContoursIP(all_contours_, "fill", 3); +} + void ImageProcessor::extractContours(){ cv::findContours(processed_img_.image, all_contours_.contours_, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE); } @@ -310,6 +356,7 @@ void ImageProcessor::mergeContours(){ all_contours_.contours_.clear(); all_contours_.contours_rect_.clear(); + // populate all_contours.contours_ for (size_t v = 0; v < large_contours_.contours_.size(); ++v){ all_contours_.contours_.push_back(large_contours_.contours_[v]); } @@ -317,7 +364,13 @@ void ImageProcessor::mergeContours(){ all_contours_.contours_.push_back(med_contours_.contours_[w]); } - all_contours_.contours_rect_ = med_contours_.contours_rect_; + // populate all_contours.contours_rect_ + for (size_t v = 0; v < large_contours_.contours_rect_.size(); ++v){ + all_contours_.contours_rect_.push_back(large_contours_.contours_rect_[v]); + } + for (size_t w = 0; w < med_contours_.contours_rect_.size(); ++w){ + all_contours_.contours_rect_.push_back(med_contours_.contours_rect_[w]); + } } void ImageProcessor::drawContoursIP(contours contour, std::string process, int n){ @@ -328,10 +381,7 @@ void ImageProcessor::drawContoursIP(contours contour, std::string process, int n cv::drawContours(temp.image, contour.contours_, i, cv::Scalar(255), cv::FILLED); } -// processed_img_ = temp; displayImage(process, temp.image, n); -// cv::namedWindow(process, cv::WINDOW_AUTOSIZE); -// cv::imshow(process, temp.image); } void ImageProcessor::histogram(cv_bridge::CvImage img){ @@ -342,11 +392,9 @@ void ImageProcessor::histogram(cv_bridge::CvImage img){ bool uniform = true, accumulate = false; cv::Mat hist, mask; mask = createMask(img); - std::cout << "Exited createMask()" << std::endl; -// displayImage("mask", mask, 1); - std::cout << "Exited displayImage()" << std::endl; -// convertImgType(mask, CV_8UC1); - cv::calcHist(&img.image, 1, 0, cv::Mat(), hist, 1, &histSize, &histRange, uniform, accumulate); + mask.convertTo(mask, CV_8U); + std::cout << "Mask type: " << mask.type() << std::endl; + cv::calcHist(&img.image, 1, 0, mask, hist, 1, &histSize, &histRange, uniform, accumulate); int hist_w = 512, hist_h = 400; int bin_w = cvRound( (double) hist_w/bins ); @@ -373,29 +421,23 @@ void ImageProcessor::reset(){ cv::Mat ImageProcessor::createMask(cv_bridge::CvImage img){ -// std::cout<< img.image << std::endl; - std::cout << "Entered createMask()" << std::endl; + cv::Mat mask_; - mask_ = cv::Mat::zeros(img.image.size(), CV_8UC1); + mask_ = cv::Mat::ones(img.image.size(), CV_32F); -// std::cout << "Image size: " << img.image.size() << std::endl; -// std::cout << "Mask size: " << mask.size() << std::endl; for(int r = 0; r < img.image.rows; r++) { for(int c = 0; c < img.image.cols; c++) { if (img.image.at(r,c) > 0.64085822 && img.image.at(r,c) < 0.64085824) { - mask_.at(r,c) = 255; + mask_.at(r,c) = 0; } } } - std::cout << "Image type: " << img.image.type() << std::endl; - std::cout << "Mask size: " << mask_.type() << std::endl; - -// displayImage("mask", mask, 8); -// cv::waitKey(0); + displayImage("mask", mask_, 8); + std::cout << "Exited displayImage()" << std::endl; return mask_; } diff --git a/plane_seg/src/StepCreator.cpp b/plane_seg/src/StepCreator.cpp new file mode 100644 index 0000000..d3e86d2 --- /dev/null +++ b/plane_seg/src/StepCreator.cpp @@ -0,0 +1,87 @@ +#include "plane_seg/StepCreator.hpp" +#include +#include +#include +#include + +namespace planeseg { + +StepCreator::StepCreator(){ +} + +StepCreator::~StepCreator(){} + +void StepCreator::go(){ + std::cout << "Entered go" << std::endl; + displayImage("processed", processed_.image, 0); + extractContours(); + + for (size_t i = 0; i < rectangles_.size(); ++i){ + cv::Mat mask; + mask = createMask(rectangles_[i]); + rectangles_[i].elevation_ = cv::mean(elevation_.image, mask); + } + + std::vector> v; + std::vector elev; + elevation_masked_.image = cv::Mat::zeros(processed_.image.size(), CV_32F); + + for (size_t j = 0; j < rectangles_.size(); ++j){ + v.push_back(rectangles_[j].points_); + elev.push_back(rectangles_[j].elevation_); + } + for (size_t k = 0; k < v.size(); ++k){ + cv::drawContours(elevation_masked_.image, v, k, elev[k], cv::FILLED); + } + displayImage("elevation masked", elevation_masked_.image, 4); + +} + +void StepCreator::setImages(cv_bridge::CvImage img1, cv_bridge::CvImage img2){ + processed_ = img1; + elevation_ = img2; +} + +void StepCreator::extractContours(){ + std::vector> pnts; + cv::findContours(processed_.image, pnts, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE); + + for (size_t j = 0; j < pnts.size(); ++j){ + contour temp; + temp.points_ = pnts[j]; + rectangles_.push_back(temp); + } +} + +cv::Mat StepCreator::createMask(contour cntr){ + cv_bridge::CvImage contour_img_; + std::vector> c; + c.push_back(cntr.points_); + contour_img_.image = cv::Mat::zeros(processed_.image.size(), CV_8U); + for(size_t k = 0; k < c.size(); ++k){ + cv::drawContours(contour_img_.image, c, k, cv::Scalar(255), cv::FILLED); + } + return contour_img_.image; +} + +void StepCreator::displayImage(std::string process, cv::Mat img, int n) { + std::cout << "Entered displayImage" << std::endl; + + cv::namedWindow(process, cv::WINDOW_AUTOSIZE); + cv::moveWindow(process, 100 + img.cols * n, 50); + cv::imshow(process, img); + cv::waitKey(0); +} + +void StepCreator::maskElevation(cv::Mat mask_){ + elevation_.image.copyTo(elevation_masked_.image, mask_); +} + +void StepCreator::reset(){ + processed_.image = cv::Mat(); + elevation_.image = cv::Mat(); + elevation_masked_.image = cv::Mat(); + rectangles_.clear(); +} + +} // namespace planeseg diff --git a/plane_seg_ros/config/elevation_map_filters_config.rviz b/plane_seg_ros/config/elevation_map_filters_config.rviz index d6734c5..9af80f5 100644 --- a/plane_seg_ros/config/elevation_map_filters_config.rviz +++ b/plane_seg_ros/config/elevation_map_filters_config.rviz @@ -11,6 +11,7 @@ Panels: - /GridMap1 - /PoseWithCovariance1 - /GridMap2 + - /Polygon1 Splitter Ratio: 0.5058823823928833 Tree Height: 726 - Class: rviz/Selection @@ -278,13 +279,12 @@ Visualization Manager: Unreliable: false Use Rainbow: true Value: true - Head Length: 0.30000001192092896 - Head Radius: 0.10000000149011612 - Name: robot pose - Shaft Length: 1 - Shaft Radius: 0.05000000074505806 - Shape: Axes - Topic: /state_estimator/pose_in_odom + - Alpha: 1 + Class: rviz/Polygon + Color: 25; 255; 0 + Enabled: true + Name: Polygon + Topic: /opencv/rectangles Unreliable: false Value: true Enabled: true @@ -322,18 +322,18 @@ Visualization Manager: Swap Stereo Eyes: false Value: false Focal Point: - X: 2.565027952194214 - Y: 2.0226247310638428 - Z: 1.0681487321853638 + X: 2.0951638221740723 + Y: 1.109635353088379 + Z: 1.0255918502807617 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.5247978568077087 + Pitch: 1.5697963237762451 Target Frame: Value: Orbit (rviz) - Yaw: 0.05730084329843521 + Yaw: 3.1723060607910156 Saved: ~ Window Geometry: Displays: diff --git a/plane_seg_ros/config/plane_seg_sequential_config.rviz b/plane_seg_ros/config/plane_seg_sequential_config.rviz index 4a04499..0ab0f32 100644 --- a/plane_seg_ros/config/plane_seg_sequential_config.rviz +++ b/plane_seg_ros/config/plane_seg_sequential_config.rviz @@ -31,7 +31,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: PointCloud2 + SyncSource: "" Preferences: PromptSaveOnExit: true Toolbars: @@ -92,7 +92,7 @@ Visualization Manager: Marker Topic: /plane_seg/hull_markers Name: Marker Namespaces: - hull lines: true + {} Queue Size: 100 Value: true - Alpha: 0.20000000298023224 @@ -151,7 +151,7 @@ Visualization Manager: Marker Topic: /plane_seg/id_strings Name: MarkerArray Namespaces: - strings: true + {} Queue Size: 100 Value: true - Class: rviz/MarkerArray @@ -167,17 +167,17 @@ Visualization Manager: Marker Topic: /plane_seg/linestrips Name: MarkerArray Namespaces: - linestrips: true + {} Queue Size: 100 Value: true - Alpha: 0.20000000298023224 Autocompute Intensity Bounds: true Class: grid_map_rviz_plugin/GridMap Color: 200; 200; 200 - Color Layer: elevation + Color Layer: product Color Transformer: GridMapLayer Enabled: true - Height Layer: elevation + Height Layer: product Height Transformer: GridMapLayer History Length: 1 Invert Rainbow: false @@ -191,6 +191,14 @@ Visualization Manager: Unreliable: false Use Rainbow: true Value: true + - Alpha: 1 + Class: rviz/Polygon + Color: 25; 255; 0 + Enabled: true + Name: Polygon + Topic: /opencv/rectangles + Unreliable: false + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -219,25 +227,25 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 6.1330952644348145 + Distance: 3.7735743522644043 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 1.9186757802963257 - Y: 0.04618240147829056 - Z: -0.26492729783058167 + X: 0.6848497986793518 + Y: 0.09153835475444794 + Z: -0.7343938946723938 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.8402029275894165 + Pitch: 1.2452025413513184 Target Frame: Value: Orbit (rviz) - Yaw: 4.375433921813965 + Yaw: 3.335430145263672 Saved: ~ Window Geometry: Displays: diff --git a/plane_seg_ros/include/plane_seg_ros/Pass.hpp b/plane_seg_ros/include/plane_seg_ros/Pass.hpp index 29fed4d..3df9be8 100644 --- a/plane_seg_ros/include/plane_seg_ros/Pass.hpp +++ b/plane_seg_ros/include/plane_seg_ros/Pass.hpp @@ -11,6 +11,7 @@ #include #include #include +#include #include #include #include @@ -46,6 +47,7 @@ class Pass{ void publishCentroidsAsSpheres(); void publishHullsAsMarkers(); void publishLineStrips(); + void publishRectangles(); void extractNthCloud(std::string filename, int n); void tic(); std::chrono::duration toc(); @@ -63,13 +65,14 @@ class Pass{ std::vector colors_3; ros::Subscriber point_cloud_sub_, grid_map_sub_, pose_sub_; - ros::Publisher received_cloud_pub_, hull_cloud_pub_, hull_markers_pub_, look_pose_pub_, elev_map_pub_, pose_pub_, id_strings_pub_, centroids_pub_, hulls_pub_, linestrips_pub_, filtered_map_pub_; + ros::Publisher received_cloud_pub_, hull_cloud_pub_, hull_markers_pub_, look_pose_pub_, elev_map_pub_, pose_pub_, id_strings_pub_, centroids_pub_, hulls_pub_, linestrips_pub_, filtered_map_pub_, rectangles_pub_; Eigen::Isometry3d last_robot_pose_; planeseg::BlockFitter::Result result_; planeseg::Tracker tracking_; planeseg::Visualizer visualizer_; planeseg::ImageProcessor imgprocessor_; + planeseg::StepCreator stepcreator_; filters::FilterChain filter_chain_; // filters::FilterChain mask_filter_; tf::TransformListener listener_; diff --git a/plane_seg_ros/include/plane_seg_ros/Visualizer.hpp b/plane_seg_ros/include/plane_seg_ros/Visualizer.hpp index 21f2905..5865967 100644 --- a/plane_seg_ros/include/plane_seg_ros/Visualizer.hpp +++ b/plane_seg_ros/include/plane_seg_ros/Visualizer.hpp @@ -2,6 +2,8 @@ #include #include #include "plane_seg/Tracker.hpp" +#include "plane_seg/StepCreator.hpp" +#include namespace planeseg{ @@ -20,6 +22,7 @@ class Visualizer { visualization_msgs::Marker displayString(planeseg::plane plane); visualization_msgs::Marker displayLineStrip(planeseg::plane plane); visualization_msgs::Marker displayHull(planeseg::plane plane); + geometry_msgs::PolygonStamped displayRectangle(planeseg::contour contour); double getR(int id); double getG(int id); double getB(int id); diff --git a/plane_seg_ros/src/Visualizer.cpp b/plane_seg_ros/src/Visualizer.cpp index 320acc0..5eaecc8 100644 --- a/plane_seg_ros/src/Visualizer.cpp +++ b/plane_seg_ros/src/Visualizer.cpp @@ -1,12 +1,14 @@ #include #include #include "plane_seg_ros/Visualizer.hpp" +//#include "plane_seg/StepCreator.hpp" #include #include #include #include #include "plane_seg/Tracker.hpp" #include +#include #include #include @@ -225,6 +227,29 @@ visualization_msgs::Marker Visualizer::displayHull(planeseg::plane plane){ return hullMarker; } +geometry_msgs::PolygonStamped Visualizer::displayRectangle(planeseg::contour contour){ + + geometry_msgs::PolygonStamped rectMarker; + std::vector pointsGM(4); + for (size_t r = 0; r < contour.points_.size(); ++r){ + + float x, y, z; + x = static_cast(contour.points_[r].x); + y = static_cast(contour.points_[r].y); + z = static_cast(contour.elevation_[0]); + + pointsGM[r].x = x; + pointsGM[r].y = y; + pointsGM[r].z = z; + } + + rectMarker.polygon.points = pointsGM; + rectMarker.header.frame_id = "odom"; + rectMarker.header.stamp = ros::Time(); + + return rectMarker; +} + double Visualizer::getR(int id){ double j; j = id % (colors_.size()/3); diff --git a/plane_seg_ros/src/plane_seg_ros.cpp b/plane_seg_ros/src/plane_seg_ros.cpp index 015efdb..a9b7d10 100644 --- a/plane_seg_ros/src/plane_seg_ros.cpp +++ b/plane_seg_ros/src/plane_seg_ros.cpp @@ -9,6 +9,7 @@ #include "plane_seg/ImageProcessor.hpp" #include "plane_seg/BlockFitter.hpp" #include "plane_seg/Tracker.hpp" +//#include "plane_seg/StepCreator.hpp" #include #include @@ -74,12 +75,14 @@ Pass::Pass(ros::NodeHandle& node): hulls_pub_ = node_.advertise("/plane_seg/hulls", 10); linestrips_pub_ = node_.advertise("/plane_seg/linestrips", 10); filtered_map_pub_ = node_.advertise("/rooster_elevation_mapping/filtered_map", 1, true); + rectangles_pub_ = node_.advertise("/opencv/rectangles", 10); last_robot_pose_ = Eigen::Isometry3d::Identity(); tracking_ = planeseg::Tracker(); visualizer_ = planeseg::Visualizer(); imgprocessor_ = planeseg::ImageProcessor(); + stepcreator_ = planeseg::StepCreator(); if(!node_.param("input_topic", elevation_map_topic_, std::string("/rooster_elevation_mapping/elevation_map"))){ ROS_WARN_STREAM("Couldn't get parameter: input_topic"); @@ -437,86 +440,7 @@ void Pass::publishHullsAsCloud(std::vector< pcl::PointCloud::Ptr } -void Pass::publishHullsAsMarkersOLD(std::vector< pcl::PointCloud::Ptr > cloud_ptrs, - int secs, int nsecs){ - geometry_msgs::Point point; - std_msgs::ColorRGBA point_color; - visualization_msgs::Marker marker; - std::string frameID; - - // define markers - marker.header.frame_id = "odom"; - marker.header.stamp = ros::Time(secs, nsecs); - marker.ns = "hull lines"; - marker.id = 0; - marker.type = visualization_msgs::Marker::LINE_LIST; //visualization_msgs::Marker::POINTS; - marker.action = visualization_msgs::Marker::ADD; - marker.pose.position.x = 0; - marker.pose.position.y = 0; - marker.pose.position.z = 0; - marker.pose.orientation.x = 0.0; - marker.pose.orientation.y = 0.0; - marker.pose.orientation.z = 0.0; - marker.pose.orientation.w = 1.0; - marker.scale.x = 0.03; - marker.scale.y = 0.03; - marker.scale.z = 0.03; - marker.color.a = 1.0; - - for (size_t i = 0; i < cloud_ptrs.size (); i++){ - - int nColor = i % (colors_.size()/3); - double r = colors_[nColor*3]*255.0; - double g = colors_[nColor*3+1]*255.0; - double b = colors_[nColor*3+2]*255.0; - - for (size_t j = 1; j < cloud_ptrs[i]->points.size (); j++){ - point.x = cloud_ptrs[i]->points[j-1].x; - point.y = cloud_ptrs[i]->points[j-1].y; - point.z = cloud_ptrs[i]->points[j-1].z; - point_color.r = r; - point_color.g = g; - point_color.b = b; - point_color.a = 1.0; - marker.colors.push_back(point_color); - marker.points.push_back(point); - - // - point.x = cloud_ptrs[i]->points[j].x; - point.y = cloud_ptrs[i]->points[j].y; - point.z = cloud_ptrs[i]->points[j].z; - point_color.r = r; - point_color.g = g; - point_color.b = b; - point_color.a = 1.0; - marker.colors.push_back(point_color); - marker.points.push_back(point); - } - // start to end line: - point.x = cloud_ptrs[i]->points[0].x; - point.y = cloud_ptrs[i]->points[0].y; - point.z = cloud_ptrs[i]->points[0].z; - point_color.r = r; - point_color.g = g; - point_color.b = b; - point_color.a = 1.0; - marker.colors.push_back(point_color); - marker.points.push_back(point); - - point.x = cloud_ptrs[i]->points[ cloud_ptrs[i]->points.size()-1 ].x; - point.y = cloud_ptrs[i]->points[ cloud_ptrs[i]->points.size()-1 ].y; - point.z = cloud_ptrs[i]->points[ cloud_ptrs[i]->points.size()-1 ].z; - point_color.r = r; - point_color.g = g; - point_color.b = b; - point_color.a = 1.0; - marker.colors.push_back(point_color); - marker.points.push_back(point); - } - marker.frame_locked = true; -// hull_markers_pub_.publish(marker); -} void Pass::publishIdsAsStrings(){ visualization_msgs::MarkerArray strings_array; @@ -642,6 +566,16 @@ void Pass::publishLineStrips(){ linestrips_pub_.publish(linestrips_array); } +void Pass::publishRectangles(){ + std::cout << "Entered publishRectangles" << std::endl; + for (size_t i = 0; i < stepcreator_.rectangles_.size(); ++i){ + geometry_msgs::PolygonStamped rectangle; + std::cout << "2" << std::endl; + rectangle = visualizer_.displayRectangle(stepcreator_.rectangles_[i]); + std::cout << "3" << std::endl; + rectangles_pub_.publish(rectangle); + } +} void Pass::extractNthCloud(std::string filename, int n){ @@ -694,24 +628,18 @@ grid_map_msgs::GridMap Pass::imageProcessingCallback(const grid_map_msgs::GridMa replaceNan(gridmap.get("slope"), nanValue); // grid_map::GridMapRosConverter::toCvImage(gridmap, "slope", sensor_msgs::image_encodings::TYPE_32FC1, imgprocessor_.original_img_); convertGridmapToFloatImage(gridmap, "slope", imgprocessor_.original_img_); - std::cout << imgprocessor_.original_img_.image.type() << std::endl; - imgprocessor_.process(); - imgprocessor_.displayResult(); - sensor_msgs::ImagePtr mask_layer; - mask_layer = imgprocessor_.final_img_.toImageMsg(); + convertGridmapToFloatImage(gridmap, "elevation", stepcreator_.elevation_); + stepcreator_.processed_ = imgprocessor_.final_img_; + stepcreator_.go(); + + grid_map::GridMapCvConverter::addLayerFromImage(imgprocessor_.final_img_.image, "mask", gridmap); +// grid_map::GridMapCvConverter::addLayerFromImage(stepcreator_.elevation_masked_.image, "reconstructed", gridmap); imgprocessor_.reset(); - grid_map::GridMapRosConverter::addLayerFromImage(*mask_layer, "mask", gridmap, 0.0, 1.0); - // need to fix the actual masking of mask over elevation map - // maybe multiply as 0/1 binary image first then replace zeros with NANs? gridmap.add("product"); - - -// gridmap["sum"] = gridmap["mask"] gridmap["elevation"]; multiplyLayers(gridmap.get("elevation"), gridmap.get("mask"), gridmap.get("product")); - replaceZeroToNan(gridmap.get("product")); @@ -719,6 +647,9 @@ grid_map_msgs::GridMap Pass::imageProcessingCallback(const grid_map_msgs::GridMa grid_map_msgs::GridMap output_msg; grid_map::GridMapRosConverter::toMessage(gridmap, output_msg); filtered_map_pub_.publish(output_msg); + std::cout << "1" << std::endl; + publishRectangles(); + stepcreator_.reset(); return output_msg; } From 6437f7983679225cc8af86386f57791ce740027e Mon Sep 17 00:00:00 2001 From: Christos Ioannou Date: Tue, 23 Feb 2021 12:56:33 +0000 Subject: [PATCH 51/58] 3 algorithms working with visualization in rviz --- plane_seg/include/plane_seg/StepCreator.hpp | 4 +- plane_seg/src/ImageProcessor.cpp | 23 +-- plane_seg/src/StepCreator.cpp | 39 +++- plane_seg/src/Tracker.cpp | 1 + .../config/elevation_map_filters_config.rviz | 38 ++-- plane_seg_ros/config/nth_cloud.yaml | 10 +- .../config/plane_seg_sequential_config.rviz | 14 +- plane_seg_ros/include/plane_seg_ros/Pass.hpp | 7 +- .../include/plane_seg_ros/Visualizer.hpp | 2 +- plane_seg_ros/src/Visualizer.cpp | 5 +- plane_seg_ros/src/plane_seg_ros.cpp | 168 +++++++++++++++--- 11 files changed, 236 insertions(+), 75 deletions(-) diff --git a/plane_seg/include/plane_seg/StepCreator.hpp b/plane_seg/include/plane_seg/StepCreator.hpp index ddf1b13..5d7fd6b 100644 --- a/plane_seg/include/plane_seg/StepCreator.hpp +++ b/plane_seg/include/plane_seg/StepCreator.hpp @@ -5,7 +5,7 @@ namespace planeseg { struct contour{ std::vector points_; - cv::Scalar elevation_; + double elevation_; }; @@ -21,10 +21,12 @@ class StepCreator{ void displayImage(std::string process, cv::Mat img, int n); void maskElevation(cv::Mat mask_); void reset(); + double medianMat(cv::Mat Input); cv::Mat createMask(contour cntr); cv_bridge::CvImage processed_; cv_bridge::CvImage elevation_; cv_bridge::CvImage elevation_masked_; + cv_bridge::CvImage reconstructed_; std::vector rectangles_; private: diff --git a/plane_seg/src/ImageProcessor.cpp b/plane_seg/src/ImageProcessor.cpp index bd737f6..343ab24 100644 --- a/plane_seg/src/ImageProcessor.cpp +++ b/plane_seg/src/ImageProcessor.cpp @@ -76,14 +76,14 @@ void contours::approxAsPoly(){ std::vector> temp; temp = contours_; - contours_.clear(); + contours_rect_.clear(); for(size_t l = 0; l < temp.size(); ++l){ double epsilon; epsilon = 3; std::vector approx; cv::approxPolyDP(temp[l], approx, epsilon, true); - contours_.push_back(approx); + contours_rect_.push_back(approx); } } @@ -122,7 +122,7 @@ bool contours::isSquare(std::vector contour_){ contour_area = cv::contourArea(contour_); rectangularity = contour_area / minarea_rect.size.area(); - if(elongation > 0.5 && elongation < 1.5 && rectangularity > 0.75){ + if(elongation > 0.5 && elongation < 1.3 && rectangularity > 0.8){ return true; } else { return false; @@ -213,9 +213,9 @@ ImageProcessor::~ImageProcessor(){} void ImageProcessor::process(){ - displayImage("original", original_img_.image, 0); +// displayImage("original", original_img_.image, 0); // histogram(original_img_); - thresholdImage(0.2); + thresholdImage(0.3); processed_img_.image.convertTo(processed_img_.image, CV_8U); // displayImage("threshold", processed_img_.image, 1); @@ -235,18 +235,20 @@ void ImageProcessor::process(){ med_contours_.filterMinRectangularity(0.6); // drawContoursIP(med_contours_, "filtered by rectangularity", 8); med_contours_.fitMinAreaRect(); - large_contours_.filterMinRectangularity(0.6); - large_contours_.fitSquare(); + large_contours_.approxAsPoly(); +// large_contours_.filterMinRectangularity(0.6); +// large_contours_.fitSquare(); mergeContours(); // all_contours_.assignIDs(); // all_contours_.assignColors(); displayResult(); - +/* int p = cv::waitKey(0); if (p == 's'){ saveImage(final_img_); } + */ } void ImageProcessor::copyOrigToProc(){ @@ -277,7 +279,7 @@ void ImageProcessor::displayResult(){ } final_img_ = rect_img_; - displayImage("final", colour_img_.image, 9); +// displayImage("final", colour_img_.image, 9); } void ImageProcessor::saveImage(cv_bridge::CvImage image_){ @@ -405,6 +407,7 @@ void ImageProcessor::histogram(cv_bridge::CvImage img){ for (int i = 1; i < histSize; i++){ cv::line(histImage, cv::Point(bin_w*(i-1), hist_h - cvRound(hist.at(i-1))), cv::Point(bin_w*(i), hist_h - cvRound(hist.at(i))), cv::Scalar(255, 0, 0), 2, 8, 0); } + std::cout << hist << std::endl; cv::imshow("original_img_ histogram", histImage); cv::waitKey(0); @@ -429,7 +432,7 @@ cv::Mat ImageProcessor::createMask(cv_bridge::CvImage img){ { for(int c = 0; c < img.image.cols; c++) { - if (img.image.at(r,c) > 0.64085822 && img.image.at(r,c) < 0.64085824) + if (img.image.at(r,c) == 1) { mask_.at(r,c) = 0; } diff --git a/plane_seg/src/StepCreator.cpp b/plane_seg/src/StepCreator.cpp index d3e86d2..29bda46 100644 --- a/plane_seg/src/StepCreator.cpp +++ b/plane_seg/src/StepCreator.cpp @@ -13,27 +13,33 @@ StepCreator::~StepCreator(){} void StepCreator::go(){ std::cout << "Entered go" << std::endl; - displayImage("processed", processed_.image, 0); +// displayImage("processed", processed_.image, 0); extractContours(); + for (size_t i = 0; i < rectangles_.size(); ++i){ + elevation_masked_.image = cv::Mat(); cv::Mat mask; mask = createMask(rectangles_[i]); - rectangles_[i].elevation_ = cv::mean(elevation_.image, mask); +// rectangles_[i].elevation_ = cv::mean(elevation_.image, mask); + maskElevation(mask); + rectangles_[i].elevation_ = medianMat(elevation_masked_.image); + + std::cout << medianMat(elevation_masked_.image) << std::endl; } std::vector> v; std::vector elev; - elevation_masked_.image = cv::Mat::zeros(processed_.image.size(), CV_32F); + reconstructed_.image = cv::Mat::zeros(processed_.image.size(), CV_32F); for (size_t j = 0; j < rectangles_.size(); ++j){ v.push_back(rectangles_[j].points_); elev.push_back(rectangles_[j].elevation_); } for (size_t k = 0; k < v.size(); ++k){ - cv::drawContours(elevation_masked_.image, v, k, elev[k], cv::FILLED); + cv::drawContours(reconstructed_.image, v, k, elev[k], cv::FILLED); } - displayImage("elevation masked", elevation_masked_.image, 4); +// displayImage("elevation masked", reconstructed_.image, 4); } @@ -81,7 +87,30 @@ void StepCreator::reset(){ processed_.image = cv::Mat(); elevation_.image = cv::Mat(); elevation_masked_.image = cv::Mat(); + reconstructed_.image = cv::Mat(); rectangles_.clear(); } +double StepCreator::medianMat(cv::Mat Input){ + Input = Input.reshape(0,1); // spread Input Mat to single row + std::vector vecFromMat, vecNoZeros; + Input.copyTo(vecFromMat); // Copy Input Mat to vector vecFromMat + for (size_t i = 0; i < vecFromMat.size(); ++i){ + if (!vecFromMat[i] == 0){ + vecNoZeros.push_back(vecFromMat[i]); + } + } + std::nth_element(vecNoZeros.begin(), vecNoZeros.begin() + vecNoZeros.size() / 2, vecNoZeros.end()); + return vecNoZeros[vecNoZeros.size() / 2]; +} +/* +double StepCreator::medianMat(cv::Mat Input){ + Input = Input.reshape(0,1); // spread Input Mat to single row + std::vector vecFromMat; + Input.copyTo(vecFromMat); // Copy Input Mat to vector vecFromMat + std::sort( vecFromMat.begin(), vecFromMat.end() ); // sort vecFromMat + if (vecFromMat.size()%2==0) {return (vecFromMat[vecFromMat.size()/2-1]+vecFromMat[vecFromMat.size()/2])/2;} // in case of even-numbered matrix + return vecFromMat[(vecFromMat.size()-1)/2]; // odd-number of elements in matrix +} +*/ } // namespace planeseg diff --git a/plane_seg/src/Tracker.cpp b/plane_seg/src/Tracker.cpp index d341b66..c11cf43 100644 --- a/plane_seg/src/Tracker.cpp +++ b/plane_seg/src/Tracker.cpp @@ -83,6 +83,7 @@ int Tracker::get_plane_id(planeseg::plane plane){ distz = fabs(oldStairs[i].centroid.z - plane.centroid.z); + // is distz small enough to suggest that this point cloud represents the next iteration of an existing frame? if(distz < threshold){ // distance = pcl::euclideanDistance(oldStairs[i].centroid, plane.centroid); diff --git a/plane_seg_ros/config/elevation_map_filters_config.rviz b/plane_seg_ros/config/elevation_map_filters_config.rviz index 9af80f5..26ac1d1 100644 --- a/plane_seg_ros/config/elevation_map_filters_config.rviz +++ b/plane_seg_ros/config/elevation_map_filters_config.rviz @@ -11,7 +11,7 @@ Panels: - /GridMap1 - /PoseWithCovariance1 - /GridMap2 - - /Polygon1 + - /MarkerArray4 Splitter Ratio: 0.5058823823928833 Tree Height: 726 - Class: rviz/Selection @@ -224,7 +224,7 @@ Visualization Manager: Scale: 1 Value: true Value: true - Enabled: true + Enabled: false Head Length: 0.20000000298023224 Head Radius: 0.10000000149011612 Name: PoseWithCovariance @@ -233,7 +233,7 @@ Visualization Manager: Shape: Axes Topic: /vilens/pose Unreliable: false - Value: true + Value: false - Class: rviz/MarkerArray Enabled: true Marker Topic: /plane_seg/id_strings @@ -279,13 +279,21 @@ Visualization Manager: Unreliable: false Use Rainbow: true Value: true - - Alpha: 1 - Class: rviz/Polygon - Color: 25; 255; 0 + - Class: rviz/MarkerArray Enabled: true - Name: Polygon - Topic: /opencv/rectangles - Unreliable: false + Marker Topic: /opencv/rectangles + Name: MarkerArray + Namespaces: + rectangles: true + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /opencv/test + Name: Marker + Namespaces: + test: true + Queue Size: 100 Value: true Enabled: true Global Options: @@ -315,25 +323,25 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 4.408163070678711 + Distance: 8.178982734680176 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 2.0951638221740723 - Y: 1.109635353088379 - Z: 1.0255918502807617 + X: 1.707107663154602 + Y: -0.001718271290883422 + Z: -0.5765842795372009 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 1.5697963237762451 + Pitch: 0.4297971725463867 Target Frame: Value: Orbit (rviz) - Yaw: 3.1723060607910156 + Yaw: 3.9073166847229004 Saved: ~ Window Geometry: Displays: diff --git a/plane_seg_ros/config/nth_cloud.yaml b/plane_seg_ros/config/nth_cloud.yaml index e70ac65..a09954d 100644 --- a/plane_seg_ros/config/nth_cloud.yaml +++ b/plane_seg_ros/config/nth_cloud.yaml @@ -58,11 +58,11 @@ plane_seg: normal_vector_positive_axis: z # Add a color layer for visualization based on the surface normal. - - name: normal_color_map - type: gridMapFilters/NormalColorMapFilter - params: - input_layers_prefix: normal_vectors_ - output_layer: normal_color + # - name: normal_color_map + # type: gridMapFilters/NormalColorMapFilter + # params: + # input_layers_prefix: normal_vectors_ + # output_layer: normal_color # Compute slope from surface normal. - name: slope diff --git a/plane_seg_ros/config/plane_seg_sequential_config.rviz b/plane_seg_ros/config/plane_seg_sequential_config.rviz index 0ab0f32..4cbbc7d 100644 --- a/plane_seg_ros/config/plane_seg_sequential_config.rviz +++ b/plane_seg_ros/config/plane_seg_sequential_config.rviz @@ -191,13 +191,13 @@ Visualization Manager: Unreliable: false Use Rainbow: true Value: true - - Alpha: 1 - Class: rviz/Polygon - Color: 25; 255; 0 + - Class: rviz/MarkerArray Enabled: true - Name: Polygon - Topic: /opencv/rectangles - Unreliable: false + Marker Topic: /opencv/rectangles + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 Value: true Enabled: true Global Options: @@ -227,7 +227,7 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 3.7735743522644043 + Distance: 15.087727546691895 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 diff --git a/plane_seg_ros/include/plane_seg_ros/Pass.hpp b/plane_seg_ros/include/plane_seg_ros/Pass.hpp index 3df9be8..bff78e2 100644 --- a/plane_seg_ros/include/plane_seg_ros/Pass.hpp +++ b/plane_seg_ros/include/plane_seg_ros/Pass.hpp @@ -56,7 +56,8 @@ class Pass{ void replaceNan(grid_map::GridMap::Matrix& m, const double newValue); void replaceZeroToNan(grid_map::GridMap::Matrix& m); void multiplyLayers(grid_map::GridMap::Matrix& factor1, grid_map::GridMap::Matrix& factor2, grid_map::GridMap::Matrix& result); - bool convertGridmapToFloatImage(const grid_map::GridMap& gridMap, const std::string& layer, cv_bridge::CvImage& cvImage); + bool convertGridmapToFloatImage(const grid_map::GridMap& gridMap, const std::string& layer, cv_bridge::CvImage& cvImage, bool negative); + bool toImageWithNegatives(const grid_map::GridMap& gridMap, const std::string& layer, const int encoding, const float lowerValue, const float upperValue, cv::Mat& image); private: ros::NodeHandle& node_; @@ -65,7 +66,7 @@ class Pass{ std::vector colors_3; ros::Subscriber point_cloud_sub_, grid_map_sub_, pose_sub_; - ros::Publisher received_cloud_pub_, hull_cloud_pub_, hull_markers_pub_, look_pose_pub_, elev_map_pub_, pose_pub_, id_strings_pub_, centroids_pub_, hulls_pub_, linestrips_pub_, filtered_map_pub_, rectangles_pub_; + ros::Publisher received_cloud_pub_, hull_cloud_pub_, hull_markers_pub_, look_pose_pub_, elev_map_pub_, pose_pub_, id_strings_pub_, centroids_pub_, hulls_pub_, linestrips_pub_, filtered_map_pub_, rectangles_pub_, test_pub_; Eigen::Isometry3d last_robot_pose_; planeseg::BlockFitter::Result result_; @@ -82,6 +83,8 @@ class Pass{ double erode_radius_; double traversability_threshold_; bool verbose_timer_; + float gm_resolution_; + std::vector gm_position_; std::chrono::high_resolution_clock::time_point last_time_; }; } diff --git a/plane_seg_ros/include/plane_seg_ros/Visualizer.hpp b/plane_seg_ros/include/plane_seg_ros/Visualizer.hpp index 5865967..f424526 100644 --- a/plane_seg_ros/include/plane_seg_ros/Visualizer.hpp +++ b/plane_seg_ros/include/plane_seg_ros/Visualizer.hpp @@ -22,7 +22,7 @@ class Visualizer { visualization_msgs::Marker displayString(planeseg::plane plane); visualization_msgs::Marker displayLineStrip(planeseg::plane plane); visualization_msgs::Marker displayHull(planeseg::plane plane); - geometry_msgs::PolygonStamped displayRectangle(planeseg::contour contour); +// geometry_msgs::PolygonStamped displayRectangle(planeseg::contour contour); double getR(int id); double getG(int id); double getB(int id); diff --git a/plane_seg_ros/src/Visualizer.cpp b/plane_seg_ros/src/Visualizer.cpp index 5eaecc8..8980d68 100644 --- a/plane_seg_ros/src/Visualizer.cpp +++ b/plane_seg_ros/src/Visualizer.cpp @@ -8,7 +8,6 @@ #include #include "plane_seg/Tracker.hpp" #include -#include #include #include @@ -226,7 +225,7 @@ visualization_msgs::Marker Visualizer::displayHull(planeseg::plane plane){ return hullMarker; } - +/* geometry_msgs::PolygonStamped Visualizer::displayRectangle(planeseg::contour contour){ geometry_msgs::PolygonStamped rectMarker; @@ -249,7 +248,7 @@ geometry_msgs::PolygonStamped Visualizer::displayRectangle(planeseg::contour con return rectMarker; } - +*/ double Visualizer::getR(int id){ double j; j = id % (colors_.size()/3); diff --git a/plane_seg_ros/src/plane_seg_ros.cpp b/plane_seg_ros/src/plane_seg_ros.cpp index a9b7d10..8aae84b 100644 --- a/plane_seg_ros/src/plane_seg_ros.cpp +++ b/plane_seg_ros/src/plane_seg_ros.cpp @@ -5,6 +5,7 @@ #include #include #include +#include #include "plane_seg/ImageProcessor.hpp" #include "plane_seg/BlockFitter.hpp" @@ -25,6 +26,7 @@ #include #include +#include #include #include #include @@ -75,7 +77,8 @@ Pass::Pass(ros::NodeHandle& node): hulls_pub_ = node_.advertise("/plane_seg/hulls", 10); linestrips_pub_ = node_.advertise("/plane_seg/linestrips", 10); filtered_map_pub_ = node_.advertise("/rooster_elevation_mapping/filtered_map", 1, true); - rectangles_pub_ = node_.advertise("/opencv/rectangles", 10); + rectangles_pub_ = node_.advertise("/opencv/rectangles", 100); + test_pub_ = node_.advertise("/opencv/test", 10); last_robot_pose_ = Eigen::Isometry3d::Identity(); @@ -168,7 +171,7 @@ void Pass::elevationMapCallback(const grid_map_msgs::GridMap& msg){ grid_map::GridMap map; grid_map::GridMapRosConverter::fromMessage(msg, map); sensor_msgs::PointCloud2 pointCloud; - grid_map::GridMapRosConverter::toPointCloud(map, "product", pointCloud); // used to take layer "elevation" + grid_map::GridMapRosConverter::toPointCloud(map, "product", pointCloud); // takes "product" for masking algorithm, else "elevation" planeseg::LabeledCloud::Ptr inCloud(new planeseg::LabeledCloud()); pcl::fromROSMsg(pointCloud, *inCloud); @@ -263,12 +266,15 @@ void Pass::stepThroughFile(std::string filename){ topics.push_back(std::string("/vilens/pose")); rosbag::View view(bag, rosbag::TopicQuery(topics)); + std::vector timing_vector; foreach(rosbag::MessageInstance const m, view){ grid_map_msgs::GridMap::ConstPtr s = m.instantiate(); if (s != NULL){ // std::cin.get(); + tic(); + ++frame; std::cout << "frames = " << frame << std::endl; @@ -282,8 +288,12 @@ void Pass::stepThroughFile(std::string filename){ // elev_map_pub_.publish(*s); p = imageProcessingCallback(n); - elevationMapCallback(p); +// elevationMapCallback(p); std::cout << "Press [Enter] to continue to next gridmap message" << std::endl; + double frame_time; + frame_time = toc().count(); + std::cout << frame_time << " ms: frame_" << frame << std::endl; + timing_vector.push_back(frame_time); } geometry_msgs::PoseWithCovarianceStamped::ConstPtr i = m.instantiate(); @@ -295,6 +305,15 @@ void Pass::stepThroughFile(std::string filename){ } bag.close(); + + + double average, sum; + for (size_t k = 0; k < timing_vector.size(); ++k){ + sum = sum + timing_vector[k]; + } + average = sum / timing_vector.size(); +// average = std::accumulate(timing_vector.begin(), timing_vector.end(), 0.0)/timing_vector.size(); + std::cout << "The average time per frame is" << average << std::endl; } @@ -568,13 +587,62 @@ void Pass::publishLineStrips(){ void Pass::publishRectangles(){ std::cout << "Entered publishRectangles" << std::endl; + visualization_msgs::MarkerArray rectangles_array; + + std::cout << "Elevation values: " << std::endl; for (size_t i = 0; i < stepcreator_.rectangles_.size(); ++i){ - geometry_msgs::PolygonStamped rectangle; - std::cout << "2" << std::endl; - rectangle = visualizer_.displayRectangle(stepcreator_.rectangles_[i]); - std::cout << "3" << std::endl; - rectangles_pub_.publish(rectangle); + + std::cout << stepcreator_.rectangles_[i].elevation_ << std::endl; + + planeseg::contour contour = stepcreator_.rectangles_[i]; + + visualization_msgs::Marker rectMarker; + rectMarker.header.frame_id = "odom"; + rectMarker.header.stamp = ros::Time::now(); + rectMarker.ns = "rectangles"; + rectMarker.id = i; + rectMarker.type = visualization_msgs::Marker::LINE_STRIP; + rectMarker.action = visualization_msgs::Marker::ADD; + rectMarker.pose.orientation.w = 0.0; + rectMarker.scale.x = 0.05; + rectMarker.color.r = 255; + rectMarker.color.g = 255; + rectMarker.color.b = 1; + rectMarker.color.a = 1; + std::vector pointsGM(contour.points_.size()+1); + int count; + + for (size_t r = 0; r < contour.points_.size(); ++r){ + + float x, y, z; + x = (static_cast(contour.points_[r].x) * gm_resolution_); //contour.points_[0].x + y = (static_cast(contour.points_[r].y) * gm_resolution_); //contour.points_[0].y + z = static_cast(contour.elevation_); + + pointsGM[r].x = -y + ((imgprocessor_.final_img_.image.rows / 2) * gm_resolution_) + gm_position_[0]; + pointsGM[r].y = -x + ((imgprocessor_.final_img_.image.cols / 2) * gm_resolution_) + gm_position_[1]; + pointsGM[r].z = z; + count = r; + } + + // re-add first point to close the loop + float x, y, z; + x = (static_cast(contour.points_[0].x) * gm_resolution_); + y = (static_cast(contour.points_[0].y) * gm_resolution_); + z = static_cast(contour.elevation_); + + pointsGM[count+1].x = -y + ((imgprocessor_.final_img_.image.rows / 2) * gm_resolution_) + gm_position_[0]; + pointsGM[count+1].y = -x + ((imgprocessor_.final_img_.image.cols / 2) * gm_resolution_) + gm_position_[1]; + pointsGM[count+1].z = z; + + rectMarker.points = pointsGM; + rectMarker.frame_locked = true; + rectangles_array.markers.push_back(rectMarker); } + + std::cout << "3" << std::endl; + rectangles_pub_.publish(rectangles_array); + } void Pass::extractNthCloud(std::string filename, int n){ @@ -598,12 +666,15 @@ void Pass::extractNthCloud(std::string filename, int n){ ++frame; if (frame == n){ std::cin.get(); + tic(); grid_map_msgs::GridMap n, p; - n = gridMapCallback(*s); +// n = gridMapCallback(*s); // elev_map_pub_.publish(*s); - p = imageProcessingCallback(n); +// p = imageProcessingCallback(n); elevationMapCallback(p); + + std::cout << toc().count() << " ms: frame_" << frame << std::endl; } } @@ -624,23 +695,35 @@ grid_map_msgs::GridMap Pass::imageProcessingCallback(const grid_map_msgs::GridMa grid_map::GridMap gridmap; grid_map::GridMapRosConverter::fromMessage(msg, gridmap); + std::cout << "Gridmap resolution = " << gridmap.getResolution() << std::endl; + std::cout << "Gridmap position = " << gridmap.getPosition()[0] << ", " << gridmap.getPosition()[1] << std::endl; + gm_resolution_ = gridmap.getResolution(); + gm_position_.clear(); + gm_position_.push_back(gridmap.getPosition()[0]); + gm_position_.push_back(gridmap.getPosition()[1]); const float nanValue = 1; replaceNan(gridmap.get("slope"), nanValue); // grid_map::GridMapRosConverter::toCvImage(gridmap, "slope", sensor_msgs::image_encodings::TYPE_32FC1, imgprocessor_.original_img_); - convertGridmapToFloatImage(gridmap, "slope", imgprocessor_.original_img_); + convertGridmapToFloatImage(gridmap, "slope", imgprocessor_.original_img_, true); imgprocessor_.process(); - convertGridmapToFloatImage(gridmap, "elevation", stepcreator_.elevation_); + convertGridmapToFloatImage(gridmap, "elevation", stepcreator_.elevation_, true); + +// double min, max; +// cv::minMaxLoc(stepcreator_.elevation_.image, &min, &max); +// std::cout << "Min = " << min << ", max = " << max << std::endl; + + // ***instead of drawing the image in imgprocessor and reextracting contours - maybe just pass the contours straight to stepcreator?????*** stepcreator_.processed_ = imgprocessor_.final_img_; stepcreator_.go(); +// std::cout << "Image size: " << imgprocessor_.final_img_.image.rows << ", " << imgprocessor_.final_img_.image.cols << std::endl; - grid_map::GridMapCvConverter::addLayerFromImage(imgprocessor_.final_img_.image, "mask", gridmap); -// grid_map::GridMapCvConverter::addLayerFromImage(stepcreator_.elevation_masked_.image, "reconstructed", gridmap); - imgprocessor_.reset(); +// grid_map::GridMapCvConverter::addLayerFromImage(imgprocessor_.final_img_.image, "mask", gridmap); + grid_map::GridMapCvConverter::addLayerFromImage(stepcreator_.elevation_masked_.image, "reconstructed", gridmap); - gridmap.add("product"); - multiplyLayers(gridmap.get("elevation"), gridmap.get("mask"), gridmap.get("product")); - replaceZeroToNan(gridmap.get("product")); +// gridmap.add("product"); +// multiplyLayers(gridmap.get("elevation"), gridmap.get("mask"), gridmap.get("product")); +// replaceZeroToNan(gridmap.get("product")); // Publish updated grid map. @@ -649,6 +732,7 @@ grid_map_msgs::GridMap Pass::imageProcessingCallback(const grid_map_msgs::GridMa filtered_map_pub_.publish(output_msg); std::cout << "1" << std::endl; publishRectangles(); + imgprocessor_.reset(); stepcreator_.reset(); return output_msg; @@ -667,7 +751,7 @@ std::chrono::duration Pass::toc(){ } grid_map_msgs::GridMap Pass::gridMapCallback(const grid_map_msgs::GridMap& msg){ - tic(); +// tic(); // Convert message to map. grid_map::GridMap input_map; @@ -681,13 +765,13 @@ grid_map_msgs::GridMap Pass::gridMapCallback(const grid_map_msgs::GridMap& msg){ grid_map::GridMapRosConverter::toMessage(input_map, failmessage); return failmessage; } - +/* if (verbose_timer_) { std::cout << toc().count() << " ms: filter chain\n"; } tic(); - +*/ // Publish filtered output grid map. grid_map_msgs::GridMap output_msg; grid_map::GridMapRosConverter::toMessage(output_map, output_msg); @@ -751,14 +835,46 @@ void Pass::multiplyLayers(grid_map::GridMap::Matrix& factor1, grid_map::GridMap: } } -bool Pass::convertGridmapToFloatImage(const grid_map::GridMap& gridMap, const std::string& layer, cv_bridge::CvImage& cvImage){ - +bool Pass::convertGridmapToFloatImage(const grid_map::GridMap& gridMap, const std::string& layer, cv_bridge::CvImage& cvImage, bool negative){ + std::cout << "Entered convertGridmapToFloatImage" << std::endl; cvImage.header.stamp.fromNSec(gridMap.getTimestamp()); cvImage.header.frame_id = gridMap.getFrameId(); cvImage.encoding = CV_32F; - const float minValue = gridMap.get(layer).minCoeffOfFinites(); - const float maxValue = gridMap.get(layer).maxCoeffOfFinites(); + if (negative == false) { + return grid_map::GridMapCvConverter::toImage(gridMap, layer, CV_32F, 0, 1, cvImage.image); + } else { + return toImageWithNegatives(gridMap, layer, CV_32F, 0, 1, cvImage.image); + } +} + +bool Pass::toImageWithNegatives(const grid_map::GridMap& gridMap, const std::string& layer, const int encoding, const float lowerValue, const float upperValue, cv::Mat& image){ + + // Initialize image. + if (gridMap.getSize()(0) > 0 && gridMap.getSize()(1) > 0) { + image = cv::Mat::zeros(gridMap.getSize()(0), gridMap.getSize()(1), encoding); + } else { + std::cerr << "Invalid grid map?" << std::endl; + return false; + } + + // Get max image value. + float imageMax; + imageMax = 1; + + grid_map::GridMap map = gridMap; + const grid_map::Matrix& data = map[layer]; + + for (grid_map::GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator) { + const grid_map::Index index(*iterator); + const float& value = data(index(0), index(1)); + if (std::isfinite(value)) { + const float imageValue = (float)(((value - lowerValue) / (upperValue - lowerValue)) * (float)imageMax); + const grid_map::Index imageIndex(iterator.getUnwrappedIndex()); + unsigned int channel = 0; + image.at>(imageIndex(0), imageIndex(1))[channel] = imageValue; + } + } - return grid_map::GridMapCvConverter::toImage(gridMap, layer, CV_32F, minValue, maxValue, cvImage.image); + return true; } From 1a508e090687ff2316fddb90a501d224c05b78b4 Mon Sep 17 00:00:00 2001 From: Christos Ioannou Date: Wed, 24 Feb 2021 17:27:24 +0000 Subject: [PATCH 52/58] choose algorithm as a parameter + cleanup --- plane_seg/include/plane_seg/StepCreator.hpp | 1 + plane_seg/src/StepCreator.cpp | 10 +- plane_seg_ros/config/default.yaml | 80 ++++++++ .../config/plane_seg_sequential_config.rviz | 28 +-- plane_seg_ros/include/plane_seg_ros/Pass.hpp | 11 +- plane_seg_ros/launch/default.launch | 16 ++ plane_seg_ros/launch/nth_cloud.launch | 9 +- plane_seg_ros/launch/rosbag_sequential.launch | 9 +- plane_seg_ros/src/plane_seg_ros.cpp | 177 +++++++++--------- plane_seg_ros/src/plane_seg_ros_node.cpp | 3 +- 10 files changed, 226 insertions(+), 118 deletions(-) create mode 100644 plane_seg_ros/config/default.yaml create mode 100644 plane_seg_ros/launch/default.launch diff --git a/plane_seg/include/plane_seg/StepCreator.hpp b/plane_seg/include/plane_seg/StepCreator.hpp index 5d7fd6b..8aded14 100644 --- a/plane_seg/include/plane_seg/StepCreator.hpp +++ b/plane_seg/include/plane_seg/StepCreator.hpp @@ -28,6 +28,7 @@ class StepCreator{ cv_bridge::CvImage elevation_masked_; cv_bridge::CvImage reconstructed_; std::vector rectangles_; + std::vector> pnts_; private: diff --git a/plane_seg/src/StepCreator.cpp b/plane_seg/src/StepCreator.cpp index 29bda46..78dca1e 100644 --- a/plane_seg/src/StepCreator.cpp +++ b/plane_seg/src/StepCreator.cpp @@ -3,6 +3,7 @@ #include #include #include +#include "plane_seg/ImageProcessor.hpp" namespace planeseg { @@ -49,12 +50,13 @@ void StepCreator::setImages(cv_bridge::CvImage img1, cv_bridge::CvImage img2){ } void StepCreator::extractContours(){ - std::vector> pnts; - cv::findContours(processed_.image, pnts, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE); - for (size_t j = 0; j < pnts.size(); ++j){ +// cv::findContours(processed_.image, pnts, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE); +// pnts_ = + + for (size_t j = 0; j < pnts_.size(); ++j){ contour temp; - temp.points_ = pnts[j]; + temp.points_ = pnts_[j]; rectangles_.push_back(temp); } } diff --git a/plane_seg_ros/config/default.yaml b/plane_seg_ros/config/default.yaml new file mode 100644 index 0000000..322358e --- /dev/null +++ b/plane_seg_ros/config/default.yaml @@ -0,0 +1,80 @@ +run_test_program: false +run_sequential_test: false +run_nth_cloud: false + +input_topic: /rooster_elevation_mapping/elevation_map +erode_radius: 0.11 +traversability_threshold: 0.7 +verbose_timer: true +grid_map_sub_topic: /rooster_elevation_mapping/elevation_map +point_cloud_sub_topic: /plane_seg/point_cloud_in +pose_sub_topic: /state_estimator/pose_in_odom + +grid_map_filters: + + - name: buffer_normalizer + type: gridMapFilters/BufferNormalizerFilter + +# # Duplicate layer. +# - name: duplicate +# type: gridMapFilters/DuplicationFilter +# params: +# input_layer: ... +# output_layer: ... + +# Delete color layer. + - name: delete_original_layers + type: gridMapFilters/DeletionFilter + params: + layers: [color] # List of layers. + +# Fill holes in the map with inpainting. +# - name: inpaint +# type: gridMapCv/InpaintFilter +# params: +# input_layer: elevation +# output_layer: elevation_inpainted +# radius: 0.05 + +# Reduce noise with a radial blurring filter. +# - name: mean_in_radius +# type: gridMapFilters/MeanInRadiusFilter +# params: +# input_layer: elevation_inpainted +# output_layer: elevation_smooth +# radius: 0.06 + +# Boxblur as an alternative to the inpaint and radial blurring filter above. +# - name: boxblur +# type: gridMapFilters/SlidingWindowMathExpressionFilter +# params: +# input_layer: elevation +# output_layer: elevation_smooth +# expression: meanOfFinites(elevation) +# compute_empty_cells: true +# edge_handling: crop # options: inside, crop, empty, mean +# window_size: 5 # optional + +# Compute surface normals. + - name: surface_normals + type: gridMapFilters/NormalVectorsFilter + params: + input_layer: elevation + output_layers_prefix: normal_vectors_ + radius: 0.05 + normal_vector_positive_axis: z + +# Add a color layer for visualization based on the surface normal. +# - name: normal_color_map +# type: gridMapFilters/NormalColorMapFilter +# params: +# input_layers_prefix: normal_vectors_ +# output_layer: normal_color + +# Compute slope from surface normal. + - name: slope + type: gridMapFilters/MathExpressionFilter + params: + output_layer: slope + expression: acos(normal_vectors_z) + diff --git a/plane_seg_ros/config/plane_seg_sequential_config.rviz b/plane_seg_ros/config/plane_seg_sequential_config.rviz index 4cbbc7d..86c38da 100644 --- a/plane_seg_ros/config/plane_seg_sequential_config.rviz +++ b/plane_seg_ros/config/plane_seg_sequential_config.rviz @@ -31,7 +31,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: "" + SyncSource: PointCloud2 Preferences: PromptSaveOnExit: true Toolbars: @@ -92,7 +92,7 @@ Visualization Manager: Marker Topic: /plane_seg/hull_markers Name: Marker Namespaces: - {} + hull lines: true Queue Size: 100 Value: true - Alpha: 0.20000000298023224 @@ -101,7 +101,7 @@ Visualization Manager: Color: 200; 200; 200 Color Layer: elevation Color Transformer: GridMapLayer - Enabled: false + Enabled: true Height Layer: elevation Height Transformer: GridMapLayer History Length: 1 @@ -115,7 +115,7 @@ Visualization Manager: Topic: /rooster_elevation_mapping/elevation_map Unreliable: false Use Rainbow: true - Value: false + Value: true - Alpha: 1 Axes Length: 0.5 Axes Radius: 0.05000000074505806 @@ -147,13 +147,13 @@ Visualization Manager: Unreliable: false Value: true - Class: rviz/MarkerArray - Enabled: true + Enabled: false Marker Topic: /plane_seg/id_strings Name: MarkerArray Namespaces: {} Queue Size: 100 - Value: true + Value: false - Class: rviz/MarkerArray Enabled: false Marker Topic: /plane_seg/centroids @@ -163,13 +163,13 @@ Visualization Manager: Queue Size: 100 Value: false - Class: rviz/MarkerArray - Enabled: true + Enabled: false Marker Topic: /plane_seg/linestrips Name: MarkerArray Namespaces: {} Queue Size: 100 - Value: true + Value: false - Alpha: 0.20000000298023224 Autocompute Intensity Bounds: true Class: grid_map_rviz_plugin/GridMap @@ -227,25 +227,25 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 15.087727546691895 + Distance: 7.339658737182617 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 0.6848497986793518 - Y: 0.09153835475444794 - Z: -0.7343938946723938 + X: 2.9226090908050537 + Y: 0.5629675388336182 + Z: 1.743936538696289 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 1.2452025413513184 + Pitch: 0.34520265460014343 Target Frame: Value: Orbit (rviz) - Yaw: 3.335430145263672 + Yaw: 5.375433921813965 Saved: ~ Window Geometry: Displays: diff --git a/plane_seg_ros/include/plane_seg_ros/Pass.hpp b/plane_seg_ros/include/plane_seg_ros/Pass.hpp index bff78e2..ae89b0c 100644 --- a/plane_seg_ros/include/plane_seg_ros/Pass.hpp +++ b/plane_seg_ros/include/plane_seg_ros/Pass.hpp @@ -31,7 +31,7 @@ class Pass{ void elevationMapCallback(const grid_map_msgs::GridMap& msg); void pointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr &msg); void robotPoseCallBack(const geometry_msgs::PoseWithCovarianceStampedConstPtr &msg); - grid_map_msgs::GridMap imageProcessingCallback(const grid_map_msgs::GridMap &msg); + void imageProcessing(grid_map::GridMap &gridmap); void processCloud(planeseg::LabeledCloud::Ptr& inCloud, Eigen::Vector3f origin, Eigen::Vector3f lookDir); void processFromFile(int test_example); @@ -51,13 +51,16 @@ class Pass{ void extractNthCloud(std::string filename, int n); void tic(); std::chrono::duration toc(); - grid_map_msgs::GridMap gridMapCallback(const grid_map_msgs::GridMap& msg); + void gridMapFilterChain(grid_map::GridMap& input_map); + void stepCreation(grid_map::GridMap &gridmap); + void reset(); void saveGridMapMsgAsPCD(const grid_map_msgs::GridMap& msg, int frame); void replaceNan(grid_map::GridMap::Matrix& m, const double newValue); void replaceZeroToNan(grid_map::GridMap::Matrix& m); void multiplyLayers(grid_map::GridMap::Matrix& factor1, grid_map::GridMap::Matrix& factor2, grid_map::GridMap::Matrix& result); bool convertGridmapToFloatImage(const grid_map::GridMap& gridMap, const std::string& layer, cv_bridge::CvImage& cvImage, bool negative); bool toImageWithNegatives(const grid_map::GridMap& gridMap, const std::string& layer, const int encoding, const float lowerValue, const float upperValue, cv::Mat& image); + void setupSubscribers(); private: ros::NodeHandle& node_; @@ -80,6 +83,10 @@ class Pass{ std::string elevation_map_topic_; std::string filter_chain_parameters_name_; std::string filtered_map_topic_; + std::string grid_map_sub_topic_; + std::string point_cloud_sub_topic_; + std::string pose_sub_topic_; + std::string algorithm_; double erode_radius_; double traversability_threshold_; bool verbose_timer_; diff --git a/plane_seg_ros/launch/default.launch b/plane_seg_ros/launch/default.launch new file mode 100644 index 0000000..8c36af9 --- /dev/null +++ b/plane_seg_ros/launch/default.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + +> + + + diff --git a/plane_seg_ros/launch/nth_cloud.launch b/plane_seg_ros/launch/nth_cloud.launch index 281344a..e8bd6a9 100644 --- a/plane_seg_ros/launch/nth_cloud.launch +++ b/plane_seg_ros/launch/nth_cloud.launch @@ -1,17 +1,14 @@ - + + + - - - - - diff --git a/plane_seg_ros/launch/rosbag_sequential.launch b/plane_seg_ros/launch/rosbag_sequential.launch index 10b5ec9..33c52bf 100644 --- a/plane_seg_ros/launch/rosbag_sequential.launch +++ b/plane_seg_ros/launch/rosbag_sequential.launch @@ -1,15 +1,12 @@ - + + + - - - - - > diff --git a/plane_seg_ros/src/plane_seg_ros.cpp b/plane_seg_ros/src/plane_seg_ros.cpp index 8aae84b..7ae7123 100644 --- a/plane_seg_ros/src/plane_seg_ros.cpp +++ b/plane_seg_ros/src/plane_seg_ros.cpp @@ -55,16 +55,6 @@ Pass::Pass(ros::NodeHandle& node): if(!node_.getParam("input_body_pose_topic", input_body_pose_topic)){ ROS_WARN_STREAM("Couldn't get parameter: input_body_pose_topic"); } -// std::string filename; -// node_.getParm("/rosbag_pass/filename", filename); -/* - grid_map_sub_ = node_.subscribe("/elevation_mapping/elevation_map", 100, - &Pass::elevationMapCallback, this); - point_cloud_sub_ = node_.subscribe("/plane_seg/point_cloud_in", 100, - &Pass::pointCloudCallback, this); - pose_sub_ = node_.subscribe("/state_estimator/pose_in_odom", 100, - &Pass::robotPoseCallBack, this); -*/ received_cloud_pub_ = node_.advertise("/plane_seg/received_cloud", 10); hull_cloud_pub_ = node_.advertise("/plane_seg/hull_cloud", 10); @@ -87,20 +77,38 @@ Pass::Pass(ros::NodeHandle& node): imgprocessor_ = planeseg::ImageProcessor(); stepcreator_ = planeseg::StepCreator(); - if(!node_.param("input_topic", elevation_map_topic_, std::string("/rooster_elevation_mapping/elevation_map"))){ + if(!node_.param("algorithm", algorithm_, std::string("A"))){ + ROS_WARN_STREAM("Couldn't get parameter: algorithm"); + } + ROS_INFO("ALGORITHM %s", algorithm_.c_str()); + + if(!node_.getParam("input_topic", elevation_map_topic_)){ ROS_WARN_STREAM("Couldn't get parameter: input_topic"); } - if(!node_.param("erode_radius", erode_radius_, 0.2)){ + if(!node_.getParam("erode_radius", erode_radius_)){ ROS_WARN_STREAM("Couldn't get parameter: erode_radius"); } ROS_INFO("Erode Radius [%f]", erode_radius_); - if(!node_.param("traversability_threshold", traversability_threshold_, 0.8)){ + if(!node_.getParam("traversability_threshold", traversability_threshold_)){ ROS_WARN_STREAM("Couldn't get parameter: traversability_threshold"); } ROS_INFO("traversability_threshold [%f]", traversability_threshold_); - if(!node_.param("verbose_timer", verbose_timer_, true)){ + if(!node_.getParam("verbose_timer", verbose_timer_)){ ROS_WARN_STREAM("Couldn't get parameter: verbose_timer"); } + if(!node_.getParam("verbose_timer", verbose_timer_)){ + ROS_WARN_STREAM("Couldn't get parameter: verbose_timer"); + } + if(!node_.getParam("grid_map_sub_topic", grid_map_sub_topic_)){ + ROS_WARN_STREAM("Couldn't get parameter: grid_map_sub_topic"); + } + if(!node_.getParam("point_cloud_sub_topic", point_cloud_sub_topic_)){ + ROS_WARN_STREAM("Couldn't get parameter: point_cloud_sub_topic"); + } + if(!node_.getParam("pose_sub_topic", pose_sub_topic_)){ + ROS_WARN_STREAM("Couldn't get parameter: pose_sub_topic"); + } + std::string param_name = "grid_map_filters"; XmlRpc::XmlRpcValue config; if(!node_.getParam(param_name, config)) { @@ -113,19 +121,6 @@ Pass::Pass(ros::NodeHandle& node): std::cout << "couldn't configure filter chain" << std::endl; return; } -/* - std::string mask_name = "grid_map_mask_filter"; - XmlRpc::XmlRpcValue config2; - if(!node_.getParam(mask_name, config2)) { - ROS_ERROR("Could not load mask filter configuration from parameter %s, are you sure it was pushed to the parameter server? Assuming that you meant to leave it empty.", mask_name.c_str()); - return; - } - - // Setup mask filter - if (!mask_filter_.configure(mask_name, node_)){ - std::cout << "couldn't configure mask filter" << std::endl; - return; - }*/ colors_ = { 1, 1, 1, // 42 @@ -170,8 +165,33 @@ void Pass::elevationMapCallback(const grid_map_msgs::GridMap& msg){ // convert message to GridMap, to PointCloud to LabeledCloud grid_map::GridMap map; grid_map::GridMapRosConverter::fromMessage(msg, map); + + if (algorithm_ == "C"){ + std::cout << "C" << std::endl; + gridMapFilterChain(map); + imageProcessing(map); + stepCreation(map); + reset(); + return; + } + + if (algorithm_ == "B"){ + std::cout << "B" << std::endl; + gridMapFilterChain(map); + imageProcessing(map); + reset(); + } + sensor_msgs::PointCloud2 pointCloud; - grid_map::GridMapRosConverter::toPointCloud(map, "product", pointCloud); // takes "product" for masking algorithm, else "elevation" + + if (algorithm_ == "A"){ + std::cout << "A end" << std::endl; + grid_map::GridMapRosConverter::toPointCloud(map, "elevation", pointCloud); // takes "elevation" for legacy algorithm + } else { + std::cout << "B end" << std::endl; + grid_map::GridMapRosConverter::toPointCloud(map, "product", pointCloud); // takes "product" for masking algorithm + } + planeseg::LabeledCloud::Ptr inCloud(new planeseg::LabeledCloud()); pcl::fromROSMsg(pointCloud, *inCloud); @@ -180,6 +200,7 @@ void Pass::elevationMapCallback(const grid_map_msgs::GridMap& msg){ lookDir = convertRobotPoseToSensorLookDir(last_robot_pose_); processCloud(inCloud, origin, lookDir); + } @@ -272,7 +293,7 @@ void Pass::stepThroughFile(std::string filename){ grid_map_msgs::GridMap::ConstPtr s = m.instantiate(); if (s != NULL){ -// std::cin.get(); + std::cin.get(); tic(); ++frame; @@ -280,15 +301,8 @@ void Pass::stepThroughFile(std::string filename){ std::cout << "frames = " << frame << std::endl; // std::cout << "received gridmap at time " << m.getTime().toNSec() << " with resolution:" << s->info.resolution << "and time: " << s->info.header.stamp << std::endl; std::cout << "rosbag time: " << s->info.header.stamp << std::endl; -// elevationMapCallback(*s); -// elev_map_pub_.publish(*s); -// saveGridMapMsgAsPCD(*s, frame); - grid_map_msgs::GridMap n, p; - n = gridMapCallback(*s); -// elev_map_pub_.publish(*s); - p = imageProcessingCallback(n); - -// elevationMapCallback(p); + elevationMapCallback(*s); + std::cout << "Press [Enter] to continue to next gridmap message" << std::endl; double frame_time; frame_time = toc().count(); @@ -660,20 +674,12 @@ void Pass::extractNthCloud(std::string filename, int n){ foreach(rosbag::MessageInstance const m, view){ grid_map_msgs::GridMap::ConstPtr s = m.instantiate(); - // std::cout << " frame = " << frame << std::endl; - if (s != NULL){ ++frame; if (frame == n){ std::cin.get(); tic(); - grid_map_msgs::GridMap n, p; -// n = gridMapCallback(*s); -// elev_map_pub_.publish(*s); -// p = imageProcessingCallback(n); - - elevationMapCallback(p); - + elevationMapCallback(*s); std::cout << toc().count() << " ms: frame_" << frame << std::endl; } } @@ -691,51 +697,50 @@ void Pass::extractNthCloud(std::string filename, int n){ bag.close(); } -grid_map_msgs::GridMap Pass::imageProcessingCallback(const grid_map_msgs::GridMap &msg){ +void Pass::imageProcessing(grid_map::GridMap &gridmap){ - grid_map::GridMap gridmap; - grid_map::GridMapRosConverter::fromMessage(msg, gridmap); - std::cout << "Gridmap resolution = " << gridmap.getResolution() << std::endl; - std::cout << "Gridmap position = " << gridmap.getPosition()[0] << ", " << gridmap.getPosition()[1] << std::endl; - gm_resolution_ = gridmap.getResolution(); - gm_position_.clear(); - gm_position_.push_back(gridmap.getPosition()[0]); - gm_position_.push_back(gridmap.getPosition()[1]); const float nanValue = 1; replaceNan(gridmap.get("slope"), nanValue); -// grid_map::GridMapRosConverter::toCvImage(gridmap, "slope", sensor_msgs::image_encodings::TYPE_32FC1, imgprocessor_.original_img_); - convertGridmapToFloatImage(gridmap, "slope", imgprocessor_.original_img_, true); - - imgprocessor_.process(); - convertGridmapToFloatImage(gridmap, "elevation", stepcreator_.elevation_, true); -// double min, max; -// cv::minMaxLoc(stepcreator_.elevation_.image, &min, &max); -// std::cout << "Min = " << min << ", max = " << max << std::endl; + convertGridmapToFloatImage(gridmap, "slope", imgprocessor_.original_img_, true); - // ***instead of drawing the image in imgprocessor and reextracting contours - maybe just pass the contours straight to stepcreator?????*** - stepcreator_.processed_ = imgprocessor_.final_img_; - stepcreator_.go(); -// std::cout << "Image size: " << imgprocessor_.final_img_.image.rows << ", " << imgprocessor_.final_img_.image.cols << std::endl; + imgprocessor_.process();; -// grid_map::GridMapCvConverter::addLayerFromImage(imgprocessor_.final_img_.image, "mask", gridmap); - grid_map::GridMapCvConverter::addLayerFromImage(stepcreator_.elevation_masked_.image, "reconstructed", gridmap); -// gridmap.add("product"); -// multiplyLayers(gridmap.get("elevation"), gridmap.get("mask"), gridmap.get("product")); -// replaceZeroToNan(gridmap.get("product")); + grid_map::GridMapCvConverter::addLayerFromImage(imgprocessor_.final_img_.image, "mask", gridmap); + if (algorithm_ == "B"){ + gridmap.add("product"); + multiplyLayers(gridmap.get("elevation"), gridmap.get("mask"), gridmap.get("product")); + replaceZeroToNan(gridmap.get("product")); + } // Publish updated grid map. grid_map_msgs::GridMap output_msg; grid_map::GridMapRosConverter::toMessage(gridmap, output_msg); filtered_map_pub_.publish(output_msg); - std::cout << "1" << std::endl; +} + +void Pass::stepCreation(grid_map::GridMap &gridmap){ + + std::cout << "Gridmap resolution = " << gridmap.getResolution() << std::endl; + std::cout << "Gridmap position = " << gridmap.getPosition()[0] << ", " << gridmap.getPosition()[1] << std::endl; + gm_resolution_ = gridmap.getResolution(); + gm_position_.push_back(gridmap.getPosition()[0]); + gm_position_.push_back(gridmap.getPosition()[1]); + + convertGridmapToFloatImage(gridmap, "elevation", stepcreator_.elevation_, true); + stepcreator_.pnts_ = imgprocessor_.all_contours_.contours_rect_; + stepcreator_.processed_ = imgprocessor_.final_img_; + stepcreator_.go(); +// grid_map::GridMapCvConverter::addLayerFromImage(stepcreator_.elevation_masked_.image, "reconstructed", gridmap); publishRectangles(); +} + +void Pass::reset(){ + gm_position_.clear(); imgprocessor_.reset(); stepcreator_.reset(); - - return output_msg; } void Pass::tic(){ @@ -750,20 +755,16 @@ std::chrono::duration Pass::toc(){ return elapsed_time; } -grid_map_msgs::GridMap Pass::gridMapCallback(const grid_map_msgs::GridMap& msg){ +void Pass::gridMapFilterChain(grid_map::GridMap& input_map){ // tic(); - // Convert message to map. - grid_map::GridMap input_map; - grid_map::GridMapRosConverter::fromMessage(msg, input_map); - // Apply filter chain. grid_map::GridMap output_map; if (!filter_chain_.update(input_map, output_map)) { std::cout << "couldn't update the grid map filter chain" << std::endl; grid_map_msgs::GridMap failmessage; grid_map::GridMapRosConverter::toMessage(input_map, failmessage); - return failmessage; + return; } /* if (verbose_timer_) { @@ -777,8 +778,7 @@ grid_map_msgs::GridMap Pass::gridMapCallback(const grid_map_msgs::GridMap& msg){ grid_map::GridMapRosConverter::toMessage(output_map, output_msg); filtered_map_pub_.publish(output_msg); - return output_msg; - + input_map = output_map; } void Pass::saveGridMapMsgAsPCD(const grid_map_msgs::GridMap& msg, int frame){ @@ -878,3 +878,12 @@ bool Pass::toImageWithNegatives(const grid_map::GridMap& gridMap, const std::str return true; } + +void Pass::setupSubscribers(){ + grid_map_sub_ = node_.subscribe(grid_map_sub_topic_, 100, + &Pass::elevationMapCallback, this); + point_cloud_sub_ = node_.subscribe(point_cloud_sub_topic_, 100, + &Pass::pointCloudCallback, this); + pose_sub_ = node_.subscribe(pose_sub_topic_, 100, + &Pass::robotPoseCallBack, this); +} diff --git a/plane_seg_ros/src/plane_seg_ros_node.cpp b/plane_seg_ros/src/plane_seg_ros_node.cpp index 3c14dfb..45df8c3 100644 --- a/plane_seg_ros/src/plane_seg_ros_node.cpp +++ b/plane_seg_ros/src/plane_seg_ros_node.cpp @@ -24,8 +24,6 @@ int main( int argc, char** argv ){ nh.param("run_test_program", run_test_program, false); std::cout << "run_test_program: " << std::boolalpha << run_test_program << "\n"; - - // Enable this to run the test programs if (run_test_program){ std::cout << "Running test examples\n"; @@ -74,6 +72,7 @@ int main( int argc, char** argv ){ return 0; } + app->setupSubscribers(); ROS_INFO_STREAM("Waiting for ROS messages"); ros::spin(); From 4d2dd7f6ae42d58f77f9c800a5d225577aeb6351 Mon Sep 17 00:00:00 2001 From: Christos Ioannou Date: Wed, 24 Feb 2021 21:44:12 +0000 Subject: [PATCH 53/58] implemented tracker to track rectangles from algorithm C --- plane_seg/CMakeLists.txt | 1 + plane_seg/include/plane_seg/StepCreator.hpp | 1 + plane_seg/include/plane_seg/Tracker.hpp | 6 +- plane_seg/include/plane_seg/Tracker2D.hpp | 28 ++++++ plane_seg/src/Tracker.cpp | 14 +-- plane_seg/src/Tracker2D.cpp | 100 +++++++++++++++++++ plane_seg_ros/include/plane_seg_ros/Pass.hpp | 4 +- plane_seg_ros/src/plane_seg_ros.cpp | 24 ++++- 8 files changed, 163 insertions(+), 15 deletions(-) create mode 100644 plane_seg/include/plane_seg/Tracker2D.hpp create mode 100644 plane_seg/src/Tracker2D.cpp diff --git a/plane_seg/CMakeLists.txt b/plane_seg/CMakeLists.txt index 0957774..a585299 100644 --- a/plane_seg/CMakeLists.txt +++ b/plane_seg/CMakeLists.txt @@ -37,6 +37,7 @@ add_library(${LIB_NAME} SHARED src/Tracker.cpp src/ImageProcessor.cpp src/StepCreator.cpp + src/Tracker2D.cpp ) add_dependencies(plane_seg ${catkin_EXPORTED_TARGETS}) target_link_libraries(plane_seg ${catkin_LIBRARIES}) diff --git a/plane_seg/include/plane_seg/StepCreator.hpp b/plane_seg/include/plane_seg/StepCreator.hpp index 8aded14..914822a 100644 --- a/plane_seg/include/plane_seg/StepCreator.hpp +++ b/plane_seg/include/plane_seg/StepCreator.hpp @@ -6,6 +6,7 @@ namespace planeseg { struct contour{ std::vector points_; double elevation_; + int id_; }; diff --git a/plane_seg/include/plane_seg/Tracker.hpp b/plane_seg/include/plane_seg/Tracker.hpp index bc31f58..71ac21a 100644 --- a/plane_seg/include/plane_seg/Tracker.hpp +++ b/plane_seg/include/plane_seg/Tracker.hpp @@ -17,10 +17,10 @@ struct IdAssigned{ int id; }; -class Tracker { +class Tracker3D { public: - Tracker(); - ~Tracker(); + Tracker3D(); + ~Tracker3D(); int get_plane_id(planeseg::plane plane_); pcl::PointXYZ find_centroid(pcl::PointCloud cloud ); diff --git a/plane_seg/include/plane_seg/Tracker2D.hpp b/plane_seg/include/plane_seg/Tracker2D.hpp new file mode 100644 index 0000000..a080e86 --- /dev/null +++ b/plane_seg/include/plane_seg/Tracker2D.hpp @@ -0,0 +1,28 @@ +#pragma once +#include +#include +#include "plane_seg/BlockFitter.hpp" +#include "plane_seg/StepCreator.hpp" + +namespace planeseg { + +class Tracker2D { +public: + Tracker2D(); + ~Tracker2D(); + + int get_contour_id(contour contour); + pcl::PointXYZ find_centroid(pcl::PointCloud cloud ); + void reset(); + void assignIDs(std::vector contours); + void printIds(); + std::vector newRects_; + +private: + + std::vector oldRects_; + std::vector::Ptr> cloud_ptrs; + int totalIds; +}; + +} // namespace planeseg diff --git a/plane_seg/src/Tracker.cpp b/plane_seg/src/Tracker.cpp index c11cf43..a8e67ed 100644 --- a/plane_seg/src/Tracker.cpp +++ b/plane_seg/src/Tracker.cpp @@ -11,13 +11,13 @@ namespace planeseg { -Tracker::Tracker(){ +Tracker3D::Tracker3D(){ totalIds = 0; } -Tracker::~Tracker(){} +Tracker3D::~Tracker3D(){} -pcl::PointXYZ Tracker::find_centroid(pcl::PointCloud cloud ){ +pcl::PointXYZ Tracker3D::find_centroid(pcl::PointCloud cloud ){ // std::cout << "Computing centroid" << std::endl; Eigen::Vector4f centroid_eigen; pcl::compute3DCentroid(cloud, centroid_eigen); @@ -31,7 +31,7 @@ pcl::PointXYZ Tracker::find_centroid(pcl::PointCloud cloud ){ } // converts the result from the BlockFitter to a vector of planes complete with pointclouds, centroids and ids, and saves it as newStairs -void Tracker::convertResult(planeseg::BlockFitter::Result result_){ +void Tracker3D::convertResult(planeseg::BlockFitter::Result result_){ std::cout << "entered convertResult" << std::endl; for (size_t i=0; i cloud; @@ -59,7 +59,7 @@ void Tracker::convertResult(planeseg::BlockFitter::Result result_){ } -int Tracker::get_plane_id(planeseg::plane plane){ +int Tracker3D::get_plane_id(planeseg::plane plane){ // std::cout << "entered get_plane_id" << std::endl; int id; @@ -106,13 +106,13 @@ int Tracker::get_plane_id(planeseg::plane plane){ return id; } -void Tracker::reset(){ +void Tracker3D::reset(){ std::cout << "entered Tracker reset" << std::endl; oldStairs = newStairs; newStairs.clear(); } -void Tracker::printIds(){ +void Tracker3D::printIds(){ // std::cout << "entered printIds" << std::endl; std::cout << "Total number of ids assigned: " << newStairs.size() << std::endl; for (size_t i = 0; i < newStairs.size(); ++i){ diff --git a/plane_seg/src/Tracker2D.cpp b/plane_seg/src/Tracker2D.cpp new file mode 100644 index 0000000..40e01b4 --- /dev/null +++ b/plane_seg/src/Tracker2D.cpp @@ -0,0 +1,100 @@ +#include "plane_seg/Tracker2D.hpp" +#include +#include +#include +#include +#include "plane_seg/BlockFitter.hpp" +#include "plane_seg/Types.hpp" +#include "pcl/common/impl/centroid.hpp" +#include +#include + +namespace planeseg { + +Tracker2D::Tracker2D(){ + totalIds = 0; +} + +Tracker2D::~Tracker2D(){} + + +// converts the result from the BlockFitter to a vector of planes complete with pointclouds, centroids and ids, and saves it as newStairs +void Tracker2D::assignIDs(std::vector contours){ + std::cout << "entered assignIDs" << std::endl; + for (size_t i = 0; i < contours.size(); ++i){ + planeseg::contour newContour; + newContour.points_ = contours[i].points_; + newContour.elevation_ = contours[i].elevation_; + newContour.id_ = get_contour_id(newContour); + + newRects_.push_back(newContour); + } +} + +int Tracker2D::get_contour_id(planeseg::contour contour){ + + std::cout << "entered get_plane_id" << std::endl; + int id; + + if(oldRects_.empty()){ + std::cout << "oldRects_ is empty" << std::endl; + id = totalIds; + ++totalIds; + } + + else{ + std::cout << "oldRects_ has size " << oldRects_.size() << std::endl; + double threshold = 0.1; + double dist; + int closest = -1; + // start with closestDist being far larger than anything that would ever happen + double closestDist = 1000000000; + + // go through each plane in oldStairs and compare it to the new plane + + for (size_t i = 0; i < oldRects_.size(); ++i){ + + dist = fabs(oldRects_[i].elevation_ - contour.elevation_); + std::cout << "oldRects_[" << i << "].elevation_ = " << oldRects_[i].elevation_; + std::cout << "contour.elevation_ = " << contour.elevation_ << std::endl; + std::cout << "dist = " << dist << std::endl; + // is distz small enough to suggest that this point cloud represents the next iteration of an existing frame? + if(dist < threshold){ + // distance = pcl::euclideanDistance(oldStairs[i].centroid, plane.centroid); + + if (dist < closestDist){ + closest = i; + closestDist = dist; + } + } + } + + if (closest != -1){ + std::cout << "Here" << std::endl; + id = oldRects_[closest].id_; + } + else{ + std::cout << "Here2" << std::endl; + id = totalIds; + ++totalIds; + } + } + std::cout << "id = " << id << std::endl; + return id; +} + +void Tracker2D::reset(){ + std::cout << "entered Tracker2D reset" << std::endl; + oldRects_ = newRects_; + newRects_.clear(); + } + +void Tracker2D::printIds(){ +// std::cout << "entered printIds" << std::endl; + std::cout << "Total number of ids assigned: " << newRects_.size() << std::endl; + for (size_t i = 0; i < newRects_.size(); ++i){ + std::cout << newRects_[i].id_ << std::endl; + } +} + +} // namespace plane_seg diff --git a/plane_seg_ros/include/plane_seg_ros/Pass.hpp b/plane_seg_ros/include/plane_seg_ros/Pass.hpp index ae89b0c..bc52af6 100644 --- a/plane_seg_ros/include/plane_seg_ros/Pass.hpp +++ b/plane_seg_ros/include/plane_seg_ros/Pass.hpp @@ -9,6 +9,7 @@ #include #include #include +#include #include #include #include @@ -73,10 +74,11 @@ class Pass{ Eigen::Isometry3d last_robot_pose_; planeseg::BlockFitter::Result result_; - planeseg::Tracker tracking_; + planeseg::Tracker3D tracking_; planeseg::Visualizer visualizer_; planeseg::ImageProcessor imgprocessor_; planeseg::StepCreator stepcreator_; + planeseg::Tracker2D tracking2D_; filters::FilterChain filter_chain_; // filters::FilterChain mask_filter_; tf::TransformListener listener_; diff --git a/plane_seg_ros/src/plane_seg_ros.cpp b/plane_seg_ros/src/plane_seg_ros.cpp index 7ae7123..54e3afd 100644 --- a/plane_seg_ros/src/plane_seg_ros.cpp +++ b/plane_seg_ros/src/plane_seg_ros.cpp @@ -72,7 +72,8 @@ Pass::Pass(ros::NodeHandle& node): last_robot_pose_ = Eigen::Isometry3d::Identity(); - tracking_ = planeseg::Tracker(); + tracking_ = planeseg::Tracker3D(); + tracking2D_ = planeseg::Tracker2D(); visualizer_ = planeseg::Visualizer(); imgprocessor_ = planeseg::ImageProcessor(); stepcreator_ = planeseg::StepCreator(); @@ -603,9 +604,18 @@ void Pass::publishRectangles(){ std::cout << "Entered publishRectangles" << std::endl; visualization_msgs::MarkerArray rectangles_array; + std::vector ids; + for (size_t m = 0; m < tracking2D_.newRects_.size(); ++m){ + ids.push_back(tracking2D_.newRects_[m].id_); + } + std::cout << "Elevation values: " << std::endl; for (size_t i = 0; i < stepcreator_.rectangles_.size(); ++i){ + double r = visualizer_.getR(ids[i]); + double g = visualizer_.getG(ids[i]); + double b = visualizer_.getB(ids[i]); + std::cout << stepcreator_.rectangles_[i].elevation_ << std::endl; planeseg::contour contour = stepcreator_.rectangles_[i]; @@ -619,9 +629,9 @@ void Pass::publishRectangles(){ rectMarker.action = visualization_msgs::Marker::ADD; rectMarker.pose.orientation.w = 0.0; rectMarker.scale.x = 0.05; - rectMarker.color.r = 255; - rectMarker.color.g = 255; - rectMarker.color.b = 1; + rectMarker.color.r = r; + rectMarker.color.g = g; + rectMarker.color.b = b; rectMarker.color.a = 1; std::vector pointsGM(contour.points_.size()+1); int count; @@ -733,14 +743,20 @@ void Pass::stepCreation(grid_map::GridMap &gridmap){ stepcreator_.pnts_ = imgprocessor_.all_contours_.contours_rect_; stepcreator_.processed_ = imgprocessor_.final_img_; stepcreator_.go(); + tracking2D_.reset(); + tracking2D_.assignIDs(stepcreator_.rectangles_); + tracking2D_.printIds(); // grid_map::GridMapCvConverter::addLayerFromImage(stepcreator_.elevation_masked_.image, "reconstructed", gridmap); publishRectangles(); +// tracking2D_.reset(); } void Pass::reset(){ gm_position_.clear(); imgprocessor_.reset(); stepcreator_.reset(); +// tracking_.reset(); +// tracking2D_.reset(); } void Pass::tic(){ From abb6564a75484ce022b0a5513587f36080bb1e62 Mon Sep 17 00:00:00 2001 From: Christos Ioannou Date: Wed, 10 Mar 2021 14:10:28 +0000 Subject: [PATCH 54/58] final cleanup of parameter and launch files for easy interface --- plane_seg/src/Tracker2D.cpp | 2 +- plane_seg_ros/config/nth_cloud.yaml | 84 ------------------- plane_seg_ros/config/rosbag_sequential.yaml | 73 ---------------- plane_seg_ros/launch/default.launch | 2 - plane_seg_ros/launch/nth_cloud.launch | 2 + plane_seg_ros/launch/rosbag_sequential.launch | 2 + plane_seg_ros/src/plane_seg_ros.cpp | 5 ++ 7 files changed, 10 insertions(+), 160 deletions(-) delete mode 100644 plane_seg_ros/config/nth_cloud.yaml delete mode 100644 plane_seg_ros/config/rosbag_sequential.yaml diff --git a/plane_seg/src/Tracker2D.cpp b/plane_seg/src/Tracker2D.cpp index 40e01b4..c5b73ca 100644 --- a/plane_seg/src/Tracker2D.cpp +++ b/plane_seg/src/Tracker2D.cpp @@ -48,7 +48,7 @@ int Tracker2D::get_contour_id(planeseg::contour contour){ double dist; int closest = -1; // start with closestDist being far larger than anything that would ever happen - double closestDist = 1000000000; + double closestDist = 1000000; // go through each plane in oldStairs and compare it to the new plane diff --git a/plane_seg_ros/config/nth_cloud.yaml b/plane_seg_ros/config/nth_cloud.yaml deleted file mode 100644 index a09954d..0000000 --- a/plane_seg_ros/config/nth_cloud.yaml +++ /dev/null @@ -1,84 +0,0 @@ -plane_seg: - run_test_program: false - run_sequential_test: false - run_nth_cloud: true - - grid_map_filters: - - - name: buffer_normalizer - type: gridMapFilters/BufferNormalizerFilter - - # # Duplicate layer. - # - name: duplicate - # type: gridMapFilters/DuplicationFilter - # params: - # input_layer: ... - # output_layer: ... - - # Delete color layer. - - name: delete_original_layers - type: gridMapFilters/DeletionFilter - params: - layers: [color] # List of layers. - - # Fill holes in the map with inpainting. - # - name: inpaint - # type: gridMapCv/InpaintFilter - # params: - # input_layer: elevation - # output_layer: elevation_inpainted - # radius: 0.05 - - # Reduce noise with a radial blurring filter. - - name: mean_in_radius - type: gridMapFilters/MeanInRadiusFilter - params: - input_layer: elevation - output_layer: elevation_smooth - radius: 0.06 - - # Boxblur as an alternative to the inpaint and radial blurring filter above. - # - name: boxblur - # type: gridMapFilters/SlidingWindowMathExpressionFilter - # params: - # input_layer: elevation - # output_layer: elevation_smooth - # expression: meanOfFinites(elevation) - # compute_empty_cells: true - # edge_handling: crop # options: inside, crop, empty, mean - # window_size: 5 # optional - - # Compute surface normals. - - name: surface_normals - type: gridMapFilters/NormalVectorsFilter - params: - input_layer: elevation_smooth - output_layers_prefix: normal_vectors_ - radius: 0.05 - normal_vector_positive_axis: z - - # Add a color layer for visualization based on the surface normal. - # - name: normal_color_map - # type: gridMapFilters/NormalColorMapFilter - # params: - # input_layers_prefix: normal_vectors_ - # output_layer: normal_color - - # Compute slope from surface normal. - - name: slope - type: gridMapFilters/MathExpressionFilter - params: - output_layer: slope - expression: acos(normal_vectors_z) - -# grid_map_mask_filter: -# -# - name: buffer_normalizer -# type: gridMapFilters/BufferNormalizerFilter - - # Use output from image processor as mask over elevation -# - name: masking -# type: gridMapFilters/MathExpressionFilter -# params: -# output_layer: masked -# expression: elevation * slope diff --git a/plane_seg_ros/config/rosbag_sequential.yaml b/plane_seg_ros/config/rosbag_sequential.yaml deleted file mode 100644 index 7a9650b..0000000 --- a/plane_seg_ros/config/rosbag_sequential.yaml +++ /dev/null @@ -1,73 +0,0 @@ -plane_seg: - run_test_program: false - run_sequential_test: true - run_nth_cloud: false - - grid_map_filters: - - - name: buffer_normalizer - type: gridMapFilters/BufferNormalizerFilter - - # # Duplicate layer. - # - name: duplicate - # type: gridMapFilters/DuplicationFilter - # params: - # input_layer: ... - # output_layer: ... - - # Delete color layer. - - name: delete_original_layers - type: gridMapFilters/DeletionFilter - params: - layers: [color] # List of layers. - - # Fill holes in the map with inpainting. - # - name: inpaint - # type: gridMapCv/InpaintFilter - # params: - # input_layer: elevation - # output_layer: elevation_inpainted - # radius: 0.05 - - # Reduce noise with a radial blurring filter. - # - name: mean_in_radius - # type: gridMapFilters/MeanInRadiusFilter - # params: - # input_layer: elevation_inpainted - # output_layer: elevation_smooth - # radius: 0.06 - - # Boxblur as an alternative to the inpaint and radial blurring filter above. - # - name: boxblur - # type: gridMapFilters/SlidingWindowMathExpressionFilter - # params: - # input_layer: elevation - # output_layer: elevation_smooth - # expression: meanOfFinites(elevation) - # compute_empty_cells: true - # edge_handling: crop # options: inside, crop, empty, mean - # window_size: 5 # optional - - # Compute surface normals. - - name: surface_normals - type: gridMapFilters/NormalVectorsFilter - params: - input_layer: elevation - output_layers_prefix: normal_vectors_ - radius: 0.05 - normal_vector_positive_axis: z - - # Add a color layer for visualization based on the surface normal. - - name: normal_color_map - type: gridMapFilters/NormalColorMapFilter - params: - input_layers_prefix: normal_vectors_ - output_layer: normal_color - - # Compute slope from surface normal. - - name: slope - type: gridMapFilters/MathExpressionFilter - params: - output_layer: slope - expression: acos(normal_vectors_z) - diff --git a/plane_seg_ros/launch/default.launch b/plane_seg_ros/launch/default.launch index 8c36af9..7c7ebf6 100644 --- a/plane_seg_ros/launch/default.launch +++ b/plane_seg_ros/launch/default.launch @@ -1,11 +1,9 @@ - - diff --git a/plane_seg_ros/launch/nth_cloud.launch b/plane_seg_ros/launch/nth_cloud.launch index e8bd6a9..739d4b4 100644 --- a/plane_seg_ros/launch/nth_cloud.launch +++ b/plane_seg_ros/launch/nth_cloud.launch @@ -3,11 +3,13 @@ + + diff --git a/plane_seg_ros/launch/rosbag_sequential.launch b/plane_seg_ros/launch/rosbag_sequential.launch index 33c52bf..63d5e93 100644 --- a/plane_seg_ros/launch/rosbag_sequential.launch +++ b/plane_seg_ros/launch/rosbag_sequential.launch @@ -2,10 +2,12 @@ + + diff --git a/plane_seg_ros/src/plane_seg_ros.cpp b/plane_seg_ros/src/plane_seg_ros.cpp index 54e3afd..46edfd4 100644 --- a/plane_seg_ros/src/plane_seg_ros.cpp +++ b/plane_seg_ros/src/plane_seg_ros.cpp @@ -166,6 +166,11 @@ void Pass::elevationMapCallback(const grid_map_msgs::GridMap& msg){ // convert message to GridMap, to PointCloud to LabeledCloud grid_map::GridMap map; grid_map::GridMapRosConverter::fromMessage(msg, map); + std::vector layers; + layers = map.getLayers(); + for (size_t k = 0; k < layers.size(); ++k){ + std::cout << layers[k] << std::endl; + }; if (algorithm_ == "C"){ std::cout << "C" << std::endl; From cbdda480d542ebe090281c3c9bab445d23f640d0 Mon Sep 17 00:00:00 2001 From: Christos Ioannou Date: Tue, 6 Apr 2021 11:46:10 +0100 Subject: [PATCH 55/58] modifications to extract info about step properties --- plane_seg/CMakeLists.txt | 1 + .../include/plane_seg/ImageProcessor.hpp | 10 +- plane_seg/src/ImageProcessor.cpp | 114 +++++++----------- plane_seg/src/StepCreator.cpp | 2 +- plane_seg/src/Tracker2D.cpp | 14 +-- plane_seg_ros/CMakeLists.txt | 4 +- .../config/elevation_map_filters_config.rviz | 87 ------------- plane_seg_ros/include/plane_seg_ros/Pass.hpp | 6 +- plane_seg_ros/src/plane_seg_ros.cpp | 42 ++++++- 9 files changed, 109 insertions(+), 171 deletions(-) diff --git a/plane_seg/CMakeLists.txt b/plane_seg/CMakeLists.txt index a585299..999664f 100644 --- a/plane_seg/CMakeLists.txt +++ b/plane_seg/CMakeLists.txt @@ -73,3 +73,4 @@ install(DIRECTORY include/${PROJECT_NAME}/ install(TARGETS block_fitter plane_seg_test plane_seg_test2 RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + diff --git a/plane_seg/include/plane_seg/ImageProcessor.hpp b/plane_seg/include/plane_seg/ImageProcessor.hpp index e92eddd..34c971e 100644 --- a/plane_seg/include/plane_seg/ImageProcessor.hpp +++ b/plane_seg/include/plane_seg/ImageProcessor.hpp @@ -20,6 +20,10 @@ struct contours{ void approxAsPoly(); void fitSquare(); bool isSquare(std::vector contour_); + void print(std::vector property); + std::vector elongations_; + std::vector convexities_; + std::vector rectangularities_; /* void setColors(); @@ -42,7 +46,7 @@ class ImageProcessor { void process(); void copyOrigToProc(); void displayImage(std::string process, cv::Mat img, int n = 0); - void saveImage(cv_bridge::CvImage image); + void saveImage(cv::Mat image); void erodeImage(int erode_size); void thresholdImage(float threshold_value); void dilateImage(int dilate_size); @@ -67,6 +71,10 @@ class ImageProcessor { contours large_contours_; contours med_contours_; + std::vector ip_elongations_; + std::vector ip_convexities_; + std::vector ip_rectangularities_; + private: }; diff --git a/plane_seg/src/ImageProcessor.cpp b/plane_seg/src/ImageProcessor.cpp index 343ab24..0c0619c 100644 --- a/plane_seg/src/ImageProcessor.cpp +++ b/plane_seg/src/ImageProcessor.cpp @@ -15,6 +15,7 @@ void contours::filterMinConvexity(int min_convexity){ std::vector> temp; temp = contours_; contours_.clear(); + convexities_.clear(); for (size_t p = 0; p < temp.size(); ++p){ std::vector hull; @@ -26,11 +27,14 @@ void contours::filterMinConvexity(int min_convexity){ hull_perimeter = cv::arcLength(hull, true); contour_perimeter = cv::arcLength(temp[p], true); convexity = hull_perimeter / contour_perimeter; + convexities_.push_back(convexity); if (convexity >= min_convexity){ contours_.push_back(temp[p]); } } + +// print(convexities_); } void contours::filterMinElongation(int min_elongation){ @@ -38,6 +42,7 @@ void contours::filterMinElongation(int min_elongation){ std::vector> temp; temp = contours_; contours_.clear(); + elongations_.clear(); for (size_t r = 0; r < temp.size(); ++r){ double elongation; @@ -47,10 +52,14 @@ void contours::filterMinElongation(int min_elongation){ double minor_axis = std::min(minarea_rect.size.height, minarea_rect.size.width); elongation = major_axis / minor_axis; + elongations_.push_back(elongation); + if (elongation >= min_elongation){ contours_.push_back(temp[r]); } } + +// print(elongations_); } void contours::filterMinRectangularity(int min_rectangularity){ @@ -58,6 +67,7 @@ void contours::filterMinRectangularity(int min_rectangularity){ std::vector> temp; temp = contours_; contours_.clear(); + rectangularities_.clear(); for (size_t r = 0; r < temp.size(); ++r){ double rectangularity, contour_area; @@ -66,10 +76,13 @@ void contours::filterMinRectangularity(int min_rectangularity){ contour_area = cv::contourArea(temp[r]); rectangularity = contour_area / minarea_rect.size.area(); + rectangularities_.push_back(rectangularity); + if (rectangularity >= min_rectangularity){ contours_.push_back(temp[r]); } } +// print(rectangularities_); } void contours::approxAsPoly(){ @@ -141,71 +154,16 @@ void contours::fitSquare(){ } } -/* -void contours::setColors(){ - colors_ = { - 1, 1, 1, // 42 - 255, 255, 120, - 1, 120, 1, - 1, 225, 1, - 120, 255, 1, - 1, 255, 255, - 120, 1, 1, - 255, 120, 255, - 120, 1, 255, - 1, 1, 120, - 255, 255, 255, - 120, 120, 1, - 120, 120, 120, - 1, 1, 255, - 255, 1, 255, - 120, 120, 255, - 120, 255, 120, - 1, 120, 120, - 1, 1, 255, - 255, 1, 1, - 155, 1, 120, - 120, 1, 120, - 255, 120, 1, - 1, 120, 255, - 255, 120, 120, - 1, 255, 120, - 255, 255, 1}; -} -void contours::assignIDs(){ - for (size_t i = 0; i < contours_.size(); ++i){ - ids[i] = i; - } -} +void contours::print(std::vector property){ -void contours::assignColors(){ - setColors(); - for (size_t i = 0; i < contours_.size(); ++i){ - contour_colours_[i].r = getR(ids[i]); - contour_colours_[i].g = getG(ids[i]); - contour_colours_[i].b = getB(ids[i]); + for(size_t i = 0; i < property.size(); ++i){ + std::cout << property[i] << ", "; } + std::cout << std::endl; } -double contours::getR(int id){ - double j; - j = id % (colors_.size()/3); - return colors_[3*j]; -} -double contours::getG(int id){ - unsigned j; - j = id % (colors_.size()/3); - return colors_[3*j+1]; -} - -double contours::getB(int id){ - unsigned j; - j = id % (colors_.size()/3); - return colors_[3*j+2]; -} -*/ ImageProcessor::ImageProcessor(){ } @@ -213,7 +171,13 @@ ImageProcessor::~ImageProcessor(){} void ImageProcessor::process(){ + + ip_convexities_.clear(); + ip_elongations_.clear(); + ip_rectangularities_.clear(); + // displayImage("original", original_img_.image, 0); +// saveImage(original_img_.image); // histogram(original_img_); thresholdImage(0.3); @@ -223,32 +187,44 @@ void ImageProcessor::process(){ erodeImage(1); dilateImage(2); erodeImage(1); -// displayImage("erode/dilate", processed_img_.image, 2); +// displayImage("morphological", processed_img_.image, 2); extractContours(); splitContours(); +// drawContoursIP(med_contours_, "med_contours", 3); +// drawContoursIP(large_contours_, "large_contours", 4); + std::cout << "Convexities: " << std::endl; med_contours_.filterMinConvexity(0.9); // have another look at the threshold for convexity + ip_convexities_ = med_contours_.convexities_; // drawContoursIP(med_contours_, "filtered by convexity", 6); + std::cout << "Elongations: " << std::endl; med_contours_.filterMinElongation(3); + ip_elongations_ = med_contours_.elongations_; // drawContoursIP(med_contours_, "filtered by elongation", 7); + std::cout << "Rectangularities: " << std::endl; med_contours_.filterMinRectangularity(0.6); + ip_rectangularities_ = med_contours_.rectangularities_; // drawContoursIP(med_contours_, "filtered by rectangularity", 8); med_contours_.fitMinAreaRect(); large_contours_.approxAsPoly(); // large_contours_.filterMinRectangularity(0.6); // large_contours_.fitSquare(); +// drawContoursIP(med_contours_, "med_filtered", 5); +// drawContoursIP(large_contours_, "large_approx", 6); mergeContours(); // all_contours_.assignIDs(); // all_contours_.assignColors(); displayResult(); -/* - int p = cv::waitKey(0); - if (p == 's'){ + + + +// int p = cv::waitKey(0); +/* if (p == 's'){ saveImage(final_img_); } - */ + */ } void ImageProcessor::copyOrigToProc(){ @@ -260,6 +236,7 @@ void ImageProcessor::displayImage(std::string process, cv::Mat img, int n) { cv::namedWindow(process, cv::WINDOW_AUTOSIZE); cv::moveWindow(process, 100 + img.cols * n, 50); cv::imshow(process, img); +// saveImage(img); } void ImageProcessor::displayResult(){ @@ -279,16 +256,16 @@ void ImageProcessor::displayResult(){ } final_img_ = rect_img_; -// displayImage("final", colour_img_.image, 9); +// displayImage("final", colour_img_.image, 7); } -void ImageProcessor::saveImage(cv_bridge::CvImage image_){ +void ImageProcessor::saveImage(cv::Mat image_){ std::string imagename; std::cout << "Enter filename to save image (don't forget .png!): " << std::endl; std::cin >> imagename; std::string homedir = getenv("HOME"); if(!homedir.empty()){ - boost::filesystem::path output_path(homedir + "/rosbags/image_processing/"); + boost::filesystem::path output_path(homedir + "/git/4YP/Figures/"); if(!boost::filesystem::is_directory(output_path)){ if(boost::filesystem::create_directory(output_path)){ std::cout << "Creating directory " << output_path.string() << " ... " << std::endl; @@ -297,7 +274,7 @@ void ImageProcessor::saveImage(cv_bridge::CvImage image_){ return; } } - cv::imwrite(output_path.string() + imagename, image_.image); + cv::imwrite(output_path.string() + imagename, image_); } else { std::cerr << "ERROR: the $HOME variable is empty!" << std::endl; return; @@ -413,6 +390,7 @@ void ImageProcessor::histogram(cv_bridge::CvImage img){ cv::waitKey(0); } + void ImageProcessor::reset(){ original_img_.image = cv::Mat(); processed_img_.image = cv::Mat(); diff --git a/plane_seg/src/StepCreator.cpp b/plane_seg/src/StepCreator.cpp index 78dca1e..f2be940 100644 --- a/plane_seg/src/StepCreator.cpp +++ b/plane_seg/src/StepCreator.cpp @@ -26,7 +26,7 @@ void StepCreator::go(){ maskElevation(mask); rectangles_[i].elevation_ = medianMat(elevation_masked_.image); - std::cout << medianMat(elevation_masked_.image) << std::endl; +// std::cout << medianMat(elevation_masked_.image) << std::endl; } std::vector> v; diff --git a/plane_seg/src/Tracker2D.cpp b/plane_seg/src/Tracker2D.cpp index c5b73ca..e3eba4d 100644 --- a/plane_seg/src/Tracker2D.cpp +++ b/plane_seg/src/Tracker2D.cpp @@ -33,17 +33,17 @@ void Tracker2D::assignIDs(std::vector contours){ int Tracker2D::get_contour_id(planeseg::contour contour){ - std::cout << "entered get_plane_id" << std::endl; +// std::cout << "entered get_plane_id" << std::endl; int id; if(oldRects_.empty()){ - std::cout << "oldRects_ is empty" << std::endl; +// std::cout << "oldRects_ is empty" << std::endl; id = totalIds; ++totalIds; } else{ - std::cout << "oldRects_ has size " << oldRects_.size() << std::endl; +// std::cout << "oldRects_ has size " << oldRects_.size() << std::endl; double threshold = 0.1; double dist; int closest = -1; @@ -55,9 +55,9 @@ int Tracker2D::get_contour_id(planeseg::contour contour){ for (size_t i = 0; i < oldRects_.size(); ++i){ dist = fabs(oldRects_[i].elevation_ - contour.elevation_); - std::cout << "oldRects_[" << i << "].elevation_ = " << oldRects_[i].elevation_; - std::cout << "contour.elevation_ = " << contour.elevation_ << std::endl; - std::cout << "dist = " << dist << std::endl; +// std::cout << "oldRects_[" << i << "].elevation_ = " << oldRects_[i].elevation_; +// std::cout << "contour.elevation_ = " << contour.elevation_ << std::endl; +// std::cout << "dist = " << dist << std::endl; // is distz small enough to suggest that this point cloud represents the next iteration of an existing frame? if(dist < threshold){ // distance = pcl::euclideanDistance(oldStairs[i].centroid, plane.centroid); @@ -79,7 +79,7 @@ int Tracker2D::get_contour_id(planeseg::contour contour){ ++totalIds; } } - std::cout << "id = " << id << std::endl; +// std::cout << "id = " << id << std::endl; return id; } diff --git a/plane_seg_ros/CMakeLists.txt b/plane_seg_ros/CMakeLists.txt index 49fe27b..e1b2a55 100644 --- a/plane_seg_ros/CMakeLists.txt +++ b/plane_seg_ros/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.6.0) +cmake_minimum_required(VERSION 3.0.2) project(plane_seg_ros) @@ -17,7 +17,7 @@ find_package(catkin REQUIRED COMPONENTS ) -find_package(OpenCV 3.0 REQUIRED COMPONENTS core highgui imgproc imgcodecs) +find_package(OpenCV REQUIRED COMPONENTS core highgui imgproc imgcodecs) catkin_package(INCLUDE_DIRS include LIBRARIES ${PROJECT_NAME} diff --git a/plane_seg_ros/config/elevation_map_filters_config.rviz b/plane_seg_ros/config/elevation_map_filters_config.rviz index 26ac1d1..581207a 100644 --- a/plane_seg_ros/config/elevation_map_filters_config.rviz +++ b/plane_seg_ros/config/elevation_map_filters_config.rviz @@ -96,86 +96,6 @@ Visualization Manager: hull lines: true Queue Size: 100 Value: true - Name: hull markers - Namespaces: - {} - Queue Size: 100 - Value: true - - Alpha: 0.5 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 255; 255 - Color Transformer: Intensity - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: input cloud - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.0020000000949949026 - Style: Flat Squares - Topic: /plane_seg/received_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - Class: rviz/Pose - Color: 255; 25; 0 - Enabled: true - Head Length: 0.30000001192092896 - Head Radius: 0.10000000149011612 - Name: look pose - Shaft Length: 1 - Shaft Radius: 0.05000000074505806 - Shape: Axes - Topic: /plane_seg/look_pose - Unreliable: false - Value: true - - Alpha: 0.4000000059604645 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 252; 175; 62 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: PointCloud2 - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.003000000026077032 - Style: Flat Squares - Topic: /plane_seg/point_cloud_in - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Alpha: 0.20000000298023224 Autocompute Intensity Bounds: true Class: grid_map_rviz_plugin/GridMap @@ -200,13 +120,6 @@ Visualization Manager: - Alpha: 1 Axes Length: 0.5 Axes Radius: 0.05000000074505806 - Topic: /elevation_mapping/elevation_map - Unreliable: false - Use Rainbow: true - Value: true - - Alpha: 1 - Axes Length: 1 - Axes Radius: 0.10000000149011612 Class: rviz/PoseWithCovariance Color: 255; 25; 0 Covariance: diff --git a/plane_seg_ros/include/plane_seg_ros/Pass.hpp b/plane_seg_ros/include/plane_seg_ros/Pass.hpp index bc52af6..2c94b33 100644 --- a/plane_seg_ros/include/plane_seg_ros/Pass.hpp +++ b/plane_seg_ros/include/plane_seg_ros/Pass.hpp @@ -16,7 +16,7 @@ #include #include #include -#include +#include #include namespace planeseg { @@ -62,6 +62,10 @@ class Pass{ bool convertGridmapToFloatImage(const grid_map::GridMap& gridMap, const std::string& layer, cv_bridge::CvImage& cvImage, bool negative); bool toImageWithNegatives(const grid_map::GridMap& gridMap, const std::string& layer, const int encoding, const float lowerValue, const float upperValue, cv::Mat& image); void setupSubscribers(); + void getContourData(); + std::vector all_elongations_; + std::vector all_convexities_; + std::vector all_rectangularities_; private: ros::NodeHandle& node_; diff --git a/plane_seg_ros/src/plane_seg_ros.cpp b/plane_seg_ros/src/plane_seg_ros.cpp index 46edfd4..b598a8b 100644 --- a/plane_seg_ros/src/plane_seg_ros.cpp +++ b/plane_seg_ros/src/plane_seg_ros.cpp @@ -327,7 +327,8 @@ void Pass::stepThroughFile(std::string filename){ bag.close(); - double average, sum; + double average = 0.0; + double sum = 0.0; for (size_t k = 0; k < timing_vector.size(); ++k){ sum = sum + timing_vector[k]; } @@ -621,7 +622,7 @@ void Pass::publishRectangles(){ double g = visualizer_.getG(ids[i]); double b = visualizer_.getB(ids[i]); - std::cout << stepcreator_.rectangles_[i].elevation_ << std::endl; +// std::cout << stepcreator_.rectangles_[i].elevation_ << std::endl; planeseg::contour contour = stepcreator_.rectangles_[i]; @@ -720,7 +721,7 @@ void Pass::imageProcessing(grid_map::GridMap &gridmap){ convertGridmapToFloatImage(gridmap, "slope", imgprocessor_.original_img_, true); imgprocessor_.process();; - + getContourData(); grid_map::GridMapCvConverter::addLayerFromImage(imgprocessor_.final_img_.image, "mask", gridmap); @@ -730,6 +731,7 @@ void Pass::imageProcessing(grid_map::GridMap &gridmap){ replaceZeroToNan(gridmap.get("product")); } + // Publish updated grid map. grid_map_msgs::GridMap output_msg; grid_map::GridMapRosConverter::toMessage(gridmap, output_msg); @@ -750,12 +752,44 @@ void Pass::stepCreation(grid_map::GridMap &gridmap){ stepcreator_.go(); tracking2D_.reset(); tracking2D_.assignIDs(stepcreator_.rectangles_); - tracking2D_.printIds(); +// tracking2D_.printIds(); // grid_map::GridMapCvConverter::addLayerFromImage(stepcreator_.elevation_masked_.image, "reconstructed", gridmap); publishRectangles(); // tracking2D_.reset(); } +void Pass::getContourData(){ + // Retrieve information about contours from image processor + for (size_t i = 0; i < imgprocessor_.ip_elongations_.size(); ++i){ + all_elongations_.push_back(imgprocessor_.ip_elongations_[i]); + } + for (size_t j = 0; j < imgprocessor_.ip_convexities_.size(); ++j){ + all_convexities_.push_back(imgprocessor_.ip_convexities_[j]); + } + for (size_t k = 0; k < imgprocessor_.ip_rectangularities_.size(); ++k){ + all_rectangularities_.push_back(imgprocessor_.ip_rectangularities_[k]); + } + + std::cout << "All elongations:" << std::endl; + for(size_t i = 0; i < all_elongations_.size(); ++i){ + std::cout << all_elongations_[i] << ", "; + } + std::cout << std::endl; + + std::cout << "All convexities:" << std::endl; + for(size_t i = 0; i < all_convexities_.size(); ++i){ + std::cout << all_convexities_[i] << ", "; + } + std::cout << std::endl; + + std::cout << "All rectangularities:" << std::endl; + for(size_t i = 0; i < all_rectangularities_.size(); ++i){ + std::cout << all_rectangularities_[i] << ", "; + } + std::cout << std::endl; +} + + void Pass::reset(){ gm_position_.clear(); imgprocessor_.reset(); From d0257f95c218d013b8998cf376e009f31edcc7f2 Mon Sep 17 00:00:00 2001 From: Marco Camurri Date: Wed, 7 Apr 2021 10:04:43 +0100 Subject: [PATCH 56/58] add config files to visualise elevation map on rooster --- .../config/rooster_elevation_mapping.yaml | 35 ++ .../elevation_map_plane_seg_config.rviz | 397 ++++++++++++++++++ .../elevation_map_with_plane_seg.launch | 39 ++ 3 files changed, 471 insertions(+) create mode 100644 plane_seg/config/rooster_elevation_mapping.yaml create mode 100644 plane_seg_ros/config/elevation_map_plane_seg_config.rviz create mode 100644 plane_seg_ros/launch/elevation_map_with_plane_seg.launch diff --git a/plane_seg/config/rooster_elevation_mapping.yaml b/plane_seg/config/rooster_elevation_mapping.yaml new file mode 100644 index 0000000..ca41a49 --- /dev/null +++ b/plane_seg/config/rooster_elevation_mapping.yaml @@ -0,0 +1,35 @@ +point_cloud_topic: "/camera/depth/color/points" +map_frame_id: "odom" +sensor_frame_id: "ground_camera_depth_optical_frame" +robot_base_frame_id: "base" +robot_pose_with_covariance_topic: "/vilens/pose_optimized" +robot_pose_cache_size: 500 +track_point_frame_id: "base" +track_point_x: 0.0 +track_point_y: 0.0 +track_point_z: 0.0 + + +# Robot. + +min_update_rate: 0.5 +time_tolerance: 0.5 +time_offset_for_point_cloud: 0.0 +sensor_processor/ignore_points_above: 0.4 +robot_motion_map_update/covariance_scale: 0.01 + +# Map. +length_in_x: 10.0 +length_in_y: 4.0 +position_x: 0.0 +position_y: 0.0 +resolution: 0.02 +min_variance: 0.0001 +max_variance: 0.05 +mahalanobis_distance_threshold: 2.5 +multi_height_noise: 0.001 +surface_normal_positive_axis: z +fused_map_publishing_rate: 0.5 +enable_visibility_cleanup: false +visibility_cleanup_rate: 1.0 +scanning_duration: 1.0 diff --git a/plane_seg_ros/config/elevation_map_plane_seg_config.rviz b/plane_seg_ros/config/elevation_map_plane_seg_config.rviz new file mode 100644 index 0000000..0d5f14b --- /dev/null +++ b/plane_seg_ros/config/elevation_map_plane_seg_config.rviz @@ -0,0 +1,397 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /PointCloud22 + - /GridMap1 + - /Image1 + - /Marker1 + - /PointCloud23 + - /PointCloud23/Status1 + Splitter Ratio: 0.31725889444351196 + Tree Height: 573 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: Image +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base: + Alpha: 1 + Show Axes: false + Show Trail: false + base_mount: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bottom_screws_center: + Alpha: 1 + Show Axes: false + Show Trail: false + ground_camera_accel_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + ground_camera_accel_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + ground_camera_bottom_screw_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + ground_camera_color_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + ground_camera_color_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + ground_camera_depth_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + ground_camera_depth_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + ground_camera_gyro_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + ground_camera_gyro_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + ground_camera_infra1_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + ground_camera_infra1_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + ground_camera_infra2_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + ground_camera_infra2_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + ground_camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + ground_camera_shim: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + os_imu: + Alpha: 1 + Show Axes: false + Show Trail: false + os_lidar: + Alpha: 1 + Show Axes: false + Show Trail: false + os_sensor: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + realsense_parent: + Alpha: 1 + Show Axes: false + Show Trail: false + Name: RobotModel + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 1786 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /os_cloud_node/points + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /camera/depth/color/points + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Class: grid_map_rviz_plugin/GridMap + Color: 200; 200; 200 + Color Layer: elevation + Color Transformer: GridMapLayer + Enabled: true + Height Layer: elevation + Height Transformer: GridMapLayer + History Length: 1 + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 10 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: GridMap + Show Grid Lines: true + Topic: /rooster_elevation_mapping/elevation_map_raw + Unreliable: false + Use Rainbow: true + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /camera/infra1/image_rect_raw + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: compressed + Unreliable: false + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /plane_seg/hull_markers + Name: Marker + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: "" + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: "" + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /camera_ground/depth/color/points + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz/PoseWithCovariance + Color: 255; 25; 0 + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: PoseWithCovariance + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Arrow + Topic: /vilens/pose_optimized + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: odom + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 7.113917350769043 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.6538326740264893 + Y: -0.9280670285224915 + Z: 0.3764760196208954 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.319797158241272 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.9804503321647644 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1025 + Hide Left Dock: false + Hide Right Dock: true + Image: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000400000000000001b300000363fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002c8000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000030b000000950000001600ffffff000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005840000036300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1853 + X: 67 + Y: 27 diff --git a/plane_seg_ros/launch/elevation_map_with_plane_seg.launch b/plane_seg_ros/launch/elevation_map_with_plane_seg.launch new file mode 100644 index 0000000..39309b8 --- /dev/null +++ b/plane_seg_ros/launch/elevation_map_with_plane_seg.launch @@ -0,0 +1,39 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From b170c1f9f29d27d4e21679b15b4501db26243f73 Mon Sep 17 00:00:00 2001 From: Marco Camurri Date: Wed, 7 Apr 2021 11:01:48 +0100 Subject: [PATCH 57/58] go back to filter_chain.h for melodic compatibility --- plane_seg_ros/include/plane_seg_ros/Pass.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/plane_seg_ros/include/plane_seg_ros/Pass.hpp b/plane_seg_ros/include/plane_seg_ros/Pass.hpp index 2c94b33..4d34a47 100644 --- a/plane_seg_ros/include/plane_seg_ros/Pass.hpp +++ b/plane_seg_ros/include/plane_seg_ros/Pass.hpp @@ -16,7 +16,7 @@ #include #include #include -#include +#include #include namespace planeseg { From 537e2e254327740e7ae9c731cc69c4c8d6221e04 Mon Sep 17 00:00:00 2001 From: Christos Ioannou Date: Wed, 21 Apr 2021 12:32:48 +0100 Subject: [PATCH 58/58] take minimum bounding rectangle instead of convex hulls --- plane_seg/src/BlockFitter.cpp | 4 +- plane_seg/src/ImageProcessor.cpp | 41 +++--- .../config/elevation_map_filters_config.rviz | 86 +++++++++++-- .../config/plane_seg_sequential_config.rviz | 120 +++++++++++++++--- plane_seg_ros/src/plane_seg_ros.cpp | 13 +- 5 files changed, 214 insertions(+), 50 deletions(-) diff --git a/plane_seg/src/BlockFitter.cpp b/plane_seg/src/BlockFitter.cpp index 5583561..f8fc2ac 100644 --- a/plane_seg/src/BlockFitter.cpp +++ b/plane_seg/src/BlockFitter.cpp @@ -338,7 +338,7 @@ go() { results.push_back(result); } - if (mDebug) { + if (true) { // Used to be mDebug std::ofstream ofs("boxes.txt"); for (int i = 0; i < (int)results.size(); ++i) { auto& res = results[i]; @@ -383,7 +383,7 @@ go() { block.mPose = res.mPose; block.mPose.translation() -= block.mPose.rotation().col(2)*mBlockDimensions[2]/2; - block.mHull = res.mConvexHull; + block.mHull = res.mPolygon; // USED TO BE res.mConvexHull result.mBlocks.push_back(block); } if (mDebug) { diff --git a/plane_seg/src/ImageProcessor.cpp b/plane_seg/src/ImageProcessor.cpp index 0c0619c..12b7753 100644 --- a/plane_seg/src/ImageProcessor.cpp +++ b/plane_seg/src/ImageProcessor.cpp @@ -76,7 +76,7 @@ void contours::filterMinRectangularity(int min_rectangularity){ contour_area = cv::contourArea(temp[r]); rectangularity = contour_area / minarea_rect.size.area(); - rectangularities_.push_back(rectangularity); +// rectangularities_.push_back(rectangularity); if (rectangularity >= min_rectangularity){ contours_.push_back(temp[r]); @@ -196,15 +196,15 @@ void ImageProcessor::process(){ // drawContoursIP(large_contours_, "large_contours", 4); std::cout << "Convexities: " << std::endl; med_contours_.filterMinConvexity(0.9); // have another look at the threshold for convexity - ip_convexities_ = med_contours_.convexities_; +// ip_convexities_ = med_contours_.convexities_; // drawContoursIP(med_contours_, "filtered by convexity", 6); std::cout << "Elongations: " << std::endl; med_contours_.filterMinElongation(3); - ip_elongations_ = med_contours_.elongations_; +// ip_elongations_ = med_contours_.elongations_; // drawContoursIP(med_contours_, "filtered by elongation", 7); std::cout << "Rectangularities: " << std::endl; med_contours_.filterMinRectangularity(0.6); - ip_rectangularities_ = med_contours_.rectangularities_; +// ip_rectangularities_ = med_contours_.rectangularities_; // drawContoursIP(med_contours_, "filtered by rectangularity", 8); med_contours_.fitMinAreaRect(); large_contours_.approxAsPoly(); @@ -219,12 +219,11 @@ void ImageProcessor::process(){ displayResult(); - // int p = cv::waitKey(0); -/* if (p == 's'){ - saveImage(final_img_); - } - */ +// if (p == 's'){ +// saveImage(original_img_.image); +// } + } void ImageProcessor::copyOrigToProc(){ @@ -251,15 +250,18 @@ void ImageProcessor::displayResult(){ // cv::drawContours(processed_img_.image, all_contours_.contours_, i, cv::Scalar(255), cv::FILLED); } for(size_t j = 0; j < all_contours_.contours_rect_.size(); ++j){ - cv::drawContours(colour_img_.image, all_contours_.contours_rect_, j, cv::Scalar(0, 0, 255), 1, cv::LINE_AA); +// cv::drawContours(colour_img_.image, all_contours_.contours_rect_, j, cv::Scalar(0, 0, 255), 1, cv::LINE_AA); cv::drawContours(rect_img_.image, all_contours_.contours_rect_, j, cv::Scalar(255), cv::FILLED); } final_img_ = rect_img_; -// displayImage("final", colour_img_.image, 7); +// displayImage("final", rect_img_.image, 7); } void ImageProcessor::saveImage(cv::Mat image_){ + cv::Mat CV_8U_img; + image_.convertTo(CV_8U_img, CV_8U); + std::string imagename; std::cout << "Enter filename to save image (don't forget .png!): " << std::endl; std::cin >> imagename; @@ -274,7 +276,7 @@ void ImageProcessor::saveImage(cv::Mat image_){ return; } } - cv::imwrite(output_path.string() + imagename, image_); + cv::imwrite(output_path.string() + imagename, CV_8U_img); } else { std::cerr << "ERROR: the $HOME variable is empty!" << std::endl; return; @@ -366,7 +368,7 @@ void ImageProcessor::drawContoursIP(contours contour, std::string process, int n void ImageProcessor::histogram(cv_bridge::CvImage img){ int bins = 30; int histSize = bins; - float range[] = {0, 1}; + float range[] = {0, 2}; const float* histRange = { range }; bool uniform = true, accumulate = false; cv::Mat hist, mask; @@ -375,6 +377,12 @@ void ImageProcessor::histogram(cv_bridge::CvImage img){ std::cout << "Mask type: " << mask.type() << std::endl; cv::calcHist(&img.image, 1, 0, mask, hist, 1, &histSize, &histRange, uniform, accumulate); + std::cout << "Start Histogram: " << std::endl; + for (int m = 0; m < histSize ; ++m){ + std::cout << hist.at(m) << ", "; + } + std::cout << "END Histogram" << std::endl; + int hist_w = 512, hist_h = 400; int bin_w = cvRound( (double) hist_w/bins ); @@ -384,10 +392,9 @@ void ImageProcessor::histogram(cv_bridge::CvImage img){ for (int i = 1; i < histSize; i++){ cv::line(histImage, cv::Point(bin_w*(i-1), hist_h - cvRound(hist.at(i-1))), cv::Point(bin_w*(i), hist_h - cvRound(hist.at(i))), cv::Scalar(255, 0, 0), 2, 8, 0); } - std::cout << hist << std::endl; - cv::imshow("original_img_ histogram", histImage); - cv::waitKey(0); +// cv::imshow("original_img_ histogram", histImage); +// cv::waitKey(0); } @@ -417,7 +424,7 @@ cv::Mat ImageProcessor::createMask(cv_bridge::CvImage img){ } } - displayImage("mask", mask_, 8); +// displayImage("mask", mask_, 8); std::cout << "Exited displayImage()" << std::endl; return mask_; diff --git a/plane_seg_ros/config/elevation_map_filters_config.rviz b/plane_seg_ros/config/elevation_map_filters_config.rviz index 581207a..9eb000f 100644 --- a/plane_seg_ros/config/elevation_map_filters_config.rviz +++ b/plane_seg_ros/config/elevation_map_filters_config.rviz @@ -93,7 +93,7 @@ Visualization Manager: Marker Topic: /plane_seg/hull_markers Name: Marker Namespaces: - hull lines: true + {} Queue Size: 100 Value: true - Alpha: 0.20000000298023224 @@ -102,7 +102,7 @@ Visualization Manager: Color: 200; 200; 200 Color Layer: elevation Color Transformer: GridMapLayer - Enabled: false + Enabled: true Height Layer: elevation Height Transformer: GridMapLayer History Length: 1 @@ -116,7 +116,7 @@ Visualization Manager: Topic: /rooster_elevation_mapping/elevation_map Unreliable: false Use Rainbow: true - Value: false + Value: true - Alpha: 1 Axes Length: 0.5 Axes Radius: 0.05000000074505806 @@ -152,7 +152,7 @@ Visualization Manager: Marker Topic: /plane_seg/id_strings Name: MarkerArray Namespaces: - strings: true + {} Queue Size: 100 Value: true - Class: rviz/MarkerArray @@ -168,7 +168,7 @@ Visualization Manager: Marker Topic: /plane_seg/linestrips Name: MarkerArray Namespaces: - linestrips: true + {} Queue Size: 100 Value: true - Alpha: 0.20000000298023224 @@ -197,7 +197,7 @@ Visualization Manager: Marker Topic: /opencv/rectangles Name: MarkerArray Namespaces: - rectangles: true + {} Queue Size: 100 Value: true - Class: rviz/Marker @@ -205,9 +205,69 @@ Visualization Manager: Marker Topic: /opencv/test Name: Marker Namespaces: - test: true + {} Queue Size: 100 Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 1824 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /os_cloud_node/points + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz/PoseWithCovariance + Color: 255; 25; 0 + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: PoseWithCovariance + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Arrow + Topic: /vilens/pose_optimized + Unreliable: false + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -236,25 +296,25 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 8.178982734680176 + Distance: 9.770825386047363 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 1.707107663154602 - Y: -0.001718271290883422 - Z: -0.5765842795372009 + X: 4.317430019378662 + Y: 0.7584229111671448 + Z: 0.11780852824449539 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.4297971725463867 + Pitch: 0.739797830581665 Target Frame: Value: Orbit (rviz) - Yaw: 3.9073166847229004 + Yaw: 0.3391360640525818 Saved: ~ Window Geometry: Displays: diff --git a/plane_seg_ros/config/plane_seg_sequential_config.rviz b/plane_seg_ros/config/plane_seg_sequential_config.rviz index 86c38da..2c8a4a8 100644 --- a/plane_seg_ros/config/plane_seg_sequential_config.rviz +++ b/plane_seg_ros/config/plane_seg_sequential_config.rviz @@ -11,8 +11,10 @@ Panels: - /GridMap1 - /PoseWithCovariance1 - /GridMap2 + - /PointCloud22 + - /Image1 Splitter Ratio: 0.5058823823928833 - Tree Height: 726 + Tree Height: 405 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -43,7 +45,7 @@ Visualization Manager: Cell Size: 1 Class: rviz/Grid Color: 160; 160; 164 - Enabled: true + Enabled: false Line Style: Line Width: 0.029999999329447746 Value: Lines @@ -56,7 +58,7 @@ Visualization Manager: Plane: XY Plane Cell Count: 10 Reference Frame: - Value: true + Value: false - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -111,7 +113,7 @@ Visualization Manager: Min Color: 0; 0; 0 Min Intensity: 0 Name: GridMap - Show Grid Lines: true + Show Grid Lines: false Topic: /rooster_elevation_mapping/elevation_map Unreliable: false Use Rainbow: true @@ -142,7 +144,7 @@ Visualization Manager: Name: PoseWithCovariance Shaft Length: 0.5 Shaft Radius: 0.05000000074505806 - Shape: Axes + Shape: Arrow Topic: /vilens/pose Unreliable: false Value: true @@ -170,7 +172,7 @@ Visualization Manager: {} Queue Size: 100 Value: false - - Alpha: 0.20000000298023224 + - Alpha: 0.5 Autocompute Intensity Bounds: true Class: grid_map_rviz_plugin/GridMap Color: 200; 200; 200 @@ -186,7 +188,7 @@ Visualization Manager: Min Color: 0; 0; 0 Min Intensity: 0 Name: GridMap - Show Grid Lines: true + Show Grid Lines: false Topic: /rooster_elevation_mapping/filtered_map Unreliable: false Use Rainbow: true @@ -199,6 +201,90 @@ Visualization Manager: {} Queue Size: 100 Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz/PoseWithCovariance + Color: 255; 25; 0 + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: PoseWithCovariance + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Arrow + Topic: /vilens/pose_optimized + Unreliable: false + Value: true + - Alpha: 0.009999999776482582 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 100 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 2601 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /os_cloud_node/points + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz/Image + Enabled: false + Image Topic: /camera_ground/infra1/image_rect_raw/compressed + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + - Class: rviz/Image + Enabled: true + Image Topic: /camera_ground/infra1/image_rect_raw + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: compressed + Unreliable: false + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -227,33 +313,35 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 7.339658737182617 + Distance: 5.6142683029174805 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 2.9226090908050537 - Y: 0.5629675388336182 - Z: 1.743936538696289 + X: 3.212801933288574 + Y: 0.8613958358764648 + Z: 0.33506956696510315 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.34520265460014343 + Pitch: 0.6597974300384521 Target Frame: Value: Orbit (rviz) - Yaw: 5.375433921813965 + Yaw: 5.950908660888672 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1023 + Height: 1025 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd00000004000000000000021800000361fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000361000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000363000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000052b0000003efc0100000002fb0000000800540069006d006501000000000000052b000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000030d0000036100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Image: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000400000000000001be00000363fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000220000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000002d8000000c80000001600fffffffb0000000a0049006d00610067006501000002630000013d0000001600ffffff000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000363000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005790000036300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -262,6 +350,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1323 + Width: 1853 X: 67 Y: 27 diff --git a/plane_seg_ros/src/plane_seg_ros.cpp b/plane_seg_ros/src/plane_seg_ros.cpp index b598a8b..0224c82 100644 --- a/plane_seg_ros/src/plane_seg_ros.cpp +++ b/plane_seg_ros/src/plane_seg_ros.cpp @@ -314,6 +314,7 @@ void Pass::stepThroughFile(std::string filename){ frame_time = toc().count(); std::cout << frame_time << " ms: frame_" << frame << std::endl; timing_vector.push_back(frame_time); + elev_map_pub_.publish(*s); } geometry_msgs::PoseWithCovarianceStamped::ConstPtr i = m.instantiate(); @@ -324,6 +325,11 @@ void Pass::stepThroughFile(std::string filename){ } } + std::cout << "Timings: " << std::endl; + for (size_t k = 0; k < timing_vector.size(); ++k){ + std::cout << timing_vector[k] << ", "; + } + std::cout << std::endl; bag.close(); @@ -721,11 +727,14 @@ void Pass::imageProcessing(grid_map::GridMap &gridmap){ convertGridmapToFloatImage(gridmap, "slope", imgprocessor_.original_img_, true); imgprocessor_.process();; - getContourData(); +// getContourData(); - grid_map::GridMapCvConverter::addLayerFromImage(imgprocessor_.final_img_.image, "mask", gridmap); + std::cout << "got to here" << std::endl; if (algorithm_ == "B"){ + + grid_map::GridMapCvConverter::addLayerFromImage(imgprocessor_.final_img_.image, "mask", gridmap); + gridmap.add("product"); multiplyLayers(gridmap.get("elevation"), gridmap.get("mask"), gridmap.get("product")); replaceZeroToNan(gridmap.get("product"));