Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 #pragma once
00019
00020 #include <vector>
00021
00022 #include <opencv2/opencv.hpp>
00023
00024 class MapAccessibilityAnalysis
00025 {
00026 public:
00027
00028 struct Pose
00029 {
00030 float x;
00031 float y;
00032 float orientation;
00033
00034 Pose()
00035 {
00036 x = 0;
00037 y = 0;
00038 orientation = 0;
00039 }
00040
00041 Pose(float x_, float y_, float orientation_)
00042 {
00043 x = x_;
00044 y = y_;
00045 orientation = orientation_;
00046 }
00047 };
00048
00049
00050 MapAccessibilityAnalysis();
00051 ~MapAccessibilityAnalysis();
00052
00058 void inflateMap(const cv::Mat& original_map, cv::Mat& inflated_map, const int robot_radius_pixel);
00059
00069 void checkPoses(const std::vector<cv::Point>& points_to_check, std::vector<bool>& accessibility_flags,
00070 const cv::Mat& inflated_map, const bool approach_path_accessibility_check, const cv::Point& robot_location);
00071
00083 void checkPerimeter(std::vector<Pose>& accessible_poses_on_perimeter,
00084 const Pose& center, const double radius, const double rotational_sampling_step,
00085 const cv::Mat& inflated_map, const bool approach_path_accessibility_check, const cv::Point& robot_location);
00086
00087 template<class T>
00088 T convertFromMeterToPixelCoordinates(const Pose& pose, const cv::Point2d& map_origin, const double inverse_map_resolution)
00089 {
00090 T val;
00091 val.x = (pose.x - map_origin.x) * inverse_map_resolution;
00092 val.y = (pose.y - map_origin.y) * inverse_map_resolution;
00093 return val;
00094 }
00095
00096 template<class T>
00097 T convertFromPixelCoordinatesToMeter(const Pose& pose, const cv::Point2d& map_origin, const double map_resolution)
00098 {
00099 T val;
00100 val.x = map_resolution * pose.x + map_origin.x;
00101 val.y = map_resolution * pose.y + map_origin.y;
00102 return val;
00103 }
00104
00105 protected:
00106
00107
00108
00109
00110 bool isApproachPositionAccessible(const cv::Point& robotLocation,
00111 const cv::Point& potentialApproachPose,
00112 std::vector<std::vector<cv::Point> > contours);
00113
00114
00115
00116
00117 void computeClosestPointOnPolygon(const cv::Mat& map_with_polygon,
00118 const Pose& pose_p, Pose& closest_point_on_polygon);
00119
00120 };
00121