From 5e77f456b7785093d5821ef17ebfb4c0ee7d2418 Mon Sep 17 00:00:00 2001 From: Omer Spalter Date: Tue, 28 Mar 2023 17:58:35 +0300 Subject: [PATCH] Fix polygon draw --- .../include/cliff_detector/cliff_detector.hpp | 12 +--- cliff_detector/src/cliff_detector.cpp | 61 +++++++++---------- nav_layer_from_points/src/costmap_layer.cpp | 7 ++- 3 files changed, 34 insertions(+), 46 deletions(-) diff --git a/cliff_detector/include/cliff_detector/cliff_detector.hpp b/cliff_detector/include/cliff_detector/cliff_detector.hpp index ee93c7b..4327d5b 100755 --- a/cliff_detector/include/cliff_detector/cliff_detector.hpp +++ b/cliff_detector/include/cliff_detector/cliff_detector.hpp @@ -158,17 +158,6 @@ class CliffDetector */ template void findCliffInDepthImage(const sensor_msgs::msg::Image::ConstSharedPtr & depth_msg); - /** - * Calculate vertical angle_min and angle_max by measuring angles between - * the top ray, bottom ray, and optical center ray - * - * @param camModel The image_geometry camera model for this image. - * @param min_angle The minimum vertical angle - * @param max_angle The maximum vertical angle - */ - void fieldOfView( - double & min, double & max, double x1, double y1, - double xc, double yc, double x2, double y2); /** * @brief calcDeltaAngleForImgRows * @param vertical_fov @@ -215,6 +204,7 @@ class CliffDetector /// Store points which contain obstacle geometry_msgs::msg::PolygonStamped stairs_points_msg_; + double horizontal_fov; }; } // namespace cliff_detector diff --git a/cliff_detector/src/cliff_detector.cpp b/cliff_detector/src/cliff_detector.cpp index e0e403f..345ca6e 100755 --- a/cliff_detector/src/cliff_detector.cpp +++ b/cliff_detector/src/cliff_detector.cpp @@ -21,6 +21,11 @@ namespace cliff_detector { +struct FieldOfViews { + double horizontal_fov; + double vertical_fov; +}; + double toRad(double alpha) { return alpha * M_PI / 180.0; @@ -36,6 +41,18 @@ double length(const cv::Point3d & vec) return sqrt(squaredLength(vec)); } +FieldOfViews cameraInfoToFov(const sensor_msgs::msg::CameraInfo::ConstSharedPtr & info) +{ + struct FieldOfViews fovs; + double fx = info->k[0]; + double fy= info->k[4]; + + fovs.horizontal_fov = 2 * std::atan2(info->width, 2 * fx); + fovs.vertical_fov = 2 * std::atan2(info->height, 2 * fy); + + return fovs; +} + geometry_msgs::msg::PolygonStamped CliffDetector::detectCliff( const sensor_msgs::msg::Image::ConstSharedPtr & image, const sensor_msgs::msg::CameraInfo::ConstSharedPtr & info) @@ -45,13 +62,12 @@ geometry_msgs::msg::PolygonStamped CliffDetector::detectCliff( if (!depth_sensor_params_update || cam_model_update_) { camera_model_.fromCameraInfo(info); - double angle_min, angle_max, vertical_fov; - const auto cx = camera_model_.cx(); - const auto cy = camera_model_.cy(); + double vertical_fov; // Calculate field of views angles - vertical and horizontal - fieldOfView(angle_min, angle_max, cx, 0, cx, cy, cx, image->height - 1); - vertical_fov = angle_max - angle_min; + auto field_of_views = cameraInfoToFov(info); + horizontal_fov = field_of_views.horizontal_fov; + vertical_fov = field_of_views.vertical_fov; // Calculate angles between optical axis and rays for each row of image calcDeltaAngleForImgRows(vertical_fov); @@ -62,7 +78,7 @@ geometry_msgs::msg::PolygonStamped CliffDetector::detectCliff( // Sensor tilt compensation calcTiltCompensationFactorsForImgRows(); - // Check scan_height vs image_height + // Check scan_ height vs image_height if (used_depth_height_ > image->height) { // ROS_ERROR("Parameter used_depth_height is higher than height of image."); used_depth_height_ = image->height; @@ -197,26 +213,6 @@ double CliffDetector::angleBetweenRays( return acos(dot / (length(ray1) * length(ray2))); } -void CliffDetector::fieldOfView( - double & min, double & max, double x1, double y1, - double xc, double yc, double x2, double y2) -{ - const cv::Point2d raw_pixel_left(x1, y1); - const auto rect_pixel_left = camera_model_.rectifyPoint(raw_pixel_left); - const auto left_ray = camera_model_.projectPixelTo3dRay(rect_pixel_left); - - const cv::Point2d raw_pixel_right(x2, y2); - const auto rect_pixel_right = camera_model_.rectifyPoint(raw_pixel_right); - const auto right_ray = camera_model_.projectPixelTo3dRay(rect_pixel_right); - - const cv::Point2d raw_pixel_center(xc, yc); - const auto rect_pixel_center = camera_model_.rectifyPoint(raw_pixel_center); - const auto center_ray = camera_model_.projectPixelTo3dRay(rect_pixel_center); - - min = -angleBetweenRays(center_ray, right_ray); - max = angleBetweenRays(left_ray, center_ray); -} - void CliffDetector::calcDeltaAngleForImgRows(double vertical_fov) { const unsigned img_height = camera_model_.fullResolution().height; @@ -381,17 +377,16 @@ void CliffDetector::findCliffInDepthImage( stairs_points_msg_.polygon.points.clear(); pt.y = 0; - const double sensor_tilt = toRad(sensor_tilt_angle_); - std::vector>::iterator it; + double center_of_fov = horizontal_fov / 2; + double col_rad_weight = horizontal_fov / img_width; + for (it = stairs_points.begin(); it != stairs_points.end(); ++it) { - // Calculate point in XZ plane -- depth (z) unsigned row = (*it)[Row]; - pt.z = sensor_mount_height_ / std::tan(sensor_tilt + delta_row_[row]); + pt.x = sensor_mount_height_ / std::tan(delta_row_[row]); - // Calculate x value - const double depth = sensor_mount_height_ / std::sin(sensor_tilt + delta_row_[row]); - pt.x = ((*it)[Col] - camera_model_.cx()) * depth / camera_model_.fx(); + double horizontal_angle = center_of_fov - (*it)[Col] * col_rad_weight; + pt.y = std::tan(horizontal_angle) * pt.x; // Add point to message stairs_points_msg_.polygon.points.push_back(pt); diff --git a/nav_layer_from_points/src/costmap_layer.cpp b/nav_layer_from_points/src/costmap_layer.cpp index b0280d3..eb24d3e 100755 --- a/nav_layer_from_points/src/costmap_layer.cpp +++ b/nav_layer_from_points/src/costmap_layer.cpp @@ -57,10 +57,12 @@ void NavLayerFromPoints::pointsCallback( void NavLayerFromPoints::clearTransformedPoints() { - if (transformed_points_.size() > 10000) + if (transformed_points_.size() > 10000) { transformed_points_.clear(); + } auto p_it = transformed_points_.begin(); + while (p_it != transformed_points_.end()) { if (clock_->now() - (*p_it).header.stamp > points_keep_time_) { p_it = transformed_points_.erase(p_it); @@ -82,8 +84,9 @@ void NavLayerFromPoints::updateBounds( std::string global_frame = layered_costmap_->getGlobalFrameID(); // Check if there are points to remove in transformed_points list and if so, then remove it - if (!transformed_points_.empty()) + if (!transformed_points_.empty()) { clearTransformedPoints(); + } // Add points to PointStamped list transformed_points_ for (const auto& point : points_list_.polygon.points) {