diff --git a/cfg/GroundSegmentation.cfg b/cfg/GroundSegmentation.cfg index 6025d769c449fffcc88e050a47e41e51d24eed12..1ebc86225c33ce2f5edb4147a606045834a75576 100644 --- a/cfg/GroundSegmentation.cfg +++ b/cfg/GroundSegmentation.cfg @@ -53,7 +53,7 @@ gen.add("elevation_grid_resolution", double_t, 0, "define the coarseness of the ## initial uncertainties gen.add("z_initial_std_dev", double_t, 0, "for ground reference initialization", 0.05, 0.01, 1.0); gen.add("initial_angular_std_dev_deg", double_t, 0, "for ground reference initialization", 1.5, 0.1, 10.0); -## propagation additive noises +## propagation X_k_additive noises gen.add("propagation_z_additive_noise_per_meter", double_t, 0, "Noise added to new ground references scaled by the range", 0.05, 0.0, 1.0); gen.add("propagation_additive_noise_deg_per_meter", double_t, 0, "Noise added to new ground references scaled by the range", 0.8, 0.0, 10.0); # measurement noise @@ -74,6 +74,7 @@ gen.add("show_dense_reconstruction", bool_t, 0, "creates dense pointcloud with g # low obstacle detection parameters (requires a good 3d odometry source) gen.add("detect_low_obstacles_using_odometry", bool_t, 0, "to accumulate ground points and analyze them to find low obstacles", True); +gen.add("ground_grid_size", double_t, 0, "ground grid will be of size 2*ground_grid_size squared", 10.0, 1.0, 50.0); gen.add("ground_grid_resolution", double_t, 0, "define the coarseness of the ground accumulation", 0.1, 0.01, 10.0); gen.add("number_of_neighbours_to_compare_height", int_t, 0, "a ground point will be declared low obstacle if the slope with any of its n neighbours is above a threshold", 4, 1, 100); gen.add("slope_threshold", double_t, 0, "Max ratio height change between ground points", 0.2, 0.01, 0.99); diff --git a/include/ground_segmentation_alg_node.h b/include/ground_segmentation_alg_node.h index 1a3adb4b8cbe5c9afbc19648dc569d3619731664..72d52bf1f08f797268fe964abe97fe2c60162423 100644 --- a/include/ground_segmentation_alg_node.h +++ b/include/ground_segmentation_alg_node.h @@ -31,6 +31,10 @@ // [publisher subscriber headers] #include "ground_segmentation_alg.h" #include <nav_msgs/Odometry.h> +#include <tf/tf.h> +#include <pcl/common/transforms.h> +#include "tf2_geometry_msgs/tf2_geometry_msgs.h" +#include <pcl/filters/passthrough.h> // [service client headers] @@ -44,6 +48,7 @@ class GroundSegmentationAlgNode : public algorithm_base::IriBaseAlgorithm<Ground { private: bool detect_low_obstacles_using_odometry_; + float ground_grid_size_; float ground_grid_resolution_; int number_of_neighbours_to_compare_height_; float slope_threshold_; @@ -88,7 +93,8 @@ private: nav_msgs::Odometry odom_3d_; void cbGetOdomMsg(const nav_msgs::Odometry::ConstPtr &msg); - pcl::PointCloud<pcl::PointXYZRGBNormal> ground_points_accumulation_; + pcl::PointCloud<pcl::PointXYZRGBNormal> ground_points_accumulation_in_odom_frame_; + Eigen::Matrix<float, 6, 1> X_k_; // x, y, z, roll, pitch, yaw in current iteration pthread_mutex_t mutex_; void mutex_enter(void); diff --git a/src/ground_segmentation_alg_node.cpp b/src/ground_segmentation_alg_node.cpp index 9c58f269d0216dc0b7d3561b12df92eef53220fa..4d0f2dfebf29ba37e3e95c5403fb06e3a6779cc8 100644 --- a/src/ground_segmentation_alg_node.cpp +++ b/src/ground_segmentation_alg_node.cpp @@ -170,6 +170,13 @@ GroundSegmentationAlgNode::GroundSegmentationAlgNode(void) : else this->detect_low_obstacles_using_odometry_ = config_.detect_low_obstacles_using_odometry; + if (!this->private_node_handle_.getParam("ground_grid_size", this->config_.ground_grid_size)) + { + ROS_WARN("GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'ground_grid_size' not found"); + } + else + this->ground_grid_size_ = config_.ground_grid_size; + if (!this->private_node_handle_.getParam("ground_grid_resolution", this->config_.ground_grid_resolution)) { ROS_WARN("GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'ground_grid_resolution' not found"); @@ -431,8 +438,127 @@ void GroundSegmentationAlgNode::showPerformanceStatistics(void) std::cout << "-------------------------------------------------------------------------------" << std::endl; } +void transformPointCloud(const Eigen::Matrix<float, 6, 1> pose, + const pcl::PointCloud<pcl::PointXYZRGBNormal> input_pcl_cloud, + pcl::PointCloud<pcl::PointXYZRGBNormal> &output_pcl_cloud) +{ + Eigen::Matrix<float, 4, 4> rigid_transform = Eigen::Matrix<float, 4, 4>::Identity(); + + rigid_transform(0, 3) = pose(0, 0); + rigid_transform(1, 3) = pose(1, 0); + rigid_transform(2, 3) = pose(2, 0); + + tf::Matrix3x3 rot; + rot.setRPY(pose(3, 0), pose(4, 0), pose(5, 0)); + + tf::Vector3 x_row = rot.getRow(0); + tf::Vector3 y_row = rot.getRow(1); + tf::Vector3 z_row = rot.getRow(2); + + rigid_transform(0, 0) = x_row.getX(); + rigid_transform(0, 1) = x_row.getY(); + rigid_transform(0, 2) = x_row.getZ(); + + rigid_transform(1, 0) = y_row.getX(); + rigid_transform(1, 1) = y_row.getY(); + rigid_transform(1, 2) = y_row.getZ(); + + rigid_transform(2, 0) = z_row.getX(); + rigid_transform(2, 1) = z_row.getY(); + rigid_transform(2, 2) = z_row.getZ(); + + //std::cout << "Transformation = " << std::endl << rigid_transform << std::endl; + + pcl::transformPointCloud(input_pcl_cloud, output_pcl_cloud, rigid_transform); +} + +void transformInversePointCloud(const Eigen::Matrix<float, 6, 1> pose, + const pcl::PointCloud<pcl::PointXYZRGBNormal> input_pcl_cloud, + pcl::PointCloud<pcl::PointXYZRGBNormal> &output_pcl_cloud) +{ + Eigen::Matrix<float, 4, 4> rigid_transform = Eigen::Matrix<float, 4, 4>::Identity(); + + rigid_transform(0, 3) = pose(0, 0); + rigid_transform(1, 3) = pose(1, 0); + rigid_transform(2, 3) = pose(2, 0); + + tf::Matrix3x3 rot; + rot.setRPY(pose(3, 0), pose(4, 0), pose(5, 0)); + + tf::Vector3 x_row = rot.getRow(0); + tf::Vector3 y_row = rot.getRow(1); + tf::Vector3 z_row = rot.getRow(2); + + rigid_transform(0, 0) = x_row.getX(); + rigid_transform(0, 1) = x_row.getY(); + rigid_transform(0, 2) = x_row.getZ(); + + rigid_transform(1, 0) = y_row.getX(); + rigid_transform(1, 1) = y_row.getY(); + rigid_transform(1, 2) = y_row.getZ(); + + rigid_transform(2, 0) = z_row.getX(); + rigid_transform(2, 1) = z_row.getY(); + rigid_transform(2, 2) = z_row.getZ(); + + rigid_transform = rigid_transform.inverse(); + + //std::cout << "Transformation = " << std::endl << rigid_transform << std::endl; + + pcl::transformPointCloud(input_pcl_cloud, output_pcl_cloud, rigid_transform); +} + +void discardPointsOutOfROI(const Eigen::Matrix<float, 6, 1> pose, const float ground_grid_size, pcl::PointCloud<pcl::PointXYZRGBNormal> &point_cloud_in_pose_frame) +{ + pcl::PointCloud<pcl::PointXYZRGBNormal> point_cloud_in_local_frame; + transformInversePointCloud(pose, point_cloud_in_pose_frame, point_cloud_in_local_frame); + + pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr point_cloud_in_local_frame_ptr(new pcl::PointCloud<pcl::PointXYZRGBNormal>); + *point_cloud_in_local_frame_ptr = point_cloud_in_local_frame; + + pcl::PassThrough<pcl::PointXYZRGBNormal> pass; + pass.setInputCloud (point_cloud_in_local_frame_ptr); + pass.setFilterFieldName ("x"); + pass.setFilterLimits (-1.0*ground_grid_size, ground_grid_size); + pass.filter (*point_cloud_in_local_frame_ptr); + + pass.setInputCloud (point_cloud_in_local_frame_ptr); + pass.setFilterFieldName ("y"); + pass.setFilterLimits (-1.0*ground_grid_size, ground_grid_size); + pass.filter (*point_cloud_in_local_frame_ptr); + + point_cloud_in_local_frame.clear(); + point_cloud_in_local_frame = *point_cloud_in_local_frame_ptr; + + transformPointCloud(pose, point_cloud_in_local_frame, point_cloud_in_pose_frame); +} + void GroundSegmentationAlgNode::detectLowObstacles(pcl::PointCloud<pcl::PointXYZRGBNormal> &lidar_pcl_cloud) { + pcl::PointCloud<pcl::PointXYZRGBNormal> ground_points; + for (pcl::PointCloud<pcl::PointXYZRGBNormal>::iterator i = lidar_pcl_cloud.begin(); i != lidar_pcl_cloud.end(); ++i) + if((int)std::floor(i->data_c[DATA_C_1_ID_CLASS]) == CLASS_GROUND) + ground_points.push_back(*i); + + pcl::PointCloud<pcl::PointXYZRGBNormal> ground_points_in_odom_frame; + + transformPointCloud(X_k_, ground_points, ground_points_in_odom_frame); + + ground_points_accumulation_in_odom_frame_ += ground_points_in_odom_frame; + + discardPointsOutOfROI(X_k_, ground_grid_size_, ground_points_accumulation_in_odom_frame_); + + pcl::PointCloud<pcl::PointXYZRGBNormal> ground_points_accumulation; + transformInversePointCloud(X_k_, ground_points_accumulation_in_odom_frame_, ground_points_accumulation); + + for (pcl::PointCloud<pcl::PointXYZRGBNormal>::iterator i = ground_points_accumulation.begin(); i != ground_points_accumulation.end(); ++i) + { + i->r = R_CLASS_GROUND; + i->g = G_CLASS_GROUND; + i->b = B_CLASS_GROUND; + + lidar_pcl_cloud.push_back(*i); + } } @@ -450,10 +576,11 @@ void GroundSegmentationAlgNode::mainNodeThread(void) this->alg_.lock(); this->alg_.segmentGround(common_header_, lidar_pcl_cloud_, roadmap_vertices_pcl_cloud_, roadmap_edges_array_, ground_dense_reconstruction_pcl_cloud_, roadmap_MarkerArray_msg_); + this->alg_.unlock(); + this->mutex_enter(); // Because we are going to use the odometry information, and we do not want the callback to collide with the processing this->detectLowObstacles(lidar_pcl_cloud_); - - this->alg_.unlock(); + this->mutex_exit(); this->convertOutputsToSuitableFormats(); this->publishAll(); @@ -480,6 +607,22 @@ void GroundSegmentationAlgNode::cbGetOdomMsg(const nav_msgs::Odometry::ConstPtr this->mutex_enter(); //std::cout << "GroundSegmentationAlgNode::cbGetOdomMsg --> odom message received!" << std::endl; odom_3d_ = *msg; + + X_k_(0) = odom_3d_.pose.pose.position.x; + X_k_(1) = odom_3d_.pose.pose.position.y; + X_k_(2) = odom_3d_.pose.pose.position.z; + + double roll, pitch, yaw; + tf2::Quaternion q_pose(odom_3d_.pose.pose.orientation.x, odom_3d_.pose.pose.orientation.y, + odom_3d_.pose.pose.orientation.z, odom_3d_.pose.pose.orientation.w); + + tf2::Matrix3x3 m_pose(q_pose); + m_pose.getRPY(roll, pitch, yaw); + + X_k_(3) = roll; //X_k_minus_one_(3, 0); + X_k_(4) = pitch; //X_k_minus_one_(4, 0); + X_k_(5) = yaw; + this->mutex_exit(); }