diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/.catkin_workspace b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/.catkin_workspace deleted file mode 100644 index 52fd97e..0000000 --- a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/.catkin_workspace +++ /dev/null @@ -1 +0,0 @@ -# This file currently only serves to mark the location of a catkin workspace for tool integration diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/.gitignore b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/.gitignore new file mode 100644 index 0000000..5243d90 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/.gitignore @@ -0,0 +1,48 @@ +# default .gitignore file for ipa320 repositories +# master file can be found in https://github.com/ipa320/care-o-bot/blob/indigo_dev/.gitignore + +build/ +bin/ +lib/ + +# generated docs +*.dox +*.wikidoc + +# eclipse +.project +.cproject + +# qcreator +qtcreator-* +*.user + +# Emacs +.#* + +# VI/VIM +*.swp + +# python files +*.pcd +*.pyc +*.pco + +# temporary files +*~ + +# merge conflict files +*.orig +*BACKUP* +*BASE* +*LOCAL* +*REMOTE* + +# Catkin custom files +CATKIN_IGNORE + +# result files +*.map + +# Kdev +*.kdev4 diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/.travis.rosinstall b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/.travis.rosinstall new file mode 100644 index 0000000..1bb8bf6 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/.travis.rosinstall @@ -0,0 +1 @@ +# empty diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/.travis.rosinstall.melodic b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/.travis.rosinstall.melodic new file mode 100644 index 0000000..0bce297 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/.travis.rosinstall.melodic @@ -0,0 +1,8 @@ +- git: + local-name: cob_extern + uri: https://github.com/ipa320/cob_extern.git + version: indigo_dev +- git: + local-name: cob_perception_common + uri: https://github.com/ipa320/cob_perception_common.git + version: indigo_dev diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/.travis.yml b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/.travis.yml new file mode 100644 index 0000000..4babe29 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/.travis.yml @@ -0,0 +1,30 @@ +language: generic +services: + - docker + +notifications: + email: + on_success: change + on_failure: always +env: + global: + - ROS_REPO=ros + matrix: + - ROS_DISTRO=kinetic UPSTREAM_WORKSPACE=file + - ROS_DISTRO=kinetic UPSTREAM_WORKSPACE=debian + - ROS_DISTRO=melodic UPSTREAM_WORKSPACE=file + - ROS_DISTRO=melodic UPSTREAM_WORKSPACE=debian + - ROS_DISTRO=noetic UPSTREAM_WORKSPACE=file + - ROS_DISTRO=noetic UPSTREAM_WORKSPACE=debian + +matrix: + allow_failures: + - env: ROS_DISTRO=kinetic UPSTREAM_WORKSPACE=file + - env: ROS_DISTRO=kinetic UPSTREAM_WORKSPACE=debian + - env: ROS_DISTRO=melodic UPSTREAM_WORKSPACE=debian + - env: ROS_DISTRO=noetic UPSTREAM_WORKSPACE=debian +install: + - git clone https://github.com/ros-industrial/industrial_ci.git .ci_config +script: + - .ci_config/travis.sh +# - ./travis.sh # Enable this when you have a package-local script diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/README.md b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/README.md new file mode 100644 index 0000000..eae5963 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/README.md @@ -0,0 +1,37 @@ +# ipa_coverage_planning +Algorithms for floor plan segmentation and systematic coverage driving patterns. + +If you find this software useful in your work, please cite our corresponding papers: +- R. Bormann, F. Jordan, W. Li, J. Hampp, and M. Hägele. Room Segmentation: Survey, Implementation, and Analysis. In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), 2016. https://ieeexplore.ieee.org/abstract/document/7487234 , https://publica.fraunhofer.de/entities/publication/0bf23149-75d5-4601-bfce-992d91698862/details +- R. Bormann, F. Jordan, J. Hampp, and M. Hägele. Indoor coverage path planning: Survey, implementation, analysis. In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), pages 1718–1725, May 2018. https://ieeexplore.ieee.org/abstract/document/8460566 , https://publica.fraunhofer.de/entities/publication/f537c15d-4cbe-4672-9d86-6e756a9ce71b/details + +## ROS Distro Support + +| | Indigo | Jade | Kinetic | Melodic | Noetic | +|:-------:|:------:|:----:|:-------:|:-------:|:-------:| +| Branch | [`indigo_dev`](https://github.com/ipa320/ipa_coverage_planning/tree/indigo_dev) | [`indigo_dev`](https://github.com/ipa320/ipa_coverage_planning/tree/indigo_dev) | [`indigo_dev`](https://github.com/ipa320/ipa_coverage_planning/tree/indigo_dev) | [`melodic_dev`](https://github.com/ipa320/ipa_coverage_planning/tree/melodic_dev) |[`noetic_dev`](https://github.com/ipa320/ipa_coverage_planning/tree/noetic_dev) | +| Status | not supported | not supported | EOL | supported | supported | +| Version | [version](http://repositories.ros.org/status_page/ros_indigo_default.html?q=ipa_coverage_planning) | [version](http://repositories.ros.org/status_page/ros_jade_default.html?q=ipa_coverage_planning) | [version](http://repositories.ros.org/status_page/ros_kinetic_default.html?q=ipa_coverage_planning) | [version](http://repositories.ros.org/status_page/ros_melodic_default.html?q=ipa_coverage_planning) | [version](http://repositories.ros.org/status_page/ros_noetic_default.html?q=ipa_coverage_planning) + +## Travis - Continuous Integration + +Status: [![Build Status](https://travis-ci.org/ipa320/ipa_coverage_planning.svg?branch=indigo_dev)](https://travis-ci.org/ipa320/ipa_coverage_planning) + + +## Quick start + +1. Bring up your robot or launch the turtlebot3 simulation + +1. Start the room exploration action server: + + ``` + roslaunch ipa_room_exploration room_exploration_action_server.launch + ``` + + +1. Start the room exploration action client: + + ``` + roslaunch ipa_room_exploration room_exploration_client.launch robot_env:=your-robot-env use_test_maps:=false + ``` + diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_msgs/CHANGELOG.rst b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_msgs/CHANGELOG.rst new file mode 100644 index 0000000..f8a3879 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_msgs/CHANGELOG.rst @@ -0,0 +1,13 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package ipa_building_msgs +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.0 (2017-02-09) +------------------ +* added a srv folder +* added a srv for ipa_room_exploration/coverage_check_server +* Contributors: Florian Jordan + +0.0.1 (2016-08-06) +------------------ +* package set up with first set of msgs +* Contributors: Florian Jordan, Richard Bormann diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_msgs/CMakeLists.txt b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_msgs/CMakeLists.txt new file mode 100644 index 0000000..0529155 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_msgs/CMakeLists.txt @@ -0,0 +1,79 @@ +cmake_minimum_required(VERSION 2.8.3) +project(ipa_building_msgs) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + actionlib + actionlib_msgs + genmsg + geometry_msgs + message_generation + sensor_msgs + std_msgs +) + +####################################### +## Declare ROS messages and services ## +####################################### + +## Add the Files for actionlib +add_action_files( + DIRECTORY action + FILES + MapSegmentation.action + FindRoomSequenceWithCheckpoints.action + RoomExploration.action +) + +## Generate messages in the 'msg' folder +add_message_files( + DIRECTORY + msg + FILES + RoomInformation.msg + RoomSequence.msg + dis_info.msg + dis_info_array.msg +) + +## Generate services in the 'srv' folder +add_service_files( + DIRECTORY + srv + FILES + CheckCoverage.srv + ExtractAreaMapFromLabeledMap.srv +) + +## Generate added messages and services with any dependencies listed here +generate_messages( + DEPENDENCIES + actionlib_msgs + geometry_msgs + sensor_msgs + std_msgs +) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if you package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + CATKIN_DEPENDS + actionlib_msgs + geometry_msgs + message_runtime + sensor_msgs + std_msgs +) + +include_directories( + ${catkin_INCLUDE_DIRS} +) diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_msgs/action/FindRoomSequenceWithCheckpoints.action b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_msgs/action/FindRoomSequenceWithCheckpoints.action new file mode 100644 index 0000000..b436771 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_msgs/action/FindRoomSequenceWithCheckpoints.action @@ -0,0 +1,17 @@ +# goal definition +sensor_msgs/Image input_map # floor plan map [mono8 format], 0=obstacle or unknown, 255=free space +float32 map_resolution # the resolution of the map in [meter/cell] +geometry_msgs/Pose map_origin # the origin of the map in [meter] +ipa_building_msgs/RoomInformation[] room_information_in_pixel # room data (min/max coordinates, center coordinates) measured in pixels +float64 robot_radius # the robot footprint radius [m], used for excluding areas from path planning that could not be visited by the robot +geometry_msgs/Pose robot_start_coordinate # current robot location (used to determine the closest checkpoint in the sequence of checkpoints) [in meter] + +--- + +# result definition +ipa_building_msgs/RoomSequence[] checkpoints # sequence of checkpoints, should be ordered in optimized traveling salesman sequence + # (checkpoint = a clique of rooms close [in terms of robot driving distance] to each other, + # and a central [in terms of rooms in the group] checkpoint location) +sensor_msgs/Image sequence_map # map that has the calculated sequence drawn in +--- +#feedback definition diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_msgs/action/MapSegmentation.action b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_msgs/action/MapSegmentation.action new file mode 100644 index 0000000..9f443ce --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_msgs/action/MapSegmentation.action @@ -0,0 +1,39 @@ +# Map Segmentation action +# Provides interface to get the segmented map + +# goal definition +sensor_msgs/Image input_map # the action server need a map as a input image to segment it, IMPORTANT: The algorithm needs a black and white 8bit single-channel image (format 8UC1), which is 0 (black) for obstacles and 255 (white) for free space +float32 map_resolution # the resolution of the map in [meter/cell] +geometry_msgs/Pose map_origin # the origin of the map in [meter] +bool return_format_in_pixel # return room data (see below) in [pixel] +bool return_format_in_meter # return room data (see below) in [meter] +float32 robot_radius # in [meter]; if this variable is set to a value greater than 0, the room centers are chosen at locations that are reachable from neighboring rooms (i.e. not inside the legs of a table surrounded by chairs) +int32 room_segmentation_algorithm # optionally overrides the parameterized segmentation method (if -1 it just takes the preset) + # 0 = take the preset (parameterized) method + # 1 = MorphologicalSegmentation + # 2 = DistanceSegmentation + # 3 = VoronoiSegmentation + # 4 = SemanticSegmentation + # 5 = RandomFieldSegmentation + # 99 = PassThrough (just get a pre-segmented map into the right output format) + +--- + +# result definition +sensor_msgs/Image segmented_map # the action server returns a map segmented into rooms which carry the segment number in every pixel cell, format 32SC1, room labels from 1 to N, room with label i -> access to room_information_in_pixel[i-1] +float32 map_resolution # the resolution of the segmented map in [meter/cell] +geometry_msgs/Pose map_origin # the origin of the segmented map in [meter] +# for the following data: value in pixel can be obtained when the value of [return_format_in_pixel] from goal definition is true +# the computed room centers are not the centroids of the room cells (which could be located outside of the rooms) +# but accessible map pixels located within the center of the largest free space in the room +ipa_building_msgs/RoomInformation[] room_information_in_pixel # room data (min/max coordinates, center coordinates) measured in pixels +# for the following data: value in meter can be obtained when the value of [return_format_in_meter] from goal definition is true +# the computed room centers are not the centroids of the room cells (which could be located outside of the rooms) +# but accessible map pixels located within the center of the largest free space in the room +ipa_building_msgs/RoomInformation[] room_information_in_meter # room data (min/max coordinates, center coordinates) measured in meters +# if wanted the 5th algorithm (vrf) can return single points labeled as a doorway +geometry_msgs/Point32[] doorway_points + +--- + +#feedback definition \ No newline at end of file diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_msgs/action/RoomExploration.action b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_msgs/action/RoomExploration.action new file mode 100644 index 0000000..3e9e029 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_msgs/action/RoomExploration.action @@ -0,0 +1,24 @@ +# Room Exploration action +# sends an occupancy grid map of a room to the server, which plans a path trough it to cover or inspect the whole room by a device or camera + +# goal definition +sensor_msgs/Image input_map # the action server needs a map as a input image to segment it, + # IMPORTANT: The algorithm needs a black and white 8bit single-channel image (format 8UC1), which is 0 (black) for obstacles and 255 (white) for free space + # todo: the image needs to be vertically mirrored compared to the map in RViz for using right coordinate systems + # OccupancyGrid map = origin lower left corner, image = origin upper left corner + # todo: take the OccupanyGrid message here instead to avoid confusion and deal with map coordinates in server +float32 map_resolution # the resolution of the map in [meter/cell] +geometry_msgs/Pose map_origin # the origin of the map in [meter], NOTE: rotations are not supported for now +float32 robot_radius # effective robot radius, taking the enlargement of the costmap into account, in [meter] +float32 coverage_radius # radius that is used to plan the coverage planning for the robot and not the field of view, assuming that the part that needs to cover everything (e.g. the cleaning part) can be represented by a fitting circle (e.g. smaller than the actual part to ensure coverage), in [meter] +geometry_msgs/Point32[] field_of_view # the 4 points that define the field of view of the robot, relatively to the robot coordinate system (with x pointing forwards and y pointing to the left), in [meter] +geometry_msgs/Point32 field_of_view_origin # the mounting position of the camera spanning the field of view, relative to the robot center (x-axis points to robot's front side, y-axis points to robot's left side, z-axis upwards), in [meter] +geometry_msgs/Pose2D starting_position # starting pose of the robot in the room coordinate system [meter,meter,rad] +int32 planning_mode # 1 = plans a path for coverage with the robot footprint, 2 = plans a path for coverage with the robot's field of view + +--- +# result definition +geometry_msgs/Pose2D[] coverage_path # when the server should return the coverage path, this is done returning the points in an array that shows the order of visiting +geometry_msgs/PoseStamped[] coverage_path_pose_stamped # (same content as coverage_path but different format) when the server should return the coverage path, this is done returning the points in an array that shows the order of visiting +--- +# feedback definition diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_msgs/msg/RoomInformation.msg b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_msgs/msg/RoomInformation.msg new file mode 100644 index 0000000..5ee2539 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_msgs/msg/RoomInformation.msg @@ -0,0 +1,2 @@ +geometry_msgs/Polygon room_min_max # first point provides the minimum (x,y)-coordinate values of the rooms, second point provides the maximum (x,y)-coordinate values of the rooms +geometry_msgs/Point32 room_center # provides the (x,y)-coordinate values of the room centers \ No newline at end of file diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_msgs/msg/RoomSequence.msg b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_msgs/msg/RoomSequence.msg new file mode 100644 index 0000000..87439d1 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_msgs/msg/RoomSequence.msg @@ -0,0 +1,5 @@ +uint32[] room_indices # indices of those rooms that belong to one group (connected to a checkpoint, + # i.e. the clique of rooms close [in terms of robot driving distance] to each other), + # indices should be ordered in optimized traveling salesman sequence +geometry_msgs/Point32 checkpoint_position_in_pixel # provides the (x,y)-coordinates of the checkpoint location for this group of rooms [in pixel] +geometry_msgs/Point32 checkpoint_position_in_meter # provides the (x,y)-coordinates of the checkpoint location for this group of rooms [in meter] \ No newline at end of file diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_msgs/msg/dis_info.msg b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_msgs/msg/dis_info.msg new file mode 100644 index 0000000..fc9a9af --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_msgs/msg/dis_info.msg @@ -0,0 +1,4 @@ +float32 distance +float32 width +float32 height +float32 angle diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_msgs/msg/dis_info_array.msg b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_msgs/msg/dis_info_array.msg new file mode 100644 index 0000000..77a722d --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_msgs/msg/dis_info_array.msg @@ -0,0 +1 @@ +ipa_building_msgs/dis_info[] dis \ No newline at end of file diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_msgs/package.xml b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_msgs/package.xml new file mode 100644 index 0000000..2162e5a --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_msgs/package.xml @@ -0,0 +1,27 @@ + + + ipa_building_msgs + 0.2.0 + + + This package contains common message type definitions for building navigation tasks such as floor plan segmentation into rooms or the optimal trajectory through rooms of a building. + + + LGPL for academic and non-commercial use; for commercial use, please contact Fraunhofer IPA + + Richard Bormann + Richard Bormann + + catkin + + message_generation + message_runtime + + actionlib + actionlib_msgs + geometry_msgs + sensor_msgs + std_msgs + + + diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_msgs/srv/CheckCoverage.srv b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_msgs/srv/CheckCoverage.srv new file mode 100644 index 0000000..76b6762 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_msgs/srv/CheckCoverage.srv @@ -0,0 +1,12 @@ +sensor_msgs/Image input_map # the action server need a map as a input image to segment it, IMPORTANT: The algorithm needs a black and white 8bit single-channel image, which is 0 (black) for obstacles and 255 (white) for free space +float32 map_resolution # resolution of the given map, in [meter/cell] +geometry_msgs/Pose map_origin # the origin of the map, in [meter] +geometry_msgs/Pose2D[] path # check the coverage along this path of the robot center, in the world frame in [meter] +geometry_msgs/Point32[] field_of_view # the points that define the field of view of the robot, relative to the robot center (x-axis points to robot's front side, y-axis points to robot's left side, z-axis upwards), in [meter] +geometry_msgs/Point32 field_of_view_origin # the mounting position of the camera spanning the field of view, relative to the robot center (x-axis points to robot's front side, y-axis points to robot's left side, z-axis upwards), in [meter] +float32 coverage_radius # radius that is used to plan the coverage planning for the robot and not the field of view, assuming that the part that needs to cover everything (e.g. the cleaning part) can be represented by a fitting circle (e.g. smaller than the actual part to ensure coverage), in [meter] +bool check_for_footprint # determine, if the coverage check should be done for the footprint or the field of view +bool check_number_of_coverages # if set, the server returns a map that shows how often one pixel has been covered during the path, return format: 32bit single-channel image +--- +sensor_msgs/Image coverage_map # the map that has the covered areas drawn in, with a value of 255, an 8bit single-channel image +sensor_msgs/Image number_of_coverage_image # the image that carries for each pixel the number of coverages when executing the path, 32bit single-channel image diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_msgs/srv/ExtractAreaMapFromLabeledMap.srv b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_msgs/srv/ExtractAreaMapFromLabeledMap.srv new file mode 100644 index 0000000..401834a --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_msgs/srv/ExtractAreaMapFromLabeledMap.srv @@ -0,0 +1,14 @@ +# The request message provides a segmented map which consists of cells with label 0 for inaccessible areas and other number from 1 to N +# for labeling membership with one of the N segmented areas. +# The return message shall deliver the same map but with only one area (segment_of_interest) labeled as 255 and the remainder labeled +# as inaccessible with 0. + +sensor_msgs/Image segmented_map # a map segmented into N areas which carry the respective segment number in every pixel cell, + # format 32SC1, room labels from 1 to N, 0 represents inaccessible cells (e.g. walls) +int32 segment_of_interest # the specific area label of the area of which a map shall be created + +--- + +sensor_msgs/Image segmented_area # a map that only contains the area which is labeled with segment_of_interest in segmented_map, + # the parts of that area are labeled as 255 in this map and the remainder is labeled as inaccessible with 0 + # data type is a 8bit single-channel image (format 8UC1) \ No newline at end of file diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/CMakeLists.txt b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/CMakeLists.txt new file mode 100644 index 0000000..c9fbd4f --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/CMakeLists.txt @@ -0,0 +1,202 @@ +cmake_minimum_required(VERSION 2.8.3) +project(ipa_building_navigation) + +# build with c++11 +add_compile_options(-std=c++11) + +set(catkin_RUN_PACKAGES + actionlib + cv_bridge + geometry_msgs + ipa_building_msgs + roscpp + roslib + sensor_msgs + visualization_msgs +) + +set(catkin_BUILD_PACKAGES + ${catkin_RUN_PACKAGES} + dynamic_reconfigure +) + +## Find catkin macros and libraries +find_package(catkin REQUIRED COMPONENTS + ${catkin_BUILD_PACKAGES} +) + +generate_dynamic_reconfigure_options( + cfg/BuildingNavigation.cfg +) + +find_package(OpenCV REQUIRED) +find_package(Boost REQUIRED COMPONENTS system thread chrono) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if you package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +# important: common/include needs to be here if you have such a directory +catkin_package( +INCLUDE_DIRS + common/include + ros/include +LIBRARIES + ${PROJECT_NAME} + tsp_solvers +CATKIN_DEPENDS + ${catkin_RUN_PACKAGES} +DEPENDS +# libconcorde_tsp_solver + OpenCV + Boost +) + + +########### +## Build ## +########### +## Specify additional locations of header files +## Your package locations should be listed before other locations +# include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) +# important: common/include also needs to be here if you have it +include_directories( + common/include + ros/include + ${catkin_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} +) + +# TSP library +add_library(tsp_solvers + common/src/A_star_pathplanner.cpp + common/src/node.cpp + common/src/nearest_neighbor_TSP.cpp + common/src/genetic_TSP.cpp + common/src/concorde_TSP.cpp +) +target_link_libraries(tsp_solvers + ${catkin_LIBRARIES} + ${OpenCV_LIBRARIES} + ${Boost_LIBRARIES} +) +add_dependencies(tsp_solvers + ${catkin_EXPORTED_TARGETS} + ${${PROJECT_NAME}_EXPORTED_TARGETS} +) + +# package library +add_library(${PROJECT_NAME} STATIC + common/src/maximal_clique_finder.cpp + common/src/set_cover_solver.cpp + common/src/trolley_position_finder.cpp +) +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} + ${OpenCV_LIBRARIES} + ${Boost_LIBRARIES} +) +add_dependencies(${PROJECT_NAME} + ${catkin_EXPORTED_TARGETS} + ${${PROJECT_NAME}_EXPORTED_TARGETS} +) + +# action provider +add_executable(room_sequence_planning_server + ros/src/room_sequence_planning_action_server.cpp + ) +target_link_libraries(room_sequence_planning_server + tsp_solvers + ${PROJECT_NAME} + ${catkin_LIBRARIES} + ${OpenCV_LIBRARIES} + ${Boost_LIBRARIES} +) +add_dependencies(room_sequence_planning_server + ${catkin_EXPORTED_TARGETS} + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${PROJECT_NAME}_gencfg +) + +# client for testing purpose +add_executable(room_sequence_planning_client ros/src/room_sequence_planning_action_client.cpp) +target_link_libraries(room_sequence_planning_client + ${catkin_LIBRARIES} + ${OpenCV_LIBRARIES} +) +add_dependencies(room_sequence_planning_client ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) + +# client for testing purpose +add_executable(room_sequence_planning_evaluation + ros/src/room_sequence_planning_evaluation.cpp + common/src/A_star_pathplanner.cpp + common/src/node.cpp +) +target_link_libraries(room_sequence_planning_evaluation + ${catkin_LIBRARIES} + ${OpenCV_LIBRARIES} +) +add_dependencies(room_sequence_planning_evaluation ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) + +# evaluation +add_executable(TSP_evaluation + ros/src/boosttest.cpp +) +target_link_libraries(TSP_evaluation + tsp_solvers + ${catkin_LIBRARIES} + ${OpenCV_LIBRARIES} +) +add_dependencies(TSP_evaluation ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) + +#tester for different functions +#add_executable(a_star_tester ros/src/tester.cpp common/src/A_star_pathplanner.cpp common/src/node.cpp common/src/nearest_neighbor_TSP.cpp common/src/genetic_TSP.cpp common/src/concorde_TSP.cpp common/src/maximal_clique_finder.cpp common/src/set_cover_solver.cpp common/src/trolley_position_finder.cpp) +#target_link_libraries(a_star_tester ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ${Boost_LIBRARIES}) +#add_dependencies(a_star_tester ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) + + +############# +## Install ## +############# +# Mark executables and/or libraries for installation +install(TARGETS ${PROJECT_NAME} tsp_solvers room_sequence_planning_server room_sequence_planning_client #a_star_tester tester + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +#uncomment this if you have a common-directory with header-files +install(DIRECTORY common/include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) + +#uncomment this if you have header-files in your project +install(DIRECTORY ros/include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) + +#install(DIRECTORY scripts +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# PATTERN ".svn" EXCLUDE +#) + +##uncomment this if you use launch and yaml files +install(DIRECTORY ros/launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/ros + PATTERN ".svn" EXCLUDE +) + +#install(DIRECTORY common/files +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/common +# PATTERN ".svn" EXCLUDE +#) diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/cfg/BuildingNavigation.cfg b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/cfg/BuildingNavigation.cfg new file mode 100644 index 0000000..cb41b52 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/cfg/BuildingNavigation.cfg @@ -0,0 +1,42 @@ +#!/usr/bin/env python +PACKAGE = "ipa_building_navigation" + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + +# TSP solver +tsp_enum = gen.enum([ gen.const("NearestNeighbor", int_t, 1, "Use the nearest neighbor TSP algorithm."), + gen.const("GeneticSolver", int_t, 2, "Use the genetic TSP algorithm."), + gen.const("ConcordeSolver", int_t, 3, "Use the Concorde TSP algorithm.")], + "TSP solver") +gen.add("tsp_solver", int_t, 0, "TSP solver", 3, 1, 3, edit_method=tsp_enum) + +# problem setting +problem_setting_enum = gen.enum([ gen.const("SimpleOrderPlanning", int_t, 1, "Plan the optimal order of a simple set of locations."), + gen.const("CheckpointBasedPlanning", int_t, 2, "Two-stage planning that creates local cliques of locations (= checkpoints) and determines the optimal order through the members of each clique as well as the optimal order through the cliques.")], + "Problem setting of the sequence planning problem") +gen.add("problem_setting", int_t, 0, "Problem setting of the sequence planning problem", 2, 1, 2, edit_method=problem_setting_enum) + + +# checkpoint-based sequence planning specifics +trolley_placement_method_enum = gen.enum([ gen.const("DragTrolleyWhenNecessary", int_t, 1, "Drag the trolley if it is too far away from next room."), + gen.const("PrecomputeRoomGroupsAndCorrespondingTrolleyPosition", int_t, 2, "Put rooms together in groups and calculate a corresponding trolley positions.")], + "Trolley placement method (only applies to CheckpointBasedPlanning problem)") +gen.add("planning_method", int_t, 0, "Trolley placement method (only applies to CheckpointBasedPlanning problem)", 2, 1, 2, edit_method=trolley_placement_method_enum) + +gen.add("max_clique_path_length", double_t, 0, "Max A* path length between two rooms that are assigned to the same clique, in [m] (only applies to CheckpointBasedPlanning problem)", 12.0, 0.0) + +gen.add("maximum_clique_size", int_t, 0, "Maximal number of nodes belonging to one trolley clique (only applies to CheckpointBasedPlanning problem)", 9001, 0) + + +# general settings +gen.add("map_downsampling_factor", double_t, 0, "The map may be downsampled during computations (e.g. of A* path lengths) in order to speed up the algorithm, if set to 1 the map will have original size, if set to 0 the algorithm won't work", 0.25, 0.00001, 1.0) + +gen.add("check_accessibility_of_rooms", bool_t, 0, "Tells the sequence planner if it should check the given room centers for accessibility from the starting position", True) + +gen.add("return_sequence_map", bool_t, 0, "Tells the server if the map with the sequence drawn in should be returned", False) + +gen.add("display_map", bool_t, 0, "Show the resulting map with paths (only if return_sequence_map=true)", False) + +exit(gen.generate(PACKAGE, "room_sequence_planning_action_server", "BuildingNavigation")) diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/common/include/ipa_building_navigation/A_star_pathplanner.h b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/common/include/ipa_building_navigation/A_star_pathplanner.h new file mode 100644 index 0000000..cd75565 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/common/include/ipa_building_navigation/A_star_pathplanner.h @@ -0,0 +1,62 @@ +// Astar.cpp +// http://en.wikipedia.org/wiki/A* +// Compiler: Dev-C++ 4.9.9.2 +// FB - 201012256 +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +#pragma once //make sure this header gets included only one time when multiple classes need it in the same project + //regarding to https://en.wikipedia.org/wiki/Pragma_once this is more efficient than #define + +//This class provides an AStar pathplanner, which calculates the pathlength from one cv::Point to another. It was taken from +// http://code.activestate.com/recipes/577457-a-star-shortest-path-algorithm/ and slightly changed (to use it with openCV). +//It needs node.h to work, which gives an implementation of nodes of the graph. +// +//!!!!!!!!!Important!!!!!!!!!!!!! +//It downsamples the map mith the given factor (0 < factor < 1) so the map gets reduced and calculationtime gets better. +//If it is set to 1 the map will have original size, if it is 0 the algorithm won't work, so make sure to not set it to 0. +//The algorithm also needs the Robot radius [m] and the map resolution [m²/pixel] to calculate the needed +//amount of erosions to include the radius in the planning. +// + +class AStarPlanner +{ +protected: + int n; + int m; + + std::string route_; + + std::string pathFind(const int& xStart, const int& yStart, const int& xFinish, const int& yFinish, const cv::Mat& map); + +public: + AStarPlanner(); + + void drawRoute(cv::Mat& map, const cv::Point start_point, const std::string& route, double step_length); + + void getRoute(const cv::Point start_point, const std::string& route, double step_length, std::vector& route_points); + + // computes the path length between start point and end point + double planPath(const cv::Mat& map, const cv::Point& start_point, const cv::Point& end_point, + const double downsampling_factor, const double robot_radius, const double map_resolution, + const int end_point_valid_neighborhood_radius=0, std::vector* route=NULL); + + // computes the path length between start point and end point, tries first with a downsampled map for fast computation and uses the original map if the first try was not successful + // if end_point_valid_neighborhood_radius [measured in cell size of downsampled_map] is set greater than 0, then it is sufficient to find a path to a cell within that neighborhood radius to end_point for a success + double planPath(const cv::Mat& map, const cv::Mat& downsampled_map, const cv::Point& start_point, const cv::Point& end_point, const double downsampling_factor, + const double robot_radius, const double map_resolution, const int end_point_valid_neighborhood_radius=0, cv::Mat* draw_path_map=NULL, + std::vector* route=NULL); + + void downsampleMap(const cv::Mat& map, cv::Mat& downsampled_map, const double downsampling_factor, const double robot_radius, const double map_resolution); +}; diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/common/include/ipa_building_navigation/concorde_TSP.h b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/common/include/ipa_building_navigation/concorde_TSP.h new file mode 100644 index 0000000..2bb1336 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/common/include/ipa_building_navigation/concorde_TSP.h @@ -0,0 +1,92 @@ +#include "ros/ros.h" +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include +#include + +#pragma once //make sure this header gets included only one time when multiple classes need it in the same project +//regarding to https://en.wikipedia.org/wiki/Pragma_once this is more efficient than #define + +//This class applies an object to solve a given TSP problem using the concorde TSP solver. This solver can be downloaded at: +// http://www.math.uwaterloo.ca/tsp/concorde.html +//A short explanation on how to build the solver is given at: +// http://www.math.uwaterloo.ca/tsp/concorde/DOC/README.html +//If you have build the solver navigate to the./TSP folder and type " ./concorde -h " to see how to use this solver. This class +//uses the concorde solver by using a systemcall. +//Make sure you have a "/common/files/TSP_order.txt" and a "/common/files/TSPlib_file.txt" file. These files are used to tell +//concorde the current problem and save the output of it. +// +//It needs a symmetrical matrix of pathlenghts between the nodes and the starting-point index in this matrix. +//If the path from one node to another doesn't exist or the path is from one node to itself, the entry in the matrix must +//be 0 or smaller. so the format for this matrix is: +// row: node to start from, column: node to go to +// --- --- +// | 0.0 1.0 3.5 5.8 1.2 | +// | 1.0 0.0 2.4 3.3 9.0 | +// | 3.5 2.4 0.0 7.7 88.0| +// | 5.8 3.3 7.7 0.0 0.0 | +// | 1.2 9.0 88.0 0.0 0.0 | +// --- --- + +class ConcordeTSPSolver +{ +protected: + + //Astar pathplanner to find the pathlengths from cv::Point to cv::Point + AStarPlanner pathplanner_; + + //Function to create neccessary TSPlib file to tell concorde what the problem is. + void writeToFile(const cv::Mat& pathlength_matrix, const std::string& tsp_lib_filename, const std::string& tsp_order_filename); + + //Function to read the saved TSP order. + std::vector readFromFile(const std::string& tsp_order_filename); + + void distance_matrix_thread(DistanceMatrix& distance_matrix_computation, cv::Mat& distance_matrix, + const cv::Mat& original_map, const std::vector& points, double downsampling_factor, + double robot_radius, double map_resolution, AStarPlanner& path_planner); + + bool abort_computation_; + std::string unique_file_identifier_; + +public: + //Constructor + ConcordeTSPSolver(); + + void abortComputation(); + + //Functions to solve the TSP. It needs a distance matrix, that shows the pathlengths between two nodes of the problem. + //This matrix has to be symmetrical or else the TSPlib must be changed. The int shows the index in the Matrix. + //There are two functions for different cases: + // 1. The distance matrix already exists + // 2. The distance matrix has to be computed and maybe returned + + //with given distance matrix + std::vector solveConcordeTSP(const cv::Mat& path_length_Matrix, const int start_Node); + + // compute distance matrix and maybe return it + // this version does not exclude infinite paths from the TSP ordering + std::vector solveConcordeTSP(const cv::Mat& original_map, const std::vector& points, double downsampling_factor, + double robot_radius, double map_resolution, const int start_Node, cv::Mat* distance_matrix = 0); + + // compute TSP from a cleaned distance matrix (does not contain any infinity paths) that has to be computed + std::vector solveConcordeTSPClean(const cv::Mat& original_map, const std::vector& points, + double downsampling_factor, double robot_radius, double map_resolution, const int start_node); + + // compute TSP with pre-computed cleaned distance matrix (does not contain any infinity paths) + std::vector solveConcordeTSPWithCleanedDistanceMatrix(const cv::Mat& distance_matrix, + const std::map& cleaned_index_to_original_index_mapping, const int start_node); +}; diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/common/include/ipa_building_navigation/contains.h b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/common/include/ipa_building_navigation/contains.h new file mode 100644 index 0000000..ab75a53 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/common/include/ipa_building_navigation/contains.h @@ -0,0 +1,24 @@ +#include "ros/ros.h" +#include +#include +#include +#include +#include +#include + +#pragma once + +//This function searches for the given element in the given vector and returns true if it is in it or false if not +template +bool inline contains(std::vector vector, T element) +{ + //this functions checks, if the given element is in the given vector (in this case for int elements) + if (!vector.empty()) + { + return vector.end() != std::find(vector.begin(), vector.end(), element); + } + else + { + return false; + } +} diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/common/include/ipa_building_navigation/distance_matrix.h b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/common/include/ipa_building_navigation/distance_matrix.h new file mode 100644 index 0000000..29b79f8 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/common/include/ipa_building_navigation/distance_matrix.h @@ -0,0 +1,261 @@ + + +#pragma once + +#include +#include +#include + +#include + +class DistanceMatrix +{ +protected: + + bool abort_computation_; + +public: + + DistanceMatrix() + : abort_computation_(false) + { + } + + void abortComputation() + { + abort_computation_ = true; + } + + //Function to construct the symmetrical distance matrix from the given points. The rows show from which node to start and + //the columns to which node to go. If the path between nodes doesn't exist or the node to go to is the same as the one to + //start from, the entry of the matrix is 0. + // REMARK: paths is a pointer that points to a 3D vector that has dimensionality NxN in the outer vectors to store + // the paths in a matrix manner + void constructDistanceMatrix(cv::Mat& distance_matrix, const cv::Mat& original_map, const std::vector& points, + double downsampling_factor, double robot_radius, double map_resolution, AStarPlanner& path_planner, + std::vector > >* paths=NULL) + { + std::cout << "DistanceMatrix::constructDistanceMatrix: Constructing distance matrix..." << std::endl; + Timer tim; + + //create the distance matrix with the right size + distance_matrix.create((int)points.size(), (int)points.size(), CV_64F); + + // hack: speed up trick + if (points.size()>500) + downsampling_factor *= 0.5; + + // reduce image size already here to avoid resizing in the planner each time + const double one_by_downsampling_factor = 1./downsampling_factor; + cv::Mat downsampled_map; + path_planner.downsampleMap(original_map, downsampled_map, downsampling_factor, robot_radius, map_resolution); + + if (points.size()>500) + std::cout << "0 10 20 30 40 50 60 70 80 90 100" << std::endl; +// int a=0, b=0; + for (int i = 0; i < points.size(); i++) + { + //std::cout << " a=" << a << " b=" << b << std::endl; + if (points.size()>500 && i%(std::max(1,(int)points.size()/100))==0) + std::cout << "." << std::flush; + //cv::Point current_center = downsampling_factor * points[i]; + for (int j = 0; j < points.size(); j++) + { + if (j != i) + { + if (j > i) //only compute upper right triangle of matrix, rest is symmetrically added + { + if (abort_computation_==true) + return; + + // try first with direct connecting line (often sufficient and a significant speedup over A*) + cv::LineIterator it(original_map, points[i], points[j]); + bool direct_connection = true; + for (int k=0; k(i, j) = length; + distance_matrix.at(j, i) = length; //symmetrical-Matrix --> saves half the computation time + if (paths!=NULL) + { + // store path + cv::LineIterator it2(original_map, points[i], points[j]); + std::vector current_path(it2.count); + for (int k=0; kat(i).at(j) = current_path; + paths->at(j).at(i) = current_path; + } +// ++a; + } + else + { + // A* path planner + if(paths!=NULL) + { + std::vector current_path; + double length = path_planner.planPath(original_map, downsampled_map, points[i], points[j], downsampling_factor, 0., map_resolution, 0, NULL, ¤t_path); + distance_matrix.at(i, j) = length; + distance_matrix.at(j, i) = length; //symmetrical-Matrix --> saves half the computation time + + // remap path points to original map size + for(std::vector::iterator point=current_path.begin(); point!=current_path.end(); ++point) + { + point->x = point->x/downsampling_factor; + point->y = point->y/downsampling_factor; + } + + paths->at(i).at(j) = current_path; + paths->at(j).at(i) = current_path; + } + else + { + double length = path_planner.planPath(original_map, downsampled_map, points[i], points[j], downsampling_factor, 0., map_resolution); + distance_matrix.at(i, j) = length; + distance_matrix.at(j, i) = length; //symmetrical-Matrix --> saves half the computation time + + } +// ++b; + } + } + } + else + { + distance_matrix.at(i, j) = 0; + } + } + } + + std::cout << "\nDistance matrix created in " << tim.getElapsedTimeInMilliSec() << " ms" << std::endl;// "\nDistance matrix:\n" << distance_matrix << std::endl; + } + + // check whether distance matrix contains infinite path lengths and if this is true, create a new distance matrix with maximum size clique of reachable points + // cleaned_index_to_original_index_mapping --> maps the indices of the cleaned distance_matrix to the original indices of the original distance_matrix + void cleanDistanceMatrix(const cv::Mat& distance_matrix, cv::Mat& distance_matrix_cleaned, std::map& cleaned_index_to_original_index_mapping) + { + // standard: use a 1:1 mapping (input = output) + cleaned_index_to_original_index_mapping.clear(); + for (int i=0; i remove_entry(distance_matrix.rows, false); // keeps track on which entries of distance_matrix need to be removed + + // loop until all lines are marked, which need to be removed + cv::Mat distance_matrix_temp = distance_matrix.clone(); + while (true) + { + // count infinite entries in each row of distance matrix + std::vector infinite_length_entries(distance_matrix_temp.rows, 0); + for (int i=0; i(i,j)>max_length) + infinite_length_entries[i]++; + + // sort rows by their number of infinite entries + std::multimap number_infinite_entries_to_row_index_mapping; // maps number of infinite entries to the corresponding row index + for (size_t i=0; i(infinite_length_entries[i], (int)i)); + + // if any row has at least one infinite entry, mark the row with most infinity entries for deletion + bool mark_line = false; + int mark_index = -1; + std::multimap::reverse_iterator number_infinite_entries_to_row_index_mapping_last = number_infinite_entries_to_row_index_mapping.rbegin(); + if (number_infinite_entries_to_row_index_mapping_last->first > 0) + { + mark_line = true; + mark_index = number_infinite_entries_to_row_index_mapping_last->second; + remove_entry[mark_index] = true; + } + if (mark_line == true) + { + for (int j=0; j(mark_index, j) = -1.; + for (int i=0; i(i, mark_index) = -1.; + } + else + break; + } + + // count entries to remove + int number_entries_to_be_removed = 0; + for (size_t i=0; i 0) + { + std::cout << " DistanceMatrix::cleanDistanceMatrix: Need to remove " << number_entries_to_be_removed << " elements out of " << distance_matrix.rows << " elements from the distance matrix." << std::endl; + + // setup new distance_matrix + const int new_size = distance_matrix.rows - number_entries_to_be_removed; + if (new_size == 0) + { + std::cout << " DistanceMatrix::cleanDistanceMatrix: Warning: Would need to remove all elements of distance_matrix. Aborting." << std::endl; + return; + } + distance_matrix_cleaned.create(new_size, new_size, CV_64F); + cleaned_index_to_original_index_mapping.clear(); + + // fill new distance_matrix + int new_index = 0; + for (size_t i=0; i(new_index, new_j) = distance_matrix.at(i,j); + new_j++; + } + } + new_index++; + } + } + if (new_index != new_size) + std::cout << "##################################################\nDistanceMatrix::cleanDistanceMatrix: Warning: new_index != new_size.\n##################################################" << std::endl; + } + } + + // calculate the distance matrix and check whether distance matrix contains infinite path lengths and if this is true, + // create a new distance matrix with maximum size clique of reachable points + // start_node --> provide the original start node to the function, it writes the new start node mapped to the new coordinates into it + // cleaned_index_to_original_index_mapping --> maps the indices of the cleaned distance_matrix to the original indices of the original distance_matrix + void computeCleanedDistanceMatrix(const cv::Mat& original_map, const std::vector& points, + double downsampling_factor, double robot_radius, double map_resolution, AStarPlanner& path_planner, + cv::Mat& distance_matrix, std::map& cleaned_index_to_original_index_mapping, int& start_node) + { + std::cout << "DistanceMatrix::computeCleanedDistanceMatrix: Constructing distance matrix..." << std::endl; + // calculate the distance matrix + cv::Mat distance_matrix_raw; + constructDistanceMatrix(distance_matrix_raw, original_map, points, downsampling_factor, robot_radius, map_resolution, path_planner); + + // check whether distance matrix contains infinite path lengths and if this is true, create a new distance matrix with maximum size clique of reachable points + cleanDistanceMatrix(distance_matrix_raw, distance_matrix, cleaned_index_to_original_index_mapping); + + // re-assign the start node to cleaned indices (use 0 if the original start node was removed from distance_matrix_cleaned) + int new_start_node = 0; + for (std::map::iterator it=cleaned_index_to_original_index_mapping.begin(); it!=cleaned_index_to_original_index_mapping.end(); ++it) + if (it->second == start_node) + new_start_node = it->first; + start_node = new_start_node; + } +}; diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/common/include/ipa_building_navigation/genetic_TSP.h b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/common/include/ipa_building_navigation/genetic_TSP.h new file mode 100644 index 0000000..e2a24e2 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/common/include/ipa_building_navigation/genetic_TSP.h @@ -0,0 +1,91 @@ +#include "ros/ros.h" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +#pragma once //make sure this header gets included only one time when multiple classes need it in the same project + //regarding to https://en.wikipedia.org/wiki/Pragma_once this is more efficient than #define + +//This class provides a solution for the TSP by taking the nearest-neighbor path and applying a genetic algorithm on it. +//It needs a symmetrical matrix of pathlenghts between the nodes and the starting-point index in this matrix. +//If the path from one node to another doesn't exist or the path is from one node to itself, the entry in the matrix must +//be 0 or smaller. so the format for this matrix is: +// row: node to start from, column: node to go to +// --- --- +// | 0.0 1.0 3.5 5.8 1.2 | +// | 1.0 0.0 2.4 3.3 9.0 | +// | 3.5 2.4 0.0 7.7 88.0| +// | 5.8 3.3 7.7 0.0 0.0 | +// | 1.2 9.0 88.0 0.0 0.0 | +// --- --- + +class GeneticTSPSolver +{ +protected: + + //Astar pathplanner to find the pathlengths from cv::Point to cv::Point + AStarPlanner pathplanner_; + + //function to get the length of a given path + double getPathLength(const cv::Mat& path_length_Matrix, std::vector given_path); + + //function to mutate (randomly change) a given Parent-path + std::vector mutatePath(const std::vector& parent_path); + + //function that selects the best path from the given paths + std::vector getBestPath(const std::vector > paths, const cv::Mat& pathlength_Matrix, bool& changed); + + void distance_matrix_thread(DistanceMatrix& distance_matrix_computation, cv::Mat& distance_matrix, + const cv::Mat& original_map, const std::vector& points, double downsampling_factor, + double robot_radius, double map_resolution, AStarPlanner& path_planner); + + bool abort_computation_; + + //indicates how many generations shall be at least taken in order to optimize the given path + int min_number_of_generations_; + + //indicates the amount of generations for which the path shouldn't have changed in order to stop the + //optimization + int const_generations_number_; + +public: + //constructor + GeneticTSPSolver(int min_number_of_gens=2300, int const_generations=100); + + void abortComputation(); + + //Solving-algorithms for the given TSP. It returns a vector of int, which is the order from this solution. The int shows + //the index in the Matrix. There are two functions for different cases: + // 1. The distance matrix already exists + // 2. The distance matrix has to be computet and maybe returned + + //with given distance matrix + std::vector solveGeneticTSP(const cv::Mat& path_length_Matrix, const int start_Node); + + + // compute distance matrix and maybe returning it + // this version does not exclude infinite paths from the TSP ordering + std::vector solveGeneticTSP(const cv::Mat& original_map, const std::vector& points, double downsampling_factor, + double robot_radius, double map_resolution, const int start_Node, cv::Mat* distance_matrix=0); + + // compute TSP from a cleaned distance matrix (does not contain any infinity paths) that has to be computed + std::vector solveGeneticTSPClean(const cv::Mat& original_map, const std::vector& points, + double downsampling_factor, double robot_radius, double map_resolution, const int start_node); + + // compute TSP with pre-computed cleaned distance matrix (does not contain any infinity paths) + std::vector solveGeneticTSPWithCleanedDistanceMatrix(const cv::Mat& distance_matrix, + const std::map& cleaned_index_to_original_index_mapping, const int start_node); +}; diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/common/include/ipa_building_navigation/maximal_clique_finder.h b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/common/include/ipa_building_navigation/maximal_clique_finder.h new file mode 100644 index 0000000..d1fcff3 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/common/include/ipa_building_navigation/maximal_clique_finder.h @@ -0,0 +1,69 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include + +#include +#include +#include + +#pragma once //make sure this header gets included only one time when multiple classes need it in the same project + //regarding to https://en.wikipedia.org/wiki/Pragma_once this is more efficient than #define + +//This algorithm provides a class that finds all maximal cliques in a given graph. It uses the Bron-Kerbosch Implementation +//in Boost to do this. As input a symmetrical distance-Matrix is needed that shows the pathlenghts from one node to another. +//If the path from one node to another doesn't exist, the entry in the matrix must be 0 or smaller. so the format for this +//Matrix is: +// row: node to start from, column: node to go to +// --- --- +// | 0.0 1.0 3.5 5.8 1.2 | +// | 1.0 0.0 2.4 3.3 9.0 | +// | 3.5 2.4 0.0 7.7 88.0| +// | 5.8 3.3 7.7 0.0 0.0 | +// | 1.2 9.0 88.0 0.0 0.0 | +// --- --- +//It also needs a maximal pathlength that shows the algorithm when the paths are too long and should not be included in the +//Graph. This is neccessary because we assume that you can reach every node from the others and with a Graph in which all +//nodes are connected the only maximal clique are the nodes itself. For example the previous matrix gets +// maxval = 7.0 +// --- --- +// | 0.0 1.0 3.5 5.8 1.2 | +// | 1.0 0.0 2.4 3.3 0.0 | +// | 3.5 2.4 0.0 0.0 0.0 | +// | 5.8 3.3 0.0 0.0 0.0 | +// | 1.2 0.0 0.0 0.0 0.0 | +// --- --- +// +//The nodes in the graph are named after their position in the distance-Matrix and the cliques are +// std::vector variables so you can easily acces the right nodes in the matrix outside this class. + +class cliqueFinder +{ +protected: + //function that allows to add a Vertex with a name to a boost::Graph. See boosts helper.hpp ( http://www.boost.org/doc/libs/1_47_0/libs/graph/example/helper.hpp ) for explanation + template + typename boost::graph_traits::vertex_descriptor addNamedVertex(Graph& g, NameMap nm, const std::string& name, VertexMap& vm); + + //function to create a graph out of the given distanceMatrix + template + void createGraph(Graph& graph, cv::Mat& distance_Matrix); + + //function to set too long paths in the distance_matrix to zero + void cutTooLongEdges(cv::Mat& complete_distance_matrix, double maxval); + +public: + cliqueFinder(); + + std::vector > getCliques(const cv::Mat& distance_matrix, double maxval); +}; diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/common/include/ipa_building_navigation/nearest_neighbor_TSP.h b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/common/include/ipa_building_navigation/nearest_neighbor_TSP.h new file mode 100644 index 0000000..45ff13a --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/common/include/ipa_building_navigation/nearest_neighbor_TSP.h @@ -0,0 +1,69 @@ +#include "ros/ros.h" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +#pragma once //make sure this header gets included only one time when multiple classes need it in the same project + //regarding to https://en.wikipedia.org/wiki/Pragma_once this is more efficient than #define + +//This class provides a solution for the TSP by taking the nearest neighbor from the current Point as next Point. +//It needs a symmetrical matrix of pathlenghts between the nodes and the starting-point index in this matrix. +//If the path from one node to another doesn't exist or the path is from one node to itself, the entry in the matrix must +//be 0 or smaller. so the format for this matrix is: +// row: node to start from, column: node to go to +// --- --- +// | 0.0 1.0 3.5 5.8 1.2 | +// | 1.0 0.0 2.4 3.3 9.0 | +// | 3.5 2.4 0.0 7.7 88.0| +// | 5.8 3.3 7.7 0.0 0.0 | +// | 1.2 9.0 88.0 0.0 0.0 | +// --- --- + +class NearestNeighborTSPSolver +{ +protected: + + //Astar pathplanner to find the pathlengths from cv::Point to cv::Point + AStarPlanner pathplanner_; + +// //Function to construct the distance matrix, showing the pathlength from node to node +// void NearestNeighborTSPSolver::constructDistanceMatrix(cv::Mat& distance_matrix, const cv::Mat& original_map, +// const std::vector& points, double downsampling_factor, double robot_radius, double map_resolution); + +public: + //constructor + NearestNeighborTSPSolver(); + + //Solving-algorithms for the given TSP. It returns a vector of int, which is the order from this solution. The int shows + //the index in the Matrix. There are two functions for different cases: + // 1. The distance matrix already exists + // 2. The distance matrix has to be computed and maybe returned + + //with given distance matrix + std::vector solveNearestTSP(const cv::Mat& path_length_matrix, const int start_node); //with given distance matrix + + // compute TSP and distance matrix without cleaning it + // this version does not exclude infinite paths from the TSP ordering + std::vector solveNearestTSP(const cv::Mat& original_map, const std::vector& points, double downsampling_factor, + double robot_radius, double map_resolution, const int start_node, cv::Mat* distance_matrix=0); + + // compute TSP from a cleaned distance matrix (does not contain any infinity paths) that has to be computed + std::vector solveNearestTSPClean(const cv::Mat& original_map, const std::vector& points, + double downsampling_factor, double robot_radius, double map_resolution, const int start_node); + + // compute TSP with pre-computed cleaned distance matrix (does not contain any infinity paths) + std::vector solveNearestTSPWithCleanedDistanceMatrix(const cv::Mat& distance_matrix, + const std::map& cleaned_index_to_original_index_mapping, const int start_node); +}; diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/common/include/ipa_building_navigation/node.h b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/common/include/ipa_building_navigation/node.h new file mode 100644 index 0000000..181b53b --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/common/include/ipa_building_navigation/node.h @@ -0,0 +1,40 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +//This is th object to reperesent a node in a Graph. It is used by A_star_pathplanner.cpp and was applied from: +// http://code.activestate.com/recipes/577457-a-star-shortest-path-algorithm/ +//In the estimate() function the distance from the node to the goal is calculated. Uncomment there which distance calculation +//you want to use (Euclidean, Manhattan or Chebyshev). Euclidean is more precisly but could slow the planner a little bit on +//long paths. + +#pragma once //make sure this header gets included only one time when multiple classes need it in the same project + //regarding to https://en.wikipedia.org/wiki/Pragma_once this is more efficient than #define + +class NodeAstar +{ +protected: + // current position + int xPos_; + int yPos_; + // total distance already travelled to reach the node + int level_; + // priority=level+remaining distance estimate + int priority_; // smaller: higher priority + +public: + NodeAstar(int xp, int yp, int d, int p); + int getxPos() const; + int getyPos() const; + int getLevel() const; + int getPriority() const; + void updatePriority(const int& xDest, const int& yDest); + void nextLevel(const int& i); // i: direction + const int& estimate(const int& xDest, const int& yDest) const; + +}; diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/common/include/ipa_building_navigation/set_cover_solver.h b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/common/include/ipa_building_navigation/set_cover_solver.h new file mode 100644 index 0000000..aaf1ca3 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/common/include/ipa_building_navigation/set_cover_solver.h @@ -0,0 +1,72 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include +#include +#include +#include + +#pragma once //make sure this header gets included only one time when multiple classes need it in the same project +//regarding to https://en.wikipedia.org/wiki/Pragma_once this is more efficient than #define + +//This algorithm provides a class to solve the set-cover problem for given cliques. This is done by using the greedy-search +//algorithm, which takes the clique with most unvisited nodes before the other nodes and removes the nodes in it from the +//unvisited. It repeats this step until no more node hasn't been visited. It then merges cliques together that have at least +//one node in common. +// +//!!!!!!!!!!!!!!!!Important!!!!!!!!!!!!!!!!! +//Make sure that the cliques cover all nodes in the graph or else this algorithm runs into an endless loop. For best results +//take the cliques from a maximal-clique finder like the Bron-Kerbosch algorithm. + +class SetCoverSolver +{ +protected: + + //Astar pathplanner to find the pathlengths from cv::Point to cv::Point + AStarPlanner pathplanner_; + + //This object finds all maximal cliques in the given Graph. It needs a symmetrical distance matrix shwoing the pathlength + //from one node to another and a miximal pathlength to cut edges that are larger than this value. See maximal_clique_finder.h + //for further information. + cliqueFinder maximal_clique_finder; + + //function to merge groups together, which have at least one node in common + std::vector > mergeGroups(const std::vector >& found_groups); + +// //Function to construct the distance matrix, showing the pathlength from node to node +// void constructDistanceMatrix(cv::Mat& distance_matrix, const cv::Mat& original_map, const std::vector& points, +// double downsampling_factor, double robot_radius, double map_resolution); + +public: + //Constructor + SetCoverSolver(); + + //algorithms to solve the set cover problem. There are three functions for different cases: + // 1. The cliques already have been found + // 2. The distance matrix already exists + // 3. The distance matrix has to be computed and may be returned + + //cliques are given + std::vector > solveSetCover(std::vector >& given_cliques, const int number_of_nodes, + const int max_number_of_clique_members, const cv::Mat& distance_matrix); + + //the distance matrix is given + std::vector > solveSetCover(const cv::Mat& distance_matrix, const std::vector& points, + const int number_of_nodes, double maximal_pathlength, const int max_number_of_clique_members); + + //the distance matrix has to be computed and may be returned + std::vector > solveSetCover(const cv::Mat& original_map, const std::vector& points, + double downsampling_factor, double robot_radius, double map_resolution, double maximal_pathlength, const int max_number_of_clique_members, cv::Mat* distance_matrix=0); +}; diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/common/include/ipa_building_navigation/timer.h b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/common/include/ipa_building_navigation/timer.h new file mode 100644 index 0000000..552d5f5 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/common/include/ipa_building_navigation/timer.h @@ -0,0 +1,183 @@ +/* + * timer.h + * + * Created on: May 13, 2013 + * Author: rmb-ce + */ + +// from: http://snipplr.com/view/40650/timer-class-for-both-unixlinuxmac-and-windows-system/ + +////////////////// +// How to Use //// +////////////////// + +//#include +//#include "timer.h" +//using namespace std; + +//int main() +//{ +// Timer timer; +// +// // start timer +// timer.start(); +// +// // do something +// ... +// +// // stop timer +// timer.stop(); +// +// // print the elapsed time in millisec +// cout << timer.getElapsedTimeInMilliSec() << " ms.\n"; +// +// return 0; +//} + + +////////////////////////////////////////////////////////////////////////////// +// Timer.h +// ======= +// High Resolution Timer. +// This timer is able to measure the elapsed time with 1 micro-second accuracy +// in both Windows, Linux and Unix system +// +// AUTHOR: Song Ho Ahn (song.ahn@gmail.com) +// CREATED: 2003-01-13 +// UPDATED: 2006-01-13 +// +// Copyright (c) 2003 Song Ho Ahn +////////////////////////////////////////////////////////////////////////////// + +#ifndef TIMER_H_DEF +#define TIMER_H_DEF + +#ifdef WIN32 // Windows system specific +//#include +#else // Unix based system specific +#include +#endif + +#include + + +class Timer +{ +public: +// default constructor + Timer() + { +#ifdef WIN32 + QueryPerformanceFrequency(&frequency); + startCount.QuadPart = 0; + endCount.QuadPart = 0; +#else + startCount.tv_sec = startCount.tv_usec = 0; + endCount.tv_sec = endCount.tv_usec = 0; +#endif + + stopped = 0; + startTimeInMicroSec = 0; + endTimeInMicroSec = 0; + + start(); + } + + // default destructor + ~Timer() + { + } + + /////////////////////////////////////////////////////////////////////////////// + // start timer. + // startCount will be set at this point. + /////////////////////////////////////////////////////////////////////////////// + void start() + { + stopped = 0; // reset stop flag +#ifdef WIN32 + QueryPerformanceCounter(&startCount); +#else + gettimeofday(&startCount, NULL); +#endif + } + + /////////////////////////////////////////////////////////////////////////////// + // stop the timer. + // endCount will be set at this point. + /////////////////////////////////////////////////////////////////////////////// + void stop() + { + stopped = 1; // set timer stopped flag + +#ifdef WIN32 + QueryPerformanceCounter(&endCount); +#else + gettimeofday(&endCount, NULL); +#endif + } + + /////////////////////////////////////////////////////////////////////////////// + // same as getElapsedTimeInSec() + /////////////////////////////////////////////////////////////////////////////// + double getElapsedTime() + { + return this->getElapsedTimeInSec(); + } + + /////////////////////////////////////////////////////////////////////////////// + // divide elapsedTimeInMicroSec by 1000000 + /////////////////////////////////////////////////////////////////////////////// + double getElapsedTimeInSec() + { + return this->getElapsedTimeInMicroSec() * 0.000001; + } + + /////////////////////////////////////////////////////////////////////////////// + // divide elapsedTimeInMicroSec by 1000 + /////////////////////////////////////////////////////////////////////////////// + double getElapsedTimeInMilliSec() + { + return this->getElapsedTimeInMicroSec() * 0.001; + } + + /////////////////////////////////////////////////////////////////////////////// + // compute elapsed time in micro-second resolution. + // other getElapsedTime will call this first, then convert to correspond resolution. + /////////////////////////////////////////////////////////////////////////////// + double getElapsedTimeInMicroSec() + { +#ifdef WIN32 + if(!stopped) + QueryPerformanceCounter(&endCount); + + startTimeInMicroSec = startCount.QuadPart * (1000000.0 / frequency.QuadPart); + endTimeInMicroSec = endCount.QuadPart * (1000000.0 / frequency.QuadPart); +#else + if (!stopped) + gettimeofday(&endCount, NULL); + + startTimeInMicroSec = (startCount.tv_sec * 1000000.0) + startCount.tv_usec; + endTimeInMicroSec = (endCount.tv_sec * 1000000.0) + endCount.tv_usec; +#endif + + return endTimeInMicroSec - startTimeInMicroSec; + } + +protected: + +private: + double startTimeInMicroSec; // starting time in micro-second + double endTimeInMicroSec; // ending time in micro-second + int stopped; // stop flag +#ifdef WIN32 + LARGE_INTEGER frequency; // ticks per second + LARGE_INTEGER startCount;// + LARGE_INTEGER endCount;// +#else + timeval startCount; // + timeval endCount; // +#endif +}; + +#endif // TIMER_H_DEF diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/common/include/ipa_building_navigation/trolley_position_finder.h b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/common/include/ipa_building_navigation/trolley_position_finder.h new file mode 100644 index 0000000..2ab9f91 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/common/include/ipa_building_navigation/trolley_position_finder.h @@ -0,0 +1,58 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include + +#include + +#pragma once //make sure this header gets included only one time when multiple classes need it in the same project + //regarding to https://en.wikipedia.org/wiki/Pragma_once this is more efficient than #define + +//This object provides an algorithm to search for the best trolley position for a group of points. A trolley position is +//the Point where the trolley of a robot should be placed during the cleaning of the group. This is done by searching in the +//bounding box of these points for the point which minimizes the pathlength to every group member. If the goup has only two +//members the algorithm chooses a Point on the optimal path between these two Points that is in the middlest of this path. +//This algorithm needs as input: +// 1. The original occupancy gridmap to get the pathlength between two Points. +// 2. A vector of found groups. This vector stores the group as integer that show the Position of the node in the +// roomcenters vector. To find the groups it is good to find all maximal cliques in the graph and then applying +// a set Cover solver on these. +// 3. A vector of roomcenters that stores the centers of each room. This algorithm was implemented for planning +// the order of cleaning rooms so this variable is called room_centers, but it can store every cv::Point with that +// you want to find the best trolley position. +// 4. A downsampling factor to reduce the size of the map. This is used by the A_star_pathplanner to heavily reduce +// calculationtime. It has to be (0, 1]. If it is 1 the map will be took as it is. +// 5. The Radius of the robot and the map resolution to make sure the A_star pathplanner stays in enough distance to the +// walls and obstacles. (See A_star_pathplanner.cpp for further information) + +class TrolleyPositionFinder +{ +protected: + + AStarPlanner path_planner_; //Object to plan a path from Point A to Point B in a given gridmap + + //Function to find a trolley position for one group + cv::Point findOneTrolleyPosition(const std::vector group_points, const cv::Mat& original_map, + const double downsampling_factor, const double robot_radius, const double map_resolution); + +public: + + //constructor + TrolleyPositionFinder(); + + //Function to find a trolley position for each group by using the findOneTrolleyPosition function + std::vector findTrolleyPositions(const cv::Mat& original_map, const std::vector >& found_groups, + const std::vector& room_centers, const double downsampling_factor, const double robot_radius, + const double map_resolution); +}; diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/common/include/ipa_building_navigation/tsp_solver_defines.h b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/common/include/ipa_building_navigation/tsp_solver_defines.h new file mode 100644 index 0000000..4ce6a45 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/common/include/ipa_building_navigation/tsp_solver_defines.h @@ -0,0 +1,63 @@ +/*! + ***************************************************************** + * \file + * + * \note + * Copyright (c) 2017 \n + * Fraunhofer Institute for Manufacturing Engineering + * and Automation (IPA) \n\n + * + ***************************************************************** + * + * \note + * Project name: Care-O-bot + * \note + * ROS stack name: autopnp + * \note + * ROS package name: ipa_building_navigation + * + * \author + * Author: Richard Bormann + * \author + * Supervised by: Richard Bormann + * + * \date Date of creation: 03.2017 + * + * \brief + * + * + ***************************************************************** + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * - Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. \n + * - Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. \n + * - Neither the name of the Fraunhofer Institute for Manufacturing + * Engineering and Automation (IPA) nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. \n + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License LGPL as + * published by the Free Software Foundation, either version 3 of the + * License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License LGPL for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License LGPL along with this program. + * If not, see . + * + ****************************************************************/ + +#pragma once + + +enum TSPSolvers {TSP_NEAREST_NEIGHBOR=1, TSP_GENETIC=2, TSP_CONCORDE=3}; diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/common/include/ipa_building_navigation/tsp_solvers.h b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/common/include/ipa_building_navigation/tsp_solvers.h new file mode 100644 index 0000000..5bc5d1b --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/common/include/ipa_building_navigation/tsp_solvers.h @@ -0,0 +1,67 @@ +/*! + ***************************************************************** + * \file + * + * \note + * Copyright (c) 2017 \n + * Fraunhofer Institute for Manufacturing Engineering + * and Automation (IPA) \n\n + * + ***************************************************************** + * + * \note + * Project name: Care-O-bot + * \note + * ROS stack name: autopnp + * \note + * ROS package name: ipa_building_navigation + * + * \author + * Author: Richard Bormann + * \author + * Supervised by: Richard Bormann + * + * \date Date of creation: 03.2017 + * + * \brief + * + * + ***************************************************************** + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * - Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. \n + * - Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. \n + * - Neither the name of the Fraunhofer Institute for Manufacturing + * Engineering and Automation (IPA) nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. \n + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License LGPL as + * published by the Free Software Foundation, either version 3 of the + * License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License LGPL for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License LGPL along with this program. + * If not, see . + * + ****************************************************************/ + +#pragma once + +#include + + +#include +#include +#include diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/common/src/A_star_pathplanner.cpp b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/common/src/A_star_pathplanner.cpp new file mode 100644 index 0000000..067c2a4 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/common/src/A_star_pathplanner.cpp @@ -0,0 +1,383 @@ +#include + +#include + +const int dir = 8; // number of possible directions to go at any position +// if dir==4 +//static int dx[dir]={1, 0, -1, 0}; +//static int dy[dir]={0, 1, 0, -1}; +// if dir==8 +static int dx[dir] = +{ 1, 1, 0, -1, -1, -1, 0, 1 }; +static int dy[dir] = +{ 0, 1, 1, 1, 0, -1, -1, -1 }; + +static int expanding_counter = 0; + +// Determine priority (in the priority queue) +bool operator<(const NodeAstar& a, const NodeAstar& b) +{ + return a.getPriority() > b.getPriority(); +} + +AStarPlanner::AStarPlanner() +{ + n = 1; + m = 1; +} + +void AStarPlanner::drawRoute(cv::Mat& map, const cv::Point start_point, const std::string& route, double step_length) +{ + // follow the route on the map and update the path length + if (route.length() > 0) + { + int j; + char c; + int x1 = start_point.x; + int y1 = start_point.y; + int x2,y2; + for (int i = 0; i < route.length(); i++) + { + //get the next char of the string and make it an integer, which shows the direction + c = route.at(i); + j=c-'0'; + x2 = x1 + dx[j]*step_length; + y2 = y1 + dy[j]*step_length; + const double progress = 0.2 + 0.6*(double)i/(double)route.length(); + cv::line(map, cv::Point(x1,y1), cv::Point(x2,y2), CV_RGB(0,progress*255,0), 1); + x1 = x2; + y1 = y2; + } + } +} + +void AStarPlanner::getRoute(const cv::Point start_point, const std::string& route, + double step_length, std::vector& route_points) +{ + // follow the route on the map and update the path length + if (route.length() > 0) + { + int j; + char c; + int x1 = start_point.x; + int y1 = start_point.y; + route_points.push_back(cv::Point(x1, y1)); + int x2,y2; + for (int i = 0; i < route.length(); i++) + { + //get the next char of the string and make it an integer, which shows the direction + c = route.at(i); + j=c-'0'; + x2 = x1 + dx[j]*step_length; + y2 = y1 + dy[j]*step_length; + route_points.push_back(cv::Point(x2, y2)); + x1 = x2; + y1 = y2; + } + } +} + +void AStarPlanner::downsampleMap(const cv::Mat& map, cv::Mat& downsampled_map, const double downsampling_factor, const double robot_radius, const double map_resolution) +{ + //erode the map so the planner doesn't go near the walls + // --> calculate the number of times for eroding from Robot Radius [m] + cv::Mat eroded_map; + int number_of_erosions = (robot_radius / map_resolution); + cv::erode(map, eroded_map, cv::Mat(), cv::Point(-1, -1), number_of_erosions); + //downsampling of the map to reduce calculation time + if (downsampling_factor != 1.) + cv::resize(eroded_map, downsampled_map, cv::Size(0, 0), downsampling_factor, downsampling_factor, cv::INTER_NEAREST);//_LINEAR); + else + downsampled_map = eroded_map; +} + +// A-star algorithm. +// The route returned is a string of direction digits. +std::string AStarPlanner::pathFind(const int & xStart, const int & yStart, const int & xFinish, const int & yFinish, const cv::Mat& map) +{ + static std::priority_queue pq[2]; // list of open (not-yet-tried) nodes + static int pqi; // pq index + static NodeAstar* n0; + static NodeAstar* m0; + static int i, j, x, y, xdx, ydy; + static char c; + pqi = 0; + + cv::Mat map_to_calculate_path(cv::Size(m, n), CV_32S); + + // create map from the given eroded map + for (int y = 0; y < map.rows; y++) + { + for (int x = 0; x < map.cols; x++) + { + if (map.at(y, x) == 255) + { + map_to_calculate_path.at(x, y) = 0; + } + else + { + map_to_calculate_path.at(x, y) = 1; + } + } + } + + cv::Mat closed_nodes_map(cv::Size(m, n), CV_32S); //map of already tried nodes + cv::Mat open_nodes_map(cv::Size(m, n), CV_32S); // map of open (not-yet-tried) nodes + cv::Mat dir_map(cv::Size(m, n), CV_32S); // map of directions + + // initialize the node maps + for (y = 0; y < closed_nodes_map.rows; y++) + { + for (x = 0; x < closed_nodes_map.cols; x++) + { + closed_nodes_map.at(y, x) = 0; + open_nodes_map.at(y, x) = 0; + } + } + + // create the start node and push into list of open nodes + n0 = new NodeAstar(xStart, yStart, 0, 0); + n0->updatePriority(xFinish, yFinish); + pq[pqi].push(*n0); + open_nodes_map.at(xStart, yStart) = n0->getPriority(); // mark it on the open nodes map + + //garbage collection + delete n0; + + // A* search + while (!pq[pqi].empty()) + { + // get the current node w/ the highest priority + // from the list of open nodes + n0 = new NodeAstar(pq[pqi].top().getxPos(), pq[pqi].top().getyPos(), pq[pqi].top().getLevel(), pq[pqi].top().getPriority()); + + x = n0->getxPos(); + y = n0->getyPos(); + + pq[pqi].pop(); // remove the node from the open list + open_nodes_map.at(x, y) = 0; + // mark it on the closed nodes map + closed_nodes_map.at(x, y) = 1; + + // quit searching when the goal state is reached + //if((*n0).estimate(xFinish, yFinish) == 0) + if (x == xFinish && y == yFinish) + { + // generate the path from finish to start + // by following the directions + std::string path = ""; + while (!(x == xStart && y == yStart)) + { + j = dir_map.at(x, y); + c = '0' + (j + dir / 2) % dir; + path = c + path; + x += dx[j]; + y += dy[j]; + } + + delete n0;// garbage collection + // empty the leftover nodes + while (!pq[pqi].empty()) + pq[pqi].pop(); + return path; + } + + // generate moves (child nodes) in all possible directions + for (i = 0; i < dir; i++) + { + xdx = x + dx[i]; + ydy = y + dy[i]; + + expanding_counter++; + + if (!(xdx < 0 || xdx > n - 1 || ydy < 0 || ydy > m - 1 || map_to_calculate_path.at(xdx, ydy) == 1 || closed_nodes_map.at(xdx, ydy) == 1)) + { + // generate a child node + m0 = new NodeAstar(xdx, ydy, n0->getLevel(), n0->getPriority()); + m0->nextLevel(i); + m0->updatePriority(xFinish, yFinish); + + // if it is not in the open list then add into that + if (open_nodes_map.at(xdx, ydy) == 0) + { + open_nodes_map.at(xdx, ydy) = m0->getPriority(); + pq[pqi].push(*m0); + // mark its parent node direction + dir_map.at(xdx, ydy) = (i + dir / 2) % dir; + } + else if (open_nodes_map.at(xdx, ydy) > m0->getPriority()) + { + // update the priority info + open_nodes_map.at(xdx, ydy) = m0->getPriority(); + // update the parent direction info + dir_map.at(xdx, ydy) = (i + dir / 2) % dir; + + // replace the node + // by emptying one pq to the other one + // except the node to be replaced will be ignored + // and the new node will be pushed in instead + while (!(pq[pqi].top().getxPos() == xdx && pq[pqi].top().getyPos() == ydy)) + { + pq[1 - pqi].push(pq[pqi].top()); + pq[pqi].pop(); + } + pq[pqi].pop(); // remove the wanted node + + // empty the larger size pq to the smaller one + if (pq[pqi].size() > pq[1 - pqi].size()) + pqi = 1 - pqi; + while (!pq[pqi].empty()) + { + pq[1 - pqi].push(pq[pqi].top()); + pq[pqi].pop(); + } + pqi = 1 - pqi; + pq[pqi].push(*m0); // add the better node instead + } + delete m0; // garbage collection + } + } + delete n0; // garbage collection + } + return ""; // no route found +} + +//This is the path planning algorithm for this class. It downsamples the map with the given factor (0 < factor < 1) so the +//map gets reduced and calculation time gets better. If it is set to 1 the map will have original size, if it is 0 the algorithm +//won't work, so make sure to not set it to 0. The algorithm also needs the Robot radius [m] and the map resolution [m²/pixel] to +//calculate the needed amount of erosions to include the radius in the planning. +double AStarPlanner::planPath(const cv::Mat& map, const cv::Point& start_point, const cv::Point& end_point, + const double downsampling_factor, const double robot_radius, const double map_resolution, + const int end_point_valid_neighborhood_radius, std::vector* route) +{ + expanding_counter = 0; + double step_length = 1./downsampling_factor; + + //length of the planned path + double path_length = 0; + + if(start_point.x == end_point.x && start_point.y == end_point.y)//if the start and end-point are the same return 0 + { + return path_length; + } + + // check for valid coordinates of start and end points + if (start_point.x < 0 || start_point.x >= map.cols || start_point.y < 0 || start_point.y >= map.rows || + end_point.x < 0 || end_point.x >= map.cols || end_point.y < 0 || end_point.y >= map.rows) + { + return 1e100; + } + + cv::Mat downsampled_map; + downsampleMap(map, downsampled_map, downsampling_factor, robot_radius, map_resolution); + + //transform the Pixel values to the downsampled ones + int start_x = downsampling_factor * start_point.x; + int start_y = downsampling_factor * start_point.y; + int end_x = downsampling_factor * end_point.x; + int end_y = downsampling_factor * end_point.y; + + //set the sizes of the map + m = downsampled_map.rows;// horizontal size of the map + n = downsampled_map.cols;// vertical size size of the map + + // get the route +// clock_t start = clock(); + route_ = pathFind(start_x, start_y, end_x, end_y, downsampled_map); + if (route_ == "") + { + if (end_point_valid_neighborhood_radius > 0) + { + for (int r=1; r<=end_point_valid_neighborhood_radius; ++r) + { + for (int dy=-r; dy<=r; ++dy) + { + for (int dx=-r; dx<=r; ++dx) + { + if ((abs(dy)!=r && abs(dx)!=r) || end_x+dx<0 || end_x+dx>=n || end_y+dy<0 || end_y+dy>=m) + continue; + route_ = pathFind(start_x, start_y, end_x+dx, end_y+dy, downsampled_map); + if (route_ != "") + break; + } + if (route_ != "") + break; + } + if (route_ != "") + break; + } + } + if (route_ == "") + { +// std::cout << "No path from " << start_point << " to " << end_point << " found for map of size " << map.rows << "x" << map.cols << " and downsampling factor " << downsampling_factor << std::endl; + return 1e100; //return extremely large distance as path length if the rout could not be generated + } + } +// clock_t end = clock(); +// double time_elapsed = double(end - start); + + // follow the route on the map and update the path length + if (route_.length() > 0) + { + int j; + char c; + int x = start_x; + int y = start_y; + const double straight_step = (1. / downsampling_factor); + const double diagonal_step = (std::sqrt(2.) / downsampling_factor); + for (int i = 0; i < route_.length(); i++) + { + //get the next char of the string and make it an integer, which shows the direction + c = route_.at(i); + j=c-'0'; + x = x + dx[j]; + y = y + dy[j]; + //Update the pathlength with the directions of the path. When the path goes vertical or horizontal add length 1. + //When it goes diagonal add sqrt(2) + if (j == 0 || j == 2 || j == 4 || j == 6) + { + path_length += straight_step; + } + if (j == 1 || j == 3 || j == 5 || j == 7) + { + path_length += diagonal_step; + } + } + } + + if(route != NULL) + getRoute(start_point, route_, step_length, *route); + + return path_length; +} + + +double AStarPlanner::planPath(const cv::Mat& map, const cv::Mat& downsampled_map, const cv::Point& start_point, const cv::Point& end_point, const double downsampling_factor, + const double robot_radius, const double map_resolution, const int end_point_valid_neighborhood_radius, cv::Mat* draw_path_map, + std::vector* route) +{ + route_ = ""; + double step_length = 1./downsampling_factor; +// cv::Mat debug = map.clone(); +// cv::circle(debug, start_point, 2, cv::Scalar(127), CV_FILLED); +// cv::circle(debug, end_point, 2, cv::Scalar(127), CV_FILLED); +// cv::imshow("debug", debug); +// cv::waitKey(); + double pathlength = step_length * planPath(downsampled_map, downsampling_factor*start_point, downsampling_factor*end_point, 1., 0., map_resolution, end_point_valid_neighborhood_radius, route); + if(pathlength > 1e90) //if no path can be found try with the original map + { + pathlength = planPath(map, start_point, end_point, 1., 0., map_resolution, 1./downsampling_factor * end_point_valid_neighborhood_radius, route); + step_length = 1.; + } + if(pathlength > 1e90) + std::cout << "######################### No path found on the originally sized map #######################" << std::endl; + else + { + if (draw_path_map!=NULL) + { + drawRoute(*draw_path_map, start_point, route_, step_length); + } + } + + return pathlength; +} diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/common/src/concorde_TSP.cpp b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/common/src/concorde_TSP.cpp new file mode 100644 index 0000000..711d718 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/common/src/concorde_TSP.cpp @@ -0,0 +1,317 @@ +# include + +#include +#include +#include + +//Default constructor +ConcordeTSPSolver::ConcordeTSPSolver() +: abort_computation_(false) +{ + +} + +void ConcordeTSPSolver::distance_matrix_thread(DistanceMatrix& distance_matrix_computation, cv::Mat& distance_matrix, + const cv::Mat& original_map, const std::vector& points, double downsampling_factor, + double robot_radius, double map_resolution, AStarPlanner& path_planner) +{ + distance_matrix_computation.constructDistanceMatrix(distance_matrix, original_map, points, downsampling_factor, + robot_radius, map_resolution, pathplanner_); +} + +void ConcordeTSPSolver::abortComputation() +{ + abort_computation_ = true; + + // kill concorde if running + const std::string pid_filename = "concorde_tsp_pid" + unique_file_identifier_ + ".txt"; + std::string pid_cmd = "pidof concorde > " + pid_filename; + int pid_result = system(pid_cmd.c_str()); + std::ifstream pid_reader(pid_filename.c_str()); + int value = -1; + std::string line; + if (pid_reader.is_open()) + { + while (getline(pid_reader, line)) + { + std::istringstream iss(line); + while (iss >> value) + { + std::cout << "PID of concorde: " << value << std::endl; + std::stringstream ss; + ss << "kill " << value; + std::string kill_cmd = ss.str(); + int kill_result = system(kill_cmd.c_str()); + std::cout << "kill result: " << kill_result << std::endl; + } + } + pid_reader.close(); + remove(pid_filename.c_str()); + } +} + +//This function generates a file with the current TSP in TSPlib format. This is necessary because concorde needs this file +//as input to solve the TSP. See http://comopt.ifi.uni-heidelberg.de/software/TSPLIB95/ for documentation. +void ConcordeTSPSolver::writeToFile(const cv::Mat& pathlength_matrix, const std::string& tsp_lib_filename, const std::string& tsp_order_filename) +{ + const std::string path_for_saving_file = tsp_lib_filename; //ros::package::getPath("libconcorde_tsp_solver") + "/common/files/TSPlib_file.txt"; + std::ofstream saving_file(path_for_saving_file.c_str()); + if (saving_file.is_open()) + { + std::cout << "Starting to create the TSPlib file: " << path_for_saving_file << std::endl; + //specify name of the Problem, Type (TSP = symmetrical TSP) and add a comment to the file. Name and Type are necessary, comment is for better understanding when you open the file. + saving_file << "NAME: ipa-building-navigation_" << tsp_lib_filename << std::endl + << "TYPE: TSP" << std::endl + << "COMMENT: This is the TSPlib file for using concorde. See http://comopt.ifi.uni-heidelberg.de/software/TSPLIB95/ for documentation." + << std::endl; + saving_file << "DIMENSION: " << pathlength_matrix.cols << std::endl; //Shows the Dimension of the problem --> the number of nodes (Necessary). + //Write the distance-matrix into the file as a full-matrix. + saving_file << "EDGE_WEIGHT_TYPE: EXPLICIT" << std::endl; + saving_file << "EDGE_WEIGHT_FORMAT: FULL_MATRIX" << std::endl; + saving_file << "EDGE_WEIGHT_SECTION" << std::endl; + + for (int row = 0; row < pathlength_matrix.rows; row++) + { + for (int col = 0; col < pathlength_matrix.cols; col++) + { + saving_file << " " + << (int) pathlength_matrix.at(row, col); + } + saving_file << std::endl; + } + //shows the end of the file + saving_file << "EOF"; + + std::cout << "Created the TSPlib file." << std::endl; + saving_file.close(); + } + else + { + std::cout << "Saving file '" << path_for_saving_file << "' for concorde could not be opened." << std::endl; + } + + // clear results file + std::ofstream reading_file(tsp_order_filename.c_str()); //open file + if (reading_file.is_open()) + { + reading_file << ""; + reading_file.close(); + } + else + { + std::cout << "Could not clear results file '" << tsp_order_filename << "'." << std::endl; + } +} + +//This function opens the file which saves the output from the concorde solver and reads the saved order. The names of the +//nodes in the graph are stored as positions in the distance matrix in this case. The first integer in the file is the number +//of nodes of this problem, so this one is not necessary. +std::vector ConcordeTSPSolver::readFromFile(const std::string& tsp_order_filename) +{ + std::string path_for_order_file = tsp_order_filename; //ros::package::getPath("libconcorde_tsp_solver") + "/common/files/TSP_order.txt"; //get path to file + std::ifstream reading_file(path_for_order_file.c_str()); //open file + + std::vector order_vector; //vector that stores the calculated TSP order + + std::string line; //current line of the file + + int value; //current node in the line + + int line_counter = 0; //variable to make sure that the first line isn't stored + + if (reading_file.is_open()) + { + //get new line in the file + while (getline(reading_file, line)) + { + std::istringstream iss(line); + while (iss >> value) + { + if (line_counter > 0) //first line shows the number of nodes and is not relevant + order_vector.push_back(value); //put the current node in the last position of the order + line_counter++; + } + } + reading_file.close(); + } + else + { + std::cout << "TSP order file '" << path_for_order_file << "' could not be opened." << std::endl; + } + return order_vector; +} + +//This function solves the given TSP using the systemcall to use the concorde TSP solver. This solver is applied from: +// http://www.math.uwaterloo.ca/tsp/concorde.html +//First you have to build the Solver in any possible way and if you don't make a ros-package out of it you have to change +//the paths to the right ones. If it is a ros-package the ros::package::getpath() function will find the right path. +//The usage of the solver is: ./concorde [-see below-] [dat_file] +//Navigate to the build Solver and then ./TSP and type ./concorde -h for a short explanation. + +//with a given distance matrix +std::vector ConcordeTSPSolver::solveConcordeTSP(const cv::Mat& path_length_matrix, const int start_Node) +{ + // generate a unique filename + timeval time; + gettimeofday(&time, NULL); + std::stringstream ss; + ss << "_" << time.tv_sec << "_" << time.tv_usec; + unique_file_identifier_ = ss.str(); + const std::string tsp_lib_filename = "TSPlib_file" + unique_file_identifier_ + ".txt"; + const std::string tsp_order_filename = "TSP_order" + unique_file_identifier_ + ".txt"; + + std::vector unsorted_order, sorted_order; + std::cout << "finding optimal order" << std::endl; + std::cout << "number of nodes: " << path_length_matrix.rows << " start node: " << start_Node << std::endl; + if (path_length_matrix.rows > 2) //check if the TSP has at least 3 nodes + { + //create the TSPlib file + writeToFile(path_length_matrix, tsp_lib_filename, tsp_order_filename); + + //use concorde to find optimal tour + std::string bin_folder; + while (bin_folder.length()==0) + { + const std::string temp_file = "temp_libconcorde_path" + unique_file_identifier_ + ".txt"; + try + { + std::string cmd = "rospack libs-only-L libconcorde_tsp_solver > " + temp_file; + int result = system(cmd.c_str()); + std::ifstream file(temp_file.c_str(), std::ifstream::in); + if (file.is_open()) + { + file >> bin_folder; + file.close(); + } + std::cout << "bin_folder: " << bin_folder << std::endl; + remove(temp_file.c_str()); + //bin_folder = ros::package::command("libs-only-L libconcorde_tsp_solver"); // this command crashes sometimes + //bin_folder.erase(std::remove(bin_folder.begin(), bin_folder.end(), '\n')); + } + catch (...) + { + std::cout << "ConcordeTSPSolver::solveConcordeTSP: ERROR: ['rospack libs-only-L libconcorde_tsp_solver > '" << temp_file << "] failed. Trying again." << std::endl; + } + } + std::string cmd = bin_folder + "/libconcorde_tsp_solver/concorde -o " + "$HOME/.ros/" + tsp_order_filename + " $HOME/.ros/" + tsp_lib_filename; + if (abort_computation_==true) + return sorted_order; + int result = system(cmd.c_str()); + if (abort_computation_==true) + return sorted_order; + assert(!result); + + //get order from saving file + unsorted_order = readFromFile(tsp_order_filename); + } + else + { + for(int node = 0; node < path_length_matrix.rows; node++) + { + unsorted_order.push_back(node); + } + } + // cleanup files + remove(tsp_lib_filename.c_str()); + remove(tsp_order_filename.c_str()); + const std::string tsp_lib_sol_filename = "TSPlib_file" + unique_file_identifier_ + ".sol"; + remove(tsp_lib_sol_filename.c_str()); + const std::string tsp_lib_res_filename = "TSPlib_file" + unique_file_identifier_ + ".res"; + remove(tsp_lib_res_filename.c_str()); + std::cout << "finished TSP" << std::endl; + + // if there is an error, just set unsorted order to 1, 2, 3, ... + if (unsorted_order.size() != path_length_matrix.rows) + { + std::cout << "ConcordeTSPSolver::solveConcordeTSP: Warning: Optimized order invalid, taking standard order 1, 2, 3, ..." << std::endl; + unsorted_order.clear(); + unsorted_order.resize(path_length_matrix.rows); + for (int i=0; i ConcordeTSPSolver::solveConcordeTSP(const cv::Mat& original_map, const std::vector& points, + double downsampling_factor, double robot_radius, double map_resolution, const int start_Node, cv::Mat* distance_matrix) +{ + //calculate the distance matrix + std::cout << "ConcordeTSPSolver::solveConcordeTSP: Constructing distance matrix..." << std::endl; + cv::Mat distance_matrix_ref; + if (distance_matrix != 0) + distance_matrix_ref = *distance_matrix; + DistanceMatrix distance_matrix_computation; + boost::thread t(boost::bind(&ConcordeTSPSolver::distance_matrix_thread, this, boost::ref(distance_matrix_computation), + boost::ref(distance_matrix_ref), boost::cref(original_map), boost::cref(points), downsampling_factor, + robot_radius, map_resolution, boost::ref(pathplanner_))); + bool finished = false; + while (finished==false) + { + if (abort_computation_==true) + distance_matrix_computation.abortComputation(); + finished = t.try_join_for(boost::chrono::milliseconds(10)); + } + +// distance_matrix_computation.constructDistanceMatrix(distance_matrix_ref, original_map, points, downsampling_factor, +// robot_radius, map_resolution, pathplanner_); + + if (abort_computation_==true) + { + std::vector sorted_order; + return sorted_order; + } + + return (solveConcordeTSP(distance_matrix_ref, start_Node)); +} + +// compute TSP from a cleaned distance matrix (does not contain any infinity paths) that has to be computed +std::vector ConcordeTSPSolver::solveConcordeTSPClean(const cv::Mat& original_map, const std::vector& points, + double downsampling_factor, double robot_radius, double map_resolution, const int start_node) +{ + // compute a cleaned distance matrix + cv::Mat distance_matrix_cleaned; + std::map cleaned_index_to_original_index_mapping; // maps the indices of the cleaned distance_matrix to the original indices of the original distance_matrix + int new_start_node = start_node; + DistanceMatrix distance_matrix_computation; + distance_matrix_computation.computeCleanedDistanceMatrix(original_map, points, downsampling_factor, robot_radius, map_resolution, pathplanner_, + distance_matrix_cleaned, cleaned_index_to_original_index_mapping, new_start_node); + + // solve TSP and re-index points to original indices + return solveConcordeTSPWithCleanedDistanceMatrix(distance_matrix_cleaned, cleaned_index_to_original_index_mapping, new_start_node); +} + + +// compute TSP with pre-computed cleaned distance matrix (does not contain any infinity paths) +std::vector ConcordeTSPSolver::solveConcordeTSPWithCleanedDistanceMatrix(const cv::Mat& distance_matrix, + const std::map& cleaned_index_to_original_index_mapping, const int start_node) +{ + // solve TSP and re-index points to original indices + std::vector optimal_order = solveConcordeTSP(distance_matrix, start_node); + for (size_t i=0; i + +#include +#include + +//Default constructor +GeneticTSPSolver::GeneticTSPSolver(int min_number_of_gens, int const_generations) +: abort_computation_(false) +{ + min_number_of_generations_ = min_number_of_gens; + const_generations_number_ = const_generations; +} + +void GeneticTSPSolver::distance_matrix_thread(DistanceMatrix& distance_matrix_computation, cv::Mat& distance_matrix, + const cv::Mat& original_map, const std::vector& points, double downsampling_factor, + double robot_radius, double map_resolution, AStarPlanner& path_planner) +{ + distance_matrix_computation.constructDistanceMatrix(distance_matrix, original_map, points, downsampling_factor, + robot_radius, map_resolution, pathplanner_); +} + +void GeneticTSPSolver::abortComputation() +{ + abort_computation_ = true; +} + +////Function to construct the distance matrix from the given points. See the definition at solveGeneticTSP for the style of this matrix. +//void GeneticTSPSolver::constructDistanceMatrix(cv::Mat& distance_matrix, const cv::Mat& original_map, const int number_of_nodes, +// const std::vector& points, double downsampling_factor, double robot_radius, double map_resolution) +//{ +// //create the distance matrix with the right size +// cv::Mat pathlengths(cv::Size(number_of_nodes, number_of_nodes), CV_64F); +// +// for (int i = 0; i < points.size(); i++) +// { +// cv::Point current_center = points[i]; +// for (int p = 0; p < points.size(); p++) +// { +// if (p != i) +// { +// if (p > i) //only compute upper right triangle of matrix, rest is symmetrically added +// { +// cv::Point neighbor = points[p]; +// double length = pathplanner_.planPath(original_map, current_center, neighbor, downsampling_factor, robot_radius, map_resolution); +// pathlengths.at(i, p) = length; +// pathlengths.at(p, i) = length; //symmetrical-Matrix --> saves half the computationtime +// } +// } +// else +// { +// pathlengths.at(i, p) = 0; +// } +// } +// } +// +// distance_matrix = pathlengths.clone(); +//} + +// This function calculates for a given path the length of it. The Path is a vector with ints, that show the index of the +// node in the path. It uses a pathlength Matrix, which should be calculated once. +// This Matrix should save the pathlengths with this logic: +// 1. The rows show from which Node the length is calculated. +// 2. For the columns in a row the Matrix shows the distance to the Node in the column. +// 3. From the node to itself the distance is 0. +double GeneticTSPSolver::getPathLength(const cv::Mat& path_length_Matrix, std::vector given_path) +{ + double length_of_given_path = 0; + + for (int i = 0; i < given_path.size() - 1; i++) + { + length_of_given_path += path_length_Matrix.at(given_path[i], given_path[i + 1]); + } + + return length_of_given_path; +} + +// This Function takes the given path and mutates it. A mutation is a random change of the path-order. For example random +// nodes can be switched, or a random intervall of nodes can be inverted. Only the first and last Node can't be changed, because +// they are given from the Main-function. +std::vector GeneticTSPSolver::mutatePath(const std::vector& parent_path) +{ + std::vector mutated_path; + + std::vector temporary_path; + std::vector saving_variable_path = parent_path; //saving-variable for the one time mutated path + + //this variable sets which aspect should be changed: + // 0: random nodes should be switched + // 1: random intervall should be inverted: + int what_to_change = (rand() % 2); + + if (what_to_change == 0) //random node-switching + { + int number_of_switches = (rand() % (parent_path.size() - 3)) + 1; // Set the number of switches that should be done. + // Because the first needs to be unchanged the number is limited. + // Also at least one change should be done. + for (int change = 0; change < number_of_switches; change++) + { + temporary_path.clear(); //reset the temporary path to fill it with new order + bool switched = false; //this variable makes sure that the switch has been done + do + { + int node_one = (rand() % (saving_variable_path.size() - 2)) + 1; //this variables random choose which nodes should be changed + int node_two = (rand() % (saving_variable_path.size() - 2)) + 1; //The first and last one should be untouched + if (node_one != node_two) //node can't be switched with himself + { + for (int node = 0; node < saving_variable_path.size(); node++) //fill the mutated path with the information + { + if (node == node_one) + { + temporary_path.push_back(saving_variable_path[node_two]); //add the second node which should be switched + } + else if (node == node_two) + { + temporary_path.push_back(saving_variable_path[node_one]); //add the first node which should be switched + } + else + { + temporary_path.push_back(saving_variable_path[node]); //add the nodes as they are + } + } + switched = true; + } + } while (!switched); + saving_variable_path = temporary_path; //save the one time mutated path + } + mutated_path = saving_variable_path; //save the finished mutated path + } + else if (what_to_change == 1) //random intervall-inverting (always from middle node on) + { + bool inverted = false; + do + { + int node_one = (rand() % (saving_variable_path.size() - 2)) + 1; //this variables random choose which intervall + int node_two = (rand() % (saving_variable_path.size() - 2)) + 1; //The first and last one should be untouched + int inverting_counter = 0; //variable to choose the node based on distance to the node_two + if (node_one > node_two) //switch variables, if the node_one is bigger than the node_two (easier to work with here) + { + int tmp_node = node_one; + node_one = node_two; + node_two = tmp_node; + } + if (node_one != node_two) + { + for (int node = 0; node < parent_path.size(); node++) + { + if (node < node_one || node > node_two) //add the nodes as they are in the mutated path + { + mutated_path.push_back(parent_path[node]); + } + else //invert the intervall + { + mutated_path.push_back(parent_path[node_two - inverting_counter]); + inverting_counter++; + } + } + inverted = true; + } + } while (!inverted); + } + else + { + ROS_INFO("Something was wrong in mutation-function."); + } + + return mutated_path; +} + +//This Function calculates the length of each given path and chooses the shortest one. It uses the getPathLength function. +std::vector GeneticTSPSolver::getBestPath(const std::vector > paths, const cv::Mat& pathlength_Matrix, bool& changed) +{ + std::vector best_path = paths[0]; + + double best_distance = getPathLength(pathlength_Matrix, paths[0]); //saving-variable for the distance of the current best path + + for (int current_path = 1; current_path < paths.size(); current_path++) + { + double current_distance = getPathLength(pathlength_Matrix, paths[current_path]); //get distance of current path + if (current_distance < best_distance) + { + best_distance = current_distance; + best_path = paths[current_path]; + changed = true; + } + } + + return best_path; +} + +//This is a solver for the TSP using a genetic algorithm. It calculates a initial path by using the nearest-neighbor +//search. It then applies an evolutional algorithm: +// +// I. Take the parent of the current generation and calculate 8 mutated children of it. A mutation can be a change +// of positions of nodes or the inversion of a intervall. The initial parent is the path from the nearest-neighbor search. +// II. It checks for the 9 paths (parent and children) for the best path (shortest) and takes this path as the parent +// of the new generation. +// III. It repeats the steps I. and II. at least a specified amount of times and then checks if the pathlength +// hasn't changed in the last steps. +// +//As input a symmetrical matrix of pathlenghts is needed. This matrix should save the pathlengths with this logic: +// 1. The rows show from which Node the length is calculated. +// 2. For the columns in a row the Matrix shows the distance to the Node in the column. +// 3. From the node to itself the distance is 0. + +//don't compute distance matrix +std::vector GeneticTSPSolver::solveGeneticTSP(const cv::Mat& path_length_Matrix, const int start_Node) +{ + std::vector return_vector; + NearestNeighborTSPSolver nearest_neighbor_solver; + + std::vector calculated_path = nearest_neighbor_solver.solveNearestTSP(path_length_Matrix, start_Node); + calculated_path.push_back(start_Node); //push the start node at the end, so the reaching of the start at the end is included in the planning + + if(path_length_Matrix.rows > 2) //check if graph has at least three members, if not the algorithm won't work properly + { + bool changed_path = false; //this variable checks if the path has been changed in the mutation process + int changing_counter = const_generations_number_; //this variable is a counter for how many times a path has been the same + + int number_of_generations = 0; + + do + { + if (abort_computation_==true) + return return_vector; + + number_of_generations++; + changed_path = false; + std::vector < std::vector > current_generation_paths; //vector to save the current generation + current_generation_paths.push_back(calculated_path); //first path is always the parent --> important for checking if the path has changed in getBestPath!! + for (int child = 0; child < 8; child++) //get 8 children and add them to the vector + { + current_generation_paths.push_back(mutatePath(calculated_path)); + } + calculated_path = getBestPath(current_generation_paths, path_length_Matrix, changed_path); //get the best path of this generation + if (number_of_generations >= min_number_of_generations_) //when a specified amount of steps have been done the algorithm checks if the last paths didn't change + { + if (changed_path) + { + changing_counter = const_generations_number_; //reset the counting-variable + } + else + { + changing_counter -= 1; //decrease the counting-variable by 1 + } + } + } while (changing_counter > 0 || number_of_generations < min_number_of_generations_); + } + + //return the calculated path without the last node (same as start node) + for(size_t node = 0; node < calculated_path.size()-1; ++node) + { + return_vector.push_back(calculated_path[node]); + } + + return return_vector; +} + +// compute distance matrix and maybe returning it +// this version does not exclude infinite paths from the TSP ordering +std::vector GeneticTSPSolver::solveGeneticTSP(const cv::Mat& original_map, const std::vector& points, double downsampling_factor, + double robot_radius, double map_resolution, const int start_Node, cv::Mat* distance_matrix) +{ + //calculate the distance matrix + std::cout << "GeneticTSPSolver::solveGeneticTSP: Constructing distance matrix..." << std::endl; + cv::Mat distance_matrix_ref; + if (distance_matrix != 0) + distance_matrix_ref = *distance_matrix; + DistanceMatrix distance_matrix_computation; + boost::thread t(boost::bind(&GeneticTSPSolver::distance_matrix_thread, this, boost::ref(distance_matrix_computation), + boost::ref(distance_matrix_ref), boost::cref(original_map), boost::cref(points), downsampling_factor, + robot_radius, map_resolution, boost::ref(pathplanner_))); + bool finished = false; + while (finished==false) + { + if (abort_computation_==true) + distance_matrix_computation.abortComputation(); + finished = t.try_join_for(boost::chrono::milliseconds(10)); + } +// DistanceMatrix distance_matrix_computation; +// distance_matrix_computation.constructDistanceMatrix(distance_matrix_ref, original_map, points, downsampling_factor, robot_radius, map_resolution, pathplanner_); + + if (abort_computation_==true) + { + std::vector return_vector; + return return_vector; + } + + return (solveGeneticTSP(distance_matrix_ref, start_Node)); +} + + +// compute TSP from a cleaned distance matrix (does not contain any infinity paths) that has to be computed +std::vector GeneticTSPSolver::solveGeneticTSPClean(const cv::Mat& original_map, const std::vector& points, + double downsampling_factor, double robot_radius, double map_resolution, const int start_node) +{ + // compute a cleaned distance matrix + cv::Mat distance_matrix_cleaned; + std::map cleaned_index_to_original_index_mapping; // maps the indices of the cleaned distance_matrix to the original indices of the original distance_matrix + int new_start_node = start_node; + DistanceMatrix distance_matrix_computation; + distance_matrix_computation.computeCleanedDistanceMatrix(original_map, points, downsampling_factor, robot_radius, map_resolution, pathplanner_, + distance_matrix_cleaned, cleaned_index_to_original_index_mapping, new_start_node); + + // solve TSP and re-index points to original indices + return solveGeneticTSPWithCleanedDistanceMatrix(distance_matrix_cleaned, cleaned_index_to_original_index_mapping, new_start_node); +} + + +// compute TSP with pre-computed cleaned distance matrix (does not contain any infinity paths) +std::vector GeneticTSPSolver::solveGeneticTSPWithCleanedDistanceMatrix(const cv::Mat& distance_matrix, + const std::map& cleaned_index_to_original_index_mapping, const int start_node) +{ + // solve TSP and re-index points to original indices + std::vector optimal_order = solveGeneticTSP(distance_matrix, start_node); + for (size_t i=0; i + +using namespace std; +using namespace boost; + +// +//***********************Maximal Clique Finder***************************** +// +//This class provides a maximal clique-finder for a given Graph that finds all maximal cliques in this. A maximal clique +//is a subgraph in the given Graph, in which all Nodes are connected to each other and cannot be enlarged by adding other +//Nodes ( https://en.wikipedia.org/wiki/Maximum_clique ). It uses the c++ Boost library by using the implementen Bron-Kerbosch +//function and defining the Graph as a boost::undirected_Graph. Due of a missing documentation for this function see +// +// http://www.boost.org/doc/libs/1_56_0/libs/graph/example/bron_kerbosch_print_cliques.cpp +// http://www.boost.org/doc/libs/1_58_0/libs/graph/example/bron_kerbosch_clique_number.cpp +// http://www.boost.org/doc/libs/1_47_0/libs/graph/example/helper.hpp +// http://stackoverflow.com/questions/23299406/maximum-weighted-clique-in-a-large-graph-with-high-density +// +//for further information. +//As input this function takes a symmetrical Matrix that stores the pathlengths from one node of the graph to another. +//If one Node has no connection to another the element in the matrix is zero, it also is at the main-diagonal. +//!!!!!!!!!!!!!See maximal_clique_finder.h for further information on formatting.!!!!!!!!!!!!! + +static std::vector > names_; //vector to save the cliques achieved by Boost + +// The Actor type stores the name of each vertex in the graph. +struct Actor +{ + string name; +}; + +//define some types that are used for the boost function +typedef undirected_graph Graph; +typedef graph_traits::vertex_descriptor Vertex; +typedef graph_traits::edge_descriptor Edge; +typedef property_map::type NameMap; + +template +typename graph_traits::vertex_descriptor cliqueFinder::addNamedVertex(Graph& g, NameMap nm, const string& name, VertexMap& vm) +{ + typedef typename graph_traits::vertex_descriptor Vertex; + typedef typename VertexMap::iterator Iterator; + + Vertex v; + Iterator iter; + bool inserted; + boost::tie(iter, inserted) = vm.insert(make_pair(name, Vertex())); + if (inserted) + { + // The name was unique so we need to add a vertex to the graph + v = add_vertex(g); + iter->second = v; + put(nm, v, name); // store the name in the name map + } + else + { + // We had already inserted this name so we can return the + // associated vertex. + v = iter->second; + } + return v; +} + +//This class is the Visitor for all nodes in the graph, which is used by boost::bron_kerbosch. the function clique() gets +//called everytime a maximal clique was found. This comes from the boost implementation. +template +struct visitor +{ + vector current_names; + visitor() + { + + } + + template + void clique(const Clique& c, const Graph& g) + { + // Iterate over the clique and print each vertex within it. + typename Clique::const_iterator i, end = c.end(); + for (i = c.begin(); i != end; ++i) + { + current_names.push_back(atoi(g[*i].name.c_str())); + } + names_.push_back(current_names); //save the clique to return all later + current_names.clear(); //clear the current clique + } +}; + +cliqueFinder::cliqueFinder() +{ + +} + +//This function creates a boost::undirected_graph out of the cutted distance-Matrix. The Graph is used for the +//boost::bron-kerbosch algorihtm. +template +void cliqueFinder::createGraph(Graph& graph, cv::Mat& distance_Matrix) +{ + vector vertexes; + NameMap nmap(get(&Actor::name, graph)); + std::map vert_map; + //add all Nodes to the graph + for (int counter = 0; counter < distance_Matrix.cols; counter++) + { + stringstream current_name; + current_name << counter; //<< counter; + Vertex current_vertex = addNamedVertex(graph, nmap, current_name.str(), vert_map); + vertexes.push_back(current_vertex); + } + //add each Edge if there is a connection between the Nodes + for (int current_vertex = 0; current_vertex < vertexes.size(); current_vertex++) + { + for (int neighbor_node = current_vertex; neighbor_node < vertexes.size(); neighbor_node++) + { + if (distance_Matrix.at(current_vertex, neighbor_node) > 0) + { + add_edge(vertexes[current_vertex], vertexes[neighbor_node], graph); + } + } + } +} + +//This function cuts an edge if the distance between the two Nodes is too large. This is neccessary to find possible +//areas in the graph for cliques. If the complete graph is connected only one clique will be found, containing all +//Nodes in the graph, which isn't very useful for planning. +void cliqueFinder::cutTooLongEdges(cv::Mat& complete_distance_matrix, double maxval) +{ + for (int row = 0; row < complete_distance_matrix.rows; row++) + { + for (int col = 0; col < complete_distance_matrix.cols; col++) + { + if (complete_distance_matrix.at(row, col) > maxval) + { + complete_distance_matrix.at(row, col) = 0; + } + } + } +} + +//This function uses the previously described functions and finds all maximal cliques in a given graph. The maxval parameter +//is used to cut edges that are too long. See maximal_clique_finder.h for further information on formatting. +std::vector > cliqueFinder::getCliques(const cv::Mat& distance_matrix, double maxval) +{ + // Create a graph object + Graph g; + + //cut the edges if they are too long + cv::Mat cutted_distance_matrix = distance_matrix.clone(); + cutTooLongEdges(cutted_distance_matrix, maxval); + + //Create a graph out of the cutted distance matrix + createGraph(g, cutted_distance_matrix); + + //Object needed from boost to return the results + visitor vis; + + // Use the Bron-Kerbosch algorithm to find all cliques + bron_kerbosch_all_cliques(g, vis); + + //Make sure that nodes, which are too far away from other nodes are in the clique-vector + //(a clique has at least two nodes, but in this case it is neccessary to have all nodes in the cliques and + //nodes that are too far away from others count also as a possible group) + for (int node = 0; node < distance_matrix.rows; node++) + { + bool add = true; + for (int group = 0; group < names_.size(); group++) + { + if (contains(names_[group], node)) + { + add = false; + } + } + if(add) + { + std::vector adding_vector; + adding_vector.push_back(node); + names_.push_back(adding_vector); + } + } + + //save the names_ vector and clear it for next usage + std::vector > returning_vector(names_); + names_.clear(); + + return returning_vector; +} diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/common/src/nearest_neighbor_TSP.cpp b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/common/src/nearest_neighbor_TSP.cpp new file mode 100644 index 0000000..33e2af4 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/common/src/nearest_neighbor_TSP.cpp @@ -0,0 +1,102 @@ +#include + +//Default Constructor +NearestNeighborTSPSolver::NearestNeighborTSPSolver() +{ + +} + +//This function calculates the order of the TSP, using the nearest neighbor method. It uses a pathlength Matrix, which +//should be calculated once. This Matrix should save the pathlengths with this logic: +// 1. The rows show from which Node the length is calculated. +// 2. For the columns in a row the Matrix shows the distance to the Node in the column. +// 3. From the node to itself the distance is 0. +std::vector NearestNeighborTSPSolver::solveNearestTSP(const cv::Mat& path_length_matrix, const int start_node) +{ + std::vector calculated_order; //solution order + + if(path_length_matrix.rows > 1) //check if clique has more than one member or else this algorithm produces a order of size=3 + { + int last_node; //index of the last spectated node + std::vector visited(path_length_matrix.rows, false); + + int current_node = start_node; //index of the current spectated node + calculated_order.push_back(current_node); + visited[current_node] = true; + + //check every Point for the next nearest neighbor and add it to the order + do + { + int next_node; //saver for next node + double min_distance = 1e100; //saver for distance to current next node + for (int current_neighbor = 0; current_neighbor < path_length_matrix.cols; current_neighbor++) + { + if (visited[current_neighbor]==false) //check if current neighbor hasn't been visited yet + { + const double length = path_length_matrix.at(current_node, current_neighbor); + if (length < min_distance && length > 0) + { + next_node = current_neighbor; + min_distance = length; + } + } + } + calculated_order.push_back(next_node); //add the found nearest neighbor to the order-vector + visited[next_node] = true; + current_node = next_node; + } while (calculated_order.size() < path_length_matrix.rows); //when the order has as many elements as the pathlength Matrix has the solver is ready + + } + else + { + calculated_order.push_back(start_node); + } + + return calculated_order; +} + +// compute TSP and distance matrix without cleaning it +// this version does not exclude infinite paths from the TSP ordering +std::vector NearestNeighborTSPSolver::solveNearestTSP(const cv::Mat& original_map, const std::vector& points, + double downsampling_factor, double robot_radius, double map_resolution, const int start_node, cv::Mat* distance_matrix) +{ + std::cout << "NearestNeighborTSPSolver::solveNearestTSP: Constructing distance matrix..." << std::endl; + //calculate the distance matrix + cv::Mat distance_matrix_ref; + if (distance_matrix != 0) + distance_matrix_ref = *distance_matrix; + DistanceMatrix distance_matrix_computation; + distance_matrix_computation.constructDistanceMatrix(distance_matrix_ref, original_map, points, downsampling_factor, robot_radius, map_resolution, pathplanner_); + + return solveNearestTSP(distance_matrix_ref, start_node); +} + + +// compute TSP from a cleaned distance matrix (does not contain any infinity paths) that has to be computed +std::vector NearestNeighborTSPSolver::solveNearestTSPClean(const cv::Mat& original_map, const std::vector& points, + double downsampling_factor, double robot_radius, double map_resolution, const int start_node) +{ + // compute a cleaned distance matrix + cv::Mat distance_matrix_cleaned; + std::map cleaned_index_to_original_index_mapping; // maps the indices of the cleaned distance_matrix to the original indices of the original distance_matrix + int new_start_node = start_node; + DistanceMatrix distance_matrix_computation; + distance_matrix_computation.computeCleanedDistanceMatrix(original_map, points, downsampling_factor, robot_radius, map_resolution, pathplanner_, + distance_matrix_cleaned, cleaned_index_to_original_index_mapping, new_start_node); + + // solve TSP and re-index points to original indices + return solveNearestTSPWithCleanedDistanceMatrix(distance_matrix_cleaned, cleaned_index_to_original_index_mapping, new_start_node); +} + + +// compute TSP with pre-computed cleaned distance matrix (does not contain any infinity paths) +std::vector NearestNeighborTSPSolver::solveNearestTSPWithCleanedDistanceMatrix(const cv::Mat& distance_matrix, + const std::map& cleaned_index_to_original_index_mapping, const int start_node) +{ + // solve TSP and re-index points to original indices + std::vector optimal_order = solveNearestTSP(distance_matrix, start_node); + for (size_t i=0; i + +const int dir_ = 8; + +NodeAstar::NodeAstar(int xp, int yp, int d, int p) +{ + xPos_ = xp; + yPos_ = yp; + level_ = d; + priority_ = p; +} + +int NodeAstar::getxPos() const +{ + return xPos_; +} +int NodeAstar::getyPos() const +{ + return yPos_; +} +int NodeAstar::getLevel() const +{ + return level_; +} +int NodeAstar::getPriority() const +{ + return priority_; +} + +void NodeAstar::updatePriority(const int& xDest, const int& yDest) +{ + priority_ = level_ + estimate(xDest, yDest); // * 10; //A* +} + +// give better priority to going strait instead of diagonally +void NodeAstar::nextLevel(const int& i) // i: direction +{ + level_ += (dir_ == 8 ? (i % 2 == 0 ? 10 : 14) : 10); +} + +// Estimation function for the remaining distance to the goal. +// +//!!!!!!!!!!!!!!!!!!!!!!!Important!!!!!!!!!!!!!!!!!!!!!!! +//Uncomment the method to calculate the distance between this node and the goal you want to use. Eclidean is more precisly +//but could take longer to get long paths. +// +const int& NodeAstar::estimate(const int& xDest, const int& yDest) const +{ + static int xd, yd, d; + xd = xDest - xPos_; + yd = yDest - yPos_; + + // Euclidian Distance + d = static_cast(sqrt(xd * xd + yd * yd)); + + // Manhattan distance +// d = abs(xd) + abs(yd); + + // Chebyshev distance +// d=std::max(abs(xd), abs(yd)); + + return (d); +} diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/common/src/set_cover_solver.cpp b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/common/src/set_cover_solver.cpp new file mode 100644 index 0000000..5cce36b --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/common/src/set_cover_solver.cpp @@ -0,0 +1,306 @@ +#include + +//Default constructor +SetCoverSolver::SetCoverSolver() +{ + +} + +////Function to construct the symmetrical distance matrix from the given points. The rows show from which node to start and +////the columns to which node to go. If the path between nodes doesn't exist or the node to go to is the same as the one to +////start from, the entry of the matrix is 0. +//void SetCoverSolver::constructDistanceMatrix(cv::Mat& distance_matrix, const cv::Mat& original_map, +// const std::vector& points, double downsampling_factor, double robot_radius, double map_resolution) +//{ +// //create the distance matrix with the right size +// cv::Mat pathlengths(cv::Size((int)points.size(), (int)points.size()), CV_64F); +// +// // reduce image size already here to avoid resizing in the planner each time +// const double one_by_downsampling_factor = 1./downsampling_factor; +// cv::Mat downsampled_map; +// pathplanner_.downsampleMap(original_map, downsampled_map, downsampling_factor, robot_radius, map_resolution); +// +// for (int i = 0; i < points.size(); i++) +// { +// cv::Point current_center = downsampling_factor * points[i]; +// for (int j = 0; j < points.size(); j++) +// { +// if (j != i) +// { +// if (j > i) //only compute upper right triangle of matrix, rest is symmetrically added +// { +// cv::Point neighbor = downsampling_factor * points[j]; +// double length = one_by_downsampling_factor * pathplanner_.planPath(downsampled_map, current_center, neighbor, 1., 0., map_resolution); +// pathlengths.at(i, j) = length; +// pathlengths.at(j, i) = length; //symmetrical-Matrix --> saves half the computationtime +// } +// } +// else +// { +// pathlengths.at(i, j) = 0; +// } +// } +// } +// +// distance_matrix = pathlengths.clone(); +//} + +//This function takes a vector of found nodes and merges them together, if they have at least one node in common. +std::vector > SetCoverSolver::mergeGroups(const std::vector >& found_groups) +{ + std::vector < std::vector > merged_groups; //The merged groups. + + std::vector done_groups; //Vector to remember which groups has already been looked at + + for (int current_group = 0; current_group < found_groups.size(); current_group++) + { + if (!contains(done_groups, current_group)) //If the group is in the done-vector don't look at it, because it has already been looked at. + { + done_groups.push_back(current_group); //add the current group to the done groups + + std::vector current_group_saver(found_groups[current_group]); //vector to save the current group + + std::vector merging_candidates; //vector to save the groups which should be merged with the current group + + //check if the current group has at least one node in common with the other groups + for (int next_group = 0; next_group < found_groups.size(); next_group++) + { + bool merge = false; //variable to check if the room should be added to the current group + + //if it is the same group or has already been looked at it doesn't need to be checked + if (next_group != current_group && !contains(done_groups, next_group)) + { + for (int node = 0; node < found_groups[next_group].size(); node++) + { + if (contains(found_groups[current_group], found_groups[next_group][node])) + { + merge = true; + } + } + } + if (merge) //If the group has at least one neighbor save it for merging + { + merging_candidates.push_back(next_group); + } + } + + //Add the merging-candidates nodes to the current group and add the candidates to the done groups + for (int merge_candidate = 0; merge_candidate < merging_candidates.size(); merge_candidate++) + { + done_groups.push_back(merging_candidates[merge_candidate]); + for (int node = 0; node < found_groups[merging_candidates[merge_candidate]].size(); node++) + { + if (!contains(current_group_saver, found_groups[merging_candidates[merge_candidate]][node])) + { + current_group_saver.push_back(found_groups[merging_candidates[merge_candidate]][node]); + } + } + } + //add the merged group to the vector + merged_groups.push_back(current_group_saver); + } + } + std::cout << "Finished merging." << std::endl; + return merged_groups; +} + +//This functions solves the set-cover Problem ( https://en.wikipedia.org/wiki/Set_cover_problem#Greedy_algorithm ) using +//the greedy-search algorithm. It chooses the clique that has the most uncovered nodes in it first. Then it uses the merge-function +//above to merge groups that have at least one node in common together. The vector stores the indexes of the nodes, which +//are the same as the ones from the clique-solver and also the distance-matrix. The variable max_number_of_clique_members +//implies how many members a clique is allowed to have. + +//the cliques are given +std::vector > SetCoverSolver::solveSetCover(std::vector >& given_cliques, + const int number_of_nodes, const int max_number_of_clique_members, const cv::Mat& distance_matrix) +{ + std::vector < std::vector > minimal_set; + + std::vector > cliques_for_covering; + + for(size_t clique = 0; clique < given_cliques.size(); clique++) + { + std::vector temporary_clique; + for(size_t node = 0; node < given_cliques[clique].size(); node++) + { + temporary_clique.push_back(given_cliques[clique][node]); + } + cliques_for_covering.push_back(temporary_clique); + } + + //Put the nodes in a open-nodes vector. The nodes are named after their position in the room-centers-vector and so every + //node from 0 to number_of_nodes-1 is in the Graph. + std::vector open_nodes; + for (int new_node = 0; new_node < number_of_nodes; new_node++) + { + open_nodes.push_back(new_node); + } + + std::cout << "Starting greedy search for set-cover-problem." << std::endl; + + //Search for the clique with the most unvisited nodes and choose this one before the others. Then remove the nodes of + //this clique from the unvisited-vector. This is done until no more nodes can be visited. + + do + { + int covered_open_nodes; + int best_covered_counter = 0; + int best_clique = -1; + for (int clique = 0; clique < cliques_for_covering.size(); clique++) + { + // skip too big cliques + if(cliques_for_covering[clique].size() > max_number_of_clique_members) + continue; + + covered_open_nodes = 0; + for (int node = 0; node < cliques_for_covering[clique].size(); node++) + { + if (contains(open_nodes, cliques_for_covering[clique][node])) + { + covered_open_nodes++; + } + } + if (covered_open_nodes > best_covered_counter) + { + best_covered_counter = covered_open_nodes; + best_clique = clique; + } + } + + // check if a allowed clique could be found, if not split the biggest clique until it consists of cliques that are of the + // allowed size + if(best_clique == -1) + { + for (int clique = 0; clique < cliques_for_covering.size(); clique++) + { + covered_open_nodes = 0; + for (int node = 0; node < cliques_for_covering[clique].size(); node++) + { + if (contains(open_nodes, cliques_for_covering[clique][node])) + { + covered_open_nodes++; + } + } + if (covered_open_nodes > best_covered_counter) + { + best_covered_counter = covered_open_nodes; + best_clique = clique; + } + } + + // save big clique + std::vector big_clique = cliques_for_covering[best_clique]; + + // iteratively remove nodes far away from the remaining nodes to create small cliques + bool removed_node = false; + std::vector > found_subgraphs; + do + { + removed_node = false; // reset checking boolean + std::vector current_subgraph = big_clique; + while(current_subgraph.size() > max_number_of_clique_members) + { + removed_node = true; + + // find the node farthest away from the other nodes + double max_distance = 0.0; + int worst_node = -1; + for(size_t node = 0; node < current_subgraph.size(); ++node) + { + // compute sum of distances from current node to neighboring nodes + double current_distance = 0; + for(size_t neighbor = 0; neighbor < current_subgraph.size(); ++neighbor) + { + // don't look at node itself + if(node == neighbor) + continue; + + current_distance += distance_matrix.at(current_subgraph[node], current_subgraph[neighbor]); + } + + // check if sum of distances is worse than the previously found ones + if(current_distance > max_distance) + { + worst_node = node; + max_distance = current_distance; + } + } + + // remove the node farthest away from all other nodes out of the subgraph + current_subgraph.erase(current_subgraph.begin() + worst_node); + } + + // save the found subgraph + found_subgraphs.push_back(current_subgraph); + + // erase the covered nodes from the big clique + for(size_t node = 0; node < current_subgraph.size(); ++node) + big_clique.erase(std::remove(big_clique.begin(), big_clique.end(), current_subgraph[node]), big_clique.end()); + + }while(removed_node == true && big_clique.size() > 0); + + // add found subgraphs to the minimal set + for(size_t subgraph = 0; subgraph < found_subgraphs.size(); ++subgraph) + minimal_set.push_back(found_subgraphs[subgraph]); + } + else + { + minimal_set.push_back(cliques_for_covering[best_clique]); + } + //remove nodes of best clique from all cliques (this is okay because if you remove a node from a clique it stays a clique, it only isn't a maximal clique anymore) + for(size_t clique = 0; clique < cliques_for_covering.size(); clique++) + { + for(int node = 0; node < cliques_for_covering[best_clique].size(); node++) + { + if(clique != best_clique) + cliques_for_covering[clique].erase(std::remove(cliques_for_covering[clique].begin(), cliques_for_covering[clique].end(), cliques_for_covering[best_clique][node]), cliques_for_covering[clique].end()); + } + } + for (int node = 0; node < cliques_for_covering[best_clique].size(); node++) + { + open_nodes.erase(std::remove(open_nodes.begin(), open_nodes.end(), cliques_for_covering[best_clique][node]), open_nodes.end()); + } +// cliques_for_covering = new_set; +// std::cout << open_nodes.size() << std::endl; + } while (open_nodes.size() > 0); + + std::cout << "Finished greedy search." << std::endl; + +// std::cout << "Starting merging the found groups." << std::endl; +// +// return mergeGroups(minimal_set); + return minimal_set; +} + +//the distance matrix is given, but not the cliques +std::vector > SetCoverSolver::solveSetCover(const cv::Mat& distance_matrix, const std::vector& points, + const int number_of_nodes, double maximal_pathlength, const int max_number_of_clique_members) +{ + //get all maximal cliques for this graph + std::vector < std::vector > maximal_cliques = maximal_clique_finder.getCliques(distance_matrix, maximal_pathlength); + + //put the single nodes in the maximal cliques vector to make sure every node gets covered from the setCover solver + for(int single_node = 0; single_node < number_of_nodes; single_node++) + { + std::vector temp; + temp.push_back(single_node); + maximal_cliques.push_back(temp); + } + + return (solveSetCover(maximal_cliques, number_of_nodes, max_number_of_clique_members, distance_matrix)); +} + +//the distance matrix and cliques aren't given and the matrix should not be returned +std::vector > SetCoverSolver::solveSetCover(const cv::Mat& original_map, const std::vector& points, + double downsampling_factor, double robot_radius, double map_resolution, double maximal_pathlength, const int max_number_of_clique_members, cv::Mat* distance_matrix) +{ + //calculate the distance matrix + cv::Mat distance_matrix_ref; + if (distance_matrix != 0) + distance_matrix_ref = *distance_matrix; + DistanceMatrix distance_matrix_computation; + distance_matrix_computation.constructDistanceMatrix(distance_matrix_ref, original_map, points, downsampling_factor, robot_radius, map_resolution, pathplanner_); + + //get all maximal cliques for this graph and solve the set cover problem + return (solveSetCover(distance_matrix_ref, points, (int)points.size(), maximal_pathlength, max_number_of_clique_members)); +} diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/common/src/trolley_position_finder.cpp b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/common/src/trolley_position_finder.cpp new file mode 100644 index 0000000..ea3144e --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/common/src/trolley_position_finder.cpp @@ -0,0 +1,231 @@ +#include + +//Defaul Constructor +TrolleyPositionFinder::TrolleyPositionFinder() +{ + +} + +//This function takes one group and calculates the trolley position for it. It does following steps: +// I. Get the bounding box for all Points in the group. Then expand it by a little factor to make sure the best +// position is found, even when it is slightly outside the bounding Box. +// II. Put a grid over the bounding box to get cells in which the trolley-position possibly is. +// Find the Point in these cells that have the largest distance to the closest zero Pixel as possible candidates +// for trolley positions. +// III. From these candidates the one is chosen, which gets the smallest pathlength to all group Points. If the group +// has only two members the algorithm chooses the candidate as trolley position that is the middlest between these. +cv::Point TrolleyPositionFinder::findOneTrolleyPosition(const std::vector group_points, const cv::Mat& original_map, + const double downsampling_factor, const double robot_radius, const double map_resolution) +{ + double largening_of_bounding_box = 5; //Variable to expand the bounding box of the roomcenters a little bit. This is done to make sure the best trolley position is found if it is a little bit outside this bounding box. + double max_x_value = group_points[0].x; //max/min values of the Points that get the bounding box. Initialized with the coordinates of the first Point of the group. + double min_x_value = group_points[0].x; + double max_y_value = group_points[0].y; + double min_y_value = group_points[0].y; + + //create eroded map, which is used to check if the trolley-position candidates are too close to the boundaries + cv::Mat eroded_map; + cv::erode(original_map, eroded_map, cv::Mat(), cv::Point(-1, -1), 4); + + //create the distance-map to find the candidates for trolley-Positions + cv::Mat temporary_map = original_map.clone(); + cv::erode(temporary_map, temporary_map, cv::Mat()); + cv::Mat distance_map; //variable for the distance-transformed map, type: CV_32FC1 +#if CV_MAJOR_VERSION<=3 + cv::distanceTransform(temporary_map, distance_map, CV_DIST_L2, 5); +#else + cv::distanceTransform(temporary_map, distance_map, cv::DIST_L2, 5); +#endif + cv::convertScaleAbs(distance_map, distance_map); // conversion to 8 bit image + + // + //******************************** I. Get bounding box of the group ******************************** + // + //go trough each Point and find the min/max x/y values --> bounding box + for (int point = 0; point < group_points.size(); point++) + { + if (group_points[point].x > max_x_value) + { + max_x_value = group_points[point].x; + } + if (group_points[point].x < min_x_value) + { + min_x_value = group_points[point].x; + } + if (group_points[point].y > max_y_value) + { + max_y_value = group_points[point].y; + } + if (group_points[point].y < min_y_value) + { + min_y_value = group_points[point].y; + } + } + + //expand the bounding box sligthly by the defined factor (check if the values aren't out of the map boundaries before doing this) + if (max_x_value + largening_of_bounding_box < original_map.cols) + { + max_x_value += largening_of_bounding_box; + } + if (min_x_value - largening_of_bounding_box > 0) + { + min_x_value -= largening_of_bounding_box; + } + if (max_y_value + largening_of_bounding_box < original_map.rows) + { + max_y_value += largening_of_bounding_box; + } + if (min_y_value - largening_of_bounding_box > 0) + { + min_y_value -= largening_of_bounding_box; + } + + // + //******************************** II. Get the candidates for trolley positions ******************************** + // + double cell_side_length = 10; + double max_x_cell = min_x_value + cell_side_length; + double min_x_cell = min_x_value; + double max_y_cell = min_y_value + cell_side_length; + double min_y_cell = min_y_value; + + bool out_of_y = false; + bool out_of_x = false; + + std::vector < cv::Point > trolley_position_candidates; + + //go trough each cell and find the candidate for each of it + do //go from y_min to y_max + { + max_x_cell = min_x_value + cell_side_length; //reset the x-values for the cell to start from beginning when a new y coordinate is reached + min_x_cell = min_x_value; + do //go from x_min to x_max + { + out_of_x = false; + double best_x = min_x_cell; + double best_y = min_y_cell; + //go trough each Pixel of the cell and take the one that is most far away of the zero Pixels. + for (int y = min_y_cell; y < max_y_cell; y++) + { + for (int x = min_x_cell; x < max_x_cell; x++) + { + if (distance_map.at(best_y, best_x) < distance_map.at(y, x) && eroded_map.at(y, x) != 0) + { + best_x = x; + best_y = y; + } + } + } + //check if candidate is far enough away from boundary + if (eroded_map.at(best_y, best_x) != 0) + { + trolley_position_candidates.push_back(cv::Point(best_x, best_y)); + } + min_x_cell = max_x_cell; //set new x values for next step + max_x_cell += cell_side_length; + + //check if x is out of box --> if so the next y values should be checked + if (min_x_cell > max_x_value) + { + out_of_x = true; + } + } while (!out_of_x); + + min_y_cell = max_y_cell; //set new y values for next step + max_y_cell += cell_side_length; + + //check if y is out of bounding box --> if true step is done + if (min_y_cell > max_y_value) + { + out_of_y = true; + } + } while (!out_of_y); + + // + //***************** III. Find the candidate that minimizes the pathlengths to all group points ***************** + // + //variables to save the best candidate + double best_pathlength = 1e10; + double best_pathlength_point_distance = 1e10; + int best_trolley_candidate = 0; + + // reduce image size already here to avoid resizing in the planner each time + const double one_by_downsampling_factor = 1./downsampling_factor; + cv::Mat downsampled_map; + path_planner_.downsampleMap(original_map, downsampled_map, downsampling_factor, robot_radius, map_resolution); + + //go trough each candidate and calculate the sum of pathlengths + for (size_t candidate = 0; candidate < trolley_position_candidates.size(); candidate++) + { + cv::Point start_point = downsampling_factor * trolley_position_candidates[candidate]; + double current_pathlength = 0; + std::vector pathlengths; + for (int room_center = 0; room_center < group_points.size(); room_center++) + { + cv::Point end_point = downsampling_factor * group_points[room_center]; + //get the pathlength to the current center and save it + double center_pathlength = one_by_downsampling_factor * path_planner_.planPath(downsampled_map, start_point, end_point, 1., 0., map_resolution); + pathlengths.push_back(center_pathlength); + //add the pathlenght to the total pathlength + current_pathlength += center_pathlength; + } + //check for the best position that has the shortest pathlength to all centers. Adding a little bit to the best_distances + //because the downsampling generates an error and with this better positions can be found. + if (group_points.size() == 2) + { + //If the group only has two members check for the position that is in the middlest of the connectionpath between + //these points or else a random point will be chosen. + double current_point_distance = std::abs(pathlengths[1] - pathlengths[0]); + if (current_pathlength <= (best_pathlength + 0.05) && current_point_distance <= (best_pathlength_point_distance + 0.05) + && downsampled_map.at(downsampling_factor * trolley_position_candidates[best_trolley_candidate]) != 0) + { + best_pathlength_point_distance = current_point_distance; + best_pathlength = current_pathlength; + best_trolley_candidate = candidate; + } + } + else + { + if (current_pathlength <= (best_pathlength + 0.05)) + { + best_pathlength = current_pathlength; + best_trolley_candidate = candidate; + } + } + } + + return trolley_position_candidates[best_trolley_candidate]; +} + +//This function takes all found groups and calculates for each of it the best trolley-position using the previously +//described functions. +std::vector TrolleyPositionFinder::findTrolleyPositions(const cv::Mat& original_map, const std::vector >& found_groups, + const std::vector& room_centers, const double downsampling_factor, const double robot_radius, const double map_resolution) +{ + std::vector < cv::Point > trolley_positions; + + //go trough each group and find the best trolley position. + for (int current_group = 0; current_group < found_groups.size(); current_group++) + { + std::vector < cv::Point > group_points_vector; //vector to save the Points for each group + + //add the Points from the given groups vector + for (int index = 0; index < found_groups[current_group].size(); index++) + { + group_points_vector.push_back(room_centers[found_groups[current_group][index]]); + } + //calculate the trolley-position for each group that has at least 2 members + if (found_groups[current_group].size() > 1) + { + trolley_positions.push_back( + findOneTrolleyPosition(group_points_vector, original_map, downsampling_factor, robot_radius, map_resolution)); + } + else //if the group has only one member this one is the trolley-position + { + cv::Point trolley_position_for_one_sized_groups = room_centers[found_groups[current_group][0]]; + trolley_positions.push_back(trolley_position_for_one_sized_groups); + } + } + + return trolley_positions; +} diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/package.xml b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/package.xml new file mode 100644 index 0000000..938ec99 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/package.xml @@ -0,0 +1,37 @@ + + + ipa_building_navigation + 0.1.0 + + This package plans the navigation trough the building in the autopnp-scenario. It has the following parts/functions: + 1. An A-star pathplanning-algorithm, which finds the shortest way between two Points in a given map. The map should be a gridmap as a OpenCV 8bit grayscale image with 0 as obstacle + and 255 as reachable area. + 2. TSP-solving functions, which solve the TSP-Problem for a given, segmented map. The segmented map comes from the functions in the pakage ipa_room_segmentation with the corresponding roomcenters. These centers need to be visited in minimal time, so a TSP-Solver is applied. There are following algorithms for this implemented: + I. nearest-neighbor: This algorithm takes the current Point and goes to the nearest neighbor, the distance is given by the A-star pathplanner. + II. genetic-solver: This algorithm takes the path from the nearest-neighbor solver and improves it using evolutional methods. For this the given path is seen as parent, which gets 7 children. These children have been mutated, meaning that the path of the parent has been changed randomly. The Mutations can be random switching of centerorder or inverting random parts of the path. After these children has been made the function calculates the length of the path, using the results from the A-star pathplanner, and compares the children and the parent (so 8 paths). The shortest path is chosen and becomes the new parent. This step is done at least 42 times and then the algorithm checks, if the pathlength hasn't changed in the last 10 steps, if so the path is close to the optimal solution. + III. The Concorde-TSP-solver: The TSP-solving-library Concorde is used. See http://www.math.uwaterloo.ca/tsp/concorde/index.html for further information. + + Florian Jordan + Richard Bormann + LGPL for academic and non-commercial use; for commercial use, please contact Fraunhofer IPA + http://ros.org/wiki/ipa_building_navigation + + catkin + + actionlib + boost + cv_bridge + dynamic_reconfigure + geometry_msgs + ipa_building_msgs + + libopencv-dev + + message_generation + message_runtime + roscpp + roslib + sensor_msgs + visualization_msgs + + diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/readme.md b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/readme.md new file mode 100644 index 0000000..f3b8417 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/readme.md @@ -0,0 +1,66 @@ +# ipa_building_navigation +Algorithms for systematic coverage of different spaces in an optimal traveling order. + +If you find this software useful in your work, please cite our corresponding paper: +- R. Bormann, F. Jordan, J. Hampp, and M. Hägele. Indoor coverage path planning: Survey, implementation, analysis. In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), pages 1718–1725, May 2018. https://ieeexplore.ieee.org/abstract/document/8460566 , https://publica.fraunhofer.de/entities/publication/f537c15d-4cbe-4672-9d86-6e756a9ce71b/details + +# General Procedure + +1. Change the algorithm parameters in ros/launch/room_sequence_planning_action_server_params.yaml in ros/launch to the wanted algorithms and settings. + * tsp_solver: Choose which traveling salesman problem (TSP) solver you want to use. + * planning_method: Choose which planning method for the trolley you want to use. + * for both planning methods the parameter **max_clique_path_length** determines how far two rooms can be away from each other until they get separated into two different cliques. + * map_downsampling_factor: The algorithm plans an Astar-path trough the environment to determine distances between the roomcenters that are used as edge weights for the TSP solver. For this you can set this parameter to reduce the size of the map, which speeds up the pathfinding. The originally sized map is checked, if no path for the downsampled map could be found. **Range**: 0. + * + ****************************************************************/ + +#ifndef _DYNAMIC_RECONFIGURE_CLIENT_H_ +#define _DYNAMIC_RECONFIGURE_CLIENT_H_ + +#include +#include + +#include + +#include +#include +#include +#include + +class DynamicReconfigureClient +{ +public: + DynamicReconfigureClient(ros::NodeHandle& nh, const std::string& dynamic_reconfigure_service_name, const std::string& parameter_updates_topic) + : dynamic_reconfigure_current_config_received_(false), node_handle_(nh), dynamic_reconfigure_service_name_(dynamic_reconfigure_service_name) + { + dynamic_reconfigure_sub_ = node_handle_.subscribe(parameter_updates_topic, 1, &DynamicReconfigureClient::dynamic_reconfigure_current_config_callback, this); + + // receive current configuration + ros::Duration sleep_rate(0.5); + while (dynamic_reconfigure_current_config_received_ == false) + { + ros::spinOnce(); + sleep_rate.sleep(); + } + } + + dynamic_reconfigure::Config& getConfig() + { + boost::mutex::scoped_lock lock(dynamic_reconfigure_lock_); + return dynamic_reconfigure_config_; + } + + bool setConfig(const std::string& param_name, const bool param_value) + { + boost::mutex::scoped_lock lock(dynamic_reconfigure_lock_); + + if (dynamic_reconfigure_current_config_received_ == false) + { + ROS_WARN("DynamicReconfigureClient: Did not receive the current configuration, yet."); + return false; + } + + bool found = false; + for (size_t i=0; i. + * + ****************************************************************/ +#pragma once + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +// Dynamic reconfigure +#include +#include + +//TSP solver +#include +#include +#include +#include + +//Set Cover solver to find room groups +#include + +//finder of trolley positions for each room group +#include + +// A* planner +#include + +// action +#include +#include + +class RoomSequencePlanningServer +{ +public: + RoomSequencePlanningServer(ros::NodeHandle nh, std::string name_of_the_action); + + ~RoomSequencePlanningServer() + { + } + +protected: + //!!Important!! + // define the Nodehandle before the action server, or else the server won't start + ros::NodeHandle node_handle_; + + ros::Publisher room_sequence_visualization_pub_; // visualization of the room sequence + visualization_msgs::MarkerArray room_sequence_visualization_msg_; + + actionlib::SimpleActionServer room_sequence_with_checkpoints_server_; + + std::string action_name_; + + //converter-> Pixel to meter for X coordinate + double convert_pixel_to_meter_for_x_coordinate(const int pixel_valued_object_x, const float map_resolution, const cv::Point2d map_origin) + { + double meter_value_obj_x = (pixel_valued_object_x * map_resolution) + map_origin.x; + return meter_value_obj_x; + } + //converter-> Pixel to meter for Y coordinate + double convert_pixel_to_meter_for_y_coordinate(int pixel_valued_object_y, const float map_resolution, const cv::Point2d map_origin) + { + double meter_value_obj_y = (pixel_valued_object_y * map_resolution) + map_origin.y; + return meter_value_obj_y; + } + + // this is the execution function used by action server + void findRoomSequenceWithCheckpointsServer(const ipa_building_msgs::FindRoomSequenceWithCheckpointsGoalConstPtr &goal); + + size_t getNearestLocation(const cv::Mat& floor_plan, const cv::Point start_coordinate, const std::vector& positions, + const double map_downsampling_factor, const double robot_radius, const double map_resolution); + + void publishSequenceVisualization(const std::vector& room_sequences, const std::vector& room_centers, + std::vector< std::vector >& cliques, const double map_resolution, const cv::Point2d& map_origin); + + // callback function for dynamic reconfigure + void dynamic_reconfigure_callback(ipa_building_navigation::BuildingNavigationConfig &config, uint32_t level); + + dynamic_reconfigure::Server room_sequence_planning_dynamic_reconfigure_server_; + + // params + int tsp_solver_; // TSP solver: 1 = Nearest Neighbor, 2 = Genetic solver, 3 = Concorde solver + int problem_setting_; // problem setting of the sequence planning problem + // 1 = SimpleOrderPlanning (plan the optimal order of a simple set of locations) + // 2 = CheckpointBasedPlanning (two-stage planning that creates local cliques of locations (= checkpoints) and determines + // the optimal order through the members of each clique as well as the optimal order through the cliques) + int planning_method_; // Method of planning the sequence: 1 = drag trolley if next room is too far away, 2 = calculate cliques as roomgroups with trolleypositions + double max_clique_path_length_; // max A* path length between two rooms that are assigned to the same clique, in [m] + double map_downsampling_factor_; // the map may be downsampled during computations (e.g. of A* path lengths) in order to speed up the algorithm, range of the factor [0 < factor <= 1], if set to 1 the map will have original size, if set to 0 the algorithm won't work + bool check_accessibility_of_rooms_; // boolean to tell the sequence planner if it should check the given room centers for accessibility from the starting position + bool return_sequence_map_; // boolean to tell the server if the map with the sequence drawn in should be returned + int max_clique_size_; // maximal number of nodes belonging to one clique, when planning trolley positions + bool display_map_; // displays the map with paths upon service call (only if return_sequence_map=true) +}; diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/ros/launch/room_sequence_planning_action_server.launch b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/ros/launch/room_sequence_planning_action_server.launch new file mode 100644 index 0000000..9eea768 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/ros/launch/room_sequence_planning_action_server.launch @@ -0,0 +1,9 @@ + + + + + + + + + \ No newline at end of file diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/ros/launch/room_sequence_planning_action_server_params.yaml b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/ros/launch/room_sequence_planning_action_server_params.yaml new file mode 100644 index 0000000..2af851c --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/ros/launch/room_sequence_planning_action_server_params.yaml @@ -0,0 +1,53 @@ +# TSP Solver +# ========== +# indicates which TSP solver should be used +# 1 = Nearest Neighbor +# 2 = Genetic solver +# 3 = Concorde solver +# int +tsp_solver: 3 + +# Problem Setting +# =============== +# problem setting of the sequence planning problem +# 1 = SimpleOrderPlanning (plan the optimal order of a simple set of locations) +# 2 = CheckpointBasedPlanning (two-stage planning that creates local cliques of locations (= checkpoints) and determines +# the optimal order through the members of each clique as well as the optimal order through the cliques) +# int +problem_setting: 2 + +# Checkpoint-based Sequence Planning Specifics +# ============================================ +# method that is used to plan the trolley positions (only applies to CheckpointBasedPlanning (2) problem_setting) +# 1 = drag the trolley if it is too far away from next room +# 2 = put rooms together in groups and calculate a corresponding trolley positions +# int +planning_method: 2 + +# max A* path length between two rooms that are assigned to the same clique, in [m] (only applies to CheckpointBasedPlanning (2) problem_setting) +# double +max_clique_path_length: 1200.0 + +# maximal nodes in one clique for one trolley position (only applies to CheckpointBasedPlanning (2) problem_setting) +# int +maximum_clique_size: 9001 + +# General Settings +# ================ +# the map may be downsampled during computations (e.g. of A* path lengths) in order to speed up the algorithm, +# range of the factor [0 < factor <= 1] +# if set to 1 the map will have original size, if set to 0 the algorithm won't work +# double +map_downsampling_factor: 0.25 + +# boolean to tell the sequence planner if it should check the given room centers for accessibility from the starting position +# bool +check_accessibility_of_rooms: true + +# boolean to tell the server if the map with the sequence drawn in should be returned +# bool +return_sequence_map: false + +# displays the map with paths upon service call (only if return_sequence_map=true) +# bool +display_map: false diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/ros/src/boosttest.cpp b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/ros/src/boosttest.cpp new file mode 100644 index 0000000..3f00ca1 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/ros/src/boosttest.cpp @@ -0,0 +1,216 @@ +#include "ros/ros.h" +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include +#include + +#include +#include + +#include +#include +#include + +#include + +int main(int argc, char **argv) +{ + srand(time(NULL)); + ros::init(argc, argv, "TSPevaluation"); + ros::NodeHandle nh; + + //define parameters to describe the Graph + const int dimension = 600; + + //define parameters for solving the TSP + double downsampling = 0.25; + double robot_radius = 0.; + double map_resolution = 0.05; + int start_node = 0; + + //saving variables for pathlengths + std::vector nearest_pathlengths; + std::vector genetic_pathlengths; + std::vector concorde_pathlengths; + + //create empty map to random generate Points in it + cv::Mat map(dimension, dimension, CV_8UC1, cv::Scalar(255)); + + //path to save the results + const std::string data_storage_path = "tsp_evaluation/"; + const std::string upper_command = "mkdir -p " + data_storage_path; + int return_value = system(upper_command.c_str()); + + //stingstreams to save the parameters + std::stringstream pathlength_output; + std::stringstream times_output; + + for(int number_of_nodes = 10; number_of_nodes <= 210; number_of_nodes += 20) + { + std::stringstream folder_name; + folder_name << number_of_nodes << "nodes"; + const std::string evaluation_path = data_storage_path + folder_name.str() + "/"; + const std::string command = "mkdir -p " + evaluation_path; + int return_value = system(command.c_str()); + + //generate random Points as nodes for the TSP solvers + int point_counter = 0; + std::vector nodes; + std::cout << "getting random Points" << std::endl; + do + { + int rand_x = (rand() % (dimension-30)) + 30; + int rand_y = (rand() % (dimension-30)) + 30; + if(map.at(rand_y, rand_x) == 255) + { + nodes.push_back(cv::Point(rand_x, rand_y)); + point_counter++; + } + }while(point_counter < number_of_nodes); + + NearestNeighborTSPSolver nearest_solver; + GeneticTSPSolver genetic_solver; + ConcordeTSPSolver concorde_solver; + + //solve the TSPs and save the calculation time and orders + cv::Mat distance_matrix; + struct timespec t0, t1, t2, t3; + + //construct distance matrix once + std::cout << "constructing distance matrix" << std::endl; + AStarPlanner planner; + DistanceMatrix distance_matrix_computation; + distance_matrix_computation.constructDistanceMatrix(distance_matrix, map, nodes, downsampling, robot_radius, map_resolution, planner); + + std::cout << "solving TSPs" << std::endl; + clock_gettime(CLOCK_MONOTONIC, &t0); + std::vector nearest_order = nearest_solver.solveNearestTSP(distance_matrix, start_node); + std::cout << "solved nearest TSP" << std::endl; + clock_gettime(CLOCK_MONOTONIC, &t1); + std::vector genetic_order = genetic_solver.solveGeneticTSP(distance_matrix, start_node); + std::cout << "solved genetic TSP" << std::endl; + clock_gettime(CLOCK_MONOTONIC, &t2); + std::vector concorde_order = concorde_solver.solveConcordeTSP(distance_matrix, start_node); + std::cout << "solved concorde TSP" << std::endl; + clock_gettime(CLOCK_MONOTONIC, &t3); + + std::cout << "number of nodes in the paths: " << nearest_order.size() << " " << genetic_order.size() << " " << concorde_order.size() << std::endl; + + //create maps to draw the paths in + cv::Mat nearest_map = map.clone(); +#if CV_MAJOR_VERSION<=3 + cv::cvtColor(nearest_map, nearest_map, CV_GRAY2BGR); +#else + cv::cvtColor(nearest_map, nearest_map, cv::COLOR_GRAY2BGR); +#endif + cv::Mat genetic_map = nearest_map.clone(); + cv::Mat concorde_map = nearest_map.clone(); + + //draw the order into the maps + // draw the start node as red + std::cout << "starting to draw the maps" << std::endl; +#if CV_MAJOR_VERSION<=3 + cv::circle(nearest_map, nodes[nearest_order[0]], 2, CV_RGB(255,0,0), CV_FILLED); + cv::circle(genetic_map, nodes[genetic_order[0]], 2, CV_RGB(255,0,0), CV_FILLED); + cv::circle(concorde_map, nodes[concorde_order[0]], 2, CV_RGB(255,0,0), CV_FILLED); +#else + cv::circle(nearest_map, nodes[nearest_order[0]], 2, CV_RGB(255,0,0), cv::FILLED); + cv::circle(genetic_map, nodes[genetic_order[0]], 2, CV_RGB(255,0,0), cv::FILLED); + cv::circle(concorde_map, nodes[concorde_order[0]], 2, CV_RGB(255,0,0), cv::FILLED); +#endif + for(size_t i = 1; i < nearest_order.size(); ++i) + { + cv::line(nearest_map, nodes[nearest_order[i-1]], nodes[nearest_order[i]], CV_RGB(128,128,255), 1); + cv::line(genetic_map, nodes[genetic_order[i-1]], nodes[genetic_order[i]], CV_RGB(128,128,255), 1); + cv::line(concorde_map, nodes[concorde_order[i-1]], nodes[concorde_order[i]], CV_RGB(128,128,255), 1); +#if CV_MAJOR_VERSION<=3 + cv::circle(nearest_map, nodes[nearest_order[i]], 2, CV_RGB(0,0,0), CV_FILLED); + cv::circle(genetic_map, nodes[genetic_order[i]], 2, CV_RGB(0,0,0), CV_FILLED); + cv::circle(concorde_map, nodes[concorde_order[i]], 2, CV_RGB(0,0,0), CV_FILLED); +#else + cv::circle(nearest_map, nodes[nearest_order[i]], 2, CV_RGB(0,0,0), cv::FILLED); + cv::circle(genetic_map, nodes[genetic_order[i]], 2, CV_RGB(0,0,0), cv::FILLED); + cv::circle(concorde_map, nodes[concorde_order[i]], 2, CV_RGB(0,0,0), cv::FILLED); +#endif + } + //draw line back to start + cv::line(nearest_map, nodes[nearest_order[0]], nodes[nearest_order.back()], CV_RGB(128,128,255), 1); + cv::line(genetic_map, nodes[genetic_order[0]], nodes[genetic_order.back()], CV_RGB(128,128,255), 1); + cv::line(concorde_map, nodes[concorde_order[0]], nodes[concorde_order.back()], CV_RGB(128,128,255), 1); + + //save the maps + std::string nearest_path = evaluation_path + "nearest_order.png"; + std::string genetic_path = evaluation_path + "genetic_order.png"; + std::string concorde_path = evaluation_path + "concorde_order.png"; + cv::imwrite(nearest_path.c_str(), nearest_map); + cv::imwrite(genetic_path.c_str(), genetic_map); + cv::imwrite(concorde_path.c_str(), concorde_map); + std::cout << "saved the maps" << std::endl; + + //get the pathlengths for each solver + double nearest_pathlength= 0; + double genetic_pathlength = 0; + double concorde_pathlength = 0; + //add each pathlength + std::cout << "starting to calculate the pathlengths " << distance_matrix.cols << std::endl; + for(size_t i = 1; i < nearest_order.size(); ++i) + { + nearest_pathlength += distance_matrix.at(nearest_order[i-1], nearest_order[i]); + genetic_pathlength += distance_matrix.at(genetic_order[i-1], genetic_order[i]); + concorde_pathlength += distance_matrix.at(concorde_order[i-1], concorde_order[i]); + std::cout << "done node: " << (int) i << std::endl; + } + //add path from end to start + std::cout << "doing last paths. Indexes: " << nearest_order.back() << " " << genetic_order.back() << " " << concorde_order.back() << std::endl; + int last_index = nearest_order.back(); + nearest_pathlength += distance_matrix.at(nearest_order[0], last_index); + std::cout << "finished nearest path" << std::endl; + last_index = genetic_order.back(); + genetic_pathlength += distance_matrix.at(genetic_order[0], last_index); + std::cout << "finished genetic path." << std::endl; + last_index = concorde_order.back(); + concorde_pathlength += distance_matrix.at(concorde_order[0], last_index); + std::cout << "finished concorde path" << std::endl; + + //calculate computation times + double nearest_time = (t1.tv_sec - t0.tv_sec) + (double) (t1.tv_nsec - t0.tv_nsec) * 1e-9; + double genetic_time = (t2.tv_sec - t1.tv_sec) + (double) (t2.tv_nsec - t1.tv_nsec) * 1e-9; + double concorde_time = (t3.tv_sec - t2.tv_sec) + (double) (t3.tv_nsec - t2.tv_nsec) * 1e-9; + + //save the pathlengths and computation times + pathlength_output << "number of nodes: " << number_of_nodes << std::endl + << nearest_pathlength << std::endl << genetic_pathlength << std::endl << concorde_pathlength << std::endl << std::endl; + times_output << "number of nodes: " << number_of_nodes << std::endl + << nearest_time << std::endl << genetic_time << std::endl << concorde_time << std::endl << std::endl; + } + std::string pathlength_log_filename = data_storage_path + "pathlengths.txt"; + std::ofstream pathlength_file(pathlength_log_filename.c_str(), std::ios::out); + if (pathlength_file.is_open()==true) + pathlength_file << pathlength_output.str(); + pathlength_file.close(); + std::cout << "finished to save the pathlengths" << std::endl; + + std::string genetic_log_filename = data_storage_path + "times.txt"; + std::ofstream genetic_file(genetic_log_filename.c_str(), std::ios::out); + if (genetic_file.is_open()==true) + genetic_file << times_output.str(); + genetic_file.close(); + std::cout << "finished to save the times" << std::endl; + + + return 0; +} diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/ros/src/room_sequence_planning_action_client.cpp b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/ros/src/room_sequence_planning_action_client.cpp new file mode 100644 index 0000000..e062e69 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/ros/src/room_sequence_planning_action_client.cpp @@ -0,0 +1,142 @@ +#include +#include + +#include +#include + +#include +#include + +#include + +#include +#include +#include +#include + +#include + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "room_sequence_planning_client"); + ros::NodeHandle nh; + + std::vector< std::string > map_names; + map_names.push_back("lab_ipa.png"); +// map_names.push_back("freiburg_building101.png"); + map_names.push_back("freiburg_building52.png"); + map_names.push_back("freiburg_building79.png"); +// map_names.push_back("intel_map.png"); +// map_names.push_back("lab_a.png"); +// map_names.push_back("lab_b.png"); + map_names.push_back("lab_c.png"); + map_names.push_back("lab_d.png"); +// map_names.push_back("lab_e.png"); + + for (size_t image_index = 0; image_index(y, x) != 255) + { + map.at(y, x) = 0; + } + } + } + + sensor_msgs::Image map_msg; + cv_bridge::CvImage cv_image; + // cv_image.header.stamp = ros::Time::now(); + cv_image.encoding = "mono8"; + cv_image.image = map; + cv_image.toImageMsg(map_msg); + // create the action client --> "name of server" + // true causes the client to spin its own thread + actionlib::SimpleActionClient ac_seg("/room_segmentation/room_segmentation_server", true); + ROS_INFO("Waiting for action server '/room_segmentation/room_segmentation_server' to start."); + // wait for the action server to start + ac_seg.waitForServer(); //will wait for infinite time + + // set algorithm parameters + ROS_INFO("Action server started, sending goal."); + DynamicReconfigureClient drc_seg(nh, "/room_segmentation/room_segmentation_server/set_parameters", "/room_segmentation/room_segmentation_server/parameter_updates"); + drc_seg.setConfig("room_segmentation_algorithm", 1); + + // send a goal to the action + ipa_building_msgs::MapSegmentationGoal goal_seg; + goal_seg.input_map = map_msg; + goal_seg.map_origin.position.x = 0; + goal_seg.map_origin.position.y = 0; + goal_seg.map_resolution = 0.05; + goal_seg.return_format_in_meter = false; + goal_seg.return_format_in_pixel = true; + ac_seg.sendGoal(goal_seg); + + //wait for the action to return + bool finished_before_timeout = ac_seg.waitForResult(ros::Duration(300.0)); + if (finished_before_timeout == false) + { + ROS_ERROR("Timeout on room segmentation."); + return -1; + } + ipa_building_msgs::MapSegmentationResultConstPtr result_seg = ac_seg.getResult(); + ROS_INFO("Finished segmentation successfully!"); + + // solve sequence problem + actionlib::SimpleActionClient ac_seq("/room_sequence_planning/room_sequence_planning_server", true); + ROS_INFO("Waiting for action server '/room_sequence_planning/room_sequence_planning_server' to start."); + // wait for the action server to start + ac_seq.waitForServer(); //will wait for infinite time + + // set algorithm parameters + ROS_INFO("Action server started, sending goal_seq."); + DynamicReconfigureClient drc_seq(nh, "/room_sequence_planning/room_sequence_planning_server/set_parameters", "/room_sequence_planning/room_sequence_planning_server/parameter_updates"); + drc_seq.setConfig("planning_method", 1); + drc_seq.setConfig("tsp_solver", 3); + drc_seq.setConfig("return_sequence_map", true); + drc_seq.setConfig("display_map", true); + + // send a goal_seg to the action + ipa_building_msgs::FindRoomSequenceWithCheckpointsGoal goal_seq; + goal_seq.input_map = map_msg; + goal_seq.map_resolution = goal_seg.map_resolution; + goal_seq.map_origin.position.x = goal_seg.map_origin.position.x; + goal_seq.map_origin.position.y = goal_seg.map_origin.position.y; + goal_seq.room_information_in_pixel = result_seg->room_information_in_pixel; + goal_seq.robot_radius = 0.3; + cv::Mat map_eroded; + cv::erode(map, map_eroded, cv::Mat(), cv::Point(-1,-1), goal_seq.robot_radius/goal_seq.map_resolution+2); + cv::Mat distance_map; //variable for the distance-transformed map, type: CV_32FC1 + cv::distanceTransform(map_eroded, distance_map, CV_DIST_L2, 5); + cv::convertScaleAbs(distance_map, distance_map); // conversion to 8 bit image + bool robot_start_coordinate_set = false; + for (int v=0; v(v,u) != 0 && distance_map.at(v,u) > 15) + { + goal_seq.robot_start_coordinate.position.x = u*goal_seq.map_resolution + goal_seg.map_origin.position.x; + goal_seq.robot_start_coordinate.position.y = v*goal_seq.map_resolution + goal_seg.map_origin.position.y; + robot_start_coordinate_set = true; + } + ac_seq.sendGoal(goal_seq); + + //wait for the action to return + finished_before_timeout = ac_seq.waitForResult(ros::Duration(300.0)); + if (finished_before_timeout == false) + { + ROS_ERROR("Timeout on room sequence planning."); + return -1; + } + ROS_INFO("Finished sequence planning successfully!"); + + } + + //exit + return 0; +} diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/ros/src/room_sequence_planning_action_server.cpp b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/ros/src/room_sequence_planning_action_server.cpp new file mode 100644 index 0000000..df4f92a --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/ros/src/room_sequence_planning_action_server.cpp @@ -0,0 +1,901 @@ +/*! + ***************************************************************** + * \file + * + * \note + * Copyright (c) 2015 \n + * Fraunhofer Institute for Manufacturing Engineering + * and Automation (IPA) \n\n + * + ***************************************************************** + * + * \note + * Project name: Care-O-bot + * \note + * ROS stack name: autopnp + * \note + * ROS package name: ipa_building_navigation + * + * \author + * Author: Florian Jordan + * \author + * Supervised by: Richard Bormann + * + * \date Date of creation: 08.2015 + * + * \brief + * + * + ***************************************************************** + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * - Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. \n + * - Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. \n + * - Neither the name of the Fraunhofer Institute for Manufacturing + * Engineering and Automation (IPA) nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. \n + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License LGPL as + * published by the Free Software Foundation, either version 3 of the + * License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License LGPL for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License LGPL along with this program. + * If not, see . + * + ****************************************************************/ + +#include + +RoomSequencePlanningServer::RoomSequencePlanningServer(ros::NodeHandle nh, std::string name_of_the_action) : + node_handle_(nh), + room_sequence_with_checkpoints_server_(node_handle_, name_of_the_action, boost::bind(&RoomSequencePlanningServer::findRoomSequenceWithCheckpointsServer, this, _1), false), + action_name_(name_of_the_action) +{ + // setup publishers + room_sequence_visualization_pub_ = nh.advertise("room_sequence_marker", 1); + + // start action server + room_sequence_with_checkpoints_server_.start(); + + // dynamic reconfigure + room_sequence_planning_dynamic_reconfigure_server_.setCallback(boost::bind(&RoomSequencePlanningServer::dynamic_reconfigure_callback, this, _1, _2)); + + // Parameters + std::cout << "\n------------------------------------\nRoom Sequence Planner Parameters:\n------------------------------------\n"; + // TSP solver + node_handle_.param("tsp_solver", tsp_solver_, (int)TSP_CONCORDE); + std::cout << "room_sequence_planning/tsp_solver = " << tsp_solver_ << std::endl; + if (tsp_solver_ == TSP_NEAREST_NEIGHBOR) + ROS_INFO("You have chosen the Nearest Neighbor TSP method."); + else if (tsp_solver_ == TSP_GENETIC) + ROS_INFO("You have chosen the Genetic TSP method."); + else if (tsp_solver_ == TSP_CONCORDE) + ROS_INFO("You have chosen the Concorde TSP solver."); + else + ROS_ERROR("Undefined TSP Solver."); + + // problem setting + node_handle_.param("problem_setting", problem_setting_, 2); + std::cout << "room_sequence_planning/problem_setting = " << problem_setting_ << std::endl; + if (problem_setting_ == 1) + ROS_INFO("You have chosen the SimpleOrderPlanning setting."); + else if (problem_setting_ == 2) + ROS_INFO("You have chosen the CheckpointBasedPlanning setting."); + else + ROS_ERROR("Undefined problem setting."); + + // checkpoint-based sequence planning specifics + if (problem_setting_ == 1) + { + planning_method_ = 1; + max_clique_path_length_ = 0.; // always split into cliques --> each clique contains only one room + max_clique_size_ = 1; // always split into cliques --> each clique contains only one room + } + else if (problem_setting_ == 2) + { + node_handle_.param("planning_method", planning_method_, 2); + std::cout << "room_sequence_planning/planning_method = " << planning_method_ << std::endl; + if (planning_method_ == 1) + ROS_INFO("You have chosen the Trolley dragging method."); + else if (planning_method_ == 2) + ROS_INFO("You have chosen the room group planning method."); + else + ROS_ERROR("Undefined planning method."); + node_handle_.param("max_clique_path_length", max_clique_path_length_, 12.0); + std::cout << "room_sequence_planning/max_clique_path_length = " << max_clique_path_length_ << std::endl; + node_handle_.param("maximum_clique_size", max_clique_size_, 9001); + std::cout << "room_sequence_planning/maximum_clique_size = " << max_clique_size_ << std::endl; + } + else + ROS_ERROR("Undefined problem setting."); + + // general settings + node_handle_.param("map_downsampling_factor", map_downsampling_factor_, 0.25); + std::cout << "room_sequence_planning/map_downsampling_factor = " << map_downsampling_factor_ << std::endl; + node_handle_.param("check_accessibility_of_rooms", check_accessibility_of_rooms_, true); + std::cout << "room_sequence_planning/check_accessibility_of_rooms = " << check_accessibility_of_rooms_ << std::endl; + node_handle_.param("return_sequence_map", return_sequence_map_, false); + std::cout << "room_sequence_planning/return_sequence_map = " << return_sequence_map_ << std::endl; + node_handle_.param("display_map", display_map_, false); + std::cout << "room_sequence_planning/display_map = " << display_map_ << std::endl; +} + +// callback function for dynamic reconfigure +void RoomSequencePlanningServer::dynamic_reconfigure_callback(ipa_building_navigation::BuildingNavigationConfig &config, uint32_t level) +{ + std::cout << "######################################################################################" << std::endl; + std::cout << "Dynamic reconfigure request:" << std::endl; + + // TSP solver + tsp_solver_ = config.tsp_solver; + std::cout << "room_sequence_planning/tsp_solver = " << tsp_solver_ << std::endl; + if (tsp_solver_ == TSP_NEAREST_NEIGHBOR) + ROS_INFO("You have chosen the Nearest Neighbor TSP method."); + else if (tsp_solver_ == TSP_GENETIC) + ROS_INFO("You have chosen the Genetic TSP method."); + else if (tsp_solver_ == TSP_CONCORDE) + ROS_INFO("You have chosen the Concorde TSP solver."); + else + ROS_ERROR("Undefined TSP Solver."); + + // problem setting + problem_setting_ = config.problem_setting; + std::cout << "room_sequence_planning/problem_setting = " << problem_setting_ << std::endl; + if (problem_setting_ == 1) + ROS_INFO("You have chosen the SimpleOrderPlanning setting."); + else if (problem_setting_ == 2) + ROS_INFO("You have chosen the CheckpointBasedPlanning setting."); + else + ROS_ERROR("Undefined problem setting."); + + // checkpoint-based sequence planning specifics + if (problem_setting_ == 1) + { + planning_method_ = 1; + max_clique_path_length_ = 0.; // always split into cliques --> each clique contains only one room + max_clique_size_ = 1; // always split into cliques --> each clique contains only one room + } + else if (problem_setting_ == 2) + { + planning_method_ = config.planning_method; + std::cout << "room_sequence_planning/planning_method = " << planning_method_ << std::endl; + if (planning_method_ == 1) + ROS_INFO("You have chosen the Trolley dragging method."); + else if (planning_method_ == 2) + ROS_INFO("You have chosen the room group planning method."); + else + ROS_ERROR("Undefined planning method."); + max_clique_path_length_ = config.max_clique_path_length; + std::cout << "room_sequence_planning/max_clique_path_length = " << max_clique_path_length_ << std::endl; + max_clique_size_ = config.maximum_clique_size; + std::cout << "room_sequence_planning/maximum_clique_size = " << max_clique_size_ << std::endl; + } + else + ROS_ERROR("Undefined problem setting."); + + // general settings + map_downsampling_factor_ = config.map_downsampling_factor; + std::cout << "room_sequence_planning/map_downsampling_factor = " << map_downsampling_factor_ << std::endl; + check_accessibility_of_rooms_ = config.check_accessibility_of_rooms; + std::cout << "room_sequence_planning/check_accessibility_of_rooms = " << check_accessibility_of_rooms_ << std::endl; + return_sequence_map_ = config.return_sequence_map; + std::cout << "room_sequence_planning/return_sequence_map = " << return_sequence_map_ << std::endl; + display_map_ = config.display_map; + std::cout << "room_sequence_planning/display_map = " << display_map_ << std::endl; +} + +void RoomSequencePlanningServer::findRoomSequenceWithCheckpointsServer(const ipa_building_msgs::FindRoomSequenceWithCheckpointsGoalConstPtr &goal) +{ + ROS_INFO("********Sequence planning started************"); + + // converting the map msg in cv format + cv_bridge::CvImagePtr cv_ptr_obj; + cv_ptr_obj = cv_bridge::toCvCopy(goal->input_map, sensor_msgs::image_encodings::MONO8); + cv::Mat floor_plan = cv_ptr_obj->image; + + //get map origin and convert robot start coordinate to [pixel] + const cv::Point2d map_origin(goal->map_origin.position.x, goal->map_origin.position.y); + cv::Point robot_start_coordinate((goal->robot_start_coordinate.position.x - map_origin.x)/goal->map_resolution, (goal->robot_start_coordinate.position.y - map_origin.y)/goal->map_resolution); + + + //create a star pathplanner to plan a path from Point A to Point B in a given gridmap + AStarPlanner a_star_path_planner; + + //get room centers and check how many of them are reachable + cv::Mat downsampled_map_for_accessibility_checking; + if(check_accessibility_of_rooms_ == true) + { + a_star_path_planner.downsampleMap(floor_plan, downsampled_map_for_accessibility_checking, map_downsampling_factor_, goal->robot_radius, goal->map_resolution); + } + std::vector room_centers; // collect the valid, accessible room_centers + std::map mapping_room_centers_index_to_original_room_index; // maps the index of each entry in room_centers to the original index in goal->room_information_in_pixel + for (size_t i=0; iroom_information_in_pixel.size(); ++i) + { + cv::Point current_center(goal->room_information_in_pixel[i].room_center.x, goal->room_information_in_pixel[i].room_center.y); + if(check_accessibility_of_rooms_ == true) + { + std::cout << "checking for accessibility of rooms" << std::endl; + double length = a_star_path_planner.planPath(floor_plan, downsampled_map_for_accessibility_checking, robot_start_coordinate, current_center, map_downsampling_factor_, 0., goal->map_resolution); + if(length < 1e9) + { + room_centers.push_back(current_center); + mapping_room_centers_index_to_original_room_index[room_centers.size()-1] = i; + std::cout << "room " << i << " added, center: " << current_center << std::endl; + } + else + std::cout << "room " << i << " not accessible, center: " << current_center << std::endl; + } + else + { + room_centers.push_back(current_center); + } + } + downsampled_map_for_accessibility_checking.release(); //release not anymore needed space + + std::cout << "number of reachable roomcenters: " << room_centers.size() << std::endl; + + if(room_centers.size() == 0) + { + ROS_ERROR("No given roomcenter reachable from starting position."); + ipa_building_msgs::FindRoomSequenceWithCheckpointsResult action_result; + room_sequence_with_checkpoints_server_.setAborted(action_result); + return; + } + + if(planning_method_ > 0 && planning_method_ < 3) + { + if(planning_method_ == 1) + ROS_INFO("You have chosen the drag planning method."); + if(planning_method_ == 2) + ROS_INFO("You have chosen the grouping planning method."); + } + + if(tsp_solver_ > 0 && tsp_solver_ < 4) + { + if(tsp_solver_ == TSP_NEAREST_NEIGHBOR) + ROS_INFO("You have chosen the nearest neighbor solver."); + if(tsp_solver_ == TSP_GENETIC) + ROS_INFO("You have chosen the genetic TSP solver."); + if(tsp_solver_ == TSP_CONCORDE) + ROS_INFO("You have chosen the concorde TSP solver."); + } + //saving vectors needed from both planning methods + std::vector > cliques; + std::vector trolley_positions; + std::vector< std::vector > room_cliques_as_points; + + //image container to draw the sequence in if needed + cv::Mat display; + + if(planning_method_ == 1) //Drag Trolley if the next room is too far away + { + std::cout << "Maximal cliquedistance [m]: "<< max_clique_path_length_ << " Maximal cliquedistance [Pixel]: "<< max_clique_path_length_/goal->map_resolution << std::endl; + + //calculate the index of the best starting position + size_t optimal_start_position = getNearestLocation(floor_plan, robot_start_coordinate, room_centers, map_downsampling_factor_, goal->robot_radius, goal->map_resolution); + + //plan the optimal path trough all given rooms + std::vector optimal_room_sequence; + if(tsp_solver_ == TSP_NEAREST_NEIGHBOR) //nearest neighbor TSP solver + { + NearestNeighborTSPSolver nearest_neighbor_tsp_solver; + optimal_room_sequence = nearest_neighbor_tsp_solver.solveNearestTSP(floor_plan, room_centers, map_downsampling_factor_, goal->robot_radius, goal->map_resolution, (int) optimal_start_position); + } + if(tsp_solver_ == TSP_GENETIC) //genetic TSP solver + { + GeneticTSPSolver genetic_tsp_solver; + optimal_room_sequence = genetic_tsp_solver.solveGeneticTSP(floor_plan, room_centers, map_downsampling_factor_, goal->robot_radius, goal->map_resolution, (int) optimal_start_position); + } + if(tsp_solver_ == TSP_CONCORDE) //concorde TSP solver + { + ConcordeTSPSolver concorde_tsp_solver; + optimal_room_sequence = concorde_tsp_solver.solveConcordeTSP(floor_plan, room_centers, map_downsampling_factor_, goal->robot_radius, goal->map_resolution, (int) optimal_start_position); + } + + //put the rooms that are close enough together into the same clique, if a new clique is needed put the first roomcenter as a trolleyposition + std::vector current_clique; + trolley_positions.push_back(robot_start_coordinate); //trolley stands close to robot on startup + //sample down map one time to reduce calculation time + cv::Mat downsampled_map; + a_star_path_planner.downsampleMap(floor_plan, downsampled_map, map_downsampling_factor_, goal->robot_radius, goal->map_resolution); + const double one_by_downsampling_factor = 1 / map_downsampling_factor_; + for(size_t i=0; imap_resolution); + if (distance_to_trolley <= max_clique_path_length_/goal->map_resolution && current_clique.size() < max_clique_size_) //expand current clique by next roomcenter + { + current_clique.push_back(optimal_room_sequence[i]); + } + else //start new clique and put the old clique into the cliques vector + { + cliques.push_back(current_clique); + current_clique.clear(); + current_clique.push_back(optimal_room_sequence[i]); + trolley_positions.push_back(room_centers[optimal_room_sequence[i]]); + } + } + //add last clique + cliques.push_back(current_clique); + + //fill vector of cv::Point for display purpose + for(size_t i=0; i current_clique; + for(size_t j=0; j0) + { +#if CV_MAJOR_VERSION<=3 + cv::circle(display, trolley_positions[t], 5, CV_RGB(0,0,255), CV_FILLED); +#else + cv::circle(display, trolley_positions[t], 5, CV_RGB(0,0,255), cv::FILLED); +#endif + cv::line(display, trolley_positions[t], trolley_positions[t-1], CV_RGB(128,128,255), 1); + } + else + { +#if CV_MAJOR_VERSION<=3 + cv::circle(display, trolley_positions[t], 5, CV_RGB(255,0,0), CV_FILLED); +#else + cv::circle(display, trolley_positions[t], 5, CV_RGB(255,0,0), cv::FILLED); +#endif + } + + // room positions and connections + for (size_t r=0; rmap_resolution << std::endl; + + std::cout << "finding trolley positions" << std::endl; + // 1. determine cliques of rooms + SetCoverSolver set_cover_solver; + cliques = set_cover_solver.solveSetCover(floor_plan, room_centers, map_downsampling_factor_, goal->robot_radius, goal->map_resolution, max_clique_path_length_/goal->map_resolution, max_clique_size_); + + // 2. determine trolley position within each clique (same indexing as in cliques) + TrolleyPositionFinder trolley_position_finder; + trolley_positions = trolley_position_finder.findTrolleyPositions(floor_plan, cliques, room_centers, map_downsampling_factor_, goal->robot_radius, goal->map_resolution); + std::cout << "Trolley positions within each clique computed" << std::endl; + + // 3. determine optimal sequence of trolley positions (solve TSP problem) + // a) find nearest trolley location to current robot location + // b) solve the TSP for the trolley positions + // reduce image size already here to avoid resizing in the planner each time + size_t optimal_trolley_start_position = getNearestLocation(floor_plan, robot_start_coordinate, trolley_positions, map_downsampling_factor_, goal->robot_radius, goal->map_resolution); + + //solve the TSP + std::vector optimal_trolley_sequence; + std::cout << "finding optimal trolley sequence. Start: " << optimal_trolley_start_position << std::endl; + if(tsp_solver_ == TSP_NEAREST_NEIGHBOR) //nearest neighbor TSP solver + { + NearestNeighborTSPSolver nearest_neighbor_tsp_solver; + optimal_trolley_sequence = nearest_neighbor_tsp_solver.solveNearestTSP(floor_plan, trolley_positions, map_downsampling_factor_, goal->robot_radius, goal->map_resolution, (int) optimal_trolley_start_position); + } + if(tsp_solver_ == TSP_GENETIC) //genetic TSP solver + { + GeneticTSPSolver genetic_tsp_solver; + optimal_trolley_sequence = genetic_tsp_solver.solveGeneticTSP(floor_plan, trolley_positions, map_downsampling_factor_, goal->robot_radius, goal->map_resolution, (int) optimal_trolley_start_position); + } + if(tsp_solver_ == TSP_CONCORDE) //concorde TSP solver + { + ConcordeTSPSolver concorde_tsp_solver; + optimal_trolley_sequence = concorde_tsp_solver.solveConcordeTSP(floor_plan, trolley_positions, map_downsampling_factor_, goal->robot_radius, goal->map_resolution, (int) optimal_trolley_start_position); + } + + // 4. determine optimal sequence of rooms with each clique (solve TSP problem) + // a) find start point for each clique closest to the trolley position + // b) create a vector< vector > to fill the groups into the TSP solver + // c) solve the TSP for each clique + + + //fill vector of cv::Point for TSP solver + for(size_t i=0; i current_clique; + for(size_t j=0; j clique_starting_points(cliques.size()); + for(size_t i=0; irobot_radius, goal->map_resolution); + //solve TSPs + std::vector< std::vector > optimal_room_sequences(cliques.size()); + // Create the TSP solver objects two times and not one time for the whole function because by this way only two objects has to be + // created and else three + if(tsp_solver_ == TSP_NEAREST_NEIGHBOR) //nearest neighbor TSP solver + { + NearestNeighborTSPSolver nearest_neighbor_tsp_solver; + for(size_t i=0; irobot_radius, goal->map_resolution, clique_starting_points[i]); + std::cout << "done one clique" << std::endl; + } + } + if(tsp_solver_ == TSP_GENETIC) //genetic TSP solver + { + GeneticTSPSolver genetic_tsp_solver; + for(size_t i=0; irobot_radius, goal->map_resolution, clique_starting_points[i]); + std::cout << "done one clique" << std::endl; + } + } + if(tsp_solver_ == TSP_CONCORDE) //concorde TSP solver + { + ConcordeTSPSolver concorde_tsp_solver; + for(size_t i=0; irobot_radius, goal->map_resolution, clique_starting_points[i]); + std::cout << "done one clique" << std::endl; + } + } + + if(return_sequence_map_ == true) + { + cv::cvtColor(floor_plan, display, CV_GRAY2BGR); + + for (size_t t=0; t0) + { +#if CV_MAJOR_VERSION<=3 + cv::circle(display, trolley_positions[ot], 5, CV_RGB(0,0,255), CV_FILLED); +#else + cv::circle(display, trolley_positions[ot], 5, CV_RGB(0,0,255), cv::FILLED); +#endif + cv::line(display, trolley_positions[ot], trolley_positions[optimal_trolley_sequence[t-1]], CV_RGB(128,128,255), 1); + } + else + { +#if CV_MAJOR_VERSION<=3 + cv::circle(display, trolley_positions[ot], 5, CV_RGB(255,0,0), CV_FILLED); +#else + cv::circle(display, trolley_positions[ot], 5, CV_RGB(255,0,0), cv::FILLED); +#endif + } + + // room positions and connections + for (size_t r=0; r > new_cliques_order(cliques.size()); + std::vector new_trolley_positions(trolley_positions.size()); + for (size_t i=0; i room_sequences(cliques.size()); + for(size_t i=0; imap_resolution, map_origin); + room_sequences[i].checkpoint_position_in_meter.y = convert_pixel_to_meter_for_y_coordinate(trolley_positions[i].y, goal->map_resolution, map_origin); + room_sequences[i].checkpoint_position_in_meter.z = 0.; + } + action_result.checkpoints = room_sequences; + if(return_sequence_map_ == true) + { + //converting the cv format in map msg format + cv_bridge::CvImage cv_image; + cv_image.header.stamp = ros::Time::now(); + cv_image.encoding = "bgr8"; + cv_image.image = display; + cv_image.toImageMsg(action_result.sequence_map); + } + + // publish visualization msg for RViz + publishSequenceVisualization(room_sequences, room_centers, cliques, goal->map_resolution, cv::Point2d(goal->map_origin.position.x, goal->map_origin.position.y)); + + room_sequence_with_checkpoints_server_.setSucceeded(action_result); + + //garbage collection + action_result.checkpoints.clear(); + + ROS_INFO("********Sequence planning finished************"); +} + +size_t RoomSequencePlanningServer::getNearestLocation(const cv::Mat& floor_plan, const cv::Point start_coordinate, const std::vector& positions, + const double map_downsampling_factor, const double robot_radius, const double map_resolution) +{ +// const double one_by_downsampling_factor = 1./map_downsampling_factor; + cv::Mat downsampled_map; + AStarPlanner a_star_path_planner; + a_star_path_planner.downsampleMap(floor_plan, downsampled_map, map_downsampling_factor, robot_radius, map_resolution); + //find nearest trolley position as start point for TSP + double min_dist = 1e10; + size_t nearest_position = 0; +// const cv::Point start_point = map_downsampling_factor * start_coordinate; + for (size_t i=0; i& room_sequences, const std::vector& room_centers, + std::vector< std::vector >& cliques, const double map_resolution, const cv::Point2d& map_origin) +{ + room_sequence_visualization_msg_.markers.clear(); + + // publish clique centers + int room_count = 0; + for (size_t i=0; i0) + { + visualization_msgs::Marker arrow; + // Set the frame ID and timestamp. See the TF tutorials for information on these. + arrow.header.frame_id = "/map"; + arrow.header.stamp = ros::Time::now(); + // Set the namespace and id for this marker. This serves to create a unique ID + // Any marker sent with the same namespace and id will overwrite the old one + arrow.ns = "room_sequence_checkpoints"; + arrow.id = 2*room_sequences.size()+i; + // Set the marker type. Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER + arrow.type = visualization_msgs::Marker::ARROW; + // Set the marker action. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL) + arrow.action = visualization_msgs::Marker::ADD; + // Set the pose of the marker. This is a full 6DOF pose relative to the frame/time specified in the header + arrow.pose.position.x = 0; + arrow.pose.position.y = 0; + arrow.pose.position.z = 0; + arrow.pose.orientation.x = 0.0; + arrow.pose.orientation.y = 0.0; + arrow.pose.orientation.z = 0.0; + arrow.pose.orientation.w = 1.0; + arrow.points.resize(2); + arrow.points[0].x = room_sequences[i-1].checkpoint_position_in_meter.x; + arrow.points[0].y = room_sequences[i-1].checkpoint_position_in_meter.y; + arrow.points[0].z = room_sequences[i-1].checkpoint_position_in_meter.z; + arrow.points[1].x = room_sequences[i].checkpoint_position_in_meter.x; + arrow.points[1].y = room_sequences[i].checkpoint_position_in_meter.y; + arrow.points[1].z = room_sequences[i].checkpoint_position_in_meter.z; + // Set the scale of the marker -- 1x1x1 here means 1m on a side + arrow.scale.x = 0.03; // this is the line width + arrow.scale.y = 0.06; + arrow.scale.z = 0.1; + // Set the color -- be sure to set alpha to something non-zero! + arrow.color.r = 1.0f; + arrow.color.g = 1.0f; + arrow.color.b = 0.0f; + arrow.color.a = 0.8; + arrow.lifetime = ros::Duration(); + room_sequence_visualization_msg_.markers.push_back(arrow); + } + room_count += room_sequences[i].room_indices.size(); + } + + // publish room centers + int room_index = 0; + for (size_t i=0; i +#include + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + + +struct EvaluationConfig +{ + int room_segmentation_algorithm_; // this variable selects the algorithm for room segmentation + // 1 = morphological segmentation + // 2 = distance segmentation + // 3 = Voronoi segmentation + // 4 = semantic segmentation + // 5 = Voronoi random field segmentation + double max_clique_path_length_; // max A* path length between two rooms that are assigned to the same clique, in [m] + int sequence_planning_method_; // Method for sequence planning + // 1 = drag trolley if next room is too far away + // 2 = calculate roomgroups and a trolleyposition for each of it + int tsp_solver_; // TSP solver that is used + // TSP_NEAREST_NEIGHBOR=1 = Nearest Neighbor + // TSP_GENETIC=2 = Genetic solver + // TSP_CONCORDE=3 = Concorde solver + int trashbins_per_trolley_; // variable that shows how many trashbins can be emptied into one trolley + + EvaluationConfig() + { + room_segmentation_algorithm_ = 1; + max_clique_path_length_ = 12.0; + sequence_planning_method_ = 2; + tsp_solver_ = TSP_CONCORDE; + trashbins_per_trolley_ = 9001; + } + + EvaluationConfig(const int room_segmentation_algorithm, const double max_clique_path_length, const int sequence_planning_method, const int tsp_solver) + { + room_segmentation_algorithm_ = room_segmentation_algorithm; + max_clique_path_length_ = max_clique_path_length; + sequence_planning_method_ = sequence_planning_method; + tsp_solver_ = tsp_solver; + trashbins_per_trolley_ = 9001; + } + + EvaluationConfig(const int room_segmentation_algorithm, const double max_clique_path_length, const int sequence_planning_method, const int tsp_solver, const double trashbins_per_trolley) + { + room_segmentation_algorithm_ = room_segmentation_algorithm; + max_clique_path_length_ = max_clique_path_length; + sequence_planning_method_ = sequence_planning_method; + tsp_solver_ = tsp_solver; + trashbins_per_trolley_ = trashbins_per_trolley; + } + + std::string generateUpperConfigurationFolderString() const + { + std::stringstream ss; + ss << "plmth" << sequence_planning_method_ << "mcl" << std::setprecision(4) << max_clique_path_length_; + + return ss.str(); + } + + std::string generateLowerConfigurationFolderString() const + { + std::stringstream ss; + ss << "seg" << room_segmentation_algorithm_ << "tsp" << tsp_solver_; + return ss.str(); + } + + std::string getConfigurationString() const + { + std::stringstream ss; + ss << "\n==================================================================================================\n" << + "Configuration " << generateUpperConfigurationFolderString() << generateLowerConfigurationFolderString() << "\n" << + "==================================================================================================\n" << + "room_segmentation_algorithm: " << roomSegmentationAlgorithmToString() << " (" << room_segmentation_algorithm_ << ")\n" << + "max_clique_path_length [m]: " << max_clique_path_length_ << + "\n" << std::endl; + + return ss.str(); + } + + std::string roomSegmentationAlgorithmToString() const + { + std::string s = ""; + if (room_segmentation_algorithm_ == 1) + s = "morphological segmentation"; + else if (room_segmentation_algorithm_ == 2) + s = "distance segmentation"; + else if (room_segmentation_algorithm_ == 3) + s = "Voronoi segmentation"; + else if (room_segmentation_algorithm_ == 4) + s = "semantic segmentation"; + else if (room_segmentation_algorithm_ == 5) + s = "Voronoi random field segmentation"; + return s; + } +}; + +struct EvaluationData +{ + std::string map_name_; // without file type + cv::Mat floor_plan_; + double map_downsampling_factor_; + float map_resolution_; + geometry_msgs::Pose map_origin_; + geometry_msgs::Pose robot_start_position_; + double robot_radius_; + std::vector< cv::Point > trash_bin_locations_; + cv::Point central_trolley_park_; + + EvaluationData() + { + map_name_ = ""; + floor_plan_ = cv::Mat(); + map_downsampling_factor_ = 0.25; + map_resolution_ = 0.05; + robot_radius_ = 0.3; + } + + EvaluationData(const std::string& map_name, const cv::Mat& floor_plan, const float map_resolution, const double map_downsampling_factor, + const double robot_radius, const std::vector< cv::Point >& trash_bin_locations, const cv::Point trolley_park_position = cv::Point(-1, -1)) + { + map_name_ = map_name; + floor_plan_ = floor_plan; + map_downsampling_factor_ = map_downsampling_factor; + map_resolution_ = map_resolution; + map_origin_.position.x = 0; + map_origin_.position.y = 0; + robot_radius_ = robot_radius; + trash_bin_locations_ = trash_bin_locations; + cv::Mat map_eroded; + cv::erode(floor_plan_, map_eroded, cv::Mat(), cv::Point(-1,-1), robot_radius_/map_resolution_+2); + cv::Mat distance_map; //variable for the distance-transformed map, type: CV_32FC1 + cv::distanceTransform(map_eroded, distance_map, CV_DIST_L2, 5); + cv::convertScaleAbs(distance_map, distance_map); // conversion to 8 bit image + bool robot_start_coordinate_set = false; + for (int v=0; v(v,u) != 0 && distance_map.at(v,u) > 20) // todo: parameter + { + robot_start_position_.position.x = u*map_resolution_ + map_origin_.position.x; + robot_start_position_.position.y = v*map_resolution_ + map_origin_.position.y; + robot_start_coordinate_set = true; + } + central_trolley_park_ = trolley_park_position; + } +}; + +struct StatisticsItem +{ + double robot_speed_without_trolley; // in [m/s] + double robot_speed_with_trolley; // in [m/s] + double time_for_trashbin_manipulation; // in [s] + double time_for_trolley_manipulation; // in [s] + int number_trash_bins; + int number_trolley_movements; + double path_length_robot; // in [m] + double path_length_trolley; // in [m] + double path_length_trash_bins; // in [m] + double pathlength; // in [m] + double cleaning_time; // in [s] + double calculation_time_segmentation; // in [s] + double calculation_time_sequencer; // in [s] + double human_way; // in [m] + + StatisticsItem() + { + robot_speed_without_trolley=0.; // in [m/s] + robot_speed_with_trolley=0.; // in [m/s] + time_for_trashbin_manipulation=0.; // in [s] + time_for_trolley_manipulation=0.; // in [s] + number_trash_bins=0; + number_trolley_movements=0; + path_length_robot=0.; // in [m] + path_length_trolley=0.; // in [m] + path_length_trash_bins=0.; // in [m] + pathlength=1e10; // in [m] + cleaning_time=1e10; // in [s] + calculation_time_segmentation=0.; // in [s] + calculation_time_sequencer=0.; + human_way=0.; + } +}; + +class Evaluation +{ +public: + + // to segment the map only once for each segmentation algorithm + ipa_building_msgs::MapSegmentationResultConstPtr result_seg_morph; + bool segmented_morph; + ipa_building_msgs::MapSegmentationResultConstPtr result_seg_dist; + bool segmented_dist; + ipa_building_msgs::MapSegmentationResultConstPtr result_seg_vor; + bool segmented_vor; + ipa_building_msgs::MapSegmentationResultConstPtr result_seg_semant; + bool segmented_semant; + ipa_building_msgs::MapSegmentationResultConstPtr result_seg_vrf; + bool segmented_vrf; + + Evaluation(ros::NodeHandle& nh, const std::string& test_map_path, const std::string& data_storage_path, const double robot_radius) + : node_handle_(nh), robot_radius_(robot_radius) + { + segmented_morph = false; + segmented_dist = false; + segmented_vor = false; + segmented_semant = false; + segmented_vrf = false; + // prepare relevant floor map data +// std::vector map_names; +//// map_names.push_back("lab_ipa"); // done +//// map_names.push_back("Freiburg52_scan"); //done +//// map_names.push_back("Freiburg79_scan"); //done +//// map_names.push_back("lab_c_scan"); //done +//// map_names.push_back("lab_b_scan"); //done +//// map_names.push_back("lab_d_scan"); //done +//// map_names.push_back("lab_intel"); //done +//// map_names.push_back("Freiburg101_scan"); //done +//// map_names.push_back("lab_a_scan"); //done +// map_names.push_back("lab_f_scan"); //done +//// map_names.push_back("NLB"); //done + + //with obstacles: +// "Freiburg52_scan_furnitures_trashbins" +// map_names.push_back("lab_ipa_furnitures"); + + std::vector< std::string > map_names; + map_names.push_back("lab_ipa"); + map_names.push_back("lab_c_scan"); + map_names.push_back("Freiburg52_scan"); + map_names.push_back("Freiburg79_scan"); + map_names.push_back("lab_b_scan"); + map_names.push_back("lab_intel"); + map_names.push_back("Freiburg101_scan"); + map_names.push_back("lab_d_scan"); + map_names.push_back("lab_f_scan"); + map_names.push_back("lab_a_scan"); + map_names.push_back("NLB"); + map_names.push_back("office_a"); + map_names.push_back("office_b"); + map_names.push_back("office_c"); + map_names.push_back("office_d"); + map_names.push_back("office_e"); + map_names.push_back("office_f"); + map_names.push_back("office_g"); + map_names.push_back("office_h"); + map_names.push_back("office_i"); + map_names.push_back("lab_ipa_furnitures"); + map_names.push_back("lab_c_scan_furnitures"); + map_names.push_back("Freiburg52_scan_furnitures"); + map_names.push_back("Freiburg79_scan_furnitures"); + map_names.push_back("lab_b_scan_furnitures"); + map_names.push_back("lab_intel_furnitures"); + map_names.push_back("Freiburg101_scan_furnitures"); + map_names.push_back("lab_d_scan_furnitures"); + map_names.push_back("lab_f_scan_furnitures"); + map_names.push_back("lab_a_scan_furnitures"); + map_names.push_back("NLB_furnitures"); + map_names.push_back("office_a_furnitures"); + map_names.push_back("office_b_furnitures"); + map_names.push_back("office_c_furnitures"); + map_names.push_back("office_d_furnitures"); + map_names.push_back("office_e_furnitures"); + map_names.push_back("office_f_furnitures"); + map_names.push_back("office_g_furnitures"); + map_names.push_back("office_h_furnitures"); + map_names.push_back("office_i_furnitures"); + + + // prepare image data for evaluation + for (size_t image_index = 0; image_index(y, x) > 250) + { + map.at(y, x) = 255; + } + else //if (map.at(y, x) != 255) + { + map.at(y, x) = 0; + } + } + } + + // read in trash bin locations + std::string map_name_basic = map_names[image_index]; + std::size_t pos = map_names[image_index].find("_furnitures"); + if (pos != std::string::npos) + map_name_basic = map_names[image_index].substr(0, pos); + image_filename = test_map_path + map_name_basic + "_trashbins.png"; + cv::Mat temp = cv::imread(image_filename.c_str()); + cv::Vec3b blue(255, 0, 0); + cv::Vec3b red(0, 0, 255); + double map_resoultion_for_evaluation = 0.05; + int number_of_erosions = (robot_radius_ / map_resoultion_for_evaluation); + cv::Mat eroded_map; + cv::erode(map, eroded_map, cv::Mat(), cv::Point(-1,-1), number_of_erosions); + std::vector< cv::Point > trash_bin_locations; + cv::Point central_trolley_park; + for (int y = 0; y < temp.rows; y++) + { + for (int x = 0; x < temp.cols; x++) + { + //find blue drawn trash bins and check if they are reachable (if one is not reachable the a star planner will give an extremely large path) + if (temp.at(y, x) == blue && eroded_map.at(y, x) != 0) + { + trash_bin_locations.push_back(cv::Point(x,y)); + std::cout << "trash: " << x << ", " << y << std::endl; + } + //find red drawn trolley park and check if they are reachable (if one is not reachable the a star planner will give an extremely large path) + if (temp.at(y, x) == red && eroded_map.at(y, x) != 0) + { + central_trolley_park = cv::Point(x, y); + std::cout << "trolley park: " << x << ", " << y << std::endl; + } + } + } + //give occupied memory free + temp.release(); + eroded_map.release(); + +// +"_furnitures_trashbins" +// evaluation_data_.push_back(EvaluationData(map_names[image_index], map, map_resoultion_for_evaluation, 0.25, robot_radius_, trash_bin_locations)); + evaluation_data_.push_back(EvaluationData(map_names[image_index], map, map_resoultion_for_evaluation, 0.25, robot_radius_, trash_bin_locations, central_trolley_park)); + } + + // set configurations + std::vector evaluation_configurations; + setConfigurations(evaluation_configurations); + + // do the evaluation + std::string bugfile = data_storage_path + "bugfile.txt"; + std::ofstream failed_maps(bugfile.c_str(), std::ios::out); + if(failed_maps.is_open()) + failed_maps << "maps that had a bug during the simulation and couldn't be finished: " << std::endl; + for (size_t i=0; i > results; // maps from [map_name]->StatisticsItems[evaluation_configurations.size()] + readFromLocalFiles(evaluation_configurations, map_names, data_storage_path, results); + + //write the calculated values to global saving files + writeGlobalStatistics(evaluation_configurations, map_names, data_storage_path, results); +// writeToGlobalFiles(map_names, data_storage_path, map_pathlengths, map_cleaning_times, sequence_solver_times); + } + + //function to read the saved results out of the files + void readFromLocalFiles(const std::vector& evaluation_configurations, const std::vector& map_names, const std::string& data_storage_path, + std::map >& results) + { + for (size_t i=0; i> stats.robot_speed_without_trolley; // in [m/s] + iss >> stats.robot_speed_with_trolley; // in [m/s] + iss >> stats.time_for_trashbin_manipulation; // in [s] + iss >> stats.time_for_trolley_manipulation; // in [s] + iss >> stats.number_trash_bins; + iss >> stats.number_trolley_movements; + iss >> stats.path_length_robot; // in [m] + iss >> stats.path_length_trolley; // in [m] + iss >> stats.path_length_trash_bins; // in [m] + iss >> stats.pathlength; // in [m] + iss >> stats.cleaning_time; // in [s] + iss >> stats.calculation_time_segmentation; // in [s] + iss >> stats.calculation_time_sequencer; // in [s] + reading_file.close(); + } + else + { + std::cout << "missing data: " << log_filename << std::endl; + } + + std::string human_filename = data_storage_path + "human_way/" + upper_folder_name + lower_folder_name + map_names[map_index] + "_human.txt"; + std::ifstream human_file(human_filename.c_str()); + + if(human_file.is_open()) + { + std::string line; + getline(human_file, line); // first line is text + getline(human_file, line); + std::istringstream iss(line); + iss >> stats.human_way; // in [m] + human_file.close(); + } + results[map_names[map_index]].push_back(stats); + } + } + } + + void writeGlobalStatistics(const std::vector& evaluation_configurations, const std::vector& map_names, const std::string& data_storage_path, + std::map >& results) + { + //define the storage path for each planning method + const std::string path = data_storage_path + "global/"; + const std::string upper_command = "mkdir -p " + path; + int return_value = system(upper_command.c_str()); + + // prepare files for different evaluation criteria + std::vector filenames; + filenames.push_back("pathlength"); + filenames.push_back("cleaning_time"); + filenames.push_back("number_trash_bins"); + filenames.push_back("number_trolley_movements"); + filenames.push_back("calculation_time_segmentation"); + filenames.push_back("calculation_time_sequencer"); + filenames.push_back("path_length_robot"); + filenames.push_back("path_length_trolley"); + filenames.push_back("path_length_trash_bins"); + filenames.push_back("human_way"); + + for (std::vector::iterator it_filename=filenames.begin(); it_filename!=filenames.end(); ++it_filename) + { + // collect column data + std::set max_clique_lengths; + for (size_t i=0; i > output_matrix(1+data_columns); // [column index][row index] ! + for (size_t i=0; i::const_iterator it=map_names.begin(); it!=map_names.end(); it++) + { + // prepare first column of output + output_matrix[0][0] = *it; + int r=1; + for (std::set::iterator mcl_it = max_clique_lengths.begin(); mcl_it != max_clique_lengths.end(); mcl_it++, r++) + { + std::stringstream ss; + ss << "mcl" << *mcl_it; + output_matrix[0][r] = ss.str(); + } + + // write remaining columns of output + for (int i=0; icompare("pathlength") == 0) + sss << results[*it][base_index+k].pathlength; + else if (it_filename->compare("cleaning_time") == 0) + sss << results[*it][base_index+k].cleaning_time; + else if (it_filename->compare("number_trash_bins") == 0) + sss << results[*it][base_index+k].number_trash_bins; + else if (it_filename->compare("number_trolley_movements") == 0) + sss << results[*it][base_index+k].number_trolley_movements; + else if (it_filename->compare("calculation_time_segmentation") == 0) + sss << results[*it][base_index+k].calculation_time_segmentation; + else if (it_filename->compare("calculation_time_sequencer") == 0) + sss << results[*it][base_index+k].calculation_time_sequencer; + else if (it_filename->compare("path_length_robot") == 0) + sss << results[*it][base_index+k].path_length_robot; + else if (it_filename->compare("path_length_trolley") == 0) + sss << results[*it][base_index+k].path_length_trolley; + else if (it_filename->compare("path_length_trash_bins") == 0) + sss << results[*it][base_index+k].path_length_trash_bins; + else if (it_filename->compare("human_way") == 0) + sss << results[*it][base_index+k].human_way; + output_matrix[1+i][1+k] = sss.str(); + } + } + // write output string + for (size_t row=0; row& map_names, const std::string& data_storage_path, const std::vector >& map_pathlengths, + const std::vector >& map_cleaning_times, const std::vector >& sequence_solver_times) + { + + //save the saved parameters in a saving file for each map + + //define the storage path for each planning method + std::stringstream ss; + ss << "global"; + const std::string upper_folder_name = ss.str() + "/"; + const std::string path = data_storage_path + upper_folder_name; + const std::string upper_command = "mkdir -p " + path; + int return_value = system(upper_command.c_str()); + + const std::string cleaning_time_folder = "cleaningtime/"; + std::string lower_command = "mkdir -p " + path + cleaning_time_folder; + return_value = system(lower_command.c_str()); + const std::string path_length_folder = "pathlengths/"; + lower_command = "mkdir -p " + path + path_length_folder; + return_value = system(lower_command.c_str()); + const std::string computation_time_folder = "comp_time/"; + lower_command = "mkdir -p " + path + computation_time_folder; + return_value = system(lower_command.c_str()); + + // outputs to log file + std::stringstream distance_output[map_names.size()]; + std::stringstream cleaning_time_output[map_names.size()]; + std::stringstream sequence_time_output[map_names.size()]; + + + //define values to show how much different algorithms has been implemented + double max_clique_path_length = 4.0; + int number_of_cliquelenghts = 8; + int number_of_segmentation_algorithms = 5; + int number_of_tsp_solver = 3; + for(size_t i = 0; i < map_names.size(); ++i) + { + distance_output[i] << "\t" << "Nachziehmethode" << "\t" << "\t" << "Trolley-Gruppen" << std::endl + << "max. Fahrdistanz" << "\t" << "nearest" << "\t" << "genetic" << "\t" << "concorde" << "\t" << "nearest" << "\t" << "genetic" << "\t" << "concorde" << std::endl; + cleaning_time_output[i] << "\t" << "\t" << "Nachziehmethode" << "\t" << "\t" << "Trolley-Gruppen" << std::endl + << "max. Fahrdistanz" << "\t" << "nearest" << "\t" << "genetic" << "\t" << "concorde" << "\t" << "nearest" << "\t" << "genetic" << "\t" << "concorde" << std::endl; + sequence_time_output[i] << "\t" << "\t" << "Nachziehmethode" << "\t" << "\t" << "Trolley-Gruppen" << std::endl + << "max. Fahrdistanz" << "\t" << "nearest" << "\t" << "genetic" << "\t" << "concorde" << "\t" << "nearest" << "\t" << "genetic" << "\t" << "concorde" << std::endl; + } + for (int max_length = 0; max_length < number_of_cliquelenghts; ++max_length) + { + max_clique_path_length += 2.0; + + int start = number_of_segmentation_algorithms * number_of_tsp_solver * max_length; + + for(size_t map = 0; map < map_names.size(); ++map) + { + //show which maximal clique pathlength this is + distance_output[map] << std::endl << max_clique_path_length; + cleaning_time_output[map] << std::endl << max_clique_path_length; + sequence_time_output[map] << std::endl << max_clique_path_length; + // header + for(size_t segmentation = 0; segmentation < number_of_segmentation_algorithms; ++segmentation) + { + if(segmentation > 0) + { + distance_output[map] << 0; + cleaning_time_output[map] << 0; + sequence_time_output[map] << 0; + } + for(int sequence_planning_method = 0; sequence_planning_method < 2; ++sequence_planning_method) + { + int plmth_start = start + number_of_segmentation_algorithms * number_of_tsp_solver * number_of_cliquelenghts * sequence_planning_method; + int index = plmth_start + number_of_tsp_solver * segmentation; + for(size_t reading_index = index; reading_index < index+number_of_tsp_solver; ++reading_index) + { + distance_output[map] << " " << map_pathlengths[map][reading_index] ; + cleaning_time_output[map] << " " << map_cleaning_times[map][reading_index]; + sequence_time_output[map] << " " << sequence_solver_times[map][reading_index]; + } + } + + distance_output[map] << std::endl; + cleaning_time_output[map] << std::endl; + sequence_time_output[map] << std::endl; + } + } + } + //write saved parameters to global file + for(size_t map = 0; map < map_names.size(); ++map) + { + std::string pathlength_filename = path + path_length_folder + map_names[map] + "_pathlengths.txt"; + std::ofstream pathlength_file(pathlength_filename.c_str(), std::ios::out); + if (pathlength_file.is_open()==true) + pathlength_file << distance_output[map].str(); + pathlength_file.close(); + + std::string cleaningtime_filename = path + cleaning_time_folder + map_names[map] + "_cleaningtimes.txt"; + std::ofstream cleaningtime_file(cleaningtime_filename.c_str(), std::ios::out); + if (cleaningtime_file.is_open()==true) + cleaningtime_file << cleaning_time_output[map].str(); + cleaningtime_file.close(); + + std::string sequencetime_filename = path + computation_time_folder + map_names[map] + "_sequencetimes.txt"; + std::ofstream sequencetime_file(sequencetime_filename.c_str(), std::ios::out); + if (sequencetime_file.is_open()==true) + sequencetime_file << sequence_time_output[map].str(); + sequencetime_file.close(); + + } + } + + void setConfigurations(std::vector< EvaluationConfig >& evaluation_configurations) + { + evaluation_configurations.clear(); + for (int room_segmentation_algorithm=1; room_segmentation_algorithm<=5; ++room_segmentation_algorithm) + { + for(int sequence_planning_method = 2; sequence_planning_method <= 2; ++sequence_planning_method) + { + for(int tsp_solver = 1; tsp_solver <= 3; ++tsp_solver) + { + cv::Mat max_clique_lengths = (cv::Mat_(1,11) << 6., 8., 10., 12., 14., 16., 18., 20., 25., 30., 50.); + //for (double max_clique_path_length = 20.; max_clique_path_length <= 20.; max_clique_path_length += 2.0) + for (int i=0; i(0,i), sequence_planning_method, tsp_solver, 10)); + } + } + } + } + + bool evaluateAllConfigs0815(const std::vector& evaluation_configuration_vector, const EvaluationData& evaluation_data, const std::string& data_storage_path) + { + // go through each configuration for the given map + for(size_t config = 0; config < evaluation_configuration_vector.size(); ++config) + { + // prepare folders for storing results + const std::string upper_folder_name = evaluation_configuration_vector[config].generateUpperConfigurationFolderString() + "/"; + const std::string path = data_storage_path + upper_folder_name; + const std::string upper_command = "mkdir -p " + path; + int return_value = system(upper_command.c_str()); + + const std::string lower_folder_name = evaluation_configuration_vector[config].generateLowerConfigurationFolderString() + "/"; + const std::string lower_path = path + lower_folder_name; + const std::string lower_command = "mkdir -p " + lower_path; + return_value = system(lower_command.c_str()); + + std::cout << "\nCurrent Configuration:" << std::endl << "map: " << evaluation_data.map_name_ << "\tsegmentation algorithm: " + << evaluation_configuration_vector[config].room_segmentation_algorithm_ + << "\tplanning method: " << evaluation_configuration_vector[config].sequence_planning_method_ + << "\tTSP solver: " << evaluation_configuration_vector[config].tsp_solver_ + << "\tmaximal clique length: " << evaluation_configuration_vector[config].max_clique_path_length_ << std::endl; + + AStarPlanner planner; + //variables for time measurement + struct timespec t0, t1, t2, t3; + + // 1. retrieve segmentation and check if the map has already been segmented + ipa_building_msgs::MapSegmentationResultConstPtr result_seg; + clock_gettime(CLOCK_MONOTONIC, &t0); //set time stamp before the segmentation + if(evaluation_configuration_vector[config].room_segmentation_algorithm_ == 1) + { + if(segmented_morph == false) + { + if (segmentFloorPlan(evaluation_data, evaluation_configuration_vector[config], result_seg_morph, t0) == false) + return false; + segmented_morph = true; + } + else + std::cout << "map has already been segmented" << std::endl; + result_seg = result_seg_morph; + } + else if (evaluation_configuration_vector[config].room_segmentation_algorithm_ == 2) + { + if(segmented_dist == false) + { + if (segmentFloorPlan(evaluation_data, evaluation_configuration_vector[config], result_seg_dist, t0) == false) + return false; + segmented_dist = true; + } + else + std::cout << "map has already been segmented" << std::endl; + result_seg = result_seg_dist; + } + else if (evaluation_configuration_vector[config].room_segmentation_algorithm_ == 3) + { + if(segmented_vor == false) + { + if (segmentFloorPlan(evaluation_data, evaluation_configuration_vector[config], result_seg_vor, t0) == false) + return false; + segmented_vor = true; + } + else + std::cout << "map has already been segmented" << std::endl; + result_seg = result_seg_vor; + } + else if (evaluation_configuration_vector[config].room_segmentation_algorithm_ == 4) + { + if(segmented_semant == false) + { + if (segmentFloorPlan(evaluation_data, evaluation_configuration_vector[config], result_seg_semant, t0) == false) + return false; + segmented_semant = true; + } + else + std::cout << "map has already been segmented" << std::endl; + result_seg = result_seg_semant; + } + else if (evaluation_configuration_vector[config].room_segmentation_algorithm_ == 5) + { + if(segmented_vrf == false) + { + if (segmentFloorPlan(evaluation_data, evaluation_configuration_vector[config], result_seg_vrf, t0) == false) + return false; + segmented_vrf = true; + } + else + std::cout << "map has already been segmented" << std::endl; + result_seg = result_seg_vrf; + } + clock_gettime(CLOCK_MONOTONIC, &t1); //set time stamp after the segmentation + std::cout << "Segmentation computed " << result_seg->room_information_in_pixel.size() << " rooms." << std::endl; + + //check for accessibility of the room centers from start position + cv::Mat downsampled_map; + Timer tim; + planner.downsampleMap(evaluation_data.floor_plan_, downsampled_map, evaluation_data.map_downsampling_factor_, evaluation_data.robot_radius_, evaluation_data.map_resolution_); + std::cout << "downsampling map: " << tim.getElapsedTimeInMilliSec() << " ms" << std::endl; + cv::Point robot_start_position((evaluation_data.robot_start_position_.position.x - evaluation_data.map_origin_.position.x)/evaluation_data.map_resolution_, + (evaluation_data.robot_start_position_.position.y - evaluation_data.map_origin_.position.y)/evaluation_data.map_resolution_); + + // get the reachable room centers as cv::Point + tim.start(); + std::cout << "Starting to check accessibility of rooms. Start position: " << robot_start_position << std::endl; + std::vector room_centers; + for(size_t i = 0; i < result_seg->room_information_in_pixel.size(); ++i) + { + cv::Point current_center(result_seg->room_information_in_pixel[i].room_center.x, result_seg->room_information_in_pixel[i].room_center.y); + double length = planner.planPath(evaluation_data.floor_plan_, downsampled_map, robot_start_position, current_center, evaluation_data.map_downsampling_factor_, 0., evaluation_data.map_resolution_); + if(length < 1e9) + room_centers.push_back(current_center); + else + std::cout << "room " << i << " not accessible, center: " << current_center << std::endl; + } + std::cout << "room centers computed: " << tim.getElapsedTimeInMilliSec() << " ms" << std::endl; + std::cout << "Number of accessible room centers: " << room_centers.size() << std::endl; + + if(room_centers.size() == 0) //no room center is reachable for the given start position --> needs to be looked at separately + { + std::cout << "++++++++++ no roomcenter reachable from given startposition ++++++++++++" << std::endl; + return false; + } + + // 2. solve sequence problem + std::cout << "Starting to solve sequence problem." << std::endl; + tim.start(); + ipa_building_msgs::FindRoomSequenceWithCheckpointsResultConstPtr result_seq; + clock_gettime(CLOCK_MONOTONIC, &t2); //set time stamp before the sequence planning + if (computeRoomSequence(evaluation_data, evaluation_configuration_vector[config], room_centers, result_seq, t2) == false) + { + std::cout << "++++++++++ computeRoomSequence failed ++++++++++++" << std::endl; + return false; + } + std::cout << "sequence problem solved: " << tim.getElapsedTimeInMilliSec() << " ms" << std::endl; + clock_gettime(CLOCK_MONOTONIC, &t3); //set time stamp after the sequence planning + + // 3. assign trash bins to rooms of the respective segmentation + std::vector< std::vector > room_trash_bins(result_seg->room_information_in_pixel.size()); + // converting the map msg in cv format + cv_bridge::CvImagePtr cv_ptr_obj; + cv_ptr_obj = cv_bridge::toCvCopy(result_seg->segmented_map, sensor_msgs::image_encodings::TYPE_32SC1); + cv::Mat segmented_map = cv_ptr_obj->image; + for (size_t i=0; i(evaluation_data.trash_bin_locations_[i]); //labeling started from 1 --> 0 is for obstacles + int room_index = -1; + for (size_t r=0; r(room_centers[r]) == trash_bin_index) + { + room_index = r; + break; + } + } + if (room_index != -1) + { + room_trash_bins[room_index].push_back(evaluation_data.trash_bin_locations_[i]); + std::cout << "trash bin " << evaluation_data.trash_bin_locations_[i] << " at room center[" << room_index << "] " << room_centers[room_index] << std::endl; + } + else + std::cout << "########## trash bin " << evaluation_data.trash_bin_locations_[i] << " does not match any room." << std::endl; + } + + // 4. do the movements + cv::Mat draw_path_map, draw_path_map2; + cv::cvtColor(evaluation_data.floor_plan_, draw_path_map, CV_GRAY2BGR); + draw_path_map2 = draw_path_map.clone(); + double path_length_robot = 0.; + double path_length_trolley = 0.; + double path_length_trash_bins = 0.; +// const double max_clique_path_length_in_pixel = evaluation_configuration.max_clique_path_length_ / evaluation_data.map_resolution_; + cv::Point robot_position = robot_start_position; + cv::Point trolley_position((evaluation_data.robot_start_position_.position.x - evaluation_data.map_origin_.position.x)/evaluation_data.map_resolution_, + (evaluation_data.robot_start_position_.position.y - evaluation_data.map_origin_.position.y)/evaluation_data.map_resolution_); + cv::circle(draw_path_map, robot_position, 3, CV_RGB(0,255,0), -1); + cv::circle(draw_path_map, trolley_position, 5, CV_RGB(0,0,255), -1); + cv::circle(draw_path_map2, trolley_position, 5, CV_RGB(0,0,255), -1); + + std::stringstream screenoutput; + for (size_t clique_index = 0; clique_indexcheckpoints.size(); ++clique_index) + { + std::cout << "cleaning new clique" << std::endl; screenoutput << "cleaning new clique" << std::endl; + // move trolley + // i) robot to trolley + path_length_robot += planner.planPath(evaluation_data.floor_plan_, downsampled_map, robot_position, trolley_position, evaluation_data.map_downsampling_factor_, 0., evaluation_data.map_resolution_, 1, &draw_path_map); + // ii) trolley to next trolley goal + cv::Point trolley_goal_position(result_seq->checkpoints[clique_index].checkpoint_position_in_pixel.x, result_seq->checkpoints[clique_index].checkpoint_position_in_pixel.y); + path_length_trolley += planner.planPath(evaluation_data.floor_plan_, downsampled_map, trolley_position, trolley_goal_position, evaluation_data.map_downsampling_factor_, 0., evaluation_data.map_resolution_, 1, &draw_path_map); + trolley_position = trolley_goal_position; + robot_position = trolley_goal_position; + cv::circle(draw_path_map, robot_position, 3, CV_RGB(0,255,0), -1); + cv::circle(draw_path_map, trolley_position, 5, CV_RGB(0,0,255), -1); + cv::circle(draw_path_map2, trolley_position, 5, CV_RGB(0,0,255), -1); + std::cout << "moved trolley to " << trolley_position << std::endl; screenoutput << "moved trolley to " << trolley_position << std::endl; + + // move robot to rooms + for(size_t room = 0; room < result_seq->checkpoints[clique_index].room_indices.size(); ++room) + { + // get next room in sequence + const int room_index = result_seq->checkpoints[clique_index].room_indices[room]; + cv::Point current_roomcenter = room_centers[room_index]; + // drive to next room + path_length_robot += planner.planPath(evaluation_data.floor_plan_, downsampled_map, robot_position, current_roomcenter, evaluation_data.map_downsampling_factor_, 0., evaluation_data.map_resolution_, 1, &draw_path_map); + robot_position = current_roomcenter; + cv::circle(draw_path_map, robot_position, 3, CV_RGB(0,255,0), -1); + // clear all trash bins: go to trash bin, go back to trolley to empty trash and then drive back to trash bin + std::cout << " arrived in room " << current_roomcenter << "\n starting to clean the trash bins" << std::endl; screenoutput << " arrived in room " << current_roomcenter << "\n starting to clean the trash bins" << std::endl; + ipa_building_msgs::FindRoomSequenceWithCheckpointsResultConstPtr result_trash_bin_seq; + std::vector trash_bin_sequence_in_this_room; + if (room_trash_bins[room_index].size()>1 && computeTrashBinSequence(evaluation_data, evaluation_configuration_vector[config], room_trash_bins[room_index], robot_position, result_trash_bin_seq) == true) + { + for (size_t cc=0; cccheckpoints.size(); ++cc) + for (size_t tt = 0; tt < result_trash_bin_seq->checkpoints[cc].room_indices.size(); ++tt) + trash_bin_sequence_in_this_room.push_back(room_trash_bins[room_index][result_trash_bin_seq->checkpoints[cc].room_indices[tt]]); + } + else + { + trash_bin_sequence_in_this_room = room_trash_bins[room_index]; + } + for (size_t t=0; tcheckpoints.size()+1); + + //get the runtimes for the segmentation and the sequence planner + double segmentation_time = (t1.tv_sec - t0.tv_sec) + (double) (t1.tv_nsec - t0.tv_nsec) * 1e-9; + double sequence_time = (t3.tv_sec - t2.tv_sec) + (double) (t3.tv_nsec - t2.tv_nsec) * 1e-9; + + // write log file + std::stringstream output; + // header + output << evaluation_configuration_vector[config].getConfigurationString(); + output << "robot_speed_without_trolley" << "\t" << "robot_speed_with_trolley" << "\t" << "time_for_trashbin_manipulation" << "\t" + << "time_for_trolley_manipulation" << "\t" << "number_of_trash_bins" << "\t" << "number_trolley_movements" << "\t" + << "path_length_robot_in_meter" << "\t" << "path_length_trolley_in_meter" << "\t" << "path_length_trash_bins_in_meter\t" + << "pathlength [m]" << "\t" + << "cleaning time [s]" << "\t" << "calculation time segmentation [s]" << "\t" << "calculation time sequencer [s]" << "\t" << std::endl; + output << robot_speed_without_trolley << "\t" << robot_speed_with_trolley << "\t" << time_for_trashbin_manipulation << "\t" << time_for_trolley_manipulation << "\t" + << evaluation_data.trash_bin_locations_.size() << "\t" + << (result_seq->checkpoints.size()+1) << "\t" << path_length_robot_in_meter << "\t" << path_length_trolley_in_meter << "\t" << path_length_trash_bins_in_meter << "\t" + << path_length_total_in_meter << "\t" + << time << "\t" << segmentation_time << "\t" << sequence_time; + output << "\n\n\n" << screenoutput.str() << std::endl; + + std::string log_filename = lower_path + evaluation_data.map_name_ + "_results.txt"; + std::ofstream file(log_filename.c_str(), std::ios::out); + if (file.is_open()==true) + file << output.str(); + else + ROS_ERROR("Error on writing file '%s'", log_filename.c_str()); + file.close(); + + // images: segmented_map, sequence_map + std::string segmented_map_filename = lower_path + evaluation_data.map_name_ + "_segmented.png"; + cv::Mat colour_segmented_map = segmented_map.clone(); + colour_segmented_map.convertTo(colour_segmented_map, CV_8U); + cv::cvtColor(colour_segmented_map, colour_segmented_map, CV_GRAY2BGR); + for(size_t i = 1; i <= result_seg->room_information_in_pixel.size(); ++i) + { + // choose random color for each room + int blue = (rand() % 250) + 1; + int green = (rand() % 250) + 1; + int red = (rand() % 250) + 1; + for(size_t u = 0; u < segmented_map.rows; ++u) + { + for(size_t v = 0; v < segmented_map.cols; ++v) + { + if(segmented_map.at(u,v) == i) + { + colour_segmented_map.at(u,v)[0] = blue; + colour_segmented_map.at(u,v)[1] = green; + colour_segmented_map.at(u,v)[2] = red; + } + } + } + } + // draw the room centers into the map + for(size_t i = 0; i < result_seg->room_information_in_pixel.size(); ++i) + { + cv::Point current_center (result_seg->room_information_in_pixel[i].room_center.x, result_seg->room_information_in_pixel[i].room_center.y); +#if CV_MAJOR_VERSION<=3 + cv::circle(colour_segmented_map, current_center, 2, CV_RGB(0,0,255), CV_FILLED); +#else + cv::circle(colour_segmented_map, current_center, 2, CV_RGB(0,0,255), cv::FILLED); +#endif + } + // color image in unique colour to show the segmentation + if (cv::imwrite(segmented_map_filename.c_str(), colour_segmented_map) == false) + ROS_ERROR("Error on writing file '%s'", segmented_map_filename.c_str()); + + std::string sequence_map_filename = lower_path + evaluation_data.map_name_ + "_sequence.png"; + cv_bridge::CvImagePtr cv_ptr_seq; + cv_ptr_seq = cv_bridge::toCvCopy(result_seq->sequence_map, sensor_msgs::image_encodings::BGR8); + cv::Mat sequence_map = cv_ptr_seq->image; + // draw in trash bins + for (size_t i=0; i& evaluation_configuration_vector, const EvaluationData& evaluation_data, const std::string& data_storage_path) + { + // go through each configuration for the given map + for(size_t config = 0; config < evaluation_configuration_vector.size(); ++config) + { + // prepare folders for storing results + const std::string upper_folder_name = evaluation_configuration_vector[config].generateUpperConfigurationFolderString() + "/"; + const std::string path = data_storage_path + upper_folder_name; + const std::string upper_command = "mkdir -p " + path; + int return_value = system(upper_command.c_str()); + + const std::string lower_folder_name = evaluation_configuration_vector[config].generateLowerConfigurationFolderString() + "/"; + const std::string lower_path = path + lower_folder_name; + const std::string lower_command = "mkdir -p " + lower_path; + return_value = system(lower_command.c_str()); + + std::cout << "\nCurrent Configuration:" << std::endl << "map: " << evaluation_data.map_name_ << "\tsegmentation algorithm: " + << evaluation_configuration_vector[config].room_segmentation_algorithm_ + << "\tplanning method: " << evaluation_configuration_vector[config].sequence_planning_method_ + << "\tTSP solver: " << evaluation_configuration_vector[config].tsp_solver_ + << "\tmaximal clique length: " << evaluation_configuration_vector[config].max_clique_path_length_ << std::endl; + + AStarPlanner planner; + //variables for time measurement + struct timespec t0, t1, t2, t3; + + // 1. retrieve segmentation and check if the map has already been segmented + ipa_building_msgs::MapSegmentationResultConstPtr result_seg; + clock_gettime(CLOCK_MONOTONIC, &t0); //set time stamp before the segmentation + if(evaluation_configuration_vector[config].room_segmentation_algorithm_ == 1) + { + if(segmented_morph == false) + { + if (segmentFloorPlan(evaluation_data, evaluation_configuration_vector[config], result_seg_morph, t0) == false) + return false; + segmented_morph = true; + } + else + std::cout << "map has already been segmented" << std::endl; + result_seg = result_seg_morph; + } + else if (evaluation_configuration_vector[config].room_segmentation_algorithm_ == 2) + { + if(segmented_dist == false) + { + if (segmentFloorPlan(evaluation_data, evaluation_configuration_vector[config], result_seg_dist, t0) == false) + return false; + segmented_dist = true; + } + else + std::cout << "map has already been segmented" << std::endl; + result_seg = result_seg_dist; + } + else if (evaluation_configuration_vector[config].room_segmentation_algorithm_ == 3) + { + if(segmented_vor == false) + { + if (segmentFloorPlan(evaluation_data, evaluation_configuration_vector[config], result_seg_vor, t0) == false) + return false; + segmented_vor = true; + } + else + std::cout << "map has already been segmented" << std::endl; + result_seg = result_seg_vor; + } + else if (evaluation_configuration_vector[config].room_segmentation_algorithm_ == 4) + { + if(segmented_semant == false) + { + if (segmentFloorPlan(evaluation_data, evaluation_configuration_vector[config], result_seg_semant, t0) == false) + return false; + segmented_semant = true; + } + else + std::cout << "map has already been segmented" << std::endl; + result_seg = result_seg_semant; + } + else if (evaluation_configuration_vector[config].room_segmentation_algorithm_ == 5) + { + if(segmented_vrf == false) + { + if (segmentFloorPlan(evaluation_data, evaluation_configuration_vector[config], result_seg_vrf, t0) == false) + return false; + segmented_vrf = true; + } + else + std::cout << "map has already been segmented" << std::endl; + result_seg = result_seg_vrf; + } + clock_gettime(CLOCK_MONOTONIC, &t1); //set time stamp after the segmentation + std::cout << "Segmentation computed " << result_seg->room_information_in_pixel.size() << " rooms." << std::endl; + + //check for accessibility of the room centers from start position + cv::Mat downsampled_map; + Timer tim; + planner.downsampleMap(evaluation_data.floor_plan_, downsampled_map, evaluation_data.map_downsampling_factor_, evaluation_data.robot_radius_, evaluation_data.map_resolution_); + std::cout << "downsampling map: " << tim.getElapsedTimeInMilliSec() << " ms" << std::endl; + cv::Point robot_start_position((evaluation_data.robot_start_position_.position.x - evaluation_data.map_origin_.position.x)/evaluation_data.map_resolution_, + (evaluation_data.robot_start_position_.position.y - evaluation_data.map_origin_.position.y)/evaluation_data.map_resolution_); + + // get the reachable room centers as cv::Point + tim.start(); + std::cout << "Starting to check accessibility of rooms. Start position: " << robot_start_position << std::endl; + std::vector room_centers; + for(size_t i = 0; i < result_seg->room_information_in_pixel.size(); ++i) + { + cv::Point current_center(result_seg->room_information_in_pixel[i].room_center.x, result_seg->room_information_in_pixel[i].room_center.y); + double length = planner.planPath(evaluation_data.floor_plan_, downsampled_map, robot_start_position, current_center, evaluation_data.map_downsampling_factor_, 0., evaluation_data.map_resolution_); + if(length < 1e9) + room_centers.push_back(current_center); + else + std::cout << "room " << i << " not accessible, center: " << current_center << std::endl; + } + std::cout << "room centers computed: " << tim.getElapsedTimeInMilliSec() << " ms" << std::endl; + std::cout << "Number of accessible room centers: " << room_centers.size() << std::endl; + + if(room_centers.size() == 0) //no room center is reachable for the given start position --> needs to be looked at separately + { + std::cout << "++++++++++ no roomcenter reachable from given startposition ++++++++++++" << std::endl; + return false; + } + + // 2. solve sequence problem + std::cout << "Starting to solve sequence problem." << std::endl; + tim.start(); + ipa_building_msgs::FindRoomSequenceWithCheckpointsResultConstPtr result_seq; + clock_gettime(CLOCK_MONOTONIC, &t2); //set time stamp before the sequence planning + if (computeRoomSequence(evaluation_data, evaluation_configuration_vector[config], room_centers, result_seq, t2) == false) + { + std::cout << "++++++++++ computeRoomSequence failed ++++++++++++" << std::endl; + return false; + } + std::cout << "sequence problem solved: " << tim.getElapsedTimeInMilliSec() << " ms" << std::endl; + clock_gettime(CLOCK_MONOTONIC, &t3); //set time stamp after the sequence planning + + // 3. assign trash bins to rooms of the respective segmentation + std::vector< std::vector > room_trash_bins(result_seg->room_information_in_pixel.size()); + // converting the map msg in cv format + cv_bridge::CvImagePtr cv_ptr_obj; + cv_ptr_obj = cv_bridge::toCvCopy(result_seg->segmented_map, sensor_msgs::image_encodings::TYPE_32SC1); + cv::Mat segmented_map = cv_ptr_obj->image; + for (size_t i=0; i(evaluation_data.trash_bin_locations_[i]); //labeling started from 1 --> 0 is for obstacles + int room_index = -1; + for (size_t r=0; r(room_centers[r]) == trash_bin_index) + { + room_index = r; + break; + } + } + if (room_index != -1) + { + room_trash_bins[room_index].push_back(evaluation_data.trash_bin_locations_[i]); + std::cout << "trash bin " << evaluation_data.trash_bin_locations_[i] << " at room center[" << room_index << "] " << room_centers[room_index] << std::endl; + } + else + std::cout << "########## trash bin " << evaluation_data.trash_bin_locations_[i] << " does not match any room." << std::endl; + } + + // 4. do the movements + cv::Mat draw_path_map, draw_path_map2; + cv::cvtColor(evaluation_data.floor_plan_, draw_path_map, CV_GRAY2BGR); + draw_path_map2 = draw_path_map.clone(); + double path_length_robot = 0.; + double path_length_trolley = 0.; + double path_length_trash_bins = 0.; + int current_emptied_trashbins = 0; + int switching_trolley_handling = 0; // int that shows how often the trolley has been handled during the switching procedure + // const double max_clique_path_length_in_pixel = evaluation_configuration.max_clique_path_length_ / evaluation_data.map_resolution_; + cv::Point robot_position = robot_start_position; + cv::Point trolley_position((evaluation_data.robot_start_position_.position.x - evaluation_data.map_origin_.position.x)/evaluation_data.map_resolution_, + (evaluation_data.robot_start_position_.position.y - evaluation_data.map_origin_.position.y)/evaluation_data.map_resolution_); + cv::circle(draw_path_map, robot_position, 3, CV_RGB(0,255,0), -1); + cv::circle(draw_path_map, trolley_position, 5, CV_RGB(0,0,255), -1); + cv::circle(draw_path_map2, trolley_position, 5, CV_RGB(0,0,255), -1); + + std::stringstream screenoutput; + for (size_t clique_index = 0; clique_indexcheckpoints.size(); ++clique_index) + { + std::cout << "cleaning new clique" << std::endl; screenoutput << "cleaning new clique" << std::endl; + // move trolley + // i) robot to trolley + path_length_robot += planner.planPath(evaluation_data.floor_plan_, downsampled_map, robot_position, trolley_position, evaluation_data.map_downsampling_factor_, 0., evaluation_data.map_resolution_, 1, &draw_path_map); + // ii) trolley to next trolley goal + cv::Point trolley_goal_position(result_seq->checkpoints[clique_index].checkpoint_position_in_pixel.x, result_seq->checkpoints[clique_index].checkpoint_position_in_pixel.y); + if(current_emptied_trashbins == evaluation_configuration_vector[config].trashbins_per_trolley_) + { + // get new trolley before going to the next trolley position + // i) from current trolley position to central trolley park + path_length_trolley += planner.planPath(evaluation_data.floor_plan_, downsampled_map, trolley_position, evaluation_data.central_trolley_park_, evaluation_data.map_downsampling_factor_, 0., evaluation_data.map_resolution_, 1, &draw_path_map); + // ii) from central trolley park to new trolley position + path_length_trolley += planner.planPath(evaluation_data.floor_plan_, downsampled_map, evaluation_data.central_trolley_park_, trolley_goal_position, evaluation_data.map_downsampling_factor_, 0., evaluation_data.map_resolution_, 1, &draw_path_map); + // increase int that shows how often the trolley has been handled during switching by one + ++switching_trolley_handling; + // reset number of emptied trashbins + current_emptied_trashbins = 0; + } + else + { + // just drive the trolley + path_length_trolley += planner.planPath(evaluation_data.floor_plan_, downsampled_map, trolley_position, trolley_goal_position, evaluation_data.map_downsampling_factor_, 0., evaluation_data.map_resolution_, 1, &draw_path_map); + } + trolley_position = trolley_goal_position; + robot_position = trolley_goal_position; + cv::circle(draw_path_map, robot_position, 3, CV_RGB(0,255,0), -1); + cv::circle(draw_path_map, trolley_position, 5, CV_RGB(0,0,255), -1); + cv::circle(draw_path_map2, trolley_position, 5, CV_RGB(0,0,255), -1); + std::cout << "moved trolley to " << trolley_position << std::endl; screenoutput << "moved trolley to " << trolley_position << std::endl; + + // move robot to rooms + for(size_t room = 0; room < result_seq->checkpoints[clique_index].room_indices.size(); ++room) + { + // check if the trolley needs to be changed + if(current_emptied_trashbins == evaluation_configuration_vector[config].trashbins_per_trolley_) + { + // if so drive the robot to the trolley and the trolley back to the park and the other way around + path_length_robot += planner.planPath(evaluation_data.floor_plan_, downsampled_map, robot_position, trolley_position, evaluation_data.map_downsampling_factor_, 0., evaluation_data.map_resolution_, 1, &draw_path_map); + path_length_trolley += 2.0 * planner.planPath(evaluation_data.floor_plan_, downsampled_map, trolley_position, evaluation_data.central_trolley_park_, evaluation_data.map_downsampling_factor_, 0., evaluation_data.map_resolution_, 1, &draw_path_map); + robot_position = trolley_position; + // increase handling of the trolley by two (grasping the robot before and during switching) + switching_trolley_handling += 2; + // reset number of emptied trashbins + current_emptied_trashbins = 0; + } + // get next room in sequence + const int room_index = result_seq->checkpoints[clique_index].room_indices[room]; + cv::Point current_roomcenter = room_centers[room_index]; + // drive to next room + path_length_robot += planner.planPath(evaluation_data.floor_plan_, downsampled_map, robot_position, current_roomcenter, evaluation_data.map_downsampling_factor_, 0., evaluation_data.map_resolution_, 1, &draw_path_map); + robot_position = current_roomcenter; + cv::circle(draw_path_map, robot_position, 3, CV_RGB(0,255,0), -1); + // clear all trash bins: go to trash bin, go back to trolley to empty trash and then drive back to trash bin + std::cout << " arrived in room " << current_roomcenter << "\n starting to plan the trash bins" << std::endl; screenoutput << " arrived in room " << current_roomcenter << "\n starting to clean the trash bins" << std::endl; + ipa_building_msgs::FindRoomSequenceWithCheckpointsResultConstPtr result_trash_bin_seq; + std::vector trash_bin_sequence_in_this_room; + if (room_trash_bins[room_index].size()>1) + { + bool result_trash = computeTrashBinSequence(evaluation_data, evaluation_configuration_vector[config], room_trash_bins[room_index], robot_position, result_trash_bin_seq); + if(result_trash == true) + { + for (size_t cc=0; cccheckpoints.size(); ++cc) + for (size_t tt = 0; tt < result_trash_bin_seq->checkpoints[cc].room_indices.size(); ++tt) + trash_bin_sequence_in_this_room.push_back(room_trash_bins[room_index][result_trash_bin_seq->checkpoints[cc].room_indices[tt]]); + } + } + else + { + trash_bin_sequence_in_this_room = room_trash_bins[room_index]; + } + std::cout << "starting to clean the trashbins. Size of sequence: " << trash_bin_sequence_in_this_room.size() << std::endl; + for (size_t t=0; t= evaluation_configuration_vector[config].trashbins_per_trolley_) + { + std::cout << "changing trolley " << std::endl; + // if so drive the robot to the trolley and the trolley back to the park and the other way around + path_length_robot += planner.planPath(evaluation_data.floor_plan_, downsampled_map, robot_position, trolley_position, evaluation_data.map_downsampling_factor_, 0., evaluation_data.map_resolution_, 1, &draw_path_map); + path_length_trolley += 2.0 * planner.planPath(evaluation_data.floor_plan_, downsampled_map, trolley_position, evaluation_data.central_trolley_park_, evaluation_data.map_downsampling_factor_, 0., evaluation_data.map_resolution_, 1, &draw_path_map); + robot_position = trolley_position; + // increase handling of the trolley by two (grasping the robot before and during switching) + switching_trolley_handling += 2; + // reset number of emptied trashbins + current_emptied_trashbins = 0; + std::cout << "changed trolley" << std::endl; + } + // drive robot to trash bin + double trash_bin_dist1 = planner.planPath(evaluation_data.floor_plan_, downsampled_map, robot_position, trash_bin_sequence_in_this_room[t], evaluation_data.map_downsampling_factor_, 0., evaluation_data.map_resolution_, 1, &draw_path_map); + path_length_robot += trash_bin_dist1; + cv::circle(draw_path_map, trash_bin_sequence_in_this_room[t], 2, CV_RGB(128,0,255), -1); + cv::circle(draw_path_map2, trash_bin_sequence_in_this_room[t], 2, CV_RGB(128,0,255), -1); + std::cout << "driven robot to trashbin" << std::endl; + // drive trash bin to trolley and back + double trash_bin_dist2 = 2. * planner.planPath(evaluation_data.floor_plan_, downsampled_map, trash_bin_sequence_in_this_room[t], trolley_position, evaluation_data.map_downsampling_factor_, 0., evaluation_data.map_resolution_, 1, &draw_path_map2); + path_length_robot += trash_bin_dist2; + path_length_trash_bins += trash_bin_dist1 + trash_bin_dist2; + std::cout << " room: " << room_index << "\ttrash bin: " << t << " at " << trash_bin_sequence_in_this_room[t] << "\ttraveling distance: " << (trash_bin_dist1 + trash_bin_dist2) * evaluation_data.map_resolution_ << " m" << std::endl; + screenoutput << " room: " << room_index << "\ttrash bin: " << t << " at " << trash_bin_sequence_in_this_room[t] << "\ttraveling distance: " << (trash_bin_dist1 + trash_bin_dist2) * evaluation_data.map_resolution_ << " m" << std::endl; + robot_position = trash_bin_sequence_in_this_room[t]; + // increase number of emptied trashbins by one + ++current_emptied_trashbins; + } + } + std::cout << " cleaned all rooms and trash bins in current clique" << std::endl; screenoutput << " cleaned all rooms and trash bins in current clique"; + } + // finally go back to trolley + path_length_robot += planner.planPath(evaluation_data.floor_plan_, downsampled_map, robot_position, trolley_position, evaluation_data.map_downsampling_factor_, 0., evaluation_data.map_resolution_, 1, &draw_path_map); + // and back to start position + path_length_trolley += planner.planPath(evaluation_data.floor_plan_, downsampled_map, trolley_position, robot_start_position, evaluation_data.map_downsampling_factor_, 0., evaluation_data.map_resolution_, 1, &draw_path_map); + + // evaluation + double path_length_robot_in_meter = path_length_robot * evaluation_data.map_resolution_; + double path_length_trolley_in_meter = path_length_trolley * evaluation_data.map_resolution_; + double path_length_total_in_meter = path_length_robot_in_meter + path_length_trolley_in_meter; + double path_length_trash_bins_in_meter = path_length_trash_bins * evaluation_data.map_resolution_; + double robot_speed_without_trolley = 0.3; // [m/s] + double robot_speed_with_trolley = 0.2; // [m/s] + double time_for_trashbin_manipulation = 150; // [s], without driving + double time_for_trolley_manipulation = 90; // [s], without driving + double time = path_length_robot_in_meter / robot_speed_without_trolley + + path_length_trolley_in_meter / robot_speed_with_trolley + + time_for_trashbin_manipulation * evaluation_data.trash_bin_locations_.size() + + time_for_trolley_manipulation * (result_seq->checkpoints.size()+1) + + switching_trolley_handling * time_for_trolley_manipulation; + + //get the runtimes for the segmentation and the sequence planner + double segmentation_time = (t1.tv_sec - t0.tv_sec) + (double) (t1.tv_nsec - t0.tv_nsec) * 1e-9; + double sequence_time = (t3.tv_sec - t2.tv_sec) + (double) (t3.tv_nsec - t2.tv_nsec) * 1e-9; + + // write log file + std::stringstream output; + // header + output << evaluation_configuration_vector[config].getConfigurationString(); + output << "robot_speed_without_trolley" << "\t" << "robot_speed_with_trolley" << "\t" << "time_for_trashbin_manipulation" << "\t" + << "time_for_trolley_manipulation" << "\t" << "number_of_trash_bins" << "\t" << "number_trolley_movements" << "\t" + << "path_length_robot_in_meter" << "\t" << "path_length_trolley_in_meter" << "\t" << "path_length_trash_bins_in_meter\t" + << "pathlength [m]" << "\t" + << "cleaning time [s]" << "\t" << "calculation time segmentation [s]" << "\t" << "calculation time sequencer [s]" << "\t" << std::endl; + output << robot_speed_without_trolley << "\t" << robot_speed_with_trolley << "\t" << time_for_trashbin_manipulation << "\t" << time_for_trolley_manipulation << "\t" + << evaluation_data.trash_bin_locations_.size() << "\t" + << (result_seq->checkpoints.size()+1) << "\t" << path_length_robot_in_meter << "\t" << path_length_trolley_in_meter << "\t" << path_length_trash_bins_in_meter << "\t" + << path_length_total_in_meter << "\t" + << time << "\t" << segmentation_time << "\t" << sequence_time; + output << "\n\n\n" << screenoutput.str() << std::endl; + + std::string log_filename = lower_path + evaluation_data.map_name_ + "_results.txt"; + std::ofstream file(log_filename.c_str(), std::ios::out); + if (file.is_open()==true) + file << output.str(); + else + ROS_ERROR("Error on writing file '%s'", log_filename.c_str()); + file.close(); + + // images: segmented_map, sequence_map + std::string segmented_map_filename = lower_path + evaluation_data.map_name_ + "_segmented.png"; + cv::Mat colour_segmented_map = segmented_map.clone(); + colour_segmented_map.convertTo(colour_segmented_map, CV_8U); + cv::cvtColor(colour_segmented_map, colour_segmented_map, CV_GRAY2BGR); + for(size_t i = 1; i <= result_seg->room_information_in_pixel.size(); ++i) + { + // choose random color for each room + int blue = (rand() % 250) + 1; + int green = (rand() % 250) + 1; + int red = (rand() % 250) + 1; + for(size_t u = 0; u < segmented_map.rows; ++u) + { + for(size_t v = 0; v < segmented_map.cols; ++v) + { + if(segmented_map.at(u,v) == i) + { + colour_segmented_map.at(u,v)[0] = blue; + colour_segmented_map.at(u,v)[1] = green; + colour_segmented_map.at(u,v)[2] = red; + } + } + } + } + // draw the room centers into the map + for(size_t i = 0; i < result_seg->room_information_in_pixel.size(); ++i) + { + cv::Point current_center (result_seg->room_information_in_pixel[i].room_center.x, result_seg->room_information_in_pixel[i].room_center.y); +#if CV_MAJOR_VERSION<=3 + cv::circle(colour_segmented_map, current_center, 2, CV_RGB(0,0,255), CV_FILLED); +#else + cv::circle(colour_segmented_map, current_center, 2, CV_RGB(0,0,255), cv::FILLED); +#endif + } + // color image in unique colour to show the segmentation + if (cv::imwrite(segmented_map_filename.c_str(), colour_segmented_map) == false) + ROS_ERROR("Error on writing file '%s'", segmented_map_filename.c_str()); + + std::string sequence_map_filename = lower_path + evaluation_data.map_name_ + "_sequence.png"; + cv_bridge::CvImagePtr cv_ptr_seq; + cv_ptr_seq = cv_bridge::toCvCopy(result_seq->sequence_map, sensor_msgs::image_encodings::BGR8); + cv::Mat sequence_map = cv_ptr_seq->image; + // draw in trash bins + for (size_t i=0; i& evaluation_configuration_vector, const EvaluationData& evaluation_data, const std::string& data_storage_path) + { + // go through each configuration for the given map + for(size_t config = 0; config < evaluation_configuration_vector.size(); ++config) + { + // prepare folders for storing results + const std::string upper_folder_name = evaluation_configuration_vector[config].generateUpperConfigurationFolderString() + "/"; + const std::string path = data_storage_path + upper_folder_name; + const std::string upper_command = "mkdir -p " + path; + int return_value = system(upper_command.c_str()); + + const std::string lower_folder_name = evaluation_configuration_vector[config].generateLowerConfigurationFolderString() + "/"; + const std::string lower_path = path + lower_folder_name; + const std::string lower_command = "mkdir -p " + lower_path; + return_value = system(lower_command.c_str()); + + std::cout << "\nCurrent Configuration:" << std::endl << "map: " << evaluation_data.map_name_ << "\tsegmentation algorithm: " + << evaluation_configuration_vector[config].room_segmentation_algorithm_ + << "\tplanning method: " << evaluation_configuration_vector[config].sequence_planning_method_ + << "\tTSP solver: " << evaluation_configuration_vector[config].tsp_solver_ + << "\tmaximal clique length: " << evaluation_configuration_vector[config].max_clique_path_length_ << std::endl; + + DynamicReconfigureClient drc(node_handle_, "/room_sequence_planning/room_sequence_planning_server/set_parameters", "/room_sequence_planning/room_sequence_planning_server/parameter_updates"); + drc.setConfig("maximum_clique_size", 10); + + AStarPlanner planner; + //variables for time measurement + struct timespec t0, t1, t2, t3; + + // 1. retrieve segmentation and check if the map has already been segmented + ipa_building_msgs::MapSegmentationResultConstPtr result_seg; + clock_gettime(CLOCK_MONOTONIC, &t0); //set time stamp before the segmentation + if(evaluation_configuration_vector[config].room_segmentation_algorithm_ == 1) + { + if(segmented_morph == false) + { + if (segmentFloorPlan(evaluation_data, evaluation_configuration_vector[config], result_seg_morph, t0) == false) + return false; + segmented_morph = true; + } + else + std::cout << "map has already been segmented" << std::endl; + result_seg = result_seg_morph; + } + else if (evaluation_configuration_vector[config].room_segmentation_algorithm_ == 2) + { + if(segmented_dist == false) + { + if (segmentFloorPlan(evaluation_data, evaluation_configuration_vector[config], result_seg_dist, t0) == false) + return false; + segmented_dist = true; + } + else + std::cout << "map has already been segmented" << std::endl; + result_seg = result_seg_dist; + } + else if (evaluation_configuration_vector[config].room_segmentation_algorithm_ == 3) + { + if(segmented_vor == false) + { + if (segmentFloorPlan(evaluation_data, evaluation_configuration_vector[config], result_seg_vor, t0) == false) + return false; + segmented_vor = true; + } + else + std::cout << "map has already been segmented" << std::endl; + result_seg = result_seg_vor; + } + else if (evaluation_configuration_vector[config].room_segmentation_algorithm_ == 4) + { + if(segmented_semant == false) + { + if (segmentFloorPlan(evaluation_data, evaluation_configuration_vector[config], result_seg_semant, t0) == false) + return false; + segmented_semant = true; + } + else + std::cout << "map has already been segmented" << std::endl; + result_seg = result_seg_semant; + } + else if (evaluation_configuration_vector[config].room_segmentation_algorithm_ == 5) + { + if(segmented_vrf == false) + { + if (segmentFloorPlan(evaluation_data, evaluation_configuration_vector[config], result_seg_vrf, t0) == false) + return false; + segmented_vrf = true; + } + else + std::cout << "map has already been segmented" << std::endl; + result_seg = result_seg_vrf; + } + clock_gettime(CLOCK_MONOTONIC, &t1); //set time stamp after the segmentation + std::cout << "Segmentation computed " << result_seg->room_information_in_pixel.size() << " rooms." << std::endl; + + //check for accessibility of the room centers from start position + cv::Mat downsampled_map; + Timer tim; + planner.downsampleMap(evaluation_data.floor_plan_, downsampled_map, evaluation_data.map_downsampling_factor_, evaluation_data.robot_radius_, evaluation_data.map_resolution_); + std::cout << "downsampling map: " << tim.getElapsedTimeInMilliSec() << " ms" << std::endl; + cv::Point robot_start_position((evaluation_data.robot_start_position_.position.x - evaluation_data.map_origin_.position.x)/evaluation_data.map_resolution_, + (evaluation_data.robot_start_position_.position.y - evaluation_data.map_origin_.position.y)/evaluation_data.map_resolution_); + + // get the reachable room centers as cv::Point + tim.start(); + std::vector room_centers; + for(size_t i = 0; i < result_seg->room_information_in_pixel.size(); ++i) + { + cv::Point current_center(result_seg->room_information_in_pixel[i].room_center.x, result_seg->room_information_in_pixel[i].room_center.y); + double length = planner.planPath(evaluation_data.floor_plan_, downsampled_map, robot_start_position, current_center, evaluation_data.map_downsampling_factor_, 0., evaluation_data.map_resolution_); + if(length < 1e9) + room_centers.push_back(current_center); + else + std::cout << "room " << i << " not accessible, center: " << current_center << std::endl; + } + std::cout << "room centers computed: " << tim.getElapsedTimeInMilliSec() << " ms" << std::endl; + + if(room_centers.size() == 0) //no room center is reachable for the given start position --> needs to be looked at separately + { + std::cout << "++++++++++ no roomcenter reachable from given startposition ++++++++++++" << std::endl; + return false; + } + + // 2. solve sequence problem + std::cout << "Starting to solve sequence problem." << std::endl; + tim.start(); + ipa_building_msgs::FindRoomSequenceWithCheckpointsResultConstPtr result_seq; + clock_gettime(CLOCK_MONOTONIC, &t2); //set time stamp before the sequence planning + if (computeRoomSequence(evaluation_data, evaluation_configuration_vector[config], evaluation_data.trash_bin_locations_, result_seq, t2) == false) + { + std::cout << "++++++++++ computeRoomSequence failed ++++++++++++" << std::endl; + return false; + } + std::cout << "sequence problem solved: " << tim.getElapsedTimeInMilliSec() << " ms" << std::endl; + clock_gettime(CLOCK_MONOTONIC, &t3); //set time stamp after the sequence planning + + // converting the map msg in cv format + cv_bridge::CvImagePtr cv_ptr_obj; + cv_ptr_obj = cv_bridge::toCvCopy(result_seg->segmented_map, sensor_msgs::image_encodings::TYPE_32SC1); + cv::Mat segmented_map = cv_ptr_obj->image; + + // 3. find rooms that belonging to a trolley position + std::vector assigned_rooms; + std::vector > rooms_for_trolley_position(result_seq->checkpoints.size()); + + for(size_t checkpoint = 0; checkpoint < result_seq->checkpoints.size(); ++checkpoint) + { + std::vector current_trashbins = result_seq->checkpoints[checkpoint].room_indices; + + for(size_t trash = 0; trash < current_trashbins.size(); ++trash) + { + int current_room_index = segmented_map.at(evaluation_data.trash_bin_locations_[current_trashbins[trash]]); + + // find romm center that is assigned with this room index + int room_vector_index = -1; + for(size_t room = 0; room < room_centers.size(); ++room) + { + if (segmented_map.at(room_centers[room]) == current_room_index) + { + room_vector_index = room; + break; + } + } + + // only save room centers that are reachable and haven't been saved yet + if(room_vector_index != -1) + { + if(contains(rooms_for_trolley_position[checkpoint], room_centers[room_vector_index]) == false) + rooms_for_trolley_position[checkpoint].push_back(room_centers[room_vector_index]); + + // save room as assigned, segmented map starts with index 1 for rooms + if(contains(assigned_rooms, current_room_index) == false) + assigned_rooms.push_back(current_room_index); + } + } + } + + // 4. check if a room has no trashbins in it, if so it hasn't been assigned to a trolley position yet and needs + // to be assigned now + for(size_t room = 0; room < room_centers.size(); ++room) + { + int current_room_index = segmented_map.at(room_centers[room]); + + if(contains(assigned_rooms, current_room_index) == false) + { + // find the trolley which is nearest to this center and assign the room to this trolley position + double smallest_distance = 1e6; + size_t trolley_index = 0; + for(size_t checkpoint = 0; checkpoint < result_seq->checkpoints.size(); ++checkpoint) + { + cv::Point trolley_position = cv::Point(result_seq->checkpoints[checkpoint].checkpoint_position_in_pixel.x, result_seq->checkpoints[checkpoint].checkpoint_position_in_pixel.y); + double current_distance = planner.planPath(evaluation_data.floor_plan_, downsampled_map, room_centers[room], trolley_position, evaluation_data.map_downsampling_factor_, 0., evaluation_data.map_resolution_);; + + if(current_distance < smallest_distance) + { + smallest_distance = current_distance; + trolley_index = checkpoint; + } + } + + // assign the room to the trolley + rooms_for_trolley_position[trolley_index].push_back(room_centers[room]); + } + } + + // 5. do the movements + drc.setConfig("maximum_clique_size", 9001); + cv::Mat draw_path_map, draw_path_map2; + cv::cvtColor(evaluation_data.floor_plan_, draw_path_map, CV_GRAY2BGR); + draw_path_map2 = draw_path_map.clone(); + double path_length_robot = 0.; + double path_length_trolley = 0.; + double path_length_trash_bins = 0.; + // const double max_clique_path_length_in_pixel = evaluation_configuration.max_clique_path_length_ / evaluation_data.map_resolution_; + cv::Point robot_position = robot_start_position; + cv::Point trolley_position((evaluation_data.robot_start_position_.position.x - evaluation_data.map_origin_.position.x)/evaluation_data.map_resolution_, + (evaluation_data.robot_start_position_.position.y - evaluation_data.map_origin_.position.y)/evaluation_data.map_resolution_); + cv::circle(draw_path_map, robot_position, 3, CV_RGB(0,0,255), -1); + + std::stringstream screenoutput; + std::vector done_rooms; + for (size_t clique_index = 0; clique_indexcheckpoints.size(); ++clique_index) + { + std::cout << "cleaning new clique" << std::endl; screenoutput << "cleaning new clique" << std::endl; + // mark new trolley position to empty the trashbins + trolley_position = cv::Point(result_seq->checkpoints[clique_index].checkpoint_position_in_pixel.x, result_seq->checkpoints[clique_index].checkpoint_position_in_pixel.y); + cv::circle(draw_path_map, trolley_position, 3, CV_RGB(0,0,255), -1); + // compute optimal room visiting order + std::vector current_room_order; + ipa_building_msgs::FindRoomSequenceWithCheckpointsResultConstPtr result_room_seq; + if(computeTrashBinSequence(evaluation_data, evaluation_configuration_vector[config], rooms_for_trolley_position[clique_index], robot_position, result_room_seq) == true) + { + for (size_t cc=0; cccheckpoints.size(); ++cc) + for (size_t tt = 0; tt < result_room_seq->checkpoints[cc].room_indices.size(); ++tt) + current_room_order.push_back(rooms_for_trolley_position[clique_index][result_room_seq->checkpoints[cc].room_indices[tt]]); + + } + + + // move robot to rooms if they haven't been cleaned yet + for(size_t room = 0; room < current_room_order.size(); ++room) + { + cv::Point current_roomcenter = current_room_order[room]; + + // find room that contains this trashbin + int room_index = segmented_map.at(current_roomcenter); + std::cout << "room index: " << room_index << std::endl; + + // check if room has been visited already, if so only empty trashbins in this room + if(contains(done_rooms, room_index) == false) + { + // room has not been cleaned yet + path_length_robot += planner.planPath(evaluation_data.floor_plan_, downsampled_map, robot_position, current_roomcenter, evaluation_data.map_downsampling_factor_, 0., evaluation_data.map_resolution_, 1, &draw_path_map); + robot_position = current_roomcenter; + cv::circle(draw_path_map, robot_position, 3, CV_RGB(0,255,0), -1); + + // mark room as cleaned + done_rooms.push_back(room_index); + } + + // find trashbins in the current room that belong to the current trolley checkpoint + // note: when iterating from the start of the optimal trashbin sequence of one trolleyposition the + // order of the trashbins after this asignment should be optimal + std::vector trashbins_for_room; + for(size_t trash = 0; trash < result_seq->checkpoints[clique_index].room_indices.size(); ++trash) + { + cv::Point trashbin = evaluation_data.trash_bin_locations_[result_seq->checkpoints[clique_index].room_indices[trash]]; + if(room_index == segmented_map.at(trashbin)) + { + // empty trashbin if it belongs to this room +// trashbins_for_room.push_back(trashbin); + path_length_robot += planner.planPath(evaluation_data.floor_plan_, downsampled_map, robot_position, trashbin, evaluation_data.map_downsampling_factor_, 0., evaluation_data.map_resolution_, 1, &draw_path_map); + path_length_trash_bins += 2.0 * planner.planPath(evaluation_data.floor_plan_, downsampled_map, trashbin, trolley_position, evaluation_data.map_downsampling_factor_, 0., evaluation_data.map_resolution_, 1, &draw_path_map); + robot_position = trashbin; + cv::circle(draw_path_map, trashbin, 2, CV_RGB(128,0,255), -1); + } + } + } + std::cout << " cleaned all rooms and trash bins in current clique" << std::endl; screenoutput << " cleaned all rooms and trash bins in current clique"; + } + // finally go back to start position + path_length_robot += planner.planPath(evaluation_data.floor_plan_, downsampled_map, robot_position, robot_start_position, evaluation_data.map_downsampling_factor_, 0., evaluation_data.map_resolution_, 1, &draw_path_map); + + // evaluation + double path_length_robot_in_meter = path_length_robot * evaluation_data.map_resolution_; + double path_length_trolley_in_meter = path_length_trolley * evaluation_data.map_resolution_; + double path_length_total_in_meter = path_length_robot_in_meter; + double path_length_trash_bins_in_meter = path_length_trash_bins * evaluation_data.map_resolution_; + double robot_speed_without_trolley = 0.3; // [m/s] + double robot_speed_with_trolley = 0.2; // [m/s] + double time_for_trashbin_manipulation = 150; // [s], without driving + double time_for_trolley_manipulation = 90; // [s], without driving + double time = path_length_robot_in_meter / robot_speed_without_trolley + + time_for_trashbin_manipulation * evaluation_data.trash_bin_locations_.size(); + + //get the runtimes for the segmentation and the sequence planner + double segmentation_time = (t1.tv_sec - t0.tv_sec) + (double) (t1.tv_nsec - t0.tv_nsec) * 1e-9; + double sequence_time = (t3.tv_sec - t2.tv_sec) + (double) (t3.tv_nsec - t2.tv_nsec) * 1e-9; + + // write log file + std::stringstream output; + // header + output << evaluation_configuration_vector[config].getConfigurationString(); + output << "robot_speed_without_trolley" << "\t" << "robot_speed_with_trolley" << "\t" << "time_for_trashbin_manipulation" << "\t" + << "time_for_trolley_manipulation" << "\t" << "number_of_trash_bins" << "\t" << "number_trolley_movements" << "\t" + << "path_length_robot_in_meter" << "\t" << "path_length_trolley_in_meter" << "\t" << "path_length_trash_bins_in_meter\t" + << "pathlength [m]" << "\t" + << "cleaning time [s]" << "\t" << "calculation time segmentation [s]" << "\t" << "calculation time sequencer [s]" << "\t" << std::endl; + output << robot_speed_without_trolley << "\t" << robot_speed_with_trolley << "\t" << time_for_trashbin_manipulation << "\t" << time_for_trolley_manipulation << "\t" + << evaluation_data.trash_bin_locations_.size() << "\t" + << 0 << "\t" << path_length_robot_in_meter << "\t" << path_length_trolley_in_meter << "\t" << path_length_trash_bins_in_meter << "\t" + << path_length_total_in_meter << "\t" + << time << "\t" << segmentation_time << "\t" << sequence_time; + output << "\n\n\n" << screenoutput.str() << std::endl; + + std::string log_filename = lower_path + evaluation_data.map_name_ + "_results.txt"; + std::ofstream file(log_filename.c_str(), std::ios::out); + if (file.is_open()==true) + file << output.str(); + else + ROS_ERROR("Error on writing file '%s'", log_filename.c_str()); + file.close(); + + // compute travel distance for humans and save them + double travel_distance_human = 0.0; + for(size_t pos = 0; pos < result_seq->checkpoints.size(); ++pos) + { + cv::Point trolley_placing_position = cv::Point(result_seq->checkpoints[pos].checkpoint_position_in_pixel.x, result_seq->checkpoints[pos].checkpoint_position_in_pixel.y); + travel_distance_human += 2.0 * planner.planPath(evaluation_data.floor_plan_, downsampled_map, trolley_placing_position, robot_start_position, evaluation_data.map_downsampling_factor_, 0., evaluation_data.map_resolution_, 1); + } + travel_distance_human = travel_distance_human * evaluation_data.map_resolution_; + std::string storage_path_human = data_storage_path + "human_way/" + upper_folder_name + evaluation_configuration_vector[config].generateLowerConfigurationFolderString() + "/"; + const std::string upper_human_command = "mkdir -p " + storage_path_human; + return_value = system(upper_human_command.c_str()); + storage_path_human = storage_path_human + evaluation_data.map_name_ + "_human.txt"; + std::stringstream human_distance_output; + human_distance_output << "dist to place all trolleys [m]: " << std::endl << travel_distance_human; + std::ofstream human_file(storage_path_human.c_str(), std::ios::out); + if (human_file.is_open()==true) + human_file << human_distance_output.str(); + else + ROS_ERROR("Error on writing file '%s'", storage_path_human.c_str()); + human_file.close(); + + // images: segmented_map, sequence_map + std::string segmented_map_filename = lower_path + evaluation_data.map_name_ + "_segmented.png"; + cv::Mat colour_segmented_map = segmented_map.clone(); + colour_segmented_map.convertTo(colour_segmented_map, CV_8U); + cv::cvtColor(colour_segmented_map, colour_segmented_map, CV_GRAY2BGR); + for(size_t i = 1; i <= result_seg->room_information_in_pixel.size(); ++i) + { + // choose random color for each room + int blue = (rand() % 250) + 1; + int green = (rand() % 250) + 1; + int red = (rand() % 250) + 1; + for(size_t u = 0; u < segmented_map.rows; ++u) + { + for(size_t v = 0; v < segmented_map.cols; ++v) + { + if(segmented_map.at(u,v) == i) + { + colour_segmented_map.at(u,v)[0] = blue; + colour_segmented_map.at(u,v)[1] = green; + colour_segmented_map.at(u,v)[2] = red; + } + } + } + } + // draw the room centers into the map + for(size_t i = 0; i < result_seg->room_information_in_pixel.size(); ++i) + { + cv::Point current_center (result_seg->room_information_in_pixel[i].room_center.x, result_seg->room_information_in_pixel[i].room_center.y); +#if CV_MAJOR_VERSION<=3 + cv::circle(colour_segmented_map, current_center, 2, CV_RGB(0,0,255), CV_FILLED); +#else + cv::circle(colour_segmented_map, current_center, 2, CV_RGB(0,0,255), cv::FILLED); +#endif + } + // color image in unique colour to show the segmentation + if (cv::imwrite(segmented_map_filename.c_str(), colour_segmented_map) == false) + ROS_ERROR("Error on writing file '%s'", segmented_map_filename.c_str()); + + std::string sequence_map_filename = lower_path + evaluation_data.map_name_ + "_sequence.png"; + cv_bridge::CvImagePtr cv_ptr_seq; + cv_ptr_seq = cv_bridge::toCvCopy(result_seq->sequence_map, sensor_msgs::image_encodings::BGR8); + cv::Mat sequence_map = cv_ptr_seq->image; + // draw in trash bins + for (size_t i=0; i ac_seg("/room_segmentation/room_segmentation_server", true); + ROS_INFO("Waiting for action server '/room_segmentation/room_segmentation_server' to start."); + ac_seg.waitForServer(ros::Duration(60)); // wait for the action server to start, will wait for infinite time + std::cout << "Action server started, sending goal_seg." << std::endl; + ros::Duration s(0.5); + s.sleep(); + + // send dynamic reconfigure config + ROS_INFO("Trying to connect to dynamic reconfigure server."); + DynamicReconfigureClient drc(node_handle_, "/room_segmentation/room_segmentation_server/set_parameters", "/room_segmentation/room_segmentation_server/parameter_updates"); + ROS_INFO("Done connecting to the dynamic reconfigure server."); + const int room_segmentation_algorithm = evaluation_configuration.room_segmentation_algorithm_; + drc.setConfig("room_segmentation_algorithm", room_segmentation_algorithm); + if(room_segmentation_algorithm == 1) //morpho + { + drc.setConfig("room_area_factor_lower_limit_morphological", 0.8); + drc.setConfig("room_area_factor_upper_limit_morphological", 47.0); + ROS_INFO("You have chosen the morphological segmentation."); + } + if(room_segmentation_algorithm == 2) //distance + { + drc.setConfig("room_area_factor_lower_limit_distance", 0.35); + drc.setConfig("room_area_factor_upper_limit_distance", 163.0); + ROS_INFO("You have chosen the distance segmentation."); + } + if(room_segmentation_algorithm == 3) //voronoi + { + drc.setConfig("room_area_factor_lower_limit_voronoi", 0.1); //1.53; + drc.setConfig("room_area_factor_upper_limit_voronoi", 1000000.); //120.0; + drc.setConfig("voronoi_neighborhood_index", 280); + drc.setConfig("max_iterations", 150); + drc.setConfig("min_critical_point_distance_factor", 0.5); //1.6; + drc.setConfig("max_area_for_merging", 12.5); + ROS_INFO("You have chosen the Voronoi segmentation"); + } + if(room_segmentation_algorithm == 4) //semantic + { + drc.setConfig("room_area_factor_lower_limit_semantic", 1.0); + drc.setConfig("room_area_factor_upper_limit_semantic", 1000000.);//23.0; + ROS_INFO("You have chosen the semantic segmentation."); + } + if(room_segmentation_algorithm == 5) //voronoi random field + { + drc.setConfig("room_area_lower_limit_voronoi_random", 1.53); //1.53 + drc.setConfig("room_area_upper_limit_voronoi_random", 1000000.); //1000000.0 + drc.setConfig("max_iterations", 150); + drc.setConfig("voronoi_random_field_epsilon_for_neighborhood", 7); + drc.setConfig("min_neighborhood_size", 5); + drc.setConfig("min_voronoi_random_field_node_distance", 7.0); // [pixel] + drc.setConfig("max_voronoi_random_field_inference_iterations", 9000); + drc.setConfig("max_area_for_merging", 12.5); + ROS_INFO("You have chosen the Voronoi random field segmentation."); + } + drc.setConfig("display_segmented_map", false); + + // send a goal to the action + ipa_building_msgs::MapSegmentationGoal goal_seg; + goal_seg.input_map = map_msg; + goal_seg.map_origin = evaluation_data.map_origin_; + goal_seg.map_resolution = evaluation_data.map_resolution_; + goal_seg.return_format_in_meter = false; + goal_seg.return_format_in_pixel = true; + //goal_seg.room_segmentation_algorithm = evaluation_configuration.room_segmentation_algorithm_; + goal_seg.robot_radius = evaluation_data.robot_radius_; + ac_seg.sendGoal(goal_seg); + + //wait for the action to return + bool finished_before_timeout = ac_seg.waitForResult(ros::Duration(600.0 + loopcounter * 100.0)); + if (finished_before_timeout == false) //if it takes too long the server should be killed and restarted + { + std::cout << "action server took too long" << std::endl; + std::string pid_cmd = "pidof room_segmentation_server > room_sequence_planning/seg_srv_pid.txt"; + int pid_result = system(pid_cmd.c_str()); + std::ifstream pid_reader("room_sequence_planning/seg_srv_pid.txt"); + int value; + std::string line; + if (pid_reader.is_open()) + { + while (getline(pid_reader, line)) + { + std::istringstream iss(line); + while (iss >> value) + { + std::cout << "PID of room_segmentation_server: " << value << std::endl; + std::stringstream ss; + ss << "kill " << value; + std::string kill_cmd = ss.str(); + int kill_result = system(kill_cmd.c_str()); + std::cout << "kill result: " << kill_result << std::endl; + } + } + pid_reader.close(); + remove("room_sequence_planning/seg_srv_pid.txt"); + } + else + { + std::cout << "missing logfile" << std::endl; + } + } + else // segmentation finished while given time --> return result + { + result_seg = ac_seg.getResult(); + segmented = true; + std::cout << "Finished segmentation successfully!" << std::endl; + } + ++loopcounter; //enlarge the loop counter so the client will wait longer for the server to start + }while(segmented == false && loopcounter <= 6); + + if(loopcounter > 6) + return false; + + return true; + } + + bool computeTrashBinSequence(const EvaluationData& evaluation_data, const EvaluationConfig& evaluation_configuration, + const std::vector& reachable_roomcenters, const cv::Point& robot_start_position, + ipa_building_msgs::FindRoomSequenceWithCheckpointsResultConstPtr& result_seq) + { + bool planned = false; + + sensor_msgs::Image map_msg; + cv_bridge::CvImage cv_image; + cv_image.encoding = "mono8"; + cv_image.image = evaluation_data.floor_plan_; + cv_image.toImageMsg(map_msg); + + actionlib::SimpleActionClient ac_seq("/room_sequence_planning/room_sequence_planning_server", true); + ROS_INFO("Waiting for action server '/room_sequence_planning/room_sequence_planning_server' to start."); + // wait for the action server to start + ac_seq.waitForServer(ros::Duration(3.0)); //will wait for infinite time + ROS_INFO("Action server for trashbin sequence planning found."); + ros::Duration s(0.5); + s.sleep(); + + //put the vector format in the msg format + std::vector roomcenters_for_sequence_planning(reachable_roomcenters.size()); + for(size_t room = 0; room < reachable_roomcenters.size(); ++room) + { + roomcenters_for_sequence_planning[room].room_center.x = reachable_roomcenters[room].x; + roomcenters_for_sequence_planning[room].room_center.y = reachable_roomcenters[room].y; + } + + // set algorithm parameters + ROS_INFO("Trying to connect to dynamic reconfigure server."); + DynamicReconfigureClient drc(node_handle_, "/room_sequence_planning/room_sequence_planning_server/set_parameters", "/room_sequence_planning/room_sequence_planning_server/parameter_updates"); + ROS_INFO("Done connecting to the dynamic reconfigure server."); + drc.setConfig("max_clique_path_length", 1e9); + drc.setConfig("map_downsampling_factor", evaluation_data.map_downsampling_factor_); + drc.setConfig("planning_method", 1); + drc.setConfig("tsp_solver", evaluation_configuration.tsp_solver_); + drc.setConfig("return_sequence_map", false); + drc.setConfig("check_accessibility_of_rooms", false); + +// std::cout << "Action server started, sending goal_seq." << std::endl; + // send a goal_seg to the action + ipa_building_msgs::FindRoomSequenceWithCheckpointsGoal goal_seq; + goal_seq.input_map = map_msg; + goal_seq.map_resolution = evaluation_data.map_resolution_; + goal_seq.map_origin = evaluation_data.map_origin_; + goal_seq.room_information_in_pixel = roomcenters_for_sequence_planning; + goal_seq.robot_radius = 0.; + goal_seq.robot_start_coordinate.position.x = robot_start_position.x*evaluation_data.map_resolution_ + evaluation_data.map_origin_.position.x; + goal_seq.robot_start_coordinate.position.y = robot_start_position.y*evaluation_data.map_resolution_ + evaluation_data.map_origin_.position.y; + ac_seq.sendGoal(goal_seq); + + //wait for the action to return + bool finished_before_timeout = ac_seq.waitForResult(ros::Duration(30.0 )); + if (finished_before_timeout == true) //if it takes too long the server should be killed and restarted + { + result_seq = ac_seq.getResult(); +// std::cout << "Finished trash bin sequence planning successfully!" << std::endl; + planned = true; + } + + return planned; + } + + bool computeRoomSequence(const EvaluationData& evaluation_data, const EvaluationConfig& evaluation_configuration, + const std::vector& reachable_roomcenters, + ipa_building_msgs::FindRoomSequenceWithCheckpointsResultConstPtr& result_seq, struct timespec& t2) + { + bool planned = false; + int loopcounter = 0; + do + { + clock_gettime(CLOCK_MONOTONIC, &t2); + sensor_msgs::Image map_msg; + cv_bridge::CvImage cv_image; + cv_image.encoding = "mono8"; + cv_image.image = evaluation_data.floor_plan_; + cv_image.toImageMsg(map_msg); + + actionlib::SimpleActionClient ac_seq("/room_sequence_planning/room_sequence_planning_server", true); + ROS_INFO("Waiting for action server '/room_sequence_planning/room_sequence_planning_server' to start."); + // wait for the action server to start + ac_seq.waitForServer(ros::Duration(60)); //will wait for infinite time + ros::Duration s(0.5); + s.sleep(); + + //put the vector format in the msg format + std::vector roomcenters_for_sequence_planning(reachable_roomcenters.size()); + for(size_t room = 0; room < reachable_roomcenters.size(); ++room) + { + roomcenters_for_sequence_planning[room].room_center.x = reachable_roomcenters[room].x; + roomcenters_for_sequence_planning[room].room_center.y = reachable_roomcenters[room].y; + } + + // set algorithm parameters + ROS_INFO("Trying to connect to dynamic reconfigure server."); + DynamicReconfigureClient drc(node_handle_, "/room_sequence_planning/room_sequence_planning_server/set_parameters", "/room_sequence_planning/room_sequence_planning_server/parameter_updates"); + ROS_INFO("Done conencting to the dynamic reconfigure server."); + drc.setConfig("max_clique_path_length", evaluation_configuration.max_clique_path_length_); + drc.setConfig("map_downsampling_factor", evaluation_data.map_downsampling_factor_); + drc.setConfig("planning_method", evaluation_configuration.sequence_planning_method_); + drc.setConfig("tsp_solver", evaluation_configuration.tsp_solver_); + drc.setConfig("return_sequence_map", true); + drc.setConfig("check_accessibility_of_rooms", false); + + std::cout << "Action server started, sending goal_seq." << std::endl; + // send a goal_seg to the action + ipa_building_msgs::FindRoomSequenceWithCheckpointsGoal goal_seq; + goal_seq.input_map = map_msg; + goal_seq.map_resolution = evaluation_data.map_resolution_; + goal_seq.map_origin = evaluation_data.map_origin_; + goal_seq.room_information_in_pixel = roomcenters_for_sequence_planning; + goal_seq.robot_radius = robot_radius_; + goal_seq.robot_start_coordinate = evaluation_data.robot_start_position_; + ac_seq.sendGoal(goal_seq); + + //wait for the action to return + bool finished_before_timeout = ac_seq.waitForResult(ros::Duration(2400.0 + 200 * loopcounter)); + if (finished_before_timeout == false) //if it takes too long the server should be killed and restarted + { + std::cout << "action server took too long" << std::endl; + std::string pid_cmd = "pidof room_sequence_planning_server > room_sequence_planning/seq_srv_pid.txt"; + int pid_result = system(pid_cmd.c_str()); + std::ifstream pid_reader("room_sequence_planning/seq_srv_pid.txt"); + int value; + std::string line; + if (pid_reader.is_open()) + { + while (getline(pid_reader, line)) + { + std::istringstream iss(line); + while (iss >> value) + { + std::cout << "PID of room_sequence_planning_server: " << value << std::endl; + std::stringstream ss; + ss << "kill " << value; + std::string kill_cmd = ss.str(); + int kill_result = system(kill_cmd.c_str()); + std::cout << "kill result: " << kill_result << std::endl; + } + } + pid_reader.close(); + remove("room_sequence_planning/seq_srv_pid.txt"); + } + else + { + std::cout << "missing logfile" << std::endl; + } + } + else + { + result_seq = ac_seq.getResult(); + std::cout << "Finished sequence planning successfully!" << std::endl; + planned = true; + } + ++loopcounter; + }while(planned == false && loopcounter <= 6); + + if(loopcounter > 6) + return false; + + return true; + } + + +private: + + ros::NodeHandle node_handle_; + + std::vector< EvaluationData > evaluation_data_; + + const double robot_radius_; +}; + + + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "room_sequence_planning_client"); + ros::NodeHandle nh; + + const std::string test_map_path = ros::package::getPath("ipa_room_segmentation") + "/common/files/test_maps/"; + const std::string data_storage_path = "room_sequence_planning/"; + Evaluation ev(nh, test_map_path, data_storage_path, 0.3); + ros::shutdown(); + + //exit + return 0; +} diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/ros/src/tester.cpp b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/ros/src/tester.cpp new file mode 100644 index 0000000..86ce050 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/ros/src/tester.cpp @@ -0,0 +1,215 @@ +#include "ros/ros.h" +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include +#include +#include +#include + +#include +#include + +#include + +int main(int argc, char **argv) +{ + srand(5); //time(NULL)); + ros::init(argc, argv, "a_star_tester"); + ros::NodeHandle nh; + + cv::Mat map = cv::imread("/home/rmb-fj/Pictures/maps/black_map.png", 0); + + AStarPlanner planner; + NearestNeighborTSPSolver TSPsolver; + GeneticTSPSolver genTSPsolver; + ConcordeTSPSolver conTSPsolver; + + cliqueFinder finder; //Object to find all maximal cliques for the given map + + SetCoverSolver setsolver; //Object to find the groups based on the found cliques + + TrolleyPositionFinder tolley_finder; + + std::vector < cv::Point > centers; + + int n = 9; + double downfactor = 0.25; + double map_resolution_factor = 0.05; + double robot_radius_test = 0.5; + + cv::Mat pathlengths(cv::Size(n, n), CV_64F); + cv::Mat distancematrix; +// cv::Mat eroded_map; +// +// cv::erode(map, eroded_map, cv::Mat(), cv::Point(-1, -1), 4); + + //Testcenters: +// x: 494 y: 535 +// x: 218 y: 176 +// x: 152 y: 148 +// x: 475 y: 417 +// x: 342 y: 333 +// x: 283 y: 205 +// x: 149 y: 229 +// x: 201 y: 456 +// x: 286 y: 125 + +// for (int i = 0; i < n; i++) //add Points for TSP to test the solvers +// { +// bool done = false; +// do +// { +// int x = rand() % map.cols; +// int y = rand() % map.rows; +// if (eroded_map.at(y, x) == 255) +// { +// centers.push_back(cv::Point(x, y)); +// done = true; +// } +// } while (!done); +// } + + centers.push_back(cv::Point(494, 535)); + centers.push_back(cv::Point(218, 176)); + centers.push_back(cv::Point(152, 148)); + centers.push_back(cv::Point(475, 417)); + centers.push_back(cv::Point(342, 333)); + centers.push_back(cv::Point(283, 205)); + centers.push_back(cv::Point(149, 229)); + centers.push_back(cv::Point(201, 456)); + centers.push_back(cv::Point(286, 125)); + + std::vector nearest_neighbor_order = genTSPsolver.solveGeneticTSP(map, n, centers, downfactor, robot_radius_test, map_resolution_factor, 0); + + std::cout << "without distance matrix:" << std::endl; + for (int i = 0; i < nearest_neighbor_order.size(); i++) + { + std::cout << nearest_neighbor_order[i] << std::endl; + } + std::cout << std::endl; + + nearest_neighbor_order = genTSPsolver.solveGeneticTSP(map, n, centers, downfactor, robot_radius_test, map_resolution_factor, 0, distancematrix); + + std::cout << "without distance matrix, returned:" << std::endl; + for (int i = 0; i < nearest_neighbor_order.size(); i++) + { + std::cout << nearest_neighbor_order[i] << std::endl; + } + std::cout << std::endl; + + std::cout << "distance matrix out of solver: " << std::endl; + for (int row = 0; row < distancematrix.rows; row++) + { + for (int col = 0; col < distancematrix.cols; col++) + { + std::cout << distancematrix.at(row, col) << " "; + } + std::cout << std::endl; + } + std::cout << std::endl; + +// cv::Mat testermap = map.clone(); +// + for (int i = 0; i < centers.size(); i++) + { + cv::Point center = centers[i]; + for (int p = 0; p < centers.size(); p++) + { + if (p != i) + { + if (p > i) //only compute upper right triangle of matrix, rest is symmetrically added + { + double length = planner.planPath(map, center, centers[p], downfactor, robot_radius_test, map_resolution_factor); + pathlengths.at(i, p) = length; + pathlengths.at(p, i) = length; //symmetrical-Matrix --> saves half the computationtime + } + } + else + { + pathlengths.at(i, p) = 0; + } + } + } + + nearest_neighbor_order = genTSPsolver.solveGeneticTSP(pathlengths, 0); + + std::cout << "with distance matrix:" << std::endl; + for (int i = 0; i < nearest_neighbor_order.size(); i++) + { + std::cout << nearest_neighbor_order[i] << std::endl; + } + std::cout << std::endl; + + std::cout << "distance matrix out of main: " << std::endl; + for (int row = 0; row < pathlengths.rows; row++) + { + for (int col = 0; col < pathlengths.cols; col++) + { + std::cout << pathlengths.at(row, col) << " "; + } + std::cout << std::endl; + } + std::cout << std::endl; + + std::vector < std::vector > cliques = finder.getCliques(pathlengths, 150.0); +// std::cout << "All maximum cliques in the graph:" << std::endl; +// +// for (int i = 0; i < cliques.size(); i++) +// { +// for (int j = 0; j < cliques[i].size(); j++) +// { +// std::cout << cliques[i][j] << std::endl; +// } +// std::cout << std::endl; +// } +// + ROS_INFO("Starting to solve the setcover problem."); + + std::vector > groups = setsolver.solveSetCover(cliques, n); + std::vector > new_groups = setsolver.solveSetCover(map, n, centers, downfactor, robot_radius_test, map_resolution_factor, 150.0); + + ROS_INFO("Starting to find the trolley positions."); + + std::vector trolley_positions = tolley_finder.findTrolleyPositions(map, groups, centers, downfactor, robot_radius_test, map_resolution_factor); + std::vector new_trolleys = tolley_finder.findTrolleyPositions(map, new_groups, centers, downfactor, robot_radius_test, map_resolution_factor); + + std::cout << "groups from new method" << std::endl; + + for(int i = 0; i < new_groups.size(); i++) + { + for(int j = 0; j < new_groups[i].size(); j++) + { + std::cout << new_groups[i][j] << std::endl; + } + std::cout << "group done. trolley position: " << new_trolleys[i] << std::endl << std::endl; + } + + std::cout << "groups from old method" << std::endl; + + for(int i = 0; i < groups.size(); i++) + { + for(int j = 0; j < groups[i].size(); j++) + { + std::cout << groups[i][j] << std::endl; + } + std::cout << "group done. trolley position: " << trolley_positions[i] << std::endl; + } +// +// cv::imwrite("/home/rmb-fj/Pictures/TSP/genetic.png", testermap); + + return 0; +} diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/ros/test/room_sequence_planning_action_client.launch b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/ros/test/room_sequence_planning_action_client.launch new file mode 100644 index 0000000..c5e8b1b --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/ros/test/room_sequence_planning_action_client.launch @@ -0,0 +1,7 @@ + + + + + + + \ No newline at end of file diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/ros/test/room_sequence_planning_evaluation.launch b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/ros/test/room_sequence_planning_evaluation.launch new file mode 100644 index 0000000..9788e89 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_building_navigation/ros/test/room_sequence_planning_evaluation.launch @@ -0,0 +1,7 @@ + + + + + + + \ No newline at end of file diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_coverage_planning/CMakeLists.txt b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_coverage_planning/CMakeLists.txt new file mode 100644 index 0000000..28f230f --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_coverage_planning/CMakeLists.txt @@ -0,0 +1,4 @@ +cmake_minimum_required(VERSION 2.8.3) +project(ipa_coverage_planning) +find_package(catkin REQUIRED) +catkin_metapackage() diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_coverage_planning/package.xml b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_coverage_planning/package.xml new file mode 100644 index 0000000..c9a3077 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_coverage_planning/package.xml @@ -0,0 +1,22 @@ + + + ipa_coverage_planning + 0.1.0 + This meta-package includes open source software for efficient coverage (cleaning, inspection, ...) of buildings. + LGPL for academic and non-commercial use; for commercial use, please contact Fraunhofer IPA + + + Florian Jordan + Richard Bormann + + catkin + + ipa_building_msgs + ipa_building_navigation + ipa_room_exploration + ipa_room_segmentation + + + + + diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/CHANGELOG.rst b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/CHANGELOG.rst new file mode 100644 index 0000000..2bcc44d --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/CHANGELOG.rst @@ -0,0 +1,15 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package ipa_room_exploration +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.1.2 (2017-02-13) +------------------ +* rotation of the map in flowNetworkExplorator +* Contributors: Florian Jordan +0.1.1 (2017-02-10) +------------------ +* excluded coverage check to standalone service server +* Contributors: Florian Jordan +0.1.0 (2017-02-09) +------------------ +* first completely working version of package +* Contributors: Florian Jordan, Richard Bormann diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/CMakeLists.txt b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/CMakeLists.txt new file mode 100644 index 0000000..5b455d5 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/CMakeLists.txt @@ -0,0 +1,320 @@ +cmake_minimum_required(VERSION 2.8.3) +project(ipa_room_exploration) + +# build with c++11 +add_compile_options(-std=c++11) + +set(catkin_RUN_PACKAGES + angles + cob_map_accessibility_analysis + cv_bridge + eigen_conversions + geometry_msgs + ipa_building_msgs + ipa_building_navigation + laser_geometry + move_base_msgs + nav_msgs + roscpp + roslib + sensor_msgs + std_msgs + std_srvs + tf + visualization_msgs + # message_generation + # message_runtime +) + +set(catkin_BUILD_PACKAGES + ${catkin_RUN_PACKAGES} + dynamic_reconfigure +) +## Find catkin macros and libraries +find_package(catkin REQUIRED COMPONENTS + ${catkin_BUILD_PACKAGES} +) +# add_message_files( +# FILES +# dis_info.msg +# dis_info_array.msg +# ) + +# generate_messages( +# DEPENDENCIES +# std_msgs +# ) +generate_dynamic_reconfigure_options( + cfg/RoomExploration.cfg + cfg/CoverageMonitor.cfg +) + +find_package(OpenCV REQUIRED) +find_package(Boost REQUIRED COMPONENTS system chrono thread) + + +find_package(Eigen3 ) +if(NOT EIGEN3_FOUND) + # Fallback to cmake_modules + find_package(cmake_modules REQUIRED) + find_package(Eigen REQUIRED) + set(EIGEN3_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS}) + set(EIGEN3_LIBRARIES ${EIGEN_LIBRARIES}) # Not strictly necessary as Eigen is head only + # Possibly map additional variables to the EIGEN3_ prefix. +else() + set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR}) +endif() + +# include the FindGUROBI.cmake file to search for Gurobi on the system +set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH};${PROJECT_SOURCE_DIR}/cmake/") +find_package(GUROBI) + +# if Gurobi was found, set a flag to use it +if(GUROBI_FOUND) + add_definitions(-DGUROBI_FOUND=${GUROBI_FOUND}) +endif(GUROBI_FOUND) +Message(STATUS "Gurobi include dirs: " ${GUROBI_INCLUDE_DIRS}) +Message(STATUS "Gurobi lib dirs: " ${GUROBI_LIBRARIES}) + +# Coin-Or and Cbc, included as ubuntu system package, without Cmake-module so include pkg-config file +# !!! Important note: the order of the libraries when linking your executable is important, if it is wrong you get undefined references !!! +find_package(PkgConfig REQUIRED) +pkg_check_modules(CoinUtils REQUIRED coinutils)# coinutils osi-clp clp osi cbc cgl +pkg_check_modules(OsiClp REQUIRED osi-clp) +pkg_check_modules(Clp REQUIRED clp) +pkg_check_modules(Osi REQUIRED osi) +pkg_check_modules(Cbc-lib REQUIRED cbc) +pkg_check_modules(Cgl REQUIRED cgl) +Message(STATUS "coinutils include dirs: " ${CoinUtils_INCLUDE_DIRS}) +Message(STATUS "osi-clp include dirs: " ${OsiClp_INCLUDE_DIRS}) +Message(STATUS "clp include dirs: " ${Clp_INCLUDE_DIRS}) +Message(STATUS "osi include dirs: " ${Osi_INCLUDE_DIRS}) +Message(STATUS "cbc include dirs: " ${Cbc-lib_INCLUDE_DIRS}) +Message(STATUS "cgl include dirs: " ${Cgl_INCLUDE_DIRS}) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if you package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +# important: common/include needs to be here if you have such a directory +catkin_package( +INCLUDE_DIRS + common/include + ros/include +LIBRARIES + ${PROJECT_NAME} + libcoverage_check_server +CATKIN_DEPENDS + ${catkin_RUN_PACKAGES} + # message_runtime +DEPENDS + OpenCV + Boost + CoinUtils + OsiClp + Clp + Osi + Cgl + Cbc-lib +) + + +########### +## Build ## +########### +## Specify additional locations of header files +## Your package locations should be listed before other locations +# important: common/include also needs to be here if you have it, order of including the coinor-packages relevant, not working when including in wrong order +include_directories( + common/include + ros/include + ${catkin_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} + ${GUROBI_INCLUDE_DIRS} + ${CoinUtils_INCLUDE_DIRS} + ${OsiClp_INCLUDE_DIRS} + ${Clp_INCLUDE_DIRS} + ${Osi_INCLUDE_DIRS} + ${Cgl_INCLUDE_DIRS} + ${Cbc-lib_INCLUDE_DIRS} +) + +### package library +add_library(${PROJECT_NAME} STATIC + common/src/grid_point_explorator.cpp + common/src/boustrophedon_explorator.cpp + common/src/neural_network_explorator.cpp + common/src/convex_sensor_placement_explorator.cpp + common/src/energy_functional_explorator.cpp + common/src/flow_network_explorator.cpp + common/src/room_rotator.cpp + common/src/meanshift2d.cpp +) +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} + ${OpenCV_LIBS} + ${Boost_LIBRARIES} + ${CoinUtils_LIBRARIES} + ${OsiClp_LIBRARIES} + ${Clp_LIBRARIES} + ${Osi_LIBRARIES} + ${Cgl_LIBRARIES} + ${Cbc-lib_LIBRARIES} + ${GUROBI_LIBRARIES} + libcoverage_check_server +) +add_dependencies(${PROJECT_NAME} + ${catkin_EXPORTED_TARGETS} + ${${PROJECT_NAME}_EXPORTED_TARGETS} +) + +### room exploration action server, note: order of linking the Coin-Or packages important +add_executable(room_exploration_server + ros/src/room_exploration_action_server.cpp + ros/src/fov_to_robot_mapper.cpp +) +target_link_libraries(room_exploration_server + ${catkin_LIBRARIES} + ${OpenCV_LIBS} + ${Boost_LIBRARIES} + ${PROJECT_NAME} + libcoverage_check_server +) +add_dependencies(room_exploration_server + ${catkin_EXPORTED_TARGETS} + ${${PROJECT_NAME}_EXPORTED_TARGETS} +) + +### library for coverage checking +add_library(libcoverage_check_server + ros/src/coverage_check_server.cpp +) +target_link_libraries(libcoverage_check_server + ${catkin_LIBRARIES} + ${OpenCV_LIBS} + ${Boost_LIBRARIES} +) +add_dependencies(libcoverage_check_server ${catkin_EXPORTED_TARGETS}) + +### server for coverage checking +add_executable(coverage_check_server + ros/src/coverage_check_server_main.cpp +) +target_link_libraries(coverage_check_server + ${catkin_LIBRARIES} + ${OpenCV_LIBS} + ${Boost_LIBRARIES} + libcoverage_check_server +) +add_dependencies(coverage_check_server ${catkin_EXPORTED_TARGETS}) + +### server for coverage monitoring +add_executable(coverage_monitor_server + ros/src/coverage_monitor_server.cpp +) +target_link_libraries(coverage_monitor_server + ${catkin_LIBRARIES} + ${OpenCV_LIBS} + ${Boost_LIBRARIES} + libcoverage_check_server +) +add_dependencies(coverage_monitor_server ${catkin_EXPORTED_TARGETS}) + +### room exploration client +add_executable(room_exploration_client + ros/src/room_exploration_action_client.cpp +) +target_link_libraries(room_exploration_client + ${catkin_LIBRARIES} + ${OpenCV_LIBS} + ${Boost_LIBRARIES} +) +add_dependencies(room_exploration_client + ${catkin_EXPORTED_TARGETS} + ${${PROJECT_NAME}_EXPORTED_TARGETS} +) + + + +### next goal +add_executable(next_goal + ros/src/next_goal.cpp +) +target_link_libraries(next_goal + ${catkin_LIBRARIES} + ${OpenCV_LIBS} + ${Boost_LIBRARIES} +) +add_dependencies(next_goal + ${catkin_EXPORTED_TARGETS} + ${${PROJECT_NAME}_EXPORTED_TARGETS} +) + +add_executable(sub + ros/src/sub.cpp +) + +target_link_libraries(sub + ${catkin_LIBRARIES} +) +### evaluation of room exploration algorithms +add_executable(room_exploration_evaluation + ros/src/room_exploration_evaluation.cpp + ros/src/fov_to_robot_mapper.cpp +) +target_link_libraries(room_exploration_evaluation + ${catkin_LIBRARIES} + ${OpenCV_LIBS} + libcoverage_check_server +) +add_dependencies(room_exploration_evaluation + ${catkin_EXPORTED_TARGETS} + ${${PROJECT_NAME}_EXPORTED_TARGETS} +) + +############# +## Install ## +############# +# Mark executables and/or libraries for installation +install(TARGETS ${PROJECT_NAME} room_exploration_server room_exploration_client + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +#uncomment this if you have a common-directory with header-files +install(DIRECTORY common/include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) + +#uncomment this if you have header-files in your project +install(DIRECTORY ros/include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) + +#install(DIRECTORY scripts +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# PATTERN ".svn" EXCLUDE +#) + +# uncomment this if you have launch files +install(DIRECTORY ros/launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/ros + PATTERN ".svn" EXCLUDE +) + +#install(DIRECTORY common/files +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/common +# PATTERN ".svn" EXCLUDE +#) diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/cfg/CoverageMonitor.cfg b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/cfg/CoverageMonitor.cfg new file mode 100644 index 0000000..31b7de4 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/cfg/CoverageMonitor.cfg @@ -0,0 +1,18 @@ +#!/usr/bin/env python +PACKAGE = "ipa_room_exploration" + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + +gen.add("map_frame", str_t, 0, "Name of the map coordinate system.", "map") +gen.add("robot_frame", str_t, 0, "Name of the robot coordinate system.", "base_link") + +gen.add("coverage_radius", double_t, 0, "Radius of the circular coverage device, in [m].", 0.25, 0.001, 100000.0) +gen.add("coverage_circle_offset_transform_x", double_t, 0, "The offset of the coverage circle from the robot center, [x,y,z] in [m].", 0.29035, -100000.0, 100000.0) +gen.add("coverage_circle_offset_transform_y", double_t, 0, "The offset of the coverage circle from the robot center, [x,y,z] in [m].", -0.114, -100000.0, 100000.0) +gen.add("coverage_circle_offset_transform_z", double_t, 0, "The offset of the coverage circle from the robot center, [x,y,z] in [m].", 0.0, -100000.0, 100000.0) + +gen.add("robot_trajectory_recording_active", bool_t, 0, "The robot trajectory is only recorded if this flag is true.", False) + +exit(gen.generate(PACKAGE, "coverage_monitor_server", "CoverageMonitor")) diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/cfg/RoomExploration.cfg b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/cfg/RoomExploration.cfg new file mode 100644 index 0000000..fa0680c --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/cfg/RoomExploration.cfg @@ -0,0 +1,123 @@ +#!/usr/bin/env python +PACKAGE = "ipa_room_exploration" + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + +# Method to plan a path trough the room +method_enum = gen.enum([ gen.const("GridPointExplorator", int_t, 1, "Use the grid point exploration algorithm."), + gen.const("BoustrophedonExplorator", int_t, 2, "Use the boustrophedon exploration algorithm."), + gen.const("NeuralNetworkExplorator", int_t, 3, "Use the neural network exploration algorithm."), + gen.const("ConvexSPPExplorator", int_t, 4, "Use the convexSPP exploration algorithm."), + gen.const("FlowNetworkExplorator", int_t, 5, "Use the flowNetwork exploration algorithm."), + gen.const("EnergyFunctionalExplorator", int_t, 6, "Use the energyFunctional exploration algorithm."), + gen.const("VoronoiExplorator", int_t, 7, "Use the voronoi exploration algorithm."), + gen.const("BoustrophedonVariantExplorator", int_t, 8, "Use a variant of the boustrophedon exploration algorithm.")], + "Exploration algorithm") +gen.add("room_exploration_algorithm", int_t, 0, "Exploration method", 2, 1, 8, edit_method=method_enum) + + +# Parameters on map correction options +# ==================================== +gen.add("map_correction_closing_neighborhood_size", int_t, 0, "Applies a closing operation to neglect inaccessible areas and map errors/artifacts if the map_correction_closing_neighborhood_size parameter is larger than 0. The parameter then specifies the iterations (or neighborhood size) of that closing operation..", 2, -1, 100); + + +# Parameters specific to the navigation of the robot along the computed coverage trajectory +# ========================================================================================= +gen.add("return_path", bool_t, 0, "Boolean used to determine whether the server should return the computed coverage path in the response message.", True) + +gen.add("execute_path", bool_t, 0, "Boolean used to determine whether the server should navigate the robot along the computed coverage path.", False) + +# double that shows how close the robot can get to a published navigation goal before another gets published +gen.add("goal_eps", double_t, 0, "Distance from robot to published goal to publish the next goal in the path.", 2.0, 1.0) + +# Use a dynamic goal distance criterion: the larger the path's curvature, the more accurate the navigation. +# Uses 'goal_eps' param as maximum distance on straight paths and zero distance (accurate goal approaching) on 90deg curves. +gen.add("use_dyn_goal_eps", bool_t, 0, "Use a dynamic goal distance criterion: the larger the path's curvature, the more accurate the navigation.", False) + +# Boolean that interrupts the publishing of the navigation goals as long as needed, e.g. when the robot sees a trashbin during a cleaning task and wants to clean it directly and resume later with the coverage path. +gen.add("interrupt_navigation_publishing", bool_t, 0, "Interrupt the publishing of navigation goals as long as needed.", False) + +# turn the functionality on/off to revisit areas that haven't been seen during the execution of the coverage path, due to uncertainties or dynamic obstacles +gen.add("revisit_areas", bool_t, 0, "Revisiting not seen areas due to dynamic obstacles on/off.", True) + +# min area before previously not seen areas have to be revisited, [m^2] +gen.add("left_sections_min_area", double_t, 0, "Minimal size of left sections to revisit them after one go [m^2].", 0.01, 1e-7) + +gen.add("global_costmap_topic", str_t, 0, "The name of the global costmap topic.", "/move_base/global_costmap/costmap") + +gen.add("coverage_check_service_name", str_t, 0, "The name of the service to call for a coverage check of the driven trajectory.", "/room_exploration/coverage_check_server/coverage_check") + +gen.add("map_frame", str_t, 0, "The name of the map frame, used for tracking of the robot.", "map") + +gen.add("camera_frame", str_t, 0, "The name of the camera frame, that is in the same kinematic chain as the map_frame and shows the camera pose.", "base_link") + + +# Grid point Explorator +# ===================== +tsp_solver_enum = gen.enum([ gen.const("NearestNeighborTSP", int_t, 1, "Use the Nearest Neighbor TSP algorithm."), + gen.const("GeneticTSP", int_t, 2, "Use the Genetic TSP solver."), + gen.const("ConcordeTSP", int_t, 3, "Use the Concorde TSP solver.")], + "Indicates which TSP solver should be used.") +gen.add("tsp_solver", int_t, 0, "Exploration method", 3, 1, 3, edit_method=tsp_solver_enum) + +gen.add("tsp_solver_timeout", int_t, 0, "A sophisticated solver like Concorde or Genetic can be interrupted if it does not find a solution within this time (in [s]), and then falls back to the nearest neighbor solver.", 600, 1); + + +# Boustrophedon Explorator +# ======================== +gen.add("min_cell_area", double_t, 0, "Minimum area of one cell for the boustrophedon explorator.", 10.0, 1e-7) + +# min distance between two points in the created coverage path +gen.add("path_eps", double_t, 0, "Minimal distance between two points on the generated path [pixel].", 2.0, 0) + +# the additional offset of the grid to obstacles, i.e. allows to displace the grid by more than the standard half_grid_size from obstacles, in [m] +gen.add("grid_obstacle_offset", double_t, 0, "Allows to displace the grid by more than the standard half_grid_size from obstacles [m].", 0.0, 0) + +# maximal allowed shift off the ideal boustrophedon track to both sides for avoiding obstacles on track +# setting max_deviation_from_track=grid_spacing is usually a good choice +# for negative values (e.g. max_deviation_from_track: -1) max_deviation_from_track is automatically set to grid_spacing +# in [pixel] +gen.add("max_deviation_from_track", int_t, 0, "Maximal allowed shift off the ideal boustrophedon track for avoiding obstacles on track, in [pixel]. For negative values max_deviation_from_track is automatically set to grid_spacing.", -1, -1) + +# enum for cell visiting order +cell_visiting_order_enum = gen.enum([gen.const("OptimalTSP", int_t, 1, "The optimal visiting order of the cells is determined as TSP problem."), + gen.const("LeftToRight", int_t, 2, "Alternative ordering from left to right (measured on y-coordinates of the cells), visits the cells in a more obvious fashion to the human observer (though it is not optimal).")], + "Cell visiting order") +gen.add("cell_visiting_order", int_t, 0, "Cell visiting order method", 1, 1, 2, edit_method=cell_visiting_order_enum) + + +# Neural network explorator, see room_exploration_action_server.params.yaml for further details +# ============================================================================================= +gen.add("step_size", double_t, 0, "Step size for integrating the state dynamics of the neural network.", 0.008, 0.0001, 1.0) + +gen.add("A", int_t, 0, "Decaying parameter that pulls the activity of a neuron closer to zero, larger value means faster decreasing.", 17, 1) + +gen.add("B", int_t, 0, "Increasing parameter that tries to increase the activity of a neuron, higher value means a higher desired value and a faster increasing at the beginning.", 5, 1) + +gen.add("D", int_t, 0, "Decreasing parameter when the neuron is labeled as obstacle, higher value means faster decreasing.", 7, 1) + +gen.add("E", int_t, 0, "External input parameter of one neuron that is used in the dynamics corresponding to if it is an obstacle or uncleaned/cleaned, E>>B.", 80, 1) + +gen.add("mu", double_t, 0, "Parameter to set the importance of the states of neighboring neurons to the dynamics, higher value means higher influence.", 1.03, 0.001) + +gen.add("delta_theta_weight", double_t, 0, "Parameter to set the importance of the traveleing direction from the previous step and the next step, a higher value means that the robot should turn less.", 0.15, 0.0) + + +# ConvexSPP explorator +# ==================== +# size of one cell when discretizing the free space +gen.add("cell_size", int_t, 0, "Side length of one grid cell, when discretizing the free space.", 0, -1) + +gen.add("delta_theta", double_t, 0, "Sampling angle when creating possible sensing poses.", 1.570796, 1e-4) + + +# flowNetwork explorator +# ====================== +gen.add("curvature_factor", double_t, 0, "Factor an arc can be longer than a straight arc.", 1.1, 1.0) + +gen.add("max_distance_factor", double_t, 0, "#Factor, an arc can be longer than the maximal distance of the room.", 1.0, 1.0) + + +exit(gen.generate(PACKAGE, "ipa_room_exploration_action_server", "RoomExploration")) diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/cmake/FindGUROBI.cmake b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/cmake/FindGUROBI.cmake new file mode 100644 index 0000000..eeb7abf --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/cmake/FindGUROBI.cmake @@ -0,0 +1,62 @@ +#### Taken from https://github.com/joschu/trajopt/blob/master/cmake/modules/FindGUROBI.cmake + + +# - Try to find GUROBI --> not a system package, needs to be searched separately +# Once done this will define +# GUROBI_FOUND - System has Gurobi +# GUROBI_INCLUDE_DIRS - The Gurobi include directories +# GUROBI_LIBRARIES - The libraries needed to use Gurobi + +if(GUROBI_INCLUDE_DIR) + # variables already in cache + set(GUROBI_FOUND TRUE) + set(GUROBI_INCLUDE_DIRS "${GUROBI_INCLUDE_DIR}" ) + set(GUROBI_LIBRARIES "${GUROBI_LIBRARY};${GUROBI_CXX_LIBRARY}" ) + +else(GUROBI_INCLUDE_DIR) + + find_path(GUROBI_INCLUDE_DIR + NAMES gurobi_c++.h + PATHS "$ENV{GUROBI_HOME}/include" + "/Library/gurobi702/linux64/include" + ) + + # search for the used version of the Gurobi library + find_library(GUROBI_LIBRARY + NAMES gurobi + gurobi45 + gurobi46 + gurobi50 + gurobi51 + gurobi52 + gurobi55 + gurobi56 + gurobi60 + gurobi70 + PATHS "$ENV{GUROBI_HOME}/lib" + "/Library/gurobi702/linux64/lib" + ) + + find_library(GUROBI_CXX_LIBRARY + NAMES gurobi_c++ + libgurobi_c++.a + PATHS "$ENV{GUROBI_HOME}/lib" + "/Library/gurobi702/linux64/lib" + ) + + if(GUROBI_LIBRARY AND GUROBI_CXX_LIBRARY AND GUROBI_INCLUDE_DIR) + set(GUROBI_INCLUDE_DIRS "${GUROBI_INCLUDE_DIR}") + set(GUROBI_LIBRARIES "${GUROBI_LIBRARY};${GUROBI_CXX_LIBRARY}") + set(GUROBI_FOUND TRUE) + else(GUROBI_LIBRARY AND GUROBI_CXX_LIBRARY AND GUROBI_INCLUDE_DIR) + Message(STATUS "Gurobi has not been found") + endif(GUROBI_LIBRARY AND GUROBI_CXX_LIBRARY AND GUROBI_INCLUDE_DIR) + + include(FindPackageHandleStandardArgs) + # handle the QUIETLY and REQUIRED arguments + find_package_handle_standard_args(GUROBI DEFAULT_MSG + GUROBI_LIBRARY GUROBI_CXX_LIBRARY GUROBI_INCLUDE_DIR) + + mark_as_advanced(GUROBI_INCLUDE_DIR GUROBI_LIBRARY GUROBI_CXX_LIBRARY) + +endif(GUROBI_INCLUDE_DIR) diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/common/include/ipa_room_exploration/boustrophedon_explorator.h b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/common/include/ipa_room_exploration/boustrophedon_explorator.h new file mode 100644 index 0000000..930f306 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/common/include/ipa_room_exploration/boustrophedon_explorator.h @@ -0,0 +1,334 @@ +/*! + ***************************************************************** + * \file + * + * \note + * Copyright (c) 2016 \n + * Fraunhofer Institute for Manufacturing Engineering + * and Automation (IPA) \n\n + * + ***************************************************************** + * + * \note + * Project name: Care-O-bot + * \note + * ROS stack name: autopnp + * \note + * ROS package name: ipa_room_exploration + * + * \author + * Author: Florian Jordan + * \author + * Supervised by: Richard Bormann + * + * \date Date of creation: 11.2016 + * + * \brief + * + * + ***************************************************************** + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * - Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. \n + * - Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. \n + * - Neither the name of the Fraunhofer Institute for Manufacturing + * Engineering and Automation (IPA) nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. \n + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License LGPL as + * published by the Free Software Foundation, either version 3 of the + * License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License LGPL for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License LGPL along with this program. + * If not, see . + * + ****************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#define PI 3.14159265359 + + + +// Class that is used to store cells and obstacles in a certain manner. For this the vertexes are stored as points and +// the edges are stored as vectors in a counter-clockwise manner. The constructor becomes a set of respectively sorted +// points and computes the vectors out of them. Additionally the accessible/visible center of the polygon gets computed, +// to simplify the visiting order later, by using a meanshift algorithm. +class GeneralizedPolygon +{ +protected: + // vertexes + std::vector vertices_; + + // accessible center: a central point inside the polygon with maximum distance to walls + cv::Point center_; + + // center of bounding rectangle of polygon, may be located outside the polygon, i.e. in an inaccessible area多边形的边界矩形的中心，可以位于多边形之外，即在无法进入的区域 + cv::Point bounding_box_center_; + + // min/max coordinates + int max_x_, min_x_, max_y_, min_y_; + + // area of the polygon (cell number), in [pixel^2] + double area_; + +public: + // constructor + GeneralizedPolygon(const std::vector& vertices, const double map_resolution) + { + // save given vertexes + vertices_ = vertices; + + // get max/min x/y coordinates + max_x_ = 0; + min_x_ = 100000; + max_y_ = 0; + min_y_ = 100000; + for(size_t point=0; point max_x_) + max_x_ = vertices_[point].x; + if(vertices_[point].y > max_y_) + max_y_ = vertices_[point].y; + if(vertices_[point].x < min_x_) + min_x_ = vertices_[point].x; + if(vertices_[point].y < min_y_) + min_y_ = vertices_[point].y; + } + + bounding_box_center_.x = (min_x_+max_x_)/2; + bounding_box_center_.y = (min_y_+max_y_)/2; + + // compute visible center + MeanShift2D ms; + cv::Mat room = cv::Mat::zeros(max_y_+10, max_x_+10, CV_8UC1); +#if CV_MAJOR_VERSION<=3 + cv::drawContours(room, std::vector >(1,vertices), -1, cv::Scalar(255), CV_FILLED);//轮廓绘制 +#else + cv::drawContours(room, std::vector >(1,vertices), -1, cv::Scalar(255), cv::FILLED); +#endif + area_ = cv::countNonZero(room); + cv::Mat distance_map; //variable for the distance-transformed map, type: CV_32FC1 +#if CV_MAJOR_VERSION<=3 + cv::distanceTransform(room, distance_map, CV_DIST_L2, 5); +#else + cv::distanceTransform(room, distance_map, cv::DIST_L2, 5); +#endif + // find point set with largest distance to obstacles + double min_val = 0., max_val = 0.; + cv::minMaxLoc(distance_map, &min_val, &max_val); + std::vector room_cells; + for (int v = 0; v < distance_map.rows; ++v) + for (int u = 0; u < distance_map.cols; ++u) + if (distance_map.at(v, u) > max_val * 0.95f) + room_cells.push_back(cv::Vec2d(u, v)); + // use meanshift to find the modes in that set + cv::Vec2d room_center = ms.findRoomCenter(room, room_cells, map_resolution); + // save found center + center_.x = room_center[0]; + center_.y = room_center[1]; + } + + std::vector getVertices() const + { + return vertices_; + } + + cv::Point getCenter() const + { + return center_; + } + + cv::Point getBoundingBoxCenter() const + { + return bounding_box_center_; + } + + double getArea() const + { + return area_; + } + + void drawPolygon(cv::Mat& image, const cv::Scalar& color) const + { + // draw polygon in an black image with necessary size + cv::Mat black_image = cv::Mat(max_y_+10, max_x_+10, CV_8UC1, cv::Scalar(0)); +#if CV_MAJOR_VERSION<=3 + cv::drawContours(black_image, std::vector >(1,vertices_), -1, color, CV_FILLED); +#else + cv::drawContours(black_image, std::vector >(1,vertices_), -1, color, cv::FILLED); +#endif + + // assign drawn map + image = black_image.clone(); + } + + void getMinMaxCoordinates(int& min_x, int& max_x, int& min_y, int& max_y) + { + min_x = min_x_; + max_x = max_x_; + min_y = min_y_; + max_y = max_y_; + } +}; + +// Structure to save edges of a path on one row, that allows to easily get the order of the edges when planning the +// boustrophedon path. +struct BoustrophedonHorizontalLine +{ + cv::Point left_corner_, right_corner_; +}; + +// Structure for saving several properties of cells +struct BoustrophedonCell +{ + typedef std::set > BoustrophedonCellSet; + typedef std::set >::iterator BoustrophedonCellSetIterator; + + int label_; // label id of the cell + double area_; // area of the cell, in [pixel^2] + cv::Rect bounding_box_; // bounding box of the cell + BoustrophedonCellSet neighbors_; // pointer to neighboring cells + + BoustrophedonCell(const int label, const double area, const cv::Rect& bounding_box) + { + label_ = label; + area_ = area; + bounding_box_ = bounding_box; + } + +}; + + +// Class that generates a room exploration path by using the morse cellular decomposition method, proposed by +// +// "H. Choset, E. Acar, A. A. Rizzi and J. Luntz, +// "Exact cellular decompositions in terms of critical points of Morse functions," Robotics and Automation, 2000. Proceedings. +// ICRA '00. IEEE International Conference on, San Francisco, CA, 2000, pp. 2270-2277 vol.3." +// +// This method decomposes the given environment into several cells which don't have any obstacle in it. For each of this +// cell the boustrophedon path is generated, which goes up and down in each cell, see the upper paper for reference. +// This class only produces a static path, regarding the given map in form of a point series. To react on dynamic +// obstacles, one has to do this in upper algorithms. +// +class BoustrophedonExplorer +{ +protected: + // pathplanner to check for the next nearest locations + AStarPlanner path_planner_; + + static const uchar BORDER_PIXEL_VALUE = 25; + + // rotates the original map for a good axis alignment and divides it into Morse cells + // the functions tries two axis alignments with 90deg rotation difference and chooses the one with the lower number of cells + void findBestCellDecomposition(const cv::Mat& room_map, const float map_resolution, const double min_cell_area, + const int min_cell_width, cv::Mat& R, cv::Rect& bbox, cv::Mat& rotated_room_map, + std::vector& cell_polygons, std::vector& polygon_centers); + + // rotates the original map for a good axis alignment and divides it into Morse cells + // @param rotation_offset can be used to put an offset to the computed rotation for good axis alignment, in [rad] + void computeCellDecompositionWithRotation(const cv::Mat& room_map, const float map_resolution, const double min_cell_area, + const int min_cell_width, const double rotation_offset, cv::Mat& R, cv::Rect& bbox, cv::Mat& rotated_room_map, + std::vector& cell_polygons, std::vector& polygon_centers); + + // divides the provided map into Morse cells + void computeCellDecomposition(const cv::Mat& room_map, const float map_resolution, const double min_cell_area, + const int min_cell_width, std::vector& cell_polygons, std::vector& polygon_centers);//该函数返回每一个区间的多边形点数组cell_polygons以及每一个区间的中心点数组polygon_centers。 + + // merges cells after a cell decomposition according to various criteria specified in function @mergeCellsSelection + // returns the number of cells after merging + int mergeCells(cv::Mat& cell_map, cv::Mat& cell_map_labels, const double min_cell_area, const int min_cell_width); + + // implements the selection criterion for cell merging, in this case: too small (area) or too thin (width or height) cells + // are merged with their largest neighboring cell. + void mergeCellsSelection(cv::Mat& cell_map, cv::Mat& cell_map_labels, std::map >& cell_index_mapping, + const double min_cell_area, const int min_cell_width); + + // executes the merger of minor cell into major cell + void mergeTwoCells(cv::Mat& cell_map, cv::Mat& cell_map_labels, const BoustrophedonCell& minor_cell, BoustrophedonCell& major_cell, + std::map >& cell_index_mapping); + + // this function corrects obstacles that are one pixel width at 45deg angle, i.e. a 2x2 pixel neighborhood with [0, 255, 255, 0] or [255, 0, 0, 255] + void correctThinWalls(cv::Mat& room_map); + + // computes the Boustrophedon path pattern for a single cell + void computeBoustrophedonPath(const cv::Mat& room_map, const float map_resolution, const GeneralizedPolygon& cell, + std::vector& fov_middlepoint_path, cv::Point& robot_pos, + const int grid_spacing_as_int, const int half_grid_spacing_as_int, const double path_eps, const int max_deviation_from_track, const int grid_obstacle_offset=0); + + // downsamples a given path original_path to waypoint distances of path_eps and appends the resulting path to downsampled_path + void downsamplePath(const std::vector& original_path, std::vector& downsampled_path, + cv::Point& cell_robot_pos, const double path_eps); + + // downsamples a given path original_path to waypoint distances of path_eps in reverse order as given in original_path + // and appends the resulting path to downsampled_path + void downsamplePathReverse(const std::vector& original_path, std::vector& downsampled_path, + cv::Point& robot_pos, const double path_eps); + + void printCells(std::map >& cell_index_mapping); + +public: + // constructor + BoustrophedonExplorer(); + + // Function that creates an exploration path for a given room. The room has to be drawn in a cv::Mat (filled with Bit-uchar), + // with free space drawn white (255) and obstacles as black (0). It returns a series of 2D poses that show to which positions + // the robot should drive at. + void getExplorationPath(const cv::Mat& room_map, std::vector& path, const float map_resolution, + const cv::Point starting_position, const cv::Point2d map_origin, const double grid_spacing_in_pixel, + const double grid_obstacle_offset, const double path_eps, const int cell_visiting_order, const bool plan_for_footprint, + const Eigen::Matrix robot_to_fov_vector, const double min_cell_area, const int max_deviation_from_track); + + enum CellVisitingOrder {OPTIMAL_TSP=1, LEFT_TO_RIGHT=2}; +}; + + +class BoustrophedonVariantExplorer : public BoustrophedonExplorer +{ +protected: + + // implements the selection criterion for cell merging, in this case: only large cells with different major axis are not merged. + void mergeCellsSelection(cv::Mat& cell_map, cv::Mat& cell_map_labels, std::map >& cell_index_mapping, + const double min_cell_area, const int min_cell_width); + +public: + BoustrophedonVariantExplorer() {}; + ~BoustrophedonVariantExplorer() {}; + + +}; diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/common/include/ipa_room_exploration/convex_sensor_placement_explorator.h b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/common/include/ipa_room_exploration/convex_sensor_placement_explorator.h new file mode 100644 index 0000000..7a40c39 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/common/include/ipa_room_exploration/convex_sensor_placement_explorator.h @@ -0,0 +1,162 @@ +/*! + ***************************************************************** + * \file + * + * \note + * Copyright (c) 2016 \n + * Fraunhofer Institute for Manufacturing Engineering + * and Automation (IPA) \n\n + * + ***************************************************************** + * + * \note + * Project name: Care-O-bot + * \note + * ROS stack name: autopnp + * \note + * ROS package name: ipa_room_exploration + * + * \author + * Author: Florian Jordan + * \author + * Supervised by: Richard Bormann + * + * \date Date of creation: 11.2016 + * + * \brief + * + * + ***************************************************************** + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * - Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. \n + * - Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. \n + * - Neither the name of the Fraunhofer Institute for Manufacturing + * Engineering and Automation (IPA) nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. \n + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License LGPL as + * published by the Free Software Foundation, either version 3 of the + * License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License LGPL for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License LGPL along with this program. + * If not, see . + * + ****************************************************************/ + +#pragma once + + +#include +#include +#include +#include +#include +#include +#include +#include + +// Eigen library for matrix/vector computations +#include +// Coin-Or Cbc linear programming solver +#include +#include +#include +#include +// if available, use Gurobi +#ifdef GUROBI_FOUND + #include "gurobi_c++.h" +#endif + +#include +#include +#include + +#include +#include +#include +#include + +#include + + +#define PI 3.14159265359 + +// This class provides a coverage path planning algorithm, based on the work of +// +// Arain, M. A., Cirillo, M., Bennetts, V. H., Schaffernicht, E., Trincavelli, M., & Lilienthal, A. J. (2015, May). Efficient measurement planning for remote gas sensing with mobile robots. In 2015 IEEE International Conference on Robotics and Automation (ICRA) (pp. 3428-3434). IEEE. +// +// In this work originally a gas sensing robot is assumed, but by specifically defining the visibility function one can +// use this work for coverage path planning, especially for planning a route s.t. the field of view covers the free space. +// In the proposed paper the free space gets discretized into cells, that need to be covered. To do so a integer linear +// program of the form +// min C^T Ts +// s.t. VC >= 1 (elementwise) +// C[i] = {0, 1} +// is used, with C a column vector, representing if a sensing pose is used (1) or not (0), Ts a column vector storing the +// sensing cost at a specific sensing pose, V a matrix storing what cell is observed from a sensing pose. The first +// constraint ensures that each cell has been seen at least once and the objective function minimizes the number of sensing +// poses. To improve the efficiency of the algorithm a re-weighted convex relaxation method is used to enhance the +// sparsity of the above linear program. This is done by solving +// min (W o C)^T Ts +// s.t. VC >= 1 (elementwise) +// 0 <= C[i] <= 1 +// a few times, with calculating W new after each step. When the sparsity of C doesn't change much anymore or a number of +// iterations is reached, the zero elements of C are discarded and the first linear program is solved for the final result. +// This gives a minimal set of sensing poses s.t. all free cells can be observed with these. +// +class convexSPPExplorator +{ +protected: + // function that is used to create and solve a Gurobi optimization problem out of the given matrices and vectors, if + // Gurobi was found on the computer + template + void solveGurobiOptimizationProblem(std::vector& C, const cv::Mat& V, const std::vector* W); + + // function that is used to create and solve a Qsopt optimization problem out of the given matrices and vectors + template + void solveOptimizationProblem(std::vector& C, const cv::Mat& V, const std::vector* W); + + // object to find a path trough the chosen sensing poses by doing a repetitive nearest neighbor algorithm + NearestNeighborTSPSolver tsp_solver_; + + // object that plans a path from A to B using the Astar method + AStarPlanner path_planner_; + +public: + // constructor + convexSPPExplorator(); + + // Function that creates an exploration path for a given room. The room has to be drawn in a cv::Mat (filled with Bit-uchar), + // with free space drawn white (255) and obstacles as black (0). It returns a series of 2D poses that show to which positions + // the robot should drive at. The footprint stores a polygon that is used to determine the visibility at a specific + // sensing pose. delta_theta provides an angular step to determine candidates for sensing poses. + // map_resolution in [m/cell] + // starting_position starting position of the robot in [px] + // cell_size_pixel the side length of a grid square in [px] + // delta_theta angular sampling step when creating possible sensing poses in [rad] + // fov_corners_meter vector of corner coordinates of the field of view in [m] + // robot_to_fov_vector_meter the center of the field of view measured from the robot center in [m] + // largest_robot_to_footprint_distance_meter is the radius of the robot footprint for footprint planning (plan_for_footprint==true); + // or the largest distance between robot and a field of view corner (plan_for_footprint==false) + // (for fov planning, this may be set 0 and the function computes the maximum distance fov corner) + // plan_for_footprint if true, plan for the robot footprint of given radius (largest_robot_to_footprint_distance_meter); + // if false, plan for the field of view + void getExplorationPath(const cv::Mat& room_map, std::vector& path, const float map_resolution, + const cv::Point starting_position, const cv::Point2d map_origin, const int cell_size_pixel, const double delta_theta, + const std::vector >& fov_corners_meter, const Eigen::Matrix& robot_to_fov_vector_meter, + const double largest_robot_to_footprint_distance_meter, const uint sparsity_check_range, const bool plan_for_footprint); +}; diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/common/include/ipa_room_exploration/energy_functional_explorator.h b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/common/include/ipa_room_exploration/energy_functional_explorator.h new file mode 100644 index 0000000..aaec33b --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/common/include/ipa_room_exploration/energy_functional_explorator.h @@ -0,0 +1,150 @@ +/*! + ***************************************************************** + * \file + * + * \note + * Copyright (c) 2016 \n + * Fraunhofer Institute for Manufacturing Engineering + * and Automation (IPA) \n\n + * + ***************************************************************** + * + * \note + * Project name: Care-O-bot + * \note + * ROS stack name: autopnp + * \note + * ROS package name: ipa_room_exploration + * + * \author + * Author: Florian Jordan + * \author + * Supervised by: Richard Bormann + * + * \date Date of creation: 01.2017 + * + * \brief + * + * + ***************************************************************** + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * - Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. \n + * - Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. \n + * - Neither the name of the Fraunhofer Institute for Manufacturing + * Engineering and Automation (IPA) nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. \n + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License LGPL as + * published by the Free Software Foundation, either version 3 of the + * License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License LGPL for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License LGPL along with this program. + * If not, see . + * + ****************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include + +#include +#include +#include + +#define PI 3.14159265359 +const double PI_2_INV = 1./(0.5*PI); + + + +// Struct that is used to create a node and save the corresponding neighbors. +struct EnergyExploratorNode +{ + cv::Point center_; + bool obstacle_; + bool visited_; + std::vector neighbors_; + + int countNonObstacleNeighbors() + { + int non_obstacle_neighbors = 0; + for(std::vector::iterator neighbor=neighbors_.begin(); neighbor!=neighbors_.end(); ++neighbor) + if((*neighbor)->obstacle_==false) + ++non_obstacle_neighbors; + return non_obstacle_neighbors; + } +}; + +// Struct used to create sets of energyExplorationNodes +struct cv_Point_cmp +{ + bool operator() (const cv::Point& lhs, const cv::Point& rhs) const + { + return ((lhs.y < rhs.y) || (lhs.y == rhs.y && lhs.x < rhs.x)); + } +}; + +// This class provides the functionality of coverage path planning, based on the work in +// +// Bormann Richard, Joshua Hampp, and Martin Hägele. "New brooms sweep clean-an autonomous robotic cleaning assistant for +// professional office cleaning." Robotics and Automation (ICRA), 2015 IEEE International Conference on. IEEE, 2015. +// +// In this method the area that should be covered is discretized into a grid, using the given grid size parameter d_0. Each of these +// resulting nodes should be visited in order to cover the whole free space. The path starts at the corner of the area that is +// closest to the starting position. After the start node, the next node is chosen from the neighbors that minimizes the energy functional +// E(l,n) = d_t(l,n) + d_r(l,n) + N(n), +// where l is the current robot pose, n the to be checked next pose. The summands represent different aspects that need to +// be taken into account. d_t is the translational distance computed as +// d_t(l,n) = ||l_xy-n_xy||_2/d_0, +// where l_xy or n_xy is the vector storing the position of the corresponding location. d_r is the rotational distance, computed as +// d_r(l,n) = 2*(l_theta-n_theta)/pi, +// where l_theta or n_theta are the angles of the corresponding poses. Function N(n) represents half the number of not yet visited +// locations among the 8 neighbors Nb8(n) around n and is computed as +// N(n) = 4 - sum_(k in Nb8(n)) |k ∩ L|/2, +// where L is the number of already visited nodes. If no accessible node in the direct neighborhood could be found, the algorithm +// searches for the next node in the whole grid. This procedure is repeated until all nodes have been visited. +// This class only produces a static path, regarding the given map in form of a point series. To react on dynamic +// obstacles, one has to do this in upper algorithms. +// +class EnergyFunctionalExplorator +{ +protected: + // function to compute the energy function for each pair of nodes + double E(const EnergyExploratorNode& location, const EnergyExploratorNode& neighbor, const double cell_size_in_pixel, const double previous_travel_angle); + +public: + // constructor + EnergyFunctionalExplorator(); + + // Function that creates an exploration path for a given room. The room has to be drawn in a cv::Mat (filled with Bit-uchar), + // with free space drawn white (255) and obstacles as black (0). It returns a series of 2D poses that show to which positions + // the robot should drive at. + void getExplorationPath(const cv::Mat& room_map, std::vector& path, const float map_resolution, + const cv::Point starting_position, const cv::Point2d map_origin, const double grid_spacing_in_pixel, + const bool plan_for_footprint, const Eigen::Matrix robot_to_fov_vector); +}; diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/common/include/ipa_room_exploration/fast_math.h b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/common/include/ipa_room_exploration/fast_math.h new file mode 100644 index 0000000..372bf15 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/common/include/ipa_room_exploration/fast_math.h @@ -0,0 +1,1620 @@ +/*=====================================================================* + * Copyright (C) 2012 Paul Mineiro * + * All rights reserved. * + * * + * Redistribution and use in source and binary forms, with * + * or without modification, are permitted provided that the * + * following conditions are met: * + * * + * * Redistributions of source code must retain the * + * above copyright notice, this list of conditions and * + * the following disclaimer. * + * * + * * Redistributions in binary form must reproduce the * + * above copyright notice, this list of conditions and * + * the following disclaimer in the documentation and/or * + * other materials provided with the distribution. * + * * + * * Neither the name of Paul Mineiro nor the names * + * of other contributors may be used to endorse or promote * + * products derived from this software without specific * + * prior written permission. * + * * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND * + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, * + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES * + * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER * + * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE * + * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR * + * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF * + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * + * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * + * POSSIBILITY OF SUCH DAMAGE. * + * * + * Contact: Paul Mineiro * + *=====================================================================*/ + +#ifndef __CAST_H_ + +#ifdef __cplusplus +#define cast_uint32_t static_cast +#else +#define cast_uint32_t (uint32_t) +#endif + +#endif // __CAST_H_ +/*=====================================================================* + * Copyright (C) 2011 Paul Mineiro * + * All rights reserved. * + * * + * Redistribution and use in source and binary forms, with * + * or without modification, are permitted provided that the * + * following conditions are met: * + * * + * * Redistributions of source code must retain the * + * above copyright notice, this list of conditions and * + * the following disclaimer. * + * * + * * Redistributions in binary form must reproduce the * + * above copyright notice, this list of conditions and * + * the following disclaimer in the documentation and/or * + * other materials provided with the distribution. * + * * + * * Neither the name of Paul Mineiro nor the names * + * of other contributors may be used to endorse or promote * + * products derived from this software without specific * + * prior written permission. * + * * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND * + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, * + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES * + * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER * + * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE * + * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR * + * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF * + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * + * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * + * POSSIBILITY OF SUCH DAMAGE. * + * * + * Contact: Paul Mineiro * + *=====================================================================*/ + +#ifndef __SSE_H_ +#define __SSE_H_ + +#ifdef __SSE2__ + +#include + +#ifdef __cplusplus +namespace { +#endif // __cplusplus + +typedef __m128 v4sf; +typedef __m128i v4si; + +#define v4si_to_v4sf _mm_cvtepi32_ps +#define v4sf_to_v4si _mm_cvttps_epi32 + +#define v4sfl(x) ((const v4sf) { (x), (x), (x), (x) }) +#define v2dil(x) ((const v4si) { (x), (x) }) +#define v4sil(x) v2dil((((long long) (x)) << 32) | (x)) +//#define v4sil(x) v2dil((((unsigned long long) (x)) << 32) | (x)) // error with Ubuntu 16.04 +//In file included from /home/rmb/git/care-o-bot/src/autopnp/ipa_room_exploration/common/src/meanshift2d.cpp:61:0: +///../fast_math.h: In function ‘{anonymous}::v4sf vfasterfc({anonymous}::v4sf)’: +///../fast_math.h:108:43: error: narrowing conversion of ‘9223372039002259456ull’ from ‘long long unsigned int’ to ‘long long int’ inside { } [-Wnarrowing] +// #define v2dil(x) ((const v4si) { (x), (x) }) +// ^ +///../fast_math.h:109:18: note: in expansion of macro ‘v2dil’ +// #define v4sil(x) v2dil((((unsigned long long) (x)) << 32) | (x)) +// ^ +///../fast_math.h:543:11: note: in expansion of macro ‘v4sil’ +// vc.i |= v4sil (0x80000000); + + +typedef union { v4sf f; float array[4]; } v4sfindexer; +#define v4sf_index(_findx, _findi) \ + ({ \ + v4sfindexer _findvx = { _findx } ; \ + _findvx.array[_findi]; \ + }) +typedef union { v4si i; int array[4]; } v4siindexer; +#define v4si_index(_iindx, _iindi) \ + ({ \ + v4siindexer _iindvx = { _iindx } ; \ + _iindvx.array[_iindi]; \ + }) + +typedef union { v4sf f; v4si i; } v4sfv4sipun; +#define v4sf_fabs(x) \ + ({ \ + v4sfv4sipun vx; \ + vx.f = x; \ + vx.i &= v4sil (0x7FFFFFFF); \ + vx.f; \ + }) + +#ifdef __cplusplus +} // end namespace +#endif // __cplusplus + +#endif // __SSE2__ + +#endif // __SSE_H_ +/*=====================================================================* + * Copyright (C) 2011 Paul Mineiro * + * All rights reserved. * + * * + * Redistribution and use in source and binary forms, with * + * or without modification, are permitted provided that the * + * following conditions are met: * + * * + * * Redistributions of source code must retain the * + * above copyright notice, this list of conditions and * + * the following disclaimer. * + * * + * * Redistributions in binary form must reproduce the * + * above copyright notice, this list of conditions and * + * the following disclaimer in the documentation and/or * + * other materials provided with the distribution. * + * * + * * Neither the name of Paul Mineiro nor the names * + * of other contributors may be used to endorse or promote * + * products derived from this software without specific * + * prior written permission. * + * * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND * + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, * + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES * + * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER * + * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE * + * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR * + * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF * + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * + * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * + * POSSIBILITY OF SUCH DAMAGE. * + * * + * Contact: Paul Mineiro * + *=====================================================================*/ + +#ifndef __FAST_EXP_H_ +#define __FAST_EXP_H_ + +#include + +// Underflow of exponential is common practice in numerical routines, +// so handle it here. + +static inline float +fastpow2 (float p) +{ + float offset = (p < 0) ? 1.0f : 0.0f; + float clipp = (p < -126) ? -126.0f : p; + int w = clipp; + float z = clipp - w + offset; + union { uint32_t i; float f; } v = { cast_uint32_t ( (1 << 23) * (clipp + 121.2740575f + 27.7280233f / (4.84252568f - z) - 1.49012907f * z) ) }; + + return v.f; +} + +static inline float +fastexp (float p) +{ + return fastpow2 (1.442695040f * p); +} + +static inline float +fasterpow2 (float p) +{ + float clipp = (p < -126) ? -126.0f : p; + union { uint32_t i; float f; } v = { cast_uint32_t ( (1 << 23) * (clipp + 126.94269504f) ) }; + return v.f; +} + +static inline float +fasterexp (float p) +{ + return fasterpow2 (1.442695040f * p); +} + +#ifdef __SSE2__ + +static inline v4sf +vfastpow2 (const v4sf p) +{ + v4sf ltzero = _mm_cmplt_ps (p, v4sfl (0.0f)); + v4sf offset = _mm_and_ps (ltzero, v4sfl (1.0f)); + v4sf lt126 = _mm_cmplt_ps (p, v4sfl (-126.0f)); + v4sf clipp = _mm_or_ps (_mm_andnot_ps (lt126, p), _mm_and_ps (lt126, v4sfl (-126.0f))); + v4si w = v4sf_to_v4si (clipp); + v4sf z = clipp - v4si_to_v4sf (w) + offset; + + const v4sf c_121_2740838 = v4sfl (121.2740575f); + const v4sf c_27_7280233 = v4sfl (27.7280233f); + const v4sf c_4_84252568 = v4sfl (4.84252568f); + const v4sf c_1_49012907 = v4sfl (1.49012907f); + union { v4si i; v4sf f; } v = { + v4sf_to_v4si ( + v4sfl (1 << 23) * + (clipp + c_121_2740838 + c_27_7280233 / (c_4_84252568 - z) - c_1_49012907 * z) + ) + }; + + return v.f; +} + +static inline v4sf +vfastexp (const v4sf p) +{ + const v4sf c_invlog_2 = v4sfl (1.442695040f); + + return vfastpow2 (c_invlog_2 * p); +} + +static inline v4sf +vfasterpow2 (const v4sf p) +{ + const v4sf c_126_94269504 = v4sfl (126.94269504f); + v4sf lt126 = _mm_cmplt_ps (p, v4sfl (-126.0f)); + v4sf clipp = _mm_or_ps (_mm_andnot_ps (lt126, p), _mm_and_ps (lt126, v4sfl (-126.0f))); + union { v4si i; v4sf f; } v = { v4sf_to_v4si (v4sfl (1 << 23) * (clipp + c_126_94269504)) }; + return v.f; +} + +static inline v4sf +vfasterexp (const v4sf p) +{ + const v4sf c_invlog_2 = v4sfl (1.442695040f); + + return vfasterpow2 (c_invlog_2 * p); +} + +#endif //__SSE2__ + +#endif // __FAST_EXP_H_ +/*=====================================================================* + * Copyright (C) 2011 Paul Mineiro * + * All rights reserved. * + * * + * Redistribution and use in source and binary forms, with * + * or without modification, are permitted provided that the * + * following conditions are met: * + * * + * * Redistributions of source code must retain the * + * above copyright notice, this list of conditions and * + * the following disclaimer. * + * * + * * Redistributions in binary form must reproduce the * + * above copyright notice, this list of conditions and * + * the following disclaimer in the documentation and/or * + * other materials provided with the distribution. * + * * + * * Neither the name of Paul Mineiro nor the names * + * of other contributors may be used to endorse or promote * + * products derived from this software without specific * + * prior written permission. * + * * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND * + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, * + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES * + * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER * + * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE * + * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR * + * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF * + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * + * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * + * POSSIBILITY OF SUCH DAMAGE. * + * * + * Contact: Paul Mineiro * + *=====================================================================*/ + +#ifndef __FAST_LOG_H_ +#define __FAST_LOG_H_ + +#include + +static inline float +fastlog2 (float x) +{ + union { float f; uint32_t i; } vx = { x }; + union { uint32_t i; float f; } mx = { (vx.i & 0x007FFFFF) | 0x3f000000 }; + float y = vx.i; + y *= 1.1920928955078125e-7f; + + return y - 124.22551499f + - 1.498030302f * mx.f + - 1.72587999f / (0.3520887068f + mx.f); +} + +static inline float +fastlog (float x) +{ + return 0.69314718f * fastlog2 (x); +} + +static inline float +fasterlog2 (float x) +{ + union { float f; uint32_t i; } vx = { x }; + float y = vx.i; + y *= 1.1920928955078125e-7f; + return y - 126.94269504f; +} + +static inline float +fasterlog (float x) +{ +// return 0.69314718f * fasterlog2 (x); + + union { float f; uint32_t i; } vx = { x }; + float y = vx.i; + y *= 8.2629582881927490e-8f; + return y - 87.989971088f; +} + +#ifdef __SSE2__ + +static inline v4sf +vfastlog2 (v4sf x) +{ + union { v4sf f; v4si i; } vx = { x }; + union { v4si i; v4sf f; } mx; mx.i = (vx.i & v4sil (0x007FFFFF)) | v4sil (0x3f000000); + v4sf y = v4si_to_v4sf (vx.i); + y *= v4sfl (1.1920928955078125e-7f); + + const v4sf c_124_22551499 = v4sfl (124.22551499f); + const v4sf c_1_498030302 = v4sfl (1.498030302f); + const v4sf c_1_725877999 = v4sfl (1.72587999f); + const v4sf c_0_3520087068 = v4sfl (0.3520887068f); + + return y - c_124_22551499 + - c_1_498030302 * mx.f + - c_1_725877999 / (c_0_3520087068 + mx.f); +} + +static inline v4sf +vfastlog (v4sf x) +{ + const v4sf c_0_69314718 = v4sfl (0.69314718f); + + return c_0_69314718 * vfastlog2 (x); +} + +static inline v4sf +vfasterlog2 (v4sf x) +{ + union { v4sf f; v4si i; } vx = { x }; + v4sf y = v4si_to_v4sf (vx.i); + y *= v4sfl (1.1920928955078125e-7f); + + const v4sf c_126_94269504 = v4sfl (126.94269504f); + + return y - c_126_94269504; +} + +static inline v4sf +vfasterlog (v4sf x) +{ +// const v4sf c_0_69314718 = v4sfl (0.69314718f); +// +// return c_0_69314718 * vfasterlog2 (x); + + union { v4sf f; v4si i; } vx = { x }; + v4sf y = v4si_to_v4sf (vx.i); + y *= v4sfl (8.2629582881927490e-8f); + + const v4sf c_87_989971088 = v4sfl (87.989971088f); + + return y - c_87_989971088; +} + +#endif // __SSE2__ + +#endif // __FAST_LOG_H_ +/*=====================================================================* + * Copyright (C) 2011 Paul Mineiro * + * All rights reserved. * + * * + * Redistribution and use in source and binary forms, with * + * or without modification, are permitted provided that the * + * following conditions are met: * + * * + * * Redistributions of source code must retain the * + * above copyright notice, this list of conditions and * + * the following disclaimer. * + * * + * * Redistributions in binary form must reproduce the * + * above copyright notice, this list of conditions and * + * the following disclaimer in the documentation and/or * + * other materials provided with the distribution. * + * * + * * Neither the name of Paul Mineiro nor the names * + * of other contributors may be used to endorse or promote * + * products derived from this software without specific * + * prior written permission. * + * * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND * + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, * + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES * + * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER * + * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE * + * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR * + * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF * + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * + * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * + * POSSIBILITY OF SUCH DAMAGE. * + * * + * Contact: Paul Mineiro * + *=====================================================================*/ + +#ifndef __FAST_ERF_H_ +#define __FAST_ERF_H_ + +#include +#include + +// fasterfc: not actually faster than erfcf(3) on newer machines! +// ... although vectorized version is interesting +// and fastererfc is very fast + +static inline float +fasterfc (float x) +{ + static const float k = 3.3509633149424609f; + static const float a = 0.07219054755431126f; + static const float b = 15.418191568719577f; + static const float c = 5.609846028328545f; + + union { float f; uint32_t i; } vc = { c * x }; + float xsq = x * x; + float xquad = xsq * xsq; + + vc.i |= 0x80000000; + + return 2.0f / (1.0f + fastpow2 (k * x)) - a * x * (b * xquad - 1.0f) * fasterpow2 (vc.f); +} + +static inline float +fastererfc (float x) +{ + static const float k = 3.3509633149424609f; + + return 2.0f / (1.0f + fasterpow2 (k * x)); +} + +// fasterf: not actually faster than erff(3) on newer machines! +// ... although vectorized version is interesting +// and fastererf is very fast + +static inline float +fasterf (float x) +{ + return 1.0f - fasterfc (x); +} + +static inline float +fastererf (float x) +{ + return 1.0f - fastererfc (x); +} + +static inline float +fastinverseerf (float x) +{ + static const float invk = 0.30004578719350504f; + static const float a = 0.020287853348211326f; + static const float b = 0.07236892874789555f; + static const float c = 0.9913030456864257f; + static const float d = 0.8059775923760193f; + + float xsq = x * x; + + return invk * fastlog2 ((1.0f + x) / (1.0f - x)) + + x * (a - b * xsq) / (c - d * xsq); +} + +static inline float +fasterinverseerf (float x) +{ + static const float invk = 0.30004578719350504f; + + return invk * fasterlog2 ((1.0f + x) / (1.0f - x)); +} + +#ifdef __SSE2__ + +static inline v4sf +vfasterfc (v4sf x) +{ + const v4sf k = v4sfl (3.3509633149424609f); + const v4sf a = v4sfl (0.07219054755431126f); + const v4sf b = v4sfl (15.418191568719577f); + const v4sf c = v4sfl (5.609846028328545f); + + union { v4sf f; v4si i; } vc; vc.f = c * x; + vc.i |= v4sil (0x80000000); + + v4sf xsq = x * x; + v4sf xquad = xsq * xsq; + + return v4sfl (2.0f) / (v4sfl (1.0f) + vfastpow2 (k * x)) - a * x * (b * xquad - v4sfl (1.0f)) * vfasterpow2 (vc.f); +} + +static inline v4sf +vfastererfc (const v4sf x) +{ + const v4sf k = v4sfl (3.3509633149424609f); + + return v4sfl (2.0f) / (v4sfl (1.0f) + vfasterpow2 (k * x)); +} + +static inline v4sf +vfasterf (v4sf x) +{ + return v4sfl (1.0f) - vfasterfc (x); +} + +static inline v4sf +vfastererf (const v4sf x) +{ + return v4sfl (1.0f) - vfastererfc (x); +} + +static inline v4sf +vfastinverseerf (v4sf x) +{ + const v4sf invk = v4sfl (0.30004578719350504f); + const v4sf a = v4sfl (0.020287853348211326f); + const v4sf b = v4sfl (0.07236892874789555f); + const v4sf c = v4sfl (0.9913030456864257f); + const v4sf d = v4sfl (0.8059775923760193f); + + v4sf xsq = x * x; + + return invk * vfastlog2 ((v4sfl (1.0f) + x) / (v4sfl (1.0f) - x)) + + x * (a - b * xsq) / (c - d * xsq); +} + +static inline v4sf +vfasterinverseerf (v4sf x) +{ + const v4sf invk = v4sfl (0.30004578719350504f); + + return invk * vfasterlog2 ((v4sfl (1.0f) + x) / (v4sfl (1.0f) - x)); +} + +#endif //__SSE2__ + +#endif // __FAST_ERF_H_ +/*=====================================================================* + * Copyright (C) 2011 Paul Mineiro * + * All rights reserved. * + * * + * Redistribution and use in source and binary forms, with * + * or without modification, are permitted provided that the * + * following conditions are met: * + * * + * * Redistributions of source code must retain the * + * above copyright notice, this list of conditions and * + * the following disclaimer. * + * * + * * Redistributions in binary form must reproduce the * + * above copyright notice, this list of conditions and * + * the following disclaimer in the documentation and/or * + * other materials provided with the distribution. * + * * + * * Neither the name of Paul Mineiro nor the names * + * of other contributors may be used to endorse or promote * + * products derived from this software without specific * + * prior written permission. * + * * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND * + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, * + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES * + * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER * + * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE * + * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR * + * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF * + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * + * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * + * POSSIBILITY OF SUCH DAMAGE. * + * * + * Contact: Paul Mineiro * + *=====================================================================*/ + +#ifndef __FAST_GAMMA_H_ +#define __FAST_GAMMA_H_ + +#include + +/* gamma/digamma functions only work for positive inputs */ + +static inline float +fastlgamma (float x) +{ + float logterm = fastlog (x * (1.0f + x) * (2.0f + x)); + float xp3 = 3.0f + x; + + return - 2.081061466f + - x + + 0.0833333f / xp3 + - logterm + + (2.5f + x) * fastlog (xp3); +} + +static inline float +fasterlgamma (float x) +{ + return - 0.0810614667f + - x + - fasterlog (x) + + (0.5f + x) * fasterlog (1.0f + x); +} + +static inline float +fastdigamma (float x) +{ + float twopx = 2.0f + x; + float logterm = fastlog (twopx); + + return (-48.0f + x * (-157.0f + x * (-127.0f - 30.0f * x))) / + (12.0f * x * (1.0f + x) * twopx * twopx) + + logterm; +} + +static inline float +fasterdigamma (float x) +{ + float onepx = 1.0f + x; + + return -1.0f / x - 1.0f / (2 * onepx) + fasterlog (onepx); +} + +#ifdef __SSE2__ + +static inline v4sf +vfastlgamma (v4sf x) +{ + const v4sf c_1_0 = v4sfl (1.0f); + const v4sf c_2_0 = v4sfl (2.0f); + const v4sf c_3_0 = v4sfl (3.0f); + const v4sf c_2_081061466 = v4sfl (2.081061466f); + const v4sf c_0_0833333 = v4sfl (0.0833333f); + const v4sf c_2_5 = v4sfl (2.5f); + + v4sf logterm = vfastlog (x * (c_1_0 + x) * (c_2_0 + x)); + v4sf xp3 = c_3_0 + x; + + return - c_2_081061466 + - x + + c_0_0833333 / xp3 + - logterm + + (c_2_5 + x) * vfastlog (xp3); +} + +static inline v4sf +vfasterlgamma (v4sf x) +{ + const v4sf c_0_0810614667 = v4sfl (0.0810614667f); + const v4sf c_0_5 = v4sfl (0.5f); + const v4sf c_1 = v4sfl (1.0f); + + return - c_0_0810614667 + - x + - vfasterlog (x) + + (c_0_5 + x) * vfasterlog (c_1 + x); +} + +static inline v4sf +vfastdigamma (v4sf x) +{ + v4sf twopx = v4sfl (2.0f) + x; + v4sf logterm = vfastlog (twopx); + + return (v4sfl (-48.0f) + x * (v4sfl (-157.0f) + x * (v4sfl (-127.0f) - v4sfl (30.0f) * x))) / + (v4sfl (12.0f) * x * (v4sfl (1.0f) + x) * twopx * twopx) + + logterm; +} + +static inline v4sf +vfasterdigamma (v4sf x) +{ + const v4sf c_1_0 = v4sfl (1.0f); + const v4sf c_2_0 = v4sfl (2.0f); + v4sf onepx = c_1_0 + x; + + return -c_1_0 / x - c_1_0 / (c_2_0 * onepx) + vfasterlog (onepx); +} + +#endif //__SSE2__ + +#endif // __FAST_GAMMA_H_ +/*=====================================================================* + * Copyright (C) 2011 Paul Mineiro * + * All rights reserved. * + * * + * Redistribution and use in source and binary forms, with * + * or without modification, are permitted provided that the * + * following conditions are met: * + * * + * * Redistributions of source code must retain the * + * above copyright notice, this list of conditions and * + * the following disclaimer. * + * * + * * Redistributions in binary form must reproduce the * + * above copyright notice, this list of conditions and * + * the following disclaimer in the documentation and/or * + * other materials provided with the distribution. * + * * + * * Neither the name of Paul Mineiro nor the names * + * of other contributors may be used to endorse or promote * + * products derived from this software without specific * + * prior written permission. * + * * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND * + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, * + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES * + * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER * + * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE * + * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR * + * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF * + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * + * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * + * POSSIBILITY OF SUCH DAMAGE. * + * * + * Contact: Paul Mineiro * + *=====================================================================*/ + +#ifndef __FAST_HYPERBOLIC_H_ +#define __FAST_HYPERBOLIC_H_ + +#include + +static inline float +fastsinh (float p) +{ + return 0.5f * (fastexp (p) - fastexp (-p)); +} + +static inline float +fastersinh (float p) +{ + return 0.5f * (fasterexp (p) - fasterexp (-p)); +} + +static inline float +fastcosh (float p) +{ + return 0.5f * (fastexp (p) + fastexp (-p)); +} + +static inline float +fastercosh (float p) +{ + return 0.5f * (fasterexp (p) + fasterexp (-p)); +} + +static inline float +fasttanh (float p) +{ + return -1.0f + 2.0f / (1.0f + fastexp (-2.0f * p)); +} + +static inline float +fastertanh (float p) +{ + return -1.0f + 2.0f / (1.0f + fasterexp (-2.0f * p)); +} + +#ifdef __SSE2__ + +static inline v4sf +vfastsinh (const v4sf p) +{ + const v4sf c_0_5 = v4sfl (0.5f); + + return c_0_5 * (vfastexp (p) - vfastexp (-p)); +} + +static inline v4sf +vfastersinh (const v4sf p) +{ + const v4sf c_0_5 = v4sfl (0.5f); + + return c_0_5 * (vfasterexp (p) - vfasterexp (-p)); +} + +static inline v4sf +vfastcosh (const v4sf p) +{ + const v4sf c_0_5 = v4sfl (0.5f); + + return c_0_5 * (vfastexp (p) + vfastexp (-p)); +} + +static inline v4sf +vfastercosh (const v4sf p) +{ + const v4sf c_0_5 = v4sfl (0.5f); + + return c_0_5 * (vfasterexp (p) + vfasterexp (-p)); +} + +static inline v4sf +vfasttanh (const v4sf p) +{ + const v4sf c_1 = v4sfl (1.0f); + const v4sf c_2 = v4sfl (2.0f); + + return -c_1 + c_2 / (c_1 + vfastexp (-c_2 * p)); +} + +static inline v4sf +vfastertanh (const v4sf p) +{ + const v4sf c_1 = v4sfl (1.0f); + const v4sf c_2 = v4sfl (2.0f); + + return -c_1 + c_2 / (c_1 + vfasterexp (-c_2 * p)); +} + +#endif //__SSE2__ + +#endif // __FAST_HYPERBOLIC_H_ +/*=====================================================================* + * Copyright (C) 2011 Paul Mineiro * + * All rights reserved. * + * * + * Redistribution and use in source and binary forms, with * + * or without modification, are permitted provided that the * + * following conditions are met: * + * * + * * Redistributions of source code must retain the * + * above copyright notice, this list of conditions and * + * the following disclaimer. * + * * + * * Redistributions in binary form must reproduce the * + * above copyright notice, this list of conditions and * + * the following disclaimer in the documentation and/or * + * other materials provided with the distribution. * + * * + * * Neither the name of Paul Mineiro nor the names * + * of other contributors may be used to endorse or promote * + * products derived from this software without specific * + * prior written permission. * + * * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND * + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, * + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES * + * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER * + * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE * + * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR * + * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF * + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * + * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * + * POSSIBILITY OF SUCH DAMAGE. * + * * + * Contact: Paul Mineiro * + *=====================================================================*/ + +#ifndef __FAST_LAMBERT_W_H_ +#define __FAST_LAMBERT_W_H_ + +#include + +// these functions compute the upper branch aka W_0 + +static inline float +fastlambertw (float x) +{ + static const float threshold = 2.26445f; + + float c = (x < threshold) ? 1.546865557f : 1.0f; + float d = (x < threshold) ? 2.250366841f : 0.0f; + float a = (x < threshold) ? -0.737769969f : 0.0f; + + float logterm = fastlog (c * x + d); + float loglogterm = fastlog (logterm); + + float minusw = -a - logterm + loglogterm - loglogterm / logterm; + float expminusw = fastexp (minusw); + float xexpminusw = x * expminusw; + float pexpminusw = xexpminusw - minusw; + + return (2.0f * xexpminusw - minusw * (4.0f * xexpminusw - minusw * pexpminusw)) / + (2.0f + pexpminusw * (2.0f - minusw)); +} + +static inline float +fasterlambertw (float x) +{ + static const float threshold = 2.26445f; + + float c = (x < threshold) ? 1.546865557f : 1.0f; + float d = (x < threshold) ? 2.250366841f : 0.0f; + float a = (x < threshold) ? -0.737769969f : 0.0f; + + float logterm = fasterlog (c * x + d); + float loglogterm = fasterlog (logterm); + + float w = a + logterm - loglogterm + loglogterm / logterm; + float expw = fasterexp (-w); + + return (w * w + expw * x) / (1.0f + w); +} + +static inline float +fastlambertwexpx (float x) +{ + static const float k = 1.1765631309f; + static const float a = 0.94537622168f; + + float logarg = fmaxf (x, k); + float powarg = (x < k) ? a * (x - k) : 0; + + float logterm = fastlog (logarg); + float powterm = fasterpow2 (powarg); // don't need accuracy here + + float w = powterm * (logarg - logterm + logterm / logarg); + float logw = fastlog (w); + float p = x - logw; + + return w * (2.0f + p + w * (3.0f + 2.0f * p)) / + (2.0f - p + w * (5.0f + 2.0f * w)); +} + +static inline float +fasterlambertwexpx (float x) +{ + static const float k = 1.1765631309f; + static const float a = 0.94537622168f; + + float logarg = fmaxf (x, k); + float powarg = (x < k) ? a * (x - k) : 0; + + float logterm = fasterlog (logarg); + float powterm = fasterpow2 (powarg); + + float w = powterm * (logarg - logterm + logterm / logarg); + float logw = fasterlog (w); + + return w * (1.0f + x - logw) / (1.0f + w); +} + +#ifdef __SSE2__ + +static inline v4sf +vfastlambertw (v4sf x) +{ + const v4sf threshold = v4sfl (2.26445f); + + v4sf under = _mm_cmplt_ps (x, threshold); + v4sf c = _mm_or_ps (_mm_and_ps (under, v4sfl (1.546865557f)), + _mm_andnot_ps (under, v4sfl (1.0f))); + v4sf d = _mm_and_ps (under, v4sfl (2.250366841f)); + v4sf a = _mm_and_ps (under, v4sfl (-0.737769969f)); + + v4sf logterm = vfastlog (c * x + d); + v4sf loglogterm = vfastlog (logterm); + + v4sf minusw = -a - logterm + loglogterm - loglogterm / logterm; + v4sf expminusw = vfastexp (minusw); + v4sf xexpminusw = x * expminusw; + v4sf pexpminusw = xexpminusw - minusw; + + return (v4sfl (2.0f) * xexpminusw - minusw * (v4sfl (4.0f) * xexpminusw - minusw * pexpminusw)) / + (v4sfl (2.0f) + pexpminusw * (v4sfl (2.0f) - minusw)); +} + +static inline v4sf +vfasterlambertw (v4sf x) +{ + const v4sf threshold = v4sfl (2.26445f); + + v4sf under = _mm_cmplt_ps (x, threshold); + v4sf c = _mm_or_ps (_mm_and_ps (under, v4sfl (1.546865557f)), + _mm_andnot_ps (under, v4sfl (1.0f))); + v4sf d = _mm_and_ps (under, v4sfl (2.250366841f)); + v4sf a = _mm_and_ps (under, v4sfl (-0.737769969f)); + + v4sf logterm = vfasterlog (c * x + d); + v4sf loglogterm = vfasterlog (logterm); + + v4sf w = a + logterm - loglogterm + loglogterm / logterm; + v4sf expw = vfasterexp (-w); + + return (w * w + expw * x) / (v4sfl (1.0f) + w); +} + +static inline v4sf +vfastlambertwexpx (v4sf x) +{ + const v4sf k = v4sfl (1.1765631309f); + const v4sf a = v4sfl (0.94537622168f); + const v4sf two = v4sfl (2.0f); + const v4sf three = v4sfl (3.0f); + const v4sf five = v4sfl (5.0f); + + v4sf logarg = _mm_max_ps (x, k); + v4sf powarg = _mm_and_ps (_mm_cmplt_ps (x, k), a * (x - k)); + + v4sf logterm = vfastlog (logarg); + v4sf powterm = vfasterpow2 (powarg); // don't need accuracy here + + v4sf w = powterm * (logarg - logterm + logterm / logarg); + v4sf logw = vfastlog (w); + v4sf p = x - logw; + + return w * (two + p + w * (three + two * p)) / + (two - p + w * (five + two * w)); +} + +static inline v4sf +vfasterlambertwexpx (v4sf x) +{ + const v4sf k = v4sfl (1.1765631309f); + const v4sf a = v4sfl (0.94537622168f); + + v4sf logarg = _mm_max_ps (x, k); + v4sf powarg = _mm_and_ps (_mm_cmplt_ps (x, k), a * (x - k)); + + v4sf logterm = vfasterlog (logarg); + v4sf powterm = vfasterpow2 (powarg); + + v4sf w = powterm * (logarg - logterm + logterm / logarg); + v4sf logw = vfasterlog (w); + + return w * (v4sfl (1.0f) + x - logw) / (v4sfl (1.0f) + w); +} + +#endif // __SSE2__ + +#endif // __FAST_LAMBERT_W_H_ + +/*=====================================================================* + * Copyright (C) 2011 Paul Mineiro * + * All rights reserved. * + * * + * Redistribution and use in source and binary forms, with * + * or without modification, are permitted provided that the * + * following conditions are met: * + * * + * * Redistributions of source code must retain the * + * above copyright notice, this list of conditions and * + * the following disclaimer. * + * * + * * Redistributions in binary form must reproduce the * + * above copyright notice, this list of conditions and * + * the following disclaimer in the documentation and/or * + * other materials provided with the distribution. * + * * + * * Neither the name of Paul Mineiro nor the names * + * of other contributors may be used to endorse or promote * + * products derived from this software without specific * + * prior written permission. * + * * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND * + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, * + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES * + * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER * + * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE * + * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR * + * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF * + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * + * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * + * POSSIBILITY OF SUCH DAMAGE. * + * * + * Contact: Paul Mineiro * + *=====================================================================*/ + +#ifndef __FAST_POW_H_ +#define __FAST_POW_H_ + +#include + +static inline float +fastpow (float x, + float p) +{ + return fastpow2 (p * fastlog2 (x)); +} + +static inline float +fasterpow (float x, + float p) +{ + return fasterpow2 (p * fasterlog2 (x)); +} + +#ifdef __SSE2__ + +static inline v4sf +vfastpow (const v4sf x, + const v4sf p) +{ + return vfastpow2 (p * vfastlog2 (x)); +} + +static inline v4sf +vfasterpow (const v4sf x, + const v4sf p) +{ + return vfasterpow2 (p * vfasterlog2 (x)); +} + +#endif //__SSE2__ + +#endif // __FAST_POW_H_ +/*=====================================================================* + * Copyright (C) 2011 Paul Mineiro * + * All rights reserved. * + * * + * Redistribution and use in source and binary forms, with * + * or without modification, are permitted provided that the * + * following conditions are met: * + * * + * * Redistributions of source code must retain the * + * above copyright notice, this list of conditions and * + * the following disclaimer. * + * * + * * Redistributions in binary form must reproduce the * + * above copyright notice, this list of conditions and * + * the following disclaimer in the documentation and/or * + * other materials provided with the distribution. * + * * + * * Neither the name of Paul Mineiro nor the names * + * of other contributors may be used to endorse or promote * + * products derived from this software without specific * + * prior written permission. * + * * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND * + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, * + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES * + * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER * + * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE * + * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR * + * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF * + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * + * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * + * POSSIBILITY OF SUCH DAMAGE. * + * * + * Contact: Paul Mineiro * + *=====================================================================*/ + +#ifndef __FAST_SIGMOID_H_ +#define __FAST_SIGMOID_H_ + +#include + +static inline float +fastsigmoid (float x) +{ + return 1.0f / (1.0f + fastexp (-x)); +} + +static inline float +fastersigmoid (float x) +{ + return 1.0f / (1.0f + fasterexp (-x)); +} + +#ifdef __SSE2__ + +static inline v4sf +vfastsigmoid (const v4sf x) +{ + const v4sf c_1 = v4sfl (1.0f); + + return c_1 / (c_1 + vfastexp (-x)); +} + +static inline v4sf +vfastersigmoid (const v4sf x) +{ + const v4sf c_1 = v4sfl (1.0f); + + return c_1 / (c_1 + vfasterexp (-x)); +} + +#endif //__SSE2__ + +#endif // __FAST_SIGMOID_H_ +/*=====================================================================* + * Copyright (C) 2011 Paul Mineiro * + * All rights reserved. * + * * + * Redistribution and use in source and binary forms, with * + * or without modification, are permitted provided that the * + * following conditions are met: * + * * + * * Redistributions of source code must retain the * + * above copyright notice, this list of conditions and * + * the following disclaimer. * + * * + * * Redistributions in binary form must reproduce the * + * above copyright notice, this list of conditions and * + * the following disclaimer in the documentation and/or * + * other materials provided with the distribution. * + * * + * * Neither the name of Paul Mineiro nor the names * + * of other contributors may be used to endorse or promote * + * products derived from this software without specific * + * prior written permission. * + * * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND * + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, * + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES * + * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER * + * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE * + * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR * + * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF * + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * + * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * + * POSSIBILITY OF SUCH DAMAGE. * + * * + * Contact: Paul Mineiro * + *=====================================================================*/ + +#ifndef __FAST_TRIG_H_ +#define __FAST_TRIG_H_ + +#include + +// http://www.devmaster.net/forums/showthread.php?t=5784 +// fast sine variants are for x \in [ -\pi, pi ] +// fast cosine variants are for x \in [ -\pi, pi ] +// fast tangent variants are for x \in [ -\pi / 2, pi / 2 ] +// "full" versions of functions handle the entire range of inputs +// although the range reduction technique used here will be hopelessly +// inaccurate for |x| >> 1000 +// +// WARNING: fastsinfull, fastcosfull, and fasttanfull can be slower than +// libc calls on older machines (!) and on newer machines are only +// slighly faster. however: +// * vectorized versions are competitive +// * faster full versions are competitive + +static inline float +fastsin (float x) +{ + static const float fouroverpi = 1.2732395447351627f; + static const float fouroverpisq = 0.40528473456935109f; + static const float q = 0.78444488374548933f; + union { float f; uint32_t i; } p = { 0.20363937680730309f }; + union { float f; uint32_t i; } r = { 0.015124940802184233f }; + union { float f; uint32_t i; } s = { -0.0032225901625579573f }; + + union { float f; uint32_t i; } vx = { x }; + uint32_t sign = vx.i & 0x80000000; + vx.i = vx.i & 0x7FFFFFFF; + + float qpprox = fouroverpi * x - fouroverpisq * x * vx.f; + float qpproxsq = qpprox * qpprox; + + p.i |= sign; + r.i |= sign; + s.i ^= sign; + + return q * qpprox + qpproxsq * (p.f + qpproxsq * (r.f + qpproxsq * s.f)); +} + +static inline float +fastersin (float x) +{ + static const float fouroverpi = 1.2732395447351627f; + static const float fouroverpisq = 0.40528473456935109f; + static const float q = 0.77633023248007499f; + union { float f; uint32_t i; } p = { 0.22308510060189463f }; + + union { float f; uint32_t i; } vx = { x }; + uint32_t sign = vx.i & 0x80000000; + vx.i &= 0x7FFFFFFF; + + float qpprox = fouroverpi * x - fouroverpisq * x * vx.f; + + p.i |= sign; + + return qpprox * (q + p.f * qpprox); +} + +static inline float +fastsinfull (float x) +{ + static const float twopi = 6.2831853071795865f; + static const float invtwopi = 0.15915494309189534f; + + int k = x * invtwopi; + float half = (x < 0) ? -0.5f : 0.5f; + return fastsin ((half + k) * twopi - x); +} + +static inline float +fastersinfull (float x) +{ + static const float twopi = 6.2831853071795865f; + static const float invtwopi = 0.15915494309189534f; + + int k = x * invtwopi; + float half = (x < 0) ? -0.5f : 0.5f; + return fastersin ((half + k) * twopi - x); +} + +static inline float +fastcos (float x) +{ + static const float halfpi = 1.5707963267948966f; + static const float halfpiminustwopi = -4.7123889803846899f; + float offset = (x > halfpi) ? halfpiminustwopi : halfpi; + return fastsin (x + offset); +} + +static inline float +fastercos (float x) +{ + static const float twooverpi = 0.63661977236758134f; + static const float p = 0.54641335845679634f; + + union { float f; uint32_t i; } vx = { x }; + vx.i &= 0x7FFFFFFF; + + float qpprox = 1.0f - twooverpi * vx.f; + + return qpprox + p * qpprox * (1.0f - qpprox * qpprox); +} + +static inline float +fastcosfull (float x) +{ + static const float halfpi = 1.5707963267948966f; + return fastsinfull (x + halfpi); +} + +static inline float +fastercosfull (float x) +{ + static const float halfpi = 1.5707963267948966f; + return fastersinfull (x + halfpi); +} + +static inline float +fasttan (float x) +{ + static const float halfpi = 1.5707963267948966f; + return fastsin (x) / fastsin (x + halfpi); +} + +static inline float +fastertan (float x) +{ + return fastersin (x) / fastercos (x); +} + +static inline float +fasttanfull (float x) +{ + static const float twopi = 6.2831853071795865f; + static const float invtwopi = 0.15915494309189534f; + + int k = x * invtwopi; + float half = (x < 0) ? -0.5f : 0.5f; + float xnew = x - (half + k) * twopi; + + return fastsin (xnew) / fastcos (xnew); +} + +static inline float +fastertanfull (float x) +{ + static const float twopi = 6.2831853071795865f; + static const float invtwopi = 0.15915494309189534f; + + int k = x * invtwopi; + float half = (x < 0) ? -0.5f : 0.5f; + float xnew = x - (half + k) * twopi; + + return fastersin (xnew) / fastercos (xnew); +} + +#ifdef __SSE2__ + +static inline v4sf +vfastsin (const v4sf x) +{ + const v4sf fouroverpi = v4sfl (1.2732395447351627f); + const v4sf fouroverpisq = v4sfl (0.40528473456935109f); + const v4sf q = v4sfl (0.78444488374548933f); + const v4sf p = v4sfl (0.20363937680730309f); + const v4sf r = v4sfl (0.015124940802184233f); + const v4sf s = v4sfl (-0.0032225901625579573f); + + union { v4sf f; v4si i; } vx = { x }; + v4si sign = vx.i & v4sil (0x80000000); + vx.i &= v4sil (0x7FFFFFFF); + + v4sf qpprox = fouroverpi * x - fouroverpisq * x * vx.f; + v4sf qpproxsq = qpprox * qpprox; + union { v4sf f; v4si i; } vy; vy.f = qpproxsq * (p + qpproxsq * (r + qpproxsq * s)); + vy.i ^= sign; + + return q * qpprox + vy.f; +} + +static inline v4sf +vfastersin (const v4sf x) +{ + const v4sf fouroverpi = v4sfl (1.2732395447351627f); + const v4sf fouroverpisq = v4sfl (0.40528473456935109f); + const v4sf q = v4sfl (0.77633023248007499f); + const v4sf plit = v4sfl (0.22308510060189463f); + union { v4sf f; v4si i; } p = { plit }; + + union { v4sf f; v4si i; } vx = { x }; + v4si sign = vx.i & v4sil (0x80000000); + vx.i &= v4sil (0x7FFFFFFF); + + v4sf qpprox = fouroverpi * x - fouroverpisq * x * vx.f; + + p.i |= sign; + + return qpprox * (q + p.f * qpprox); +} + +static inline v4sf +vfastsinfull (const v4sf x) +{ + const v4sf twopi = v4sfl (6.2831853071795865f); + const v4sf invtwopi = v4sfl (0.15915494309189534f); + + v4si k = v4sf_to_v4si (x * invtwopi); + + v4sf ltzero = _mm_cmplt_ps (x, v4sfl (0.0f)); + v4sf half = _mm_or_ps (_mm_and_ps (ltzero, v4sfl (-0.5f)), + _mm_andnot_ps (ltzero, v4sfl (0.5f))); + + return vfastsin ((half + v4si_to_v4sf (k)) * twopi - x); +} + +static inline v4sf +vfastersinfull (const v4sf x) +{ + const v4sf twopi = v4sfl (6.2831853071795865f); + const v4sf invtwopi = v4sfl (0.15915494309189534f); + + v4si k = v4sf_to_v4si (x * invtwopi); + + v4sf ltzero = _mm_cmplt_ps (x, v4sfl (0.0f)); + v4sf half = _mm_or_ps (_mm_and_ps (ltzero, v4sfl (-0.5f)), + _mm_andnot_ps (ltzero, v4sfl (0.5f))); + + return vfastersin ((half + v4si_to_v4sf (k)) * twopi - x); +} + +static inline v4sf +vfastcos (const v4sf x) +{ + const v4sf halfpi = v4sfl (1.5707963267948966f); + const v4sf halfpiminustwopi = v4sfl (-4.7123889803846899f); + v4sf lthalfpi = _mm_cmpnlt_ps (x, halfpi); + v4sf offset = _mm_or_ps (_mm_and_ps (lthalfpi, halfpiminustwopi), + _mm_andnot_ps (lthalfpi, halfpi)); + return vfastsin (x + offset); +} + +static inline v4sf +vfastercos (v4sf x) +{ + const v4sf twooverpi = v4sfl (0.63661977236758134f); + const v4sf p = v4sfl (0.54641335845679634); + + v4sf vx = v4sf_fabs (x); + v4sf qpprox = v4sfl (1.0f) - twooverpi * vx; + + return qpprox + p * qpprox * (v4sfl (1.0f) - qpprox * qpprox); +} + +static inline v4sf +vfastcosfull (const v4sf x) +{ + const v4sf halfpi = v4sfl (1.5707963267948966f); + return vfastsinfull (x + halfpi); +} + +static inline v4sf +vfastercosfull (const v4sf x) +{ + const v4sf halfpi = v4sfl (1.5707963267948966f); + return vfastersinfull (x + halfpi); +} + +static inline v4sf +vfasttan (const v4sf x) +{ + const v4sf halfpi = v4sfl (1.5707963267948966f); + return vfastsin (x) / vfastsin (x + halfpi); +} + +static inline v4sf +vfastertan (const v4sf x) +{ + return vfastersin (x) / vfastercos (x); +} + +static inline v4sf +vfasttanfull (const v4sf x) +{ + const v4sf twopi = v4sfl (6.2831853071795865f); + const v4sf invtwopi = v4sfl (0.15915494309189534f); + + v4si k = v4sf_to_v4si (x * invtwopi); + + v4sf ltzero = _mm_cmplt_ps (x, v4sfl (0.0f)); + v4sf half = _mm_or_ps (_mm_and_ps (ltzero, v4sfl (-0.5f)), + _mm_andnot_ps (ltzero, v4sfl (0.5f))); + v4sf xnew = x - (half + v4si_to_v4sf (k)) * twopi; + + return vfastsin (xnew) / vfastcos (xnew); +} + +static inline v4sf +vfastertanfull (const v4sf x) +{ + const v4sf twopi = v4sfl (6.2831853071795865f); + const v4sf invtwopi = v4sfl (0.15915494309189534f); + + v4si k = v4sf_to_v4si (x * invtwopi); + + v4sf ltzero = _mm_cmplt_ps (x, v4sfl (0.0f)); + v4sf half = _mm_or_ps (_mm_and_ps (ltzero, v4sfl (-0.5f)), + _mm_andnot_ps (ltzero, v4sfl (0.5f))); + v4sf xnew = x - (half + v4si_to_v4sf (k)) * twopi; + + return vfastersin (xnew) / vfastercos (xnew); +} + +#endif //__SSE2__ + +#endif // __FAST_TRIG_H_ diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/common/include/ipa_room_exploration/flow_network_explorator.h b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/common/include/ipa_room_exploration/flow_network_explorator.h new file mode 100644 index 0000000..1225864 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/common/include/ipa_room_exploration/flow_network_explorator.h @@ -0,0 +1,509 @@ +// OpenCV +#include +#include +// standard c++ includes +#include +#include +#include +#include +#include +#include +#include +#include +// Eigen library for matrix/vector computations +#include +// Coin-Or library with Cbc mixed integer programming solver +#include +#include +#include +#include +// Coin-Or library with Clp linear programming solver +#include +// Boost libraries +#include +#include +#include +// package specific includes +#include +#include +#include +#include +#include +// msgs +#include +#include +#include +// if available, use Gurobi +#ifdef GUROBI_FOUND + #include "gurobi_c++.h" +#endif + +/*! + ***************************************************************** + * \file + * + * \note + * Copyright (c) 2016 \n + * Fraunhofer Institute for Manufacturing Engineering + * and Automation (IPA) \n\n + * + ***************************************************************** + * + * \note + * Project name: Care-O-bot + * \note + * ROS stack name: autopnp + * \note + * ROS package name: ipa_room_exploration + * + * \author + * Author: Florian Jordan + * \author + * Supervised by: Richard Bormann + * + * \date Date of creation: 11.2016 + * + * \brief + * + * + ***************************************************************** + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * - Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. \n + * - Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. \n + * - Neither the name of the Fraunhofer Institute for Manufacturing + * Engineering and Automation (IPA) nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. \n + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License LGPL as + * published by the Free Software Foundation, either version 3 of the + * License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License LGPL for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License LGPL along with this program. + * If not, see . + * + ****************************************************************/ + +// This struct is used to create arcs for the flow network, used to plan a coverage path trough the given map. +struct arcStruct +{ + cv::Point start_point; + cv::Point end_point; + double weight; + std::vector edge_points; +}; + +// typedef for boost, defining a directed graph +typedef boost::adjacency_list directedGraph; + +// TODO: update +// This class provides a coverage path planning algorithm based on a flow network. It spans such a network by going trough +// the given room map with a sweep line and defining edges of it whenever it touches an obstacle. From this touchpoint +// the edge is generated in a distance along the sweep line, that is equivalent to the given coverage radius, because the +// free space should determine the area that should be covered after this procedure. After this the arcs of the network +// are generated by taking the distance of two edges as its weight (gives two directed edges), if the arc is not too long or +// not straight enough (to reduce the dimensionality). To find a coverage path trough this flow network then a linear +// program of the form +// min c^T*w +// +// s.t. Vc >= 1 (row-wise) +// sum(flows_into_node) - sum(flows_out_of_node) = 0; +// flows_into_nodes: variables corresponding to arcs flowing into an edge/node, excluding the final arcs +// flows_out_of_nodes: variables corresponding to arcs flowing out of an edge/node, excluding the start arcs +// sum(start_flows) = 1 +// sum(terminal_flows) = 1 +// sum(flows_of_subset) <= |subset|-1; for any node subset S with |S|>=2 +// c(i)={0;1} +// is solved, where 3 stages are taken, the initial step to start the path somewhere, the coverage stage in which the arcs +// should cover the most of the area and the final step where the path can be terminated. These three stages are necessary, +// because the second constraint ensures that the path is connected and not consisting of separated parts. The initial step +// then starts the path at the desired position and the final step allows a termination of this flow conservativity constraint. +// The first constraint ensures that each cell of the free space is covered. The fourth constraint simply ensures to start +// at the node that one specifies. The last constraint prevents that the solution consists of different cycles. A cycle would +// fulfill the conservativity constraint, but of course may not be connected to the rest of the path. By saying that the +// number of goen arcs in a subset of nodes has to be smaller than the size of the subset lesser 1, not all arcs can be +// gone together, which prevents cycles. These cosntraints are added in a lazy fashion, meaning that after a solution has +// been obtained it is checked if the constraints have been violated in any way. If so, the constraints are added and the +// model is resolved. This step repeats, until no more cycles, that are not connected to the rest of the path, appear. +// +#pragma once + +#define PI 3.14159265359 + +#ifdef GUROBI_FOUND + class CyclePreventionCallbackClass: public GRBCallback + { + public: + std::vector* vars; + int n; + std::vector > flows_out_of_nodes, flows_into_nodes; + std::vector start_arcs; + std::vector > lhs; + std::vector rhs; + + CyclePreventionCallbackClass(std::vector* xvars, int xn, const std::vector >& outflows, + const std::vector >& inflows, const std::vector& start_indices) + { + vars = xvars; + n = xn; + flows_out_of_nodes = outflows; + flows_into_nodes = inflows; + start_arcs = start_indices; + } + + protected: + void callback() + { + try + { + if (where==GRB_CB_MIPSOL) + { + // Found an integer feasible solution + std::vector solution(vars->size()); + for (int var=0; varsize(); var++) + { + double current_value = GRBCallback::getSolution(vars->operator[](var)); + solution[var] = (current_value>=0) ? current_value : 0.0; +// std::cout << solution[var] << std::endl; + } + + // check if cycles appear in the solution + // get the arcs that are used in the previously calculated solution + std::set used_arcs; // set that stores the indices of the arcs corresponding to non-zero elements in the solution + + // go trough the start arcs + std::cout << "getting used arcs: "; + // TODO: add again, better reading out of the paths +// for(uint start_arc=0; start_arc0.01) // precision of the solver +// { +// // insert start index +// used_arcs.insert(start_arcs[start_arc]); +// std::cout << start_arcs[start_arc] << " "; +// } +// } + + // go trough the coverage stage + std::cout << "| "; + for(uint cover_arc=start_arcs.size(); cover_arc0.01) // precision of the solver + { + // insert index, relative to the first coverage variable + used_arcs.insert(cover_arc-start_arcs.size()); + std::cout << cover_arc-start_arcs.size() << " "; + } + } + + std::cout << "| "; + // TODO: add again, better reading out of the paths + // go trough the final stage and find the remaining used arcs +// for(uint flow=start_arcs.size()+n; flow0.01) // precision of the solver +// { +// // insert saved outgoing flow index +// used_arcs.insert(flow-start_arcs.size()-n); +// std::cout << flow-start_arcs.size()-n << " "; +// } +// } + + std::cout << "got " << used_arcs.size() << " used arcs" << std::endl; + + // construct the directed edges out of the used arcs + std::vector > directed_edges; // vector that stores the directed edges for each node + for(uint start_node=0; start_node originating_flows; + bool originating = false; + for(std::set::iterator arc=used_arcs.begin(); arc!=used_arcs.end(); ++arc) + { + if(contains(flows_out_of_nodes[start_node], *arc)==true) + { + originating = true; + originating_flows.push_back(*arc); + } + } + + // if the arc is originating from this node, find its destination + std::vector current_directed_edges; + if(originating==true) + { + for(uint end_node=0; end_node::iterator arc=originating_flows.begin(); arc!=originating_flows.end(); ++arc) + if(contains(flows_into_nodes[end_node], *arc)==true) + current_directed_edges.push_back(end_node); + } + } + + // if the current node doesn't flow into another node insert a vector storing -1 + if(current_directed_edges.size()==0) + current_directed_edges.push_back(-1); + + // save the found used directed edges + directed_edges.push_back(current_directed_edges); + } + + // construct the support graph out of the directed edges + directedGraph support_graph(flows_out_of_nodes.size()); // initialize with the right number of edges + + int number_of_not_used_nodes = 0; + for(size_t start=0; start c(flows_into_nodes.size()); + int number_of_strong_components = boost::strong_components(support_graph, boost::make_iterator_property_map(c.begin(), boost::get(boost::vertex_index, support_graph), c[0])); + std::cout << "got " << number_of_strong_components << " strongly connected components" << std::endl; +// for (std::vector ::iterator i = c.begin(); i != c.end(); ++i) +// std::cout << "Vertex " << i - c.begin() << " is in component " << *i << std::endl; + + // check how many cycles there are in the solution (components with a size >= 2) + int number_of_cycles = 0; + std::set done_components; // set that stores the component indices for that the nodes already were found + for(std::vector::iterator comp=c.begin(); comp!=c.end(); ++comp) + { + // don't check a component more than one time + if(done_components.find(*comp)!=done_components.end()) + continue; + + int elements = std::count(c.begin(), c.end(), *comp); + if(elements>=2) + ++number_of_cycles; + + // check if a tsp path is computed (number of arcs is same as number of nodes), each arc is a strongly + // connected component itself or all the nodes belong to one strongly connected component + // TODO: add again, better reading out of the paths +// if(elements==used_arcs.size() || elements==flows_out_of_nodes.size()) +// number_of_cycles=0; + + // add it to done components + done_components.insert(*comp); + } + std::cout << "current number of cycles: " << number_of_cycles << std::endl; + + // if more than one cycle appears find it and add the prevention constraint to the problem and resolve it + if(number_of_cycles!=0) + { + // go trough the components and find components with more than 1 element in it + std::vector > cycle_nodes; + done_components.clear(); + for (int component=0; component current_component_nodes; + int elements = std::count(c.begin(), c.end(), c[component]); +// std::cout << c[component] << std::endl; + // TODO: if connected to rest of path --> okay, don't add constraint + if(elements>=2) + { +// std::cout << "size: " << elements << std::endl; + for(std::vector::iterator element=c.begin(); element!=c.end(); ++element) + { + if(*element==c[component]) + { + current_component_nodes.push_back(element-c.begin()); + } + } + + // save the found cycle + if(current_component_nodes.size()>0) + cycle_nodes.push_back(current_component_nodes); + + // add it to done components + done_components.insert(c[component]); + } + } + } + + // add the cycle prevention constraints for each found cycle to the optimization problem + for(size_t cycle=0; cycle cpc_indices; + std::vector cpc_coefficients; + for(size_t node=0; node::iterator outflow=flows_out_of_nodes[cycle_nodes[cycle][node]].begin(); outflow!=flows_out_of_nodes[cycle_nodes[cycle][node]].end(); ++outflow) + { + for(size_t neighbor=0; neighbor::const_iterator outflow=flows_out_of_nodes[cycle_nodes[cycle][neighbor]].begin(); outflow!=flows_out_of_nodes[cycle_nodes[cycle][neighbor]].end(); ++outflow) +// { +// // if the current arc is used in the solution, search for it in the incoming flows of +// // the other nodes in the cycle +// if(used_arcs.find(*outflow)!=used_arcs.end()) +// for(size_t new_node=0; new_node::const_iterator outflow=flows_out_of_nodes[cycle_nodes[cycle][neighbor]].begin(); outflow!=flows_out_of_nodes[cycle_nodes[cycle][neighbor]].end(); ++outflow) +// { +// // search for the current flow in the incoming flows of the other nodes in the cycle +// for(size_t new_node=0; new_node current_lhs; + for(size_t var=0; varoperator[](cpc_indices[var]); + current_lhs.push_back(cpc_coefficients[var]*cpc_indices[var]); +// std::cout << cpc_coefficients[var] << "*" << cpc_indices[var] << std::endl; + } +// std::cout << "adding lazy, size of rhs: " << cycle_nodes[cycle].size()-1 << std::endl; + addLazy(current_cpc_constraint<=cycle_nodes[cycle].size()-1); + lhs.push_back(current_lhs); + rhs.push_back(cycle_nodes[cycle].size()-1); + } + } + } + } + catch (GRBException e) + { + std::cout << "Error number: " << e.getErrorCode() << std::endl; + std::cout << e.getMessage() << std::endl; + } + catch (...) + { + std::cout << "Error during callback" << std::endl; + } + } + }; +#endif + +class FlowNetworkExplorator +{ +protected: + // function that is used to create and solve a Cbc optimization problem out of the given matrices and vectors, using + // the three-stage ansatz and single-flow cycle prevention constraints + void solveThreeStageOptimizationProblem(std::vector& C, const cv::Mat& V, const std::vector& weights, + const std::vector >& flows_into_nodes, const std::vector >& flows_out_of_nodes, + const std::vector& start_arcs); + + // function that is used to create and solve a Gurobi optimization problem out of the given matrices and vectors, using + // the three-stage ansatz and lazy generalized cutset inequalities (GCI) + void solveGurobiOptimizationProblem(std::vector& C, const cv::Mat& V, const std::vector& weights, + const std::vector >& flows_into_nodes, const std::vector >& flows_out_of_nodes, + const std::vector& start_arcs); + + // function that is used to create and solve a Cbc optimization problem out of the given matrices and vectors, using + // the three-stage ansatz and lazy generalized cutset inequalities (GCI) + void solveLazyConstraintOptimizationProblem(std::vector& C, const cv::Mat& V, const std::vector& weights, + const std::vector >& flows_into_nodes, const std::vector >& flows_out_of_nodes, + const std::vector& start_arcs); + + // function that checks if the given point is more close enough to any point in the given vector + bool pointClose(const std::vector& points, const cv::Point& point, const double min_distance); + + // object that plans a path from A to B using the Astar method + AStarPlanner path_planner_; + +public: + // constructor + FlowNetworkExplorator(); + + // Function that creates an exploration path for a given room. The room has to be drawn in a cv::Mat (filled with Bit-uchar), + // with free space drawn white (255) and obstacles as black (0). It returns a series of 2D poses that show to which positions + // the robot should drive at. The footprint stores a polygon that is used to determine the visibility at a specific + // sensing pose. delta_theta provides an angular step to determine candidates for sensing poses. + void getExplorationPath(const cv::Mat& room_map, std::vector& path, const float map_resolution, + const cv::Point starting_position, const cv::Point2d map_origin, + const int cell_size, const Eigen::Matrix& robot_to_fov_middlepoint_vector, const float coverage_radius, + const bool plan_for_footprint, const double path_eps, const double curvature_factor, const double max_distance_factor); + + // test function + void testFunc(); +}; diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/common/include/ipa_room_exploration/grid.h b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/common/include/ipa_room_exploration/grid.h new file mode 100644 index 0000000..d3c41a4 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/common/include/ipa_room_exploration/grid.h @@ -0,0 +1,461 @@ +/*! + ***************************************************************** + * \file + * + * \note + * Copyright (c) 2017 \n + * Fraunhofer Institute for Manufacturing Engineering + * and Automation (IPA) \n\n + * + ***************************************************************** + * + * \note + * Project name: Care-O-bot + * \note + * ROS stack name: autopnp + * \note + * ROS package name: ipa_room_exploration + * + * \author + * Author: Richard Bormann + * \author + * Supervised by: Richard Bormann + * + * \date Date of creation: 03.2017 + * + * \brief + * + * + ***************************************************************** + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * - Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. \n + * - Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. \n + * - Neither the name of the Fraunhofer Institute for Manufacturing + * Engineering and Automation (IPA) nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. \n + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License LGPL as + * published by the Free Software Foundation, either version 3 of the + * License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License LGPL for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License LGPL along with this program. + * If not, see . + * + ****************************************************************/ + +#pragma once +#include +#include + +class BoustrophedonLine +{ +public: + std::vector upper_line; // points of the upper part of the line (always filled first) + std::vector lower_line; // points of the potentially existing lower part of the line + bool has_two_valid_lines; // true if both lines, upper and lower, concurrently provide valid alternative points at some locations (i.e. only if this is true, there are two individual lines of places to visit) + + BoustrophedonLine() + : has_two_valid_lines(false) + { + } +}; + +class BoustrophedonGrid : public std::vector +{ +}; + +class GridGenerator +{ +public: + GridGenerator() + { + } + + // generates a standard grid with fixed spacing starting at the upper left accessible cell (if the map is provided + // with walls grown by the size of half robot radius, i.e. the map is eroded by the inaccessible areas) or starting at half grid distance + // from the walls (if the original map is provided). If the map is provided in its normal appearance, then the start_grid_at_first_free_pixel + // parameter should be set to false and the first grid center will be placed with appropriate distance to the walls. + // room_map = (optionally eroded) map of the room in [pixels] (erosion = growing of walls by robot radius --> inaccessible areas are already excluded) + // cell_centers = the computed grid cell center points in [pixels] + // cell_size = the grid spacing in [pixels] + // complete_cell_test = if this is set false, cells are only inserted to the grid if their centers are accessible. If set to true, then the cell + // is created if there are any accessible cells within the cell area. The cell center is however set to one of the accessible pixels. + // start_grid_at_first_free_pixel = if set to true, it is assumed that room_map is eroded by the inaccessible areas and hence the first cell center + // should be placed in the upper left pixel. If this is false, the cell center will be placed off the wall with + // cell_size spacing. + void generateStandardGrid(const cv::Mat& room_map, std::vector& cell_centers, const int cell_size, + const bool complete_cell_test=true, const bool start_grid_at_first_free_pixel=true) + { + // determine min and max coordinates of the room + int min_y = 1000000, max_y = 0, min_x = 1000000, max_x = 0; + for (int y=0; y(y,x)==255) + { + if(xmax_x) + max_x = x; + if(ymax_y) + max_y = y; + } + } + } + + // if the map was not eroded, the grid cell centers might be placed with cell_size/2 distance from the wall + const uint16_t half_cell_size = (uint16_t)cell_size/(uint16_t)2; // just ensure that the rounding is always reproducible + if (start_grid_at_first_free_pixel == false) + { + min_x += half_cell_size; + min_y += half_cell_size; + } + + // create the grid + if (complete_cell_test == true) + { + for(int y=min_y; y<=max_y; y+=cell_size) + { + for(int x=min_x; x<=max_x; x+=cell_size) + { + cv::Point cell_center(x,y); + if (completeCellTest(room_map, cell_center, cell_size) == true) + cell_centers.push_back(cell_center); + } + } + } + else + { + // only create cells where the cell center is accessible + for (int y=min_y; y<=max_y; y+=cell_size) + for (int x=min_x; x<=max_x; x+=cell_size) + if (room_map.at(y,x)==255) + cell_centers.push_back(cv::Point(x,y)); + } + } + + // checks the whole cell for accessible areas and sets cell_center to the cell-center-most accessible point in the cell + // room_map = the map with inaccessible areas = 0 and accessible areas = 255 + // cell_center = the provided cell center point to check, is updated with a new cell center if the original cell_center is not accessible but some other pixels in the cell around + // cell_size = the grid spacing in [pixels] + // returns true if any accessible cell was found in the cell area and then cell_center is returned with an updated value. If the cell does not contain + // any accessible pixel, the return value is false. + static bool completeCellTest(const cv::Mat& room_map, cv::Point& cell_center, const int cell_size) + { + const int x = cell_center.x; + const int y = cell_center.y; + if (room_map.at(y,x)==255) + { + // just take cell center if accessible + return true; + } + else + { + const uint16_t half_cell_size = (uint16_t)cell_size/(uint16_t)2; // just ensure that the rounding is always reproducible + const bool even_grid_size = ((cell_size%2)==0); + + // check whether there are accessible pixels within the cell + const int upper_bound = even_grid_size==true ? half_cell_size-1 : half_cell_size; // adapt the neighborhood accordingly for even and odd grid sizes + cv::Mat cell_pixels = cv::Mat::zeros(cell_size, cell_size, CV_8UC1); + int accessible_pixels = 0; + for (int dy=-half_cell_size; dy<=upper_bound; ++dy) + { + for (int dx=-half_cell_size; dx<=upper_bound; ++dx) + { + const int nx = x+dx; + const int ny = y+dy; + if (nx<0 || nx>=room_map.cols || ny<0 || ny>=room_map.rows) + continue; + if (room_map.at(ny,nx)==255) + { + ++accessible_pixels; + cell_pixels.at(half_cell_size+dy, half_cell_size+dx) = 255; + } + } + } + // if there are accessible pixels within the cell, find their center and use this as cell center + if (accessible_pixels>0) + { + // use distance transform to find the pixels with maximum distance to obstacles, take from the maximum distance pixels the one + // closest to the original cell center + cv::Mat distances; +#if CV_MAJOR_VERSION<=3 + cv::distanceTransform(cell_pixels, distances, CV_DIST_L2, 5); +#else + cv::distanceTransform(cell_pixels, distances, cv::DIST_L2, 5); +#endif + double max_distance = 0.; + cv::minMaxLoc(distances, 0, &max_distance, 0, &cell_center); + cell_center.x += x-half_cell_size; + cell_center.y += y-half_cell_size; + // if there are multiple candidates with same max distance, take the one closest to the center + double min_squared_center_distance = (x-cell_center.x)*(x-cell_center.x) + (y-cell_center.y)*(y-cell_center.y); + for (int v=0; v(v,u)==max_distance) + { + const double squared_center_distance = (u-half_cell_size)*(u-half_cell_size)+(v-half_cell_size)*(v-half_cell_size); + if (squared_center_distance < min_squared_center_distance) + { + cell_center = cv::Point(x-half_cell_size+u, y-half_cell_size+v); + min_squared_center_distance = squared_center_distance; + } + } + } + } + + // display new center point +// cv::Mat disp = distances.clone(); +// cv::Mat disp2; +// cv::normalize(disp, disp, 0, 1., cv::NORM_MINMAX); +// cv::resize(disp, disp2, cv::Size(), 10., 10., cv::INTER_AREA); +// cv::circle(disp2, 10*cv::Point(cell_center.x-x+half_cell_size, cell_center.y-y+half_cell_size), 2, cv::Scalar(0), CV_FILLED); +// cv::imshow("distance transform", disp2); +// cv::waitKey(); + + return true; + } + } + return false; + } + + + // Generates a grid with fixed vertical spacing and variable distance between the points in a line (parameter), s.t. the algorithm may create + // ordinary grid points or horizontal lines with defined vertical spacing. The generator considers obstacle safety distances and only places + // grid cells with sufficient distance to obstacles (parameter). If a grid point cannot be placed because it would be located in an obstacle + // or in the inaccessible area around an obstacle, then the algorithm tries to shift the point vertically up and down up to grid_spacing + // (parameter) units. As soon as it finds free space, the grid point is set there. It can happen that such a free point exists above and below + // the original placement of the grid point. Then the two options are store in an upper and lower trajectory and the user must later chose how + // to deal with these option. + // room_map = original map of the room as a CV_8UC1 map with 0=obstacles, 255=free space, in [pixels] + // inflated_room_map = map of the room with inflated obstacles (can be provided, if cv::Mat() is provided, it is computed here with map_inflation_radius) + // map_inflation_radius = the number of pixels obstacles shall be inflated if no precomputed inflated_room_map is provided (map_inflation_radius can be -1 otherwise), in [pixels] + // grid_points = a vector of BoustrophedonLine objects, each of them containing line information in upper_line and optionally another line in lower_line if two_valid_lines is true, in [pixels] + // min_max_map_coordinates = optionally precomputed min/max coordinates (min_x, max_x, min_y, max_y) of the free space in inflated_room_map, if cv::Vec4i(-1,-1,-1,-1) is provided, min/max map coordinates are computed by this function, in [pixels] + // grid_spacing = the basic distance between two grid cell centers, is used for vertical grid spacing, in [pixels] + // half_grid_spacing = the rounded half distance between two grid cell centers (the user shall define how it is rounded), in [pixels] + // grid_spacing_horizontal = this value allows to specify the horizontal basic distance between two grid cell centers, it can be set to grid_spacing if the basic horizontal spacing shall be identical to the vertical spacing, in [pixels] + // max_deviation_from_track = maximal allowed shift off the track to both sides for avoiding obstacles on track, setting max_deviation_from_track=grid_spacing is usually a good choice, for negative values max_deviation_from_track is set to grid_spacing, in [pixels] + static void generateBoustrophedonGrid(const cv::Mat& room_map, cv::Mat& inflated_room_map, const int map_inflation_radius, + BoustrophedonGrid& grid_points, const cv::Vec4i& min_max_map_coordinates, const int grid_spacing, const int half_grid_spacing, + const int grid_spacing_horizontal, int max_deviation_from_track = -1) + { + if (max_deviation_from_track < 0) + max_deviation_from_track = grid_spacing; + + // compute inflated_room_map if not provided + if (inflated_room_map.rows!=room_map.rows || inflated_room_map.cols!=room_map.cols) + cv::erode(room_map, inflated_room_map, cv::Mat(), cv::Point(-1, -1), map_inflation_radius); + + // compute min/max map coordinates if necessary + int min_x=inflated_room_map.cols, max_x=-1, min_y=inflated_room_map.rows, max_y=-1; + if (min_max_map_coordinates[0]==-1 && min_max_map_coordinates[1]==-1 && min_max_map_coordinates[2]==-1 && min_max_map_coordinates[3]==-1) + { + for (int v=0; v(v,u) == 255) + { + if (min_x > u) + min_x = u; + if (max_x < u) + max_x = u; + if (min_y > v) + min_y = v; + if (max_y < v) + max_y = v; + } + } + } + } + else + { + min_x = min_max_map_coordinates[0]; + max_x = min_max_map_coordinates[1]; + min_y = min_max_map_coordinates[2]; + max_y = min_max_map_coordinates[3]; + } + // if the room has no accessible cells, hence no min/max coordinates, return + if ((min_x==inflated_room_map.cols) || (max_x==-1) || (min_y==inflated_room_map.rows) || (max_y==-1)) + return; + + // create grid + const int squared_grid_spacing_horizontal = grid_spacing_horizontal*grid_spacing_horizontal; + //std::cout << "((max_y - min_y) <= grid_spacing): min_y=" << min_y << " max_y=" << max_y << " grid_spacing=" << grid_spacing << std::endl; + int y=min_y; + // loop through the vertical grid lines with regular grid spacing + for (; y<=max_y+half_grid_spacing; y += grid_spacing) // we use max_y+half_grid_spacing as upper bound to cover the bottom-most line as well + { + if (y > max_y) // this should happen at most once for the bottom line + y = max_y; + + BoustrophedonLine line; + const cv::Point invalid_point(-1,-1); + cv::Point last_added_grid_point_above(-10000,-10000), last_added_grid_point_below(-10000,-10000); // for keeping the horizontal grid distance + cv::Point last_valid_grid_point_above(-1,-1), last_valid_grid_point_below(-1,-1); // for adding the rightmost possible point + // loop through the horizontal grid points with horizontal grid spacing length + for (int x=min_x; x<=max_x; x+=1) + { + // points are added to the grid line as follows: + // 1. if the original point is accessible --> point is added to upper_line, invalid point (-1,-1) is added to lower_line + // 2. if the original point is not accessible: + // a) and no other point in the y-vicinity --> upper_line and lower_line are not updated + // b) but some point above and none below --> valid point is added to upper_line, invalid point (-1,-1) is added to lower_line + // c) but some point below and none above --> valid point is added to lower_line, invalid point (-1,-1) is added to upper_line + // d) but some point below and above are --> valid points are added to upper_line and lower_line, respectively + + // 1. check accessibility on regular location + if (inflated_room_map.at(y,x)==255) + { + if (squaredPointDistance(last_added_grid_point_above,cv::Point(x,y))>=squared_grid_spacing_horizontal) + { + line.upper_line.push_back(cv::Point(x,y)); + line.lower_line.push_back(invalid_point); + last_added_grid_point_above = cv::Point(x,y); + } + else + last_valid_grid_point_above = cv::Point(x,y); // store this point and add it to the upper line if it was the rightmost point + } + // todo: add parameter to switch else branch off + else // 2. check accessibility above or below the targeted point + { + // check accessibility above the target location + bool found_above = false; + int dy = -1; + for (; dy>-max_deviation_from_track; --dy) + { + if (y+dy>=0 && inflated_room_map.at(y+dy,x)==255) + { + found_above = true; + break; + } + } + if (found_above == true) + { + if (squaredPointDistance(last_added_grid_point_above,cv::Point(x,y+dy))>=squared_grid_spacing_horizontal) + { + line.upper_line.push_back(cv::Point(x,y+dy)); + line.lower_line.push_back(invalid_point); // can be overwritten below if this point also exists + last_added_grid_point_above = cv::Point(x,y+dy); + } + else + last_valid_grid_point_above = cv::Point(x,y+dy); // store this point and add it to the upper line if it was the rightmost point + } + + // check accessibility below the target location + bool found_below = false; + dy = 1; + for (; dy(y+dy,x)==255) + { + found_below = true; + break; + } + } + if (found_below == true) + { + if (squaredPointDistance(last_added_grid_point_below,cv::Point(x,y+dy))>=squared_grid_spacing_horizontal) + { + if (found_above == true) // update the existing entry + { + line.has_two_valid_lines = true; + line.lower_line.back().x = x; + line.lower_line.back().y = y+dy; + } + else // create a new entry + { + line.upper_line.push_back(invalid_point); + line.lower_line.push_back(cv::Point(x,y+dy)); + } + last_added_grid_point_below = cv::Point(x,y+dy); + } + else + last_valid_grid_point_below = cv::Point(x,y+dy); // store this point and add it to the lower line if it was the rightmost point + } + } + } + // add the rightmost points if available + if (last_valid_grid_point_above.x > -1 && last_valid_grid_point_above.x > last_added_grid_point_above.x) + { + // upper point is valid + line.upper_line.push_back(last_valid_grid_point_above); + if (last_valid_grid_point_below.x > -1 && last_valid_grid_point_below.x > last_added_grid_point_below.x) + line.lower_line.push_back(last_valid_grid_point_below); + else + line.lower_line.push_back(invalid_point); + } + else + { + // upper point is invalid + if (last_valid_grid_point_below.x > -1 && last_valid_grid_point_below.x > last_added_grid_point_below.x) + { + // lower point is valid + line.upper_line.push_back(invalid_point); + line.lower_line.push_back(last_valid_grid_point_below); + } + } + + // clean the grid line data + // 1. if there are no valid points --> do not add the line + // 2. if two_valid_lines is true, there are two individual lines available with places to visit + // 3. else there is just one valid line with data potentially distributed over upper_line and lower_line + BoustrophedonLine cleaned_line; + if (line.upper_line.size()>0 && line.lower_line.size()>0) // 1. check that there is valid data in the lines + { + // 2. if two_valid_lines is true, create two individual lines with places to visit + if (line.has_two_valid_lines == true) + { + cleaned_line.has_two_valid_lines = true; + for (size_t i=0; i. + * + ****************************************************************/ + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + + + +// Class that generates a room exploration path by laying a small grid over the given map and then planning the best path trough +// all points, by defining an traveling salesman problem (TSP). This class only produces a static path, regarding the given map +// in form of a point series. To react on dynamic obstacles, one has to do this in upper algorithms. +// +class GridPointExplorator +{ +public: + // constructor + GridPointExplorator(); + + // separate, interruptible thread for the external solvers + void tsp_solver_thread_concorde(ConcordeTSPSolver& tsp_solver, std::vector& optimal_order, + const cv::Mat& distance_matrix, const std::map& cleaned_index_to_original_index_mapping, const int start_node); + + void tsp_solver_thread_genetic(GeneticTSPSolver& tsp_solver, std::vector& optimal_order, + const cv::Mat& distance_matrix, const std::map& cleaned_index_to_original_index_mapping, const int start_node); + + void tsp_solver_thread(const int tsp_solver, std::vector& optimal_order, const cv::Mat& original_map, + const std::vector& points, const double downsampling_factor, const double robot_radius, const double map_resolution, + const int start_node); + + // Function that creates an exploration path for a given room. The room has to be drawn in a cv::Mat (filled with Bit-uchar), + // with free space drawn white (255) and obstacles as black (0). It returns a series of 2D poses that show to which positions + // the robot should drive at. + void getExplorationPath(const cv::Mat& room_map, std::vector& path, const double map_resolution, + const cv::Point starting_position, const cv::Point2d map_origin, const int cell_size, const bool plan_for_footprint, + const Eigen::Matrix robot_to_fov_vector, int tsp_solver, int64_t tsp_solver_timeout); +}; diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/common/include/ipa_room_exploration/histogram.h b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/common/include/ipa_room_exploration/histogram.h new file mode 100644 index 0000000..152b055 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/common/include/ipa_room_exploration/histogram.h @@ -0,0 +1,132 @@ +/*! + ***************************************************************** + * \file + * + * \note + * Copyright (c) 2016 \n + * Fraunhofer Institute for Manufacturing Engineering + * and Automation (IPA) \n\n + * + ***************************************************************** + * + * \note + * Project name: Care-O-bot + * \note + * ROS stack name: autopnp + * \note + * ROS package name: ipa_room_exploration + * + * \author + * Author: Richard Bormann + * \author + * Supervised by: Richard Bormann + * + * \date Date of creation: 02.2017 + * + * \brief + * + * + ***************************************************************** + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * - Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. \n + * - Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. \n + * - Neither the name of the Fraunhofer Institute for Manufacturing + * Engineering and Automation (IPA) nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. \n + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License LGPL as + * published by the Free Software Foundation, either version 3 of the + * License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License LGPL for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License LGPL along with this program. + * If not, see . + * + ****************************************************************/ + +#pragma once + +#include +#include + +template +class Histogram +{ +public: + Histogram(const T lower_bound, const T upper_bound, const size_t histogram_bins) + { + lower_bound_ = lower_bound; + upper_bound_ = upper_bound; + range_inverse_ = 1.0/(upper_bound_-lower_bound_); + histogram_bins_ = histogram_bins; + data_.resize(histogram_bins); + for (size_t i=0; i(val, weight)); + } + + size_t getMaxBin() + { + double max_val = 0.; + size_t max_bin = 0; + for (size_t i=0; i > RawData; + + std::vector data_; // stores the histogram + std::vector raw_data_; // stores all entered data pairs (data, weight) for each histogram bin + T lower_bound_; // lowest possible value + T upper_bound_; // highest possible value + T range_inverse_; // = 1.0/(upper_bound_-lower_bound_) + size_t histogram_bins_; // number of histogram bins +}; diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/common/include/ipa_room_exploration/meanshift2d.h b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/common/include/ipa_room_exploration/meanshift2d.h new file mode 100644 index 0000000..d1ae641 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/common/include/ipa_room_exploration/meanshift2d.h @@ -0,0 +1,78 @@ +/*! + ***************************************************************** + * \file + * + * \note + * Copyright (c) 2015 \n + * Fraunhofer Institute for Manufacturing Engineering + * and Automation (IPA) \n\n + * + ***************************************************************** + * + * \note + * Project name: Care-O-bot + * \note + * ROS stack name: autopnp + * \note + * ROS package name: ipa_room_segmentation + * + * \author + * Author: Richard Bormann + * \author + * Supervised by: + * + * \date Date of creation: 22.07.2015 + * + * \brief + * + * + ***************************************************************** + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * - Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. \n + * - Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. \n + * - Neither the name of the Fraunhofer Institute for Manufacturing + * Engineering and Automation (IPA) nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. \n + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License LGPL as + * published by the Free Software Foundation, either version 3 of the + * License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License LGPL for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License LGPL along with this program. + * If not, see . + * + ****************************************************************/ + +#include + +#include +#include + +#pragma once + +class MeanShift2D +{ +public: + MeanShift2D(void) {}; + + void filter(const std::vector& data, std::vector& filtered_data, const double bandwidth, const int maximumIterations=100); + + void computeConvergencePoints(const std::vector& filtered_data, std::vector& convergence_points, std::vector< std::vector >& convergence_sets, const double sensitivity); + + // map_resolution in [m/cell] + cv::Vec2d findRoomCenter(const cv::Mat& room_image, const std::vector& room_cells, double map_resolution); +}; diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/common/include/ipa_room_exploration/nanoflann.hpp b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/common/include/ipa_room_exploration/nanoflann.hpp new file mode 100644 index 0000000..445aa29 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/common/include/ipa_room_exploration/nanoflann.hpp @@ -0,0 +1,1398 @@ +/*********************************************************************** + * Software License Agreement (BSD License) + * + * Copyright 2008-2009 Marius Muja (mariusm@cs.ubc.ca). All rights reserved. + * Copyright 2008-2009 David G. Lowe (lowe@cs.ubc.ca). All rights reserved. + * Copyright 2011-2016 Jose Luis Blanco (joseluisblancoc@gmail.com). + * All rights reserved. + * + * THE BSD LICENSE + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR + * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES + * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF + * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *************************************************************************/ + +/** \mainpage nanoflann C++ API documentation + * nanoflann is a C++ header-only library for building KD-Trees, mostly + * optimized for 2D or 3D point clouds. + * + * nanoflann does not require compiling or installing, just an + * #include in your code. + * + * See: + * - C++ API organized by modules + * - Online README + * - Doxygen documentation + */ + +#ifndef NANOFLANN_HPP_ +#define NANOFLANN_HPP_ + +#include +#include +#include +#include +#include // for fwrite() +#include // for abs() +#include // for abs() +#include + +// Avoid conflicting declaration of min/max macros in windows headers +#if !defined(NOMINMAX) && (defined(_WIN32) || defined(_WIN32_) || defined(WIN32) || defined(_WIN64)) +# define NOMINMAX +# ifdef max +# undef max +# undef min +# endif +#endif + +namespace nanoflann +{ +/** @addtogroup nanoflann_grp nanoflann C++ library for ANN + * @{ */ + + /** Library version: 0xMmP (M=Major,m=minor,P=patch) */ + #define NANOFLANN_VERSION 0x123 + + /** @addtogroup result_sets_grp Result set classes + * @{ */ + template + class KNNResultSet + { + IndexType * indices; + DistanceType* dists; + CountType capacity; + CountType count; + + public: + inline KNNResultSet(CountType capacity_) : indices(0), dists(0), capacity(capacity_), count(0) + { + } + + inline void init(IndexType* indices_, DistanceType* dists_) + { + indices = indices_; + dists = dists_; + count = 0; + if (capacity) + dists[capacity-1] = (std::numeric_limits::max)(); + } + + inline CountType size() const + { + return count; + } + + inline bool full() const + { + return count == capacity; + } + + + inline void addPoint(DistanceType dist, IndexType index) + { + CountType i; + for (i=count; i>0; --i) { +#ifdef NANOFLANN_FIRST_MATCH // If defined and two points have the same distance, the one with the lowest-index will be returned first. + if ( (dists[i-1]>dist) || ((dist==dists[i-1])&&(indices[i-1]>index)) ) { +#else + if (dists[i-1]>dist) { +#endif + if (i + class RadiusResultSet + { + public: + const DistanceType radius; + + std::vector >& m_indices_dists; + + inline RadiusResultSet(DistanceType radius_, std::vector >& indices_dists) : radius(radius_), m_indices_dists(indices_dists) + { + init(); + } + + inline ~RadiusResultSet() { } + + inline void init() { clear(); } + inline void clear() { m_indices_dists.clear(); } + + inline size_t size() const { return m_indices_dists.size(); } + + inline bool full() const { return true; } + + inline void addPoint(DistanceType dist, IndexType index) + { + if (dist 0 + */ + std::pair worst_item() const + { + if (m_indices_dists.empty()) throw std::runtime_error("Cannot invoke RadiusResultSet::worst_item() on an empty list of results."); + typedef typename std::vector >::const_iterator DistIt; + DistIt it = std::max_element(m_indices_dists.begin(), m_indices_dists.end()); + return *it; + } + }; + + /** operator "<" for std::sort() */ + struct IndexDist_Sorter + { + /** PairType will be typically: std::pair */ + template + inline bool operator()(const PairType &p1, const PairType &p2) const { + return p1.second < p2.second; + } + }; + + /** @} */ + + + /** @addtogroup loadsave_grp Load/save auxiliary functions + * @{ */ + template + void save_value(FILE* stream, const T& value, size_t count = 1) + { + fwrite(&value, sizeof(value),count, stream); + } + + template + void save_value(FILE* stream, const std::vector& value) + { + size_t size = value.size(); + fwrite(&size, sizeof(size_t), 1, stream); + fwrite(&value[0], sizeof(T), size, stream); + } + + template + void load_value(FILE* stream, T& value, size_t count = 1) + { + size_t read_cnt = fread(&value, sizeof(value), count, stream); + if (read_cnt != count) { + throw std::runtime_error("Cannot read from file"); + } + } + + + template + void load_value(FILE* stream, std::vector& value) + { + size_t size; + size_t read_cnt = fread(&size, sizeof(size_t), 1, stream); + if (read_cnt!=1) { + throw std::runtime_error("Cannot read from file"); + } + value.resize(size); + read_cnt = fread(&value[0], sizeof(T), size, stream); + if (read_cnt!=size) { + throw std::runtime_error("Cannot read from file"); + } + } + /** @} */ + + + /** @addtogroup metric_grp Metric (distance) classes + * @{ */ + + /** Manhattan distance functor (generic version, optimized for high-dimensionality data sets). + * Corresponding distance traits: nanoflann::metric_L1 + * \tparam T Type of the elements (e.g. double, float, uint8_t) + * \tparam _DistanceType Type of distance variables (must be signed) (e.g. float, double, int64_t) + */ + template + struct L1_Adaptor + { + typedef T ElementType; + typedef _DistanceType DistanceType; + + const DataSource &data_source; + + L1_Adaptor(const DataSource &_data_source) : data_source(_data_source) { } + + inline DistanceType operator()(const T* a, const size_t b_idx, size_t size, DistanceType worst_dist = -1) const + { + DistanceType result = DistanceType(); + const T* last = a + size; + const T* lastgroup = last - 3; + size_t d = 0; + + /* Process 4 items with each loop for efficiency. */ + while (a < lastgroup) { + const DistanceType diff0 = std::abs(a[0] - data_source.kdtree_get_pt(b_idx,d++)); + const DistanceType diff1 = std::abs(a[1] - data_source.kdtree_get_pt(b_idx,d++)); + const DistanceType diff2 = std::abs(a[2] - data_source.kdtree_get_pt(b_idx,d++)); + const DistanceType diff3 = std::abs(a[3] - data_source.kdtree_get_pt(b_idx,d++)); + result += diff0 + diff1 + diff2 + diff3; + a += 4; + if ((worst_dist>0)&&(result>worst_dist)) { + return result; + } + } + /* Process last 0-3 components. Not needed for standard vector lengths. */ + while (a < last) { + result += std::abs( *a++ - data_source.kdtree_get_pt(b_idx,d++) ); + } + return result; + } + + template + inline DistanceType accum_dist(const U a, const V b, int ) const + { + return std::abs(a-b); + } + }; + + /** Squared Euclidean distance functor (generic version, optimized for high-dimensionality data sets). + * Corresponding distance traits: nanoflann::metric_L2 + * \tparam T Type of the elements (e.g. double, float, uint8_t) + * \tparam _DistanceType Type of distance variables (must be signed) (e.g. float, double, int64_t) + */ + template + struct L2_Adaptor + { + typedef T ElementType; + typedef _DistanceType DistanceType; + + const DataSource &data_source; + + L2_Adaptor(const DataSource &_data_source) : data_source(_data_source) { } + + inline DistanceType operator()(const T* a, const size_t b_idx, size_t size, DistanceType worst_dist = -1) const + { + DistanceType result = DistanceType(); + const T* last = a + size; + const T* lastgroup = last - 3; + size_t d = 0; + + /* Process 4 items with each loop for efficiency. */ + while (a < lastgroup) { + const DistanceType diff0 = a[0] - data_source.kdtree_get_pt(b_idx,d++); + const DistanceType diff1 = a[1] - data_source.kdtree_get_pt(b_idx,d++); + const DistanceType diff2 = a[2] - data_source.kdtree_get_pt(b_idx,d++); + const DistanceType diff3 = a[3] - data_source.kdtree_get_pt(b_idx,d++); + result += diff0 * diff0 + diff1 * diff1 + diff2 * diff2 + diff3 * diff3; + a += 4; + if ((worst_dist>0)&&(result>worst_dist)) { + return result; + } + } + /* Process last 0-3 components. Not needed for standard vector lengths. */ + while (a < last) { + const DistanceType diff0 = *a++ - data_source.kdtree_get_pt(b_idx,d++); + result += diff0 * diff0; + } + return result; + } + + template + inline DistanceType accum_dist(const U a, const V b, int ) const + { + return (a-b)*(a-b); + } + }; + + /** Squared Euclidean (L2) distance functor (suitable for low-dimensionality datasets, like 2D or 3D point clouds) + * Corresponding distance traits: nanoflann::metric_L2_Simple + * \tparam T Type of the elements (e.g. double, float, uint8_t) + * \tparam _DistanceType Type of distance variables (must be signed) (e.g. float, double, int64_t) + */ + template + struct L2_Simple_Adaptor + { + typedef T ElementType; + typedef _DistanceType DistanceType; + + const DataSource &data_source; + + L2_Simple_Adaptor(const DataSource &_data_source) : data_source(_data_source) { } + + inline DistanceType operator()(const T* a, const size_t b_idx, size_t size) const { + return data_source.kdtree_distance(a,b_idx,size); + } + + template + inline DistanceType accum_dist(const U a, const V b, int ) const + { + return (a-b)*(a-b); + } + }; + + /** Metaprogramming helper traits class for the L1 (Manhattan) metric */ + struct metric_L1 { + template + struct traits { + typedef L1_Adaptor distance_t; + }; + }; + /** Metaprogramming helper traits class for the L2 (Euclidean) metric */ + struct metric_L2 { + template + struct traits { + typedef L2_Adaptor distance_t; + }; + }; + /** Metaprogramming helper traits class for the L2_simple (Euclidean) metric */ + struct metric_L2_Simple { + template + struct traits { + typedef L2_Simple_Adaptor distance_t; + }; + }; + + /** @} */ + + /** @addtogroup param_grp Parameter structs + * @{ */ + + /** Parameters (see README.md) */ + struct KDTreeSingleIndexAdaptorParams + { + KDTreeSingleIndexAdaptorParams(size_t _leaf_max_size = 10) : + leaf_max_size(_leaf_max_size) + {} + + size_t leaf_max_size; + }; + + /** Search options for KDTreeSingleIndexAdaptor::findNeighbors() */ + struct SearchParams + { + /** Note: The first argument (checks_IGNORED_) is ignored, but kept for compatibility with the FLANN interface */ + SearchParams(int checks_IGNORED_ = 32, float eps_ = 0, bool sorted_ = true ) : + checks(checks_IGNORED_), eps(eps_), sorted(sorted_) {} + + int checks; //!< Ignored parameter (Kept for compatibility with the FLANN interface). + float eps; //!< search for eps-approximate neighbours (default: 0) + bool sorted; //!< only for radius search, require neighbours sorted by distance (default: true) + }; + /** @} */ + + + /** @addtogroup memalloc_grp Memory allocation + * @{ */ + + /** + * Allocates (using C's malloc) a generic type T. + * + * Params: + * count = number of instances to allocate. + * Returns: pointer (of type T*) to memory buffer + */ + template + inline T* allocate(size_t count = 1) + { + T* mem = static_cast( ::malloc(sizeof(T)*count)); + return mem; + } + + + /** + * Pooled storage allocator + * + * The following routines allow for the efficient allocation of storage in + * small chunks from a specified pool. Rather than allowing each structure + * to be freed individually, an entire pool of storage is freed at once. + * This method has two advantages over just using malloc() and free(). First, + * it is far more efficient for allocating small objects, as there is + * no overhead for remembering all the information needed to free each + * object or consolidating fragmented memory. Second, the decision about + * how long to keep an object is made at the time of allocation, and there + * is no need to track down all the objects to free them. + * + */ + + const size_t WORDSIZE=16; + const size_t BLOCKSIZE=8192; + + class PooledAllocator + { + /* We maintain memory alignment to word boundaries by requiring that all + allocations be in multiples of the machine wordsize. */ + /* Size of machine word in bytes. Must be power of 2. */ + /* Minimum number of bytes requested at a time from the system. Must be multiple of WORDSIZE. */ + + + size_t remaining; /* Number of bytes left in current block of storage. */ + void* base; /* Pointer to base of current block of storage. */ + void* loc; /* Current location in block to next allocate memory. */ + + void internal_init() + { + remaining = 0; + base = NULL; + usedMemory = 0; + wastedMemory = 0; + } + + public: + size_t usedMemory; + size_t wastedMemory; + + /** + Default constructor. Initializes a new pool. + */ + PooledAllocator() { + internal_init(); + } + + /** + * Destructor. Frees all the memory allocated in this pool. + */ + ~PooledAllocator() { + free_all(); + } + + /** Frees all allocated memory chunks */ + void free_all() + { + while (base != NULL) { + void *prev = *(static_cast( base)); /* Get pointer to prev block. */ + ::free(base); + base = prev; + } + internal_init(); + } + + /** + * Returns a pointer to a piece of new memory of the given size in bytes + * allocated from the pool. + */ + void* malloc(const size_t req_size) + { + /* Round size up to a multiple of wordsize. The following expression + only works for WORDSIZE that is a power of 2, by masking last bits of + incremented size to zero. + */ + const size_t size = (req_size + (WORDSIZE - 1)) & ~(WORDSIZE - 1); + + /* Check whether a new block must be allocated. Note that the first word + of a block is reserved for a pointer to the previous block. + */ + if (size > remaining) { + + wastedMemory += remaining; + + /* Allocate new storage. */ + const size_t blocksize = (size + sizeof(void*) + (WORDSIZE-1) > BLOCKSIZE) ? + size + sizeof(void*) + (WORDSIZE-1) : BLOCKSIZE; + + // use the standard C malloc to allocate memory + void* m = ::malloc(blocksize); + if (!m) { + fprintf(stderr,"Failed to allocate memory.\n"); + return NULL; + } + + /* Fill first word of new block with pointer to previous block. */ + static_cast(m)[0] = base; + base = m; + + size_t shift = 0; + //int size_t = (WORDSIZE - ( (((size_t)m) + sizeof(void*)) & (WORDSIZE-1))) & (WORDSIZE-1); + + remaining = blocksize - sizeof(void*) - shift; + loc = (static_cast(m) + sizeof(void*) + shift); + } + void* rloc = loc; + loc = static_cast(loc) + size; + remaining -= size; + + usedMemory += size; + + return rloc; + } + + /** + * Allocates (using this pool) a generic type T. + * + * Params: + * count = number of instances to allocate. + * Returns: pointer (of type T*) to memory buffer + */ + template + T* allocate(const size_t count = 1) + { + T* mem = static_cast(this->malloc(sizeof(T)*count)); + return mem; + } + + }; + /** @} */ + + /** @addtogroup nanoflann_metaprog_grp Auxiliary metaprogramming stuff + * @{ */ + + // ---------------- CArray ------------------------- + /** A STL container (as wrapper) for arrays of constant size defined at compile time (class imported from the MRPT project) + * This code is an adapted version from Boost, modifed for its integration + * within MRPT (JLBC, Dec/2009) (Renamed array -> CArray to avoid possible potential conflicts). + * See + * http://www.josuttis.com/cppcode + * for details and the latest version. + * See + * http://www.boost.org/libs/array for Documentation. + * for documentation. + * + * (C) Copyright Nicolai M. Josuttis 2001. + * Permission to copy, use, modify, sell and distribute this software + * is granted provided this copyright notice appears in all copies. + * This software is provided "as is" without express or implied + * warranty, and with no claim as to its suitability for any purpose. + * + * 29 Jan 2004 - minor fixes (Nico Josuttis) + * 04 Dec 2003 - update to synch with library TR1 (Alisdair Meredith) + * 23 Aug 2002 - fix for Non-MSVC compilers combined with MSVC libraries. + * 05 Aug 2001 - minor update (Nico Josuttis) + * 20 Jan 2001 - STLport fix (Beman Dawes) + * 29 Sep 2000 - Initial Revision (Nico Josuttis) + * + * Jan 30, 2004 + */ + template + class CArray { + public: + T elems[N]; // fixed-size array of elements of type T + + public: + // type definitions + typedef T value_type; + typedef T* iterator; + typedef const T* const_iterator; + typedef T& reference; + typedef const T& const_reference; + typedef std::size_t size_type; + typedef std::ptrdiff_t difference_type; + + // iterator support + inline iterator begin() { return elems; } + inline const_iterator begin() const { return elems; } + inline iterator end() { return elems+N; } + inline const_iterator end() const { return elems+N; } + + // reverse iterator support +#if !defined(BOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION) && !defined(BOOST_MSVC_STD_ITERATOR) && !defined(BOOST_NO_STD_ITERATOR_TRAITS) + typedef std::reverse_iterator reverse_iterator; + typedef std::reverse_iterator const_reverse_iterator; +#elif defined(_MSC_VER) && (_MSC_VER == 1300) && defined(BOOST_DINKUMWARE_STDLIB) && (BOOST_DINKUMWARE_STDLIB == 310) + // workaround for broken reverse_iterator in VC7 + typedef std::reverse_iterator > reverse_iterator; + typedef std::reverse_iterator > const_reverse_iterator; +#else + // workaround for broken reverse_iterator implementations + typedef std::reverse_iterator reverse_iterator; + typedef std::reverse_iterator const_reverse_iterator; +#endif + + reverse_iterator rbegin() { return reverse_iterator(end()); } + const_reverse_iterator rbegin() const { return const_reverse_iterator(end()); } + reverse_iterator rend() { return reverse_iterator(begin()); } + const_reverse_iterator rend() const { return const_reverse_iterator(begin()); } + // operator[] + inline reference operator[](size_type i) { return elems[i]; } + inline const_reference operator[](size_type i) const { return elems[i]; } + // at() with range check + reference at(size_type i) { rangecheck(i); return elems[i]; } + const_reference at(size_type i) const { rangecheck(i); return elems[i]; } + // front() and back() + reference front() { return elems[0]; } + const_reference front() const { return elems[0]; } + reference back() { return elems[N-1]; } + const_reference back() const { return elems[N-1]; } + // size is constant + static inline size_type size() { return N; } + static bool empty() { return false; } + static size_type max_size() { return N; } + enum { static_size = N }; + /** This method has no effects in this class, but raises an exception if the expected size does not match */ + inline void resize(const size_t nElements) { if (nElements!=N) throw std::logic_error("Try to change the size of a CArray."); } + // swap (note: linear complexity in N, constant for given instantiation) + void swap (CArray& y) { std::swap_ranges(begin(),end(),y.begin()); } + // direct access to data (read-only) + const T* data() const { return elems; } + // use array as C array (direct read/write access to data) + T* data() { return elems; } + // assignment with type conversion + template CArray& operator= (const CArray& rhs) { + std::copy(rhs.begin(),rhs.end(), begin()); + return *this; + } + // assign one value to all elements + inline void assign (const T& value) { for (size_t i=0;i= size()) { throw std::out_of_range("CArray<>: index out of range"); } } + }; // end of CArray + + /** Used to declare fixed-size arrays when DIM>0, dynamically-allocated vectors when DIM=-1. + * Fixed size version for a generic DIM: + */ + template + struct array_or_vector_selector + { + typedef CArray container_t; + }; + /** Dynamic size version */ + template + struct array_or_vector_selector<-1,T> { + typedef std::vector container_t; + }; + /** @} */ + + /** @addtogroup kdtrees_grp KD-tree classes and adaptors + * @{ */ + + /** kd-tree index + * + * Contains the k-d trees and other information for indexing a set of points + * for nearest-neighbor matching. + * + * The class "DatasetAdaptor" must provide the following interface (can be non-virtual, inlined methods): + * + * \code + * // Must return the number of data poins + * inline size_t kdtree_get_point_count() const { ... } + * + * // [Only if using the metric_L2_Simple type] Must return the Euclidean (L2) distance between the vector "p1[0:size-1]" and the data point with index "idx_p2" stored in the class: + * inline DistanceType kdtree_distance(const T *p1, const size_t idx_p2,size_t size) const { ... } + * + * // Must return the dim'th component of the idx'th point in the class: + * inline T kdtree_get_pt(const size_t idx, int dim) const { ... } + * + * // Optional bounding-box computation: return false to default to a standard bbox computation loop. + * // Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again. + * // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds) + * template + * bool kdtree_get_bbox(BBOX &bb) const + * { + * bb[0].low = ...; bb[0].high = ...; // 0th dimension limits + * bb[1].low = ...; bb[1].high = ...; // 1st dimension limits + * ... + * return true; + * } + * + * \endcode + * + * \tparam DatasetAdaptor The user-provided adaptor (see comments above). + * \tparam Distance The distance metric to use: nanoflann::metric_L1, nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. + * \tparam DIM Dimensionality of data points (e.g. 3 for 3D points) + * \tparam IndexType Will be typically size_t or int + */ + template + class KDTreeSingleIndexAdaptor + { + private: + /** Hidden copy constructor, to disallow copying indices (Not implemented) */ + KDTreeSingleIndexAdaptor(const KDTreeSingleIndexAdaptor&); + public: + typedef typename Distance::ElementType ElementType; + typedef typename Distance::DistanceType DistanceType; + protected: + + /** + * Array of indices to vectors in the dataset. + */ + std::vector vind; + + size_t m_leaf_max_size; + + + /** + * The dataset used by this index + */ + const DatasetAdaptor &dataset; //!< The source of our data + + const KDTreeSingleIndexAdaptorParams index_params; + + size_t m_size; //!< Number of current poins in the dataset + size_t m_size_at_index_build; //!< Number of points in the dataset when the index was built + int dim; //!< Dimensionality of each data point + + + /*--------------------- Internal Data Structures --------------------------*/ + struct Node + { + /** Union used because a node can be either a LEAF node or a non-leaf node, so both data fields are never used simultaneously */ + union { + struct leaf + { + IndexType left, right; //!< Indices of points in leaf node + } lr; + struct nonleaf + { + int divfeat; //!< Dimension used for subdivision. + DistanceType divlow, divhigh; //!< The values used for subdivision. + } sub; + } node_type; + Node* child1, * child2; //!< Child nodes (both=NULL mean its a leaf node) + }; + typedef Node* NodePtr; + + + struct Interval + { + ElementType low, high; + }; + + /** Define "BoundingBox" as a fixed-size or variable-size container depending on "DIM" */ + typedef typename array_or_vector_selector::container_t BoundingBox; + + /** Define "distance_vector_t" as a fixed-size or variable-size container depending on "DIM" */ + typedef typename array_or_vector_selector::container_t distance_vector_t; + + /** The KD-tree used to find neighbours */ + NodePtr root_node; + BoundingBox root_bbox; + + /** + * Pooled memory allocator. + * + * Using a pooled memory allocator is more efficient + * than allocating memory directly when there is a large + * number small of memory allocations. + */ + PooledAllocator pool; + + public: + + Distance distance; + + /** + * KDTree constructor + * + * Refer to docs in README.md or online in https://github.com/jlblancoc/nanoflann + * + * The KD-Tree point dimension (the length of each point in the datase, e.g. 3 for 3D points) + * is determined by means of: + * - The \a DIM template parameter if >0 (highest priority) + * - Otherwise, the \a dimensionality parameter of this constructor. + * + * @param inputData Dataset with the input features + * @param params Basically, the maximum leaf node size + */ + KDTreeSingleIndexAdaptor(const int dimensionality, const DatasetAdaptor& inputData, const KDTreeSingleIndexAdaptorParams& params = KDTreeSingleIndexAdaptorParams() ) : + dataset(inputData), index_params(params), root_node(NULL), distance(inputData) + { + m_size = dataset.kdtree_get_point_count(); + m_size_at_index_build = m_size; + dim = dimensionality; + if (DIM>0) dim=DIM; + m_leaf_max_size = params.leaf_max_size; + + // Create a permutable array of indices to the input vectors. + init_vind(); + } + + /** Standard destructor */ + ~KDTreeSingleIndexAdaptor() { } + + /** Frees the previously-built index. Automatically called within buildIndex(). */ + void freeIndex() + { + pool.free_all(); + root_node=NULL; + m_size_at_index_build = 0; + } + + /** + * Builds the index + */ + void buildIndex() + { + init_vind(); + freeIndex(); + m_size_at_index_build = m_size; + if(m_size == 0) return; + computeBoundingBox(root_bbox); + root_node = divideTree(0, m_size, root_bbox ); // construct the tree + } + + /** Returns number of points in dataset */ + size_t size() const { return m_size; } + + /** Returns the length of each point in the dataset */ + size_t veclen() const { + return static_cast(DIM>0 ? DIM : dim); + } + + /** + * Computes the inde memory usage + * Returns: memory used by the index + */ + size_t usedMemory() const + { + return pool.usedMemory+pool.wastedMemory+dataset.kdtree_get_point_count()*sizeof(IndexType); // pool memory and vind array memory + } + + /** \name Query methods + * @{ */ + + /** + * Find set of nearest neighbors to vec[0:dim-1]. Their indices are stored inside + * the result object. + * + * Params: + * result = the result object in which the indices of the nearest-neighbors are stored + * vec = the vector for which to search the nearest neighbors + * + * \tparam RESULTSET Should be any ResultSet + * \return True if the requested neighbors could be found. + * \sa knnSearch, radiusSearch + */ + template + bool findNeighbors(RESULTSET& result, const ElementType* vec, const SearchParams& searchParams) const + { + assert(vec); + if (size() == 0) + return false; + if (!root_node) + throw std::runtime_error("[nanoflann] findNeighbors() called before building the index."); + float epsError = 1+searchParams.eps; + + distance_vector_t dists; // fixed or variable-sized container (depending on DIM) + dists.assign((DIM>0 ? DIM : dim) ,0); // Fill it with zeros. + DistanceType distsq = computeInitialDistances(vec, dists); + searchLevel(result, vec, root_node, distsq, dists, epsError); // "count_leaf" parameter removed since was neither used nor returned to the user. + return result.full(); + } + + /** + * Find the "num_closest" nearest neighbors to the \a query_point[0:dim-1]. Their indices are stored inside + * the result object. + * \sa radiusSearch, findNeighbors + * \note nChecks_IGNORED is ignored but kept for compatibility with the original FLANN interface. + * \return Number `N` of valid points in the result set. Only the first `N` entries in `out_indices` and `out_distances_sq` will be valid. + * Return may be less than `num_closest` only if the number of elements in the tree is less than `num_closest`. + */ + size_t knnSearch(const ElementType *query_point, const size_t num_closest, IndexType *out_indices, DistanceType *out_distances_sq, const int /* nChecks_IGNORED */ = 10) const + { + nanoflann::KNNResultSet resultSet(num_closest); + resultSet.init(out_indices, out_distances_sq); + this->findNeighbors(resultSet, query_point, nanoflann::SearchParams()); + return resultSet.size(); + } + + /** + * Find all the neighbors to \a query_point[0:dim-1] within a maximum radius. + * The output is given as a vector of pairs, of which the first element is a point index and the second the corresponding distance. + * Previous contents of \a IndicesDists are cleared. + * + * If searchParams.sorted==true, the output list is sorted by ascending distances. + * + * For a better performance, it is advisable to do a .reserve() on the vector if you have any wild guess about the number of expected matches. + * + * \sa knnSearch, findNeighbors, radiusSearchCustomCallback + * \return The number of points within the given radius (i.e. indices.size() or dists.size() ) + */ + size_t radiusSearch(const ElementType *query_point,const DistanceType &radius, std::vector >& IndicesDists, const SearchParams& searchParams) const + { + RadiusResultSet resultSet(radius,IndicesDists); + const size_t nFound = radiusSearchCustomCallback(query_point,resultSet,searchParams); + if (searchParams.sorted) + std::sort(IndicesDists.begin(),IndicesDists.end(), IndexDist_Sorter() ); + return nFound; + } + + /** + * Just like radiusSearch() but with a custom callback class for each point found in the radius of the query. + * See the source of RadiusResultSet<> as a start point for your own classes. + * \sa radiusSearch + */ + template + size_t radiusSearchCustomCallback(const ElementType *query_point,SEARCH_CALLBACK &resultSet, const SearchParams& searchParams = SearchParams() ) const + { + this->findNeighbors(resultSet, query_point, searchParams); + return resultSet.size(); + } + + /** @} */ + + private: + /** Make sure the auxiliary list \a vind has the same size than the current dataset, and re-generate if size has changed. */ + void init_vind() + { + // Create a permutable array of indices to the input vectors. + m_size = dataset.kdtree_get_point_count(); + if (vind.size()!=m_size) vind.resize(m_size); + for (size_t i = 0; i < m_size; i++) vind[i] = i; + } + + /// Helper accessor to the dataset points: + inline ElementType dataset_get(size_t idx, int component) const { + return dataset.kdtree_get_pt(idx,component); + } + + + void save_tree(FILE* stream, NodePtr tree) + { + save_value(stream, *tree); + if (tree->child1!=NULL) { + save_tree(stream, tree->child1); + } + if (tree->child2!=NULL) { + save_tree(stream, tree->child2); + } + } + + + void load_tree(FILE* stream, NodePtr& tree) + { + tree = pool.allocate(); + load_value(stream, *tree); + if (tree->child1!=NULL) { + load_tree(stream, tree->child1); + } + if (tree->child2!=NULL) { + load_tree(stream, tree->child2); + } + } + + + void computeBoundingBox(BoundingBox& bbox) + { + bbox.resize((DIM>0 ? DIM : dim)); + if (dataset.kdtree_get_bbox(bbox)) + { + // Done! It was implemented in derived class + } + else + { + const size_t N = dataset.kdtree_get_point_count(); + if (!N) throw std::runtime_error("[nanoflann] computeBoundingBox() called but no data points found."); + for (int i=0; i<(DIM>0 ? DIM : dim); ++i) { + bbox[i].low = + bbox[i].high = dataset_get(0,i); + } + for (size_t k=1; k0 ? DIM : dim); ++i) { + if (dataset_get(k,i)bbox[i].high) bbox[i].high = dataset_get(k,i); + } + } + } + } + + + /** + * Create a tree node that subdivides the list of vecs from vind[first] + * to vind[last]. The routine is called recursively on each sublist. + * + * @param left index of the first vector + * @param right index of the last vector + */ + NodePtr divideTree(const IndexType left, const IndexType right, BoundingBox& bbox) + { + NodePtr node = pool.allocate(); // allocate memory + + /* If too few exemplars remain, then make this a leaf node. */ + if ( (right-left) <= static_cast(m_leaf_max_size) ) { + node->child1 = node->child2 = NULL; /* Mark as leaf node. */ + node->node_type.lr.left = left; + node->node_type.lr.right = right; + + // compute bounding-box of leaf points + for (int i=0; i<(DIM>0 ? DIM : dim); ++i) { + bbox[i].low = dataset_get(vind[left],i); + bbox[i].high = dataset_get(vind[left],i); + } + for (IndexType k=left+1; k0 ? DIM : dim); ++i) { + if (bbox[i].low>dataset_get(vind[k],i)) bbox[i].low=dataset_get(vind[k],i); + if (bbox[i].highnode_type.sub.divfeat = cutfeat; + + BoundingBox left_bbox(bbox); + left_bbox[cutfeat].high = cutval; + node->child1 = divideTree(left, left+idx, left_bbox); + + BoundingBox right_bbox(bbox); + right_bbox[cutfeat].low = cutval; + node->child2 = divideTree(left+idx, right, right_bbox); + + node->node_type.sub.divlow = left_bbox[cutfeat].high; + node->node_type.sub.divhigh = right_bbox[cutfeat].low; + + for (int i=0; i<(DIM>0 ? DIM : dim); ++i) { + bbox[i].low = std::min(left_bbox[i].low, right_bbox[i].low); + bbox[i].high = std::max(left_bbox[i].high, right_bbox[i].high); + } + } + + return node; + } + + + void computeMinMax(IndexType* ind, IndexType count, int element, ElementType& min_elem, ElementType& max_elem) + { + min_elem = dataset_get(ind[0],element); + max_elem = dataset_get(ind[0],element); + for (IndexType i=1; imax_elem) max_elem = val; + } + } + + void middleSplit_(IndexType* ind, IndexType count, IndexType& index, int& cutfeat, DistanceType& cutval, const BoundingBox& bbox) + { + const DistanceType EPS=static_cast(0.00001); + ElementType max_span = bbox[0].high-bbox[0].low; + for (int i=1; i<(DIM>0 ? DIM : dim); ++i) { + ElementType span = bbox[i].high-bbox[i].low; + if (span>max_span) { + max_span = span; + } + } + ElementType max_spread = -1; + cutfeat = 0; + for (int i=0; i<(DIM>0 ? DIM : dim); ++i) { + ElementType span = bbox[i].high-bbox[i].low; + if (span>(1-EPS)*max_span) { + ElementType min_elem, max_elem; + computeMinMax(ind, count, i, min_elem, max_elem); + ElementType spread = max_elem-min_elem;; + if (spread>max_spread) { + cutfeat = i; + max_spread = spread; + } + } + } + // split in the middle + DistanceType split_val = (bbox[cutfeat].low+bbox[cutfeat].high)/2; + ElementType min_elem, max_elem; + computeMinMax(ind, count, cutfeat, min_elem, max_elem); + + if (split_valmax_elem) cutval = max_elem; + else cutval = split_val; + + IndexType lim1, lim2; + planeSplit(ind, count, cutfeat, cutval, lim1, lim2); + + if (lim1>count/2) index = lim1; + else if (lim2cutval + */ + void planeSplit(IndexType* ind, const IndexType count, int cutfeat, DistanceType &cutval, IndexType& lim1, IndexType& lim2) + { + /* Move vector indices for left subtree to front of list. */ + IndexType left = 0; + IndexType right = count-1; + for (;; ) { + while (left<=right && dataset_get(ind[left],cutfeat)=cutval) --right; + if (left>right || !right) break; // "!right" was added to support unsigned Index types + std::swap(ind[left], ind[right]); + ++left; + --right; + } + /* If either list is empty, it means that all remaining features + * are identical. Split in the middle to maintain a balanced tree. + */ + lim1 = left; + right = count-1; + for (;; ) { + while (left<=right && dataset_get(ind[left],cutfeat)<=cutval) ++left; + while (right && left<=right && dataset_get(ind[right],cutfeat)>cutval) --right; + if (left>right || !right) break; // "!right" was added to support unsigned Index types + std::swap(ind[left], ind[right]); + ++left; + --right; + } + lim2 = left; + } + + DistanceType computeInitialDistances(const ElementType* vec, distance_vector_t& dists) const + { + assert(vec); + DistanceType distsq = DistanceType(); + + for (int i = 0; i < (DIM>0 ? DIM : dim); ++i) { + if (vec[i] < root_bbox[i].low) { + dists[i] = distance.accum_dist(vec[i], root_bbox[i].low, i); + distsq += dists[i]; + } + if (vec[i] > root_bbox[i].high) { + dists[i] = distance.accum_dist(vec[i], root_bbox[i].high, i); + distsq += dists[i]; + } + } + + return distsq; + } + + /** + * Performs an exact search in the tree starting from a node. + * \tparam RESULTSET Should be any ResultSet + */ + template + void searchLevel(RESULTSET& result_set, const ElementType* vec, const NodePtr node, DistanceType mindistsq, + distance_vector_t& dists, const float epsError) const + { + /* If this is a leaf node, then do check and return. */ + if ((node->child1 == NULL)&&(node->child2 == NULL)) { + //count_leaf += (node->lr.right-node->lr.left); // Removed since was neither used nor returned to the user. + DistanceType worst_dist = result_set.worstDist(); + for (IndexType i=node->node_type.lr.left; inode_type.lr.right; ++i) { + const IndexType index = vind[i];// reorder... : i; + DistanceType dist = distance(vec, index, (DIM>0 ? DIM : dim)); + if (distnode_type.sub.divfeat; + ElementType val = vec[idx]; + DistanceType diff1 = val - node->node_type.sub.divlow; + DistanceType diff2 = val - node->node_type.sub.divhigh; + + NodePtr bestChild; + NodePtr otherChild; + DistanceType cut_dist; + if ((diff1+diff2)<0) { + bestChild = node->child1; + otherChild = node->child2; + cut_dist = distance.accum_dist(val, node->node_type.sub.divhigh, idx); + } + else { + bestChild = node->child2; + otherChild = node->child1; + cut_dist = distance.accum_dist( val, node->node_type.sub.divlow, idx); + } + + /* Call recursively to search next level down. */ + searchLevel(result_set, vec, bestChild, mindistsq, dists, epsError); + + DistanceType dst = dists[idx]; + mindistsq = mindistsq + cut_dist - dst; + dists[idx] = cut_dist; + if (mindistsq*epsError<=result_set.worstDist()) { + searchLevel(result_set, vec, otherChild, mindistsq, dists, epsError); + } + dists[idx] = dst; + } + + public: + /** Stores the index in a binary file. + * IMPORTANT NOTE: The set of data points is NOT stored in the file, so when loading the index object it must be constructed associated to the same source of data points used while building it. + * See the example: examples/saveload_example.cpp + * \sa loadIndex */ + void saveIndex(FILE* stream) + { + save_value(stream, m_size); + save_value(stream, dim); + save_value(stream, root_bbox); + save_value(stream, m_leaf_max_size); + save_value(stream, vind); + save_tree(stream, root_node); + } + + /** Loads a previous index from a binary file. + * IMPORTANT NOTE: The set of data points is NOT stored in the file, so the index object must be constructed associated to the same source of data points used while building the index. + * See the example: examples/saveload_example.cpp + * \sa loadIndex */ + void loadIndex(FILE* stream) + { + load_value(stream, m_size); + load_value(stream, dim); + load_value(stream, root_bbox); + load_value(stream, m_leaf_max_size); + load_value(stream, vind); + load_tree(stream, root_node); + } + + }; // class KDTree + + + /** An L2-metric KD-tree adaptor for working with data directly stored in an Eigen Matrix, without duplicating the data storage. + * Each row in the matrix represents a point in the state space. + * + * Example of usage: + * \code + * Eigen::Matrix mat; + * // Fill out "mat"... + * + * typedef KDTreeEigenMatrixAdaptor< Eigen::Matrix > my_kd_tree_t; + * const int max_leaf = 10; + * my_kd_tree_t mat_index(dimdim, mat, max_leaf ); + * mat_index.index->buildIndex(); + * mat_index.index->... + * \endcode + * + * \tparam DIM If set to >0, it specifies a compile-time fixed dimensionality for the points in the data set, allowing more compiler optimizations. + * \tparam Distance The distance metric to use: nanoflann::metric_L1, nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. + */ + template + struct KDTreeEigenMatrixAdaptor + { + typedef KDTreeEigenMatrixAdaptor self_t; + typedef typename MatrixType::Scalar num_t; + typedef typename MatrixType::Index IndexType; + typedef typename Distance::template traits::distance_t metric_t; + typedef KDTreeSingleIndexAdaptor< metric_t,self_t,DIM,IndexType> index_t; + + index_t* index; //! The kd-tree index for the user to call its methods as usual with any other FLANN index. + + /// Constructor: takes a const ref to the matrix object with the data points + KDTreeEigenMatrixAdaptor(const int dimensionality, const MatrixType &mat, const int leaf_max_size = 10) : m_data_matrix(mat) + { + const IndexType dims = mat.cols(); + if (dims!=dimensionality) throw std::runtime_error("Error: 'dimensionality' must match column count in data matrix"); + if (DIM>0 && static_cast(dims)!=DIM) + throw std::runtime_error("Data set dimensionality does not match the 'DIM' template argument"); + index = new index_t( dims, *this /* adaptor */, nanoflann::KDTreeSingleIndexAdaptorParams(leaf_max_size ) ); + index->buildIndex(); + } + private: + /** Hidden copy constructor, to disallow copying this class (Not implemented) */ + KDTreeEigenMatrixAdaptor(const self_t&); + public: + + ~KDTreeEigenMatrixAdaptor() { + delete index; + } + + const MatrixType &m_data_matrix; + + /** Query for the \a num_closest closest points to a given point (entered as query_point[0:dim-1]). + * Note that this is a short-cut method for index->findNeighbors(). + * The user can also call index->... methods as desired. + * \note nChecks_IGNORED is ignored but kept for compatibility with the original FLANN interface. + */ + inline void query(const num_t *query_point, const size_t num_closest, IndexType *out_indices, num_t *out_distances_sq, const int /* nChecks_IGNORED */ = 10) const + { + nanoflann::KNNResultSet resultSet(num_closest); + resultSet.init(out_indices, out_distances_sq); + index->findNeighbors(resultSet, query_point, nanoflann::SearchParams()); + } + + /** @name Interface expected by KDTreeSingleIndexAdaptor + * @{ */ + + const self_t & derived() const { + return *this; + } + self_t & derived() { + return *this; + } + + // Must return the number of data points + inline size_t kdtree_get_point_count() const { + return m_data_matrix.rows(); + } + + // Returns the L2 distance between the vector "p1[0:size-1]" and the data point with index "idx_p2" stored in the class: + inline num_t kdtree_distance(const num_t *p1, const IndexType idx_p2,IndexType size) const + { + num_t s=0; + for (IndexType i=0; i + bool kdtree_get_bbox(BBOX& /*bb*/) const { + return false; + } + + /** @} */ + + }; // end of KDTreeEigenMatrixAdaptor + /** @} */ + +/** @} */ // end of grouping +} // end of NS + + +#endif /* NANOFLANN_HPP_ */ diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/common/include/ipa_room_exploration/neural_network_explorator.h b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/common/include/ipa_room_exploration/neural_network_explorator.h new file mode 100644 index 0000000..cfa0f0e --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/common/include/ipa_room_exploration/neural_network_explorator.h @@ -0,0 +1,148 @@ +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +/*! + ***************************************************************** + * \file + * + * \note + * Copyright (c) 2016 \n + * Fraunhofer Institute for Manufacturing Engineering + * and Automation (IPA) \n\n + * + ***************************************************************** + * + * \note + * Project name: Care-O-bot + * \note + * ROS stack name: autopnp + * \note + * ROS package name: ipa_room_exploration + * + * \author + * Author: Florian Jordan + * \author + * Supervised by: Richard Bormann + * + * \date Date of creation: 11.2016 + * + * \brief + * + * + ***************************************************************** + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * - Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. \n + * - Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. \n + * - Neither the name of the Fraunhofer Institute for Manufacturing + * Engineering and Automation (IPA) nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. \n + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License LGPL as + * published by the Free Software Foundation, either version 3 of the + * License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License LGPL for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License LGPL along with this program. + * If not, see . + * + ****************************************************************/ + +#pragma once + +#define PI 3.14159265359 + +// Definition if the operator == for geometry_msgs::Pose2D for checking how often a specific pose is in the so far calculated +// path. +inline bool operator==(const geometry_msgs::Pose2D& A, const geometry_msgs::Pose2D& B) +{ + if(A.x == B.x && A.y == B.y) + return true; + return false; +} + +// This class provides a room explorator based on an artificial neural network. This network is used to compute a +// coverage path s.t. the whole environment is visited at least once. The used method is stated in: +// +// Yang, Simon X., and Chaomin Luo. "A neural network approach to complete coverage path planning." IEEE Transactions on Systems, Man, and Cybernetics, Part B (Cybernetics) 34.1 (2004): 718-724. +// +// In this algorithm the free space gets sampled into several neurons that span a neural network over the space. Each +// neuron then needs to be visited once to clean it. Each neuron has a state that is used later to determine the next +// neuron that needs to be visited. Going then trough the space over time produces a path that covers all neurons, see +// the stated paper for reference. +// This class provides the functionality to provide a room map and discretize it into several neurons, based on the given +// sampling distance and the radius of the robot/field-of-view (assuming that the footprint/fov gets approximated by a +// inner circle). After this the coverage path gets computed based on the stated paper. This implementation only provides +// a static path, any reaction to unexpected behavior (e.g. sudden obstacles) need to be done in an upper program. +// +class NeuralNetworkExplorator +{ +protected: + + // vector that stores the neurons of the given map + std::vector > neurons_; + + // step size used for integrating the states of the neurons + double step_size_; + + // parameters for the neural network + double A_, B_, D_, E_, mu_, delta_theta_weight_; + +public: + + // constructor + NeuralNetworkExplorator(); + + // function to set the step size to a certain value + void setStepSize(double step_size) + { + step_size_ = step_size; + } + + // function to set the parameters needed for the neural network + void setParameters(double A, double B, double D, double E, double mu, double step_size, double delta_theta_weight) + { + A_ = A; + B_ = B; + D_ = D; + E_ = E; + mu_ = mu; + step_size_ = step_size; + delta_theta_weight_ = delta_theta_weight; + } + + // Function that creates an exploration path for a given room. The room has to be drawn in a cv::Mat (filled with Bit-uchar), + // with free space drawn white (255) and obstacles as black (0). It returns a series of 2D poses that show to which positions + // the robot should drive at. + void getExplorationPath(const cv::Mat& room_map, std::vector& path, const float map_resolution, + const cv::Point starting_position, const cv::Point2d map_origin, const double grid_spacing_in_pixel, + const bool plan_for_footprint, const Eigen::Matrix robot_to_fov_vector, bool show_path_evolution=false); +}; diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/common/include/ipa_room_exploration/neuron_class.h b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/common/include/ipa_room_exploration/neuron_class.h new file mode 100644 index 0000000..37c33ab --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/common/include/ipa_room_exploration/neuron_class.h @@ -0,0 +1,211 @@ +#include +#include +#include +#include +#include +#include + +#include + +/*! + ***************************************************************** + * \file + * + * \note + * Copyright (c) 2016 \n + * Fraunhofer Institute for Manufacturing Engineering + * and Automation (IPA) \n\n + * + ***************************************************************** + * + * \note + * Project name: Care-O-bot + * \note + * ROS stack name: autopnp + * \note + * ROS package name: ipa_room_exploration + * + * \author + * Author: Florian Jordan + * \author + * Supervised by: Richard Bormann + * + * \date Date of creation: 11.2016 + * + * \brief + * + * + ***************************************************************** + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * - Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. \n + * - Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. \n + * - Neither the name of the Fraunhofer Institute for Manufacturing + * Engineering and Automation (IPA) nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. \n + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License LGPL as + * published by the Free Software Foundation, either version 3 of the + * License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License LGPL for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License LGPL along with this program. + * If not, see . + * + ****************************************************************/ + +#pragma once + +// This class provides a neuron for an artificial neural network. This network is used to compute a coverage path s.t. +// the whole environment is visited at least once. The used method is stated in: +// +// Yang, Simon X., and Chaomin Luo. "A neural network approach to complete coverage path planning." IEEE Transactions on Systems, Man, and Cybernetics, Part B (Cybernetics) 34.1 (2004): 718-724. +// +// In this algorithm the free space gets sampled into several neurons that span a neural network over the space. Each +// neuron then needs to be visited once to clean it. Each neuron has a state that is used later to determine the next +// neuron that needs to be visited. Going then trough the space over time produces a path that covers all neurons, see +// the stated paper for reference. +// This class additionally contains the position of the neuron, the parameters used to update the state of it and +// booleans to mark if this neuron has already been cleaned or not. The function I() provides an "external input" to the +// neuron, see the paper for reference. +// +class Neuron +{ +protected: + + // vector that stores the direct neighbors of this neuron + std::vector neighbors_; + + // vector that stores the weights to the neighbors --> used for updating the state + std::vector weights_; + + // position of the neuron + cv::Point position_; + + // booleans to check if this neuron is cleaned or an obstacle + bool visited_, obstacle_; + + // state (activity) of this neuron at current time step and last + double state_, previous_state_; + + // parameters used to update the state + double A_, B_, D_, E_, mu_; + + // step size for updating the state + double step_size_; + + // function to generate the external input + double I() + { + if(obstacle_ == true) + return -1.0*E_; + else if(visited_ == false) + return E_; + else + return 0.0; + } + +public: + + // constructor + Neuron(cv::Point position, double A, double B, double D, double E, double mu, double step_size, bool obstacle, bool visited=false) + { + state_ = 0; + previous_state_ = 0; + position_ = position; + A_ = A; + B_ = B; + D_ = D; + E_ = E; + mu_ = mu; + step_size_ = step_size; + obstacle_ = obstacle; + visited_ = visited; + } + + // function to insert a neighbor + void addNeighbor(Neuron* new_neighbor) + { + // save pointer to neighbor + neighbors_.push_back(new_neighbor); + + // calculate distance and corresponding weight to it + cv::Point difference = position_ - new_neighbor->position_; + double distance = cv::norm(difference); + weights_.push_back(mu_/distance); + } + + // function to get the position of the neuron + cv::Point getPosition() + { + return position_; + } + + // function to get the state of the neuron, return the previous state if wanted + double getState(bool previous=false) + { + if(previous == true) + return previous_state_; + return state_; + } + + // function to save the current state as previous state + void saveState() + { + previous_state_ = state_; + } + + // function to get the neighbors + void getNeighbors(std::vector& neighbors) + { + neighbors = neighbors_; + } + + // function to mark the neuron as cleaned + void markAsVisited() + { + visited_ = true; + } + + // function to check if the current neuron is an obstacle or not + bool isObstacle() + { + return obstacle_; + } + + // function to check if the neuron has been visited or not + bool visitedNeuron() + { + return visited_; + } + + // function to update the state of the neuron using euler discretization + void updateState() + { + // get external input + const double input = I(); + + // get the current sum of weights times the state of the neighbor + double weight_sum = 0; + for(size_t neighbor=0; neighborgetState(true), 0.0); + + // calculate current gradient --> see stated paper from the beginning + double gradient = -A_*state_ + (B_-state_)*(std::max(input, 0.0) + weight_sum) - (D_+state_)*std::max(-1.0*input, 0.0); + + // update state using euler method + state_ += step_size_*gradient; + } +}; diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/common/include/ipa_room_exploration/room_rotator.h b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/common/include/ipa_room_exploration/room_rotator.h new file mode 100644 index 0000000..5a56e76 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/common/include/ipa_room_exploration/room_rotator.h @@ -0,0 +1,104 @@ +/*! + ***************************************************************** + * \file + * + * \note + * Copyright (c) 2017 \n + * Fraunhofer Institute for Manufacturing Engineering + * and Automation (IPA) \n\n + * + ***************************************************************** + * + * \note + * Project name: Care-O-bot + * \note + * ROS stack name: autopnp + * \note + * ROS package name: ipa_room_exploration + * + * \author + * Author: Richard Bormann + * \author + * Supervised by: Richard Bormann + * + * \date Date of creation: 02.2017 + * + * \brief + * + * + ***************************************************************** + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * - Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. \n + * - Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. \n + * - Neither the name of the Fraunhofer Institute for Manufacturing + * Engineering and Automation (IPA) nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. \n + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License LGPL as + * published by the Free Software Foundation, either version 3 of the + * License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License LGPL for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License LGPL along with this program. + * If not, see . + * + ****************************************************************/ + +#pragma once + +#include + +#include +#include + +#include + +#include + + +class RoomRotator +{ +public: + RoomRotator() + { + } + + void rotateRoom(const cv::Mat& room_map, cv::Mat& rotated_room_map, const cv::Mat& R, const cv::Rect& bounding_rect); + + // compute the affine rotation matrix for rotating a room into parallel alignment with x-axis (longer side of the room is aligned with x-axis) + // R is the transform + // bounding_rect is the ROI of the warped image + // rotation_offset is an optional offset to the determined rotation, in [rad] + // returns the computed optimal rotation angle, in [rad] + double computeRoomRotationMatrix(const cv::Mat& room_map, cv::Mat& R, cv::Rect& bounding_rect, const double map_resolution, + const cv::Point* center=0, const double rotation_offset=0.); + + // computes the major direction of the walls from a map (preferably one room) + // the map (room_map, CV_8UC1) is black (0) at impassable areas and white (255) on drivable areas + double computeRoomMainDirection(const cv::Mat& room_map, const double map_resolution); + + // transforms a vector of points back to the original map and generates poses + void transformPathBackToOriginalRotation(const std::vector& fov_middlepoint_path, std::vector& path_fov_poses, const cv::Mat& R); + + // converts a point path to a pose path with angles + void transformPointPathToPosePath(const std::vector& point_path, std::vector& pose_path); + + // converts a point path to a pose path with angles, the points are already stored in pose_path + void transformPointPathToPosePath(std::vector& pose_path); + + // get min/max coordinates of free pixels (255) + void getMinMaxCoordinates(const cv::Mat& map, cv::Point& min_room, cv::Point& max_room); +}; diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/common/include/ipa_room_exploration/timer.h b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/common/include/ipa_room_exploration/timer.h new file mode 100644 index 0000000..552d5f5 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/common/include/ipa_room_exploration/timer.h @@ -0,0 +1,183 @@ +/* + * timer.h + * + * Created on: May 13, 2013 + * Author: rmb-ce + */ + +// from: http://snipplr.com/view/40650/timer-class-for-both-unixlinuxmac-and-windows-system/ + +////////////////// +// How to Use //// +////////////////// + +//#include +//#include "timer.h" +//using namespace std; + +//int main() +//{ +// Timer timer; +// +// // start timer +// timer.start(); +// +// // do something +// ... +// +// // stop timer +// timer.stop(); +// +// // print the elapsed time in millisec +// cout << timer.getElapsedTimeInMilliSec() << " ms.\n"; +// +// return 0; +//} + + +////////////////////////////////////////////////////////////////////////////// +// Timer.h +// ======= +// High Resolution Timer. +// This timer is able to measure the elapsed time with 1 micro-second accuracy +// in both Windows, Linux and Unix system +// +// AUTHOR: Song Ho Ahn (song.ahn@gmail.com) +// CREATED: 2003-01-13 +// UPDATED: 2006-01-13 +// +// Copyright (c) 2003 Song Ho Ahn +////////////////////////////////////////////////////////////////////////////// + +#ifndef TIMER_H_DEF +#define TIMER_H_DEF + +#ifdef WIN32 // Windows system specific +//#include +#else // Unix based system specific +#include +#endif + +#include + + +class Timer +{ +public: +// default constructor + Timer() + { +#ifdef WIN32 + QueryPerformanceFrequency(&frequency); + startCount.QuadPart = 0; + endCount.QuadPart = 0; +#else + startCount.tv_sec = startCount.tv_usec = 0; + endCount.tv_sec = endCount.tv_usec = 0; +#endif + + stopped = 0; + startTimeInMicroSec = 0; + endTimeInMicroSec = 0; + + start(); + } + + // default destructor + ~Timer() + { + } + + /////////////////////////////////////////////////////////////////////////////// + // start timer. + // startCount will be set at this point. + /////////////////////////////////////////////////////////////////////////////// + void start() + { + stopped = 0; // reset stop flag +#ifdef WIN32 + QueryPerformanceCounter(&startCount); +#else + gettimeofday(&startCount, NULL); +#endif + } + + /////////////////////////////////////////////////////////////////////////////// + // stop the timer. + // endCount will be set at this point. + /////////////////////////////////////////////////////////////////////////////// + void stop() + { + stopped = 1; // set timer stopped flag + +#ifdef WIN32 + QueryPerformanceCounter(&endCount); +#else + gettimeofday(&endCount, NULL); +#endif + } + + /////////////////////////////////////////////////////////////////////////////// + // same as getElapsedTimeInSec() + /////////////////////////////////////////////////////////////////////////////// + double getElapsedTime() + { + return this->getElapsedTimeInSec(); + } + + /////////////////////////////////////////////////////////////////////////////// + // divide elapsedTimeInMicroSec by 1000000 + /////////////////////////////////////////////////////////////////////////////// + double getElapsedTimeInSec() + { + return this->getElapsedTimeInMicroSec() * 0.000001; + } + + /////////////////////////////////////////////////////////////////////////////// + // divide elapsedTimeInMicroSec by 1000 + /////////////////////////////////////////////////////////////////////////////// + double getElapsedTimeInMilliSec() + { + return this->getElapsedTimeInMicroSec() * 0.001; + } + + /////////////////////////////////////////////////////////////////////////////// + // compute elapsed time in micro-second resolution. + // other getElapsedTime will call this first, then convert to correspond resolution. + /////////////////////////////////////////////////////////////////////////////// + double getElapsedTimeInMicroSec() + { +#ifdef WIN32 + if(!stopped) + QueryPerformanceCounter(&endCount); + + startTimeInMicroSec = startCount.QuadPart * (1000000.0 / frequency.QuadPart); + endTimeInMicroSec = endCount.QuadPart * (1000000.0 / frequency.QuadPart); +#else + if (!stopped) + gettimeofday(&endCount, NULL); + + startTimeInMicroSec = (startCount.tv_sec * 1000000.0) + startCount.tv_usec; + endTimeInMicroSec = (endCount.tv_sec * 1000000.0) + endCount.tv_usec; +#endif + + return endTimeInMicroSec - startTimeInMicroSec; + } + +protected: + +private: + double startTimeInMicroSec; // starting time in micro-second + double endTimeInMicroSec; // ending time in micro-second + int stopped; // stop flag +#ifdef WIN32 + LARGE_INTEGER frequency; // ticks per second + LARGE_INTEGER startCount;// + LARGE_INTEGER endCount;// +#else + timeval startCount; // + timeval endCount; // +#endif +}; + +#endif // TIMER_H_DEF diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/common/include/ipa_room_exploration/voronoi.hpp b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/common/include/ipa_room_exploration/voronoi.hpp new file mode 100644 index 0000000..d7dfcb3 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/common/include/ipa_room_exploration/voronoi.hpp @@ -0,0 +1,1273 @@ +/*! + ***************************************************************** + * \file + * + * \note + * Copyright (c) 2016 \n + * Fraunhofer Institute for Manufacturing Engineering + * and Automation (IPA) \n\n + * + ***************************************************************** + * + * \author + * Author: Joshua Hampp + * + * \date Date of creation: 2016 + * + * \brief + * classes for grid map generation in Voronoi manner + * + ****************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include + + +#include + +#include + + +//debug includes +#include + +#define APPROX_COVERED 3 + +//* Helper class containing 2d vector +struct Pos { + int x_; ///< x position in pixels + int y_; ///< y position in pixels + + /*! + * \brief setter constructor + */ + Pos(const int x, const int y) : x_(x), y_(y) + {} + + inline bool operator==(const Pos &o) const { + return x_==o.x_ && y_==o.y_; + } + + /*! + * \brief add other position to this one + */ + inline void operator+=(const Pos &h) { + x_ += h.x_; + y_ += h.y_; + } + + /*! + * \brief subtracts other position from this one + */ + inline void operator-=(const Pos &h) { + x_ -= h.x_; + y_ -= h.y_; + } + + /*! + * \brief returns sum of two positions + */ + inline Pos operator+(const Pos &h) const { + Pos c=*this; + c+=h; + return c; + } + + /*! + * \brief returns difference of two positions + */ + inline Pos operator-(const Pos &h) const { + Pos c=*this; + c-=h; + return c; + } + + /*! + * \brief squared Euclidean distance to another postition + */ + inline int dist2(const Pos &h) const { + return (x_-h.x_)*(x_-h.x_) + (y_-h.y_)*(y_-h.y_); + } + + /*! + * \brief squared Euclidean distance of this position + */ + inline int dist2() const { + return x_*x_ + y_*y_; + } + +}; + +std::ostream &operator<<(std::ostream &os, const Pos &p) { + return os< Ptr; + + /*! + * \brief default constructor + */ + Cell() : pos_(0,0), hops_(0), sx_(0), sy_(0), id_(-1), id2_(-1) + {} + /*! + * \brief constructor for defined position in grid map + */ + Cell(const int x, const int y) : pos_(x,y), hops_(1), sx_(0), sy_(0), id_(-1), id2_(-1) + {} + + /*! + * \brief traverse to next cell by relative position increment + */ + inline void operator+=(const Pos &h) { + hops_ += 1; + sx_ += h.x_; + sy_ += h.y_; + } + + /*! + * \brief create a traversed cell by relative position increment + */ + inline Cell operator+(const Pos &h) const { + Cell c=*this; + c+=h; + return c; + } + + /*! + * \brief set relative movement and hops from another cell to this one + */ + inline void set(const Cell &h) { + hops_ = h.hops_; + sx_ = h.sx_; + sy_ = h.sy_; + } + + /*! + * \brief increases position, relative movement and hops from another cell to this one + */ + inline void operator+=(const Cell &h) { + hops_ += h.hops_; + pos_ += h.pos_; + assert(sx_*h.sx_>=0); + assert(sy_*h.sy_>=0); + sx_ += h.sx_; + sy_ += h.sy_; + } + + /*! + * \brief returns summed up position, relative movement and hops from another cell and this one (rest of parameters are copied from this) + */ + inline Cell operator+(const Cell &h) const { + Cell c=*this; + c+=h; + return c; + } + + /*! + * \brief squared Euclidean distance of relative movement + */ + inline int dist2() const { + return sx_*sx_ + sy_*sy_; + } + + /*! + * \brief compares the Euclidean distances of relative movement + */ + inline bool operator<(const Cell &o) const { return dist2() 0) + */ + inline operator bool() const { return hops_>0; } + +}; + +//* Helper class for grid maps storing relevant meta information (Cell) +struct CellMap { + std::vector cells_; ///< ordered map of cells containing meta information + int w_; ///< width in pixels + int h_; ///< height in pixels + + /*! + * \brief constructor for creating a empty grid map of width*height + */ + CellMap(const int w, const int h) : cells_(w*h), w_(w), h_(h) + { + for(int x=0; x=0 && p.x_=0 && p.y_=0 && x=0 && y=0 && x=0 && y > +struct pless : public std::binary_function { + bool operator()(const Type *x, const Type *y) const + { + if(x->dist2()==y->dist2()) { + if(x->pos_.x_==y->pos_.x_) + return x->pos_.y_>y->pos_.y_; + return x->pos_.x_>y->pos_.x_; + } + return x->dist2()>y->dist2(); + } +}; + +//* Helper class for finding nearest neighbours +template +struct PointCloud +{ + struct Point + { + T x,y; + + Point() {} + Point(const T &x, const T &y):x(x),y(y) {} + }; + + std::vector pts; + + // Must return the number of data points + inline size_t kdtree_get_point_count() const { return pts.size(); } + + // Returns the distance between the vector "p1[0:size-1]" and the data point with index "idx_p2" stored in the class: + inline T kdtree_distance(const T *p1, const size_t idx_p2,size_t /*size*/) const + { + const T d0=p1[0]-pts[idx_p2].x; + const T d1=p1[1]-pts[idx_p2].y; + return d0*d0+d1*d1; + } + + // Returns the dim'th component of the idx'th point in the class: + // Since this is inlined and the "dim" argument is typically an immediate value, the + // "if/else's" are actually solved at compile time. + inline T kdtree_get_pt(const size_t idx, int dim) const + { + if (dim==0) return pts[idx].x; + else return pts[idx].y; + } + + // Optional bounding-box computation: return false to default to a standard bbox computation loop. + // Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again. + // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds) + template + bool kdtree_get_bbox(BBOX& /*bb*/) const { return false; } + +}; + +struct LineSegment { + double length_; + double effective_length_; + std::vector inds_; + + inline bool operator<(const LineSegment &o) const { + return effective_length_>o.effective_length_; + } +}; + +template +struct TSPItem { + Pos in_, out_; + cv::Point2f in_dir_, out_dir_; + T *ref_; + + TSPItem() : in_(0,0), out_(0,0), ref_(NULL) {} + + TSPItem(const Pos &a, const Pos &b, const cv::Point2f &ad, const cv::Point2f &bd, T *ref): + in_(a), out_(b), in_dir_(ad), out_dir_(bd), ref_(ref) + {} + + TSPItem swap() const { + return TSPItem(out_, in_, out_dir_, in_dir_, ref_); + } +}; + +template +struct TSPTour { + Pos start_; + std::vector items_; + + TSPTour(const Pos &s) : start_(s) + {} + + inline size_t size() const {return 2*items_.size();} + + inline double costs() const { + //TODO: include direction change as costs + // depending on: + // * translation speed + // * rotation speed + + /* + (1-last_dir.dot(dir)) + */ + const double relation = (M_PI/0.2) * (0.05/0.3); //assumption: rotation_speed=0.2rad/s, translation_speed=0.3m/s, resolution=0.05m + + double c=0; + Pos last = start_; + cv::Point2f last_dir(0,0); + for(size_t i=0; i10) { //we replace last_dir + Pos t = items_[i].in_-last; + cv::Point2f interm(t.x_,t.y_); + interm*= 1./dist; + + rotation = (1+interm.dot(items_[i].in_dir_)); + rotation+= (1-last_dir.dot(interm)); + } + else { + rotation = (1+last_dir.dot(items_[i].in_dir_)); + } + + c += dist + + rotation*relation; + last = items_[i].out_; + last_dir = items_[i].out_dir_; + } + return c; + } + + T get(const size_t i) const { + assert(i/2 +struct TSPalgorithm { + TSPTour tour_; + + TSPalgorithm(const Pos &p) : tour_(p) + {} + + // Do all 2-opt combinations + void optimize() + { + // repeat until no improvement is made + bool improved = true; + time_t start = time(NULL); + + while ( improved ) + { + improved=false; + double best_distance = tour_.costs(); + + for ( int i = 0; i < tour_.size() - 1; i+=2 ) + { + for ( int k = i + 1; k < tour_.size(); k++) + { + TSPTour new_tour(tour_.start_); + swap2opt(new_tour, i, k ); + + const double new_distance = new_tour.costs(); + + if ( new_distance < best_distance ) + { + // Improvement found so reset + improved = true; + tour_ = new_tour; + best_distance = new_distance; + + //std::cout<<"new costs "<15) { + std::cout<<"TIMEOUT!"< &new_tour, const int& i, const int& k ) + { + // 1. take route[0] to route[i-1] and add them in order to new_route + int c = 0; + for (; c+1 <= i - 1; c+=2 ) + { + new_tour.set( c, tour_.get( c ) ); + } + + // 2. take route[i] to route[k] and add them in reverse order to new_route + int dec = 0;//(i%2==1?2:0); + for (; c<=k; c+=2 ) + { + //std::cout< +void connect(const Points &pts, std::vector &out, const cv::Mat &coverage, const Pos &start, const int approx_cover_dist, const int min_segment_length=2, const int max_segment_length=5) +{ + if(pts.size()==0) return; + else if(pts.size()==1) { + out.push_back(0); + out.push_back(0); + return; + } + + std::vector used(pts.size(), 0); + + int current=0, num=0, last=0; + + //costs for already used/visited point + const int USED_INC = 20; + + if(coverage.cols*coverage.rows>0) { + for(size_t i=0; i(pts[i].y_, pts[i].x_)) ) + ++num; + } + } + + /////////////////////////////////// + + PointCloud cloud; + for(size_t i=0; i::Point(pts[i].x_, pts[i].y_)); + + // construct a kd-tree index: + typedef nanoflann::KDTreeSingleIndexAdaptor< + nanoflann::L2_Simple_Adaptor > , + PointCloud, + 2 /* dim */ + > my_kd_tree_t; + + my_kd_tree_t index(2 /*dim*/, cloud, nanoflann::KDTreeSingleIndexAdaptorParams(10 /* max leaf */) ); + index.buildIndex(); + + int query_pt[2]; + + //1. build line segments + std::vector segs; + //use first unused pt + for(size_t j=0; j0) continue; + + LineSegment seg; + seg.length_ = 0; + + size_t nexts[4]={j}; + double last_lenghts[10]; + int num = 1; + int nnn=0; + while(num>0) { + --num; + size_t i = nexts[num]; + used[i] += USED_INC; + + //if(nnn%2==0) //only every 2nd point (to avoid odd patterns) + { + if(seg.inds_.size()>0) + seg.length_ += std::sqrt(pts[seg.inds_.back()].dist2(pts[i])); + last_lenghts[seg.inds_.size()%10] = seg.length_; + seg.inds_.push_back(i); + } + ++nnn; + + /*if(seg.length_>=max_segment_length) { + seg.effective_length_ = seg.length_; + segs.push_back(seg); + + seg = LineSegment(); + seg.length_ = 0; + }*/ + + //search for neighbouring pts + query_pt[0] = pts[nexts[num]].x_; + query_pt[1] = pts[nexts[num]].y_; + + std::vector > ret_matches; + nanoflann::SearchParams params; + params.sorted = true; + + const size_t nMatches = index.radiusSearch(&query_pt[0], 5, ret_matches, params); + //const size_t nMatches = index.knnSearch(&query_pt[0], 3 /*direct neighbours*/, ret_matches, params); + //std::cout<<"nMatches "<=0;j--) { + if(used[ret_matches[j].first]>0) continue; + + if(num>=4) { //finish segment + //num = 1; + break; + } + + if(/*seg.inds_.size()==0 &&*/ num>0) { // only in one direction + used[ret_matches[j].first] += USED_INC; + } + else { + nexts[num] = ret_matches[j].first; + ++num; + } + } + + /*if(num==1 && n==1) { //if we had before 2 pts (front/back) and now only one again + std::reverse(seg.inds_.begin(), seg.inds_.end()); + }*/ + if(seg.inds_.size()>10+min_segment_length) { + Pos p1 = pts[seg.inds_[seg.inds_.size()-1]]; + Pos p2 = pts[seg.inds_[seg.inds_.size()-6]]; + Pos p3 = pts[seg.inds_[seg.inds_.size()-10]]; + Pos d1 = p2-p1; + Pos d2 = p3-p2; + if(d1.x_*d2.x_ + d1.y_*d2.y_<10) { + std::vector inds(seg.inds_.begin()+seg.inds_.size()-6, seg.inds_.end()); + double l = seg.length_-last_lenghts[(seg.inds_.size()-6)%10]; + seg.length_ = last_lenghts[(seg.inds_.size()-6)%10]; + seg.effective_length_ = seg.length_; + seg.inds_.erase(seg.inds_.begin()+seg.inds_.size()-5, seg.inds_.end()); + + if(seg.inds_.size()>0 && seg.effective_length_>=min_segment_length) segs.push_back(seg); + + seg.inds_=inds; + seg.length_ = l; + } + } + else if(seg.length_>80) { + seg.effective_length_ = seg.length_; + if(seg.inds_.size()>0 && seg.effective_length_>=min_segment_length) segs.push_back(seg); + seg=LineSegment(); + seg.length_ = 0; + } + } + + seg.effective_length_ = seg.length_; + if(seg.inds_.size()>0 && seg.effective_length_>=min_segment_length) segs.push_back(seg); + } + + //3. do TSP for line segments + TSPalgorithm > opt(start); + LineSegment segs_start={}; + for(size_t i=0; i1) { + const size_t off = std::min((size_t)9, segs[i].inds_.size()-1); + dir_a = cv::Point2f(pts[segs[i].inds_.front()].x_-pts[segs[i].inds_[off]].x_, pts[segs[i].inds_.front()].y_-pts[segs[i].inds_[off]].y_); + dir_b = cv::Point2f(pts[segs[i].inds_.back()].x_-pts[segs[i].inds_[segs[i].inds_.size()-off-1]].x_, pts[segs[i].inds_.back()].y_-pts[segs[i].inds_[segs[i].inds_.size()-off-1]].y_); + dir_a*= 1./cv::norm(dir_a); + dir_b*= 1./cv::norm(dir_b); + + assert(!(dir_a!=dir_a)); + assert(!(dir_b!=dir_b)); + } + opt.tour_.set(2*i, TSPItem(pts[segs[i].inds_.front()], pts[segs[i].inds_.back()], dir_a, dir_b, &segs[i])); + } + + opt.optimize(); + + //4. create output + Pos ll(start); + for(size_t i=0; iinds_.back()]) + std::reverse(opt.tour_.items_[i].ref_->inds_.begin(), opt.tour_.items_[i].ref_->inds_.end()); + + ll = opt.tour_.items_[i].out_; + + for(size_t j=0; jinds_.size(); j++) + out.push_back(opt.tour_.items_[i].ref_->inds_[j]); + } +} + +//* Class for building a modified Voronoi graph for a given grid map (also computes waypoints and areas) +class VoronoiMap { + CellMap map_; //< internal gridmap + std::vector centers_; //< central points definining connected areas (points which results from different obstacles -> local maxima) + int max_track_width_; //< width of a track in pixels + int wall_offset_; //< distance to wall for path generation in pixels + bool single_room_; //< if true the map is handled as one complete room + + typedef std::priority_queue, pless > > T_WAVE; + + enum { + OCC=100, ///< value for occupied cell in ROS gridmap + FREE=0, ///< value for free cell in ROS gridmap + UNK=-1, ///< value for unknown cell in ROS gridmap + SET=3 ///< value for internal handled cell + }; + + /*! + * \brief access operator for occopuancy grid map with width/height at position + */ + inline int8_t &operator()(int8_t *occ, const int w, const int h, const Pos &c) { + return occ[c.y_*w+c.x_]; + } + + /*! + * \brief visit undhandled cells around given position and add them to our processing wave + */ + inline int add(int8_t *occ, const int w, const int h, const Pos &org, T_WAVE &wave) { + const int NUM=4; + static const Pos dirs[NUM] = {Pos(-1,0), Pos(0,-1), Pos(1,0), Pos(0,1)}; + + const Cell &cur = map_[org]; + + int n=0; + for(int d=0; dset(cur); + *c += dirs[d]; + wave.push(c); + (*this)(occ,w,h, p) = SET; + ++n; + } + } + + return n; + } + + /*! + * \brief visit handled cells which are further away as from new position around given position and add them to our processing wave + */ + inline int add2(int8_t *occ, const int w, const int h, const Pos &org, T_WAVE &wave) { + const int NUM=4; + static const Pos dirs[NUM] = {Pos(-1,0), Pos(0,-1), Pos(1,0), Pos(0,1)}; + + const Cell &cur = map_[org]; + + int n=0; + for(int d=0; dt.dist2()) + { + Cell *c = &map_[p]; + c->set(cur); + *c += dirs[d]; + wave.push(c); + ++n; + } + } + + return n; + } + +public: + + bool getSingleRoom() const {return single_room_;} + void setSingleRoom(const bool b) {single_room_=b;} + + /*! + * \brief constructor for creating simple version of modified Voronoi map (without room segmenation...) for wall tracking purposes (potential field -> specified distance to wall) + */ + VoronoiMap(const int max_track_width, int8_t *occ, const int w, const int h) : map_(w,h), max_track_width_(max_track_width), wall_offset_(0), single_room_(false) { + T_WAVE wave; + + for(int x=0; x0) { + int added = add(occ,w,h, wave.top()->pos_, wave); + + if(added==0 && wave.top()->hops_>0 && wave.top()->dist2()>4) + centers_.push_back(wave.top()); + + wave.pop(); + } + } + + /*! + * \brief constructor for creating "complete" modified Voronoi map (with room segmenation...) for maeander calculation (does not generate path by itself) + */ + VoronoiMap(int8_t *occ, const int w, const int h, const int max_track_width, const int merge_tracks=2, const bool single_room=false) : map_(w,h), max_track_width_(max_track_width), wall_offset_(0), single_room_(single_room) { + T_WAVE wave; + + //std::cout<<"single_room "<0) { + int added = add(occ,w,h, wave.top()->pos_, wave); + + if(added==0 && wave.top()->hops_>0 && wave.top()->dist2()>=max_track_width_*max_track_width_*2) { + centers_.push_back(wave.top()); + if(single_room_) centers_.back()->id_=1; + } + + wave.pop(); + } + + if(single_room) return; + + const int NUM=9; + //static const Pos dirs[NUM] = {Pos(-1,0), Pos(0,-1), Pos(1,0), Pos(0,1), Pos(0,0)}; + static const Pos dirs[NUM] = {Pos(-1,-1), Pos(0,-1), Pos(1,-1), Pos(-1,0), Pos(1,0), Pos(-1,1), Pos(0,1), Pos(1,1), Pos(0,0)}; + + PointCloud cloud; + for(size_t i=0; i::Point(centers_[i]->pos_.x_, centers_[i]->pos_.y_)); + + // construct a kd-tree index: + typedef nanoflann::KDTreeSingleIndexAdaptor< + nanoflann::L2_Simple_Adaptor > , + PointCloud, + 2 /* dim */ + > my_kd_tree_t; + + my_kd_tree_t index(2 /*dim*/, cloud, nanoflann::KDTreeSingleIndexAdaptorParams(10 /* max leaf */) ); + index.buildIndex(); + + int query_pt[2]; + std::vector > ws_old; + ws_old.resize(centers_.size()); + + //start from central points with highest distance to next obstacle + for(int i=centers_.size()-1; i>=0; --i) { + + //if already processed skip + if(centers_[i]->id_>=0) continue; + + //start with this point + std::vector ws; + ws.push_back(centers_[i]); + + //iterate over all connected, unprocessed center points + //for clustering + for(size_t j=0; jid_>=0) continue; + + //distance to obstacle + const int last_dist = std::sqrt(ws[j]->dist2()); + //track no. + const int id = last_dist/(merge_tracks*max_track_width_); + + ws[j]->id_ = i+1; //generate id for area + + //look now in direct neighbourhood for other center points + query_pt[0] = ws[j]->pos_.x_; + query_pt[1] = ws[j]->pos_.y_; + + std::vector > ret_matches; + nanoflann::SearchParams params; + params.sorted = true; + + //const size_t nMatches = index.radiusSearch(&query_pt[0], 2*last_dist+2*max_track_width_, ret_matches, params); + const size_t nMatches = index.radiusSearch(&query_pt[0], std::pow(last_dist+max_track_width_,2), ret_matches, params); + + int mid = id; + for (size_t ii=0;iiid_>=0) continue; //already processed + + const int dist = std::sqrt(c->dist2()); + const int _id = dist/(merge_tracks*max_track_width_); + + //point would be within track no. --> clustering + //so the clustered center points get the same id + if(_id<=mid) + ws.push_back(c); + else break; + } + } + } + + if(centers_.size()<1) return; + + //assign id to complete area around center point + const int factor=merge_tracks; + + //biggest distance to obstacle + const int first_dist2 = centers_[centers_.size()-1]->dist2(); + const int first_id = std::sqrt(first_dist2)/(factor*max_track_width_); + + std::vector > wc; + wc.resize(centers_.size()); + //seed points + for(int i=0; i=0; --i) { + //distance to obstacle + const int last_dist = std::sqrt(centers_[i]->dist2()); + //calc. track no. + int id = last_dist/(factor*max_track_width_); + + //only process areas with at least this track no. + if(idid_; //get area id + if(id<0) continue; //not processed by step before + + //do one loop through stack (initial one center point) + std::vector w2; + for(size_t j=0; jid2_<<" "<id_<id2_==99) { + + //if differnt area ids contract, + //we mark this as border between two areas + if(wc[i][j]->id_ != id || map_[wc[i][j]->pos_].dist2()<=0) { + wc[i][j]->hops_=0; + wc[i][j]->sx_=0; + wc[i][j]->sy_=0; + wave.push(wc[i][j]); //remember border + } + //otherwise it's the same area, we can skip + + continue; + } + + wc[i][j]->id_ = id; //assign area id + wc[i][j]->id2_ = 99; //mark as visited + + //add surrounding points for next loop + for(int d=0; dpos_+dirs[d]; + //if not an obstacle + if(map_.valid(p)) + //if(map_[p].dist2()>0) + w2.push_back(&map_[p]); + } + } + + //std::cout<<"SSS44 "<0) { + int added = add2(occ,w,h, wave.top()->pos_, wave); + //std::cout<dist2()<=map_.w_-1 || y>=map_.h_-1) + return -1; + + if(!map_(x,y).sx_ && !map_(x,y).sy_) + return 0; //obstacle + + const double v = std::sqrt(map_(x,y).dist2())-max_track_width_/2.; + if(v=map_.w_-1 || y>=map_.h_-1) + return -1; + + if(!map_(x,y).sx_ && !map_(x,y).sy_) + return 0; //obstacle + + if(map_(x,y).hops_<=0) return 0; + + const float V = std::abs((float)fmod(std::sqrt((float)map_(x,y).dist2())-(float)wall_offset_, (float)max_track_width_)-(float)max_track_width_/2.f); + if(V<0.45f) return 2222; + + const int xx = roundf(float(map_(x,y).sx_) / std::max(std::abs(map_(x,y).sx_), std::abs(map_(x,y).sy_)) ); + const int yy = roundf(float(map_(x,y).sy_) / std::max(std::abs(map_(x,y).sx_), std::abs(map_(x,y).sy_)) ); + + const int xx2 = roundf(float(map_(x,y).sx_)*2 / std::max(std::abs(map_(x,y).sx_), std::abs(map_(x,y).sy_)) ); + const int yy2 = roundf(float(map_(x,y).sy_)*2 / std::max(std::abs(map_(x,y).sx_), std::abs(map_(x,y).sy_)) ); + + const float v[5]={ + std::abs((float)fmod(std::sqrt((float)map_(x+xx,y+yy).dist2())-(float)wall_offset_, (float)max_track_width_)-(float)max_track_width_/2.f), + V, + std::abs((float)fmod(std::sqrt((float)map_(x-xx,y-yy).dist2())-(float)wall_offset_, (float)max_track_width_)-(float)max_track_width_/2.f), + + std::abs((float)fmod(std::sqrt((float)map_(x-xx2,y-yy2).dist2())-(float)wall_offset_, (float)max_track_width_)-(float)max_track_width_/2.f), + std::abs((float)fmod(std::sqrt((float)map_(x+xx2,y+yy2).dist2())-(float)wall_offset_, (float)max_track_width_)-(float)max_track_width_/2.f) + }; + + //if(v[1]map_(x,y).hops_) ++num; + + return (num==0 && map_(x,y).dist2()*12>=max_track_width_*max_track_width_)?2222:(map_(x,y).hops_>0?1000:0); + } + + /*! + * \brief generate maeander using start position and coverage, simplifies path by minimum distance between points (in pixels) and deviation (distance) from a thought line (in pixel) + */ + template + void generatePath(PathVector &path, const cv::Mat &coverage, const int start_x, const int start_y, const int min_dist=40, const float dir_dev=0.1f) { + std::vector pts; + size_t num=0; + + const int cid = map_(start_x,start_y).id_; + + for(int x=0; xstd::pow(wall_offset_,2) && (*this)(x,y) == 2222 && (single_room_||map_(x,y).id_==cid) ) { + pts.push_back(Pos(x,y)); + ++num; + } + } + } + + //sort pts in relation to distance to start + //-->circles wil be split near to start + std::sort(pts.begin(), pts.end(), CmpDist2Ref(Pos(start_x, start_y))); + + num = 0; + + std::vector out; + connect(pts, out, coverage, Pos(start_x, start_y), APPROX_COVERED); + + //helper structure for optimized removal + struct OptVal { + struct OptValCompare { + bool operator()(const typename std::list::iterator& lhs, const typename std::list::iterator& rhs) const { + if(lhs->val==rhs->val) return lhs->val_secval_sec; + return lhs->valval; + } + }; + + typedef std::multimap::iterator, char, OptValCompare> MM; + + double val; //value defining costs if we remove the coresponding point from line + double val_sec; + int ind; //index to order list (out) (double indexed to pts) + typename MM::iterator mm; //remember iterator to sorted multimap list + }; + //function to update costs + auto ValueFunc = [pts, out, this](typename std::list::iterator cur) { + typename std::list::iterator b = cur; + typename std::list::iterator a = cur; + --b; + ++a; + + auto dx1 = pts[out[b->ind]].x_-pts[out[cur->ind]].x_; + auto dy1 = pts[out[b->ind]].y_-pts[out[cur->ind]].y_; + auto dx2 = pts[out[a->ind]].x_-pts[out[cur->ind]].x_; + auto dy2 = pts[out[a->ind]].y_-pts[out[cur->ind]].y_; + auto dx3 = pts[out[a->ind]].x_-pts[out[b->ind]].x_; + auto dy3 = pts[out[a->ind]].y_-pts[out[b->ind]].y_; + + auto Dist = [](int dx, int dy) {return std::sqrt(std::pow(dx,2)+std::pow(dy,2));}; + auto l = Dist(dx1,dy1)+Dist(dx2,dy2); + /*if(l<=2*this->max_track_width_ && std::abs(dx1*dy2 - dx2*dy1)max_track_width_*this->max_track_width_/3) + cur->val=0; + else*/ + cur->val = (l-Dist(dx3,dy3))*Dist(dx3,dy3); + //cur->val = (10000*(Dist(dx1,dy1)+Dist(dx2,dy2))-4)/Dist(dx3,dy3)-10000; + //cur->val = std::abs(dx1*dy2 - dx2*dy1); + cur->val_sec = Dist(dx3,dy3); + }; + + std::list vals; //ordered list of line + for(size_t i=0; i::iterator it = ++vals.begin(); it!=--vals.end(); ++it) { + ValueFunc(it); + it->mm = vals_sorted.insert(std::pair::iterator, char>(it, 0)); + } + + const double THR=8;//std::pow(9,2); + while(!vals_sorted.empty()) { + typename std::list::iterator it = vals_sorted.begin()->first; + if(it->val>=THR) break; + + //remove top (keep in mind, first and last point won't be removed!) + typename std::list::iterator a=it, b=it; + ++a; --b; + + vals_sorted.erase(it->mm); + if(a!=--vals.end()) vals_sorted.erase(a->mm); + if(b!=vals.begin()) vals_sorted.erase(b->mm); + vals.erase(it); + + //update values + if(a!=--vals.end()) { + ValueFunc(a); + a->mm = vals_sorted.insert(std::pair::iterator, char>(a, 0)); + } + if(b!=vals.begin()) { + ValueFunc(b); + b->mm = vals_sorted.insert(std::pair::iterator, char>(b, 0)); + } + } + + path.resize(vals.size()); + size_t i=0; + for(typename std::list::iterator it = vals.begin(); it!=vals.end(); ++it,++i) { + path[i].x=pts[out[it->ind]].x_; + path[i].y=pts[out[it->ind]].y_; + } + + } + + /*! + * \brief gernates a direct path (2 points) to next uncovered area (e.g. next room to clean; shortest path) + */ + template + bool nextArea(PathVector &path, const cv::Mat &coverage, const int start_x, const int start_y) { + path.resize(2); + path[0].x = start_x; + path[0].y = start_y; + + const int NUM=4; + static const Pos dirs[NUM] = {Pos(-1,0), Pos(0,-1), Pos(1,0), Pos(0,1)}; + + const int cid = map_(start_x,start_y).id_; + Pos p(start_x, start_y); + + std::list stack; + stack.push_back(p); + + while(stack.size()>0) { + p = stack.front(); + stack.pop_front(); + + if( map_(p.x_,p.y_).id_!=cid && (*this)(p.x_,p.y_) == 2222 && + (p.x_>=coverage.cols || p.y_>=coverage.rows || !coverage.at(p.y_, p.x_) ) ) { + path[1].x = p.x_; + path[1].y = p.y_; + + return true; + } + + int n=0; + for(int d=0; d=0 && map_(n.x_,n.y_).id2_!=333 ) { + map_(n.x_,n.y_).id2_=333; + stack.push_back(n); + } + } + } + + return false; + } + + /*! + * \brief generates a direct path (2 points) to next border (e.g. wall) which was noch covered yet (allows to search in global map or only in current area; shortest path) + */ + template + bool nextBorderPoint(PathVector &path, const cv::Mat &coverage, const int start_x, const int start_y, const bool stay_inside=false) { + const int NUM=4; + static const Pos dirs[NUM] = {Pos(-1,0), Pos(0,-1), Pos(1,0), Pos(0,1)}; + + Pos p(start_x, start_y); + + std::list stack; + stack.push_back(p); + + path.resize(2); + path[0].x = start_x; + path[0].y = start_y; + + while(stack.size()>0) { + p = stack.front(); + stack.pop_front(); + + int d = map_(p.x_,p.y_).dist2(); + + if( 4*d>=std::pow(3*max_track_width_-1,2) && 4*d<=std::pow(3*max_track_width_+1,2) && + (p.x_<0 || p.y_<0 || p.x_>=coverage.cols || p.y_>=coverage.rows || !coverage.at(p.y_, p.x_) ) ) { + path[1].x = p.x_; + path[1].y = p.y_; + return true; + } + + int n=0; + for(int d=0; d=0 && map_(n.x_,n.y_).id2_!=444 && + (!stay_inside || map_(n.x_,n.y_).hops_>0) //TODO: check this statement + ) { + map_(n.x_,n.y_).id2_=444; + stack.push_back(n); + } + } + } + + path.clear(); + return false; + } + +}; + diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/common/src/boustrophedon_explorator.cpp b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/common/src/boustrophedon_explorator.cpp new file mode 100644 index 0000000..14232dd --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/common/src/boustrophedon_explorator.cpp @@ -0,0 +1,1160 @@ +#include + +//#define DEBUG_VISUALIZATION + +// Constructor +BoustrophedonExplorer::BoustrophedonExplorer() +{ + +} + +// Function that creates a room exploration path for the given map, by using the morse cellular decomposition method proposed in +// +// "H. Choset, E. Acar, A. A. Rizzi and J. Luntz, +// "Exact cellular decompositions in terms of critical points of Morse functions," Robotics and Automation, 2000. Proceedings. +// ICRA '00. IEEE International Conference on, San Francisco, CA, 2000, pp. 2270-2277 vol.3." +// +// This method takes the given map and separates it into several cells. Each cell is obstacle free and so allows an +// easier path planning. For each cell then a boustrophedon path is planned, which goes up, down and parallel to the +// upper and lower boundaries of the cell, see the referenced paper for details. This function does the following steps: +// I. Using the Sobel operator the direction of the gradient at each pixel is computed. Using this information, the direction is +// found that suits best for calculating the cells, i.e. such that longer cells occur, and the map is rotated in this manner. +// This allows to use the algorithm as it was and in the last step, the found path points simply will be transformed back to the +// original orientation. +// II. Sweep a slice (a morse function) trough the given map and check for connectivity of this line, +// i.e. how many connected segments there are. If the connectivity increases, i.e. more segments appear, +// an IN event occurs that opens new separate cells, if it decreases, i.e. segments merge, an OUT event occurs that +// merges two cells together. If an event occurs, the algorithm checks along the current line for critical points, +// that are points that trigger the events. From these the boundary of the cells are drawn, starting from the CP +// and going left/right until a black pixel is hit. +// III. After all cells have been determined by the sweeping slice, the algorithm finds these by using cv::findContours(). +// This gives a set of points for each cell, that are used to create a generalizedPolygon out of each cell. +// IV. After all polygons have been created, plan the path trough all of them for the field of view s.t. the whole area +// is covered. To do so, first a global path trough all cells is generated, using the traveling salesmen problem +// formulation. This produces an optimal visiting order of the cells. Next for each cell a boustrophedon path is +// determined, which goes back and forth trough the cell and between the horizontal paths along the boundaries of +// the cell, what ensures that the whole area of the cell is covered. For each cell the longest edge is found and it is transformed +// s.t. this edge lies horizontal to the x-direction. This produces longer but fewer edges, what improves the path for small but long +// cells. The startpoint of the cell-path is determined by the endpoint of the previous cell, s.t. the distance between two +// cell-paths is minimized. The cell-path is determined in the rotated manner, so in a last step, the cell-path is transformed to +// the originally transformed cell and after that inserted in the global path. +// V. The previous step produces a path for the field of view. If wanted this path gets mapped to the robot path s.t. +// the field of view follows the wanted path. To do so simply a vector transformation is applied. If the computed robot +// pose is not in the free space, another accessible point is generated by finding it on the radius around the field of view (fov) +// middlepoint s.t. the distance to the last robot position is minimized. If this is not wanted one has to set the +// corresponding Boolean to false (shows that the path planning should be done for the robot footprint). +// room_map = expects to receive the original, not inflated room map +void BoustrophedonExplorer::getExplorationPath(const cv::Mat& room_map, std::vector& path, + const float map_resolution, const cv::Point starting_position, const cv::Point2d map_origin, + const double grid_spacing_in_pixel, const double grid_obstacle_offset, const double path_eps, const int cell_visiting_order, + const bool plan_for_footprint, const Eigen::Matrix robot_to_fov_vector, const double min_cell_area, const int max_deviation_from_track) +{ + ROS_INFO("Planning the boustrophedon path trough the room."); + const int grid_spacing_as_int = (int)std::floor(grid_spacing_in_pixel); // convert fov-radius to int + const int half_grid_spacing_as_int = (int)std::floor(0.5*grid_spacing_in_pixel); // convert fov-radius to int + const int min_cell_width = half_grid_spacing_as_int + 2.*grid_obstacle_offset/map_resolution; + + // *********************** I. Find the main directions of the map and rotate it in this manner. *********************** + // *********************** II. Sweep a slice trough the map and mark the found cell boundaries. *********************** + // *********************** III. Find the separated cells. *********************** + cv::Mat R; + cv::Rect bbox; + cv::Mat rotated_room_map; + std::vector cell_polygons; + std::vector polygon_centers; + computeCellDecompositionWithRotation(room_map, map_resolution, min_cell_area, min_cell_width, 0., R, bbox, rotated_room_map, cell_polygons, polygon_centers); + // does not work so well: findBestCellDecomposition(room_map, map_resolution, min_cell_area, R, bbox, rotated_room_map, cell_polygons, polygon_centers); + + ROS_INFO("Found the cells in the given map."); + + + // *********************** IV. Determine the cell paths. *********************** + // determine the start cell that contains the start position + std::vector starting_point_vector(1, starting_position); // opencv syntax + cv::transform(starting_point_vector, starting_point_vector, R); + const cv::Point rotated_starting_point = starting_point_vector[0]; // point that keeps track of the last point after the boustrophedon path in each cell + +// testing +// cv::Mat center_map = room_map.clone(); +// for(size_t i=0; i::iterator cell=cell_polygons.begin(); cell!=cell_polygons.end(); ++cell) + if(cv::pointPolygonTest(cell->getVertices(), rotated_starting_point, false) >= 0) + start_cell_index = cell - cell_polygons.begin(); + + // determine the visiting order of the cells + std::vector optimal_order; + if (cell_visiting_order == OPTIMAL_TSP) + { + // determine the optimal visiting order of the cells + // ConcordeTSPSolver tsp_solver; + // std::vector optimal_order = tsp_solver.solveConcordeTSP(rotated_room_map, polygon_centers, 0.25, 0.0, map_resolution, start_cell_index, 0); + GeneticTSPSolver tsp_solver; + optimal_order = tsp_solver.solveGeneticTSP(rotated_room_map, polygon_centers, 0.25, 0.0, map_resolution, start_cell_index, 0); + if (optimal_order.size()!=polygon_centers.size()) + { + std::cout << "=====================> Genetic TSP failed with 25% resolution, falling back to 100%. <=======================" << std::endl; + optimal_order = tsp_solver.solveGeneticTSP(rotated_room_map, polygon_centers, 1.0, 0.0, map_resolution, start_cell_index, 0); + } + } + else if (cell_visiting_order == LEFT_TO_RIGHT) + { + // we are using an alternative ordering here, which visits the cells in a more obvious fashion to the human observer (though it is not optimal) + std::multimap y_coordinate_ordering; // + for (size_t i=0; i(polygon_centers[i].y, (int)i)); + for (std::multimap::iterator it=y_coordinate_ordering.begin(); it!=y_coordinate_ordering.end(); ++it) + optimal_order.push_back(it->second); + } + else + { + std::cout << "Error: BoustrophedonExplorer::getExplorationPath: The specified cell_visiting_order=" << cell_visiting_order << " is invalid." << std::endl; + return; + } + + // go trough the cells [in optimal visiting order] and determine the boustrophedon paths + ROS_INFO("Starting to get the paths for each cell, number of cells: %d", (int)cell_polygons.size()); + std::cout << "Boustrophedon grid_spacing_as_int=" << grid_spacing_as_int << std::endl; + cv::Point robot_pos = rotated_starting_point; // point that keeps track of the last point after the boustrophedon path in each cell + std::vector fov_middlepoint_path; // this is the trajectory of centers of the robot footprint or the field of view + for(size_t cell=0; cell fov_poses; // this is the trajectory of poses of the robot footprint or the field of view, in [pixels] + room_rotation.transformPathBackToOriginalRotation(fov_middlepoint_path, fov_poses, R); +#ifdef DEBUG_VISUALIZATION + std::cout << "printing path" << std::endl; + cv::Mat room_map_path = room_map.clone(); + cv::circle(room_map_path, starting_position, 3, cv::Scalar(160), CV_FILLED); + for(size_t i=0; i::iterator pose=fov_poses.begin(); pose != fov_poses.end(); ++pose) + { + geometry_msgs::Pose2D current_pose; + current_pose.x = (pose->x * map_resolution) + map_origin.x; + current_pose.y = (pose->y * map_resolution) + map_origin.y; + current_pose.theta = pose->theta; + path.push_back(current_pose); + } + return; + } + + // *********************** V. Get the robot path out of the fov path. *********************** + // go trough all computed fov poses and compute the corresponding robot pose + ROS_INFO("Starting to map from field of view pose to robot pose"); + cv::Point robot_starting_position = (fov_poses.size()>0 ? cv::Point(cvRound(fov_poses[0].x), cvRound(fov_poses[0].y)) : starting_position); + cv::Mat inflated_room_map; + cv::erode(room_map, inflated_room_map, cv::Mat(), cv::Point(-1, -1), half_grid_spacing_as_int); + mapPath(inflated_room_map, path, fov_poses, robot_to_fov_vector, map_resolution, map_origin, robot_starting_position); + + +#ifdef DEBUG_VISUALIZATION + // testing + std::cout << "printing path" << std::endl; + cv::Mat fov_path_map = room_map.clone(); + if (path.size() > 0) + cv::circle(fov_path_map, cv::Point((path[0].x-map_origin.x)/map_resolution, (path[0].y-map_origin.y)/map_resolution), 2, cv::Scalar(150), CV_FILLED); + for(size_t i=1; i& cell_polygons, std::vector& polygon_centers) +{ + // *********************** I. Find the main directions of the map and rotate it in this manner. *********************** + // *********************** II. Sweep a slice trough the map and mark the found cell boundaries. *********************** + // *********************** III. Find the separated cells. *********************** + cv::Mat R_1, R_2; + cv::Rect bbox_1, bbox_2; + cv::Mat rotated_room_map_1, rotated_room_map_2; + std::vector cell_polygons_1, cell_polygons_2; + std::vector polygon_centers_1, polygon_centers_2; + computeCellDecompositionWithRotation(room_map, map_resolution, min_cell_area, min_cell_width, 0., R_1, bbox_1, rotated_room_map_1, cell_polygons_1, polygon_centers_1); + computeCellDecompositionWithRotation(room_map, map_resolution, min_cell_area, min_cell_width, 90./180.*CV_PI, R_2, bbox_2, rotated_room_map_2, cell_polygons_2, polygon_centers_2); + + // select the cell decomposition with good axis alignment which produces less cells + if (cell_polygons_1.size() <= cell_polygons_2.size()) + { + R = R_1; + bbox = bbox_1; + rotated_room_map = rotated_room_map_1; + cell_polygons = cell_polygons_1; + polygon_centers = polygon_centers_1; + } + else + { + R = R_2; + bbox = bbox_2; + rotated_room_map = rotated_room_map_2; + cell_polygons = cell_polygons_2; + polygon_centers = polygon_centers_2; + } +} + +void BoustrophedonExplorer::computeCellDecompositionWithRotation(const cv::Mat& room_map, const float map_resolution, const double min_cell_area, + const int min_cell_width, const double rotation_offset, cv::Mat& R, cv::Rect& bbox, cv::Mat& rotated_room_map, + std::vector& cell_polygons, std::vector& polygon_centers) +{ + // *********************** I. Find the main directions of the map and rotate it in this manner. *********************** + RoomRotator room_rotation; + room_rotation.computeRoomRotationMatrix(room_map, R, bbox, map_resolution, 0, rotation_offset); + room_rotation.rotateRoom(room_map, rotated_room_map, R, bbox); + +#ifdef DEBUG_VISUALIZATION +// // testing +// cv::Mat room_map_disp = room_map.clone(); +// cv::circle(room_map_disp, starting_position, 3, cv::Scalar(160), CV_FILLED); +// cv::imshow("room_map", room_map_disp); +// std::vector tester; +// tester.push_back(cv::Point(10,10)); +// cv::Mat tester_map = room_map.clone(); +// cv::circle(tester_map, tester[0], 3, cv::Scalar(127), CV_FILLED); +// cv::transform(tester, tester, R); +// cv::circle(rotated_room_map, tester[0], 3, cv::Scalar(127), CV_FILLED); +// cv::imshow("original", tester_map); +// cv::imshow("rotated_im.png", rotated_room_map); +// cv::waitKey(); +#endif + + // *********************** II. Sweep a slice trough the map and mark the found cell boundaries. *********************** + // *********************** III. Find the separated cells. *********************** + computeCellDecomposition(rotated_room_map, map_resolution, min_cell_area, min_cell_width, cell_polygons, polygon_centers); +} + +void BoustrophedonExplorer::computeCellDecomposition(const cv::Mat& room_map, const float map_resolution, const double min_cell_area, + const int min_cell_width, std::vector& cell_polygons, std::vector& polygon_centers) +{ + // *********************** II. Sweep a slice trough the map and mark the found cell boundaries. *********************** + // create a map copy to mark the cell boundaries + cv::Mat cell_map = room_map.clone(); + + // find smallest y-value for that a white pixel occurs, to set initial y value and find initial number of segments + size_t y_start = 0; + bool found = false, obstacle = false; + int previous_number_of_segments = 0; + std::vector previous_obstacles_end_x; // keep track of the end points of obstacles + for(size_t y=0; y(y,x) == 255) + { + y_start = y; + found = true; + } + else if(found == true && obstacle == false && room_map.at(y,x) == 0) + { + ++previous_number_of_segments; + obstacle = true; + } + else if(found == true && obstacle == true && room_map.at(y,x) == 255) + { + obstacle = false; + previous_obstacles_end_x.push_back(x); + } + } + + if(found == true) + break; + } + + // sweep trough the map and detect critical points + for(size_t y=y_start+1; y current_obstacles_start_x; + std::vector current_obstacles_end_x; + bool obstacle_hit = false; // bool to check if the line currently hit an obstacle, s.t. not all black pixels trigger an event + bool hit_white_pixel = false; // bool to check if a white pixel has been hit at the current slice, to start the slice at the first white pixel + + // count number of segments within this row + for(size_t x=0; x(y,x) == 255) + hit_white_pixel = true; + else if(hit_white_pixel == true) + { + if(obstacle_hit == false && room_map.at(y,x) == 0) // check for obstacle + { + ++number_of_segments; + obstacle_hit = true; + current_obstacles_start_x.push_back(x); + } + else if(obstacle_hit == true && room_map.at(y,x) == 255) // check for leaving obstacle + { + obstacle_hit = false; + current_obstacles_end_x.push_back(x); + } + } + } + + // if the number of segments did not change, check whether the position of segments has changed so that there is a gap between them + bool segment_shift_detected = false; + if (previous_number_of_segments == number_of_segments && current_obstacles_start_x.size() == previous_obstacles_end_x.size()+1) + { + for (size_t i=0; i previous_obstacles_end_x[i]) + { + segment_shift_detected = true; + break; + } + } + + // reset hit_white_pixel to use this Boolean later + hit_white_pixel = false; + + // check if number of segments has changed --> event occurred + if(previous_number_of_segments < number_of_segments || segment_shift_detected == true) // IN event (or shift) + { + // check the current slice again for critical points + for(int x=0; x(y,x) == 255) + hit_white_pixel = true; + else if(hit_white_pixel == true && room_map.at(y,x) == 0) + { + // check over black pixel for other black pixels, if none occur a critical point is found + bool critical_point = true; + for(int dx=-1; dx<=1; ++dx) + if(room_map.at(y-1,std::max(0,std::min(x+dx, room_map.cols-1))) == 0) + critical_point = false; + + // if a critical point is found mark the separation, note that this algorithm goes left and right + // starting at the critical point until an obstacle is hit, because this prevents unnecessary cells + // behind other obstacles on the same y-value as the critical point + if(critical_point == true) + { + // to the left until a black pixel is hit + for(int dx=-1; x+dx>=0; --dx) + { + uchar& val = cell_map.at(y,x+dx); + if(val == 255 && cell_map.at(y-1,x+dx) == 255) + val = BORDER_PIXEL_VALUE; + else if(val == 0) + break; + } + + // to the right until a black pixel is hit + for(int dx=1; x+dx(y,x+dx); + if(val == 255 && cell_map.at(y-1,x+dx) == 255) + val = BORDER_PIXEL_VALUE; + else if(val == 0) + break; + } + } + } + } + } + else if(previous_number_of_segments > number_of_segments) // OUT event + { + // check the previous slice again for critical points --> y-1 + for(int x=0; x(y-1,x) == 255 && hit_white_pixel == false) + hit_white_pixel = true; + else if(hit_white_pixel == true && room_map.at(y-1,x) == 0) + { + // check over black pixel for other black pixels, if none occur a critical point is found + bool critical_point = true; + for(int dx=-1; dx<=1; ++dx) + if(room_map.at(y,std::max(0,std::min(x+dx, room_map.cols-1))) == 0) // check at side after obstacle + critical_point = false; + + // if a critical point is found mark the separation, note that this algorithm goes left and right + // starting at the critical point until an obstacle is hit, because this prevents unnecessary cells + // behind other obstacles on the same y-value as the critical point + if(critical_point == true) + { + const int ym2 = std::max(0,(int)y-2); + + // to the left until a black pixel is hit + for(int dx=-1; x+dx>=0; --dx) + { + uchar& val = cell_map.at(y-1,x+dx); + if(val == 255 && cell_map.at(ym2,x+dx) == 255) + val = BORDER_PIXEL_VALUE; + else if(val == 0) + break; + } + + // to the right until a black pixel is hit + for(int dx=1; x+dx(y-1,x+dx); + if(val == 255 && cell_map.at(ym2,x+dx) == 255) + val = BORDER_PIXEL_VALUE; + else if(val == 0) + break; + } + } + } + } + } + + // save the found number of segments and the obstacle end points + previous_number_of_segments = number_of_segments; + previous_obstacles_end_x = current_obstacles_end_x; + } + +#ifdef DEBUG_VISUALIZATION + cv::imshow("cell_map", cell_map); +#endif + + // *********************** II.b) merge too small cells into bigger cells *********************** + cv::Mat cell_map_labels; + const int number_of_cells = mergeCells(cell_map, cell_map_labels, min_cell_area, min_cell_width); + + // *********************** III. Find the separated cells. *********************** + std::vector > cells; + for (int i=1; i<=number_of_cells; ++i) + { + cv::Mat cell_copy(cell_map_labels == i); + std::vector > cellsi; +#if CV_MAJOR_VERSION<=3 + cv::findContours(cell_copy, cellsi, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE); +#else + cv::findContours(cell_copy, cellsi, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE); +#endif + cells.insert(cells.end(), cellsi.begin(), cellsi.end()); + } + +#ifdef DEBUG_VISUALIZATION +// // testing +// cv::Mat black_map = cv::Mat::zeros(cell_map.rows, cell_map.cols, cell_map.type()); +// for(size_t i=0; i=min_cell_area) + { + cell_polygons.push_back(current_cell); + polygon_centers.push_back(current_cell.getCenter()); + } + else + { + std::cout << "WARN: BoustrophedonExplorer::computeCellDecomposition: dropped cell " << cell+1 << " with area=" << current_cell.getArea() << ". This should only happen for small unconnected cells." << std::endl; + } + } +} + +int BoustrophedonExplorer::mergeCells(cv::Mat& cell_map, cv::Mat& cell_map_labels, const double min_cell_area, const int min_cell_width) +{ + // label all cells + // --> create a label map with 0=walls/obstacles, -1=cell borders, 1,2,3,4...=cell labels + cell_map.convertTo(cell_map_labels, CV_32SC1, 256, 0); + // --> re-assign the cell borders with -1 + for (int v=0; v(v,u) == BORDER_PIXEL_VALUE*256) + cell_map_labels.at(v,u) = -1; + // --> flood fill cell regions with unique id labels + std::map > cell_index_mapping; // maps each cell label --> to the cell object + int label_index = 1; + for (int v=0; v skip + if (cell_map_labels.at(v,u)!=65280) + continue; + + // fill each cell with a unique id + cv::Rect bounding_box; + const double area = cv::floodFill(cell_map_labels, cv::Point(u,v), label_index, &bounding_box, 0, 0, 4); + cell_index_mapping[label_index] = boost::shared_ptr(new BoustrophedonCell(label_index, area, bounding_box)); + label_index++; + if (label_index == INT_MAX) + std::cout << "WARN: BoustrophedonExplorer::mergeCells: label_index exceeds range of int." << std::endl; + } + } + std::cout << "INFO: BoustrophedonExplorer::mergeCells: found " << label_index-1 << " cells before merging." << std::endl; + + // determine the neighborhood relationships between all cells + for (int v=1; v(v,u)==-1) // only check the border points for neighborhood relationships + { + const int label_left = cell_map_labels.at(v,u-1); + const int label_right = cell_map_labels.at(v,u+1); + if (label_left>0 && label_right>0) + { + cell_index_mapping[label_left]->neighbors_.insert(cell_index_mapping[label_right]); + cell_index_mapping[label_right]->neighbors_.insert(cell_index_mapping[label_left]); + } + const int label_up = cell_map_labels.at(v-1,u); + const int label_down = cell_map_labels.at(v+1,u); + if (label_up>0 && label_down>0) + { + cell_index_mapping[label_up]->neighbors_.insert(cell_index_mapping[label_down]); + cell_index_mapping[label_down]->neighbors_.insert(cell_index_mapping[label_up]); + } + } + } + } +#ifdef DEBUG_VISUALIZATION +// printCells(cell_index_mapping); +// cv::imshow("cell_map",cell_map); +// cv::waitKey(); +#endif + + // iteratively merge cells + mergeCellsSelection(cell_map, cell_map_labels, cell_index_mapping, min_cell_area, min_cell_width); + + // re-assign area labels to 1,2,3,4,... + int new_cell_label = 1; + for (std::map >::iterator itc=cell_index_mapping.begin(); itc!=cell_index_mapping.end(); ++itc, ++new_cell_label) + for (int v=0; v(v,u)==itc->second->label_) + cell_map_labels.at(v,u) = new_cell_label; + + std::cout << "INFO: BoustrophedonExplorer::mergeCells: " << cell_index_mapping.size() << " cells remaining after merging." << std::endl; + return cell_index_mapping.size(); +} + +void BoustrophedonExplorer::mergeCellsSelection(cv::Mat& cell_map, cv::Mat& cell_map_labels, std::map >& cell_index_mapping, + const double min_cell_area, const int min_cell_width) +{ + // iteratively merge cells + // merge small cells below min_cell_area with their largest neighboring cell + std::multimap > area_to_region_id_mapping; // maps the area of each cell --> to the respective cell + for (std::map >::iterator itc=cell_index_mapping.begin(); itc!=cell_index_mapping.end(); ++itc) + area_to_region_id_mapping.insert(std::pair >(itc->second->area_, itc->second)); + for (std::multimap >::iterator it=area_to_region_id_mapping.begin(); it!=area_to_region_id_mapping.end();) + { + // skip if segment is large enough (area and side length criteria) + if (it->first >= min_cell_area && it->second->bounding_box_.width >= min_cell_width && it->second->bounding_box_.height >= min_cell_width) + { + ++it; + continue; + } + + // skip segments which have no neighbors + if (it->second->neighbors_.size() == 0) + { + std::cout << "WARN: BoustrophedonExplorer::mergeCells: skipping small cell without neighbors." << std::endl; + ++it; + continue; + } + + // determine the largest neighboring cell + const BoustrophedonCell& small_cell = *(it->second); + std::multimap, std::greater > area_sorted_neighbors; + for (BoustrophedonCell::BoustrophedonCellSetIterator itn = small_cell.neighbors_.begin(); itn != small_cell.neighbors_.end(); ++itn) + area_sorted_neighbors.insert(std::pair >((*itn)->area_, *itn)); + BoustrophedonCell& large_cell = *(area_sorted_neighbors.begin()->second); + + // merge the cells + mergeTwoCells(cell_map, cell_map_labels, small_cell, large_cell, cell_index_mapping); + + // update area_to_region_id_mapping + area_to_region_id_mapping.clear(); + for (std::map >::iterator itc=cell_index_mapping.begin(); itc!=cell_index_mapping.end(); ++itc) + area_to_region_id_mapping.insert(std::pair >(itc->second->area_, itc->second)); + it = area_to_region_id_mapping.begin(); + +#ifdef DEBUG_VISUALIZATION +// printCells(cell_index_mapping); +// cv::imshow("cell_map",cell_map); +// cv::waitKey(); +#endif + } + + // label remaining border pixels with label of largest neighboring region label + for (int v=1; v(v,u) == BORDER_PIXEL_VALUE) + { + std::set neighbor_labels; + for (int dv=-1; dv<=1; ++dv) + { + for (int du=-1; du<=1; ++du) + { + const int& val = cell_map_labels.at(v+dv,u+du); + if (val>0) + neighbor_labels.insert(val); + } + } + if (neighbor_labels.size() > 0) + { + int new_label = -1; + for (std::multimap >::reverse_iterator it=area_to_region_id_mapping.rbegin(); it!=area_to_region_id_mapping.rend(); ++it) + { + if (neighbor_labels.find(it->second->label_) != neighbor_labels.end()) + { + cell_map_labels.at(v,u) = it->second->label_; + break; + } + } + } + else + std::cout << "WARN: BoustrophedonExplorer::mergeCells: border pixel has no labeled neighbors." << std::endl; + } + } + } +} + +void BoustrophedonExplorer::mergeTwoCells(cv::Mat& cell_map, cv::Mat& cell_map_labels, const BoustrophedonCell& minor_cell, BoustrophedonCell& major_cell, + std::map >& cell_index_mapping) +{ + // execute merging the minor cell into the major cell + // --> remove border from maps + for (int v=0; v(v,u) == BORDER_PIXEL_VALUE && + ((cell_map_labels.at(v,u-1)==minor_cell.label_ && cell_map_labels.at(v,u+1)==major_cell.label_) || + (cell_map_labels.at(v,u-1)==major_cell.label_ && cell_map_labels.at(v,u+1)==minor_cell.label_) || + (cell_map_labels.at(v-1,u)==minor_cell.label_ && cell_map_labels.at(v+1,u)==major_cell.label_) || + (cell_map_labels.at(v-1,u)==major_cell.label_ && cell_map_labels.at(v+1,u)==minor_cell.label_))) + { + cell_map.at(v,u) = 255; + cell_map_labels.at(v,u) = major_cell.label_; + major_cell.area_ += 1; + } + // --> update old label in cell_map_labels + for (int v=0; v(v,u) == minor_cell.label_) + cell_map_labels.at(v,u) = major_cell.label_; + // --> update major_cell + major_cell.area_ += minor_cell.area_; + for (BoustrophedonCell::BoustrophedonCellSetIterator itn = major_cell.neighbors_.begin(); itn != major_cell.neighbors_.end(); ++itn) + if ((*itn)->label_ == minor_cell.label_) + { + major_cell.neighbors_.erase(itn); + break; + } + for (BoustrophedonCell::BoustrophedonCellSetIterator itn = minor_cell.neighbors_.begin(); itn != minor_cell.neighbors_.end(); ++itn) + if ((*itn)->label_ != major_cell.label_) + major_cell.neighbors_.insert(*itn); + + // clean all references to minor_cell + cell_index_mapping.erase(minor_cell.label_); + for (std::map >::iterator itc=cell_index_mapping.begin(); itc!=cell_index_mapping.end(); ++itc) + for (BoustrophedonCell::BoustrophedonCellSetIterator itn = itc->second->neighbors_.begin(); itn != itc->second->neighbors_.end(); ++itn) + if ((*itn)->label_ == minor_cell.label_) + { + (*itn)->label_ = major_cell.label_; + break; + } +} + +void BoustrophedonExplorer::correctThinWalls(cv::Mat& room_map) +{ + for (int v=1; v(v-1,u-1)==255 && room_map.at(v-1,u)==0 && room_map.at(v,u-1)==0 && room_map.at(v,u)==255) + room_map.at(v,u)=0; + else if (room_map.at(v-1,u-1)==0 && room_map.at(v-1,u)==255 && room_map.at(v,u-1)==255 && room_map.at(v,u)==0) + room_map.at(v,u-1)=0; + } + } +} + +void BoustrophedonExplorer::computeBoustrophedonPath(const cv::Mat& room_map, const float map_resolution, const GeneralizedPolygon& cell, + std::vector& fov_middlepoint_path, cv::Point& robot_pos, + const int grid_spacing_as_int, const int half_grid_spacing_as_int, const double path_eps, const int max_deviation_from_track, const int grid_obstacle_offset) +{ + // get a map that has only the current cell drawn in + // Remark: single cells are obstacle free so it is sufficient to use the cell to check if a position can be reached during the + // execution of the coverage path + cv::Mat cell_map; + cell.drawPolygon(cell_map, cv::Scalar(255)); + + // align the longer dimension of the cell horizontally with the x-axis + cv::Point cell_center = cell.getBoundingBoxCenter(); + cv::Mat R_cell; + cv::Rect cell_bbox; + cv::Mat rotated_cell_map; + RoomRotator cell_rotation; + cell_rotation.computeRoomRotationMatrix(cell_map, R_cell, cell_bbox, map_resolution, &cell_center); + cell_rotation.rotateRoom(cell_map, rotated_cell_map, R_cell, cell_bbox); + + // create inflated obstacles room map and rotate according to cell + // --> used later for checking accessibility of Boustrophedon path inside the cell + cv::Mat inflated_room_map, rotated_inflated_room_map; + cv::erode(room_map, inflated_room_map, cv::Mat(), cv::Point(-1, -1), half_grid_spacing_as_int+grid_obstacle_offset); + cell_rotation.rotateRoom(inflated_room_map, rotated_inflated_room_map, R_cell, cell_bbox); + cv::Mat rotated_inflated_cell_map = rotated_cell_map.clone(); + for (int v=0; v(v,u)!=0 && rotated_inflated_room_map.at(v,u)==0) + rotated_inflated_cell_map.at(v,u) = 128; +#ifdef DEBUG_VISUALIZATION + cv::imshow("rotated_cell_map_with_inflation", rotated_inflated_cell_map); +#endif + + // this was deactivated because it is not as accurate as the direct check within GridGenerator::generateBoustrophedonGrid, + // because the rotation introduces some rounding errors +// // get the min/max x/y values for this cell +// int min_x=100000, max_x=0, min_y=100000, max_y=0; +// std::vector rotated_vertexes = cell.getVertices(); +// cv::transform(rotated_vertexes, rotated_vertexes, R_cell); +// for(size_t point=0; point max_x) +// max_x = rotated_vertexes[point].x; +// if(rotated_vertexes[point].y > max_y) +// max_y = rotated_vertexes[point].y; +// if(rotated_vertexes[point].x < min_x) +// min_x = rotated_vertexes[point].x; +// if(rotated_vertexes[point].y < min_y) +// min_y = rotated_vertexes[point].y; +// } + + // compute the basic Boustrophedon grid lines + BoustrophedonGrid grid_lines; + GridGenerator::generateBoustrophedonGrid(rotated_cell_map, rotated_inflated_cell_map, -1, grid_lines, cv::Vec4i(-1, -1, -1, -1), //cv::Vec4i(min_x, max_x, min_y, max_y), + grid_spacing_as_int, half_grid_spacing_as_int, 1, max_deviation_from_track); + +#ifdef DEBUG_VISUALIZATION + cv::Mat rotated_cell_map_disp = rotated_cell_map.clone(); + for (size_t i=0; i outer_corners(4); + outer_corners[0] = grid_lines[0].upper_line[0]; // upper left corner + outer_corners[1] = grid_lines[0].upper_line.back(); // upper right corner + outer_corners[2] = grid_lines.back().upper_line[0]; // lower left corner + outer_corners[3] = grid_lines.back().upper_line.back(); // lower right corner + cv::Mat R_cell_inv; + cv::invertAffineTransform(R_cell, R_cell_inv); // invert the rotation matrix to remap the determined points to the original cell + cv::transform(outer_corners, outer_corners, R_cell_inv); + double min_corner_dist = path_planner_.planPath(room_map, robot_pos, outer_corners[0], 1.0, 0.0, map_resolution); + int min_corner_index = 0; + for (int i=1; i<4; ++i) + { + double dist = path_planner_.planPath(room_map, robot_pos, outer_corners[i], 1.0, 0.0, map_resolution); + if (dist < min_corner_dist) + { + min_corner_dist = dist; + min_corner_index = i; + } + } + bool start_from_upper_path = (min_corner_index<2 ? true : false); + bool start_from_left = (min_corner_index%2==0 ? true : false); // boolean to determine on which side the path should start and to check where the path ended + +#ifdef DEBUG_VISUALIZATION + cv::Mat room_map_disp = room_map.clone(); + for (size_t i=0; i current_fov_path; + if(start_from_upper_path == true) // plan the path starting from upper horizontal line + { + for(BoustrophedonGrid::iterator line=grid_lines.begin(); line!=grid_lines.end(); ++line) + { + if(start == true) // at the beginning of path planning start at first horizontal line --> no vertical points between lines + { + if(start_from_left == true) + cell_robot_pos = line->upper_line[0]; + else + cell_robot_pos = line->upper_line.back(); + start = false; + } + + if(start_from_left == true) // plan path from left to right corner + { + // get points on transition between horizontal lines by using the Astar-path + std::vector astar_path; + path_planner_.planPath(rotated_inflated_cell_map, cell_robot_pos, line->upper_line[0], 1.0, 0.0, map_resolution, 0, &astar_path); + downsamplePath(astar_path, current_fov_path, cell_robot_pos, path_eps); + + // get points between left and right corner + downsamplePath(line->upper_line, current_fov_path, cell_robot_pos, path_eps); + + // add the lower path of the current line if available (and then start from the left side again next time) + if (line->has_two_valid_lines == true) + downsamplePathReverse(line->lower_line, current_fov_path, cell_robot_pos, path_eps); + else + start_from_left = false; // start from the right side next time + } + else // plan path from right to left corner + { + // get points on transition between horizontal lines by using the Astar-path + std::vector astar_path; + path_planner_.planPath(rotated_inflated_cell_map, cell_robot_pos, line->upper_line.back(), 1.0, 0.0, map_resolution, 0, &astar_path); + downsamplePath(astar_path, current_fov_path, cell_robot_pos, path_eps); + + // get points between right and left corner + downsamplePathReverse(line->upper_line, current_fov_path, cell_robot_pos, path_eps); + + // add the lower path of the current line if available (and then start from the right side again next time) + if (line->has_two_valid_lines == true) + downsamplePath(line->lower_line, current_fov_path, cell_robot_pos, path_eps); + else + start_from_left = true; // start from the left side next time + } + } + } + else // plan the path from the lower horizontal line + { + for(BoustrophedonGrid::reverse_iterator line=grid_lines.rbegin(); line!=grid_lines.rend(); ++line) + { + if(start == true) // at the beginning of path planning start at first horizontal line --> no vertical points between lines + { + if(start_from_left == true) + cell_robot_pos = line->upper_line[0]; + else + cell_robot_pos = line->upper_line.back(); + start = false; + } + + if(start_from_left == true) // plan path from left to right corner + { + // get points on transition between horizontal lines by using the Astar-path + std::vector astar_path; + path_planner_.planPath(rotated_inflated_cell_map, cell_robot_pos, line->upper_line[0], 1.0, 0.0, map_resolution, 0, &astar_path); + downsamplePath(astar_path, current_fov_path, cell_robot_pos, path_eps); + + // get points between left and right corner + downsamplePath(line->upper_line, current_fov_path, cell_robot_pos, path_eps); + + // add the lower path of the current line if available (and then start from the left side again next time) + if (line->has_two_valid_lines == true) + downsamplePathReverse(line->lower_line, current_fov_path, cell_robot_pos, path_eps); + else + start_from_left = false; // start from the right side next time + } + else // plan path from right to left corner + { + // get points on transition between horizontal lines by using the Astar-path + std::vector astar_path; + path_planner_.planPath(rotated_inflated_cell_map, cell_robot_pos, line->upper_line.back(), 1.0, 0.0, map_resolution, 0, &astar_path); + downsamplePath(astar_path, current_fov_path, cell_robot_pos, path_eps); + + // get points between right and left corner + downsamplePathReverse(line->upper_line, current_fov_path, cell_robot_pos, path_eps); + + // add the lower path of the current line if available (and then start from the right side again next time) + if (line->has_two_valid_lines == true) + downsamplePath(line->lower_line, current_fov_path, cell_robot_pos, path_eps); + else + start_from_left = true; // start from the left side next time + } + } + } +#ifdef DEBUG_VISUALIZATION + cv::Mat rotated_cell_fov_path_disp = rotated_cell_map.clone(); + for (size_t i=1; i fov_middlepoint_path_part; + for(std::vector::iterator point=current_fov_path.begin(); point!=current_fov_path.end(); ++point) + fov_middlepoint_path_part.push_back(cv::Point2f(point->x, point->y)); + cv::transform(fov_middlepoint_path_part, fov_middlepoint_path_part, R_cell_inv); + fov_middlepoint_path.insert(fov_middlepoint_path.end(), fov_middlepoint_path_part.begin(), fov_middlepoint_path_part.end()); + +#ifdef DEBUG_VISUALIZATION + cv::Mat cell_fov_path_disp = cell_map.clone(); + for (size_t i=1; i current_pos_vector(1, cell_robot_pos); + cv::transform(current_pos_vector, current_pos_vector, R_cell_inv); + robot_pos = current_pos_vector[0]; +} + +void BoustrophedonExplorer::downsamplePath(const std::vector& original_path, std::vector& downsampled_path, + cv::Point& robot_pos, const double path_eps) +{ + // downsample path + for(size_t path_point=0; path_point= path_eps) + { + downsampled_path.push_back(original_path[path_point]); + robot_pos = original_path[path_point]; + } + } + // add last element + if (original_path.size() > 0) + { + downsampled_path.push_back(original_path.back()); + robot_pos = original_path.back(); + } +} + +void BoustrophedonExplorer::downsamplePathReverse(const std::vector& original_path, std::vector& downsampled_path, + cv::Point& robot_pos, const double path_eps) +{ + // downsample path + for(size_t path_point=original_path.size()-1; ; --path_point) + { + if(cv::norm(robot_pos-original_path[path_point]) >= path_eps) + { + downsampled_path.push_back(original_path[path_point]); + robot_pos = original_path[path_point]; + } + if (path_point == 0) + break; + } + // add last element + if (original_path.size() > 0) + { + downsampled_path.push_back(original_path[0]); + robot_pos = original_path[0]; + } +} + +void BoustrophedonExplorer::printCells(std::map >& cell_index_mapping) +{ + std::cout << "---\n"; + for (std::map >::iterator itc=cell_index_mapping.begin(); itc!=cell_index_mapping.end(); ++itc) + { + std::cout << itc->first << ": l=" << itc->second->label_ << " a=" << itc->second->area_ << " n="; + for (BoustrophedonCell::BoustrophedonCellSetIterator its=itc->second->neighbors_.begin(); its!=itc->second->neighbors_.end(); ++its) + std::cout << (*its)->label_ << ", "; + std::cout << std::endl; + } +} + + +///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +// BoustrophedonVariantExplorer +///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + + +void BoustrophedonVariantExplorer::mergeCellsSelection(cv::Mat& cell_map, cv::Mat& cell_map_labels, std::map >& cell_index_mapping, + const double min_cell_area, const int min_cell_width) +{ + // iteratively merge cells + //todo: + // - take one major cell (the largest) and its major direction + // - merge every other cell into the major cell, except + // - the width along the major direction is too small and the cell is sufficiently large + // - the bounding box orientation (side length ratio) deviates strongly from the major direction + // - the cell main direction is not well aligned with the major direction (skew, 90 deg) + + + RoomRotator room_rotator; + //double rotation_angle = room_rotator.computeRoomMainDirection(cell_map, map_resolution); + + // merge small cells below min_cell_area with their largest neighboring cell + std::multimap > area_to_region_id_mapping; // maps the area of each cell --> to the respective cell + for (std::map >::iterator itc=cell_index_mapping.begin(); itc!=cell_index_mapping.end(); ++itc) + area_to_region_id_mapping.insert(std::pair >(itc->second->area_, itc->second)); + for (std::multimap >::iterator it=area_to_region_id_mapping.begin(); it!=area_to_region_id_mapping.end();) + { + // abort if no cells below min_cell_area remain unmerged into bigger cells + if (it->first >= min_cell_area && it->second->bounding_box_.width >= min_cell_width && it->second->bounding_box_.height >= min_cell_width) + { + ++it; + continue; + } + + // skip segments which have no neighbors + if (it->second->neighbors_.size() == 0) + { + std::cout << "WARN: BoustrophedonExplorer::mergeCells: skipping small cell without neighbors." << std::endl; + ++it; + continue; + } + + // determine the largest neighboring cell + const BoustrophedonCell& small_cell = *(it->second); + std::multimap, std::greater > area_sorted_neighbors; + for (BoustrophedonCell::BoustrophedonCellSetIterator itn = small_cell.neighbors_.begin(); itn != small_cell.neighbors_.end(); ++itn) + area_sorted_neighbors.insert(std::pair >((*itn)->area_, *itn)); + BoustrophedonCell& large_cell = *(area_sorted_neighbors.begin()->second); + + // merge the cells + mergeTwoCells(cell_map, cell_map_labels, small_cell, large_cell, cell_index_mapping); + + // update area_to_region_id_mapping + area_to_region_id_mapping.clear(); + for (std::map >::iterator itc=cell_index_mapping.begin(); itc!=cell_index_mapping.end(); ++itc) + area_to_region_id_mapping.insert(std::pair >(itc->second->area_, itc->second)); + it = area_to_region_id_mapping.begin(); + +#ifdef DEBUG_VISUALIZATION +// printCells(cell_index_mapping); +// cv::imshow("cell_map",cell_map); +// cv::waitKey(); +#endif + } + + // label remaining border pixels with label of largest neighboring region label + for (int v=1; v(v,u) == BORDER_PIXEL_VALUE) + { + std::set neighbor_labels; + for (int dv=-1; dv<=1; ++dv) + { + for (int du=-1; du<=1; ++du) + { + const int& val = cell_map_labels.at(v+dv,u+du); + if (val>0) + neighbor_labels.insert(val); + } + } + if (neighbor_labels.size() > 0) + { + int new_label = -1; + for (std::multimap >::reverse_iterator it=area_to_region_id_mapping.rbegin(); it!=area_to_region_id_mapping.rend(); ++it) + { + if (neighbor_labels.find(it->second->label_) != neighbor_labels.end()) + { + cell_map_labels.at(v,u) = it->second->label_; + break; + } + } + } + else + std::cout << "WARN: BoustrophedonExplorer::mergeCells: border pixel has no labeled neighbors." << std::endl; + } + } + } +} + +//void BoustrophedonVariantExplorer::computeCellDecomposition(const cv::Mat& room_map, const float map_resolution, const double min_cell_area, +// std::vector& cell_polygons, std::vector& polygon_centers) +//{ +// std::cout << "Calling BoustrophedonVariantExplorer::computeCellDecomposition..." << std::endl; +// +// // *********************** II. Sweep a slice trough the map and mark the found cell boundaries. *********************** +// // create a map copy to mark the cell boundaries +// cv::Mat cell_map = room_map.clone(); +//#ifdef DEBUG_VISUALIZATION +// cv::imshow("cell_map", cell_map); +//#endif +// +// +// // *********************** III. Find the separated cells. *********************** +// std::vector > cells; +// cv::Mat cell_copy = cell_map.clone(); +// correctThinWalls(cell_copy); // just adds a few obstacle pixels to avoid merging independent segments +// cv::findContours(cell_copy, cells, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE); +//#ifdef DEBUG_VISUALIZATION +//// testing +//// cv::Mat black_map = cv::Mat(cell_map.rows, cell_map.cols, cell_map.type(), cv::Scalar(0)); +//// for(size_t i=0; i=min_cell_area) +// { +// GeneralizedPolygon current_cell(cells[cell], map_resolution); +// cell_polygons.push_back(current_cell); +// polygon_centers.push_back(current_cell.getCenter()); +// } +// } +//} diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/common/src/convex_sensor_placement_explorator.cpp b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/common/src/convex_sensor_placement_explorator.cpp new file mode 100644 index 0000000..1dbbd1f --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/common/src/convex_sensor_placement_explorator.cpp @@ -0,0 +1,596 @@ +#include + +// Constructor +convexSPPExplorator::convexSPPExplorator() +{ + +} + +// function that is used to create and solve a Gurobi optimization problem out of the given matrices and vectors, if +// Gurobi was found on the computer +template +void convexSPPExplorator::solveGurobiOptimizationProblem(std::vector& C, const cv::Mat& V, const std::vector* W) +{ +#ifdef GUROBI_FOUND + std::cout << "Creating and solving linear program with Gurobi." << std::endl; + // initialize the problem + GRBEnv *env = new GRBEnv(); + GRBModel model = GRBModel(*env); + + // vector that stores the variables of the problem + std::vector optimization_variables; + + // add the optimization variables to the problem + int number_of_variables = 0; + for(size_t var=0; varoperator[](var), GRB_CONTINUOUS); + optimization_variables.push_back(current_variable); + ++number_of_variables; + } + else + { + GRBVar current_variable = model.addVar(0.0, 1.0, 1.0, GRB_BINARY); + optimization_variables.push_back(current_variable); + ++number_of_variables; + } + } + std::cout << "number of variables in the problem: " << number_of_variables << std::endl; + + // inequality constraints to ensure that every position has been seen at least once + for(size_t row=0; row variable_indices; + for(size_t col=0; col(row, col) == 1) + variable_indices.push_back((int) col); + + // add the constraint, if the current cell can be covered by the given arcs, indices=1 in this constraint + if(variable_indices.size()>0) + { + GRBLinExpr current_coverage_constraint; + for(size_t var=0; var=1); + } + } + + // solve the optimization + model.optimize(); + + // retrieve solution + std::cout << "retrieving solution" << std::endl; + for(size_t var=0; var +void convexSPPExplorator::solveOptimizationProblem(std::vector& C, const cv::Mat& V, const std::vector* W) +{ + // initialize the problem + CoinModel problem_builder; + + ROS_INFO("Creating and solving linear program."); + + // add the optimization variables to the problem + int rval; + for(size_t variable=0; variableoperator[](variable)); + } + else + { + problem_builder.setColBounds(variable, 0.0, 1.0); + problem_builder.setObjective(variable, 1.0); + problem_builder.setInteger(variable); + } + } + + // inequality constraints to ensure that every position has been seen at least once + for(size_t row=0; row variable_indices; + for(size_t col=0; col(row, col) == 1) + variable_indices.push_back((int) col); + + // all indices are 1 in this constraint + std::vector variable_coefficients(variable_indices.size(), 1.0); + + // add the constraint + problem_builder.addRow((int) variable_indices.size(), &variable_indices[0], &variable_coefficients[0], 1.0); + } + + // load the created LP problem to the solver + OsiClpSolverInterface LP_solver; + OsiClpSolverInterface* solver_pointer = &LP_solver; + + solver_pointer->loadFromCoinModel(problem_builder); + + // testing + solver_pointer->writeLp("lin_cpp_prog", "lp"); + + // solve the created optimization problem + CbcModel model(*solver_pointer); + model.solver()->setHintParam(OsiDoReducePrint, true, OsiHintTry); + + model.initialSolve(); + model.branchAndBound(); + + // retrieve solution + const double * solution = model.solver()->getColSolution(); + + for(size_t res=0; res& path, + const float map_resolution, const cv::Point starting_position, const cv::Point2d map_origin, + const int cell_size_pixel, const double delta_theta, const std::vector >& fov_corners_meter, + const Eigen::Matrix& robot_to_fov_vector_meter, const double largest_robot_to_footprint_distance_meter, + const uint sparsity_check_range, const bool plan_for_footprint) +{ + const int half_cell_size = cell_size_pixel/2; + + // *********************** I. Find the main directions of the map and rotate it in this manner. *********************** + // rotate map + cv::Mat R; + cv::Rect bbox; + cv::Mat rotated_room_map; + RoomRotator room_rotation; + const double room_rotation_angle = room_rotation.computeRoomRotationMatrix(room_map, R, bbox, map_resolution); + room_rotation.rotateRoom(room_map, rotated_room_map, R, bbox); + + // transform starting position + std::vector starting_point_vector(1, starting_position); // opencv syntax + cv::transform(starting_point_vector, starting_point_vector, R); + cv::Point rotated_starting_position = starting_point_vector[0]; + + // ************* II. Go trough the map and discretize it. ************* + // get cells + // compute the basic Boustrophedon grid lines + std::vector cell_centers_rotated, cell_centers; // in [pixels] + cv::Mat inflated_rotated_room_map; + BoustrophedonGrid grid_lines; + GridGenerator::generateBoustrophedonGrid(rotated_room_map, inflated_rotated_room_map, half_cell_size, grid_lines, cv::Vec4i(-1, -1, -1, -1), + cell_size_pixel, half_cell_size, cell_size_pixel); + // convert grid points format + for (BoustrophedonGrid::iterator line=grid_lines.begin(); line!=grid_lines.end(); ++line) + { + cell_centers_rotated.insert(cell_centers_rotated.end(), line->upper_line.begin(), line->upper_line.end()); + if (line->has_two_valid_lines == true) + cell_centers_rotated.insert(cell_centers_rotated.end(), line->lower_line.begin(), line->lower_line.end()); + } + // rotate back to original image + cv::Mat R_inv; + cv::invertAffineTransform(R, R_inv); + std::vector fov_middlepoint_path_transformed; + cv::transform(cell_centers_rotated, cell_centers, R_inv); + +// // print grid +// cv::Mat point_map = room_map.clone(); +// for(std::vector::iterator point = cell_centers.begin(); point != cell_centers.end(); ++point) +// { +// cv::circle(point_map, *point, 2, cv::Scalar(127), CV_FILLED); +// std::cout << " - " << *point << "\n"; +// } +// cv::imshow("grid", point_map); +// cv::waitKey(); + + // get candidate sensing poses + std::vector candidate_sensing_poses; + double delta_angle = (plan_for_footprint == true ? 4.*PI : delta_theta); + for(std::vector::iterator center=cell_centers.begin(); center!=cell_centers.end(); ++center) + { + for(double angle=room_rotation_angle; angle<2.0*PI+room_rotation_angle; angle+=delta_angle) + { + // create and save pose + geometry_msgs::Pose2D candidate_pose; + candidate_pose.x = center->x; + candidate_pose.y = center->y; + candidate_pose.theta = angle; + while (candidate_pose.theta < -PI) + candidate_pose.theta += 2*PI; + while (candidate_pose.theta > PI) + candidate_pose.theta -= 2*PI; + candidate_sensing_poses.push_back(candidate_pose); + } + } + + // ************* III. Construct the matrices needed in the linear program. ************* + // set or compute largest_robot_to_footprint_distance_pixel depending on plan_for_footprint + double max_dist=0.; + if (plan_for_footprint==true || largest_robot_to_footprint_distance_meter > 0.) + max_dist = largest_robot_to_footprint_distance_meter; + else + { + // find largest_robot_to_footprint_distance_pixel by checking the fov corners + for (size_t i=0; i max_dist) + max_dist = dist; + } + } + const double largest_robot_to_footprint_distance_pixel = max_dist / map_resolution; + const double cell_outcircle_radius_pixel = cell_size_pixel/sqrt(2); + + // construct W + int number_of_candidates=candidate_sensing_poses.size(); + std::vector W(number_of_candidates, 1.0); // initial weights + + // construct V + cv::Mat V = cv::Mat::zeros(cell_centers.size(), number_of_candidates, CV_8U); // binary variables + + // check observable cells from each candidate pose + const double map_resolution_inverse = 1./map_resolution; + for(std::vector::iterator pose=candidate_sensing_poses.begin(); pose!=candidate_sensing_poses.end(); ++pose) + { + // get the transformed field of view + // get the rotation matrix + const float sin_theta = std::sin(pose->theta); + const float cos_theta = std::cos(pose->theta); + Eigen::Matrix R_fov; + R_fov << cos_theta, -sin_theta, sin_theta, cos_theta; + + // transform field of view points, if the planning should be done for the field of view + std::vector transformed_fov_points; + Eigen::Matrix pose_as_matrix; + if(plan_for_footprint==false) + { + pose_as_matrix << (pose->x*map_resolution)+map_origin.x, (pose->y*map_resolution)+map_origin.y; // convert to [meter] + for(size_t point = 0; point < fov_corners_meter.size(); ++point) + { + // linear transformation + Eigen::Matrix transformed_vector = pose_as_matrix + R_fov * fov_corners_meter[point]; + + // save the transformed point as cv::Point, also check if map borders are satisfied and transform it into pixel + // values + cv::Point current_point = cv::Point((transformed_vector(0, 0) - map_origin.x)*map_resolution_inverse, (transformed_vector(1, 0) - map_origin.y)*map_resolution_inverse); + current_point.x = std::max(current_point.x, 0); + current_point.y = std::max(current_point.y, 0); + current_point.x = std::min(current_point.x, room_map.cols); + current_point.y = std::min(current_point.y, room_map.rows); + transformed_fov_points.push_back(current_point); + } + } + + // for each pose check the cells that are closer than the max distance from robot to fov-corner and more far away + // than the min distance, also only check points that span an angle to the robot-to-fov vector smaller than the + // max found angle to the corners + // when planning for the robot footprint simply check if its distance to the pose is at most the given coverage radius + for(std::vector::iterator neighbor=cell_centers.begin(); neighbor!=cell_centers.end(); ++neighbor) + { + // compute pose to neighbor vector + Eigen::Matrix pose_to_neighbor; + pose_to_neighbor << neighbor->x-pose->x, neighbor->y-pose->y; + double distance = pose_to_neighbor.norm(); + + // if neighbor is in the possible distance range check it further, distances given in [pixel] + if(plan_for_footprint==false && distance<=largest_robot_to_footprint_distance_pixel) + { + + if(cv::pointPolygonTest(transformed_fov_points, *neighbor, false) >= 0) // point inside + { + // check if the line from the robot pose to the neighbor crosses an obstacle, if so it is not observable from the pose + cv::LineIterator border_line(room_map, cv::Point(pose->x, pose->y), *neighbor, 8); // opencv implementation of bresenham algorithm, 8: color, irrelevant + bool hit_obstacle = false; + for(size_t i = 0; i < border_line.count; ++i, ++border_line) + if(room_map.at(border_line.pos()) == 0) + hit_obstacle = true; + + if(hit_obstacle == false) + { + V.at(neighbor-cell_centers.begin(), pose-candidate_sensing_poses.begin()) = 1; + } + else // neighbor cell not observable + V.at(neighbor-cell_centers.begin(), pose-candidate_sensing_poses.begin()) = 0; + } + else // neighbor cell outside the field of view + V.at(neighbor-cell_centers.begin(), pose-candidate_sensing_poses.begin()) = 0; + } + // check if neighbor is covered by footprint when planning for it + else if(plan_for_footprint==true && (distance+cell_outcircle_radius_pixel)<=largest_robot_to_footprint_distance_pixel) + { + // check if the line from the robot pose to the neighbor crosses an obstacle, if so it is not observable from the pose + cv::LineIterator border_line(room_map, cv::Point(pose->x, pose->y), *neighbor, 8); // opencv implementation of bresenham algorithm, 8: color, irrelevant + bool hit_obstacle = false; + for(size_t i = 0; i < border_line.count; ++i, ++border_line) + if(room_map.at(border_line.pos()) == 0) + hit_obstacle = true; + if(hit_obstacle == false) + V.at(neighbor-cell_centers.begin(), pose-candidate_sensing_poses.begin()) = 1; + else // neighbor cell not observable + V.at(neighbor-cell_centers.begin(), pose-candidate_sensing_poses.begin()) = 0; + } + else // point not in the right range to be inside the fov + V.at(neighbor-cell_centers.begin(), pose-candidate_sensing_poses.begin()) = 0; + } + } + std::cout << "number of optimization variables: " << W.size() << std::endl; + +// testing +// for(size_t i=0; i(i, j) == 1) +// { +// cv::circle(black_map, cv::Point(candidate_sensing_poses[j].x, candidate_sensing_poses[j].y), 2, cv::Scalar(100), CV_FILLED); +// cv::imshow("candidates", black_map); +// cv::waitKey(); +// } +// } +// } + + // ************* IV. Solve the different linear problems. ************* + // 1. solve the weighted optimization problem until a convergence in the sparsity is reached or a defined number of + // iterations is reached + std::vector C(W.size()); //initialize the objective vector + bool sparsity_converged = false; // boolean to check, if the sparsity of C has converged to a certain value + double weight_epsilon = 0.0; // parameter that is used to update the weights after one solution has been obtained + uint number_of_iterations = 0; + std::vector sparsity_measures; // vector that stores the computed sparsity measures to check convergence + const double euler_constant = std::exp(1.0); + Timer tim; + do + { + // increase number of iterations + ++number_of_iterations; + + // solve optimization of the current step + #ifdef GUROBI_FOUND + solveGurobiOptimizationProblem(C, V, &W); + #else + solveOptimizationProblem(C, V, &W); + #endif + + // update epsilon and W + const int exponent = 1 + (number_of_iterations - 1)*0.1; + weight_epsilon = std::pow(1./(euler_constant-1.), exponent); + for(size_t weight=0; weight= sparsity_check_range) + { + uint number_of_last_measure = 0; + for(std::vector::reverse_iterator measure=sparsity_measures.rbegin(); measure!=sparsity_measures.rbegin()+sparsity_check_range && measure!=sparsity_measures.rend(); ++measure) + if(*measure >= sparsity_measures.back()) + ++number_of_last_measure; + + if(number_of_last_measure == sparsity_check_range) + sparsity_converged = true; + } + + std::cout << "Iteration: " << number_of_iterations << ", sparsity: " << sparsity_measures.back() << std::endl; + } while(sparsity_converged == false && number_of_iterations <= 150 && tim.getElapsedTimeInSec() < 1200); // wait no longer than 20 minutes + + // 2. Reduce the optimization problem by discarding the candidate poses that correspond to an optimization variable + // equal to 0, i.e. those that are not considered any further. + uint new_number_of_variables = 0; + cv::Mat V_reduced = cv::Mat(cell_centers.size(), 1, CV_8U); // initialize one column because opencv wants it this way, add other columns later + std::vector reduced_sensing_candidates; + for(std::vector::iterator result=C.begin(); result!=C.end(); ++result) + { + if(*result != 0.0) + { + // increase number of optimization variables + ++new_number_of_variables; + + // gather column corresponding to this candidate pose and add it to the new observability matrix + cv::Mat column = V.col(result-C.begin()); + cv::hconcat(V_reduced, column, V_reduced); + + // save the new possible sensing candidate + reduced_sensing_candidates.push_back(candidate_sensing_poses[result-C.begin()]); + } + } + + // remove the first initial column + V_reduced = V_reduced.colRange(1, V_reduced.cols); + + // solve the final optimization problem + std::cout << "new_number_of_variables=" << new_number_of_variables << std::endl; + std::vector C_reduced(new_number_of_variables); +#ifdef GUROBI_FOUND + solveGurobiOptimizationProblem(C_reduced, V_reduced, NULL); +#else + solveOptimizationProblem(C_reduced, V_reduced, NULL); +#endif + + // ************* V. Retrieve solution and find a path trough the chosen poses. ************* + // read out solution + std::vector chosen_positions; // vector to determine the tsp solution, in [pixels] + std::vector chosen_poses; // in [px,px,rad] + for(std::vector::iterator result=C_reduced.begin(); result!=C_reduced.end(); ++result) + { + if(*result == 1) + { + chosen_poses.push_back(reduced_sensing_candidates[result-C_reduced.begin()]); + chosen_positions.push_back(cv::Point(reduced_sensing_candidates[result-C_reduced.begin()].x, reduced_sensing_candidates[result-C_reduced.begin()].y)); + } + } + + if (chosen_positions.size()==0) + { + std::cout << "Convex SPP: chosen_positions is empty." << std::endl; + return; + } + + + // get the distance matrix + ROS_INFO("Constructing distance matrix"); + cv::Mat distance_matrix; + DistanceMatrix distance_matrix_computation; + distance_matrix_computation.constructDistanceMatrix(distance_matrix, room_map, chosen_positions, 0.25, 0.0, map_resolution, path_planner_); + + // go trough the matrix and multiply the distances between the poses with a factor corresponding to the difference of + // the travel angle from the first pose to the other pose + for(size_t pose=0; pose(pose, neighbor) = penalizing_factor*distance_matrix.at(pose, neighbor); + distance_matrix.at(neighbor, pose) = penalizing_factor*distance_matrix.at(neighbor, pose); //symmetrical-Matrix --> saves half the computation time + } + } + } + + // do the repetitive nearest neighbor algorithm + ROS_INFO("Solving TSP with repetitive nearest neighbor"); + std::vector best_order; + double min_distance = 1e9; + if (chosen_positions.size()>100) + std::cout << "0 10 20 30 40 50 60 70 80 90 100" << std::endl; + for(int start=0; start500 && start%(std::max(1,(int)chosen_positions.size()/100))==0) + std::cout << "." << std::flush; + + // obtain nearest neighbor solution for this start + std::vector current_order = tsp_solver_.solveNearestTSP(distance_matrix, start); + + // get pathlength for this solution + double current_pathlength = 0.0; + for(size_t vertex=0; vertex(current_order[vertex], current_order[(vertex+1)%current_order.size()]); + + // if current path is better than previous one save it + if(current_pathlength < min_distance) + { + min_distance = current_pathlength; + best_order = current_order; + } + } + std::cout << std::endl; + + // find the node that is closest to the starting position + min_distance = 1e9; + int starting_index = 0; + for(std::vector::iterator position=chosen_positions.begin(); position!=chosen_positions.end(); ++position) + { + // calculate current distance + double current_distance = cv::norm(rotated_starting_position - *position); + + // check if current length is smaller than optimal + if(current_distance < min_distance) + { + min_distance = current_distance; + starting_index = position-chosen_positions.begin(); + } + } + + // create the path starting from the found start + std::vector fov_poses; + std::vector::iterator start = std::find(best_order.begin(), best_order.end(), starting_index); // obtain iterator to index in best order to start path creation from there + for(size_t pose=start-best_order.begin(); path.size()!=chosen_poses.size() && fov_poses.size()!=chosen_poses.size(); ++pose) + { + + // check if end has been reached, if true start from the beginning again + if(pose == best_order.size()) + pose = 0; + + // insert pose mapped to global coordinates + geometry_msgs::Pose2D current_pose; + if (plan_for_footprint == false) + { + // take the viewing directions as computed for fov mode, convert locations from pixels to meters + current_pose.x = (chosen_poses[best_order[pose]].x*map_resolution)+map_origin.x; + current_pose.y = (chosen_poses[best_order[pose]].y*map_resolution)+map_origin.y; + current_pose.theta = chosen_poses[best_order[pose]].theta; + path.push_back(current_pose); + } + else + { + // for footprint planning the viewing direction has to be computed from the trajectory + fov_poses.push_back(cv::Point2f(chosen_poses[best_order[pose]].x, chosen_poses[best_order[pose]].y)); + } + } + + // *********************** VI. Get the robot path out of the fov path. *********************** + // determine the correct viewing angles for the poses (footprint planning just used one fixed dummy direction) + if (plan_for_footprint == true) + { + // compute viewing directions + room_rotation.transformPointPathToPosePath(fov_poses, path); + // convert to meters + for (size_t i=0; i + +// Constructor +EnergyFunctionalExplorator::EnergyFunctionalExplorator() +{ + +} + +// Function that computes the energy functional for each pair of nodes. +double EnergyFunctionalExplorator::E(const EnergyExploratorNode& location, const EnergyExploratorNode& neighbor, const double cell_size_in_pixel, const double previous_travel_angle) +{ + float energy_functional = 0.0; + + // 1. translational distance + cv::Point diff = neighbor.center_ - location.center_; + energy_functional += cv::norm(diff)/cell_size_in_pixel; + + // 2. rotational distance + const double travel_angle_to_node = std::atan2(diff.y, diff.x); + double diff_angle = travel_angle_to_node - previous_travel_angle; + while (diff_angle < -PI) + diff_angle += 2*PI; + while (diff_angle > PI) + diff_angle -= 2*PI; + energy_functional += std::abs(diff_angle)*PI_2_INV; // 1.01 for punishing turns a little bit more on a tie + + // 3. neighboring function, determining how many neighbors of the neighbor have been visited + int visited_neighbors = 0; + for(std::vector::const_iterator n=neighbor.neighbors_.begin(); n!=neighbor.neighbors_.end(); ++n) + if ((*n)->obstacle_ == false && (*n)->visited_ == true) + ++visited_neighbors; + energy_functional += 4. - 0.5*visited_neighbors; + + int wall_points = 0; + for(std::vector::const_iterator n=neighbor.neighbors_.begin(); n!=neighbor.neighbors_.end(); ++n) + if ((*n)->obstacle_ == true) + ++wall_points; + energy_functional += 0.72 - 0.09*wall_points; + + //std::cout << "E: " << cv::norm(diff)/cell_size << " + " << std::abs(diff_angle)*PI_2_INV << " + " << 4. - 0.5*visited_neighbors << " + " << 0.72 - 0.09*wall_points << " angles: " << travel_angle_to_node << ", " << previous_travel_angle << " diff ang: " << diff_angle << std::endl; + + return energy_functional; +} + +// Function that plans a coverage path trough the given map, using the method proposed in +// +// Bormann Richard, Joshua Hampp, and Martin Hägele. "New brooms sweep clean-an autonomous robotic cleaning assistant for +// professional office cleaning." Robotics and Automation (ICRA), 2015 IEEE International Conference on. IEEE, 2015. +// +// This method discretizes the free space, that should be covered, into several nodes. Each of the node has to be covered, in order +// to cover the whole area. The path starts at the node that is closest to the given starting position and chooses the next node as +// the one that minimizes the energy functional, provided in the paper above. To do this here the following steps are done. +// I. The free area gets discretized into several nodes, using the given cell_size parameter, starting at the upper left white pixel of +// the room. Whenever the overlaid grid then hits a white pixel, this point is added as a node. Then after all nodes have been found +// the direct 8 neighbors for each node are found, which will be used later in the energy functional. +// II. After all nodes have been found, the coverage path is computed. +// i. The start node gets chosen as the one that is closest to the given starting position and is an edge of the given room, i.e +// a node that has less than 4 neighbors. +// ii. The next node is then iteratively chosen from the directly neighboring ones, by finding the node that minimizes the given +// energy functional and wasn't visited before. +// iii.If in the neighborhood no accessible point could be found, search for the next node in the whole grid to continue the path. +// iv. This procedure is repeated, until all created nodes have been covered. +// III. If wanted use the given vector from the robot middlepoint to the fov middlepoint to map the fov poses to the +// robot footprint poses. To do so simply a vector transformation is applied. If the computed robot pose is not in the +// free space, another accessible point is generated by finding it on the radius around the fov middlepoint s.t. +// the distance to the last robot position is minimized. If this is not wanted one has to set the corresponding +// Boolean to false (shows that the path planning should be done for the robot footprint). +// +void EnergyFunctionalExplorator::getExplorationPath(const cv::Mat& room_map, std::vector& path, const float map_resolution, + const cv::Point starting_position, const cv::Point2d map_origin, const double grid_spacing_in_pixel, + const bool plan_for_footprint, const Eigen::Matrix robot_to_fov_vector) +{ + const int grid_spacing_as_int = std::floor(grid_spacing_in_pixel); + const int half_grid_spacing_as_int = std::floor(grid_spacing_in_pixel*0.5); + + // *********************** I. Find the main directions of the map and rotate it in this manner. *********************** + cv::Mat R; + cv::Rect bbox; + cv::Mat rotated_room_map; + RoomRotator room_rotation; + room_rotation.computeRoomRotationMatrix(room_map, R, bbox, map_resolution); + room_rotation.rotateRoom(room_map, rotated_room_map, R, bbox); + + // compute min/max room coordinates + cv::Point min_room(1000000, 1000000), max_room(0, 0); + for (int v=0; v(v,u)==255) + { + min_room.x = std::min(min_room.x, u); + min_room.y = std::min(min_room.y, v); + max_room.x = std::max(max_room.x, u); + max_room.y = std::max(max_room.y, v); + } + } + } + cv::Mat inflated_rotated_room_map; + cv::erode(rotated_room_map, inflated_rotated_room_map, cv::Mat(), cv::Point(-1, -1), half_grid_spacing_as_int); + + // *********************** II. Find the nodes and their neighbors *********************** + // get the nodes in the free space + std::vector > nodes; // 2-dimensional vector to easily find the neighbors + int number_of_nodes = 0; + +// // compute min/max room coordinates +// int min_y = 1000000, min_x = 1000000; +// for (int v=0; v(v,u)==255) +// { +// min_x = std::min(min_x, u); +// min_y = std::min(min_y, v); +// } +// } +// } +// if (min_x-grid_spacing_as_int > 0) +// min_x -= grid_spacing_as_int; +// if (min_y-grid_spacing_as_int > 0) +// min_y -= grid_spacing_as_int; + + // todo: create grid in external class - it is the same in all approaches + // todo: if first/last row or column in grid has accessible areas but center is inaccessible, create a node in the accessible area + for(int y=min_room.y+half_grid_spacing_as_int; y current_row; + for(int x=min_room.x+half_grid_spacing_as_int; x(y,x) == 255) // could make sense to test all pixels of the cell, not only the center + if (GridGenerator::completeCellTest(inflated_rotated_room_map, current_node.center_, grid_spacing_as_int) == true) + { + current_node.obstacle_ = false; + current_node.visited_ = false; + ++number_of_nodes; + } + // add the obstacle nodes as already visited + else + { + current_node.obstacle_ = true; + current_node.visited_ = true; + ++number_of_nodes; + } + current_row.push_back(current_node); + } + + // insert the current row into grid + nodes.push_back(current_row); + } + std::cout << "found " << number_of_nodes << " nodes" << std::endl; + + // find the neighbors for each node + EnergyExploratorNode* first_accessible_node = 0; + std::vector corner_nodes; // vector that stores the corner nodes, i.e. nodes with 3 or less neighbors + for(size_t row=0; row& current_neighbors = nodes[row][column].neighbors_; + for(int dy=-1; dy<=1; ++dy) + { + // don't exceed the current row + if(row+dy < 0 || row+dy >= nodes.size()) + continue; + + // get the neighbors left from the current neuron + if(column > 0) + current_neighbors.push_back(&nodes[row+dy][column-1]); + + // get the nodes on the same column as the current neuron + if(dy != 0) + current_neighbors.push_back(&nodes[row+dy][column]); + + // get the nodes right from the current neuron + if(column < nodes[row].size()-1) + current_neighbors.push_back(&nodes[row+dy][column+1]); + } + + // check if the current node is a corner, i.e. nodes that have 3 or less neighbors that are not obstacles + int non_obstacle_neighbors = nodes[row][column].countNonObstacleNeighbors(); + if(non_obstacle_neighbors<=3 && nodes[row][column].obstacle_==false) + corner_nodes.push_back(&nodes[row][column]); + + if (first_accessible_node==0 && nodes[row][column].obstacle_==false) + first_accessible_node = &nodes[row][column]; + } + } + std::cout << "found neighbors, corners: " << corner_nodes.size() << std::endl; + if (first_accessible_node == 0) + { + std::cout << "Warning: there are no accessible points in this room." << std::endl; + return; + } + +// // testing +// cv::Mat test_map = rotated_room_map.clone(); +// for (size_t i=0; i neighbors = nodes[i][j].neighbors_; +// for(std::vector::iterator n=neighbors.begin(); n!=neighbors.end(); ++n) +// cv::circle(test_map, (*n)->center_, 2, cv::Scalar(127), CV_FILLED); +// +// cv::imshow("neighbors", test_map); +// cv::waitKey(); +// } +// } + + // *********************** III. Plan the coverage path *********************** + // i. find the start node of the path as a corner that is closest to the starting position + std::vector starting_point_vector(1, starting_position); // opencv syntax + cv::transform(starting_point_vector, starting_point_vector, R); + const cv::Point rotated_starting_point = starting_point_vector[0]; // Point that keeps track of the last point after the boustrophedon path in each cell + EnergyExploratorNode* start_node = first_accessible_node; + double min_distance = 1e10; + for(std::vector::iterator corner=corner_nodes.begin(); corner!=corner_nodes.end(); ++corner) + { + cv::Point diff = (*corner)->center_ - rotated_starting_point; + double current_distance = diff.x*diff.x+diff.y*diff.y; + if(current_distance<=min_distance) + { + start_node = *corner; + min_distance = current_distance; + } + } + std::cout << "start node: " << start_node->center_ << std::endl; + + // insert start node into coverage path + std::vector fov_coverage_path; + fov_coverage_path.push_back(cv::Point2f(start_node->center_.x, start_node->center_.y)); + start_node->visited_ = true; // mark visited nodes as obstacles + + // ii. starting at the start node, find the coverage path, by choosing the node that min. the energy functional + EnergyExploratorNode* last_node = start_node; + double previous_travel_angle = 0; //always use x-direction in the rotated map //std::atan2(rotated_starting_point.y-start_node->center_.y, rotated_starting_point.x-start_node->center_.x); + for(std::vector::iterator neighbor=last_node->neighbors_.begin(); neighbor!=last_node->neighbors_.end(); ++neighbor) + { + if ((*neighbor)->obstacle_==false && (*neighbor)->center_.y==start_node->center_.y && (*neighbor)->center_.x>start_node->center_.x) + { + previous_travel_angle = 0; + break; + } + if ((*neighbor)->obstacle_==false && (*neighbor)->center_.y==start_node->center_.y && (*neighbor)->center_.xcenter_.x) + { + previous_travel_angle = PI; + break; + } + if ((*neighbor)->obstacle_==false && (*neighbor)->center_.ycenter_.y && (*neighbor)->center_.x==start_node->center_.x) + { + previous_travel_angle = -0.5*PI; + } + if ((*neighbor)->obstacle_==false && (*neighbor)->center_.y>start_node->center_.y && (*neighbor)->center_.x==start_node->center_.x) + { + previous_travel_angle = 0.5*PI; + } + } +// cv::Mat path_map = rotated_room_map.clone(); +// cv::circle(path_map, fov_coverage_path[0], 2, cv::Scalar(100), CV_FILLED); + do + { + //std::cout << "Point: " << last_node->center_ << std::endl; + // check the direct neighbors, if at least one is not already visited + std::vector not_visited_neighbors; + for(std::vector::iterator neighbor=last_node->neighbors_.begin(); neighbor!=last_node->neighbors_.end(); ++neighbor) + if ((*neighbor)->obstacle_ == false && (*neighbor)->visited_ == false) + not_visited_neighbors.push_back(*neighbor); + + // if there are not visited direct neighbors, find the one of them that minimizes the energy functional + double min_energy = 1e10; + EnergyExploratorNode* next_node = 0; + if (not_visited_neighbors.size() > 0) + { + // find best neighbor + for (std::vector::iterator candidate=not_visited_neighbors.begin(); candidate!=not_visited_neighbors.end(); ++candidate) + { + const double current_energy = E(*last_node, **candidate, grid_spacing_in_pixel, previous_travel_angle); + //std::cout << "Neighbor: " << (*candidate)->center_ << " energy: " << current_energy << std::endl; + if(current_energy < min_energy) + { + min_energy = current_energy; + next_node = *candidate; + } + } + } + // if no direct neighbor is unvisited, search for the next node in all unvisited nodes + else + { + // find best next node + for (size_t row=0; rowcenter_.y-last_node->center_.y, next_node->center_.x-last_node->center_.x); + fov_coverage_path.push_back(cv::Point2f(next_node->center_.x, next_node->center_.y)); + next_node->visited_ = true; // mark visited nodes as obstacles + +// cv::circle(path_map, next_node->center_, 2, cv::Scalar(100), CV_FILLED); +// cv::line(path_map, next_node->center_, last_node->center_, cv::Scalar(127)); +// cv::imshow("path", path_map); +// cv::waitKey(); + + last_node = next_node; + } while (true); + + // transform the calculated path back to the originally rotated map + std::vector fov_poses; + room_rotation.transformPathBackToOriginalRotation(fov_coverage_path, fov_poses, R); + +// // go trough the found fov-path and compute the angles of the poses s.t. it points to the next pose that should be visited +// for(unsigned int point_index=0; point_index::iterator pose=fov_poses.begin(); pose!=fov_poses.end(); ++pose) + { + geometry_msgs::Pose2D current_pose; + current_pose.x = (pose->x * map_resolution) + map_origin.x; + current_pose.y = (pose->y * map_resolution) + map_origin.y; + current_pose.theta = pose->theta; + path.push_back(current_pose); + } + return; + } + +// // testing +// cv::Mat path_map = rotated_room_map.clone(); +// cv::circle(path_map, fov_coverage_path[0], 2, cv::Scalar(100), CV_FILLED); +// for(std::vector::iterator path_node=fov_coverage_path.begin()+1; path_node!=fov_coverage_path.end(); ++path_node) +// { +// cv::circle(path_map, *path_node, 2, cv::Scalar(100), CV_FILLED); +// cv::line(path_map, *path_node, *(path_node-1), cv::Scalar(127)); +// cv::imshow("path", path_map); +// cv::waitKey(); +// } +// cv::imshow("path", path_map); +// cv::waitKey(); + + // ****************** IV. Map the found fov path to the robot path ****************** + //mapPath(room_map, path, fov_poses, robot_to_fov_vector, map_resolution, map_origin, starting_position); + ROS_INFO("Starting to map from field of view pose to robot pose"); + cv::Point robot_starting_position = (fov_poses.size()>0 ? cv::Point(fov_poses[0].x, fov_poses[0].y) : starting_position); + cv::Mat inflated_room_map; + cv::erode(room_map, inflated_room_map, cv::Mat(), cv::Point(-1, -1), half_grid_spacing_as_int); + mapPath(inflated_room_map, path, fov_poses, robot_to_fov_vector, map_resolution, map_origin, robot_starting_position); +} diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/common/src/flow_network_explorator.cpp b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/common/src/flow_network_explorator.cpp new file mode 100644 index 0000000..24a68cd --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/common/src/flow_network_explorator.cpp @@ -0,0 +1,2045 @@ +#include + +// Constructor +FlowNetworkExplorator::FlowNetworkExplorator() +{ + +} + +// Function that creates a Cbc optimization problem and solves it, using the given matrices and vectors and the 3-stage +// ansatz, that takes an initial step going from the start node and then a coverage stage assuming that the number of +// flows into and out of a node must be the same. At last a final stage is gone, that terminates the path in one of the +// possible nodes. +void FlowNetworkExplorator::solveThreeStageOptimizationProblem(std::vector& C, const cv::Mat& V, const std::vector& weights, + const std::vector >& flows_into_nodes, const std::vector >& flows_out_of_nodes, + const std::vector& start_arcs) +{ + // initialize the problem + CoinModel problem_builder; + + std::cout << "Creating and solving linear program." << std::endl; + + // add the optimization variables to the problem + int number_of_variables = 0; + for(size_t arc=0; arc variable_indices; + + for(size_t col=0; col(row, start_arcs[col])==1) + variable_indices.push_back((int) col); + + // coverage and final stage + for(size_t col=0; col(row, col)==1) + { + variable_indices.push_back((int) col + start_arcs.size()); // coverage stage + variable_indices.push_back((int) col + start_arcs.size() + V.cols); // final stage + } + } + + // all indices are 1 in this constraint + std::vector variable_coefficients(variable_indices.size(), 1.0); + + // add the constraint, if the current cell can be covered by the given arcs + if(variable_indices.size()>0) + problem_builder.addRow((int) variable_indices.size(), &variable_indices[0], &variable_coefficients[0], 1.0); + } + + + // equality constraint to ensure that the number of flows out of one node is the same as the number of flows into the + // node during the coverage stage + // Remark: for initial stage ensure that exactly one arc is gone, because there only the outgoing flows are taken + // into account + // initial stage + std::vector start_indices(start_arcs.size()); + std::vector start_coefficients(start_arcs.size()); + for(size_t start=0; start variable_indices; + std::vector variable_coefficients; + + // vectors for the decreasing equality constraint that ensures that the flow gets reduced by 1, every time it passes a node, + // cycles are prevented by this, because a start of a cycle is also an end of it, producing a constraint that the flow + // trough this node needs to be larger than from any other arc in this cycle but also it needs to be smaller than + // any other flow, which is not possible + std::vector flow_decrease_indices; + std::vector flow_decrease_coefficients; + + // vectors for the node indicator equality constraints that sets the indicator for the node to 1, if an arc flows + // into this node during the initial or coverage stage + std::vector indicator_indices; + std::vector indicator_coefficients; + + // gather flows into node + for(size_t inflow=0; inflow final_indices(V.cols); + std::vector final_coefficients(final_indices.size()); + // gather indices + for(size_t node=0; node aux_flow_indices(2); + std::vector aux_flow_coefficients(2); + + // first entry shows indication variable + aux_flow_indices[0] = node; + aux_flow_coefficients[0] = flows_into_nodes.size()-1; // allow a high flow if the arc is chosen in the path + + // second entry shows the flow variable + aux_flow_indices[1] = node+start_arcs.size()+2.0*V.cols; + aux_flow_coefficients[1] = -1.0; + + // add constraint + problem_builder.addRow((int) aux_flow_indices.size(), &aux_flow_indices[0], &aux_flow_coefficients[0], 0.0); + } + + // equality constraints to set the flow out of the start to the number of gone nodes + std::cout << "init flow constraints" << std::endl; + std::vector start_flow_indices(start_arcs.size()+flows_into_nodes.size()); + std::vector start_flow_coefficients(start_flow_indices.size()); + for(size_t node=0; nodeloadFromCoinModel(problem_builder); + + // testing + solver_pointer->writeLp("lin_flow_prog", "lp"); + + // solve the created integer optimization problem + CbcModel model(*solver_pointer); + model.solver()->setHintParam(OsiDoReducePrint, true, OsiHintTry); + +// CbcHeuristicLocal heuristic2(model); + CbcHeuristicFPump heuristic(model); + model.addHeuristic(&heuristic); + + model.initialSolve(); + model.branchAndBound(); + + // retrieve solution + const double* solution = model.solver()->getColSolution(); + + for(size_t res=0; res& C, const cv::Mat& V, const std::vector& weights, + const std::vector >& flows_into_nodes, const std::vector >& flows_out_of_nodes, + const std::vector& start_arcs) +{ +#ifdef GUROBI_FOUND + std::cout << "Creating and solving linear program with Gurobi." << std::endl; + // initialize the problem + GRBEnv *env = new GRBEnv(); + GRBModel model = GRBModel(*env); +// model.set("Threads", "1"); + + // one must set LazyConstraints parameter when using lazy constraints + model.set(GRB_IntParam_LazyConstraints, 1); + + // vector that stores the variables of the problem + std::vector optimization_variables; + + // add the optimization variables to the problem + int number_of_variables = 0; + for(size_t arc=0; arc variable_indices; + + for(size_t col=0; col(row, start_arcs[col])==1) + variable_indices.push_back((int) col); + + // coverage and final stage + for(size_t col=0; col(row, col)==1) + { + variable_indices.push_back((int) col + start_arcs.size()); // coverage stage + variable_indices.push_back((int) col + start_arcs.size() + V.cols); // final stage + } + } + + // add the constraint, if the current cell can be covered by the given arcs, indices=1 in this constraint + if(variable_indices.size()>0) + { + GRBLinExpr current_coverage_constraint; + for(size_t var=0; var=1); + } + } + + + // equality constraint to ensure that the number of flows out of one node is the same as the number of flows into the + // node during the coverage stage + // Remark: for initial stage ensure that exactly one arc is gone, because there only the outgoing flows are taken + // into account + // initial stage + GRBLinExpr initial_stage_constraint; + for(size_t start=0; start variable_indices; + std::vector variable_coefficients; + + // gather flows into node + for(size_t inflow=0; inflow0.01) +// std::cout << var << std::endl; + } + + // garbage collection + delete env; + + return; +#endif +} + +// Function that creates a Cbc optimization problem and solves it, using the given matrices and vectors and the 3-stage +// ansatz, that takes an initial step going from the start node and then a coverage stage assuming that the number of +// flows into and out of a node must be the same. At last a final stage is gone, that terminates the path in one of the +// possible nodes. This function uses lazy generalized cutset inequalities (GCI) to prevent cycles. For that a solution +// without cycle prevention constraints is determined and then cycles are detected in this solution. For these cycles +// then additional constraints are added and a new solution is determined. This procedure gets repeated until no cycle +// is detected in the solution or the only cycle contains all visited nodes, because such a solution is a traveling +// salesman like solution, which is a valid solution. +void FlowNetworkExplorator::solveLazyConstraintOptimizationProblem(std::vector& C, const cv::Mat& V, const std::vector& weights, + const std::vector >& flows_into_nodes, const std::vector >& flows_out_of_nodes, + const std::vector& start_arcs) +{ + // initialize the problem + CoinModel problem_builder; + + std::cout << "Creating and solving linear program." << std::endl; + + // add the optimization variables to the problem + int number_of_variables = 0; + for(size_t arc=0; arc variable_indices; + + for(size_t col=0; col(row, start_arcs[col])==1) + variable_indices.push_back((int) col); + + // coverage and final stage + for(size_t col=0; col(row, col)==1) + { + variable_indices.push_back((int) col + start_arcs.size()); // coverage stage + variable_indices.push_back((int) col + start_arcs.size() + V.cols); // final stage + } + } + + // all indices are 1 in this constraint + std::vector variable_coefficients(variable_indices.size(), 1.0); + + // add the constraint, if the current cell can be covered by the given arcs + if(variable_indices.size()>0) + problem_builder.addRow((int) variable_indices.size(), &variable_indices[0], &variable_coefficients[0], 1.0); + } + + + // equality constraint to ensure that the number of flows out of one node is the same as the number of flows into the + // node during the coverage stage + // Remark: for initial stage ensure that exactly one arc is gone, because there only the outgoing flows are taken + // into account + // initial stage + std::vector start_indices(start_arcs.size()); + std::vector start_coefficients(start_arcs.size()); + for(size_t start=0; start variable_indices; + std::vector variable_coefficients; + + // gather flows into node + for(size_t inflow=0; inflow final_indices(V.cols); + std::vector final_coefficients(final_indices.size()); + // gather indices + for(size_t node=0; nodeloadFromCoinModel(problem_builder); + + // testing + solver_pointer->writeLp("lin_flow_prog", "lp"); + + // solve the created integer optimization problem + CbcModel model(*solver_pointer); + model.solver()->setHintParam(OsiDoReducePrint, true, OsiHintTry); + + CbcHeuristicFPump heuristic(model); + model.addHeuristic(&heuristic); + model.initialSolve(); + model.branchAndBound(); + +// testing +// std::vector test_row(2); +// std::vector test_coeff(2); +// +// test_row[0] = 0; +// test_row[1] = 1; +// +// test_coeff[0] = 1.0; +// test_coeff[1] = 1.0; +// solver_pointer->addRow(2, &test_row[0], &test_coeff[0], 0.0, 0.0); +// solver_pointer->writeLp("lin_flow_prog", "lp"); +// solver_pointer->resolve(); + + // retrieve solution + const double* solution = model.solver()->getColSolution(); + + // search for cycles in the retrieved solution, if one is found add a constraint to prevent this cycle + bool cycle_free = false; + + do + { + // get the arcs that are used in the previously calculated solution + std::set used_arcs; // set that stores the indices of the arcs corresponding to non-zero elements in the solution + + // go trough the start arcs + for(size_t start_arc=0; start_arc0) + { + // insert saved outgoing flow index + used_arcs.insert(flow-start_arcs.size()-V.cols); + } + } +// go trough the final stage and find the remaining used arcs +// for(size_t node=0; node::iterator sol=used_arcs.begin(); sol!=used_arcs.end(); ++sol) +// std::cout << *sol << std::endl; +// std::cout << std::endl; + + // construct the directed edges out of the used arcs + std::vector > directed_edges; // vector that stores the directed edges for each node + for(uint start_node=0; start_node originating_flows; + bool originating = false; + for(std::set::iterator arc=used_arcs.begin(); arc!=used_arcs.end(); ++arc) + { + if(contains(flows_out_of_nodes[start_node], *arc)==true) + { + originating = true; + originating_flows.push_back(*arc); + } + } + + // if the arc is originating from this node, find its destination + std::vector current_directed_edges; + if(originating==true) + { + for(uint end_node=0; end_node::iterator arc=originating_flows.begin(); arc!=originating_flows.end(); ++arc) + if(contains(flows_into_nodes[end_node], *arc)==true) + current_directed_edges.push_back(end_node); + } + } + + // if the current node doesn't flow into another node insert a vector storing -1 + if(current_directed_edges.size()==0) + current_directed_edges.push_back(-1); + + // save the found used directed edges + directed_edges.push_back(current_directed_edges); + } + +// // testing +// std::cout << "used destinations: " << std::endl; +//// directed_edges[1].push_back(0); +// for(size_t i=0; i c(flows_into_nodes.size()); + int number_of_strong_components = boost::strong_components(support_graph, boost::make_iterator_property_map(c.begin(), boost::get(boost::vertex_index, support_graph), c[0])); + std::cout << "got " << number_of_strong_components << " strongly connected components" << std::endl; +// for (std::vector ::iterator i = c.begin(); i != c.end(); ++i) +// std::cout << "Vertex " << i - c.begin() << " is in component " << *i << std::endl; + + // check how many cycles there are in the solution (components with a size >= 2) + int number_of_cycles = 0; + std::set done_components; // set that stores the component indices for that the nodes already were found + for(std::vector::iterator comp=c.begin(); comp!=c.end(); ++comp) + { + // don't check a component more than one time + if(done_components.find(*comp)!=done_components.end()) + continue; + + int elements = std::count(c.begin(), c.end(), *comp); + if(elements>=2) + ++number_of_cycles; + + // check if a tsp path is computed (number of arcs is same as number of nodes), each arc is a strongly + // connected component itself or all the nodes belong to one strongly connected component + if(elements==used_arcs.size() || elements==flows_out_of_nodes.size()) + number_of_cycles = 0; + + // add it to done components + done_components.insert(*comp); + } + + // check if no cycle appears in the solution, i.e. if not each node is a component of its own or a traveling + // salesman path has been computed (not_used_nodes+1) or each arc flows to another node + if(number_of_cycles==0) + cycle_free = true; + + // if a cycle appears find it and add the prevention constraint to the problem and resolve it + if(cycle_free==false) + { + // go trough the components and find components with more than 1 element in it + std::vector > cycle_nodes; + done_components.clear(); + for (int component=0; component current_component_nodes; + int elements = std::count(c.begin(), c.end(), c[component]); +// std::cout << c[component] << std::endl; + if(elements>=2 && elements!=used_arcs.size()) + { + for(std::vector::iterator element=c.begin(); element!=c.end(); ++element) + { + if(*element==c[component]) + { + current_component_nodes.push_back(element-c.begin()); + } + } + + // save the found cycle + if(current_component_nodes.size()>0) + cycle_nodes.push_back(current_component_nodes); + + // add it to done components + done_components.insert(c[component]); + } + } + } + +// std::cout << "found nodes that are part of a cycle: " << std::endl; +// for(size_t i=0; i cpc_indices; +// std::vector cpc_coefficients; +// +// // gather the arcs in outgoing from all neighbors +// for(size_t neighbor=0; neighbor::const_iterator outflow=flows_out_of_nodes[cycle_nodes[cycle][neighbor]].begin(); outflow!=flows_out_of_nodes[cycle_nodes[cycle][neighbor]].end(); ++outflow) +// { +// // if the current arc is used in the solution, search for it in the incoming flows of +// // the other nodes in the cycle +// if(used_arcs.find(*outflow)!=used_arcs.end()) +// for(size_t new_node=0; new_node::const_iterator outflow=flows_out_of_nodes[cycle_nodes[cycle][neighbor]].begin(); outflow!=flows_out_of_nodes[cycle_nodes[cycle][neighbor]].end(); ++outflow) +// { +// // search for the current flow in the incoming flows of the other nodes in the cycle +// for(size_t new_node=0; new_nodeaddRow((int) cpc_indices.size(), &cpc_indices[0], &cpc_coefficients[0], 0.0, COIN_DBL_MAX); +// } + // for each cycle find the arcs that lie in it + +// std::cout << "size: " << cycle_nodes[cycle].size() << std::endl; +// std::cout << "constraint: " << std::endl; + std::vector cpc_indices; + std::vector cpc_coefficients; + for(size_t node=0; node::const_iterator outflow=flows_out_of_nodes[cycle_nodes[cycle][node]].begin(); outflow!=flows_out_of_nodes[cycle_nodes[cycle][node]].end(); ++outflow) + { + for(size_t neighbor=0; neighboraddRow((int) cpc_indices.size(), &cpc_indices[0], &cpc_coefficients[0], COIN_DBL_MIN , cycle_nodes[cycle].size()-1); + } + +// testing + solver_pointer->writeLp("lin_flow_prog", "lp"); + + // resolve the problem with the new constraints + solver_pointer->resolve(); + + // create a new model with the updated optimization problem and solve it + CbcModel new_model(*solver_pointer); + new_model.solver()->setHintParam(OsiDoReducePrint, true, OsiHintTry); + + CbcHeuristicFPump heuristic_new(new_model); + new_model.addHeuristic(&heuristic_new); + +// new_model.initialSolve(); + new_model.branchAndBound(); + + // retrieve new solution + solution = new_model.solver()->getColSolution(); + + } + }while(cycle_free == false); + + for(size_t res=0; res& points, const cv::Point& point, const double min_distance) +{ + double square_distance = min_distance * min_distance; + for(std::vector::const_iterator current_point = points.begin(); current_point != points.end(); ++current_point) + { + double dx = current_point->x - point.x; + double dy = current_point->y - point.y; + if( ((dx*dx + dy*dy)) <= square_distance) + return true; + } + return false; +} + +// Function that uses the flow network based method to determine a coverage path. To do so the following steps are done +// I. Using the Sobel operator the direction of the gradient at each pixel is computed. Using this information, the direction is +// found that suits best for calculating the edges, i.e. such that longer edges occur, and the map is rotated in this manner. +// This allows to use the algorithm as it was and in the last step, the found path points simply will be transformed back to the +// original orientation. +// II. Discretize the free space into cells that have to be visited a least once by using the sampling distance given to +// the function. Also create a flow network by sweeping a line along the y-/x-axis and creating an edge, whenever it +// hits an obstacle. From this hit point go back along the sweep line until the distance is equal to the coverage +// radius, because the free space should represent the area that should be totally covered. If in both directions +// along the sweep line no point in the free space can be found, ignore it. +// III. Create the matrices and vectors for the optimization problem: +// 1. The weight vector w, storing the distances between edges. +// 2. The coverage matrix V, storing which cell can be covered when going along the arcs. +// remark: A cell counts as covered, when its center is in the coverage radius around an arc. +// 3. The sets of arcs for each node, that store the incoming and outgoing arcs +// IV. Create and solve the optimization problems in the following order: +// 1. Find the start node that is closest to the given starting position. This start node is used as initial step +// in the optimization problem. +// 2. Solve the optimization problem, using lazy constraints to prevent cycles in the solution. See the upper +// function for further details. +// 3. Retrieve the solution provided by the optimizer and create a path trough the environment. +// 4. Construct a pose path out of the calculated point path. +// V. The previous step produces a path for the field of view. If wanted this path gets mapped to the robot path s.t. +// the field of view follows the wanted path. To do so simply a vector transformation is applied. If the computed robot +// pose is not in the free space, another accessible point is generated by finding it on the radius around the fov +// middlepoint s.t. the distance to the last robot position is minimized. If this is not wanted one has to set the +// corresponding Boolean to false (shows that the path planning should be done for the robot footprint). +void FlowNetworkExplorator::getExplorationPath(const cv::Mat& room_map, std::vector& path, + const float map_resolution, const cv::Point starting_position, const cv::Point2d map_origin, + const int cell_size, const Eigen::Matrix& robot_to_fov_middlepoint_vector, const float coverage_radius, + const bool plan_for_footprint, const double path_eps, const double curvature_factor, const double max_distance_factor) +{ + // *********************** I. Find the main directions of the map and rotate it in this manner. *********************** + cv::Mat R; + cv::Rect bbox; + cv::Mat rotated_room_map; + RoomRotator room_rotation; + room_rotation.computeRoomRotationMatrix(room_map, R, bbox, map_resolution); + room_rotation.rotateRoom(room_map, rotated_room_map, R, bbox); + +// // generate matrices for gradient in x/y direction +// cv::Mat gradient_x, gradient_y; +// +// // compute gradient in x direction +// cv::Sobel(room_map, gradient_x, CV_64F, 1, 0, 5, 1.0, 0.0, cv::BORDER_DEFAULT); +// +// // compute gradient in y direction +// cv::Sobel(room_map, gradient_y, CV_64F, 0, 1, 5, 1.0, 0.0, cv::BORDER_DEFAULT); +// +// // compute the direction of the gradient for each pixel and save the occurring gradients +// std::vector gradient_directions; +// for(size_t y=0; y(y,x); +// int dy= gradient_y.at(y,x); +// if(dy*dy+dx*dx > 0.0) +// { +// double current_gradient = std::atan2(dy, dx); +// gradient_directions.push_back(0.1*(double)((int)((current_gradient*10)+0.5))); // round to one digit +// } +// } +// } +// +// // find the gradient that occurs most often, this direction is used to rotate the map +// int max_number = 0; +// double most_occurring_gradient = 0.0; +// std::set done_gradients; +// for(std::vector::iterator grad=gradient_directions.begin(); grad!=gradient_directions.end(); ++grad) +// { +// // if gradient has been done, don't check it again +// if(done_gradients.find(*grad)==done_gradients.end()) +// { +// int current_count = std::count(gradient_directions.begin(), gradient_directions.end(), *grad); +//// std::cout << "current gradient: " << *grad << ", occurs " << current_count << " times." << std::endl; +// if(current_count > max_number) +// { +// max_number = current_count; +// most_occurring_gradient = *grad; +// } +// done_gradients.insert(*grad); +// } +// } +// std::cout << "most occurring gradient angle: " << most_occurring_gradient << std::endl; +// +// // rotation angle of the map s.t. the most occurring gradient is in 0 degree to the x-axis +// double rotation_angle = std::abs(most_occurring_gradient); +// std::cout << "rotation angle: " << rotation_angle << std::endl; +// +// // get rotation matrix R for rotating the image around the center of the room contour +// // Remark: rotation angle in degrees for opencv +// std::vector < std::vector > contour; +// cv::Mat contour_map = room_map.clone(); +// cv::findContours(contour_map, contour, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE); +// +// // get the moment--> for a given map, there should only be one contour +// cv::Moments moment = cv::moments(contour[0], false); +// +// // calculate rotation center +// cv::Point2f center = cv::Point(moment.m10/moment.m00 , moment.m01/moment.m00 ); +// cv::Mat R = cv::getRotationMatrix2D(center, (rotation_angle*180)/PI, 1.0); +// +// // determine bounding rectangle to find the size of the new image +// cv::Rect bbox = cv::RotatedRect(center, room_map.size(), (rotation_angle*180)/PI).boundingRect(); +// +// // adjust transformation matrix +// R.at(0,2) += bbox.width/2.0 - center.x; +// R.at(1,2) += bbox.height/2.0 - center.y; +// +// // rotate the image +// cv::Mat rotated_room_map; +// cv::warpAffine(room_map, rotated_room_map, R, bbox.size(), cv::INTER_AREA); +// +// // apply a binary filter to create a binary image, also use a closing operator to smooth the output (the rotation might produce +// // black pixels reaching into the white area that were not there before, causing new, wrong cells to open) +// cv::threshold(rotated_room_map, rotated_room_map, 200, 255, CV_THRESH_BINARY); +// cv::dilate(rotated_room_map, rotated_room_map, cv::Mat(), cv::Point(-1,-1), 1); +// cv::erode(rotated_room_map, rotated_room_map, cv::Mat(), cv::Point(-1,-1), 1); +// cv::imshow("rotated_room_map", rotated_room_map); +// cv::imshow("room_map", room_map); +// cv::waitKey(); + + // *********** II. Discretize the free space and create the flow network *********** + // find the min/max coordinates + int min_y = 1000000, max_y = 0, min_x = 1000000, max_x = 0; + for (int y=0; y(y,x)==255) + { + if(ymax_y) + max_y = y; + if(xmax_x) + max_x = x; + } + } + } +// min_y -= 1; +// min_x -= 1; +// max_y += 1; +// max_x += 1; + +// testing +// cv::circle(rotated_room_map, cv::Point(min_x, min_y), 3, cv::Scalar(127), CV_FILLED); +// cv::circle(rotated_room_map, cv::Point(max_x, max_y), 3, cv::Scalar(127), CV_FILLED); +// cv::imshow("rotated", rotated_room_map); +// cv::waitKey(); + + // todo: create grid in external class - it is the same in all approaches + // todo: if first/last row or column in grid has accessible areas but center is inaccessible, create a node in the accessible area + // find cell centers that need to be covered + std::vector cell_centers; + for(size_t y=min_y; y<=max_y; y+=cell_size) + for(size_t x=min_x; x<=max_x; x+=cell_size) + if(rotated_room_map.at(y,x)==255) + cell_centers.push_back(cv::Point(x,y)); + + // find edges for the flow network, sweeping along the y-axis + std::vector edges; + int coverage_int = (int) std::floor(coverage_radius); + std::cout << "y sweeping, radius: " << coverage_int << std::endl; + for(size_t y=min_y-1; y<=max_y+1; ++y) + { +// cv::Mat test_map = rotated_room_map.clone(); +// for(std::vector::iterator center=cell_centers.begin(); center!=cell_centers.end(); ++center) +// cv::circle(test_map, *center, 2, cv::Scalar(50), CV_FILLED); +// cv::line(test_map, cv::Point(0, y), cv::Point(rotated_room_map.cols, y), cv::Scalar(127), 1); + for(size_t x=min_x; x<=max_x+1; x+=2.0*coverage_int) + { + // check if an obstacle has been found, only check outer parts of the occupied space + if(rotated_room_map.at(y,x)==0 && (rotated_room_map.at(y-1,x)==255 || rotated_room_map.at(y+1,x)==255)) + { +// cv::circle(test_map, cv::Point(x,y), 2, cv::Scalar(127), CV_FILLED); + // check on both sides along the sweep line if a free point is available, don't exceed matrix dimensions + if(rotated_room_map.at(y-coverage_int, x)==255 && y-coverage_int>=0) + edges.push_back(cv::Point(x, y-coverage_int)); + else if(rotated_room_map.at(y+coverage_int, x)==255 && y+coverage_int(y,x)==0 && (rotated_room_map.at(y,x-1)==255 || rotated_room_map.at(y,x+1)==255)) +// { +//// cv::circle(test_map, cv::Point(x,y), 2, cv::Scalar(127), CV_FILLED); +// // check on both sides along the sweep line if a free point is available, don't exceed matrix dimensions +// if(rotated_room_map.at(y, x-coverage_int)==255 && x-coverage_int>=0) +// edges.push_back(cv::Point(x-coverage_int, y)); +// else if(rotated_room_map.at(y, x+coverage_int)==255 && x+coverage_int::iterator p=edges.begin(); p!=edges.end(); ++p) +// cv::circle(edges_map, *p, 2, cv::Scalar(100), CV_FILLED); +// cv::imshow("edges", edges_map); +// cv::waitKey(); + + // create the arcs for the flow network + std::cout << "Constructing distance matrix" << std::endl; + cv::Mat distance_matrix; // determine weights + DistanceMatrix distance_matrix_computation; + distance_matrix_computation.constructDistanceMatrix(distance_matrix, rotated_room_map, edges, 0.25, 0.0, map_resolution, path_planner_); + std::cout << "Constructed distance matrix, defining arcs" << std::endl; + std::vector arcs; + double max_distance = max_y - min_y; // arcs should at least go the maximal room distance to allow straight arcs + if(max_x-min_x>max_distance) + max_distance=max_x-min_x; + for(size_t start=0; startstart) + { + arcStruct current_forward_arc; + current_forward_arc.start_point = edges[start]; + current_forward_arc.end_point = edges[end]; + current_forward_arc.weight = distance_matrix.at(start, end); + arcStruct current_backward_arc; + current_backward_arc.start_point = edges[end]; + current_backward_arc.end_point = edges[start]; + current_backward_arc.weight = distance_matrix.at(end, start); + cv::Point vector = current_forward_arc.start_point - current_forward_arc.end_point; + // don't add too long arcs to reduce dimensionality, because they certainly won't get chosen anyway + // also don't add arcs that are too far away from the straight line (start-end) because they are likely + // to go completely around obstacles and are not good + if(current_forward_arc.weight <= max_distance_factor*max_distance && current_forward_arc.weight <= curvature_factor*cv::norm(vector)) + { + std::vector astar_path; + path_planner_.planPath(rotated_room_map, current_forward_arc.start_point, current_forward_arc.end_point, 1.0, 0.0, map_resolution, 0, &astar_path); + current_forward_arc.edge_points = astar_path; + // reverse path for backward arc + std::reverse(astar_path.begin(), astar_path.end()); + current_backward_arc.edge_points = astar_path; + arcs.push_back(current_forward_arc); + arcs.push_back(current_backward_arc); + } + } + } + } + // TODO: exclude nodes that aren't connected to the rest of the edges + std::cout << "arcs: " << arcs.size() << std::endl; + +// testing +// cv::Mat arc_map = rotated_room_map.clone(); +// for(size_t i=0; i(current_arc.edge_points[j]) = 127; +// } +// cv::imshow("test", arc_map); +// cv::waitKey(); + + // *********** III. Construct the matrices for the optimization problem *********** + std::cout << "Starting to construct the matrices for the optimization problem." << std::endl; + // 1. weight vector + int number_of_candidates = arcs.size(); + std::vector w(number_of_candidates); + for(std::vector::iterator arc=arcs.begin(); arc!=arcs.end(); ++arc) + w[arc-arcs.begin()] = arc->weight; + + // 2. visibility matrix, storing which call can be covered when going along the arc + // remark: a cell counts as covered, when the center of each cell is in the coverage radius around the arc + cv::Mat V = cv::Mat(cell_centers.size(), number_of_candidates, CV_8U); // binary variables + for(std::vector::iterator arc=arcs.begin(); arc!=arcs.end(); ++arc) + { + // use the pointClose function to check if a cell can be covered along the path + for(std::vector::iterator cell=cell_centers.begin(); cell!=cell_centers.end(); ++cell) + { + if(pointClose(arc->edge_points, *cell, 1.1*coverage_radius) == true) + V.at(cell-cell_centers.begin(), arc-arcs.begin()) = 1; + else + V.at(cell-cell_centers.begin(), arc-arcs.begin()) = 0; + } + } + + // 3. set of arcs (indices) that are going into and out of one node + std::vector > flows_into_nodes(edges.size()); + std::vector > flows_out_of_nodes(edges.size()); + int number_of_outflows = 0; + for(std::vector::iterator edge=edges.begin(); edge!=edges.end(); ++edge) + { + for(std::vector::iterator arc=arcs.begin(); arc!=arcs.end(); ++arc) + { + // if the start point of the arc is the edge save it as incoming flow + if(arc->end_point == *edge) + { + flows_into_nodes[edge-edges.begin()].push_back(arc-arcs.begin()); + } + // if the end point of the arc is the edge save it as outgoing flow + else if(arc->start_point == *edge) + { + flows_out_of_nodes[edge-edges.begin()].push_back(arc-arcs.begin()); + ++number_of_outflows; + } + } + } + +// testing +// for(size_t i=0; i path = arcs[flows_out_of_nodes[node][flow]].edge_points; +// for(size_t p=0; p(path[p]) = 127; +// } +// cv::imshow("paths", paths); +// cv::waitKey(); +// } + + std::cout << "Constructed all matrices for the optimization problem. Checking if all cells can be covered." << std::endl; + + // print out warning if a defined cell is not coverable with the chosen arcs + bool all_cells_covered = true; + for(size_t row=0; row(row, col)==1) + ++number_of_paths; + if(number_of_paths==0) + { + std::cout << "!!!!!!!! EMPTY ROW OF VISIBILITY MATRIX !!!!!!!!!!!!!" << std::endl << "cell " << row << " not coverable" << std::endl; + all_cells_covered = false; + } + } + if(all_cells_covered == false) + std::cout << "!!!!! WARNING: Not all cells could be covered with the given parameters, try changing them or ignore it to not cover the whole free space." << std::endl; + + // *********** IV. Solve the optimization problem *********** + // 1. Find the start node closest to the starting position. + double min_distance = 1e5; + uint start_index = 0; + for(std::vector::iterator edge=edges.begin(); edge!=edges.end(); ++edge) + { + cv::Point difference_vector = *edge - starting_position; + double current_distance = cv::norm(difference_vector); + if(current_distance C(2.0*(flows_out_of_nodes[start_index].size()+number_of_candidates) + number_of_outflows + edges.size()); + std::cout << "number of outgoing arcs: " << number_of_outflows << std::endl; +#ifdef GUROBI_FOUND + solveGurobiOptimizationProblem(C, V, w, flows_into_nodes, flows_out_of_nodes, flows_out_of_nodes[start_index]); +#else + solveLazyConstraintOptimizationProblem(C, V, w, flows_into_nodes, flows_out_of_nodes, flows_out_of_nodes[start_index]); +#endif + +// testing +// for(size_t i=0; i::iterator p=edges.begin(); p!=edges.end(); ++p) +// cv::circle(test_map, *p, 2, cv::Scalar(100), CV_FILLED); + + std::set used_arcs; // set that stores the indices of the arcs corresponding to non-zero elements in the solution + // go trough the start arcs and determine the new start arcs + uint path_start = 0; +// cv::Mat test_map = rotated_room_map.clone(); +// for(std::vector::iterator p=edges.begin(); p!=edges.end(); ++p) +// cv::circle(test_map, *p, 2, cv::Scalar(100), CV_FILLED); + for(size_t start_arc=0; start_arc0.01) // taking integer precision in solver into account + { + // insert start index +// used_arcs.insert(flows_out_of_nodes[start_index][start_arc]); + path_start = flows_out_of_nodes[start_index][start_arc]; + +// std::vector path=arcs[flows_out_of_nodes[start_index][start_arc]].edge_points; +// for(size_t j=0; j(path[j])=50; +// +// cv::imshow("discretized", test_map); +// cv::waitKey(); + } + } + + // go trough the coverage stage + for(size_t cover_arc=flows_out_of_nodes[start_index].size(); cover_arc::iterator p=edges.begin(); p!=edges.end(); ++p) +// cv::circle(test_map, *p, 2, cv::Scalar(100), CV_FILLED); + if(C[cover_arc]>0.01) // taking integer precision in solver into account + { +// std::cout << cover_arc-flows_out_of_nodes[start_index].size() << std::endl; + + // insert index, relative to the first coverage variable + used_arcs.insert(cover_arc-flows_out_of_nodes[start_index].size()); + +// std::vector path=arcs[cover_arc-flows_out_of_nodes[start_index].size()].edge_points; +// for(size_t j=0; j(path[j])=100; +// +// cv::imshow("discretized", test_map); +// cv::waitKey(); + } + } + + // go trough the final stage and find the remaining used arcs + std::cout << "final: " << std::endl; + uint path_end = 0; + for(uint final_arc=flows_out_of_nodes[start_index].size()+arcs.size(); final_arc::iterator p=edges.begin(); p!=edges.end(); ++p) +// cv::circle(test_map, *p, 2, cv::Scalar(100), CV_FILLED); + if(C[final_arc]>0.01) + { + // insert saved outgoing flow index +// used_arcs.insert(final_arc-flows_out_of_nodes[start_index].size()-V.cols); + path_end = final_arc-flows_out_of_nodes[start_index].size()-V.cols; + +// std::vector path=arcs[final_arc-flows_out_of_nodes[start_index].size()-arcs.size()].edge_points; +// for(size_t j=0; j(path[j])=150; +// +// cv::imshow("discretized", test_map); +// cv::waitKey(); + } + } +// for(size_t node=0; node0.01) // taking integer precision in solver into account +// { +// // insert saved outgoing flow index +// used_arcs.insert(flows_out_of_nodes[node][flow]); +// std::vector path=arcs[flows_out_of_nodes[node][flow]].edge_points; +// for(size_t j=0; j(path[j])=150; +// +// cv::imshow("discretized", test_map); +// cv::waitKey(); +// } +// } +// } +// cv::imshow("discretized", test_map); +// cv::waitKey(); + std::cout << "got " << used_arcs.size() << " used arcs" << std::endl; + +// // testing --> check how often a node is a start/end-node of the arcs +// std::cout << "appereances of the nodes: " << std::endl; +// cv::Mat node_map = rotated_room_map.clone(); +// for(size_t i=0; i::iterator used=used_arcs.begin(); used!=used_arcs.end(); ++used) +// if(contains(flows_out_of_nodes[i], *used)==true || contains(flows_into_nodes[i], *used)==true) +// ++number; +// std::cout << "n" << i << ": " << number << std::endl; +// +// if(i==5) +// { +// cv::circle(node_map, edges[i], 2, cv::Scalar(127), CV_FILLED); +// for(std::set::iterator used=used_arcs.begin(); used!=used_arcs.end(); ++used) +// { +// if(contains(flows_out_of_nodes[i], *used)==true || contains(flows_into_nodes[i], *used)==true) +// { +// std::vector points=arcs[*used].edge_points; +// for(size_t j=0; j(points[j])=150; +// } +// } +// cv::imshow("node", node_map); +// cv::waitKey(); +// } +// } + + + // starting from the start node, go trough the arcs and create a coverage path + std::vector path_positions; + path_positions.push_back(edges[start_index]); + cv::Point last_point = edges[start_index]; + int last_index = start_index; + std::set gone_arcs; + std::cout << "getting path using arcs" << std::endl; + // start path at start node + std::vector start_edge = arcs[path_start].edge_points; + for(std::vector::iterator pos=start_edge.begin(); pos!=start_edge.end(); ++pos) + { + cv::Point difference = last_point - *pos; + // if the next point is far enough away from the last point insert it into the coverage path + if(difference.x*difference.x+difference.y*difference.y<=path_eps*path_eps) + { + path_positions.push_back(*pos); + last_point = *pos; + } + } + // get index of the start arcs end-node + cv::Point end_start_node = arcs[path_start].end_point; + last_index = std::find(edges.begin(), edges.end(), end_start_node)-edges.begin(); + // TODO: find path in directed graph, covering all edges --> allow cycles connected to the rest + int number_of_gone_arcs = 0, loopcounter = 0; + do + { + ++loopcounter; +// std::cout << "n: " << number_of_gone_arcs << std::endl; + // go trough the arcs and find the one that has the last point as begin and go along it + for(std::set::iterator arc_index=used_arcs.begin(); arc_index!=used_arcs.end(); ++arc_index) + { +// std::cout << arcs[*arc_index].start_point << " " << last_point << std::endl; +// if(arcs[*arc_index].start_point==last_point) +// { +// std::vector edge = arcs[*arc_index].edge_points; +// for(std::vector::iterator pos=edge.begin(); pos!=edge.end(); ++pos) +// { +// cv::Point difference = last_point - *pos; +// // if the next point is far enough away from the last point insert it into the coverage path +// if(difference.x*difference.x+difference.y*difference.y<=path_eps*path_eps) +// { +// path_positions.push_back(*pos); +// last_point = *pos; +// } +// } +// +// // increase number of gone arcs +// ++number_of_gone_arcs; +// } + + // check if current arc has been gone already (e.g. when a cycle is appended to the rest of the path --> still a valid solution) + if(gone_arcs.find(*arc_index)!=gone_arcs.end()) + continue; + + if(contains(flows_out_of_nodes[last_index], *arc_index)==true) + { + std::vector edge = arcs[*arc_index].edge_points; + for(std::vector::iterator pos=edge.begin(); pos!=edge.end(); ++pos) + { + cv::Point difference = last_point - *pos; + // if the next point is far enough away from the last point insert it into the coverage path + if(difference.x*difference.x+difference.y*difference.y<=path_eps*path_eps) + { + path_positions.push_back(*pos); + last_point = *pos; + } + } + + // get index of last edge + cv::Point node = arcs[*arc_index].end_point; + last_index = std::find(edges.begin(), edges.end(), node)-edges.begin(); + gone_arcs.insert(*arc_index); + + // increase number of gone arcs + ++number_of_gone_arcs; + + // reset the loop-counter + loopcounter = 0; + } +// std::cout << number_of_gone_arcs << std::endl; + } + }while(number_of_gone_arcs final_edge = arcs[path_end].edge_points; + for(std::vector::iterator pos=final_edge.begin(); pos!=final_edge.end(); ++pos) + { + cv::Point difference = last_point - *pos; + // if the next point is far enough away from the last point insert it into the coverage path + if(difference.x*difference.x+difference.y*difference.y<=path_eps*path_eps) + { + path_positions.push_back(*pos); + last_point = *pos; + } + } + std::cout << "got path" << std::endl; + + // transform the calculated path back to the originally rotated map and create poses with an angle + std::vector fov_poses; + std::vector path_positions_2f(path_positions.size()); + for (size_t i=0; i fov_poses; +// for(unsigned int point_index=0; point_index::iterator pose=fov_poses.begin(); pose != fov_poses.end(); ++pose) + { + geometry_msgs::Pose2D current_pose; + current_pose.x = (pose->x * map_resolution) + map_origin.x; + current_pose.y = (pose->y * map_resolution) + map_origin.y; + current_pose.theta = pose->theta; + path.push_back(current_pose); + } + return; + } + + // *********************** V. Get the robot path out of the fov path. *********************** + // clean path from double occurrences of the same pose in a row + std::vector fov_path; + fov_path.push_back(fov_poses[0]); + cv::Point last_added_point(fov_poses[0].x, fov_poses[0].y); + const double min_dist_squared = 5 * 5; // [pixel] + for (size_t i=1; i min_dist_squared || i==fov_poses.size()-1) + { + fov_path.push_back(fov_poses[i]); + last_added_point = current_point; + } + } + + // go trough all computed fov poses and compute the corresponding robot pose + std::cout << "mapping path" << std::endl; + mapPath(room_map, path, fov_path, robot_to_fov_middlepoint_vector, map_resolution, map_origin, starting_position); + +//// testing +// // transform the found path back to the original map +// cv::invertAffineTransform(R, R); +// cv::transform(path_positions, path_positions, R); +// cv::Mat path_map = room_map.clone(); +//// cv::imshow("solution", test_map); +//// cv::imwrite("/home/rmbce/Pictures/room_exploration/coverage_path.png", test_map); +// for(std::vector::iterator point=path_positions.begin(); point!=path_positions.end(); ++point) +// { +// cv::circle(path_map, *point, 2, cv::Scalar(127), CV_FILLED); +//// cv::imshow("path", path_map); +//// cv::waitKey(); +// } +// cv::imshow("path", path_map); +// cv::waitKey(); +} + +// test function for an easy case to check correctness +void FlowNetworkExplorator::testFunc() +{ +// std::vector w(6, 1.0); +// std::vector C(2+6+6); +// cv::Mat V = cv::Mat(8, 6, CV_8U, cv::Scalar(0)); +// std::vector > flows_out_of_nodes(3); +// std::vector > flows_in_nodes(3); +// +// // cell 1 +// V.at(0,0) = 1; +// V.at(0,1) = 1; +// //cell 2 +// V.at(1,0) = 1; +// V.at(1,1) = 1; +// // cell 3 +// V.at(2,4) = 1; +// V.at(2,5) = 1; +// // cell 4 +// V.at(3,0) = 1; +// V.at(3,1) = 1; +// V.at(3,4) = 1; +// V.at(3,5) = 1; +// // cell 5 +// V.at(4,0) = 1; +// V.at(4,1) = 1; +// V.at(4,2) = 1; +// V.at(4,3) = 1; +// // cell 6 +// V.at(5,2) = 1; +// V.at(5,3) = 1; +// // cell 7 +// V.at(6,4) = 1; +// V.at(6,5) = 1; +// // cell 8 +// V.at(7,2) = 1; +// V.at(7,3) = 1; +// +// flows_out_of_nodes[0].push_back(0); +// flows_out_of_nodes[0].push_back(4); +// flows_out_of_nodes[1].push_back(1); +// flows_out_of_nodes[1].push_back(2); +// flows_out_of_nodes[2].push_back(3); +// flows_out_of_nodes[2].push_back(5); +// +// flows_in_nodes[0].push_back(1); +// flows_in_nodes[0].push_back(5); +// flows_in_nodes[1].push_back(0); +// flows_in_nodes[1].push_back(3); +// flows_in_nodes[2].push_back(2); +// flows_in_nodes[2].push_back(4); +// +// for(size_t row=0; row(row, col) << " "; +// } +// std::cout << std::endl; +// } + std::vector w(14, 1.0); + std::vector C(2+14+14+2+14+6); + cv::Mat V = cv::Mat(12, 14, CV_8U, cv::Scalar(0)); + std::vector > flows_out_of_nodes(6); + std::vector > flows_in_nodes(6); + + // cell 1 + V.at(0,0) = 1; + V.at(0,1) = 1; + //cell 2 + V.at(1,0) = 1; + V.at(1,1) = 1; + V.at(1,4) = 1; + V.at(1,5) = 1; + // cell 3 + V.at(2,4) = 1; + V.at(2,5) = 1; + V.at(2,10) = 1; + V.at(2,11) = 1; + // cell 4 + V.at(3,0) = 1; + V.at(3,1) = 1; + // cell 5 + V.at(4,0) = 1; + V.at(4,1) = 1; + V.at(4,6) = 1; + V.at(4,7) = 1; + // cell 6 + V.at(5,6) = 1; + V.at(5,7) = 1; + V.at(5,10) = 1; + V.at(5,11) = 1; + // cell 7 + V.at(6,2) = 1; + V.at(6,3) = 1; + // cell 8 + V.at(7,2) = 1; + V.at(7,3) = 1; + V.at(7,6) = 1; + V.at(7,7) = 1; + // cell 9 + V.at(8,6) = 1; + V.at(8,7) = 1; + V.at(8,12) = 1; + V.at(8,13) = 1; + // cell 10 + V.at(9,2) = 1; + V.at(9,3) = 1; + // cell 11 + V.at(10,2) = 1; + V.at(10,3) = 1; + V.at(10,8) = 1; + V.at(10,9) = 1; + // cell 12 + V.at(11,8) = 1; + V.at(11,9) = 1; + V.at(11,12) = 1; + V.at(11,13) = 1; + + flows_out_of_nodes[0].push_back(0); + flows_out_of_nodes[0].push_back(4); + + flows_out_of_nodes[1].push_back(1); + flows_out_of_nodes[1].push_back(2); + flows_out_of_nodes[1].push_back(6); + + flows_out_of_nodes[2].push_back(3); + flows_out_of_nodes[2].push_back(8); + + flows_out_of_nodes[3].push_back(5); + flows_out_of_nodes[3].push_back(10); + + flows_out_of_nodes[4].push_back(7); + flows_out_of_nodes[4].push_back(11); + flows_out_of_nodes[4].push_back(12); + + flows_out_of_nodes[5].push_back(9); + flows_out_of_nodes[5].push_back(13); + + + flows_in_nodes[0].push_back(1); + flows_in_nodes[0].push_back(5); + + flows_in_nodes[1].push_back(0); + flows_in_nodes[1].push_back(3); + flows_in_nodes[1].push_back(7); + + flows_in_nodes[2].push_back(2); + flows_in_nodes[2].push_back(9); + + flows_in_nodes[3].push_back(4); + flows_in_nodes[3].push_back(11); + + flows_in_nodes[4].push_back(6); + flows_in_nodes[4].push_back(10); + flows_in_nodes[4].push_back(13); + + flows_in_nodes[5].push_back(8); + flows_in_nodes[5].push_back(12); + + for(size_t row=0; row(row, col) << " "; + } + std::cout << std::endl; + } + + std::vector W(C.size(), 1.0); + w[10] = 0.25; + w[11] = 0.25; + w[12] = 0.25; + w[13] = 0.25; + double weight_epsilon = 0.0; + double euler_constant = std::exp(1.0); + for(size_t i=1; i<=1; ++i) + { + +// solveThreeStageOptimizationProblem(C, V, w, flows_in_nodes, flows_out_of_nodes, flows_out_of_nodes[0]);//, &W); + solveGurobiOptimizationProblem(C, V, w, flows_in_nodes, flows_out_of_nodes, flows_out_of_nodes[0]); + for(size_t c=0; c used_arcs; // set that stores the indices of the arcs corresponding to non-zero elements in the solution + // go trough the start arcs and determine the new start arcs + std::cout << "initial: " << std::endl; + uint n = (uint) V.cols; + for(size_t start_arc=0; start_arc0) + { + // insert saved outgoing flow index + used_arcs.insert(flow-flows_out_of_nodes[0].size()-n); + } + } +// for(size_t node=0; node::iterator var=used_arcs.begin(); var!=used_arcs.end(); ++var) + { + // gather column corresponding to this candidate pose and add it to the new observability matrix + cv::Mat column = V.col(*var); + cv::hconcat(V_reduced, column, V_reduced); + } + V_reduced = V_reduced.colRange(1, V_reduced.cols); + + for(size_t row=0; row(row, col) << " "; + if(V_reduced.at(row, col)!=0) + ++one_count; + } + std::cout << std::endl; + if(one_count == 0) + std::cout << "!!!!!!!!!!!!! empty row !!!!!!!!!!!!!!!!!!" << std::endl; + } + + V_reduced = V_reduced.colRange(1, V_reduced.cols); + + std::cout << "read out: " << std::endl; + for(size_t c=0; cloadFromCoinModel(problem_builder); + + solver_pointer->writeLp("test_lp", "lp"); + + CbcModel model(*solver_pointer); + model.solver()->setHintParam(OsiDoReducePrint, true, OsiHintTry); + +// CbcHeuristicLocal heuristic2(model); +// model.addHeuristic(&heuristic2); + + model.initialSolve(); + model.branchAndBound(); + + const double * solution = model.solver()->getColSolution(); + + for(size_t i=0; i + +#include +#include +#include +#include + +// Constructor +GridPointExplorator::GridPointExplorator() +{ +} + +void GridPointExplorator::tsp_solver_thread_concorde(ConcordeTSPSolver& tsp_solver, std::vector& optimal_order, + const cv::Mat& distance_matrix, const std::map& cleaned_index_to_original_index_mapping, const int start_node) +{ + try + { + optimal_order = tsp_solver.solveConcordeTSPWithCleanedDistanceMatrix(distance_matrix, cleaned_index_to_original_index_mapping, start_node); + } + catch (boost::thread_interrupted&) + { + std::cout << "GridPointExplorator::tsp_solver_thread_concorde: Thread with Concorde TSP solver was interrupted." << std::endl; + } + + std::cout << "GridPointExplorator::tsp_solver_thread_concorde: finished TSP with solver 3=Concorde and optimal_order.size=" << optimal_order.size() << std::endl; +} + +void GridPointExplorator::tsp_solver_thread_genetic(GeneticTSPSolver& tsp_solver, std::vector& optimal_order, + const cv::Mat& distance_matrix, const std::map& cleaned_index_to_original_index_mapping, const int start_node) +{ + try + { + optimal_order = tsp_solver.solveGeneticTSPWithCleanedDistanceMatrix(distance_matrix, cleaned_index_to_original_index_mapping, start_node); + } + catch (boost::thread_interrupted&) + { + std::cout << "GridPointExplorator::tsp_solver_thread_genetic: Thread with Genetic TSP solver was interrupted." << std::endl; + } + + std::cout << "GridPointExplorator::tsp_solver_thread_genetic: finished TSP with solver 2=Genetic and optimal_order.size=" << optimal_order.size() << std::endl; +} + +void GridPointExplorator::tsp_solver_thread(const int tsp_solver, std::vector& optimal_order, const cv::Mat& original_map, + const std::vector& points, const double downsampling_factor, const double robot_radius, const double map_resolution, + const int start_node) +{ + try + { + if (tsp_solver == TSP_NEAREST_NEIGHBOR) + { + NearestNeighborTSPSolver tsp_solve; + optimal_order = tsp_solve.solveNearestTSP(original_map, points, downsampling_factor, robot_radius, map_resolution, start_node, 0); + } + else if (tsp_solver == TSP_GENETIC) + { + GeneticTSPSolver tsp_solve; + optimal_order = tsp_solve.solveGeneticTSP(original_map, points, downsampling_factor, robot_radius, map_resolution, start_node, 0); + } + else if (tsp_solver == TSP_CONCORDE) + { + ConcordeTSPSolver tsp_solve; + optimal_order = tsp_solve.solveConcordeTSP(original_map, points, downsampling_factor, robot_radius, map_resolution, start_node, 0); + } + else + { + std::cout << "GridPointExplorator::tsp_solver_thread: Error: tsp_solver " << tsp_solver << " is undefined." << std::endl; + } + } + catch (boost::thread_interrupted&) + { + std::cout << "GridPointExplorator::tsp_solver_thread: Thread with TSP solver was interrupted." << std::endl; + } + + std::cout << "GridPointExplorator::tsp_solver_thread: finished TSP with solver " << tsp_solver << " and optimal_order.size=" << optimal_order.size() << std::endl; +} + + +// Function to create a static pose series that has the goal to inspect the complete floor of the given room. +// This is done in the following steps: +// I. It lays a grid over the given map, with a line_size defined by the constructor/set-function. All intersection points +// that are not laying on an obstacle, or near to one, become one point of the path. The grid starts at the upper left +// point that is reachable. +// II. It plans an optimal series of the previously found points by solving a Traveling-Salesman-Problem (TSP). This produces +// a path that covers all nodes and ends at the node where the path started. depending on this series the angle of the +// Poses are computed, by calculating a vector from the old node to the next and using the angle of this with the x-axis +// as angle for the Poses. +// room_map = expects to receive the original, not inflated room map +void GridPointExplorator::getExplorationPath(const cv::Mat& room_map, std::vector& path, const double map_resolution, + const cv::Point starting_position, const cv::Point2d map_origin, const int cell_size, const bool plan_for_footprint, + const Eigen::Matrix robot_to_fov_vector, int tsp_solver, int64_t tsp_solver_timeout) +{ + const int half_cell_size = cell_size/2; + + // *********************** I. Find the main directions of the map and rotate it in this manner. *********************** + // rotate map + cv::Mat R; + cv::Rect bbox; + cv::Mat rotated_room_map; + RoomRotator room_rotation; + room_rotation.computeRoomRotationMatrix(room_map, R, bbox, map_resolution); + room_rotation.rotateRoom(room_map, rotated_room_map, R, bbox); + + // transform starting position + std::vector starting_point_vector(1, starting_position); // opencv syntax + cv::transform(starting_point_vector, starting_point_vector, R); + cv::Point rotated_starting_position = starting_point_vector[0]; + + //******************* II. Get grid points ************************* + // vector to store all found points + std::vector grid_points; + + // compute the basic Boustrophedon grid lines + cv::Mat inflated_rotated_room_map; + BoustrophedonGrid grid_lines; + GridGenerator::generateBoustrophedonGrid(rotated_room_map, inflated_rotated_room_map, half_cell_size, grid_lines, cv::Vec4i(-1, -1, -1, -1), + cell_size, half_cell_size, cell_size-1); // using cell_size-1 instead of cell_size for grid_spacing_horizontal helps + // the TSP planner to avoid unnecessary rotations by following a preferred direction + // convert grid points format + for (BoustrophedonGrid::iterator line=grid_lines.begin(); line!=grid_lines.end(); ++line) + { + grid_points.insert(grid_points.end(), line->upper_line.begin(), line->upper_line.end()); + if (line->has_two_valid_lines == true) + grid_points.insert(grid_points.end(), line->lower_line.begin(), line->lower_line.end()); + } + +// // print results +// cv::Mat point_map = rotated_room_map.clone(); +// for(std::vector::iterator point = grid_points.begin(); point != grid_points.end(); ++point) +// { +// cv::circle(point_map, *point, 2, cv::Scalar(127), CV_FILLED); +// std::cout << " - " << *point << "\n"; +// } +// cv::imshow("grid", point_map); +// cv::waitKey(); + + //******************* II. Plan a path trough the found points ************************* + // find the index of the point, which is closest to the starting position + int min_index = 0; + double min_distance = 1e10; + + for(std::vector::iterator point = grid_points.begin(); point != grid_points.end(); ++point) + { + double distance = cv::norm(rotated_starting_position - *point); + + if(distance < min_distance) + { + min_distance = distance; + min_index = point - grid_points.begin(); + } + } + + + // solve the Traveling Salesman Problem + std::cout << "Finding optimal order of the " << grid_points.size() << " found points. Start-index: " << min_index << std::endl; + const double map_downsampling_factor = 0.25; + // compute distance matrix for TSP (outside of time limits for solving TSP) + cv::Mat distance_matrix_cleaned; + std::map cleaned_index_to_original_index_mapping; // maps the indices of the cleaned distance_matrix to the original indices of the original distance_matrix + AStarPlanner path_planner; + DistanceMatrix distance_matrix_computation; + distance_matrix_computation.computeCleanedDistanceMatrix(rotated_room_map, grid_points, map_downsampling_factor, 0.0, map_resolution, path_planner, + distance_matrix_cleaned, cleaned_index_to_original_index_mapping, min_index); + + // solve TSP + bool finished = false; + std::vector optimal_order; + if (tsp_solver == TSP_CONCORDE) + { + // start TSP solver in extra thread + ConcordeTSPSolver tsp_solve; + boost::thread t(boost::bind(&GridPointExplorator::tsp_solver_thread_concorde, this, boost::ref(tsp_solve), boost::ref(optimal_order), + boost::cref(distance_matrix_cleaned), boost::cref(cleaned_index_to_original_index_mapping), min_index)); + if (tsp_solver_timeout > 0) + { + finished = t.try_join_for(boost::chrono::seconds(tsp_solver_timeout)); + if (finished == false) + { + tsp_solve.abortComputation(); + std::cout << "GridPointExplorator::getExplorationPath: INFO: Terminated tsp_solver " << tsp_solver << " because of time out. Taking the Nearest Neighbor TSP instead." << std::endl; + } + } + else + finished = true; + t.join(); + } + else if (tsp_solver == TSP_GENETIC) + { + // start TSP solver in extra thread + GeneticTSPSolver tsp_solve; + boost::thread t(boost::bind(&GridPointExplorator::tsp_solver_thread_genetic, this, boost::ref(tsp_solve), boost::ref(optimal_order), + boost::cref(distance_matrix_cleaned), boost::cref(cleaned_index_to_original_index_mapping), min_index)); + if (tsp_solver_timeout > 0) + { + finished = t.try_join_for(boost::chrono::seconds(tsp_solver_timeout)); + if (finished == false) + { + tsp_solve.abortComputation(); + std::cout << "GridPointExplorator::getExplorationPath: INFO: Terminated tsp_solver " << tsp_solver << " because of time out. Taking the Nearest Neighbor TSP instead." << std::endl; + } + } + else + finished = true; + t.join(); + } + // fall back to nearest neighbor TSP if the other approach was timed out + if (tsp_solver==TSP_NEAREST_NEIGHBOR || finished==false) + { + NearestNeighborTSPSolver tsp_solve; + optimal_order = tsp_solve.solveNearestTSPWithCleanedDistanceMatrix(distance_matrix_cleaned, cleaned_index_to_original_index_mapping, min_index); + std::cout << "GridPointExplorator::getExplorationPath: finished TSP with solver 1 and optimal_order.size=" << optimal_order.size() << std::endl; + } + + // rearrange the found points in the optimal order and convert them to the right format + std::vector fov_middlepoint_path(optimal_order.size()); + for(unsigned int point_index = 0; point_index < optimal_order.size(); ++point_index) + fov_middlepoint_path[point_index] = cv::Point2f(grid_points[optimal_order[point_index]].x, grid_points[optimal_order[point_index]].y); + + // transform the calculated path back to the originally rotated map and create poses with an angle + std::vector path_fov_poses; + room_rotation.transformPathBackToOriginalRotation(fov_middlepoint_path, path_fov_poses, R); + +// for(unsigned int point_index = 0; point_index < fov_middlepoint_path.size(); ++point_index) +// { +// // get the vector from the current point to the next point +// cv::Point current_point = grid_points[optimal_order[point_index]]; +// cv::Point next_point = grid_points[optimal_order[(point_index+1)%(optimal_order.size())]]; +// cv::Point vector = next_point - current_point; +// +// float angle = std::atan2(vector.y, vector.x);//std::acos(quotient); +// +// // add the next navigation goal to the path +// geometry_msgs::Pose2D navigation_goal; +// navigation_goal.x = (current_point.x * map_resolution) + map_origin.x; // coordinate systems are rotated to each other +// navigation_goal.y = (current_point.y * map_resolution) + map_origin.y; +// navigation_goal.theta = angle; +// +// path.push_back(navigation_goal); +// } + + // if the path should be planned for the robot footprint create the path and return here + if(plan_for_footprint == true) + { + for(std::vector::iterator pose=path_fov_poses.begin(); pose != path_fov_poses.end(); ++pose) + { + geometry_msgs::Pose2D current_pose; + current_pose.x = (pose->x * map_resolution) + map_origin.x; + current_pose.y = (pose->y * map_resolution) + map_origin.y; + current_pose.theta = pose->theta; + path.push_back(current_pose); + } + return; + } + + // *********************** III. Get the robot path out of the fov path. *********************** + // go trough all computed fov poses and compute the corresponding robot pose + //mapPath(room_map, path, path_fov_poses, robot_to_fov_vector, map_resolution, map_origin, starting_position); + ROS_INFO("Starting to map from field of view pose to robot pose"); + cv::Point robot_starting_position = (path_fov_poses.size()>0 ? cv::Point(path_fov_poses[0].x, path_fov_poses[0].y) : starting_position); + cv::Mat inflated_room_map; + cv::erode(room_map, inflated_room_map, cv::Mat(), cv::Point(-1, -1), half_cell_size); + mapPath(inflated_room_map, path, path_fov_poses, robot_to_fov_vector, map_resolution, map_origin, robot_starting_position); +} diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/common/src/meanshift2d.cpp b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/common/src/meanshift2d.cpp new file mode 100644 index 0000000..b955d51 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/common/src/meanshift2d.cpp @@ -0,0 +1,161 @@ +/*! + ***************************************************************** + * \file + * + * \note + * Copyright (c) 2015 \n + * Fraunhofer Institute for Manufacturing Engineering + * and Automation (IPA) \n\n + * + ***************************************************************** + * + * \note + * Project name: Care-O-bot + * \note + * ROS stack name: autopnp + * \note + * ROS package name: ipa_room_segmentation + * + * \author + * Author: Richard Bormann + * \author + * Supervised by: + * + * \date Date of creation: 22.07.2015 + * + * \brief + * + * + ***************************************************************** + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * - Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. \n + * - Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. \n + * - Neither the name of the Fraunhofer Institute for Manufacturing + * Engineering and Automation (IPA) nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. \n + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License LGPL as + * published by the Free Software Foundation, either version 3 of the + * License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License LGPL for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License LGPL along with this program. + * If not, see . + * + ****************************************************************/ + +#include "ipa_room_exploration/meanshift2d.h" +#include "ipa_room_exploration/fast_math.h" + +void MeanShift2D::filter(const std::vector& data, std::vector& filtered_data, const double bandwidth, const int maximumIterations) +{ + // prepare mean shift set + std::vector mean_shift_set(data.size()); + filtered_data = data; + + // mean shift iteration + for (int iter=0; iter& filtered_data, std::vector& convergence_points, std::vector< std::vector >& convergence_sets, const double sensitivity) +{ + // cluster data according to convergence points + convergence_sets.resize(1, std::vector(1, 0)); + convergence_points.resize(1, filtered_data[0]); + for (int i=1; i<(int)filtered_data.size(); i++) + { + bool create_new_set = true; + for (int j=0; j<(int)convergence_points.size(); j++) + { + if (cv::norm(filtered_data[i]-convergence_points[j]) < sensitivity) + { + convergence_sets[j].push_back(i); + convergence_points[j] = (convergence_points[j]*(convergence_sets[j].size()-1.) + filtered_data[i]) / (double)convergence_sets[j].size(); // update mean of convergence point + create_new_set = false; + break; + } + } + if (create_new_set == true) + { + convergence_sets.push_back(std::vector(1, i)); + convergence_points.push_back(filtered_data[i]); + } + } +} + +cv::Vec2d MeanShift2D::findRoomCenter(const cv::Mat& room_image, const std::vector& room_cells, double map_resolution) +{ + // downsample data if too big + std::vector room_cells_sampled; + if (room_cells.size() > 1000) + { + const int factor = room_cells.size()/1000; + for (size_t i=0; i filtered_room_cells; + filter(room_cells_sampled, filtered_room_cells, bandwidth, 100); + + // take mean of filtering result in simple rooms, i.e. if no obstacles is located at the computed cell + cv::Scalar mean_coordinates = cv::mean(filtered_room_cells); + cv::Vec2d room_center(mean_coordinates[0], mean_coordinates[1]); + if (room_image.at(room_center[1], room_center[0]) == 255) + return room_center; + + // otherwise compute convergence sets of filtering results and return mean of largest convergence set + // determine convergence points + std::vector convergence_points; + std::vector< std::vector > convergence_sets; + computeConvergencePoints(filtered_room_cells, convergence_points, convergence_sets, 1./bandwidth*0.1); + + // select convergence point with largest point support + size_t max_index = 0; + size_t max_size = 0; + for (size_t i=0; i max_size) + { + max_size = convergence_sets[i].size(); + max_index = i; + } + } + return convergence_points[max_index]; +} diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/common/src/neural_network_explorator.cpp b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/common/src/neural_network_explorator.cpp new file mode 100644 index 0000000..d3fe3ca --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/common/src/neural_network_explorator.cpp @@ -0,0 +1,367 @@ +#include + +// Default constructor +NeuralNetworkExplorator::NeuralNetworkExplorator() +{ + // default values TODO: param + step_size_ = 0.008; // 0.008 + A_ = 17; // 17 + B_ = 5; // 5 + D_ = 7; // 7 + E_ = 80; // E >> B, 80 + mu_ = 1.03; // 1.03 + delta_theta_weight_ = 0.15; // 0.15 +} + +// Function that calculates an exploration path trough the given map s.t. everything has been covered by the robot-footprint +// or the field of view. The algorithm is programmed after +// +// Yang, Simon X., and Chaomin Luo. "A neural network approach to complete coverage path planning." IEEE Transactions on Systems, Man, and Cybernetics, Part B (Cybernetics) 34.1 (2004): 718-724. +// +// and uses a artificial neural network to produce the path. For this the following steps are done: +// I. Construct the neural network by sampling the given map, using the given fitting circle radius as step size. This is +// done because it allows that when all neurons (the samples) are covered the whole space have been cleaned/inspected. +// II. Starting with the given robot pose go trough the found network. At every time-step choose the next Neuron by +// solving x_n = max(x_j + c*y_j), with c as a positive scalar and y_j a function penalizing movements of the robot +// into a direction he is currently not pointing at. At every time-step an update of the states of the neurons is done. +// After this step a path consisting of poses for the fov middlepoint is obtained. If the plan should be planned for +// the robot footprint, then the algorithm returns this path and is finished. +// III. If wanted use the given vector from the robot middlepoint to the fov middlepoint to map the fov poses to the +// robot footprint poses. To do so simply a vector transformation is applied. If the computed robot pose is not in the +// free space, another accessible point is generated by finding it on the radius around the fov middlepoint s.t. +// the distance to the last robot position is minimized. If this is not wanted one has to set the corresponding +// Boolean to false (shows that the path planning should be done for the robot footprint). +void NeuralNetworkExplorator::getExplorationPath(const cv::Mat& room_map, std::vector& path, const float map_resolution, + const cv::Point starting_position, const cv::Point2d map_origin, const double grid_spacing_in_pixel, + const bool plan_for_footprint, const Eigen::Matrix robot_to_fov_vector, bool show_path_computation) +{ + const int grid_spacing_as_int = std::floor(grid_spacing_in_pixel); + const int half_grid_spacing_as_int = std::floor(grid_spacing_in_pixel*0.5); + + // *********************** I. Find the main directions of the map and rotate it in this manner. *********************** + cv::Mat R; + cv::Rect bbox; + cv::Mat rotated_room_map; + RoomRotator room_rotation; + room_rotation.computeRoomRotationMatrix(room_map, R, bbox, map_resolution); + room_rotation.rotateRoom(room_map, rotated_room_map, R, bbox); + + // compute min/max room coordinates + cv::Point min_room(1000000, 1000000), max_room(0, 0); + for (int v=0; v(v,u)==255) + { + min_room.x = std::min(min_room.x, u); + min_room.y = std::min(min_room.y, v); + max_room.x = std::max(max_room.x, u); + max_room.y = std::max(max_room.y, v); + } + } + } + cv::Mat inflated_rotated_room_map; + cv::erode(rotated_room_map, inflated_rotated_room_map, cv::Mat(), cv::Point(-1, -1), half_grid_spacing_as_int); + + // ****************** II. Create the neural network ****************** + // reset previously computed neurons + neurons_.clear(); + + // go trough the map and create the neurons + int number_of_free_neurons = 0; + for(int y=min_room.y+half_grid_spacing_as_int; y current_network_row; + for(int x=min_room.x+half_grid_spacing_as_int; x(y,x) == 255) + { + Neuron current_neuron(cell_center, A_, B_, D_, E_, mu_, step_size_, false); + current_network_row.push_back(current_neuron); + ++number_of_free_neurons; + } + else // obstacle neuron + { + Neuron current_neuron(cell_center, A_, B_, D_, E_, mu_, step_size_, true); + current_network_row.push_back(current_neuron); + } + } + + // insert the current row into the network + neurons_.push_back(current_network_row); + } + + // todo: do not limit to direct neighbors but cycle through all neurons for finding the best next + // go trough the found neurons and get the direct neighbors of each + for(size_t row=0; row= neurons_.size()) + continue; + + // get the neighbors left from the current neuron + if(column > 0) + neurons_[row][column].addNeighbor(&neurons_[row+dy][column-1]); + + // get the neurons on the same column as the current neuron + if(dy != 0) + neurons_[row][column].addNeighbor(&neurons_[row+dy][column]); + + // get the neurons right from the current neuron + if(column < neurons_[row].size()-1) + neurons_[row][column].addNeighbor(&neurons_[row+dy][column+1]); + } + } + } + +// testing +// cv::Mat black_map = cv::Mat(rotated_room_map.rows, rotated_room_map.cols, rotated_room_map.type(), cv::Scalar(0)); +// for(size_t i=0; i neighbors; +// neurons_[i][j].getNeighbors(neighbors); +// for(size_t k=0; kgetPosition(), 2, cv::Scalar(127), CV_FILLED); +// } +// } +// } +// cv::imshow("neighbors", black_map); +// cv::waitKey(); + + // ****************** III. Find the coverage path ****************** + // mark the first non-obstacle neuron as starting node + Neuron* starting_neuron = 0; + bool found = false; + for(size_t row=0; rowmarkAsVisited(); + + // initial updates of the states to mark obstacles and unvisited free neurons as such + for(size_t init=1; init<=100; ++init) + { + for(size_t row=0; row(row*fitting_radius_as_int, column*fitting_radius_as_int) = (float) 1000.0*neurons_[row][column].getState(false); +// } +// std::cout << std::endl; +// } +// std::cout << std::endl; +// cv::namedWindow("states", cv::WINDOW_NORMAL); +// cv::imshow("states", black_map); +// cv::resizeWindow("states", 600, 600); +// cv::waitKey(); + + // iteratively choose the next neuron until all neurons have been visited or the algorithm is stuck in a + // limit cycle like path (i.e. the same neurons get visited over and over) + int visited_neurons = 1; + bool stuck_in_cycle = false; + std::vector fov_coverage_path; + fov_coverage_path.push_back(cv::Point(starting_neuron->getPosition().x, starting_neuron->getPosition().y)); + double previous_traveling_angle = 0.0; // save the travel direction to the current neuron to determine the next neuron + cv::Mat black_map = rotated_room_map.clone(); + Neuron* previous_neuron = starting_neuron; + int loop_counter = 0; + do + { + //std::cout << "Point: " << previous_neuron->getPosition() << std::endl; + ++loop_counter; + + // get the current neighbors and choose the next out of them + std::vector neighbors; + previous_neuron->getNeighbors(neighbors); + Neuron* next_neuron = 0; + + // go through the neighbors and find the next one + double max_value = -1e10, travel_angle = 0.0, best_angle = 0.0; + for(size_t neighbor=0; neighborgetPosition().y-previous_neuron->getPosition().y, neighbors[neighbor]->getPosition().x-previous_neuron->getPosition().x); + + // compute penalizing function y_j + double diff_angle = travel_angle - previous_traveling_angle; + while (diff_angle < -PI) + diff_angle += 2*PI; + while (diff_angle > PI) + diff_angle -= 2*PI; + double y = 1 - (std::abs(diff_angle)/PI); + + // compute transition function value + //std::cout << " Neighbor: " << neighbors[neighbor]->getPosition() << " " << neighbors[neighbor]->getState(false) << ", " << delta_theta_weight_ * y << std::endl; + double trans_fct_value = neighbors[neighbor]->getState(false) + delta_theta_weight_ * y; + + // check if neighbor is next neuron to be visited + if(trans_fct_value > max_value && rotated_room_map.at(neighbors[neighbor]->getPosition()) != 0) + { + max_value = trans_fct_value; + next_neuron = neighbors[neighbor]; + best_angle = travel_angle; + } + } + // catch errors + if (next_neuron == 0) + { + if (loop_counter <= 20) + continue; + else + break; + } + loop_counter = 0; + + // if the next neuron was previously uncleaned, increase number of visited neurons + if(next_neuron->visitedNeuron() == false) + ++visited_neurons; + + // mark next neuron as visited + next_neuron->markAsVisited(); + previous_traveling_angle = best_angle; + + // add neuron to path + const cv::Point current_pose(next_neuron->getPosition().x, next_neuron->getPosition().y); + fov_coverage_path.push_back(current_pose); + + // check the fov path for a limit cycle by searching the path for the next neuron, if it occurs too often + // and the previous/following neuron is always the same the algorithm probably is stuck in a cycle + int number_of_neuron_in_path = 0; + for(std::vector::iterator pose=fov_coverage_path.begin(); pose!=fov_coverage_path.end(); ++pose) + if(*pose==current_pose) + ++number_of_neuron_in_path; + + if(number_of_neuron_in_path >= 20) + { + // check number of previous neuron + cv::Point previous_pose = fov_coverage_path[fov_coverage_path.size()-2]; + int number_of_previous_neuron_in_path = 0; + for(std::vector::iterator pose=fov_coverage_path.begin()+1; pose!=fov_coverage_path.end()-1; ++pose) + { + // check if the the previous pose always has the current pose as neighbor + if(*pose==previous_pose) + { + if(*(pose+1)==current_pose) + ++number_of_previous_neuron_in_path; + else if(*(pose-1)==current_pose) + ++number_of_previous_neuron_in_path; + } + } + + //if(number_of_previous_neuron_in_path >= number_of_neuron_in_path) + if(number_of_previous_neuron_in_path >= 20) + { + std::cout << "Warning: the algorithm is probably stuck in a cycle. Aborting." << std::endl; + stuck_in_cycle = true; + } + } + + // update the states of the network + for (int i=0; i<100; ++i) + { + for(size_t row=0; rowgetPosition(), 2, cv::Scalar((visited_neurons*5)%250), CV_FILLED); +#else + cv::circle(black_map, next_neuron->getPosition(), 2, cv::Scalar((visited_neurons*5)%250), cv::FILLED); +#endif + cv::line(black_map, previous_neuron->getPosition(), next_neuron->getPosition(), cv::Scalar(128), 1); + cv::imshow("next_neuron", black_map); + cv::waitKey(); + } + + // save neuron that has been visited + previous_neuron = next_neuron; + } while (visited_neurons < number_of_free_neurons && stuck_in_cycle == false); //TODO: test terminal condition + + // transform the calculated path back to the originally rotated map + std::vector fov_poses; + std::vector fov_coverage_path_2f(fov_coverage_path.size()); + for (size_t i=0; i::iterator pose=fov_poses.begin(); pose != fov_poses.end(); ++pose) + { + geometry_msgs::Pose2D current_pose; + current_pose.x = (pose->x * map_resolution) + map_origin.x; + current_pose.y = (pose->y * map_resolution) + map_origin.y; + current_pose.theta = pose->theta; + path.push_back(current_pose); + } + return; + } + + // ****************** III. Map the found fov path to the robot path ****************** + // go trough all computed fov poses and compute the corresponding robot pose + ROS_INFO("Starting to map from field of view pose to robot pose"); + cv::Point robot_starting_position = (fov_poses.size()>0 ? cv::Point(fov_poses[0].x, fov_poses[0].y) : starting_position); + cv::Mat inflated_room_map; + cv::erode(room_map, inflated_room_map, cv::Mat(), cv::Point(-1, -1), half_grid_spacing_as_int); + mapPath(inflated_room_map, path, fov_poses, robot_to_fov_vector, map_resolution, map_origin, robot_starting_position); +} diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/common/src/room_rotator.cpp b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/common/src/room_rotator.cpp new file mode 100644 index 0000000..053c66b --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/common/src/room_rotator.cpp @@ -0,0 +1,237 @@ +/*! + ***************************************************************** + * \file + * + * \note + * Copyright (c) 2017 \n + * Fraunhofer Institute for Manufacturing Engineering + * and Automation (IPA) \n\n + * + ***************************************************************** + * + * \note + * Project name: Care-O-bot + * \note + * ROS stack name: autopnp + * \note + * ROS package name: ipa_room_exploration + * + * \author + * Author: Richard Bormann + * \author + * Supervised by: Richard Bormann + * + * \date Date of creation: 02.2017 + * + * \brief + * + * + ***************************************************************** + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * - Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. \n + * - Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. \n + * - Neither the name of the Fraunhofer Institute for Manufacturing + * Engineering and Automation (IPA) nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. \n + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License LGPL as + * published by the Free Software Foundation, either version 3 of the + * License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License LGPL for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License LGPL along with this program. + * If not, see . + * + ****************************************************************/ + + +#include + +void RoomRotator::rotateRoom(const cv::Mat& room_map, cv::Mat& rotated_room_map, const cv::Mat& R, const cv::Rect& bounding_rect) +{ + // rotate the image + cv::warpAffine(room_map, rotated_room_map, R, bounding_rect.size(), cv::INTER_AREA); + + // apply a binary filter to create a binary image, also use a closing operator to smooth the output (the rotation might produce + // black pixels reaching into the white area that were not there before, causing new, wrong cells to open) +#if CV_MAJOR_VERSION<=3 + cv::threshold(rotated_room_map, rotated_room_map, 127, 255, CV_THRESH_BINARY); +#else + cv::threshold(rotated_room_map, rotated_room_map, 127, 255, cv::THRESH_BINARY); +#endif +// this should not be used because it removes smaller obstacles like thin walls from the room and hence lets a planner generate paths through walls +// cv::dilate(rotated_room_map, rotated_room_map, cv::Mat(), cv::Point(-1,-1), 1); +// cv::erode(rotated_room_map, rotated_room_map, cv::Mat(), cv::Point(-1,-1), 1); +} + +// compute the affine rotation matrix for rotating a room into parallel alignment with x-axis (longer side of the room is aligned with x-axis) +// R is the transform +// bounding_rect is the ROI of the warped image +double RoomRotator::computeRoomRotationMatrix(const cv::Mat& room_map, cv::Mat& R, cv::Rect& bounding_rect, + const double map_resolution, const cv::Point* center, const double rotation_offset) +{ + // rotation angle of the map s.t. the most occurring gradient is in 90 degree to the x-axis + double rotation_angle = computeRoomMainDirection(room_map, map_resolution) + rotation_offset; + std::cout << "RoomRotator::computeRoomRotationMatrix: main rotation angle: " << rotation_angle << std::endl; + + // get rotation matrix R for rotating the image around the center of the room contour + // Remark: rotation angle in degrees for opencv + cv::Point center_of_rotation; + if (center == 0) + { + cv::Point min_room, max_room; + getMinMaxCoordinates(room_map, min_room, max_room); + center_of_rotation.x = 0.5*(min_room.x+max_room.x); + center_of_rotation.y = 0.5*(min_room.y+max_room.y); + } + else + center_of_rotation = *center; + + // compute rotation + R = cv::getRotationMatrix2D(center_of_rotation, (rotation_angle*180)/CV_PI, 1.0); + + // determine bounding rectangle to find the size of the new image + bounding_rect = cv::RotatedRect(center_of_rotation, room_map.size(), (rotation_angle*180)/CV_PI).boundingRect(); + // adjust transformation matrix + R.at(0,2) += 0.5*bounding_rect.width - center_of_rotation.x; + R.at(1,2) += 0.5*bounding_rect.height - center_of_rotation.y; + + return rotation_angle; +} + +// computes the major direction of the walls from a map (preferably one room) +// the map (room_map, CV_8UC1) is black (0) at impassable areas and white (255) on drivable areas +double RoomRotator::computeRoomMainDirection(const cv::Mat& room_map, const double map_resolution) +{ + const double map_resolution_inverse = 1./map_resolution; + + // compute Hough transform on edge image of the map + cv::Mat edge_map; + cv::Canny(room_map, edge_map, 50, 150, 3); + std::vector lines; + double min_line_length = 1.0; // in [m] + for (; min_line_length > 0.1; min_line_length -= 0.2) + { + cv::HoughLinesP(edge_map, lines, 1, CV_PI/180, min_line_length*map_resolution_inverse, min_line_length*map_resolution_inverse, 1.5*min_line_length*map_resolution_inverse); + cv::Mat room_hough = edge_map.clone(); + for (size_t i=0; i= 4) + break; + } + // setup a histogram on the line directions weighted by their length to determine the major direction + Histogram direction_histogram(0, CV_PI, 36); + for (size_t i=0; i 0.0) + { + double current_direction = std::atan2(dy, dx); + while (current_direction < 0.) + current_direction += CV_PI; + while (current_direction > CV_PI) + current_direction -= CV_PI; + direction_histogram.addData(current_direction, sqrt(dy*dy+dx*dx)); + //std::cout << " dx=" << dx << " dy=" << dy << " dir=" << current_direction << " len=" << sqrt(dy*dy+dx*dx) << std::endl; + } + } + return direction_histogram.getMaxBinPreciseVal(); +} + +void RoomRotator::transformPathBackToOriginalRotation(const std::vector& fov_middlepoint_path, std::vector& path_fov_poses, const cv::Mat& R) +{ + path_fov_poses.clear(); + + // transform the calculated path back to the originally rotated map + cv::Mat R_inv; + cv::invertAffineTransform(R, R_inv); + std::vector fov_middlepoint_path_transformed; + cv::transform(fov_middlepoint_path, fov_middlepoint_path_transformed, R_inv); + + // create poses with an angle + transformPointPathToPosePath(fov_middlepoint_path_transformed, path_fov_poses); +} + +void RoomRotator::transformPointPathToPosePath(const std::vector& point_path, std::vector& pose_path) +{ + // create poses with an angle + for(size_t point_index = 0; point_index < point_path.size(); ++point_index) + { + // get the vector from the previous to the current point + const cv::Point2f& current_point = point_path[point_index]; + + // add the next navigation goal to the path + geometry_msgs::Pose2D current_pose; + current_pose.x = current_point.x; + current_pose.y = current_point.y; + current_pose.theta = 0.; + cv::Point2f vector(0,0); + if (point_index > 0) + { + // compute the direction as the line from the previous point to the current point + vector = current_point - point_path[point_index-1]; + } + else if (point_path.size() >= 2) + { + // for the first point take the direction between first and second point + vector = point_path[point_index+1] - current_point; + } + // only sample different points + if (vector.x!=0 || vector.y!=0) + { + current_pose.theta = std::atan2(vector.y, vector.x); + pose_path.push_back(current_pose); + } + } +} + +void RoomRotator::transformPointPathToPosePath(std::vector& pose_path) +{ + // create point vector + std::vector point_path; + for (size_t i=0; i::max(); + min_room.y = std::numeric_limits::max(); + max_room.x = 0; + max_room.y = 0; + for (int v=0; v(v,u)==255) + { + min_room.x = std::min(min_room.x, u); + min_room.y = std::min(min_room.y, v); + max_room.x = std::max(max_room.x, u); + max_room.y = std::max(max_room.y, v); + } + } + } +} diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/package.xml b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/package.xml new file mode 100644 index 0000000..591affd --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/package.xml @@ -0,0 +1,48 @@ + + + ipa_room_exploration + 0.1.2 + + This package plans the navigation in a room such that the floor of this room gets inspected completely for dirt. + + Florian Jordan + Richard Bormann + LGPL for academic and non-commercial use; for commercial use, please contact Fraunhofer IPA + http://ros.org/wiki/ipa_room_exploration + + catkin + + + angles + boost + cmake_modules + cob_map_accessibility_analysis + coinor-libcbc-dev + coinor-libcbc3 + coinor-libcgl-dev + coinor-libclp-dev + coinor-libcoinutils-dev + coinor-libosi-dev + cv_bridge + dynamic_reconfigure + + eigen_conversions + geometry_msgs + ipa_building_msgs + ipa_building_navigation + laser_geometry + + libopencv-dev + move_base_msgs + nav_msgs + roscpp + roslib + sensor_msgs + std_msgs + std_srvs + tf + visualization_msgs + message_generation + message_runtime + + diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/readme.md b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/readme.md new file mode 100644 index 0000000..109b41e --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/readme.md @@ -0,0 +1,29 @@ +# ipa_room_exploration +Algorithms for systematic coverage driving patterns. + +If you find this software useful in your work, please cite our corresponding paper: +- R. Bormann, F. Jordan, J. Hampp, and M. Hägele. Indoor coverage path planning: Survey, implementation, analysis. In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), pages 1718–1725, May 2018. https://ieeexplore.ieee.org/abstract/document/8460566 , https://publica.fraunhofer.de/entities/publication/f537c15d-4cbe-4672-9d86-6e756a9ce71b/details + +# General Procedure + +1. Change the algorithm parameters in ros/launch/room_room_exploration_action_server_params.yaml in ros/launch to the wanted algorithms and settings. + * room_exploration_algorithm: Choose which exploration algorithm you want to use. + * plan_for_footprint: Choose if you want to plan the coverage path for the field of view (fov) or the robot footprint. When planning for the fov, the generated path is transformed to robot poses, s.t. the given fov follows the generated path. + * revisit_areas: Choose if you want to revisit areas, that haven't been covered bythe robot or the fov during the execution of the coverage path. + * left_sections_min_area: When the revisiting of areas is enabled, this parameter defines how small not covered ares can be, before they will not be taken into account any further. This is useful, when you rather want a fast execution, than a very detailed coverage. This parameter is in [meter^2]. + * goal_eps: This parameter allows the action server to publish a new navigation goal, when the robot is in a specific range around the current goal. This allows the path to be more smoother, but of course not so exactly. If you don't want this feature, then simpply set this parameter to 0. This parameter is in [pixel]. + * Each exploration algorithm has it's own specific parameters, see the ros/launch/room_room_exploration_action_server_params.yaml for further descriptions of these. + +2. Start the action server using the file /ros/launch/room_exploration_action_server.launch, which executes the /ros/src/room_exploration_action_server.cpp file. If the server crashes for some reason (e.g. no valid map given by the client) it respawns with a little delay. + +3. Start an action client, which sends a goal to the action server, corresponding to the RoomExploration.action message, which lies in ipa_building_msgs/action. The goal consists of the following parts + + * input_map: The map of the whole area the robot moves in, as sensor_msgs/Image. **Has to be a 8-Bit single channel image, with 0 as occupied space and 255 as free space**. + * map_resolution: The resolution of the map as float32 in [meter/cell] + * map_origin: The origin of the map, will be used to transform the computed map-coordinates into real-world-coordinates. + * robot_radius: The radius of the robot as float32, used to find the areas that the robot can access, in [meter]. + * coverage_radius: The radius of the device that should cover the defined area as float32. This can be a cleaning device or an inspection tool, the value should be given in [meter]. + * field_of_view: Array of 4 geometry_msgs/Points32 that define the field of view or footprint (points around robot origin) of the robot. Used to check what areas have been left uncovered during the execution (only used when the action server shall execute the path). + * field_of_view_origin: The mounting position of the camera spanning the field of view, relative to the robot center (x-axis points to robot's front side, y-axis points to robot's left side, z-axis upwards), in [meter]. + * starting_position: The position at which the robot shall start the execution of the path in the room coordinate system [meter, meter, rad]. + * planning_mode: Int32 that controls if the robot footprint or the defined field-of-view shall cover the whole area (1: footprint, 2: fov). diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/config/cleaning_costmap_params.yaml b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/config/cleaning_costmap_params.yaml new file mode 100644 index 0000000..0340003 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/config/cleaning_costmap_params.yaml @@ -0,0 +1,15 @@ +cleaning_costmap: + global_frame: map + robot_base_frame: base_footprint + update_frequency: 1.0 + publish_frequency: 1.0 + static_map: true + rolling_window: false + resolution: 0.05 + transform_tolerance: 1.0 + inflation_radius: 0.15 + map_type: costmap + +cleaning_plan_nodehandle: + size_of_cell: 3 + grid_covered_value: 200 diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/config/costmap_common_params.yaml b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/config/costmap_common_params.yaml new file mode 100644 index 0000000..f819012 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/config/costmap_common_params.yaml @@ -0,0 +1,13 @@ +obstacle_range: 2.5 #2.5 +raytrace_range: 3.0 #3.0 +footprint: [[-0.205, -0.155], [-0.205, 0.155], [0.077, 0.155], [0.077, -0.155]] +#footprint_inflation: 0.01 +# robot_radius: 0.175 #0.175 +max_obstacle_height: 0.6 +min_obstacle_height: 0.0 +observation_sources: scan #观察数据来源 +scan: {data_type: LaserScan, topic: /scan, marking: true, clearing: true, expected_update_rate: 0} + +# if set to false,then obstacle layer take unknow space to be free. +obstacle_layer: + track_unknown_space: true diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/config/costmap_common_params_burger.yaml b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/config/costmap_common_params_burger.yaml new file mode 100644 index 0000000..7b0d46e --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/config/costmap_common_params_burger.yaml @@ -0,0 +1,12 @@ +obstacle_range: 3.0 +raytrace_range: 3.5 + +footprint: [[-0.105, -0.105], [-0.105, 0.105], [0.041, 0.105], [0.041, -0.105]] +#robot_radius: 0.105 + +inflation_radius: 0.5 +cost_scaling_factor: 4.0 + +map_type: costmap +observation_sources: scan +scan: {sensor_frame: base_scan, data_type: LaserScan, topic: scan, marking: true, clearing: true} \ No newline at end of file diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/config/costmap_common_params_waffle.yaml b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/config/costmap_common_params_waffle.yaml new file mode 100644 index 0000000..f3a4a4e --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/config/costmap_common_params_waffle.yaml @@ -0,0 +1,22 @@ +obstacle_range: 3.0 +raytrace_range: 3.5 + +footprint: [[-0.205, -0.155], [-0.205, 0.155], [0.077, 0.155], [0.077, -0.155]] +#robot_radius: 0.17 + +inflation_radius: 0.3 +cost_scaling_factor: 5.0 + +map_type: costmap +observation_sources: scan +scan: {sensor_frame: base_scan, data_type: LaserScan, topic: scan, marking: true, clearing: true} +sonar_layer: + enabled: true + no_readings_timout: 0.0 #此参数如果为0 ，没有任何影响.否则，如果在这个时间段内没有收到任何声纳数据，超声波层会警告用户，并且此层不被使用 + clear_threshold: 0.8 #栅格概率比该值低的标记为free + mark_threshold: 0.9 #栅格概率大于该值标定为占据 +# topics: ["/robot0/sonar_0","/robot0/sonar_1","/robot0/sonar_2","/robot0/sonar_3"] #超声波话题 + clear_on_max_reading: true #是否将超出sonar最大距离清除 + +# observation_sources: scan +# scan: {sensor_frame: base_scan, data_type: LaserScan, topic: sonar, marking: true, clearing: true} \ No newline at end of file diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/config/costmap_common_params_waffle_pi.yaml b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/config/costmap_common_params_waffle_pi.yaml new file mode 100644 index 0000000..bd27ed3 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/config/costmap_common_params_waffle_pi.yaml @@ -0,0 +1,12 @@ +obstacle_range: 3.0 +raytrace_range: 3.5 + +footprint: [[-0.205, -0.155], [-0.205, 0.155], [0.077, 0.155], [0.077, -0.155]] +#robot_radius: 0.17 + +inflation_radius: 0.3 +cost_scaling_factor: 5.0 + +map_type: costmap +observation_sources: scan +scan: {sensor_frame: base_scan, data_type: LaserScan, topic: scan, marking: true, clearing: true} diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/config/global_costmap_params.yaml b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/config/global_costmap_params.yaml new file mode 100644 index 0000000..3492775 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/config/global_costmap_params.yaml @@ -0,0 +1,15 @@ +global_costmap: + global_frame: map + robot_base_frame: base_footprint + + update_frequency: 10.0 + publish_frequency: 10.0 + transform_tolerance: 0.5 + + static_map: true + +# plugins: +# - {name: static_layer, type: "costmap_2d::StaticLayer"} +# - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"} +# - {name: sonar_layer, type:"range_sensor_layer::RangeSensorLayer"} +# - {name: inflation_layer, type: "costmap_2d::InflationLayer"} diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/include/ipa_room_exploration/coverage_check_server.h b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/include/ipa_room_exploration/coverage_check_server.h new file mode 100644 index 0000000..0e6b938 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/include/ipa_room_exploration/coverage_check_server.h @@ -0,0 +1,66 @@ +// Ros +#include +// OpenCV +#include +#include +#include +// Eigen library +#include +// c++ standard libraries +#include +#include +#include +#include +#include +#include +// services +#include + +// Class that provides a service server to check which areas have been covered, when going along the given poses. It returns an image that +// has all covered areas drawn in as 127. +// REMARK: The given map has to be a 8 bit single channel image, with 0 as obstacle and 255 as free space drawn in. +// If wanted, the server can check how often a pixel has been covered during the execution. When this is wished, an image is returned, in +// which the number of observations (or coverages) is assigned to each pixel (32 bit image in this case). +class CoverageCheckServer +{ +protected: + // node handle + ros::NodeHandle node_handle_; + + // Function to draw the covered areas into the given map. This is done by going through all given robot-poses and calculating + // the field of view. The field of view is given in robot base coordinates (x-axis shows to the front and y-axis to left side). + // The function then calculates the field_of_view in the global frame by using the given robot pose. + // After this the function does a ray casting from the field of view origin (i.e. the camera location expressed in the robot base coordinate system) + // to each cell of the field of view in order to verify whether the cell has been obstructed by an obstacle and could not be observed. + // This ensures that no point is wrongly classified as seen. + // @param fov_origin The mounting position of the camera spanning the field of view, given in robot base coordinates, in [m] + void drawCoveredPointsPolygon(cv::Mat& reachable_areas_map, const std::vector& robot_poses, + const std::vector>& field_of_view_points, const Eigen::Matrix& fov_origin, + const float map_resolution, const cv::Point2d map_origin, cv::Mat* number_of_coverages_image=NULL); + + // Function that takes the given robot poses and draws the circular footprint with coverage_radius at these positions into the given map. + // Used when the server should plan a coverage path for the robot coverage area (a circle). This drawing function does not test occlusions + // since a footprint can usually be assumed to reach all covered map positions (otherwise it would be a collision). + void drawCoveredPointsCircle(cv::Mat& reachable_areas_map, const std::vector& robot_poses, + const double coverage_radius, const float map_resolution, + const cv::Point2d map_origin, cv::Mat* number_of_coverages_image=NULL); + + // reduces image coordinates of a point to valid values inside the image, i.e. between [0,rows-1]/[0,cols-1] + cv::Point clampImageCoordinates(const cv::Point& p, const int rows, const int cols); + + // ros server object + ros::ServiceServer coverage_check_server_; +public: + // constructor + CoverageCheckServer(); + CoverageCheckServer(ros::NodeHandle nh); + + // callback function for the server + bool checkCoverage(ipa_building_msgs::CheckCoverageRequest& request, ipa_building_msgs::CheckCoverageResponse& response); + + // ROS-independent coverage check library interface + bool checkCoverage(const cv::Mat& map, const float map_resolution, const cv::Point2d& map_origin, const std::vector& path, + const std::vector >& field_of_view, const Eigen::Matrix& fov_origin, const float coverage_radius, + const bool check_for_footprint, const bool check_number_of_coverages, cv::Mat& coverage_map, cv::Mat& number_of_coverage_image); + +}; diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/include/ipa_room_exploration/dynamic_reconfigure_client.h b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/include/ipa_room_exploration/dynamic_reconfigure_client.h new file mode 100644 index 0000000..8c9907f --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/include/ipa_room_exploration/dynamic_reconfigure_client.h @@ -0,0 +1,248 @@ +/*! + ***************************************************************** + * \file + * + * \note + * Copyright (c) 2015 \n + * Fraunhofer Institute for Manufacturing Engineering + * and Automation (IPA) \n\n + * + ***************************************************************** + * + * \note + * Project name: Care-O-bot + * \note + * ROS stack name: autopnp + * \note + * ROS package name: ipa_room_segmentation + * + * \author + * Author: Richard Bormann + * \author + * Supervised by: Richard Bormann + * + * \date Date of creation: 08.2016 + * + * \brief + * + * + ***************************************************************** + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * - Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. \n + * - Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. \n + * - Neither the name of the Fraunhofer Institute for Manufacturing + * Engineering and Automation (IPA) nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. \n + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License LGPL as + * published by the Free Software Foundation, either version 3 of the + * License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License LGPL for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License LGPL along with this program. + * If not, see . + * + ****************************************************************/ + +#ifndef _DYNAMIC_RECONFIGURE_CLIENT_H_ +#define _DYNAMIC_RECONFIGURE_CLIENT_H_ + +#include +#include + +#include + +#include +#include +#include +#include + +class DynamicReconfigureClient +{ +public: + DynamicReconfigureClient(ros::NodeHandle& nh, const std::string& dynamic_reconfigure_service_name, const std::string& parameter_updates_topic) + : dynamic_reconfigure_current_config_received_(false), node_handle_(nh), dynamic_reconfigure_service_name_(dynamic_reconfigure_service_name) + { + dynamic_reconfigure_sub_ = node_handle_.subscribe(parameter_updates_topic, 1, &DynamicReconfigureClient::dynamic_reconfigure_current_config_callback, this); + + // receive current configuration + ros::Duration sleep_rate(0.5); + while (dynamic_reconfigure_current_config_received_ == false) + { + ros::spinOnce(); + sleep_rate.sleep(); + } + } + + dynamic_reconfigure::Config& getConfig() + { + boost::mutex::scoped_lock lock(dynamic_reconfigure_lock_); + return dynamic_reconfigure_config_; + } + + bool setConfig(const std::string& param_name, const bool param_value) + { + boost::mutex::scoped_lock lock(dynamic_reconfigure_lock_); + + if (dynamic_reconfigure_current_config_received_ == false) + { + ROS_WARN("DynamicReconfigureClient: Did not receive the current configuration, yet."); + return false; + } + + bool found = false; + for (size_t i=0; i. + * + ****************************************************************/ + +#pragma once + +// c++ standard libraries +#include +#include +#include +#include +#include +#include +// Ros +#include +// service +#include +// OpenCv +#include +#include +// msgs +#include +#include +// Eigen library +#include +// specific from this package +#include + + +#define PI 3.14159265359 + +// Function that provides the functionality that a given field of view (fov) path gets mapped to a robot path by using the given parameters. +// To do so simply a vector operation is applied. If the computed robot pose is not in the free space, another accessible +// point is generated by finding it on the radius around the fov middlepoint s.t. the distance to the last robot position +// is minimized. +// Important: the room map needs to be an unsigned char single channel image, if inaccessible areas should be excluded, provide the inflated map +// robot_to_fov_vector in [m] +// returns robot_path in [m,m,rad] +void mapPath(const cv::Mat& room_map, std::vector& robot_path, + const std::vector& fov_path, const Eigen::Matrix& robot_to_fov_vector, + const double map_resolution, const cv::Point2d map_origin, const cv::Point& starting_point); + +// computes the field of view center and the radius of the maximum incircle of a given field of view quadrilateral +// fitting_circle_center_point_in_meter this is also considered the center of the field of view, because around this point the maximum radius incircle can be found that is still inside the fov +// fov_resolution resolution of the fov center and incircle computations, in [pixels/m] +void computeFOVCenterAndRadius(const std::vector >& fov_corners_meter, float& fitting_circle_radius_in_meter, + Eigen::Matrix& fitting_circle_center_point_in_meter, const double fov_resolution=1000); diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/include/ipa_room_exploration/room_exploration_action_server.h b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/include/ipa_room_exploration/room_exploration_action_server.h new file mode 100644 index 0000000..84e63fb --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/include/ipa_room_exploration/room_exploration_action_server.h @@ -0,0 +1,325 @@ +/*! + ***************************************************************** + * \file + * + * \note + * Copyright (c) 2016 \n + * Fraunhofer Institute for Manufacturing Engineering + * and Automation (IPA) \n\n + * + ***************************************************************** + * + * \note + * Project name: Care-O-bot + * \note + * ROS stack name: autopnp + * \note + * ROS package name: ipa_room_exploration + * + * \author + * Author: Florian Jordan + * \author + * Supervised by: Richard Bormann + * + * \date Date of creation: 03.2016 + * + * \brief + * + * + ***************************************************************** + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * - Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. \n + * - Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. \n + * - Neither the name of the Fraunhofer Institute for Manufacturing + * Engineering and Automation (IPA) nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. \n + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License LGPL as + * published by the Free Software Foundation, either version 3 of the + * License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License LGPL for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License LGPL along with this program. + * If not, see . + * + ****************************************************************/ + +#pragma once + + +// Ros specific +#include +#include +#include +#include +#include +#include +#include +#include +// OpenCV specific +#include +#include +// Eigen library +#include +#include +// standard c++ libraries +#include +#include +#include +#include +#include +#include +// services and actions +#include +#include +#include +// messages +#include +#include +#include +#include +#include +#include +#include +#include +// specific from this package +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include "std_msgs/String.h" +#include +#include "ipa_building_msgs/dis_info.h" +#include "ipa_building_msgs/dis_info_array.h" + + + +#define PI 3.14159265359 + +typedef actionlib::SimpleActionClient MoveBaseClient; + +class RoomExplorationServer +{ +protected: + + int planning_mode_; // 1 = plans a path for coverage with the robot footprint, 2 = plans a path for coverage with the robot's field of view + + ros::Publisher path_pub_; // a publisher sending the path as a nav_msgs::Path before executing + ros::Publisher cmd_vel_pub_; + ros::Publisher cancel_pub_; + + geometry_msgs::Twist cmd_vel_msg; + actionlib_msgs::GoalID cancel_msg; + geometry_msgs::Twist cmd_vel_; + + GridPointExplorator grid_point_planner; // object that uses the grid point method to plan a path trough a room + BoustrophedonExplorer boustrophedon_explorer_; // object that uses the boustrophedon exploration method to plan a path trough the room + NeuralNetworkExplorator neural_network_explorator_; // object that uses the neural network method to create an exploration path + convexSPPExplorator convex_SPP_explorator_; // object that uses the convex spp exploration methd to create an exploration path + FlowNetworkExplorator flow_network_explorator_; // object that uses the flow network exploration method to create an exploration path + EnergyFunctionalExplorator energy_functional_explorator_; // object that uses the energy functional exploration method to create an exploration path + BoustrophedonVariantExplorer boustrophedon_variant_explorer_; // object that uses the boustrophedon variant exploration method to plan a path trough the room + + // parameters + int room_exploration_algorithm_; // variable to specify which algorithm is going to be used to plan a path + // 1: grid point explorator + // 2: boustrophedon explorator + // 3: neural network explorator + // 4: convexSPP explorator + // 5: flowNetwork explorator + // 6: energyFunctional explorator + // 7: Voronoi explorator + // 8: boustrophedon variant explorator + bool display_trajectory_; // display final trajectory plan step by step + + // parameters on map correction + int map_correction_closing_neighborhood_size_; // Applies a closing operation to neglect inaccessible areas and map errors/artifacts if the + // map_correction_closing_neighborhood_size parameter is larger than 0. + // The parameter then specifies the iterations (or neighborhood size) of that closing operation. + + // parameters specific to the navigation of the robot along the computed coverage trajectory + bool return_path_; // boolean used to determine if the server should return the computed coverage path in the response message + bool execute_path_; // boolean used to determine whether the server should navigate the robot along the computed coverage path + double goal_eps_; // distance between the published navigation goal and the robot to publish the next + // navigation goal in the path + bool use_dyn_goal_eps_; // using a dynamic goal distance criterion: the larger the path's curvature, the more accurate the navigation + bool interrupt_navigation_publishing_; // variable that interrupts the publishing of navigation goals as long as needed, e.g. when during the execution + // of the coverage path a trash bin is found and one wants to empty it directly and resume the path later. + bool revisit_areas_; // variable that turns functionality on/off to revisit areas that haven't been seen during the + // execution of the coverage path, due to uncertainties or dynamic obstacles + double left_sections_min_area_; // variable to determine the minimal area that not seen sections must have before they + // are revisited after one run through the room + std::string global_costmap_topic_; // name of the global costmap topic + std::string coverage_check_service_name_; // name of the service to call for a coverage check of the driven trajectory + std::string map_frame_; // string that carries the name of the map frame, used for tracking of the robot + std::string camera_frame_; // string that carries the name of the camera frame, that is in the same kinematic chain as the map_frame and shows the camera pose + + // parameters specific to the grid point explorator + int tsp_solver_; // indicates which TSP solver should be used + // 1 = Nearest Neighbor + // 2 = Genetic solver + // 3 = Concorde solver + int64_t tsp_solver_timeout_; // a sophisticated solver like Concorde or Genetic can be interrupted if it does not find a solution within this time, in [s], and then falls back to the nearest neighbor solver + + // parameters specific for the boustrophedon explorator + double min_cell_area_; // minimal area a cell can have, when using the boustrophedon explorator + double path_eps_; // the distance between points when generating a path + double grid_obstacle_offset_; // in [m], the additional offset of the grid to obstacles, i.e. allows to displace the grid by more than the standard half_grid_size from obstacles + int max_deviation_from_track_; // in [pixel], maximal allowed shift off the ideal boustrophedon track to both sides for avoiding obstacles on track + // setting max_deviation_from_track=grid_spacing is usually a good choice + // for negative values (e.g. max_deviation_from_track: -1) max_deviation_from_track is automatically set to grid_spacing + int cell_visiting_order_; // cell visiting order + // 1 = optimal visiting order of the cells determined as TSP problem + // 2 = alternative ordering from left to right (measured on y-coordinates of the cells), visits the cells in a more obvious fashion to the human observer (though it is not optimal) + + + // parameters specific for the neural network explorator, see "A Neural Network Approach to Complete Coverage Path Planning" from Simon X. Yang and Chaomin Luo + double step_size_; // step size for integrating the state dynamics + int A_; // decaying parameter that pulls the activity of a neuron closer to zero, larger value means faster decreasing + int B_; // increasing parameter that tries to increase the activity of a neuron when it's not too big already, higher value means a higher desired value and a faster increasing at the beginning + int D_; // decreasing parameter when the neuron is labeled as obstacle, higher value means faster decreasing + int E_; // external input parameter of one neuron that is used in the dynamics corresponding to if it is an obstacle or uncleaned/cleaned, E>>B + double mu_; // parameter to set the importance of the states of neighboring neurons to the dynamics, higher value means higher influence + double delta_theta_weight_; // parameter to set the importance of the traveleing direction from the previous step and the next step, a higher value means that the robot should turn less + + // parameters specific for the convexSPP explorator + int cell_size_; // size of one cell that is used to discretize the free space + double delta_theta_; // sampling angle when creating possible sensing poses in the convexSPP explorator + + // parameters specific for the flowNetwork explorator + double curvature_factor_; // double that shows the factor, an arc can be longer than a straight arc when using the flowNetwork explorator + double max_distance_factor_; // double that shows how much an arc can be longer than the maximal distance of the room, which is determined by the min/max coordinates that are set in the goal + + + // callback function for dynamic reconfigure + void dynamic_reconfigure_callback(ipa_room_exploration::RoomExplorationConfig &config, uint32_t level); + + // this is the execution function used by action server + void exploreRoom(const ipa_building_msgs::RoomExplorationGoalConstPtr &goal); + + // remove unconnected, i.e. inaccessible, parts of the room (i.e. obstructed by furniture), only keep the room with the largest area + bool removeUnconnectedRoomParts(cv::Mat& room_map); + + // clean path from subsequent double occurrences of the same pose + // min_dist_squared is the squared minimum distance between two points on the trajectory, in [pixel] (i.e. grid cells) + void downsampleTrajectory(const std::vector& path_uncleaned, std::vector& path, const double min_dist_squared); + + + // excute the planned trajectory and drive to unexplored areas after moving along the computed path + void navigateExplorationPath(const std::vector& exploration_path, const std::vector& field_of_view, + const geometry_msgs::Point32& field_of_view_origin, const double coverage_radius, const double distance_robot_fov_middlepoint, + const float map_resolution, const geometry_msgs::Pose& map_origin, const double grid_spacing_in_pixel, const double map_height); + + // function to publish a navigation goal, it returns true if the goal could be reached + // eps_x and eps_y are used to define a epsilon neighborhood around the goal in which a new nav_goal gets published + // --> may smooth the process, move_base often slows before and stops at the goal + bool publishNavigationGoal(const geometry_msgs::Pose2D& nav_goal, const std::string map_frame, + const std::string camera_frame, std::vector& robot_poses, + const double robot_to_fov_middlepoint_distance, const double eps = 0.0, + const bool perimeter_check = false); + + // converter-> Pixel to meter for X coordinate + double convertPixelToMeterForXCoordinate(const int pixel_valued_object_x, const float map_resolution, const cv::Point2d map_origin) + { + double meter_value_obj_x = (pixel_valued_object_x * map_resolution) + map_origin.x; + return meter_value_obj_x; + } + // converter-> Pixel to meter for Y coordinate + double convertPixelToMeterForYCoordinate(int pixel_valued_object_y, const float map_resolution, const cv::Point2d map_origin) + { + double meter_value_obj_y = (pixel_valued_object_y * map_resolution) + map_origin.y; + return meter_value_obj_y; + } + + // function to transform the given map in a way s.t. the OpenCV and room coordinate system are the same + // --> the map_saver from ros saves maps as images with the origin laying in the lower left corner of it, but openCV assumes + // that the origin is in the upper left corner, also they are rotated around the image-x-axis about each other + void transformImageToRoomCordinates(cv::Mat& map) + { + cv::Point2f src_center(map.cols/2.0F, map.rows/2.0F); + cv::Mat rot_mat = getRotationMatrix2D(src_center, 180, 1.0); + cv::Mat dst; + cv::warpAffine(map, dst, rot_mat, map.size()); + cv::flip(dst, map, 1); + } + + // function to create an occupancyGrid map out of a given cv::Mat + void matToMap(nav_msgs::OccupancyGrid &map, const cv::Mat &mat) + { + map.info.width = mat.cols; + map.info.height = mat.rows; + map.data.resize(mat.cols*mat.rows); + + for(int x=0; x(y,x)?0:100; + } + + // function to create a cv::Mat out of a given occupancyGrid map + void mapToMat(const nav_msgs::OccupancyGrid &map, cv::Mat &mat) + { + mat = cv::Mat(map.info.height, map.info.width, CV_8U); + + for(int x=0; x(y,x) = map.data[y*mat.cols+x]; + } + + + // !!Important!! + // define the Nodehandle before the action server, or else the server won't start + // + ros::NodeHandle node_handle_; + actionlib::SimpleActionServer room_exploration_server_; + dynamic_reconfigure::Server room_exploration_dynamic_reconfigure_server_; + + + +private: + void turn(int lr,int time);//rad/s ,0.1s + void gostraight(int time);// 0.1s + void publishZeroVelocity();//Publishes a velocity command of zero to the base + + + bool flag_=0; + +public: + enum PlanningMode {PLAN_FOR_FOOTPRINT=1, PLAN_FOR_FOV=2}; + ros::Subscriber obstacles_sub_; + ros::Subscriber cmd_vel_sub_; + + void stereo_CB(const ipa_building_msgs::dis_info_array::ConstPtr& flag); + void cmd_CB(const geometry_msgs::Twist::ConstPtr& cmd_vel); + + + // initialize the action-server + RoomExplorationServer(ros::NodeHandle nh, std::string name_of_the_action); + + // default destructor for the class + ~RoomExplorationServer(void) + { + } +}; diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/launch/amcl.launch b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/launch/amcl.launch new file mode 100644 index 0000000..972efe1 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/launch/amcl.launch @@ -0,0 +1,46 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/launch/clean_work.launch b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/launch/clean_work.launch new file mode 100644 index 0000000..e3af067 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/launch/clean_work.launch @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/launch/cob_map_accessibility_analysis.launch b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/launch/cob_map_accessibility_analysis.launch new file mode 100644 index 0000000..c8eb094 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/launch/cob_map_accessibility_analysis.launch @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/launch/cob_map_accessibility_analysis_params.yaml b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/launch/cob_map_accessibility_analysis_params.yaml new file mode 100644 index 0000000..cb9501e --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/launch/cob_map_accessibility_analysis_params.yaml @@ -0,0 +1,19 @@ +# if true, the path to a goal position must be accessible as well +# bool +approach_path_accessibility_check: true + +# link name of the map in tf +# string +map_link_name: "/map" + +# link name of the robot base in tf +# string +robot_base_link_name: "/base_footprint" + +# update rate for incoming messages about obstacles (provided in [Hz]) +# double +obstacle_topic_update_rate: 5.0 + +# publish the inflated map for viewing purposes +# bool +publish_inflated_map: false diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/launch/coverage_check_server.launch b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/launch/coverage_check_server.launch new file mode 100644 index 0000000..ebf28a8 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/launch/coverage_check_server.launch @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/launch/coverage_monitor_server.launch b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/launch/coverage_monitor_server.launch new file mode 100644 index 0000000..cb9efb4 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/launch/coverage_monitor_server.launch @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/launch/coverage_monitor_server_params.yaml b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/launch/coverage_monitor_server_params.yaml new file mode 100644 index 0000000..0a58600 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/launch/coverage_monitor_server_params.yaml @@ -0,0 +1,19 @@ +# name of the map coordinate system +# string +map_frame: "map" + +# name of the robot base frame +# string +robot_frame: "base_link" + +# radius of the circular coverage device, in [m] +# double +coverage_radius: 0.25 + +# the offset of the coverage circle from the robot center, [x,y,z] in [m] +# double[3] +coverage_circle_offset_transform: [0.29035, -0.114, 0.] + +# the robot trajectory is only recorded if this is true, usually it should be false on startup (can also be set from dynamic reconfigure) +# bool +robot_trajectory_recording_active: false \ No newline at end of file diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/launch/gazebo.launch b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/launch/gazebo.launch new file mode 100644 index 0000000..b007551 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/launch/gazebo.launch @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/launch/room_exploration_action_server.launch b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/launch/room_exploration_action_server.launch new file mode 100644 index 0000000..10b3b51 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/launch/room_exploration_action_server.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/launch/room_exploration_action_server_params.yaml b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/launch/room_exploration_action_server_params.yaml new file mode 100644 index 0000000..a78b786 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/launch/room_exploration_action_server_params.yaml @@ -0,0 +1,167 @@ +# this variable selects the algorithm for room exploration +# 1 = grid point explorator +# 2 = boustrophedon explorator +# 3 = neural network explorator +# 4 = convexSPP explorator +# 5 = flowNetwork explorator +# 6 = energyFunctional explorator +# 7 = voronoi explorator +# 8 = boustrophedon variant explorator +# int +room_exploration_algorithm: 8 + +# display final trajectory plan step by step +# bool +display_trajectory: false #false + + +# map correction +# ============== +# Applies a closing operation to neglect inaccessible areas and map errors/artifacts if the map_correction_closing_neighborhood_size parameter is larger than 0. +# The parameter then specifies the iterations (or neighborhood size) of that closing operation. +# int +map_correction_closing_neighborhood_size: 2 + + +# parameters specific to the navigation of the robot along the computed coverage trajectory +# ========================================================================================= +# boolean used to determine if the server should return the computed coverage path in the response message +# bool +return_path: true + +# boolean used to determine whether the server should navigate the robot along the computed coverage path +# bool +# execute_path: false +execute_path: true + +# min distance between the robot and the published navigation goal before the next goal in the path gets published +# hack for slow turtle sim (publish new goals already when close to goal, then takes some time to command new goal) +# can be turned off (0) when used with real robot +# double +# [pixel], where one pixel is the size of one cell in the navigation grid map +goal_eps: 0.0 #0.35 + +# use a dynamic goal distance criterion: the larger the path's curvature, the more accurate the navigation. +# Use 'goal_eps' param as maximum distance on straight paths and zero distance (accurate goal approaching) on 90° curves. +# bool +use_dyn_goal_eps: true #false + +# variable that interrupts the publishing of navigation goals as long as needed, e.g. when during the execution of the coverage path +# a trash bin is found and one wants to empty it directly and resume the path later. +# bool +interrupt_navigation_publishing: false + +# this variable turns the functionality on/off to revisit areas that haven't been seen during the execution of the coverage path, +# due to uncertainties or dynamic obstacles +# bool +revisit_areas: false + +# minimal area of not seen sections that they are revisited after the first run through the room +# double +# [m^2] +left_sections_min_area: 0.01 + +# name of the global costmap topic +# string +global_costmap_topic: "/move_base/global_costmap/costmap" + +# name of the service to call for a coverage check of the driven trajectory +# string +coverage_check_service_name: "/room_exploration/coverage_check_server/coverage_check" + +# string that carries the name of the map frame, used for tracking of the robot +# string +map_frame: "map" + +# string that carries the name of the camera frame, that is in the same kinematic chain as the map_frame and shows the camera pose +# string +camera_frame: "base_link" + + +# parameters specific to the grid point explorator +# ================================================= +# indicates which TSP solver should be used +# 1 = Nearest Neighbor (often 10-15% longer paths than Concorde but computes by orders faster and considering traveling time (path length and rotations) it is often the fastest of all) +# 2 = Genetic solver (slightly shorter than Nearest Neighbor) +# 3 = Concorde solver (usually gives the shortest path while computing the longest) +# int +tsp_solver: 1 + +# a sophisticated solver like Concorde or Genetic can be interrupted if it does not find a solution within this time, in [s], +# and then falls back to the nearest neighbor solver +# int [s] +tsp_solver_timeout: 600 + + +# parameters specific for the boustrophedon explorator +# ==================================================== +# min area a boustrophedon cell (i.e. a boustrophedon region of the map) must have to be determined for the path generation +# [pixel^2] +# double +min_cell_area: 200.0 #10.0 + +# distance between two points when generating the (line) path +# [pixel], where one pixel is the size of one cell in the navigation grid map +# double +path_eps: 6.0 + +# the additional offset of the grid to obstacles, i.e. allows to displace the grid by more than the standard half_grid_size from obstacles +# [m] +# double +grid_obstacle_offset: 0.251 #0.101 #0.251 + +# maximal allowed shift off the ideal boustrophedon track to both sides for avoiding obstacles on track +# setting max_deviation_from_track=grid_spacing is usually a good choice +# for negative values (e.g. max_deviation_from_track: -1) max_deviation_from_track is automatically set to grid_spacing +# [pixel] +max_deviation_from_track: -1 #0 + +# cell visiting order +# 1 = the optimal visiting order of the cells is determined as TSP problem +# 2 = alternative ordering from left to right (measured on y-coordinates of the cells), visits the cells in a more obvious fashion to the human observer (though it is not optimal) +# int +cell_visiting_order: 2 + +# parameters specific for the neural network explorator, see "A Neural Network Approach to Complete Coverage Path Planning" from Simon X. Yang and Chaomin Luo +# ===================================================== +# step size for integrating the state dynamics +step_size: 0.008 +# decaying parameter that pulls the activity of a neuron closer to zero, larger value means faster decreasing +A: 17 +# increasing parameter that tries to increase the activity of a neuron when it's not too big already, higher value means a higher desired value and a faster increasing at the beginning +B: 5 +# decreasing parameter when the neuron is labeled as obstacle, higher value means faster decreasing +D: 7 +# external input parameter of one neuron that is used in the dynamics corresponding to if it is an obstacle or uncleaned/cleaned, E>>B +E: 80 +# parameter to set the importance of the states of neighboring neurons to the dynamics, higher value means higher influence +mu: 1.03 +# parameter to set the importance of the traveling direction from the previous step and the next step, a higher value means that the robot should turn less +delta_theta_weight: 0.15 + +# parameters specific for the convexSPP explorator +# ================================================ +# size of one cell when discretizing the free space, smaller value means a more detailed discretization +# in [pixel], where one pixel is the size of one cell in the navigation grid map +# (if set to 0 or smaller, the cell size is automatically determined with the largest possible value) +# (this parameter is only relevant to the algorithms #4 = convexSPP explorator, #5 = flowNetwork explorator, +# the other methods utilize the computed optimal maximum grid size) +# int +cell_size: 0 #4 #7 + +# angular sampling step when creating possible sensing poses, in [rad] +# double +delta_theta: 0.78539816339 #1.570796 + +# parameters specific for the flowNetwork explorator +# ================================================== +# factor, an arc can be longer than a straight arc, higher values allow more arcs to be considered in the optimization problem +# Important: if too small, maybe the problem becomes infeasible and the optimizer won't work correctly +curvature_factor: 1.1 + +# factor, an arc can be longer than the maximal distance of the room, which is determined by the min/max coordinates that are set in the goal +max_distance_factor: 1.0 + +# cell_size: see # parameters specific for the convexSPP explorator + +# path_eps: see # parameters specific for the boustrophedon explorator diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/launch/talkerUltraSound.launch b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/launch/talkerUltraSound.launch new file mode 100644 index 0000000..25798fd --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/launch/talkerUltraSound.launch @@ -0,0 +1,6 @@ + + + + + \ No newline at end of file diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/launch/turtlebot3_navigation.launch b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/launch/turtlebot3_navigation.launch new file mode 100644 index 0000000..eab8352 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/launch/turtlebot3_navigation.launch @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/maps/sim.pgm b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/maps/sim.pgm new file mode 100644 index 0000000..cfb5c7d --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/maps/sim.pgm @@ -0,0 +1,5 @@ +P5 +# CREATOR: map_saver.cpp 0.050 m/pix +1984 1984 +255 + \ No newline at end of file diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/maps/sim.yaml b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/maps/sim.yaml new file mode 100644 index 0000000..c4fd1ab --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/maps/sim.yaml @@ -0,0 +1,7 @@ +image: sim.pgm +resolution: 0.050000 +origin: [-50.000000, -50.000000, 0.000000] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 + diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/rviz/turtlebot3_karto.rviz b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/rviz/turtlebot3_karto.rviz new file mode 100644 index 0000000..019ee20 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/rviz/turtlebot3_karto.rviz @@ -0,0 +1,295 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /TF1/Frames1 + - /Path2 + - /Map2 + - /Map3 + Splitter Ratio: 0.594406009 + Tree Height: 776 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679016 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: LaserScan +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.0299999993 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 100 + Reference Frame: map + 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_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_scan: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + caster_back_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + wheel_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wheel_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz/TF + Enabled: false + Frame Timeout: 15 + Frames: + All Enabled: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + {} + Update Interval: 0 + 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/LaserScan + Color: 0; 255; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 11799 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.0300000012 + Style: Flat Squares + Topic: /scan + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /raspicam_node/image + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: compressed + Unreliable: false + Value: false + - Alpha: 0.699999988 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: /map + Unreliable: false + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.300000012 + Head Length: 0.200000003 + Length: 0.300000012 + Line Style: Billboards + Line Width: 0.0299999993 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.0299999993 + Shaft Diameter: 0.100000001 + Shaft Length: 0.100000001 + Topic: /move_base/NavfnROS/plan + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 85; 0; 255 + Enabled: true + Head Diameter: 0.300000012 + Head Length: 0.200000003 + Length: 0.300000012 + Line Style: Billboards + Line Width: 0.0299999993 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.0299999993 + Shaft Diameter: 0.100000001 + Shaft Length: 0.100000001 + Topic: /move_base/DWAPlannerROS/local_plan + Unreliable: false + Value: true + - Alpha: 0.699999988 + Class: rviz/Map + Color Scheme: costmap + Draw Behind: false + Enabled: false + Name: Map + Topic: /move_base/global_costmap/costmap + Unreliable: false + Use Timestamp: false + Value: false + - Alpha: 0.699999988 + Class: rviz/Map + Color Scheme: costmap + Draw Behind: false + Enabled: false + Name: Map + Topic: /move_base/local_costmap/costmap + Unreliable: false + Use Timestamp: false + Value: false + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: map + 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 + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Angle: -1.57079637 + Class: rviz/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.0599999987 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.00999999978 + Scale: 136.475739 + Target Frame: map + Value: TopDownOrtho (rviz) + X: -0.542091846 + Y: 0.0558035932 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1056 + Hide Left Dock: false + Hide Right Dock: true + Image: + collapsed: false + QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065000000020f000001900000001600ffffff000000010000010f0000035cfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000430000035c000000ac00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000005cf0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1855 + X: 65 + Y: 24 diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/rviz/turtlebot3_navigation.rviz b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/rviz/turtlebot3_navigation.rviz new file mode 100644 index 0000000..ea1fbca --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/rviz/turtlebot3_navigation.rviz @@ -0,0 +1,571 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /TF1/Frames1 + - /TF1/Tree1 + - /Global Map1/Costmap1 + - /Local Map1/Polygon1 + - /Local Map1/Costmap1 + - /Local Map1/Planner1 + - /Amcl Particles1 + - /Range1 + - /Imu1 + Splitter Ratio: 0.5 + Tree Height: 634 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Name: Time + SyncMode: 0 + SyncSource: "" +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: 20 + 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_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_scan: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_depth_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_depth_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_rgb_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_rgb_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + caster_back_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + caster_back_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + wheel_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wheel_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz/TF + Enabled: false + Filter (blacklist): "" + Filter (whitelist): "" + Frame Timeout: 15 + Frames: + All Enabled: false + Marker Alpha: 1 + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + {} + Update Interval: 0 + 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/LaserScan + Color: 89; 28; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: LaserScan + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.030000001192092896 + Style: Flat Squares + Topic: /scan + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz/Image + Enabled: false + Image Topic: /raspicam_node/image + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: compressed + Unreliable: false + Value: false + - Alpha: 0.699999988079071 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: /map + Unreliable: false + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 0; 0; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Planner Plan + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /move_base/GlobalPlanner/plan + Unreliable: false + Value: true + - Class: rviz/Group + Displays: + - Alpha: 0.699999988079071 + Class: rviz/Map + Color Scheme: costmap + Draw Behind: true + Enabled: true + Name: Costmap + Topic: /move_base/global_costmap/costmap + Unreliable: false + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 0; 0 + Enabled: false + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Planner + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /move_base/NavfnROS/plan + Unreliable: false + Value: false + Enabled: true + Name: Global Map + - Class: rviz/Group + Displays: + - Alpha: 1 + Class: rviz/Polygon + Color: 0; 0; 0 + Enabled: true + Name: Polygon + Queue Size: 10 + Topic: /move_base/local_costmap/footprint + Unreliable: false + Value: true + - Alpha: 0.699999988079071 + Class: rviz/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Costmap + Topic: /move_base/local_costmap/costmap + Unreliable: false + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Planner + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /move_base/DWAPlannerROS/local_plan + Unreliable: false + Value: true + Enabled: false + Name: Local Map + - Alpha: 1 + Arrow Length: 0.05000000074505806 + Axes Length: 0.30000001192092896 + Axes Radius: 0.009999999776482582 + Class: rviz/PoseArray + Color: 0; 192; 0 + Enabled: false + Head Length: 0.07000000029802322 + Head Radius: 0.029999999329447746 + Name: Amcl Particles + Queue Size: 10 + Shaft Length: 0.23000000417232513 + Shaft Radius: 0.009999999776482582 + Shape: Arrow (Flat) + Topic: /particlecloud + Unreliable: false + Value: false + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.05000000074505806 + Head Radius: 0.10000000149011612 + Name: Goal + Queue Size: 10 + Shaft Length: 0.05000000074505806 + Shaft Radius: 0.05000000074505806 + Shape: Arrow + Topic: /move_base_simple/goal + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /room_exploration/room_exploration_server/coverage_path + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 170; 0; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.3499999940395355 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /move_base/DWAPlannerROS/local_plan + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 255; 127 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /move_base/DWAPlannerROS/global_plan + Unreliable: false + Value: true + - Class: rviz/Camera + Enabled: false + Image Rendering: background and overlay + Image Topic: /camera/rgb/image_raw + Name: Camera + Overlay Alpha: 0.5 + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Visibility: + "": true + Amcl Particles: true + Camera: true + Global Map: + Costmap: true + Planner: true + Value: true + Goal: true + Grid: true + Image: true + LaserScan: true + Local Map: + "": true + Value: true + Map: true + Path: true + Planner Plan: true + Range: true + RobotModel: true + TF: true + Value: true + Zoom Factor: 1 + - Class: rviz/Camera + Enabled: false + Image Rendering: background and overlay + Image Topic: /camera/depth/image_raw + Name: Camera + Overlay Alpha: 0.5 + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Visibility: + "": true + Amcl Particles: true + Camera: true + Global Map: + Costmap: true + Planner: true + Value: true + Goal: true + Grid: true + Image: true + LaserScan: true + Local Map: + "": true + Value: true + Map: true + Path: true + Planner Plan: true + Range: true + RobotModel: true + TF: true + Value: true + Zoom Factor: 1 + - Alpha: 0.5 + Buffer Length: 1 + Class: rviz/Range + Color: 255; 255; 255 + Enabled: true + Name: Range + Queue Size: 10 + Topic: "" + Unreliable: false + Value: true + - Alpha: 0.699999988079071 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: /move_base/local_costmap/costmap + Unreliable: false + Use Timestamp: false + Value: true + - Alpha: 1 + Class: rviz_plugin_tutorials/Imu + Color: 20; 51; 204 + Enabled: true + History Length: 1 + Name: Imu + Queue Size: 10 + Topic: /imu + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/MoveCamera + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/Select + - 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/Measure + Value: true + Views: + Current: + Angle: -0.9057965874671936 + Class: rviz/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Scale: 33.79556655883789 + Target Frame: + X: 10.210562705993652 + Y: 1.2031383514404297 + Saved: ~ +Window Geometry: + Camera: + collapsed: false + Displays: + collapsed: false + Height: 863 + Hide Left Dock: false + Hide Right Dock: true + Image: + collapsed: false + QMainWindow State: 000000ff00000000fd00000004000000000000016a00000305fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000305000000c900fffffffb0000000a0049006d0061006700650000000317000000cc0000001600fffffffb0000000a0049006d0061006700650000000330000000ce0000000000000000fb0000000c00430061006d00650072006100000001ea000000610000001600fffffffb0000000c00430061006d006500720061000000022c000001160000001600ffffff000000010000010f000003a0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000043000003a0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a00000041800fffffffb0000000800540069006d00650100000000000004500000000000000000000002040000030500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 884 + X: 72 + Y: 27 diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/src/coverage_check_server.cpp b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/src/coverage_check_server.cpp new file mode 100644 index 0000000..7c5d1f8 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/src/coverage_check_server.cpp @@ -0,0 +1,216 @@ +#include + +// The default constructors +CoverageCheckServer::CoverageCheckServer() +{ +} + +CoverageCheckServer::CoverageCheckServer(ros::NodeHandle nh) +:node_handle_(nh) +{ + coverage_check_server_ = node_handle_.advertiseService("coverage_check", &CoverageCheckServer::checkCoverage, this); + ROS_INFO("Server for coverage checking initialized....."); +} + +// Callback function for the server. +bool CoverageCheckServer::checkCoverage(ipa_building_msgs::CheckCoverageRequest& request, ipa_building_msgs::CheckCoverageResponse& response) +{ + // convert the map msg to cv format + cv_bridge::CvImagePtr cv_ptr_obj; + std::cout << "request.input_map.encoding:" << request.input_map.encoding << std::endl; + cv_ptr_obj = cv_bridge::toCvCopy(request.input_map, sensor_msgs::image_encodings::MONO8); + cv::Mat map = cv_ptr_obj->image; + + // convert field of view to Eigen format + std::vector > field_of_view; + for(size_t i = 0; i < request.field_of_view.size(); ++i) + { + Eigen::Matrix current_vector; + current_vector << request.field_of_view[i].x, request.field_of_view[i].y; + field_of_view.push_back(current_vector); + } + + // convert field of view origin to Eigen format + Eigen::Matrix fov_origin; + fov_origin << request.field_of_view_origin.x, request.field_of_view_origin.y; + + // convert path to cv format + std::vector path; + for (size_t i=0; i& path, + const std::vector >& field_of_view, const Eigen::Matrix& fov_origin, const float coverage_radius, + const bool check_for_footprint, const bool check_number_of_coverages, cv::Mat& coverage_map, cv::Mat& number_of_coverage_image) +{ + // create a map that stores the number of coverages during the execution, if wanted + cv::Mat* image_pointer = NULL; + if(check_number_of_coverages==true) + { + number_of_coverage_image = cv::Mat::zeros(map.rows, map.cols, CV_32SC1); + image_pointer = &number_of_coverage_image; + ROS_INFO("Checking number of coverages."); + } + + // check if the coverage check should be done for the footprint or the field of view + coverage_map = map.clone(); + if(check_for_footprint==false) + { + ROS_INFO("Checking coverage for FOV."); + drawCoveredPointsPolygon(coverage_map, path, field_of_view, fov_origin, map_resolution, map_origin, image_pointer); + } + else + { + ROS_INFO("Checking coverage for footprint."); + drawCoveredPointsCircle(coverage_map, path, coverage_radius, map_resolution, map_origin, image_pointer); + } + ROS_INFO("Finished coverage check."); + + return true; +} + + +void CoverageCheckServer::drawCoveredPointsPolygon(cv::Mat& reachable_areas_map, const std::vector& robot_poses, + const std::vector >& field_of_view, const Eigen::Matrix& fov_origin, + const float map_resolution, const cv::Point2d map_origin, cv::Mat* number_of_coverages_image) +{ + const float map_resolution_inverse = 1./map_resolution; + + // go trough each given robot pose + for(std::vector::const_iterator current_pose = robot_poses.begin(); current_pose != robot_poses.end(); ++current_pose) + { + // get the rotation matrix + float sin_theta = std::sin(current_pose->z); + float cos_theta = std::cos(current_pose->z); + Eigen::Matrix R; + R << cos_theta, -sin_theta, sin_theta, cos_theta; + + // current pose as Eigen matrix + Eigen::Matrix pose_as_matrix; + pose_as_matrix << current_pose->x, current_pose->y; + + // transform field of view points + std::vector transformed_fov_points; + for(size_t point = 0; point < field_of_view.size(); ++point) + { + // linear transformation + const Eigen::Matrix transformed_fov_point = pose_as_matrix + R * field_of_view[point]; + + // save the transformed point as cv::Point, also check if map borders are satisfied and transform it into pixel values + transformed_fov_points.push_back(clampImageCoordinates(cv::Point((transformed_fov_point(0, 0)-map_origin.x)*map_resolution_inverse, (transformed_fov_point(1, 0)-map_origin.y)*map_resolution_inverse), reachable_areas_map.rows, reachable_areas_map.cols)); + } + + // transform field of view origin + const Eigen::Matrix transformed_fov_origin = pose_as_matrix + R * fov_origin; + const cv::Point transformed_fov_origin_point = clampImageCoordinates(cv::Point((transformed_fov_origin(0, 0)-map_origin.x)*map_resolution_inverse, (transformed_fov_origin(1, 0)-map_origin.y)*map_resolution_inverse), reachable_areas_map.rows, reachable_areas_map.cols); + + // draw current field of view in map + cv::Mat fov_mat = cv::Mat::zeros(reachable_areas_map.rows, reachable_areas_map.cols, reachable_areas_map.type()); + std::vector > contours(1, transformed_fov_points); +#if CV_MAJOR_VERSION<=3 + cv::drawContours(fov_mat, contours, 0, cv::Scalar(255), CV_FILLED); +#else + cv::drawContours(fov_mat, contours, 0, cv::Scalar(255), cv::FILLED); +#endif + + // check visibility for each pixel of the fov area + for (int v=0; v(v,u)==0) + continue; + + // create a line iterator from fov_origin to current fov point and verify visibility + bool point_visible = true; + const cv::Point current_goal(u,v); + cv::LineIterator ray_points(reachable_areas_map, transformed_fov_origin_point, current_goal, 8, false); + for(size_t point = 0; point < ray_points.count; ++point, ++ray_points) + { + if (reachable_areas_map.at(ray_points.pos()) == 0) + { + point_visible = false; + break; + } + } + + // mark visible point in map + if (point_visible == true) + { + reachable_areas_map.at(current_goal) = 127; + + // if wanted, count the coverage + if(number_of_coverages_image!=NULL) + { + number_of_coverages_image->at(current_goal) = number_of_coverages_image->at(current_goal)+1; + } + } + } + } + } +} + + +void CoverageCheckServer::drawCoveredPointsCircle(cv::Mat& reachable_areas_map, const std::vector& robot_poses, + const double coverage_radius, const float map_resolution, + const cv::Point2d map_origin, cv::Mat* number_of_coverages_image) +{ + const float map_resolution_inverse = 1./map_resolution; + + cv::Mat map_copy = reachable_areas_map.clone(); // copy to draw positions that get later drawn into free space of the original map + const int coverage_radius_pixel = coverage_radius*map_resolution_inverse; + // iterate trough all poses and draw them into the given map + for(std::vector::const_iterator pose=robot_poses.begin(); pose!=robot_poses.end(); ++pose) + { + // draw the transformed robot footprint + cv::Point current_point((pose->x-map_origin.x)*map_resolution_inverse, (pose->y-map_origin.y)*map_resolution_inverse); + cv::circle(map_copy, current_point, coverage_radius_pixel, cv::Scalar(127), -1); + + // update the number of visits at this location, if wanted + if(number_of_coverages_image!=NULL) + { + cv::Mat coverage_area = cv::Mat::zeros(map_copy.rows, map_copy.cols, CV_32SC1); + cv::circle(coverage_area, current_point, coverage_radius_pixel, cv::Scalar(1), -1); + *number_of_coverages_image = *number_of_coverages_image + coverage_area; + } + } + + // draw visited areas into free space of the original map + for(size_t y=0; y(y, x) == 255) + reachable_areas_map.at(y, x) = map_copy.at(y, x); +} + + +inline cv::Point CoverageCheckServer::clampImageCoordinates(const cv::Point& p, const int rows, const int cols) +{ + cv::Point c; + c.x = std::min(std::max(p.x, 0), cols-1); + c.y = std::min(std::max(p.y, 0), rows-1); + return c; +} diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/src/coverage_check_server_main.cpp b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/src/coverage_check_server_main.cpp new file mode 100644 index 0000000..9bcc32e --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/src/coverage_check_server_main.cpp @@ -0,0 +1,13 @@ +#include + + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "coverage_check_server"); + ros::NodeHandle nh("~"); + + CoverageCheckServer coverage_checker(nh); + + ros::spin(); + return 0; +} diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/src/coverage_monitor_server.cpp b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/src/coverage_monitor_server.cpp new file mode 100644 index 0000000..b658e0d --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/src/coverage_monitor_server.cpp @@ -0,0 +1,462 @@ +/*! + ***************************************************************** + * \file + * + * \note + * Copyright (c) 2016 \n + * Fraunhofer Institute for Manufacturing Engineering + * and Automation (IPA) \n\n + * + ***************************************************************** + * + * \note + * Project name: Care-O-bot + * \note + * ROS stack name: autopnp + * \note + * ROS package name: ipa_room_exploration + * + * \author + * Author: Richard Bormann + * \author + * Supervised by: + * + * \date Date of creation: 01.2018 + * + * \brief + * + * + ***************************************************************** + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * - Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. \n + * - Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. \n + * - Neither the name of the Fraunhofer Institute for Manufacturing + * Engineering and Automation (IPA) nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. \n + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License LGPL as + * published by the Free Software Foundation, either version 3 of the + * License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License LGPL for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License LGPL along with this program. + * If not, see . + * + ****************************************************************/ + + +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include + +#include + + +class CoverageMonitor +{ +public: + CoverageMonitor(ros::NodeHandle nh) : + node_handle_(nh) + { + // dynamic reconfigure + coverage_monitor_dynamic_reconfigure_server_.setCallback(boost::bind(&CoverageMonitor::dynamicReconfigureCallback, this, _1, _2)); + + // Parameters + std::cout << "\n--------------------------\nCoverage Monitor Parameters:\n--------------------------\n"; + node_handle_.param("map_frame", map_frame_, "map"); + std::cout << "coverage_monitor/map_frame = " << map_frame_ << std::endl; + node_handle_.param("robot_frame", robot_frame_, "base_link"); + std::cout << "coverage_monitor/robot_frame = " << robot_frame_ << std::endl; + node_handle_.param("coverage_radius", coverage_radius_, 0.25); + std::cout << "coverage_monitor/coverage_radius = " << coverage_radius_ << std::endl; + coverage_circle_offset_transform_.setIdentity(); + std::vector temp; + node_handle_.getParam("coverage_circle_offset_translation", temp); + if (temp.size() == 3) + coverage_circle_offset_transform_.setOrigin(tf::Vector3(temp[0], temp[1], temp[2])); + else + coverage_circle_offset_transform_.setOrigin(tf::Vector3(0.29035, -0.114, 0.)); + node_handle_.param("robot_trajectory_recording_active", robot_trajectory_recording_active_, false); + std::cout << "coverage_monitor/robot_trajectory_recording_active = " << robot_trajectory_recording_active_ << std::endl; + + // setup publishers and subscribers + coverage_marker_pub_ = node_handle_.advertise("coverage_marker", 1); + computed_trajectory_marker_pub_ = node_handle_.advertise("computed_trajectory_marker", 1); + commanded_trajectory_marker_pub_ = node_handle_.advertise("commanded_trajectory_marker", 1); + + computed_trajectory_sub_ = node_handle_.subscribe("computed_target_trajectory_monitor", 500, &CoverageMonitor::computedTrajectoryCallback, this); + commanded_trajectory_sub_ = node_handle_.subscribe("commanded_target_trajectory_monitor", 500, &CoverageMonitor::commandedTrajectoryCallback, this); + + // setup services + start_coverage_monitoring_srv_ = node_handle_.advertiseService("start_coverage_monitoring", &CoverageMonitor::startCoverageMonitoringCallback, this); + stop_coverage_monitoring_srv_ = node_handle_.advertiseService("stop_coverage_monitoring", &CoverageMonitor::stopCoverageMonitoringCallback, this); + get_coverage_image_srv_ = node_handle_.advertiseService("get_coverage_image", &CoverageMonitor::getCoverageImageCallback, this); + + // prepare coverage_marker_msg message + visualization_msgs::Marker coverage_marker_msg; + // Set the frame ID and timestamp. See the TF tutorials for information on these. + coverage_marker_msg.header.frame_id = "/map"; + coverage_marker_msg.header.stamp = ros::Time::now(); + // Set the namespace and id for this marker. This serves to create a unique ID + // Any marker sent with the same namespace and id will overwrite the old one + coverage_marker_msg.ns = "coverage_marker"; + coverage_marker_msg.id = 0; + // Set the marker type. Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER + coverage_marker_msg.type = visualization_msgs::Marker::LINE_STRIP; + // Set the marker action. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL) + coverage_marker_msg.action = visualization_msgs::Marker::ADD; + // Set the pose of the marker. This is a full 6DOF pose relative to the frame/time specified in the header + coverage_marker_msg.pose.position.x = 0; + coverage_marker_msg.pose.position.y = 0; + coverage_marker_msg.pose.position.z = 0; + coverage_marker_msg.pose.orientation.x = 0.0; + coverage_marker_msg.pose.orientation.y = 0.0; + coverage_marker_msg.pose.orientation.z = 0.0; + coverage_marker_msg.pose.orientation.w = 1.0; + // Set the scale of the marker -- 1x1x1 here means 1m on a side + coverage_marker_msg.scale.x = 2*coverage_radius_; // this is the line width + coverage_marker_msg.scale.y = 1.0; + coverage_marker_msg.scale.z = 1.0; + // Set the color -- be sure to set alpha to something non-zero! + coverage_marker_msg.color.r = 0.0f; + coverage_marker_msg.color.g = 1.0f; + coverage_marker_msg.color.b = 0.0f; + coverage_marker_msg.color.a = 0.33; + coverage_marker_msg.lifetime = ros::Duration(); + geometry_msgs::Point p; p.x=0; p.y=0; p.z=0; + coverage_marker_msg.points.push_back(p); + + // prepare computed_trajectory_marker_msg message + visualization_msgs::Marker computed_trajectory_marker_msg; + // Set the frame ID and timestamp. See the TF tutorials for information on these. + computed_trajectory_marker_msg.header.frame_id = "/map"; + computed_trajectory_marker_msg.header.stamp = ros::Time::now(); + // Set the namespace and id for this marker. This serves to create a unique ID + // Any marker sent with the same namespace and id will overwrite the old one + computed_trajectory_marker_msg.ns = "computed_trajectory_marker"; + computed_trajectory_marker_msg.id = 0; + // Set the marker type. Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER + computed_trajectory_marker_msg.type = visualization_msgs::Marker::LINE_STRIP; + // Set the marker action. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL) + computed_trajectory_marker_msg.action = visualization_msgs::Marker::ADD; + // Set the pose of the marker. This is a full 6DOF pose relative to the frame/time specified in the header + computed_trajectory_marker_msg.pose.position.x = 0; + computed_trajectory_marker_msg.pose.position.y = 0; + computed_trajectory_marker_msg.pose.position.z = 0.2; + computed_trajectory_marker_msg.pose.orientation.x = 0.0; + computed_trajectory_marker_msg.pose.orientation.y = 0.0; + computed_trajectory_marker_msg.pose.orientation.z = 0.0; + computed_trajectory_marker_msg.pose.orientation.w = 1.0; + // Set the scale of the marker -- 1x1x1 here means 1m on a side + computed_trajectory_marker_msg.scale.x = 0.06; // this is the line width + computed_trajectory_marker_msg.scale.y = 1.0; + computed_trajectory_marker_msg.scale.z = 1.0; + // Set the color -- be sure to set alpha to something non-zero! + computed_trajectory_marker_msg.color.r = 1.0f; + computed_trajectory_marker_msg.color.g = 0.0f; + computed_trajectory_marker_msg.color.b = 0.0f; + computed_trajectory_marker_msg.color.a = 0.8; + computed_trajectory_marker_msg.lifetime = ros::Duration(); + computed_trajectory_marker_msg.points.push_back(p); + + // prepare commanded_trajectory_marker_msg message + visualization_msgs::Marker commanded_trajectory_marker_msg; + // Set the frame ID and timestamp. See the TF tutorials for information on these. + commanded_trajectory_marker_msg.header.frame_id = "/map"; + commanded_trajectory_marker_msg.header.stamp = ros::Time::now(); + // Set the namespace and id for this marker. This serves to create a unique ID + // Any marker sent with the same namespace and id will overwrite the old one + commanded_trajectory_marker_msg.ns = "commanded_trajectory_marker"; + commanded_trajectory_marker_msg.id = 0; + // Set the marker type. Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER + commanded_trajectory_marker_msg.type = visualization_msgs::Marker::LINE_STRIP; + // Set the marker action. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL) + commanded_trajectory_marker_msg.action = visualization_msgs::Marker::ADD; + // Set the pose of the marker. This is a full 6DOF pose relative to the frame/time specified in the header + commanded_trajectory_marker_msg.pose.position.x = 0; + commanded_trajectory_marker_msg.pose.position.y = 0; + commanded_trajectory_marker_msg.pose.position.z = 0.4; + commanded_trajectory_marker_msg.pose.orientation.x = 0.0; + commanded_trajectory_marker_msg.pose.orientation.y = 0.0; + commanded_trajectory_marker_msg.pose.orientation.z = 0.0; + commanded_trajectory_marker_msg.pose.orientation.w = 1.0; + // Set the scale of the marker -- 1x1x1 here means 1m on a side + commanded_trajectory_marker_msg.scale.x = 0.03; // this is the line width + commanded_trajectory_marker_msg.scale.y = 1.0; + commanded_trajectory_marker_msg.scale.z = 1.0; + // Set the color -- be sure to set alpha to something non-zero! + commanded_trajectory_marker_msg.color.r = 0.0f; + commanded_trajectory_marker_msg.color.g = 0.0f; + commanded_trajectory_marker_msg.color.b = 1.0f; + commanded_trajectory_marker_msg.color.a = 0.8; + commanded_trajectory_marker_msg.lifetime = ros::Duration(); + commanded_trajectory_marker_msg.points.push_back(p); + + // cyclically publish marker messages + ros::AsyncSpinner spinner(2); // asynch. spinner (2) is needed to call dynamic reconfigure from this node without blocking the node + spinner.start(); + ros::Rate r(5); +// int index = 0; + while (ros::ok()) + { + // receive the current robot pose + if (robot_trajectory_recording_active_ == true) + { + ros::Time time = ros::Time(0); + bool transform_available = transform_listener_.waitForTransform(map_frame_, robot_frame_, time, ros::Duration(0.5)); + if (transform_available == true) + { + tf::StampedTransform transform; + transform_listener_.lookupTransform(map_frame_, robot_frame_, time, transform); + { + boost::mutex::scoped_lock lock(robot_trajectory_vector_mutex_); + robot_trajectory_vector_.push_back(transform); + } + } +// // this can be used for testing if no data is available +// tf::StampedTransform transform(tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.1*index, 0., 0.)), ros::Time::now(), map_frame_, robot_frame_); +// robot_trajectory_vector_.push_back(transform); +// robot_computed_trajectory_vector_.push_back(transform); +// robot_commanded_trajectory_vector_.push_back(transform); +// ++index; + } + + // update and publish coverage_marker_msg + coverage_marker_msg.header.stamp = ros::Time::now(); + coverage_marker_msg.points.resize(robot_trajectory_vector_.size()); + for (size_t i=0; i. + * + ****************************************************************/ + +#include + +// Function that provides the functionality that a given fov path gets mapped to a robot path by using the given parameters. +// To do so simply a vector operation is applied. If the computed robot pose is not in the free space, another accessible +// point is generated by finding it on the radius around the fov middlepoint s.t. the distance to the last robot position +// is minimized. +// Important: the room map needs to be an unsigned char single channel image, if inaccessible areas should be excluded, provide the inflated map +// robot_to_fov_vector in [m] +void mapPath(const cv::Mat& room_map, std::vector& robot_path, + const std::vector& fov_path, const Eigen::Matrix& robot_to_fov_vector, + const double map_resolution, const cv::Point2d map_origin, const cv::Point& starting_point) +{ + // initialize helper classes + MapAccessibilityAnalysis map_accessibility; + AStarPlanner path_planner; + const double map_resolution_inv = 1.0/map_resolution; + + // initialize the robot position in accessible space to enable the Astar planner to find a path from the beginning + cv::Point robot_pos(starting_point.x, starting_point.y); +// std::vector accessible_start_poses_on_perimeter; +// map_accessibility.checkPerimeter(accessible_start_poses_on_perimeter, fov_center, fov_radius_pixel, PI/64., room_map, false, robot_pos); + + // map the given robot to fov vector into pixel coordinates + Eigen::Matrix robot_to_fov_vector_pixel; + robot_to_fov_vector_pixel << robot_to_fov_vector(0,0)*map_resolution_inv, robot_to_fov_vector(1,0)*map_resolution_inv; + const double fov_radius_pixel = robot_to_fov_vector_pixel.norm(); + const double fov_to_front_offset_angle = atan2((double)robot_to_fov_vector(1,0), (double)robot_to_fov_vector(0,0)); + std::cout << "mapPath: fov_to_front_offset_angle: " << fov_to_front_offset_angle << "rad (" << fov_to_front_offset_angle*180./PI << "deg)" << std::endl; + std::cout << "fov_radius_pixel: " << fov_radius_pixel << " robot_to_fov_vector: " << robot_to_fov_vector(0,0) << ", " << robot_to_fov_vector(1,0) << std::endl; + + // go trough the given poses and calculate accessible robot poses + // first try with A*, if this fails, call map_accessibility_analysis and finally try a directly computed pose shift + int found_with_astar = 0, found_with_map_acc = 0, found_with_shift = 0, not_found = 0; + for(std::vector::const_iterator pose=fov_path.begin(); pose!=fov_path.end(); ++pose) + { + bool found_pose = false; + + // 1. try with map_accessibility_analysis + // compute accessible locations on perimeter around target fov center + MapAccessibilityAnalysis::Pose fov_center(pose->x, pose->y, pose->theta); + std::vector accessible_poses_on_perimeter; + map_accessibility.checkPerimeter(accessible_poses_on_perimeter, fov_center, fov_radius_pixel, PI/64., room_map, false, robot_pos); + + //std::cout << " fov_center: " << fov_center.x << ", " << fov_center.y << ", " << fov_center.orientation << " accessible_poses_on_perimeter.size: " << accessible_poses_on_perimeter.size() << std::endl; + + if(accessible_poses_on_perimeter.size()!=0) + { + // todo: also consider complete visibility of the fov_center (or whole cell) as a selection criterion + // todo: extend with a complete consideration of the exact robot footprint + // go trough the found accessible positions and take the one that minimizes the angle between approach vector and robot heading direction at the target position + // and which lies in the half circle around fov_center which is "behind" the fov_center pose's orientation +// double max_cos_alpha = -10; + std::map > cos_alpha_to_perimeter_pose_mapping; // maps (positive) cos_alpha to their perimeter poses + MapAccessibilityAnalysis::Pose best_pose; + //std::cout << "Perimeter: \n robot_pos = " << robot_pos.x << ", " << robot_pos.y << " fov_center = " << fov_center.x << ", " << fov_center.y << "\n"; + for(std::vector::iterator perimeter_pose = accessible_poses_on_perimeter.begin(); perimeter_pose != accessible_poses_on_perimeter.end(); ++perimeter_pose) + { + // exclude positions that are ahead of the moving direction + //cv::Point2d heading = cv::Point2d(fov_center.x, fov_center.y) - cv::Point2d(perimeter_pose->x, perimeter_pose->y); + //const double heading_norm = sqrt((double)heading.x*heading.x+heading.y*heading.y); + perimeter_pose->orientation -= fov_to_front_offset_angle; // robot heading correction of off-center fov + const cv::Point2d perimeter_heading = cv::Point2d(cos(perimeter_pose->orientation), sin(perimeter_pose->orientation)); + const double perimeter_heading_norm = 1.; + const cv::Point2d fov_center_heading = cv::Point2d(cos(fov_center.orientation), sin(fov_center.orientation)); + const double fov_center_heading_norm = 1.; + const double cos_alpha = (fov_center_heading.x*perimeter_heading.x+fov_center_heading.y*perimeter_heading.y)/(fov_center_heading_norm*perimeter_heading_norm); + //std::cout << " cos_alpha: " << cos_alpha << std::endl; +// if (cos_alpha < 0) +// continue; + if (cos_alpha >= 0.) + cos_alpha_to_perimeter_pose_mapping[cos_alpha] = *perimeter_pose; // rank by cos(angle) between approach direction and viewing direction + + // rank by cos(angle) between approach direction and viewing direction + //cv::Point2d approach = cv::Point2d(perimeter_pose->x, perimeter_pose->y) - cv::Point2d(robot_pos.x, robot_pos.y); + //const double approach_norm = sqrt(approach.x*approach.x+approach.y*approach.y); +// double cos_alpha = 1.; // only remains 1.0 if robot_pose and perimeter_pose are identical +// if (fov_center_heading.x!=0 || fov_center_heading.y!=0) // compute the cos(angle) between approach direction and viewing direction +// cos_alpha = (fov_center_heading.x*perimeter_heading.x + fov_center_heading.y*perimeter_heading.y)/(fov_center_heading_norm*perimeter_heading_norm); + //std::cout << " - perimeter_pose = " << perimeter_pose->x << ", " << perimeter_pose->y << " cos_alpha = " << cos_alpha << " max_cos_alpha = " << max_cos_alpha << std::endl; +// if(cos_alpha>max_cos_alpha) +// { +// max_cos_alpha = cos_alpha; +// best_pose = *perimeter_pose; +// found_pose = true; +// } + } +// std::cout << " cos_alpha_to_perimeter_pose_mapping.size: " << cos_alpha_to_perimeter_pose_mapping.size() << std::endl; + if (cos_alpha_to_perimeter_pose_mapping.size() > 0) + { + // rank by cos(angle) between approach direction and viewing direction + double max_cos_alpha = cos_alpha_to_perimeter_pose_mapping.begin()->first; + double closest_dist = std::numeric_limits::max(); + for (std::map >::iterator it=cos_alpha_to_perimeter_pose_mapping.begin(); it!=cos_alpha_to_perimeter_pose_mapping.end(); ++it) + { +// std::cout << " cos_alpha: " << it->first << std::endl; + // only consider the best fitting angles + if (it->first < 0.95*max_cos_alpha) + break; + // from those select the position with shortest approach path from current position + const double dist = cv::norm(robot_pos-cv::Point(it->second.x, it->second.y)); + if (dist < closest_dist) + { + closest_dist = dist; + best_pose = it->second; + found_pose = true; + } + } +// std::cout << " closest_dist: " << closest_dist << " best_pose: " << best_pose.x << ", " << best_pose.y << ", " << best_pose.orientation << std::endl; + } + + // add pose to path and set robot position to it + if (found_pose == true) + { + geometry_msgs::Pose2D best_pose_msg; + best_pose_msg.x = best_pose.x*map_resolution + map_origin.x; + best_pose_msg.y = best_pose.y*map_resolution + map_origin.y; + best_pose_msg.theta = best_pose.orientation; + robot_path.push_back(best_pose_msg); + robot_pos = cv::Point(cvRound(best_pose.x), cvRound(best_pose.y)); + //std::cout << " best_pose = " << best_pose.x << ", " << best_pose.y << " max_cos_alpha = " << max_cos_alpha << std::endl; + ++found_with_map_acc; + } + } + + // 2. if no accessible pose was found, try with a directly computed pose shift + if (found_pose==false) + { + // get the rotation matrix + const float sin_theta = std::sin(pose->theta); + const float cos_theta = std::cos(pose->theta); + Eigen::Matrix R; + R << cos_theta, -sin_theta, sin_theta, cos_theta; + + // calculate the resulting rotated relative vector and the corresponding robot position + Eigen::Matrix v_rel_rot = R * robot_to_fov_vector_pixel; + Eigen::Matrix robot_position; + robot_position << pose->x-v_rel_rot(0,0), pose->y-v_rel_rot(1,0); + + // check the accessibility of the found point + geometry_msgs::Pose2D current_pose; + if(robot_position(0,0) >= 0 && robot_position(1,0) >= 0 && robot_position(0,0) < room_map.cols && + robot_position(1,0) < room_map.rows && room_map.at((int)robot_position(1,0), (int)robot_position(0,0)) == 255) // position accessible + { + current_pose.x = (robot_position(0,0) * map_resolution) + map_origin.x; + current_pose.y = (robot_position(1,0) * map_resolution) + map_origin.y; + current_pose.theta = pose->theta; + found_pose = true; + robot_path.push_back(current_pose); + + // set robot position to computed pose s.t. further planning is possible + robot_pos = cv::Point((int)robot_position(0,0), (int)robot_position(1,0)); + + ++found_with_shift; + } + } + + if (found_pose==false) + { + // 3. if still no accessible position was found, try with computing the A* path from robot position to fov_center and stop at the right distance + // get vector from current position to desired fov position + cv::Point fov_position(pose->x, pose->y); + std::vector astar_path; + path_planner.planPath(room_map, robot_pos, fov_position, 1.0, 0.0, map_resolution, 0, &astar_path); + + // find the point on the astar path that is on the viewing circle around the fov middlepoint + cv::Point accessible_position; + for(std::vector::iterator point=astar_path.begin(); point!=astar_path.end(); ++point) + { + if(cv::norm(*point-fov_position) <= fov_radius_pixel) + { + accessible_position = *point; + found_pose = true; + break; + } + } + + // add pose to path and set robot position to it + if(found_pose == true) + { + // get the angle s.t. the pose points to the fov middlepoint and save it + geometry_msgs::Pose2D current_pose; + current_pose.x = (accessible_position.x * map_resolution) + map_origin.x; + current_pose.y = (accessible_position.y * map_resolution) + map_origin.y; + current_pose.theta = std::atan2(pose->y-accessible_position.y, pose->x-accessible_position.x) - fov_to_front_offset_angle; // todo: check -fov_to_front_offset_angle + robot_path.push_back(current_pose); + // set robot position to computed pose s.t. further planning is possible + robot_pos = accessible_position; + ++found_with_astar; + } + } + + if (found_pose==false) + { + ++not_found; + std::cout << " not found." << std::endl; + } + +// testing +// std::cout << robot_pos << ", " << cv::Point(pose->x, pose->y) << std::endl; +// cv::Mat room_copy = room_map.clone(); +// cv::line(room_copy, robot_pos, cv::Point(pose->x, pose->y), cv::Scalar(127), 1); +// cv::circle(room_copy, robot_pos, 2, cv::Scalar(100), CV_FILLED); +// cv::imshow("pose", room_copy); +// cv::waitKey(); + +// if (robot_path.size()>0) +// std::cout << " robot_pos: " << robot_path.back().x << ", " << robot_path.back().y << ", " << robot_path.back().theta << std::endl; + } + std::cout << "Found with map_accessibility: " << found_with_map_acc << ", with shift: " << found_with_shift + << ", with A*: " << found_with_astar << ", not found: " << not_found << std::endl; +} + + +// computes the field of view center and the radius of the maximum incircle of a given field of view quadrilateral +// the metric coordinates are relative to the robot base coordinate system (i.e. the robot center) +// coordinate system definition: x points in forward direction of robot and camera, y points to the left side of the robot and z points upwards. x and y span the ground plane. +// fitting_circle_center_point_in_meter this is also considered the center of the field of view, because around this point the maximum radius incircle can be found that is still inside the fov +// fov_resolution resolution of the fov center and incircle computations, in [pixels/m] +void computeFOVCenterAndRadius(const std::vector >& fov_corners_meter, + float& fitting_circle_radius_in_meter, Eigen::Matrix& fitting_circle_center_point_in_meter, const double fov_resolution) +{ + // The general solution for the largest incircle is to find the critical points of a Voronoi graph and select the one + // with largest distance to the sides as circle center and its closest distance to the quadrilateral sides as radius + // see: http://math.stackexchange.com/questions/1948356/largest-incircle-inside-a-quadrilateral-radius-calculation + // -------------> easy solution: distance transform on fov, take max. value that is closest to center + + // read out field of view and convert to pixel coordinates, determine min, max and center coordinates + std::vector fov_corners_pixel; + cv::Point center_point_pixel(0,0); + cv::Point min_point(100000, 100000); + cv::Point max_point(-100000, -100000); + for(int i = 0; i < 4; ++i) + { + fov_corners_pixel.push_back(cv::Point(fov_corners_meter[i](0,0)*fov_resolution, fov_corners_meter[i](1,0)*fov_resolution)); + center_point_pixel += fov_corners_pixel.back(); + min_point.x = std::min(min_point.x, fov_corners_pixel.back().x); + min_point.y = std::min(min_point.y, fov_corners_pixel.back().y); + max_point.x = std::max(max_point.x, fov_corners_pixel.back().x); + max_point.y = std::max(max_point.y, fov_corners_pixel.back().y); + } + center_point_pixel.x = center_point_pixel.x/4 - min_point.x + 1; + center_point_pixel.y = center_point_pixel.y/4 - min_point.y + 1; + + // draw an image of the field of view and compute a distance transform + cv::Mat fov_image = cv::Mat::zeros(4+max_point.y-min_point.y, 4+max_point.x-min_point.x, CV_8UC1); + std::vector > polygon_array(1,fov_corners_pixel); + for (size_t i=0; i(v,u)>max_dist_val) + { + max_dist_val = fov_distance_transform.at(v,u); + max_dist_point = cv::Point(u,v); + center_dist = (center_point_pixel.x-u)*(center_point_pixel.x-u) + (center_point_pixel.y-v)*(center_point_pixel.y-v); + } + else if (fov_distance_transform.at(v,u) == max_dist_val) + { + double cdist = (center_point_pixel.x-u)*(center_point_pixel.x-u) + (center_point_pixel.y-v)*(center_point_pixel.y-v); + if (cdist < center_dist) + { + max_dist_val = fov_distance_transform.at(v,u); + max_dist_point = cv::Point(u,v); + center_dist = cdist; + } + } + } + } + + // compute fitting_circle_radius and center point (round last digit) + fitting_circle_radius_in_meter = 10*(int)(((max_dist_val-1.f)+5)*0.1) / fov_resolution; + std::cout << "fitting_circle_radius: " << fitting_circle_radius_in_meter << " m" << std::endl; + cv::Point2d center_point = cv::Point2d(10*(int)(((max_dist_point.x+min_point.x-1)+5.)*0.1), 10*(int)(((max_dist_point.y+min_point.y-1)+5.)*0.1))*(1./fov_resolution); + std::cout << "center point: " << center_point << " m" << std::endl; + fitting_circle_center_point_in_meter << center_point.x, center_point.y; + +// // display +// cv::normalize(fov_distance_transform, fov_distance_transform, 0, 1, cv::NORM_MINMAX); +// cv::circle(fov_distance_transform, max_dist_point, 2, cv::Scalar(0.2), -1); +// cv::imshow("fov_image", fov_image); +// cv::imshow("fov_distance_transform", fov_distance_transform); +// cv::waitKey(); +} diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/src/next_goal.cpp b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/src/next_goal.cpp new file mode 100644 index 0000000..47dbc76 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/src/next_goal.cpp @@ -0,0 +1,245 @@ + + +/*This code is used to plan the trajectory of the robot +*/ + +#include +#include +#include +#include +#include "nav_msgs/Odometry.h" +#include "nav_msgs/Path.h" +#include "geometry_msgs/PoseStamped.h" +#include +#include +#include + +using namespace std; +using namespace tf; + +float x_current; +float y_current; +bool flag = false; + +float normeNextGoal; + +class quaternion_ros +{ +public: + float w; + float x; + float y; + float z; + + quaternion_ros(); + + void toQuaternion(float pitch, float roll, float yaw); +}; + +void quaternion_ros::toQuaternion(float pitch, float roll, float yaw) +{ + + float cy = cos(yaw * 0.5); + float sy = sin(yaw * 0.5); + float cr = cos(roll * 0.5); + float sr = sin(roll * 0.5); + float cp = cos(pitch * 0.5); + float sp = sin(pitch * 0.5); + + w = cy * cr * cp + sy * sr * sp; + x = cy * sr * cp - sy * cr * sp; + y = cy * cr * sp + sy * sr * cp; + z = sy * cr * cp - cy * sr * sp; +} + +quaternion_ros::quaternion_ros() +{ + w = 1; + x = 0; + y = 0; + z = 0; +} + +class Path_planned +{ +public: + struct Goal + { + float x; + float y; + bool visited; + }; + + vector Path; + + Path_planned(); + //Path_planned(float x, float y, bool visited); + void addGoal(float X, float Y, bool visit); +}; + +Path_planned::Path_planned() +{ +} + +//Path_planned(float x, float y, bool visited) + +void Path_planned::addGoal(float X, float Y, bool visit) +{ + Path_planned::Goal newGoal; + newGoal.x = X; + newGoal.y = Y; + newGoal.visited = visit; + Path.push_back(newGoal); +} + +Path_planned planned_path; +nav_msgs::Path passed_path; +ros::Publisher pub_passed_path; +void pose_callback(const nav_msgs::Odometry &poses) +{ //里程计回调函数,用来计算当前机器人位置与前面目标点的距离,判断是否要发新的幕摆点 + x_current = poses.pose.pose.position.x; + y_current = poses.pose.pose.position.y; + passed_path.header = poses.header; + geometry_msgs::PoseStamped p; + p.header = poses.header; + p.pose = poses.pose.pose; + passed_path.poses.emplace_back(p); + //----------------------------------------------------------------------- + // pub_passed_path.publish(passed_path); +} + +int taille_last_path = 0; +bool new_path = false; + +//接受规划的路径 +void path_callback(const nav_msgs::Path &path) +{ + //注意为了rviz显示方便 路径一直在发,但是这里只用接受一次就好,当规划的路径发生变化时候再重新装载 + if ((planned_path.Path.size() == 0) || (path.poses.size() != taille_last_path)) + { + planned_path.Path.clear(); + new_path = true; + for (int i = 0; i < path.poses.size(); i++) + { + planned_path.addGoal(path.poses[i].pose.position.x, path.poses[i].pose.position.y, false); + + cout << path.poses[i].pose.position.x << " " << path.poses[i].pose.position.y << endl; + } + cout << "Recv path size:" << path.poses.size() << endl; + taille_last_path = path.poses.size(); + } +} + +// int **count_antonin(char *) + +void onesec(const ros::TimerEvent &even){ + flag = true; +} + +int main(int argc, char *argv[]) +{ + srand(time(0)); + ros::init(argc, argv, "next_goal"); + ros::NodeHandle next_goal; + ros::Subscriber sub1 = next_goal.subscribe("/odom", 1000, pose_callback); + ros::Subscriber sub2 = next_goal.subscribe("/room_exploration/room_exploration_server/coverage_path", 10000, path_callback); + + ros::Publisher pub1 = next_goal.advertise("/move_base_simple/goal", 1000); + // pub_passed_path = next_goal.advertise("/passed_path", 1000); + + ros::Rate loop_rate(10); + ros::Timer timer = next_goal.createTimer(ros::Duration(5),onesec,true,false); + + geometry_msgs::PoseStamped goal_msgs; + int count = 0; + double angle; + bool goal_reached = false; + + //获取发送下一个点的阈值 + if (!next_goal.getParam("/NextGoal/tolerance_goal", normeNextGoal)) + { + ROS_ERROR("Please set your tolerance_goal"); + return 0; + } + ROS_INFO("tolerance_goal=%f", normeNextGoal); + + while (ros::ok()) + { + ros::spinOnce(); + if (new_path) + { + count = 0; + new_path = false; + } + //当前处理的点 + // cout << " count : " << count << endl; + if (!planned_path.Path.empty()) + { + //当前距离达到了 + if (sqrt(pow(x_current - planned_path.Path[count].x, 2) + pow(y_current - planned_path.Path[count].y, 2)) <= normeNextGoal) + { + count++; + goal_reached = false; + flag = false; + timer.stop(); + timer.start(); + } + else if (flag == true) + { + count++; + goal_reached = false; + flag = false; + timer.stop(); + timer.start(); + } + + + if (goal_reached == false) + { + goal_msgs.header.frame_id = "odom"; + goal_msgs.header.stamp = ros::Time::now(); + goal_msgs.pose.position.x = planned_path.Path[count].x; + goal_msgs.pose.position.y = planned_path.Path[count].y; + goal_msgs.pose.position.z = 0; + if (count < planned_path.Path.size()) + {//计算发布的yaw，不过还有bug 但是不影响使用，yaw不会产生太大影响 + angle = atan2(planned_path.Path[count + 1].y - planned_path.Path[count].y, planned_path.Path[count + 1].x - planned_path.Path[count].x); + } + else + { + angle = atan2(planned_path.Path[0].y - planned_path.Path[count].y, planned_path.Path[0].x - planned_path.Path[count].x); + } + cout << angle << endl; + quaternion_ros q; + q.toQuaternion(0, 0, float(angle)); + goal_msgs.pose.orientation.w = q.w; + goal_msgs.pose.orientation.x = q.x; + goal_msgs.pose.orientation.y = q.y; + if (planned_path.Path[count].x < planned_path.Path[count + 1].x) + { + goal_msgs.pose.orientation.z = 0; + } + if (planned_path.Path[count].x > planned_path.Path[count + 1].x) + { + goal_msgs.pose.orientation.z = 2; + } + + cout << " NEW GOAL " << endl; + cout << " x = " << planned_path.Path[count].x << " y = " << planned_path.Path[count].y << endl; + + goal_reached = true; + pub1.publish(goal_msgs); + } + cout << x_current << " " << y_current << endl; + //当前 + cout << planned_path.Path[count].x << " " << planned_path.Path[count].y << endl; + //目标 + cout << " DISTANCE : " << sqrt((x_current - planned_path.Path[count].x) * (x_current - planned_path.Path[count].x) + (y_current - planned_path.Path[count].y) * (y_current - planned_path.Path[count].y)) << endl; + // 距离公式 + } + + loop_rate.sleep(); + } + + return 0; +} diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/src/room_exploration_action_client.cpp b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/src/room_exploration_action_client.cpp new file mode 100644 index 0000000..fc8eeb2 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/src/room_exploration_action_client.cpp @@ -0,0 +1,225 @@ +#include +#include + +#include +#include + +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +// overload of << operator for geometry_msgs::Pose2D to wanted format +std::ostream& operator<<(std::ostream& os, const geometry_msgs::Pose2D& obj) +{ + std::stringstream ss; + ss << "[" << obj.x << ", " << obj.y << ", " << obj.theta << "]"; + os << ss.rdbuf(); + return os; +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "room_exploration_client"); + ros::NodeHandle priv_nh("~"); + ros::NodeHandle nh; + + actionlib::SimpleActionClient ac("room_exploration_server", true); + + // read params + bool use_test_maps; + priv_nh.param("use_test_maps", use_test_maps, true); + double resolution; + priv_nh.param("resolution", resolution, 0.05); + std::vector origin (3,0); + priv_nh.param("origin", origin, origin); + double robot_radius; + priv_nh.param("robot_radius", robot_radius, 0.15); + double coverage_radius; + priv_nh.param("coverage_radius", coverage_radius, 0.15); + std::vector start_pos = {0, 0, 0}; + priv_nh.param("starting_position", start_pos, start_pos); + + if (start_pos.size() != 3) + { + ROS_FATAL("starting_position must contain 3 values"); + return -1; + } + + std::string image_path; + if (use_test_maps) + { + // read in test map + const std::string test_map_path = ros::package::getPath("ipa_room_segmentation") + "/common/files/test_maps/"; + image_path = test_map_path + "500_500.png"; + } + else + { + std::string env_pack_path; + priv_nh.param("env_pack", env_pack_path, "ipa_room_segmentation"); + std::string file_name; + priv_nh.param("image", file_name, "map.pgm"); + std::string map_name; + priv_nh.param("robot_env", map_name, "lab_ipa"); + + image_path = env_pack_path + "/envs/" + map_name + "/" + file_name; + } + + cv::Mat map_flipped = cv::imread(image_path, 0);//读入灰度图片 + cv::Mat map; + cv::flip(map_flipped, map, 0); + //make non-white pixels black + for (int y = 0; y < map.rows; y++) + { + for (int x = 0; x < map.cols; x++) + { + //find not reachable regions and make them black + if (map.at(y, x) < 250) + { + map.at(y, x) = 0; + } + //else make it white + else + { + map.at(y, x) = 255; + } + } + } + std::cout << "map-size: " << map.rows << "x" << map.cols << std::endl; + +// const std::string topic = "/move_base/global_costmap/costmap"; +// nav_msgs::OccupancyGrid grid; +// grid = *(ros::topic::waitForMessage(topic, nh)); +// ROS_INFO("got grid"); +// std::vector& dats = grid.data; +// std::cout << dats.size() << std::endl; +// cv::Mat test_map = cv::Mat(grid.info.height, grid.info.width, map.type()); +// for(size_t v = 0; v < test_map.rows; ++v) +// for(size_t u = 0; u < test_map.cols; ++u) +// test_map.at(v,u) = (uchar)(2.55*(double)dats[v*grid.info.width+u]); +// cv::imshow("testtt", test_map); +// cv::waitKey(); + + ROS_INFO("Waiting for action server to start."); + // wait for the action server to start + ac.waitForServer(); //will wait for infinite time + + ROS_INFO("Action server started, sending goal."); + + DynamicReconfigureClient drc_exp(nh, "room_exploration_server/set_parameters", "room_exploration_server/parameter_updates"); + drc_exp.setConfig("room_exploration_algorithm", 8); + // drc_exp.setConfig("room_exploration_algorithm", 3); + drc_exp.setConfig("execute_path", true); +// drc_exp.setConfig("path_eps", 3); +// drc_exp.setConfig("grid_line_length", 15); +// drc_exp.setConfig("path_eps", 10); +// drc_exp.setConfig("cell_size", 10); +// drc_exp.setConfig("plan_for_footprint", true); +// drc_exp.setConfig("goal_eps", 0.0); +// drc_exp.setConfig("delta_theta", 0.005); + +// cv::Point2f src_center(map.cols/2.0F, map.rows/2.0F); +// cv::Mat rot_mat = getRotationMatrix2D(src_center, 180, 1.0); +// cv::Mat dst; +// cv::warpAffine(map, dst, rot_mat, map.size()); +// cv::flip(dst, map, 1); +// cv::imshow("map", map); +// cv::waitKey(); + + sensor_msgs::Image labeling; + cv_bridge::CvImage cv_image; +// cv_image.header.stamp = ros::Time::now(); + cv_image.encoding = "mono8"; + cv_image.image = map; + cv_image.toImageMsg(labeling); + + geometry_msgs::Pose map_origin; + map_origin.position.x = origin[0]; + map_origin.position.y = origin[1]; + map_origin.position.z = origin[2]; + + geometry_msgs::Pose2D starting_position; + starting_position.x = start_pos[0]; + starting_position.y = start_pos[1]; + starting_position.theta = start_pos[2]; + + std::vector fov_points(4); + fov_points[0].x = 0.04035; // this field of view represents the off-center iMop floor wiping device + fov_points[0].y = -0.136; + fov_points[1].x = 0.04035; + fov_points[1].y = 0.364; + fov_points[2].x = 0.54035; // todo: this definition is mirrored on x (y-coordinates are inverted) to work properly --> check why, make it work the intuitive way + fov_points[2].y = 0.364; + fov_points[3].x = 0.54035; + fov_points[3].y = -0.136; + int planning_mode = 2; // viewpoint planning +// fov_points[0].x = 0.15; // this field of view fits a Asus Xtion sensor mounted at 0.63m height (camera center) pointing downwards to the ground in a respective angle +// fov_points[0].y = 0.35; +// fov_points[1].x = 0.15; +// fov_points[1].y = -0.35; +// fov_points[2].x = 1.15; +// fov_points[2].y = -0.65; +// fov_points[3].x = 1.15; +// fov_points[3].y = 0.65; +// int planning_mode = 2; // viewpoint planning +// fov_points[0].x = -0.3; // this is the working area of a vacuum cleaner with 60 cm width +// fov_points[0].y = 0.3; +// fov_points[1].x = -0.3; +// fov_points[1].y = -0.3; +// fov_points[2].x = 0.3; +// fov_points[2].y = -0.3; +// fov_points[3].x = 0.3; +// fov_points[3].y = 0.3; +// int planning_mode = 1; // footprint planning + geometry_msgs::Point32 fov_origin; + fov_origin.x = 0.; + fov_origin.y = 0.; + + ipa_building_msgs::RoomExplorationGoal goal; + goal.input_map = labeling; + goal.map_resolution = resolution; + goal.map_origin = map_origin; + goal.robot_radius = robot_radius; // turtlebot, used for sim 0.177, 0.4 + goal.coverage_radius = coverage_radius; + goal.field_of_view = fov_points; + goal.field_of_view_origin = fov_origin; + goal.starting_position = starting_position; + goal.planning_mode = planning_mode; + ac.sendGoal(goal); + + ac.waitForResult(ros::Duration()); + ipa_building_msgs::RoomExplorationResultConstPtr action_result = ac.getResult(); + + std::cout << "Got a path with " << action_result->coverage_path.size() << " nodes." << std::endl; + + // display path + // const double inverse_map_resolution = 1./goal.map_resolution; + // cv::Mat path_map = map.clone(); + // for (size_t point=0; pointcoverage_path.size(); ++point) + // { + // const cv::Point point1((action_result->coverage_path[point].x-map_origin.position.x)*inverse_map_resolution, (action_result->coverage_path[point].y-map_origin.position.y)*inverse_map_resolution); + // cv::circle(path_map, point1, 2, cv::Scalar(128), -1); + // if (point > 0) + // { + // const cv::Point point2((action_result->coverage_path[point-1].x-map_origin.position.x)*inverse_map_resolution, (action_result->coverage_path[point-1].y-map_origin.position.y)*inverse_map_resolution); + // cv::line(path_map, point1, point2, cv::Scalar(128), 1); + // } + // std::cout << "coverage_path[" << point << "]: x=" << action_result->coverage_path[point].x << ", y=" << action_result->coverage_path[point].y << ", theta=" << action_result->coverage_path[point].theta << std::endl; + // } + // cv::imshow("path", path_map); + // cv::waitKey(); + + return 0; +} diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/src/room_exploration_action_server.cpp b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/src/room_exploration_action_server.cpp new file mode 100644 index 0000000..730b27d --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/src/room_exploration_action_server.cpp @@ -0,0 +1,1281 @@ +/*! + ***************************************************************** + * \file + * + * \note + * Copyright (c) 2016 \n + * Fraunhofer Institute for Manufacturing Engineering + * and Automation (IPA) \n\n + * + ***************************************************************** + * + * \note + * Project name: Care-O-bot + * \note + * ROS stack name: autopnp + * \note + * ROS package name: ipa_room_exploration + * + * \author + * Author: Florian Jordan, Richard Bormann + * \author + * Supervised by: Richard Bormann + * + * \date Date of creation: 03.2016 + * + * \brief + * + * + ***************************************************************** + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * - Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. \n + * - Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. \n + * - Neither the name of the Fraunhofer Institute for Manufacturing + * Engineering and Automation (IPA) nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. \n + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License LGPL as + * published by the Free Software Foundation, either version 3 of the + * License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License LGPL for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License LGPL along with this program. + * If not, see . + * + ****************************************************************/ + + +#include + +// constructor +RoomExplorationServer::RoomExplorationServer(ros::NodeHandle nh, std::string name_of_the_action) : + node_handle_(nh), + room_exploration_server_(node_handle_, name_of_the_action, boost::bind(&RoomExplorationServer::exploreRoom, this, _1), false) +{ + // dynamic reconfigure + room_exploration_dynamic_reconfigure_server_.setCallback(boost::bind(&RoomExplorationServer::dynamic_reconfigure_callback, this, _1, _2)); + + // Parameters + std::cout << "\n--------------------------\nRoom Exploration Parameters:\n--------------------------\n"; + node_handle_.param("room_exploration_algorithm", room_exploration_algorithm_, 1); + std::cout << "room_exploration/room_exploration_algorithm = " << room_exploration_algorithm_ << std::endl; + node_handle_.param("display_trajectory", display_trajectory_, false); + std::cout << "room_exploration/display_trajectory = " << display_trajectory_ << std::endl; + + node_handle_.param("map_correction_closing_neighborhood_size", map_correction_closing_neighborhood_size_, 2); + std::cout << "room_exploration/map_correction_closing_neighborhood_size = " << map_correction_closing_neighborhood_size_ << std::endl; + + node_handle_.param("return_path", return_path_, true); + std::cout << "room_exploration/return_path = " << return_path_ << std::endl; + node_handle_.param("execute_path", execute_path_, false); + std::cout << "room_exploration/execute_path = " << execute_path_ << std::endl; + node_handle_.param("goal_eps", goal_eps_, 0.35); + std::cout << "room_exploration/goal_eps = " << goal_eps_ << std::endl; + node_handle_.param("use_dyn_goal_eps", use_dyn_goal_eps_, false); + std::cout << "room_exploration/use_dyn_goal_eps = " << use_dyn_goal_eps_ << std::endl; + node_handle_.param("interrupt_navigation_publishing", interrupt_navigation_publishing_, false); + std::cout << "room_exploration/interrupt_navigation_publishing = " << interrupt_navigation_publishing_ << std::endl; + node_handle_.param("revisit_areas", revisit_areas_, false); + std::cout << "room_exploration/revisit_areas = " << revisit_areas_ << std::endl; + node_handle_.param("left_sections_min_area", left_sections_min_area_, 0.01); + std::cout << "room_exploration/left_sections_min_area_ = " << left_sections_min_area_ << std::endl; + global_costmap_topic_ = "/move_base/global_costmap/costmap"; + node_handle_.param("global_costmap_topic", global_costmap_topic_); + std::cout << "room_exploration/global_costmap_topic = " << global_costmap_topic_ << std::endl; + node_handle_.param("coverage_check_service_name", coverage_check_service_name_, "/room_exploration/coverage_check_server/coverage_check"); + std::cout << "room_exploration/coverage_check_service_name = " << coverage_check_service_name_ << std::endl; + map_frame_ = "map"; + node_handle_.param("map_frame", map_frame_); + std::cout << "room_exploration/map_frame = " << map_frame_ << std::endl; + camera_frame_ = "base_link"; + node_handle_.param("camera_frame", camera_frame_); + std::cout << "room_exploration/camera_frame = " << camera_frame_ << std::endl; + + + if (room_exploration_algorithm_ == 1) + ROS_INFO("You have chosen the grid exploration method."); + else if(room_exploration_algorithm_ == 2) + ROS_INFO("You have chosen the boustrophedon exploration method."); + else if(room_exploration_algorithm_ == 3) + ROS_INFO("You have chosen the neural network exploration method."); + else if(room_exploration_algorithm_ == 4) + ROS_INFO("You have chosen the convexSPP exploration method."); + else if(room_exploration_algorithm_ == 5) + ROS_INFO("You have chosen the flow network exploration method."); + else if(room_exploration_algorithm_ == 6) + ROS_INFO("You have chosen the energy functional exploration method."); + else if(room_exploration_algorithm_ == 7) + ROS_INFO("You have chosen the voronoi exploration method."); + else if(room_exploration_algorithm_ == 8) + ROS_INFO("You have chosen the boustrophedon variant exploration method."); + + if (room_exploration_algorithm_ == 1) // get grid point exploration parameters + { + node_handle_.param("tsp_solver", tsp_solver_, (int)TSP_CONCORDE); + std::cout << "room_exploration/tsp_solver = " << tsp_solver_ << std::endl; + int timeout=0; + node_handle_.param("tsp_solver_timeout", timeout, 600); + tsp_solver_timeout_ = timeout; + std::cout << "room_exploration/tsp_solver_timeout = " << tsp_solver_timeout_ << std::endl; + + } + else if ((room_exploration_algorithm_ == 2) || (room_exploration_algorithm_ == 8)) // set boustrophedon (variant) exploration parameters + { + node_handle_.param("min_cell_area", min_cell_area_, 10.0); + std::cout << "room_exploration/min_cell_area_ = " << min_cell_area_ << std::endl; + node_handle_.param("path_eps", path_eps_, 2.0); + std::cout << "room_exploration/path_eps_ = " << path_eps_ << std::endl; + node_handle_.param("grid_obstacle_offset", grid_obstacle_offset_, 0.0); + std::cout << "room_exploration/grid_obstacle_offset_ = " << grid_obstacle_offset_ << std::endl; + node_handle_.param("max_deviation_from_track", max_deviation_from_track_, -1); + std::cout << "room_exploration/max_deviation_from_track_ = " << max_deviation_from_track_ << std::endl; + node_handle_.param("cell_visiting_order", cell_visiting_order_, 1); + std::cout << "room_exploration/cell_visiting_order = " << cell_visiting_order_ << std::endl; + } + else if (room_exploration_algorithm_ == 3) // set neural network explorator parameters + { + node_handle_.param("step_size", step_size_, 0.008); + std::cout << "room_exploration/step_size_ = " << step_size_ << std::endl; + node_handle_.param("A", A_, 17); + std::cout << "room_exploration/A_ = " << A_ << std::endl; + node_handle_.param("B", B_, 5); + std::cout << "room_exploration/B_ = " << B_ << std::endl; + node_handle_.param("D", D_, 7); + std::cout << "room_exploration/D_ = " << D_ << std::endl; + node_handle_.param("E", E_, 80); + std::cout << "room_exploration/E_ = " << E_ << std::endl; + node_handle_.param("mu", mu_, 1.03); + std::cout << "room_exploration/mu_ = " << mu_ << std::endl; + node_handle_.param("delta_theta_weight", delta_theta_weight_, 0.15); + std::cout << "room_exploration/delta_theta_weight_ = " << delta_theta_weight_ << std::endl; + } + else if (room_exploration_algorithm_ == 4) // set convexSPP explorator parameters + { + node_handle_.param("cell_size", cell_size_, 0); + std::cout << "room_exploration/cell_size_ = " << cell_size_ << std::endl; + node_handle_.param("delta_theta", delta_theta_, 1.570796); + std::cout << "room_exploration/delta_theta = " << delta_theta_ << std::endl; + } + else if (room_exploration_algorithm_ == 5) // set flowNetwork explorator parameters + { + node_handle_.param("curvature_factor", curvature_factor_, 1.1); + std::cout << "room_exploration/curvature_factor = " << curvature_factor_ << std::endl; + node_handle_.param("max_distance_factor", max_distance_factor_, 1.0); + std::cout << "room_exploration/max_distance_factor_ = " << max_distance_factor_ << std::endl; + node_handle_.param("cell_size", cell_size_, 0); + std::cout << "room_exploration/cell_size_ = " << cell_size_ << std::endl; + node_handle_.param("path_eps", path_eps_, 3.0); + std::cout << "room_exploration/path_eps_ = " << path_eps_ << std::endl; + } + else if (room_exploration_algorithm_ == 6) // set energyfunctional explorator parameters + { + } + else if (room_exploration_algorithm_ == 7) // set voronoi explorator parameters + { + } + + if (revisit_areas_ == true) + ROS_INFO("Areas not seen after the initial execution of the path will be revisited."); + else + ROS_INFO("Areas not seen after the initial execution of the path will NOT be revisited."); + + + // min area for revisiting left sections + + path_pub_ = node_handle_.advertise("coverage_path", 10000); + cmd_vel_pub_ = node_handle_.advertise("cmd_vel", 10); + obstacles_sub_ = node_handle_.subscribe("stereo_msg",1,boost::bind(&RoomExplorationServer::stereo_CB,this,_1)); + cmd_vel_sub_ = node_handle_.subscribe("cmd_vel",1,boost::bind(&RoomExplorationServer::cmd_CB,this,_1)); + cancel_pub_ = node_handle_.advertise("/move_base/cancel", 10); + //Start action server + room_exploration_server_.start(); + + ROS_INFO("Action server for room exploration has been initialized......"); +} + + +// Callback function for dynamic reconfigure. +void RoomExplorationServer::dynamic_reconfigure_callback(ipa_room_exploration::RoomExplorationConfig &config, uint32_t level) +{ + // set segmentation algorithm + std::cout << "######################################################################################" << std::endl; + std::cout << "Dynamic reconfigure request:" << std::endl; + + room_exploration_algorithm_ = config.room_exploration_algorithm; + std::cout << "room_exploration/path_planning_algorithm_ = " << room_exploration_algorithm_ << std::endl; + + map_correction_closing_neighborhood_size_ = config.map_correction_closing_neighborhood_size; + std::cout << "room_exploration/map_correction_closing_neighborhood_size_ = " << map_correction_closing_neighborhood_size_ << std::endl; + + return_path_ = config.return_path; + std::cout << "room_exploration/return_path_ = " << return_path_ << std::endl; + // execute_path_ = config.execute_path; + execute_path_ = true; + std::cout << "room_exploration/execute_path_ = " << execute_path_ << std::endl; + goal_eps_ = config.goal_eps; + std::cout << "room_exploration/goal_eps_ = " << goal_eps_ << std::endl; + use_dyn_goal_eps_ = config.use_dyn_goal_eps; + std::cout << "room_exploration/use_dyn_goal_eps_ = " << use_dyn_goal_eps_ << std::endl; + interrupt_navigation_publishing_ = config.interrupt_navigation_publishing; + std::cout << "room_exploration/interrupt_navigation_publishing_ = " << interrupt_navigation_publishing_ << std::endl; + revisit_areas_ = config.revisit_areas; + std::cout << "room_exploration/revisit_areas_ = " << revisit_areas_ << std::endl; + left_sections_min_area_ = config.left_sections_min_area; + std::cout << "room_exploration/left_sections_min_area = " << left_sections_min_area_ << std::endl; + global_costmap_topic_ = config.global_costmap_topic; + std::cout << "room_exploration/global_costmap_topic_ = " << global_costmap_topic_ << std::endl; + coverage_check_service_name_ = config.coverage_check_service_name; + std::cout << "room_exploration/coverage_check_service_name_ = " << coverage_check_service_name_ << std::endl; + map_frame_ = config.map_frame; + std::cout << "room_exploration/map_frame_ = " << map_frame_ << std::endl; + camera_frame_ = config.camera_frame; + std::cout << "room_exploration/camera_frame_ = " << camera_frame_ << std::endl; + + // set parameters regarding the chosen algorithm + if (room_exploration_algorithm_ == 1) // set grid point exploration parameters + { + tsp_solver_ = config.tsp_solver; + std::cout << "room_exploration/tsp_solver_ = " << tsp_solver_ << std::endl; + tsp_solver_timeout_ = config.tsp_solver_timeout; + std::cout << "room_exploration/tsp_solver_timeout_ = " << tsp_solver_timeout_ << std::endl; + } + else if ((room_exploration_algorithm_ == 2) || (room_exploration_algorithm_ == 8)) // set boustrophedon (variant) exploration parameters + { + min_cell_area_ = config.min_cell_area; + std::cout << "room_exploration/min_cell_area_ = " << min_cell_area_ << std::endl; + path_eps_ = config.path_eps; + std::cout << "room_exploration/path_eps_ = " << path_eps_ << std::endl; + grid_obstacle_offset_ = config.grid_obstacle_offset; + std::cout << "room_exploration/grid_obstacle_offset_ = " << grid_obstacle_offset_ << std::endl; + max_deviation_from_track_ = config.max_deviation_from_track; + std::cout << "room_exploration/max_deviation_from_track_ = " << max_deviation_from_track_ << std::endl; + cell_visiting_order_ = config.cell_visiting_order; + std::cout << "room_exploration/cell_visiting_order = " << cell_visiting_order_ << std::endl; + } + else if (room_exploration_algorithm_ == 3) // set neural network explorator parameters + { + step_size_ = config.step_size; + std::cout << "room_exploration/step_size_ = " << step_size_ << std::endl; + A_ = config.A; + std::cout << "room_exploration/A_ = " << A_ << std::endl; + B_ = config.B; + std::cout << "room_exploration/B_ = " << B_ << std::endl; + D_ = config.D; + std::cout << "room_exploration/D_ = " << D_ << std::endl; + E_ = config.E; + std::cout << "room_exploration/E_ = " << E_ << std::endl; + mu_ = config.mu; + std::cout << "room_exploration/mu_ = " << mu_ << std::endl; + delta_theta_weight_ = config.delta_theta_weight; + std::cout << "room_exploration/delta_theta_weight_ = " << delta_theta_weight_ << std::endl; + } + else if (room_exploration_algorithm_ == 4) // set convexSPP explorator parameters + { + cell_size_ = config.cell_size; + std::cout << "room_exploration/cell_size_ = " << cell_size_ << std::endl; + delta_theta_ = config.delta_theta; + std::cout << "room_exploration/delta_theta_ = " << delta_theta_ << std::endl; + } + else if (room_exploration_algorithm_ == 5) // set flowNetwork explorator parameters + { + curvature_factor_ = config.curvature_factor; + std::cout << "room_exploration/delta_theta_ = " << delta_theta_ << std::endl; + max_distance_factor_ = config.max_distance_factor; + std::cout << "room_exploration/max_distance_factor_ = " << max_distance_factor_ << std::endl; + cell_size_ = config.cell_size; + std::cout << "room_exploration/cell_size_ = " << cell_size_ << std::endl; + path_eps_ = config.path_eps; + std::cout << "room_exploration/path_eps_ = " << path_eps_ << std::endl; + } + else if (room_exploration_algorithm_ == 6) // set energyFunctional explorator parameters + { + } + else if (room_exploration_algorithm_ == 7) // set voronoi explorator parameters + { + } + + + if (revisit_areas_ == true) + std::cout << "Areas not seen after the initial execution of the path will be revisited." << std::endl; + else + std::cout << "Areas not seen after the initial execution of the path will NOT be revisited." << std::endl; + + std::cout << "######################################################################################" << std::endl; +} + + +// Function executed by Call. +void RoomExplorationServer::exploreRoom(const ipa_building_msgs::RoomExplorationGoalConstPtr &goal) +{ + ROS_INFO("*****Room Exploration action server*****"); + + // ***************** I. read the given parameters out of the goal ***************** + // todo: this is only correct if the map is not rotated + const cv::Point2d map_origin(goal->map_origin.position.x, goal->map_origin.position.y); + const float map_resolution = goal->map_resolution; // in [m/cell] + const float map_resolution_inverse = 1./map_resolution; + std::cout << "map origin: " << map_origin << " m map resolution: " << map_resolution << " m/cell" << std::endl; + + const float robot_radius = goal->robot_radius; + const int robot_radius_in_pixel = (robot_radius / map_resolution); + std::cout << "robot radius: " << robot_radius << " m (" << robot_radius_in_pixel << " px)" << std::endl; + + const cv::Point starting_position((goal->starting_position.x-map_origin.x)/map_resolution, (goal->starting_position.y-map_origin.y)/map_resolution); + std::cout << "starting point: (" << goal->starting_position.x << ", " << goal->starting_position.y << ") m (" << starting_position << " px)" << std::endl; + + planning_mode_ = goal->planning_mode; + if (planning_mode_==PLAN_FOR_FOOTPRINT) + std::cout << "planning mode: planning coverage path with robot's footprint" <input_map, sensor_msgs::image_encodings::MONO8); + cv::Mat room_map = cv_ptr_obj->image; + + // determine room size + int area_px = 0; // room area in pixels + for (int v=0; v(v,u) >= 250) + area_px++; + std::cout << "### room area = " << area_px*map_resolution*map_resolution << " m^2" << std::endl; + + // closing operation to neglect inaccessible areas and map errors/artifacts + cv::Mat temp; + cv::erode(room_map, temp, cv::Mat(), cv::Point(-1, -1), map_correction_closing_neighborhood_size_); + cv::dilate(temp, room_map, cv::Mat(), cv::Point(-1, -1), map_correction_closing_neighborhood_size_); + + // remove unconnected, i.e. inaccessible, parts of the room (i.e. obstructed by furniture), only keep the room with the largest area + const bool room_not_empty = removeUnconnectedRoomParts(room_map); + if (room_not_empty == false) + { + std::cout << "RoomExplorationServer::exploreRoom: Warning: the requested room is too small for generating exploration trajectories." << std::endl; + ipa_building_msgs::RoomExplorationResult action_result; + room_exploration_server_.setAborted(action_result); + return; + } + + // get the grid size, to check the areas that should be revisited later + double grid_spacing_in_meter = 0.0; // is the square grid cell side length that fits into the circle with the robot's coverage radius or fov coverage radius + float fitting_circle_radius_in_meter = 0; + Eigen::Matrix fitting_circle_center_point_in_meter; // this is also considered the center of the field of view, because around this point the maximum radius incircle can be found that is still inside the fov + std::vector > fov_corners_meter(4); + const double fov_resolution = 1000; // in [cell/meter] + if(planning_mode_ == PLAN_FOR_FOV) // read out the given fov-vectors, if needed + { + // Get the size of one grid cell s.t. the grid can be completely covered by the field of view (fov) from all rotations around it. + for(int i = 0; i < 4; ++i) + fov_corners_meter[i] << goal->field_of_view[i].x, goal->field_of_view[i].y; + computeFOVCenterAndRadius(fov_corners_meter, fitting_circle_radius_in_meter, fitting_circle_center_point_in_meter, fov_resolution); + // get the edge length of the grid square that fits into the fitting_circle_radius + grid_spacing_in_meter = fitting_circle_radius_in_meter*std::sqrt(2); + } + else // if planning should be done for the footprint, read out the given coverage radius + { + grid_spacing_in_meter = goal->coverage_radius*std::sqrt(2); + } + // map the grid size to an int in pixel coordinates, using floor method + const double grid_spacing_in_pixel = grid_spacing_in_meter/map_resolution; // is the square grid cell side length that fits into the circle with the robot's coverage radius or fov coverage radius, multiply with sqrt(2) to receive the whole working width + std::cout << "grid size: " << grid_spacing_in_meter << " m (" << grid_spacing_in_pixel << " px)" << std::endl; + // set the cell_size_ for #4 convexSPP explorator or #5 flowNetwork explorator if it is not provided + if (cell_size_ <= 0) + cell_size_ = std::floor(grid_spacing_in_pixel); + + + // ***************** II. plan the path using the wanted planner ***************** + // todo: consider option to provide the inflated map or the robot radius to the functions instead of inflating with half cell size there + Eigen::Matrix zero_vector; + zero_vector << 0, 0; + std::vector exploration_path; + if (room_exploration_algorithm_ == 1) // use grid point explorator + { + // plan path + if(planning_mode_ == PLAN_FOR_FOV) + grid_point_planner.getExplorationPath(room_map, exploration_path, map_resolution, starting_position, map_origin, std::floor(grid_spacing_in_pixel), false, fitting_circle_center_point_in_meter, tsp_solver_, tsp_solver_timeout_); + else + grid_point_planner.getExplorationPath(room_map, exploration_path, map_resolution, starting_position, map_origin, std::floor(grid_spacing_in_pixel), true, zero_vector, tsp_solver_, tsp_solver_timeout_); + } + else if (room_exploration_algorithm_ == 2) // use boustrophedon explorator + { + // plan path + if(planning_mode_ == PLAN_FOR_FOV) + boustrophedon_explorer_.getExplorationPath(room_map, exploration_path, map_resolution, starting_position, map_origin, grid_spacing_in_pixel, grid_obstacle_offset_, path_eps_, cell_visiting_order_, false, fitting_circle_center_point_in_meter, min_cell_area_, max_deviation_from_track_); + else + boustrophedon_explorer_.getExplorationPath(room_map, exploration_path, map_resolution, starting_position, map_origin, grid_spacing_in_pixel, grid_obstacle_offset_, path_eps_, cell_visiting_order_, true, zero_vector, min_cell_area_, max_deviation_from_track_); + } + else if (room_exploration_algorithm_ == 3) // use neural network explorator + { + neural_network_explorator_.setParameters(A_, B_, D_, E_, mu_, step_size_, delta_theta_weight_); + // plan path + if(planning_mode_ == PLAN_FOR_FOV) + neural_network_explorator_.getExplorationPath(room_map, exploration_path, map_resolution, starting_position, map_origin, grid_spacing_in_pixel, false, fitting_circle_center_point_in_meter, false); + else + neural_network_explorator_.getExplorationPath(room_map, exploration_path, map_resolution, starting_position, map_origin, grid_spacing_in_pixel, true, zero_vector, false); + } + else if (room_exploration_algorithm_ == 4) // use convexSPP explorator + { + // plan coverage path + if(planning_mode_ == PLAN_FOR_FOV) + convex_SPP_explorator_.getExplorationPath(room_map, exploration_path, map_resolution, starting_position, map_origin, cell_size_, delta_theta_, fov_corners_meter, fitting_circle_center_point_in_meter, 0., 7, false); + else + convex_SPP_explorator_.getExplorationPath(room_map, exploration_path, map_resolution, starting_position, map_origin, cell_size_, delta_theta_, fov_corners_meter, zero_vector, goal->coverage_radius, 7, true); + } + else if (room_exploration_algorithm_ == 5) // use flow network explorator + { + if(planning_mode_ == PLAN_FOR_FOV) + flow_network_explorator_.getExplorationPath(room_map, exploration_path, map_resolution, starting_position, map_origin, cell_size_, fitting_circle_center_point_in_meter, grid_spacing_in_pixel, false, path_eps_, curvature_factor_, max_distance_factor_); + else + flow_network_explorator_.getExplorationPath(room_map, exploration_path, map_resolution, starting_position, map_origin, cell_size_, zero_vector, grid_spacing_in_pixel, true, path_eps_, curvature_factor_, max_distance_factor_); + } + else if (room_exploration_algorithm_ == 6) // use energy functional explorator + { + if(planning_mode_ == PLAN_FOR_FOV) + energy_functional_explorator_.getExplorationPath(room_map, exploration_path, map_resolution, starting_position, map_origin, grid_spacing_in_pixel, false, fitting_circle_center_point_in_meter); + else + energy_functional_explorator_.getExplorationPath(room_map, exploration_path, map_resolution, starting_position, map_origin, grid_spacing_in_pixel, true, zero_vector); + } + else if (room_exploration_algorithm_ == 7) // use voronoi explorator + { + // create a usable occupancyGrid map out of the given room map + nav_msgs::OccupancyGrid room_gridmap; + matToMap(room_gridmap, room_map); + + // do not find nearest pose to starting-position and start there because of issue in planner when starting position is provided + if(planning_mode_==PLAN_FOR_FOV) + { +// cv::Mat distance_transform; +// cv::distanceTransform(room_map, distance_transform, CV_DIST_L2, CV_DIST_MASK_PRECISE); +// cv::Mat display = room_map.clone(); +// // todoo: get max dist from map and parametrize loop +// for (int s=5; s<100; s+=10) +// { +// for (int v=0; v(v,u)) == s) +// { +// display.at(v,u) = 0; +// } +// } +// } +// } +// cv::imshow("distance_transform", distance_transform); +// cv::imshow("trajectories", display); +// cv::waitKey(); + + // convert fov-radius to pixel integer + const int grid_spacing_as_int = (int)std::floor(grid_spacing_in_pixel); + std::cout << "grid spacing in pixel: " << grid_spacing_as_int << std::endl; + + // create the object that plans the path, based on the room-map + VoronoiMap vm(room_gridmap.data.data(), room_gridmap.info.width, room_gridmap.info.height, grid_spacing_as_int, 2, true); // a perfect alignment of the paths cannot be assumed here (in contrast to footprint planning) because the well-aligned fov trajectory is mapped to robot locations that may not be on parallel tracks + // get the exploration path + std::vector fov_path_uncleaned; + vm.setSingleRoom(true); //to force to consider all rooms + vm.generatePath(fov_path_uncleaned, cv::Mat(), starting_position.x, starting_position.y); // start position in room center + + // clean path from subsequent double occurrences of the same pose + std::vector fov_path; + downsampleTrajectory(fov_path_uncleaned, fov_path, 2.*2.); //5*5); + + // convert to poses with angles + RoomRotator room_rotation; + room_rotation.transformPointPathToPosePath(fov_path); + + // map fov-path to robot-path + //cv::Point start_pos(fov_path.begin()->x, fov_path.begin()->y); + //mapPath(room_map, exploration_path, fov_path, fitting_circle_center_point_in_meter, map_resolution, map_origin, start_pos); + ROS_INFO("Starting to map from field of view pose to robot pose"); + cv::Point robot_starting_position = (fov_path.size()>0 ? cv::Point(fov_path[0].x, fov_path[0].y) : starting_position); + cv::Mat inflated_room_map; + cv::erode(room_map, inflated_room_map, cv::Mat(), cv::Point(-1, -1), (int)std::floor(goal->robot_radius/map_resolution)); + mapPath(inflated_room_map, exploration_path, fov_path, fitting_circle_center_point_in_meter, map_resolution, map_origin, robot_starting_position); + } + else + { + // convert coverage-radius to pixel integer + //int coverage_diameter = (int)std::floor(2.*goal->coverage_radius/map_resolution); + //std::cout << "coverage radius in pixel: " << coverage_diameter << std::endl; + const int grid_spacing_as_int = (int)std::floor(grid_spacing_in_pixel); + std::cout << "grid spacing in pixel: " << grid_spacing_as_int << std::endl; + + // create the object that plans the path, based on the room-map + VoronoiMap vm(room_gridmap.data.data(), room_gridmap.info.width, room_gridmap.info.height, grid_spacing_as_int, 2, true); //coverage_diameter-1); // diameter in pixel (full working width can be used here because tracks are planned in parallel motion) + // get the exploration path + std::vector exploration_path_uncleaned; + vm.setSingleRoom(true); //to force to consider all rooms + vm.generatePath(exploration_path_uncleaned, cv::Mat(), starting_position.x, starting_position.y); // start position in room center + + // clean path from subsequent double occurrences of the same pose + downsampleTrajectory(exploration_path_uncleaned, exploration_path, 3.5*3.5); //3.5*3.5); + + // convert to poses with angles + RoomRotator room_rotation; + room_rotation.transformPointPathToPosePath(exploration_path); + + // transform to global coordinates + for(size_t pos=0; pos 0) +#if CV_MAJOR_VERSION<=3 + cv::circle(fov_path_map, 2*cv::Point((exploration_path[0].x-map_origin.x)/map_resolution, (exploration_path[0].y-map_origin.y)/map_resolution), 2, cv::Scalar(150), CV_FILLED); +#else + cv::circle(fov_path_map, 2*cv::Point((exploration_path[0].x-map_origin.x)/map_resolution, (exploration_path[0].y-map_origin.y)/map_resolution), 2, cv::Scalar(150), cv::FILLED); +#endif + for(size_t i=1; i<=step; ++i) + { + cv::Point p1((exploration_path[i-1].x-map_origin.x)/map_resolution, (exploration_path[i-1].y-map_origin.y)/map_resolution); + cv::Point p2((exploration_path[i].x-map_origin.x)/map_resolution, (exploration_path[i].y-map_origin.y)/map_resolution); +#if CV_MAJOR_VERSION<=3 + cv::circle(fov_path_map, 2*p2, 2, cv::Scalar(200), CV_FILLED); +#else + cv::circle(fov_path_map, 2*p2, 2, cv::Scalar(200), cv::FILLED); +#endif + cv::line(fov_path_map, 2*p1, 2*p2, cv::Scalar(150), 1); + cv::Point p3(p2.x+5*cos(exploration_path[i].theta), p2.y+5*sin(exploration_path[i].theta)); + if (i==step) + { +#if CV_MAJOR_VERSION<=3 + cv::circle(fov_path_map, 2*p2, 2, cv::Scalar(80), CV_FILLED); +#else + cv::circle(fov_path_map, 2*p2, 2, cv::Scalar(80), cv::FILLED); +#endif + cv::line(fov_path_map, 2*p1, 2*p2, cv::Scalar(150), 1); + cv::line(fov_path_map, 2*p2, 2*p3, cv::Scalar(50), 1); + } + } +// cv::imshow("cell path", fov_path_map); +// cv::waitKey(); + } + cv::imshow("cell path", fov_path_map); + cv::waitKey(); + } + + ROS_INFO("Room exploration planning finished."); + + ipa_building_msgs::RoomExplorationResult action_result; + // check if the size of the exploration path is larger then zero + if(exploration_path.size()==0) + { + room_exploration_server_.setAborted(action_result); + return; + } + + // if wanted, return the path as the result + if(return_path_ == true) + { + action_result.coverage_path = exploration_path; + // return path in PoseStamped format as well (e.g. necessary for move_base commands) + std::vector exploration_path_pose_stamped(exploration_path.size()); + std_msgs::Header header; + header.stamp = ros::Time::now(); + header.frame_id = "/map"; + for (size_t i=0; ifield_of_view, goal->field_of_view_origin, goal->coverage_radius, fitting_circle_center_point_in_meter.norm(), + map_resolution, goal->map_origin, grid_spacing_in_pixel, room_map.rows * map_resolution); + ROS_INFO("Explored room."); + } + + room_exploration_server_.setSucceeded(action_result); + + return; +} + + // remove unconnected, i.e. inaccessible, parts of the room (i.e. obstructed by furniture), only keep the room with the largest area +bool RoomExplorationServer::removeUnconnectedRoomParts(cv::Mat& room_map) +{ + // create new map with segments labeled by increasing labels from 1,2,3,... + cv::Mat room_map_int(room_map.rows, room_map.cols, CV_32SC1); + for (int v=0; v(v,u) == 255) + room_map_int.at(v,u) = -100; + else + room_map_int.at(v,u) = 0; + } + } + + std::map area_to_label_map; // maps area=number of segment pixels (keys) to the respective label (value) + int label = 1; + for (int v=0; v(v,u) == -100) + { + const int area = cv::floodFill(room_map_int, cv::Point(u,v), cv::Scalar(label), 0, 0, 0, 8 | cv::FLOODFILL_FIXED_RANGE); + area_to_label_map[area] = label; + ++label; + } + } + } + // abort if area_to_label_map.size() is empty + if (area_to_label_map.size() == 0) + return false; + + // remove all room pixels from room_map which are not accessible + const int label_of_biggest_room = area_to_label_map.rbegin()->second; + std::cout << "label_of_biggest_room=" << label_of_biggest_room << std::endl; + for (int v=0; v(v,u) != label_of_biggest_room) + room_map.at(v,u) = 0; + + return true; +} + + +void RoomExplorationServer::downsampleTrajectory(const std::vector& path_uncleaned, std::vector& path, const double min_dist_squared) +{ + // clean path from subsequent double occurrences of the same pose + path.push_back(path_uncleaned[0]); + cv::Point last_added_point(path_uncleaned[0].x, path_uncleaned[0].y); + for (size_t i=1; i min_dist_squared || i==path_uncleaned.size()-1) + { + path.push_back(path_uncleaned[i]); + last_added_point = current_point; + } + } +} + + +void RoomExplorationServer::navigateExplorationPath(const std::vector& exploration_path, + const std::vector& field_of_view, const geometry_msgs::Point32& field_of_view_origin, + const double coverage_radius, const double distance_robot_fov_middlepoint, const float map_resolution, + const geometry_msgs::Pose& map_origin, const double grid_spacing_in_pixel, const double map_height) +{ + // ***************** III. Navigate trough all points and save the robot poses to check what regions have been seen ***************** + // 1. publish navigation goals + std::vector robot_poses; + geometry_msgs::Pose2D last_pose; + geometry_msgs::Pose2D pose; + for(size_t map_oriented_pose = 0; map_oriented_pose < exploration_path.size(); ++map_oriented_pose) + { + // check if the path should be continued or not + bool interrupted = false; + if(interrupt_navigation_publishing_==true) + { + ROS_INFO("Interrupt order received, resuming coverage path later."); + interrupted = true; + } + while(interrupt_navigation_publishing_==true) + { + // sleep for 1s because else this loop would produce errors + std::cout << "sleeping... (-.-)zzZZ" << std::endl; + ros::Duration sleep_rate(1); + sleep_rate.sleep(); + } + if(interrupted==true) + ROS_INFO("Interrupt order canceled, resuming coverage path now."); + if(flag_==1) + { + cancel_msg.stamp = ros::Time::now(); + cancel_msg.id = std::to_string(ros::Time::now().toSec()); + cancel_pub_.publish(cancel_msg); + publishZeroVelocity(); + while(flag_==1) + { + // ROS_INFO("------bizhang------"); + turn(-1,17);//1 16 ,-1 17 + gostraight(20); //time 0.2m/s 20*0.1s + turn(1,16); + flag_ = 0; + + ros::Duration sleep_rate(2.0); + sleep_rate.sleep(); + } + gostraight(30); + map_oriented_pose++; + } + + // if no interrupt is wanted, publish the navigation goal + pose = exploration_path[map_oriented_pose]; + // todo: convert map to image properly, then this coordinate correction here becomes obsolete + //pose.y = map_height - (pose.y - map_origin.position.y) + map_origin.position.y; + double temp_goal_eps = 0; + if (use_dyn_goal_eps_) + { + if (map_oriented_pose != 0) + { + double delta_theta = std::fabs(last_pose.theta - pose.theta); + if (delta_theta > M_PI * 0.5) + delta_theta = M_PI * 0.5; + temp_goal_eps = (M_PI * 0.5 - delta_theta) / (M_PI * 0.5) * goal_eps_; + } + } + else + { + temp_goal_eps = goal_eps_; + } + std::cout<< map_oriented_pose <(global_costmap_topic_)); + ROS_INFO("Found global gridmap."); + + std::vector pixel_values; + pixel_values = global_costmap.data; + + // if wanted check for areas that haven't been seen during the execution of the path and revisit them, if wanted + if(revisit_areas_ == true) + { + // save the costmap as Mat of the same type as the given map (8UC1) + cv::Mat costmap_as_mat;//(global_map.cols, global_map.rows, CV_8UC1); + + mapToMat(global_costmap, costmap_as_mat); + + // 70% probability of being an obstacle + cv::threshold(costmap_as_mat, costmap_as_mat, 75, 255, cv::THRESH_BINARY_INV); + + // 3. draw the seen positions so the server can check what points haven't been seen + std::cout << "checking coverage using the coverage_check_server" << std::endl; + cv::Mat coverage_map, number_of_coverage_image; + // use the coverage check server to check which areas have been seen + // --> convert path to cv format + std::vector path; + for (size_t i=0; i convert field of view to Eigen format + std::vector > fov; + for(size_t i = 0; i < field_of_view.size(); ++i) + { + Eigen::Matrix current_vector; + current_vector << field_of_view[i].x, field_of_view[i].y; + fov.push_back(current_vector); + } + // --> convert field of view origin to Eigen format + Eigen::Matrix fov_origin; + fov_origin < call coverage checker + CoverageCheckServer coverage_checker; + if (coverage_checker.checkCoverage(costmap_as_mat, map_resolution, cv::Point2d(map_origin.position.x, map_origin.position.y), + path, fov, fov_origin, coverage_radius, (planning_mode_==PLAN_FOR_FOOTPRINT), false, coverage_map, number_of_coverage_image) == true) + { + std::cout << "got the service response" << std::endl; + } + else + { + ROS_WARN("Coverage check failed, is the coverage_check_server running?"); + room_exploration_server_.setAborted(); + return; + } + +// // service interface - can be deleted +// // define the request for the coverage check +// ipa_building_msgs::CheckCoverageRequest coverage_request; +// ipa_building_msgs::CheckCoverageResponse coverage_response; +// // fill request +// sensor_msgs::ImageConstPtr service_image; +// cv_bridge::CvImage cv_image; +// cv_image.encoding = "mono8"; +// cv_image.image = costmap_as_mat; +// service_image = cv_image.toImageMsg(); +// coverage_request.input_map = *service_image; +// coverage_request.map_resolution = map_resolution; +// coverage_request.map_origin = map_origin; +// coverage_request.path = robot_poses; +// coverage_request.field_of_view = field_of_view; +// coverage_request.field_of_view_origin = field_of_view_origin; +// coverage_request.coverage_radius = coverage_radius; +// coverage_request.check_number_of_coverages = false; +// std::cout << "filled service request for the coverage check" << std::endl; +// if(planning_mode_ == PLAN_FOR_FOV) +// coverage_request.check_for_footprint = false; +// else +// coverage_request.check_for_footprint = true; +// // send request +// if(ros::service::call(coverage_check_service_name_, coverage_request, coverage_response) == true) +// { +// std::cout << "got the service response" << std::endl; +// cv_bridge::CvImagePtr cv_ptr_obj; +// cv_ptr_obj = cv_bridge::toCvCopy(coverage_response.coverage_map, sensor_msgs::image_encodings::MONO8); +// coverage_map = cv_ptr_obj->image; +// } +// else +// { +// ROS_WARN("Coverage check failed, is the coverage_check_server running?"); +// room_exploration_server_.setAborted(); +// return; +// } + + // testing, parameter to show +// cv::namedWindow("initially seen areas", cv::WINDOW_NORMAL); +// cv::imshow("initially seen areas", seen_positions_map); +// cv::resizeWindow("initially seen areas", 600, 600); +// cv::waitKey(); + + // apply a binary filter on the image, making the drawn seen areas black + cv::threshold(coverage_map, coverage_map, 150, 255, cv::THRESH_BINARY); + + // ***************** IV. Find leftover areas and lay a grid over it, then plan a path trough all grids s.t. they can be covered by the fov. ***************** + // 1. find regions with an area that is bigger than a defined value, which have not been seen by the fov. + // hierarchy[{0,1,2,3}]={next contour (same level), previous contour (same level), child contour, parent contour} + // child-contour = 1 if it has one, = -1 if not, same for parent_contour + std::vector < std::vector > left_areas, areas_to_revisit; + std::vector < cv::Vec4i > hierarchy; + cv::findContours(coverage_map, left_areas, hierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_NONE); + + // find valid regions + for(size_t area = 0; area < left_areas.size(); ++area) + { + // don't look at hole contours + if (hierarchy[area][3] == -1) + { + double room_area = map_resolution * map_resolution * cv::contourArea(left_areas[area]); + //subtract the area from the hole contours inside the found contour, because the contour area grows extremly large if it is a closed loop + for(int hole = 0; hole < left_areas.size(); ++hole) + { + if(hierarchy[hole][3] == area)//check if the parent of the hole is the current looked at contour + { + room_area -= map_resolution * map_resolution * cv::contourArea(left_areas[hole]); + } + } + + // save the contour if the area of it is larger than the defined value + if(room_area >= left_sections_min_area_) + areas_to_revisit.push_back(left_areas[area]); + } + } + + // check if areas need to be visited again, if not cancel here + if(areas_to_revisit.size() == 0) + { + ROS_INFO("Explored room."); + + room_exploration_server_.setSucceeded(); + + return; + } + + // draw found regions s.t. they can be intersected later + cv::Mat black_map(costmap_as_mat.cols, costmap_as_mat.rows, costmap_as_mat.type(), cv::Scalar(0)); +#if CV_MAJOR_VERSION<=3 + cv::drawContours(black_map, areas_to_revisit, -1, cv::Scalar(255), CV_FILLED); +#else + cv::drawContours(black_map, areas_to_revisit, -1, cv::Scalar(255), cv::FILLED); +#endif + for(size_t contour = 0; contour < left_areas.size(); ++contour) + if(hierarchy[contour][3] != -1) +#if CV_MAJOR_VERSION<=3 + cv::drawContours(black_map, left_areas, contour, cv::Scalar(0), CV_FILLED); +#else + cv::drawContours(black_map, left_areas, contour, cv::Scalar(0), cv::FILLED); +#endif + + // 2. Intersect the left areas with respect to the calculated grid length. + geometry_msgs::Polygon min_max_coordinates; // = goal->room_min_max; + for(size_t i = 0/*min_max_coordinates.points[0].y*/; i < black_map.cols; i += std::floor(grid_spacing_in_pixel)) + cv::line(black_map, cv::Point(0, i), cv::Point(black_map.cols, i), cv::Scalar(0), 1); + for(size_t i = 0/*min_max_coordinates.points[0].x*/; i < black_map.rows; i += std::floor(grid_spacing_in_pixel)) + cv::line(black_map, cv::Point(i, 0), cv::Point(i, black_map.rows), cv::Scalar(0), 1); + + // 3. find the centers of the global_costmap areas + std::vector < std::vector > grid_areas; + cv::Mat contour_map = black_map.clone(); + cv::findContours(contour_map, grid_areas, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE); + + // get the moments + std::vector moments(grid_areas.size()); + for( int i = 0; i < grid_areas.size(); i++) + { + moments[i] = cv::moments(grid_areas[i], false); + } + + // get the mass centers + std::vector area_centers(grid_areas.size()); + for( int i = 0; i < grid_areas.size(); i++ ) + { + // check if the current contour has an area and isn't just a few pixels + if(moments[i].m10 != 0 && moments[i].m01 != 0) + { + area_centers[i] = cv::Point( moments[i].m10/moments[i].m00 , moments[i].m01/moments[i].m00 ); + } + // if contour is too small for moment calculation, take one point on this contour and use it as center + else + { + area_centers[i] = grid_areas[i][0]; + } + } + + // testing +// black_map = room_map.clone(); +// for(size_t i = 0; i < area_centers.size(); ++i) +// { +// cv::circle(black_map, area_centers[i], 2, cv::Scalar(127), CV_FILLED); +// std::cout << area_centers[i] << std::endl; +// } +// cv::namedWindow("revisiting areas", cv::WINDOW_NORMAL); +// cv::imshow("revisiting areas", black_map); +// cv::resizeWindow("revisiting areas", 600, 600); +// cv::waitKey(); + + // 4. plan a tsp path trough the centers of the left areas + // find the center that is nearest to the current robot position, which becomes the start node for the tsp + geometry_msgs::Pose2D current_robot_pose = robot_poses.back(); + cv::Point current_robot_point(current_robot_pose.x, current_robot_pose.y); + double min_dist = 9001; + int min_index = 0; + for(size_t current_center_index = 0; current_center_index < area_centers.size(); ++current_center_index) + { + cv::Point current_center = area_centers[current_center_index]; + double current_squared_distance = std::pow(current_center.x - current_robot_point.x, 2.0) + std::pow(current_center.y - current_robot_point.y, 2.0); + + if(current_squared_distance <= min_dist) + { + min_dist = current_squared_distance; + min_index = current_center_index; + } + } + ConcordeTSPSolver tsp_solver; + std::vector revisiting_order = tsp_solver.solveConcordeTSP(costmap_as_mat, area_centers, 0.25, 0.0, map_resolution, min_index, 0); + + // 5. go to each center and use the map_accessability_server to find a robot pose around it s.t. it can be covered by the field of view or robot center + const double pi_8 = PI/8; + const std::string perimeter_service_name = "/room_exploration/map_accessibility_analysis/map_perimeter_accessibility_check"; + // robot_poses.clear(); + for(size_t center = 0; center < revisiting_order.size(); ++center) + { + geometry_msgs::Pose2D current_center; + current_center.x = (area_centers[revisiting_order[center]].x * map_resolution) + map_origin.position.x; + current_center.y = (area_centers[revisiting_order[center]].y * map_resolution) + map_origin.position.y; + + // define request + cob_map_accessibility_analysis::CheckPerimeterAccessibility::Request check_request; + cob_map_accessibility_analysis::CheckPerimeterAccessibility::Response response; + check_request.center = current_center; + if(planning_mode_ == PLAN_FOR_FOV) + { + check_request.radius = distance_robot_fov_middlepoint; + check_request.rotational_sampling_step = pi_8; + } + else + { + check_request.radius = 0.0; + check_request.rotational_sampling_step = 2.0*PI; + } + std::cout << "checking center: " << std::endl << current_center << "radius: " << check_request.radius << std::endl; + + // send request + if(ros::service::call(perimeter_service_name, check_request, response) == true) + { + std::cout << "successful check of accessibility" << std::endl; + // go trough the found accessible positions and try to reach one of them + for(std::vector::iterator pose = response.accessible_poses_on_perimeter.begin(); pose != response.accessible_poses_on_perimeter.end(); ++pose) + if(publishNavigationGoal(*pose, map_frame_, camera_frame_, robot_poses, 0.0) == true) + break; + } + else + { + // todo: return areas that were not visible on radius + std::cout << "center not reachable on perimeter" << std::endl; + } + } + +// drawSeenPoints(copy, robot_poses, goal->field_of_view, corner_point_1, corner_point_2, map_resolution, map_origin); +// cv::namedWindow("seen areas", cv::WINDOW_NORMAL); +// cv::imshow("seen areas", copy); +// cv::resizeWindow("seen areas", 600, 600); +// cv::waitKey(); + } +} + + +// Function to publish a navigation goal for move_base. It returns true, when the goal could be reached. +// The function tracks the robot pose while moving to the goal and adds these poses to the given pose-vector. This is done +// because it allows to calculate where the robot field of view has theoretically been and identify positions of the map that +// the robot hasn't seen. +bool RoomExplorationServer::publishNavigationGoal(const geometry_msgs::Pose2D& nav_goal, const std::string map_frame, + const std::string camera_frame, std::vector& robot_poses, const double robot_to_fov_middlepoint_distance, + const double eps, const bool perimeter_check) +{ + // move base client, that sends navigation goals to a move_base action server + MoveBaseClient mv_base_client("/move_base", true); + + // wait for the action server to come up + while(mv_base_client.waitForServer(ros::Duration(5.0)) == false) + { + ROS_INFO("Waiting for the move_base action server to come up"); + } + + + geometry_msgs::Pose2D map_oriented_pose; + + map_oriented_pose.x = nav_goal.x; + map_oriented_pose.y = nav_goal.y; + map_oriented_pose.theta = nav_goal.theta; + + std::cout << "navigation goal: (" << map_oriented_pose.x << ", " << map_oriented_pose.y << ", " << map_oriented_pose.theta << ")" << std::endl; + + move_base_msgs::MoveBaseGoal move_base_goal; + + // create move_base_goal + move_base_goal.target_pose.header.frame_id = "map"; + move_base_goal.target_pose.header.stamp = ros::Time::now(); + + + move_base_goal.target_pose.pose.position.x = map_oriented_pose.x; + move_base_goal.target_pose.pose.position.y = map_oriented_pose.y; + move_base_goal.target_pose.pose.orientation.z = std::sin(map_oriented_pose.theta/2); + move_base_goal.target_pose.pose.orientation.w = std::cos(map_oriented_pose.theta/2); + + // send goal to the move_base sever, when one is found + ROS_INFO_STREAM("Sending goal with eps " << eps); + mv_base_client.sendGoal(move_base_goal); + + // wait until goal is reached or the goal is aborted +// ros::Duration sleep_rate(0.1); + tf::TransformListener listener; + tf::StampedTransform transform; + ros::Duration sleep_duration(0.15); // todo: param + bool near_pos; + do + { + near_pos = false; + double roll, pitch, yaw; + // try to get the transformation from map_frame to base_frame, wait max. 2 seconds for this transform to come up + try + { + ros::Time time = ros::Time(0); + listener.waitForTransform(map_frame, camera_frame, time, ros::Duration(2.0)); // 5.0 + listener.lookupTransform(map_frame, camera_frame, time, transform); + + sleep_duration.sleep(); + + // save the current pose if a transform could be found + geometry_msgs::Pose2D current_pose; + + current_pose.x = transform.getOrigin().x(); + current_pose.y = transform.getOrigin().y(); + transform.getBasis().getRPY(roll, pitch, yaw); + current_pose.theta = yaw; + + if((current_pose.x-map_oriented_pose.x)*(current_pose.x-map_oriented_pose.x) + (current_pose.y-map_oriented_pose.y)*(current_pose.y-map_oriented_pose.y) <= eps*eps) + near_pos = true; + + robot_poses.push_back(current_pose); + } + catch(tf::TransformException &ex) + { + ROS_WARN_STREAM("Couldn't get transform from " << camera_frame << " to " << map_frame << "!");// %s", ex.what()); + } + + }while(mv_base_client.getState() != actionlib::SimpleClientGoalState::ABORTED && mv_base_client.getState() != actionlib::SimpleClientGoalState::SUCCEEDED + && near_pos == false); + + // check if point could be reached or not + if(mv_base_client.getState() == actionlib::SimpleClientGoalState::SUCCEEDED || near_pos == true) + { + ROS_INFO("current goal could be reached."); + return true; + } + // if the goal couldn't be reached, find another point around the desired fov-position + else if(perimeter_check == true) + { + ROS_INFO("current goal could not be reached, checking for other goal."); + + // get the desired fov-position + geometry_msgs::Pose2D relative_vector; + relative_vector.x = std::cos(map_oriented_pose.theta)*robot_to_fov_middlepoint_distance; + relative_vector.y = std::sin(map_oriented_pose.theta)*robot_to_fov_middlepoint_distance; + geometry_msgs::Pose2D center; + center.x = map_oriented_pose.x + relative_vector.x; + center.y = map_oriented_pose.y + relative_vector.y; + + // check for another robot pose to reach the desired fov-position + const std::string perimeter_service_name = "/room_exploration/map_accessibility_analysis/map_perimeter_accessibility_check"; + cob_map_accessibility_analysis::CheckPerimeterAccessibility::Response response; + cob_map_accessibility_analysis::CheckPerimeterAccessibility::Request check_request; + check_request.center = center; + if(planning_mode_ == PLAN_FOR_FOV) + { + check_request.radius = robot_to_fov_middlepoint_distance; + check_request.rotational_sampling_step = PI/8; + } + else + { + check_request.radius = 0.0; + check_request.rotational_sampling_step = 2.0*PI; + } + + // send request + if(ros::service::call(perimeter_service_name, check_request, response) == true) + { + // go trough the found accessible positions and try to reach one of them + for(std::vector::iterator pose = response.accessible_poses_on_perimeter.begin(); pose != response.accessible_poses_on_perimeter.end(); ++pose) + { + if(publishNavigationGoal(*pose, map_frame, camera_frame, robot_poses, 0.0) == true) + { + ROS_INFO("Perimeter check for not reachable goal succeeded."); + return true; + } + } + } + else + { + ROS_INFO("Desired position not reachable."); + } + return false; + } + else + { + return false; + } +} + +void RoomExplorationServer::publishZeroVelocity() +{ + // ROS_INFO("stop!!"); + cmd_vel_msg.angular.z = 0; + cmd_vel_msg.linear.x = 0; + cmd_vel_pub_.publish(cmd_vel_msg); + +} +void RoomExplorationServer::turn(int lr,int time) +{ + ros::Rate rate(10); + + cmd_vel_msg.angular.z = lr; + // if(lr==-1) + // { + // std::cout<<"turn right"<dis) + { + float distance = obstacle_info.distance; + // float width = obstacle_info.width; + // float height = obstacle_info.height; + float angle = obstacle_info.angle; + + // if(distance>15&&distance<35&&cmd_vel_.linear.x!=0) + if(distance>5&&distance<45) + { + // ROS_INFO("distance: %.2f ", distance); + flag_ = 1; + // std::cout<<"distance===="<. + * + ****************************************************************/ + + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include + +#include + +#define PI 3.14159265359 + +// Overload of << operator for geometry_msgs::Pose2D to wanted format. +std::ostream& operator<<(std::ostream& os, const geometry_msgs::Pose2D& obj) +{ + std::stringstream ss; + ss << "[" << obj.x << ", " << obj.y << ", " << obj.theta << "]"; + os << ss.rdbuf(); + return os; +} + +// Struct used to define, which segmentation algorithm together with which exploration algorithm should be used. Also a function is +// provided, that returns a string showing the current configuration --> used to save the results. +struct ExplorationConfig +{ + int exploration_algorithm_; // this variable selects, which exploration algorithm should be used + // 1: grid point explorator + // 2: boustrophedon explorator + // 3: neural network explorator + // 4: convexSPP explorator + // 5: flowNetwork explorator + // 6: energyFunctional explorator + // 7: Voronoi explorator + + // default values + ExplorationConfig() + { + exploration_algorithm_ = 2; + } + + // create one configuration + ExplorationConfig(const int exploration_algorithm) + { + exploration_algorithm_ = exploration_algorithm; + } + + // function that returns the current configuration as string + std::string generateConfigurationFolderString() const + { + std::stringstream ss; + ss << "expl" << exploration_algorithm_; + return ss.str(); + } + + // function that returns the name of the chosen exploration algorithm + std::string roomExplorationAlgorithmToString() const + { + std::string s = ""; + if (exploration_algorithm_ == 1) + s = "grid point exploration"; + else if (exploration_algorithm_ == 2) + s = "boustrophedon exploration"; + else if (exploration_algorithm_ == 3) + s = "neural network exploration"; + else if (exploration_algorithm_ == 4) + s = "convex SPP exploration"; + else if (exploration_algorithm_ == 5) + s = "flow network exploration"; + else if (exploration_algorithm_ == 6) + s = "energy functional exploration"; + else if (exploration_algorithm_ == 7) + s = "voronoi exploration"; + + return s; + } +}; + +// Struct that carries several parameters for the action servers +enum PlanningMode {FOOTPRINT=1, FIELD_OF_VIEW=2}; +struct ExplorationData +{ + std::string map_name_; // without file type + cv::Mat floor_plan_; + std::vector room_maps_; + std::vector bounding_boxes_; + float map_resolution_; // [m/pixel] + geometry_msgs::Pose map_origin_; // [m] + geometry_msgs::Pose2D robot_start_position_; + double robot_radius_; // [m], effective robot radius, taking the enlargement of the costmap into account, in [meter] + double coverage_radius_; // [m], radius that is used to plan the coverage planning for the robot and not the field of view, assuming that the part that needs to cover everything (e.g. the cleaning part) can be represented by a fitting circle (e.g. smaller than the actual part to ensure coverage), in [meter] + std::vector fov_points_; // [m], the points that define the field of view of the robot, relative to the robot center (x-axis points to robot's front side, y-axis points to robot's left side, z-axis upwards), in [meter] + geometry_msgs::Point32 fov_origin_; // [m], the mounting position of the camera spanning the field of view, relative to the robot center (x-axis points to robot's front side, y-axis points to robot's left side, z-axis upwards), in [meter] + enum PlanningMode planning_mode_; // 1 = plans a path for coverage with the robot footprint, 2 = plans a path for coverage with the robot's field of view + double robot_speed_; // [m/s] + double robot_rotation_speed_; // [rad/s] + + // empty values as default + ExplorationData() + { + map_name_ = ""; + floor_plan_ = cv::Mat(); + map_resolution_ = 0.05; + map_origin_.position.x = 0; + map_origin_.position.y = 0; + robot_radius_ = 0.35; + coverage_radius_ = 0.35; + planning_mode_ = FOOTPRINT; + robot_speed_ = 0.3; + robot_rotation_speed_ = 0.1; + } + + // set data used in this evaluation + ExplorationData(const std::string map_name, const cv::Mat floor_plan, const float map_resolution, const double robot_radius, + const double coverage_radius, const std::vector& fov_points, const geometry_msgs::Point32& fov_origin, + const int planning_mode, const double robot_speed, const double robot_rotation_speed) + { + map_name_ = map_name; + floor_plan_ = floor_plan; + map_resolution_ = map_resolution; + map_origin_.position.x = 0; + map_origin_.position.y = 0; + robot_radius_ = robot_radius; + coverage_radius_ = coverage_radius; + fov_points_ = fov_points; + fov_origin_ = fov_origin; + planning_mode_ = (PlanningMode)planning_mode; + robot_speed_ = robot_speed; + robot_rotation_speed_ = robot_rotation_speed; + cv::Mat map_eroded; + cv::erode(floor_plan_, map_eroded, cv::Mat(), cv::Point(-1,-1), robot_radius_/map_resolution_+2); + cv::Mat distance_map; //variable for the distance-transformed map, type: CV_32FC1 + cv::distanceTransform(map_eroded, distance_map, CV_DIST_L2, 5); + cv::convertScaleAbs(distance_map, distance_map); // conversion to 8 bit image + float max_distance = 0; + for (int v=0; v(v,u) != 0 && distance_map.at(v,u) > max_distance) + { + max_distance = distance_map.at(v,u); + robot_start_position_.x = u*map_resolution_ + map_origin_.position.x; + robot_start_position_.y = v*map_resolution_ + map_origin_.position.y; + } + } +}; + + +// class that segments the wanted maps, finds for each resulting room a coverage path and saves these paths +class ExplorationEvaluation +{ +protected: + + // function that creates configurations to get all possible combinations of exploration algorithms + void setConfigurations(std::vector& configurations, const std::vector& exploration_algorithms) + { + for(std::vector::const_iterator expl=exploration_algorithms.begin(); expl!=exploration_algorithms.end(); ++expl) + { + ExplorationConfig current_config(*expl); + configurations.push_back(current_config); + } + } + + template + double stddev(const std::vector& values, const double mean) + { + if (values.size() < 2) + return 0.; + double stddev = 0.; + for (typename std::vector::const_iterator val=values.begin(); val!=values.end(); ++val) + stddev += ((double)*val-mean)*((double)*val-mean); + return sqrt(stddev)/(values.size()-1); + } + + ros::NodeHandle node_handle_; + +public: + + + ExplorationEvaluation(ros::NodeHandle& nh, const std::string& test_map_path, const std::vector& map_names, const float map_resolution, + const std::string& data_storage_path, const double robot_radius, const double coverage_radius, + const std::vector& fov_points, const geometry_msgs::Point32& fov_origin, const int planning_mode, + const std::vector& exploration_algorithms, const double robot_speed, const double robot_rotation_speed, bool do_path_planning=true, bool do_evaluation=true) + : node_handle_(nh) + { + // 1. create all needed configurations + std::vector configs; + setConfigurations(configs, exploration_algorithms); + + // 2. prepare images and evaluation data + std::vector evaluation_data; + for (size_t image_index = 0; image_index(y, x)>250) + map.at(y, x)=255; + else //if (map.at(y, x) != 255) + map.at(y, x)=0; + } + } + + // create evaluation data + evaluation_data.push_back(ExplorationData(map_names[image_index], map, map_resolution, robot_radius, coverage_radius, fov_points, fov_origin, + planning_mode, robot_speed, robot_rotation_speed)); + } + // get the room maps for each evaluation data + getRoomMaps(evaluation_data); + + // 3. compute exploration paths for each room in the maps + if (do_path_planning == true) + { + std::string bugfile = data_storage_path + "bugfile.txt"; + std::ofstream failed_maps(bugfile.c_str(), std::ios::out); + if (failed_maps.is_open()) + failed_maps << "Maps that had a bug during the simulation and couldn't be finished: " << std::endl; + ROS_INFO("Evaluating the maps."); + for (size_t i=0; i1) + failed_maps << evaluation_data[i].map_name_ << std::endl << error_output.str(); + } + if (failed_maps.is_open()) + failed_maps.close(); + } + + // 4. evaluate the generated paths + // read out the computed paths and calculate the evaluation values + if (do_evaluation == true) + { + ROS_INFO("Reading out all saved paths."); + for (size_t i=0; i& data_saver) + { + for(std::vector::iterator data=data_saver.begin(); data!=data_saver.end(); ++data) + { + // 1. read out the ground truth map + std::string map_name_basic = data->map_name_; + std::size_t pos = data->map_name_.find("_furnitures"); + if (pos != std::string::npos) + map_name_basic = data->map_name_.substr(0, pos); + std::string gt_image_filename = ros::package::getPath("ipa_room_segmentation") + "/common/files/test_maps/" + map_name_basic + "_gt_segmentation.png"; + std::cout << "Loading ground truth segmentation from: " << gt_image_filename << std::endl; + cv::Mat gt_map = cv::imread(gt_image_filename.c_str(), CV_8U); + cv::threshold(gt_map, gt_map, 250, 255, CV_THRESH_BINARY); + + // load the original map (without furniture if applicable) + std::string original_image_filename = ros::package::getPath("ipa_room_segmentation") + "/common/files/test_maps/" + map_name_basic + ".png"; + std::cout << "Loading original image from: " << original_image_filename << std::endl; + cv::Mat original_map = cv::imread(original_image_filename.c_str(), CV_8U); + + // combine real floor plan (but not its furniture) and gt_map + for (int y = 0; y < gt_map.rows; y++) + for (int x = 0; x < gt_map.cols; x++) + if (original_map.at(y,x) <= 250) + gt_map.at(y,x) = 0; + + // 2. retrieve the rooms for each ground truth map and get the maps that show only one room each + int label = 1; + std::vector bounding_boxes; + cv::Mat labeled_map; + gt_map.convertTo(labeled_map, CV_32SC1); + for (int y = 0; y < gt_map.rows; y++) + { + for (int x = 0; x < gt_map.cols; x++) + { + if (gt_map.at(y,x)!=255 || labeled_map.at(y,x)!=255) + continue; + + // fill each room area with a unique id + cv::Rect rect; + cv::floodFill(labeled_map, cv::Point(x,y), label, &rect, 0, 0, 8); + + // save the bounding box to retrieve the min/max coordinates + bounding_boxes.push_back(rect); + + ++label; + } + } + std::vector room_maps; + std::vector chosen_bb; + for(int room=1; room(y,x)==room && data->floor_plan_.at(y,x)==255) + room_map.at(y,x) = 255; + + // check for the eroded map (the map that shows the in reality reachable areas) to have enough free pixels + cv::Mat eroded_map; + const int robot_radius_in_pixel = (data->robot_radius_ / data->map_resolution_); + cv::erode(room_map, eroded_map, cv::Mat(), cv::Point(-1, -1), robot_radius_in_pixel); + int number_of_pixels = 0; + for(size_t y=0; y(y,x)==255) + ++number_of_pixels; + + // save room map, if region is big enough + if(number_of_pixels>0) + { + room_maps.push_back(room_map); + chosen_bb.push_back(bounding_boxes[room-1]); +// cv::rectangle(room_map, bounding_boxes[room-1], cv::Scalar(127), 2); +// cv::imshow("room", room_map); +// cv::waitKey(); + } + } + + // combine real floor plan (now including its furniture) and gt_map + for (int y = 0; y < gt_map.rows; y++) + for (int x = 0; x < gt_map.cols; x++) + if (data->floor_plan_.at(y,x) != 255) + gt_map.at(y,x) = 0; + + // save the found room maps and bounding boxes + data->floor_plan_ = gt_map; + data->room_maps_ = room_maps; + data->bounding_boxes_ = chosen_bb; + } + } + + // function that does the whole evaluation for all configs on all rooms of one map + bool planCoveragePaths(ExplorationData& data, const std::vector& configs, const std::string data_storage_path, std::stringstream& error_output) + { + // go trough all configs and do the evaluations + for(std::vector::const_iterator config=configs.begin(); config!=configs.end(); ++config) + { + // create a folder for the log directory + const std::string configuration_folder_name = config->generateConfigurationFolderString() + "/"; + const std::string upper_command = "mkdir -p " + data_storage_path + configuration_folder_name; + int return_value = system(upper_command.c_str()); + + std::cout << "Exploration algorithm: " << config->exploration_algorithm_ << std::endl; + //variables for time measurement + struct timespec t0, t1; + + // go trough all rooms and find the coverage path trough it + std::stringstream output; + cv::Mat path_map = data.floor_plan_.clone(); + for(size_t room_index=0; room_index coverage_path = result_expl->coverage_path; + // transform path to map coordinates + std::cout << "length of path: " << coverage_path.size() << std::endl; + if(coverage_path.size()==0) + { + output << "room " << room_index << " has zero length path" << std::endl << std::endl; + error_output << " room " << room_index << " has zero length path" << std::endl; + continue; + } + const double map_resolution_inv = 1.0/data.map_resolution_; + for(size_t point=0; point 0) + cv::line(path_map, cv::Point(coverage_path[point].x, coverage_path[point].y), cv::Point(coverage_path[point-1].x, coverage_path[point-1].y), cv::Scalar(128), 1); +// std::cout << "coverage_path[" << point << "]: x=" << coverage_path[point].x << ", y=" << coverage_path[point].y << ", theta=" << coverage_path[point].theta << std::endl; +// cv::imshow("path", path_map); +// cv::waitKey(); + } +// cv::imshow("path", path_map); +// cv::waitKey(); + } + const std::string img_filename = data_storage_path + configuration_folder_name + data.map_name_ + "_paths.png"; + cv::imwrite(img_filename.c_str(), path_map); + + const std::string log_filename = data_storage_path + configuration_folder_name + data.map_name_ + "_results.txt"; + std::cout << log_filename << std::endl; + std::ofstream file(log_filename.c_str(), std::ios::out); + if (file.is_open()==true) + file << output.str(); + else + ROS_ERROR("Error on writing file '%s'", log_filename.c_str()); + file.close(); + } + + // if all configurations finished, return a boolean showing success + return true; + } + + // function that plans one coverage path for the given room map + bool planCoveragePath(const cv::Mat& room_map, const ExplorationData& evaluation_data, const ExplorationConfig& evaluation_configuration, + ipa_building_msgs::RoomExplorationResultConstPtr& result_expl) + { + // convert image to message + sensor_msgs::Image map_msg; + cv_bridge::CvImage cv_image; + cv_image.encoding = "mono8"; + cv_image.image = room_map; + cv_image.toImageMsg(map_msg); + + // initialize action server for room exploration + actionlib::SimpleActionClient ac_exp("room_exploration_server", true); + ROS_INFO("Waiting for action server to start."); + ac_exp.waitForServer(); //will wait for infinite time + ROS_INFO("Action server started."); + + // connect to dynamic reconfigure and set planning algorithm + ROS_INFO("Trying to connect to dynamic reconfigure server."); + DynamicReconfigureClient drc_exp(node_handle_, "room_exploration_server/set_parameters", "room_exploration_server/parameter_updates"); + ROS_INFO("Done connecting to the dynamic reconfigure server."); + if (evaluation_configuration.exploration_algorithm_==1) + { + drc_exp.setConfig("room_exploration_algorithm", 1); + ROS_INFO("You have chosen the grid exploration method."); + } + else if(evaluation_configuration.exploration_algorithm_==2) + { + drc_exp.setConfig("room_exploration_algorithm", 2); + ROS_INFO("You have chosen the boustrophedon exploration method."); + } + else if(evaluation_configuration.exploration_algorithm_==3) + { + drc_exp.setConfig("room_exploration_algorithm", 3); + ROS_INFO("You have chosen the neural network exploration method."); + } + else if(evaluation_configuration.exploration_algorithm_==4) + { + drc_exp.setConfig("room_exploration_algorithm", 4); + ROS_INFO("You have chosen the convexSPP exploration method."); + } + else if(evaluation_configuration.exploration_algorithm_==5) + { + drc_exp.setConfig("room_exploration_algorithm", 5); + ROS_INFO("You have chosen the flow network exploration method."); + } + else if(evaluation_configuration.exploration_algorithm_==6) + { + drc_exp.setConfig("room_exploration_algorithm", 6); + ROS_INFO("You have chosen the energy functional exploration method."); + } + else if(evaluation_configuration.exploration_algorithm_==7) + { + drc_exp.setConfig("room_exploration_algorithm", 7); + ROS_INFO("You have chosen the Voronoi exploration method."); + } + + // prepare and send the action message + ipa_building_msgs::RoomExplorationGoal goal; + goal.input_map = map_msg; + goal.map_resolution = evaluation_data.map_resolution_; + goal.map_origin = evaluation_data.map_origin_; + goal.robot_radius = evaluation_data.robot_radius_; + goal.coverage_radius = evaluation_data.coverage_radius_; + goal.field_of_view = evaluation_data.fov_points_; + goal.field_of_view_origin = evaluation_data.fov_origin_; + goal.planning_mode = evaluation_data.planning_mode_; + goal.starting_position = evaluation_data.robot_start_position_; + ac_exp.sendGoal(goal); + + // wait for results for some time + bool finished = false; + if(evaluation_configuration.exploration_algorithm_==5) // different timeout for the flowNetworkExplorator, because it can be much slower than the others + finished = ac_exp.waitForResult(ros::Duration(600)); // todo: adapt if necessary + else + finished = ac_exp.waitForResult(ros::Duration(36000)); + + //if it takes too long the server should be killed and restarted + if (finished == false) + { + std::cout << "action server took too long" << std::endl; + std::string pid_cmd = "pidof room_exploration_server > room_exploration_evaluation/expl_srv_pid.txt"; + int pid_result = system(pid_cmd.c_str()); + std::ifstream pid_reader("room_exploration_evaluation/expl_srv_pid.txt"); + int value; + std::string line; + if (pid_reader.is_open()) + { + while (getline(pid_reader, line)) + { + std::istringstream iss(line); + while (iss >> value) + { + std::cout << "PID of room_exploration_server: " << value << std::endl; + std::stringstream ss; + ss << "kill " << value; + std::string kill_cmd = ss.str(); + int kill_result = system(kill_cmd.c_str()); + std::cout << "kill result: " << kill_result << std::endl; + } + } + pid_reader.close(); + remove("room_exploration_evaluation/expl_srv_pid.txt"); + } + else + { + std::cout << "missing logfile" << std::endl; + } + return false; + } + else + { + // retrieve solution + result_expl = ac_exp.getResult(); + std::cout << "Finished coverage planning successfully!" << std::endl; + + // show success + return true; + } + } + + // function that reads out the calculated paths and does the evaluation + void evaluateCoveragePaths(const ExplorationData& data, const std::vector& configs, const std::string data_storage_path) + { + // evaluate the individual configurations + for(std::vector::const_iterator config=configs.begin(); config!=configs.end(); ++config) + { + evaluateCoveragePaths(data, *config, data_storage_path); + } + } + + // function that reads out the calculated paths and does the evaluation for one configuration + void evaluateCoveragePaths(const ExplorationData& data, const ExplorationConfig& config, const std::string data_storage_path) + { + const std::string configuration_folder_name = config.generateConfigurationFolderString() + "/"; + std::cout << configuration_folder_name << data.map_name_ << std::endl; + + + // 1. get the location of the results and open this file, read out the given paths and computation times for all rooms + std::vector > paths; // in [pixels] + std::vector calculation_times; + readResultsFile(data, config, data_storage_path, paths, calculation_times); + + + // 2. prepare the data + cv::Mat map = data.floor_plan_.clone(); + cv::Point2d fov_circle_center_point_in_px; + double grid_spacing_in_pixel = 0; + computeFOVCenterAndGridSpacing(data, fov_circle_center_point_in_px, grid_spacing_in_pixel); + + // 3. path map, path length, turns, crossings statistics + // overall, average path length and variance of it for the calculated paths and get the numbers of turns + std::vector pathlengths_for_map; // in [m], stores the individual path lengths for each trajectory in each room + std::vector > interpolated_paths; // in [m], variable that stores the rough path points of paths and the cell-fine trajectories between them + int nonzero_paths = 0; + std::vector rotation_values; + std::vector number_of_rotations, number_of_crossings; + cv::Mat path_map = map.clone(); + statisticsPathLengthCrossingsTurns(data, map, path_map, fov_circle_center_point_in_px, paths, interpolated_paths, + pathlengths_for_map, nonzero_paths, number_of_crossings, rotation_values, number_of_rotations); + // save the map with the drawn in coverage paths + const std::string image_path = data_storage_path + configuration_folder_name + data.map_name_ + "_paths_eval.png"; + cv::imwrite(image_path.c_str(), path_map); +// cv::imshow("path_map", path_map); +// cv::waitKey(); + // calculate the overall path length, the average and the stddev + const double pathlength_total = std::accumulate(pathlengths_for_map.begin(), pathlengths_for_map.end(), 0.0); + const double pathlength_mean = pathlength_total / std::max(1.0, (double)pathlengths_for_map.size()); + const double pathlength_stddev = stddev(pathlengths_for_map, pathlength_mean); + std::cout << "Computing and drawing paths completed." << std::endl; + + + // 4. calculate turn specific values + const double rotation_values_total = std::accumulate(rotation_values.begin(), rotation_values.end(), 0.0); + const double rotation_values_mean = rotation_values_total / std::max(1.0, (double)rotation_values.size()); + const double rotation_values_stddev = stddev(rotation_values, rotation_values_mean); + const double number_of_rotations_total = std::accumulate(number_of_rotations.begin(), number_of_rotations.end(), 0.0); + const double number_of_rotations_mean = number_of_rotations_total / std::max(1.0, (double)number_of_rotations.size()); + const double number_of_rotations_stddev = stddev(number_of_rotations, number_of_rotations_mean); + + + // 5. calculate the execution time by using the robot speed and the rotation speed + std::vector travel_times_in_rooms(pathlengths_for_map.size()); + for(size_t i=0; i room_areas; // in [m^2], the area of each room + std::vector area_covered_percentages; // in [0,1], the ratio of coverage for each room + std::vector numbers_of_coverages; // counts how often a map cell has been covered + cv::Mat map_coverage; + cv::Mat map_path_coverage; + statisticsCoverageArea(data, map, path_map, map_coverage, map_path_coverage, paths, interpolated_paths, room_areas, area_covered_percentages, numbers_of_coverages); + // save the map with the drawn in coverage areas + const std::string coverage_image_path = data_storage_path + configuration_folder_name + data.map_name_ + "_coverage_eval.png"; + cv::imwrite(coverage_image_path.c_str(), map_coverage); + // save the map with the drawn in path and coverage areas + const std::string path_coverage_image_path = data_storage_path + configuration_folder_name + data.map_name_ + "_coverage_paths_eval.png"; + cv::imwrite(path_coverage_image_path.c_str(), map_path_coverage); + // calculate average coverage and deviation + const double room_area_mean = std::accumulate(room_areas.begin(), room_areas.end(), 0.0) / std::max(1.0, (double)room_areas.size()); + const double coverage_percentage_mean = std::accumulate(area_covered_percentages.begin(), area_covered_percentages.end(), 0.0) / std::max(1.0, (double)area_covered_percentages.size()); + const double coverage_percentage_stddev = stddev(area_covered_percentages, coverage_percentage_mean); + const double coverage_number_mean = std::accumulate(numbers_of_coverages.begin(), numbers_of_coverages.end(), 0.0) / std::max(1.0, (double)numbers_of_coverages.size()); + const double coverage_number_stddev = stddev(numbers_of_coverages, coverage_number_mean); + std::cout << "Checked coverage for all rooms." << std::endl; + + + // 7. compute average computation time and deviation + const double computation_time_mean = std::accumulate(calculation_times.begin(), calculation_times.end(), 0.0) / std::max(1.0, (double)calculation_times.size()); + const double computation_time_stddev = stddev(calculation_times, computation_time_mean); + + + // 8. parallelism: for each part of the path calculate the parallelism with respect to the nearest wall and the nearest trajectory part + std::vector wall_angle_score_means, trajectory_angle_score_means; // score for parallelism of trajectory to walls and previous parts of the trajectory itself, in range [0,1], high values are good + std::vector revisit_time_means; // vector that stores the index-differences of the current pose and the point of its nearest neighboring trajectory, values in [0,1], low values are good + statisticsParallelism(data, map, path_map, paths, interpolated_paths, grid_spacing_in_pixel, wall_angle_score_means, trajectory_angle_score_means, revisit_time_means); + // calculate mean and stddev of the wall angle scores + const double wall_angle_score_mean = std::accumulate(wall_angle_score_means.begin(), wall_angle_score_means.end(), 0.0) / std::max(1.0, (double)wall_angle_score_means.size()); + const double wall_angle_score_stddev = stddev(wall_angle_score_means, wall_angle_score_mean); + // calculate mean and stddev of the trajectory angle scores + const double trajectory_angle_score_mean = std::accumulate(trajectory_angle_score_means.begin(), trajectory_angle_score_means.end(), 0.0) / std::max(1.0, (double)trajectory_angle_score_means.size()); + const double trajectory_angle_score_stddev = stddev(trajectory_angle_score_means, trajectory_angle_score_mean); + // calculate mean and stddev of the trajectory revisit times + const double revisit_time_mean = std::accumulate(revisit_time_means.begin(), revisit_time_means.end(), 0.0) / std::max(1.0, (double)revisit_time_means.size());; + const double revisit_time_stddev = stddev(revisit_time_means, revisit_time_mean); + std::cout << "Checked parallelism for all rooms." << std::endl; + + + // 9. calculate the number of crossings related values + const double crossings_mean = std::accumulate(number_of_crossings.begin(), number_of_crossings.end(), 0.0) / std::max(1.0, (double)number_of_crossings.size()); + const double crossings_stddev = stddev(number_of_crossings, crossings_mean); + + // 10. calculate the subjective measure for the paths + const double subjective_measure_mean = wall_angle_score_mean + trajectory_angle_score_mean + -0.25*(crossings_mean/room_area_mean + pathlength_mean/room_area_mean + rotation_values_mean/(PI*room_area_mean) + revisit_time_mean); + + + // ---------- 11. store all results to a file ---------- + // print the found average evaluation values to a local file + std::stringstream output; + output << "Expl" << config.exploration_algorithm_ << ", number of rooms: " << paths.size() << ", number of valid paths: " + << nonzero_paths << std::endl; + output << "calculation time average [s]\t" << "calculation time stddev [s]\t" + << "pathlength total [m]\t" << "pathlength average [m]\t" << "pathlength stddev [m]\t" + << "rotation values total [rad]\t" << "rotation values average [rad]\t" << "rotation values stddev [rad]\t" + << "number of rotations total\t" << "number of rotations average\t" << "number of rotations stddev\t" + << "execution time total [s]\t" << "execution time average [s]\t" << "execution time stddev\t" + << "covered area average [0,1]\t" << "covered area stddev\t" << "coverage per map cell average\t" << "coverage per map cell deviation\t" + << "number of crossings average\t" << "number of crossings stddev\t" + << "wall angle score average\t" << "wall angle score stddev\t" + << "trajectory angle score average\t" << "trajectory angle score stddev\t" + << "average time until traj. is near previous traj.\t" << "stddev of revisit time\t" + << "subjective measure\t"<< std::endl; + output << computation_time_mean << "\t" << computation_time_stddev << "\t" + << pathlength_total << "\t" << pathlength_mean << "\t" << pathlength_stddev << "\t" + << rotation_values_total << "\t" << rotation_values_mean << "\t" << rotation_values_stddev << "\t" + << number_of_rotations_total << "\t" << number_of_rotations_mean << "\t" << number_of_rotations_stddev << "\t" + << execution_time_total << "\t" << execution_time_mean << "\t" << execution_time_stddev << "\t" + << coverage_percentage_mean << "\t" << coverage_percentage_stddev << "\t" << coverage_number_mean << "\t" << coverage_number_stddev << "\t" + << crossings_mean << "\t" << crossings_stddev << "\t" + << wall_angle_score_mean << "\t" << wall_angle_score_stddev << "\t" + << trajectory_angle_score_mean << "\t" << trajectory_angle_score_stddev << "\t" + << revisit_time_mean << "\t" << revisit_time_stddev << "\t" + << subjective_measure_mean; + std::string filename = data_storage_path + configuration_folder_name + data.map_name_ + "_results_eval.txt"; + std::ofstream file(filename.c_str(), std::ofstream::out); + if (file.is_open()) + file << output.str(); + else + ROS_ERROR("Could not write to file '%s'.", filename.c_str()); + file.close(); + + // print detailed information for each room to a separate file + if (calculation_times.size()!=pathlengths_for_map.size() || calculation_times.size()!=rotation_values.size() || + calculation_times.size()!=area_covered_percentages.size() || calculation_times.size()!= room_areas.size() || + calculation_times.size()!=trajectory_angle_score_means.size() || calculation_times.size()!= wall_angle_score_means.size() || calculation_times.size()!= revisit_time_means.size() || + calculation_times.size()!=number_of_crossings.size() || calculation_times.size()!=number_of_rotations.size()) + { + std::cout << "Error in evaluation: array sizes do not match:\n calculation_times.size()=" << calculation_times.size() + << "\n pathlengths_for_map.size()=" << pathlengths_for_map.size() << "\n rotation_values.size()=" << rotation_values.size() + << "\n area_covered_percentages.size()=" << area_covered_percentages.size() << "\n room_areas.size()=" << room_areas.size() + << "\n trajectory_angle_score_means.size()=" << trajectory_angle_score_means.size() << "\n room_wall_averages.size()=" << wall_angle_score_means.size() + << "\n room_revisit_averages.size()=" << revisit_time_means.size() << "\n numbers_of_crossings.size()=" << number_of_crossings.size() + << "\n number_of_rotations.size()=" << number_of_rotations.size() << std::endl; + } + std::stringstream output2; + for (size_t i=0; i >& paths, std::vector& calculation_times) + { + // 1. get the location of the results and open this file + const std::string configuration_folder_name = config.generateConfigurationFolderString() + "/"; + std::string log_filename = data_storage_path + configuration_folder_name + data.map_name_ + "_results.txt"; + std::cout << "Reading file " << log_filename << std::endl; + std::ifstream reading_file(log_filename.c_str(), std::ios::in); + + // 2. if the file could be opened, read out the given paths for all rooms + std::vector current_path; + if (reading_file.is_open()==true) + { + std::string line; + bool initial = true; + while(getline(reading_file, line)) + { + // check if the current line is empty --> shows a new room + if(line.empty()==true) + { + // save the previously found calculation times and paths + paths.push_back(current_path); + // reset temporary vectors + current_path.clear(); + // set the flag to a new room + initial = true; + // ignore the empty line + continue; + } + + // if the new line is the first after an empty line, it contains the calculation time + if(initial==true) + { + // if the time limit was exceeded or a bug appeared, save a -1 + if(line.find("exceeded")!=std::string::npos || line.find("bug")!=std::string::npos) + { + //std::cout << "bug or exceeded calculation time" << std::endl; + // set max calculation time +// if(config.exploration_algorithm_==5) // higher max time for flowNetwork explorator +// calculation_time = 10800; +// else +// calculation_time = 1800; + + // save a invalid pose, to show that this room has no coverage path + geometry_msgs::Pose2D false_pose; + false_pose.x = -1; + false_pose.y = -1; + current_path.push_back(false_pose); + } + else + { + const char* str = line.c_str(); + double calculation_time = 0.; + sscanf(str, "%*[^0-9]%lf", &calculation_time); + calculation_times.push_back(calculation_time); + //std::cout << "calculation time: " << calculation_time << "s" << std::endl; + } + initial = false; + } + // else read out x,y and theta and create a new Pose + else + { + double x=0., y=0., theta=0.; + std::istringstream iss(line); + int pos_counter = 0; + std::string buffer; + + // get saved output until comma or the end of the line is reached + while(getline(iss, buffer, ',')) + { + // remove brackets + buffer.erase(std::remove(buffer.begin(), buffer.end(), '['), buffer.end()); + buffer.erase(std::remove(buffer.begin(), buffer.end(), ']'), buffer.end()); + + // save value regarding the position of it + if(pos_counter==0) + x = atof(buffer.c_str()); + else if(pos_counter==1) + y = atof(buffer.c_str()); + else if(pos_counter==2) + theta = atof(buffer.c_str()); + + // increase position counter + ++pos_counter; + + } + //std::cout << " x: " << x << ", y: " << y << ", theta: " << theta << std::endl; + + // save the found pose + if(x>0 && y>0) + { + geometry_msgs::Pose2D current_pose; + current_pose.x = x; + current_pose.y = y; + current_pose.theta = theta; + current_path.push_back(current_pose); + } + } + } + } + else + { + ROS_WARN("Error on reading file '%s'", log_filename.c_str()); + return false; + } + reading_file.close(); + + std::cout << "Finished reading file " << log_filename << std::endl; + + return true; + } + + void computeFOVCenterAndGridSpacing(const ExplorationData& data, cv::Point2d& fov_circle_center_point_in_px, double& grid_spacing_in_pixel) + { + // find the middle-point distance of the given field of view + std::vector > fov_corners_meter(4); + for(int i = 0; i < 4; ++i) + fov_corners_meter[i] << data.fov_points_[i].x, data.fov_points_[i].y; + float fitting_circle_radius_in_meter=0; + Eigen::Matrix fitting_circle_center_point_in_meter; + computeFOVCenterAndRadius(fov_corners_meter, fitting_circle_radius_in_meter, fitting_circle_center_point_in_meter, 1000); + // convert the middle-point to pixel measures + fov_circle_center_point_in_px = cv::Point2d(fitting_circle_center_point_in_meter(0,0)/data.map_resolution_, fitting_circle_center_point_in_meter(1,0)/data.map_resolution_); + // determine the grid spacing + double grid_spacing_in_meter = 0.0; // is the square grid cell side length that fits into the circle with the robot's coverage radius or fov coverage radius + if(data.planning_mode_ == FIELD_OF_VIEW) // derive grid spacing from FOV + { + // get the edge length of the grid square that fits into the fitting_circle_radius + grid_spacing_in_meter = fitting_circle_radius_in_meter*std::sqrt(2); + } + else // if planning should be done for the footprint, read out the given coverage radius + { + grid_spacing_in_meter = data.coverage_radius_*std::sqrt(2); + } + grid_spacing_in_pixel = grid_spacing_in_meter/data.map_resolution_; // is the square grid cell side length that fits into the circle with the robot's coverage radius or fov coverage radius, multiply with sqrt(2) to receive the whole working width + std::cout << "grid size: " << grid_spacing_in_meter << " m (" << grid_spacing_in_pixel << " px)" << std::endl; + } + + // compute the direction of the gradient for each pixel and save the occurring gradients + cv::Mat computeGradientMap(const cv::Mat& map, bool return_angles=false) + { + // calculate the gradient x/y directions for each pixel in the map + cv::Mat gradient_x, gradient_y; + cv::Mat gradient_map; + cv::Sobel(map, gradient_x, CV_64F, 1, 0, 3, 1.0, 0.0, cv::BORDER_DEFAULT); + cv::Sobel(map, gradient_y, CV_64F, 0, 1, 3, 1.0, 0.0, cv::BORDER_DEFAULT); + + if (return_angles==true) + { + gradient_map = cv::Mat(map.rows, map.cols, CV_64F, cv::Scalar(0)); + // compute the direction of the gradient for each pixel and save the occurring gradients + for(size_t y=0; y(y,x); + int dy = gradient_y.at(y,x); + if(dy*dy+dx*dx!=0) + { + double current_gradient = std::atan2(dy, dx); + gradient_map.at(y,x) = current_gradient; + } + } + } + } + else + { + std::vector channels; + channels.push_back(gradient_x); + channels.push_back(gradient_y); + cv::merge(channels, gradient_map); + } + return gradient_map; + } + + // path map, path length, turns, crossings statistics + void statisticsPathLengthCrossingsTurns(const ExplorationData& data, const cv::Mat& map, cv::Mat& path_map, const cv::Point2d& fov_circle_center_point_in_px, + const std::vector >& paths, std::vector >& interpolated_paths, + std::vector& pathlengths_for_map, int& nonzero_paths, std::vector& number_of_crossings, + std::vector& rotation_values, std::vector& number_of_rotations) + { + AStarPlanner path_planner; + MapAccessibilityAnalysis map_accessibility_analysis; + cv::Mat inflated_map; + const int robot_radius_in_pixel = floor(data.robot_radius_ / data.map_resolution_); + map_accessibility_analysis.inflateMap(map, inflated_map, robot_radius_in_pixel); + interpolated_paths.resize(paths.size()); + + // draw paths + for(size_t room=0; room this should never happen."); + continue; + } + found_initial_pose = findAccessiblePose(inflated_map, current_pose_px, current_pose_px, data, fov_circle_center_point_in_px, false); + if (found_initial_pose == true) + break; + } + if(found_initial_pose == false) + { + // if any starting pose works, use first pose for further computations to achieve proper error handling + ROS_WARN("ExplorationEvaluation:evaluateCoveragePaths: No starting position is accessible."); + current_pose_px = paths[room][0]; + size_t initial_pose_index = 0; + } + geometry_msgs::Pose2D initial_pose_m; // in [m] + initial_pose_m.x = (current_pose_px.x*data.map_resolution_)+data.map_origin_.position.x; + initial_pose_m.y = (current_pose_px.y*data.map_resolution_)+data.map_origin_.position.y; + initial_pose_m.theta = current_pose_px.theta; + std::vector current_pose_path_meter; // in [m,m,rad] + current_pose_path_meter.push_back(initial_pose_m); + + // loop through trajectory points + for(std::vector::const_iterator pose_px=paths[room].begin()+initial_pose_index+1; pose_px!=paths[room].end(); ++pose_px) + { + // if a false pose has been saved, skip it + if(current_pose_px.x==-1 && current_pose_px.y==-1) + { + ROS_WARN("ExplorationEvaluation:evaluateCoveragePaths: current_pose_px.x==-1 && current_pose_px.y==-1 --> this should never happen."); + continue; + } + + // find an accessible next pose + geometry_msgs::Pose2D next_pose_px = *pose_px; + bool found_next = findAccessiblePose(inflated_map, current_pose_px, next_pose_px, data, fov_circle_center_point_in_px); + if(found_next==false) + { + std::cout << " skipping next_pose_px=(" << next_pose_px.x << "," << next_pose_px.y << ") inaccessible from current_pose_px=(" << current_pose_px.x << "," << current_pose_px.y << ")" << std::endl; + continue; // if no accessible position could be found, go to next possible path point + } + + // find pathlength and path between two consecutive poses + std::vector current_interpolated_path; // vector that stores the current path from one pose to another + const cv::Point current_pose_px_pt(current_pose_px.x, current_pose_px.y); + const cv::Point next_pose_px_pt(next_pose_px.x, next_pose_px.y); + // first query for direct current_pose_px_pt to next_pose_px_pt connection + double length_planner = generateDirectConnection(inflated_map, current_pose_px_pt, next_pose_px_pt, current_interpolated_path); + if (length_planner < 0.) // kind of a hack: if there is no accessible connection between two points, try to find a path on the original (not inflated) map, this path could possibly not be driven by the robot in reality + length_planner = generateDirectConnection(map, current_pose_px_pt, next_pose_px_pt, current_interpolated_path); + // use A* if there is no direct connection + if (length_planner < 0.) + length_planner = path_planner.planPath(inflated_map, current_pose_px_pt, next_pose_px_pt, 1.0, 0.0, data.map_resolution_, 0, ¤t_interpolated_path); + // kind of a hack: if there is no accessible connection between two points, try to find a path on the original (not inflated) map, this path could possibly not be driven by the robot in reality + if (current_interpolated_path.size()==0) + length_planner = path_planner.planPath(map, current_pose_px_pt, next_pose_px_pt, 1.0, 0.0, data.map_resolution_, 0, ¤t_interpolated_path); + current_pathlength += (length_planner>1e90 || length_planner<0 ? cv::norm(cv::Point(next_pose_px.x-current_pose_px.x, next_pose_px.y-current_pose_px.y)) : length_planner); + + // if there is any proper connection between the two points, just use the goal point as "path" + if (current_interpolated_path.size()<2) + { + current_interpolated_path.push_back(cv::Point(current_pose_px.x, current_pose_px.y)); + current_interpolated_path.push_back(cv::Point(next_pose_px.x, next_pose_px.y)); + } + + // transform the cv::Point path to geometry_msgs::Pose2D --> last point has, first point was already gone a defined angle + // also create output map to show path --> and check if one point has already been visited + bool has_crossing = false; + //cv::circle(path_map, cv::Point(next_pose_px.x, next_pose_px.y), 1, cv::Scalar(196), CV_FILLED); + for(std::vector::iterator point=current_interpolated_path.begin()+1; point!=current_interpolated_path.end(); ++point) + { + // check if point has been visited before and draw point into map + if(path_map.at(*point)==127) + has_crossing = true; + else + path_map.at(*point)=127; + + // transform to world coordinates + geometry_msgs::Pose2D current_pose; + current_pose.x = (point->x*data.map_resolution_)+data.map_origin_.position.x; + current_pose.y = (point->y*data.map_resolution_)+data.map_origin_.position.y; + current_pose.theta = 0; // the angles are computed afterwards with some smoothing interpolation + + // add the pose to the path + current_pose_path_meter.push_back(current_pose); + } + if (has_crossing == true) + ++current_number_of_crossings; + + // set robot_position to new one + current_pose_px = next_pose_px; + } + + // angles and turn: compute the angles along the pixel-wise path and add to the cumulative rotation + const int offset = 2; + for (size_t i=0; i1) + { + double angle_difference = current_pose_path_meter[i].theta - current_pose_path_meter[i-1].theta; + angle_difference = std::abs(angles::normalize_angle(angle_difference)); + current_rotation_abs += angle_difference; + if (angle_difference > 0.52) // only count substantial rotations > 30deg + ++current_number_of_rotations; + } + } + + // save number of crossings of the path + number_of_crossings.push_back(current_number_of_crossings); + + // save rotation values + rotation_values.push_back(current_rotation_abs); + number_of_rotations.push_back(current_number_of_rotations); + + // save the interpolated path between + interpolated_paths[room]=current_pose_path_meter; + + // transform the pixel length to meter + current_pathlength *= data.map_resolution_; + pathlengths_for_map.push_back(current_pathlength); + } + } + + void statisticsCoverageArea(const ExplorationData& data, const cv::Mat& map, const cv::Mat& path_map, cv::Mat& map_coverage, cv::Mat& map_path_coverage, + const std::vector >& paths, const std::vector >& interpolated_paths, + std::vector& room_areas, std::vector& area_covered_percentages, std::vector& numbers_of_coverages) + { + map_coverage = map.clone(); + for(size_t room=0; room convert path to cv format + std::vector path; + for (size_t i=0; i convert field of view to Eigen format + std::vector > field_of_view; + for(size_t i = 0; i < data.fov_points_.size(); ++i) + { + Eigen::Matrix current_vector; + current_vector << data.fov_points_[i].x, data.fov_points_[i].y; + field_of_view.push_back(current_vector); + } + // --> convert field of view origin to Eigen format + Eigen::Matrix fov_origin; + fov_origin << data.fov_origin_.x, data.fov_origin_.y; + // --> call coverage checker + CoverageCheckServer coverage_checker; + if (coverage_checker.checkCoverage(data.room_maps_[room], data.map_resolution_, cv::Point2d(data.map_origin_.position.x, data.map_origin_.position.y), + path, field_of_view, fov_origin, data.coverage_radius_, (data.planning_mode_==FOOTPRINT), true, coverage_map, number_of_coverage_image) == true) + { + for (int v=0; v(v,u)==127) + map_coverage.at(v,u)=208; + } + else + { + ROS_INFO("Error when calling the coverage check server."); + } + + // service interface - can be deleted +// // use the coverage check server to check which areas have been seen +// ipa_building_msgs::CheckCoverageRequest coverage_request; +// ipa_building_msgs::CheckCoverageResponse coverage_response; +// // fill request +// std::string coverage_service_name = "/room_exploration/coverage_check_server/coverage_check"; +// // cv::Mat eroded_room_map; +// // cv::erode(data.room_maps_[room], eroded_room_map, cv::Mat(), cv::Point(-1, -1), robot_radius_in_pixel); +// sensor_msgs::ImageConstPtr service_image; +// cv_bridge::CvImage cv_image; +// cv_image.encoding = "mono8"; +// cv_image.image = data.room_maps_[room]; //eroded_room_map; +// service_image = cv_image.toImageMsg(); +// coverage_request.map_resolution = data.map_resolution_; +// coverage_request.input_map = *service_image; +// coverage_request.map_origin = data.map_origin_; +// coverage_request.path = interpolated_paths[room]; +// coverage_request.field_of_view = data.fov_points_; +// coverage_request.field_of_view_origin = data.fov_origin_; +// coverage_request.coverage_radius = data.coverage_radius_; +// if (data.planning_mode_ == FOOTPRINT) +// coverage_request.check_for_footprint = true; +// else if (data.planning_mode_ == FIELD_OF_VIEW) +// coverage_request.check_for_footprint = false; +// coverage_request.check_number_of_coverages = true; +// // send request +// if(ros::service::call(coverage_service_name, coverage_request, coverage_response)==true) +// { +// cv_bridge::CvImagePtr cv_ptr_obj; +// cv_ptr_obj = cv_bridge::toCvCopy(coverage_response.coverage_map, sensor_msgs::image_encodings::MONO8); +// coverage_map = cv_ptr_obj->image; +// +// for (int v=0; v(v,u)==127) +// map_coverage.at(v,u)=208; +// +// cv_ptr_obj = cv_bridge::toCvCopy(coverage_response.number_of_coverage_image, sensor_msgs::image_encodings::TYPE_32SC1); +// number_of_coverages_image = cv_ptr_obj->image; +// } +// else +// { +// ROS_INFO("Error when calling the coverage check server."); +// } +// cv::imshow("seen", coverage_map); +// cv::waitKey(); + + // get the area of the whole room + const int white_room_pixels = cv::countNonZero(data.room_maps_[room]); + const double room_area = data.map_resolution_ * data.map_resolution_ * (double) white_room_pixels; + room_areas.push_back(room_area); + + // get the covered area of the room + cv::threshold(coverage_map, coverage_map, 150, 255, cv::THRESH_BINARY); // covered area drawn in as 127 --> find still white pixels + const int not_covered_pixels = cv::countNonZero(coverage_map); + const double not_covered_area = data.map_resolution_ * data.map_resolution_ * (double) not_covered_pixels; + + // get and save the percentage of coverage + double coverage_percentage = (room_area-not_covered_area)/room_area; + area_covered_percentages.push_back(coverage_percentage); + + // check how often pixels have been covered + double average_coverage_number = 0.0, coverage_number_deviation = 0.0; + for(size_t u=0; u(u,v)!=0) + numbers_of_coverages.push_back(number_of_coverage_image.at(u,v)); + } + // create the map with the drawn in path and coverage areas + map_path_coverage = map.clone(); + for (int v=0; v(v,u)==255) + map_path_coverage.at(v,u) = 176; // leftover uncovered areas + if (path_map.at(v,u)==127 || path_map.at(v,u)==196) + map_path_coverage.at(v,u) = path_map.at(v,u); + } + } + } + + void statisticsParallelism(const ExplorationData& data, const cv::Mat& map, const cv::Mat& path_map, + const std::vector >& paths, const std::vector >& interpolated_paths, + const double grid_spacing_in_pixel, + std::vector& wall_angle_score_means, std::vector& trajectory_angle_score_means, std::vector& revisit_time_means) + { + // compute the direction of the gradient for each pixel and save the occurring gradients + cv::Mat gradient_map = computeGradientMap(map); + + const double trajectory_parallelism_check_range = 2.0*grid_spacing_in_pixel; //1.0/data.map_resolution_; // valid check-radius when checking for the parallelism to another part of the trajectory, [pixels] + for (size_t room=0; room current_wall_angle_scores, current_trajectory_angle_scores; // values in [0,1], high values are good + std::vector current_revisit_times; // values in [0,1], low values are good + for (std::vector::const_iterator pose=paths[room].begin(); pose!=paths[room].end()-1; ++pose) + { + double dx = (pose+1)->x - pose->x; + double dy = (pose+1)->y - pose->y; + double norm = std::sqrt(dy*dy + dx*dx); + if(norm==0) + continue; // skip if the point and its successor are the same + dx = dx/norm; + dy = dy/norm; + + // go in the directions of both normals and find the nearest wall + bool hit_wall = false, hit_trajectory = false, exceeded_trajectory_parallelism_check_range = false; + bool n1_ok = true, n2_ok = true; + cv::Point2f n1(pose->x, pose->y), n2(pose->x, pose->y); + cv::Point wall_pixel, trajectory_pixel; + do + { + // update normals + n1.x -= dy; + n1.y += dx; + n2.x += dy; + n2.y -= dx; + + // test for coordinates inside image + if (n1.x<0.f || n1.y<0.f || (int)n1.x >= map.cols || (int)n1.y >= map.rows) + n1_ok = false; + if (n2.x<0.f || n2.y<0.f || (int)n2.x >= map.cols || (int)n2.y >= map.rows) + n2_ok = false; + + // test if a wall/obstacle has been hit + if (hit_wall==false && n1_ok==true && map.at(n1)==0) + { + hit_wall = true; + n1_ok = false; // do not further search with direction that has found a wall + wall_pixel = n1; + } + else if (hit_wall==false && n2_ok==true && map.at(n2)==0) + { + hit_wall = true; + n2_ok = false; // do not further search with direction that has found a wall + wall_pixel = n2; + } + + // only search for the parallelism to another trajectory if the range hasn't been exceeded yet + if (exceeded_trajectory_parallelism_check_range==false && hit_trajectory==false) + { + // test if another trajectory part has been hit, if the check-radius is still satisfied + const double dist1 = cv::norm(n1-cv::Point2f(pose->x, pose->y)); + const double dist2 = cv::norm(n2-cv::Point2f(pose->x, pose->y)); + + if (n1_ok==true && dist1<=trajectory_parallelism_check_range && path_map.at(n1)==127) + { + hit_trajectory = true; + trajectory_pixel = n1; + } + else if (n2_ok==true && dist2<=trajectory_parallelism_check_range && path_map.at(n2)==127) + { + hit_trajectory = true; + trajectory_pixel = n2; + } + + // if both distances exceed the valid check range, mark as finished + if (dist1>trajectory_parallelism_check_range && dist2>trajectory_parallelism_check_range) + exceeded_trajectory_parallelism_check_range = true; + } + +// cv::Mat test_map = map.clone(); +// cv::circle(test_map, cv::Point(pose->x, pose->y), 2, cv::Scalar(127), CV_FILLED); +// cv::circle(test_map, cv::Point((pose+1)->x, (pose+1)->y), 2, cv::Scalar(127), CV_FILLED); +// cv::circle(test_map, n1, 2, cv::Scalar(127), CV_FILLED); +// cv::circle(test_map, n2, 2, cv::Scalar(127), CV_FILLED); +// cv::imshow("normals", test_map); +// cv::waitKey(); + } while ((n1_ok || n2_ok) && ((hit_wall==false) || (hit_trajectory==false && exceeded_trajectory_parallelism_check_range==false))); + + // if a wall/obstacle was found, determine the gradient at this position and compare it to the direction of the path + if (hit_wall==true) + { + cv::Vec2d gradient = gradient_map.at(wall_pixel); + cv::Point2f normal_vector(-gradient.val[1], gradient.val[0]); + const double normal_norm = cv::norm(normal_vector); + normal_vector *= (float)(normal_norm!=0. ? 1./normal_norm : 1.); + const double delta_theta = std::acos(normal_vector.x*dx + normal_vector.y*dy); + const double delta_theta_score = std::abs(0.5*PI-delta_theta)*(1./(0.5*PI));// parallel if delta_theta close to 0 or PI + current_wall_angle_scores.push_back(delta_theta_score); + } + + // if another trajectory part could be found, determine the parallelism to it + if (hit_trajectory==true) + { + // find the trajectory point in the interpolated path + cv::Point2f trajectory_point_m((trajectory_pixel.x*data.map_resolution_)+data.map_origin_.position.x, (trajectory_pixel.y*data.map_resolution_)+data.map_origin_.position.y); // transform in world coordinates + int pose_index = pose-paths[room].begin(); + int neighbor_index = -1; + for (std::vector::const_iterator neighbor=interpolated_paths[room].begin(); neighbor!=interpolated_paths[room].end(); ++neighbor) + if (cv::norm(trajectory_point_m-cv::Point2f(neighbor->x,neighbor->y)) < 0.5*data.map_resolution_) + neighbor_index = neighbor-interpolated_paths[room].begin(); + if (neighbor_index == -1) + ROS_WARN("ExplorationEvaluation:evaluateCoveragePaths: parallelism check to trajectory, neighbor_index==-1 --> did not find the neighbor for trajectory point (%f,%f)m.", trajectory_point_m.x, trajectory_point_m.y); +// std::cout << "index: " << pose_index << ", n: " << neighbor_index << std::endl; + + // save the found index difference, i.e. the difference in percentage of path completion between current node and neighboring path point + current_revisit_times.push_back(std::abs((double)pose_index/(double)paths[room].size() - (double)neighbor_index/(double)interpolated_paths[room].size())); + + // calculate the trajectory direction at the neighbor to get the difference + const double n_dx = cos(interpolated_paths[room][neighbor_index].theta); + const double n_dy = sin(interpolated_paths[room][neighbor_index].theta); + const double delta_theta = std::acos(n_dx*dx + n_dy*dy); // acos delivers in range [0,Pi] + const double delta_theta_score = std::abs(0.5*PI-delta_theta)*(1./(0.5*PI));// parallel if delta_theta close to 0 or PI + current_trajectory_angle_scores.push_back(delta_theta_score); + } + } + // save found values + wall_angle_score_means.push_back(std::accumulate(current_wall_angle_scores.begin(), current_wall_angle_scores.end(), 0.0) / std::max(1.0, (double)current_wall_angle_scores.size())); + trajectory_angle_score_means.push_back(std::accumulate(current_trajectory_angle_scores.begin(), current_trajectory_angle_scores.end(), 0.0) / std::max(1.0, (double)current_trajectory_angle_scores.size())); + revisit_time_means.push_back(std::accumulate(current_revisit_times.begin(), current_revisit_times.end(), 0.0) / std::max(1.0, (double)current_revisit_times.size())); + } + } + + bool findAccessiblePose(const cv::Mat& inflated_map, const geometry_msgs::Pose2D& current_pose_px, geometry_msgs::Pose2D& target_pose_px, const ExplorationData& data, + const cv::Point2d fov_circle_center_point_in_px, const bool approach_path_accessibility_check=true) + { + MapAccessibilityAnalysis map_accessibility_analysis; + bool found_next = false; + if(inflated_map.at(target_pose_px.y, target_pose_px.x)!=0) // if calculated target pose is accessible, use it as next pose + { + found_next = true; + } + else // use the map accessibility server to find another accessible target pose + { + const MapAccessibilityAnalysis::Pose target_pose_px_copy(target_pose_px.x, target_pose_px.y, target_pose_px.theta); + if (data.planning_mode_ == FOOTPRINT || (fov_circle_center_point_in_px.x==0 && fov_circle_center_point_in_px.y==0)) // if the fov center is at the robot center it behaves like footprint planning + { + const int max_radius = std::max(1, cvRound(1.55*data.coverage_radius_/data.map_resolution_)); // in [pixel] + // check circles with growing radius around the desired point until a dislocation of data.coverage_radius_ would be exceeded + for (double radius=1; radius<=max_radius && found_next==false; ++radius) + { + // check perimeter for accessible poses + std::vector accessible_poses_on_perimeter; + map_accessibility_analysis.checkPerimeter(accessible_poses_on_perimeter, target_pose_px_copy, radius, PI/32., inflated_map, + approach_path_accessibility_check, cv::Point(current_pose_px.x, current_pose_px.y)); + + // find the closest accessible point on this perimeter + double min_distance_sqr = std::numeric_limits::max(); + for(std::vector::iterator new_pose=accessible_poses_on_perimeter.begin(); new_pose!=accessible_poses_on_perimeter.end(); ++new_pose) + { + const double dist_sqr = (new_pose->x-current_pose_px.x)*(new_pose->x-current_pose_px.x) + (new_pose->y-current_pose_px.y)*(new_pose->y-current_pose_px.y); + if (dist_sqr < min_distance_sqr) + { + target_pose_px.x = cvRound(new_pose->x); // the approach_path_accessibility_check uses (u,v) coordinates obtained with cvRound, so this has to be used + target_pose_px.y = cvRound(new_pose->y); // here for rounding as well, otherwise the robot can slip into the inaccessible space through rounding + //target_pose_px.theta = target_pose_px.theta; // use the orientation of the original pose + min_distance_sqr = dist_sqr; + found_next = true; + } + } + } + } + else if (data.planning_mode_ == FIELD_OF_VIEW) + { + // get the desired FoV-center position + MapAccessibilityAnalysis::Pose fov_center_px; // in [px,px,rad] + fov_center_px.x = (target_pose_px_copy.x + std::cos(target_pose_px_copy.orientation)*fov_circle_center_point_in_px.x - std::sin(target_pose_px_copy.orientation)*fov_circle_center_point_in_px.y); + //fov_center_px.x = (fov_center_px.x-data.map_origin_.position.x) / data.map_resolution_; + fov_center_px.y = (target_pose_px_copy.y + std::sin(target_pose_px_copy.orientation)*fov_circle_center_point_in_px.x + std::cos(target_pose_px_copy.orientation)*fov_circle_center_point_in_px.y); + //fov_center_px.y = (fov_center_px.y-data.map_origin_.position.y) / data.map_resolution_; + fov_center_px.orientation = target_pose_px_copy.orientation; + + std::cout << "target_pose_px_copy: " << target_pose_px_copy.x << ", " << target_pose_px_copy.y << ", " << target_pose_px_copy.orientation << std::endl; + std::cout << "fov_center_px: " << fov_center_px.x << ", " << fov_center_px.y << ", " << fov_center_px.orientation << std::endl; + + const double optimal_distance_to_fov_center = cv::norm(fov_circle_center_point_in_px); + for (double factor_add=0.; factor_add<0.45 && found_next==false; factor_add*=-1.) + { + const double factor = 1.0 + factor_add; + if (factor_add<=0.) + factor_add -= 0.1; + + // check perimeter for accessible poses + std::vector accessible_poses_on_perimeter; + map_accessibility_analysis.checkPerimeter(accessible_poses_on_perimeter, fov_center_px, factor*optimal_distance_to_fov_center, + PI/32., inflated_map, approach_path_accessibility_check, cv::Point(current_pose_px.x, current_pose_px.y)); + + // find the closest accessible point on this perimeter + double min_distance_sqr = std::numeric_limits::max(); + for(std::vector::iterator new_pose=accessible_poses_on_perimeter.begin(); new_pose!=accessible_poses_on_perimeter.end(); ++new_pose) + { + const double dist_sqr = (new_pose->x-target_pose_px_copy.x)*(new_pose->x-target_pose_px_copy.x) + (new_pose->y-target_pose_px_copy.y)*(new_pose->y-target_pose_px_copy.y); + if (dist_sqr < min_distance_sqr) + { + target_pose_px.x = cvRound(new_pose->x); // the approach_path_accessibility_check uses (u,v) coordinates obtained with cvRound, so this has to be used + target_pose_px.y = cvRound(new_pose->y); // here for rounding as well, otherwise the robot can slip into the inaccessible space through rounding + target_pose_px.theta = new_pose->orientation; + min_distance_sqr = dist_sqr; + found_next = true; + } + } + } + } + } + return found_next; + } + + // return path length if a direct connection is possible, otherwise -1 + double generateDirectConnection(const cv::Mat& map, const cv::Point& start, const cv::Point& goal, std::vector& current_interpolated_path) + { + if (start==goal) + return 0.; + + // try with direct connecting line + cv::LineIterator it(map, start, goal); + bool direct_connection = true; + for (int k=0; k1 ? std::sqrt(2.) : 1); + current_interpolated_path[k] = it2.pos(); + } + return length; + } + + return -1.; + } + + // accumulate all statistics into one file + void writeCumulativeStatistics(const std::vector& evaluation_data, const std::vector& configs, + const std::string& data_storage_path) + { + for(std::vector::const_iterator config=configs.begin(); config!=configs.end(); ++config) + { + const std::string configuration_folder_name = config->generateConfigurationFolderString() + "/"; + std::stringstream cumulative_statistics; + for (size_t i=0; i0) + cumulative_statistics << line << std::endl; + } + else + ROS_ERROR("Could not open file '%s' for reading cumulative data.", filename.c_str()); + file.close(); + } + const std::string filename_out = data_storage_path + configuration_folder_name + "all_evaluations_per_room.txt"; + std::ofstream file_out(filename_out.c_str(), std::ofstream::out); + if (file_out.is_open()) + file_out << cumulative_statistics.str(); + else + ROS_ERROR("Could not open file '%s' for writing cumulative data.", filename_out.c_str()); + file_out.close(); + } + } +}; + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "room_exploration_evaluation"); + ros::NodeHandle nh; + + const std::string test_map_path = ros::package::getPath("ipa_room_segmentation") + "/common/files/test_maps/"; + const std::string data_storage_path = "room_exploration_evaluation/"; + + // prepare relevant floor map data + std::vector< std::string > map_names; + map_names.push_back("lab_ipa"); + map_names.push_back("lab_c_scan"); + map_names.push_back("Freiburg52_scan"); +// map_names.push_back("Freiburg79_scan"); +// map_names.push_back("lab_b_scan"); +// map_names.push_back("lab_intel"); +// map_names.push_back("Freiburg101_scan"); +// map_names.push_back("lab_d_scan"); +// map_names.push_back("lab_f_scan"); +// map_names.push_back("lab_a_scan"); +// map_names.push_back("NLB"); +// map_names.push_back("office_a"); +// map_names.push_back("office_b"); +// map_names.push_back("office_c"); +// map_names.push_back("office_d"); +// map_names.push_back("office_e"); +// map_names.push_back("office_f"); +// map_names.push_back("office_g"); +// map_names.push_back("office_h"); +// map_names.push_back("office_i"); +// map_names.push_back("lab_ipa_furnitures"); +// map_names.push_back("lab_c_scan_furnitures"); +// map_names.push_back("Freiburg52_scan_furnitures"); +// map_names.push_back("Freiburg79_scan_furnitures"); +// map_names.push_back("lab_b_scan_furnitures"); +// map_names.push_back("lab_intel_furnitures"); +// map_names.push_back("Freiburg101_scan_furnitures"); +// map_names.push_back("lab_d_scan_furnitures"); +// map_names.push_back("lab_f_scan_furnitures"); +// map_names.push_back("lab_a_scan_furnitures"); +// map_names.push_back("NLB_furnitures"); +// map_names.push_back("office_a_furnitures"); +// map_names.push_back("office_b_furnitures"); +// map_names.push_back("office_c_furnitures"); +// map_names.push_back("office_d_furnitures"); +// map_names.push_back("office_e_furnitures"); +// map_names.push_back("office_f_furnitures"); +// map_names.push_back("office_g_furnitures"); +// map_names.push_back("office_h_furnitures"); +// map_names.push_back("office_i_furnitures"); + + std::vector exploration_algorithms; +// exploration_algorithms.push_back(1); // grid point exploration + exploration_algorithms.push_back(2); // boustrophedon exploration +// exploration_algorithms.push_back(3); // neural network exploration +// exploration_algorithms.push_back(4); // convex SPP exploration +// exploration_algorithms.push_back(5); // flow network exploration +// exploration_algorithms.push_back(6); // energy functional exploration +// exploration_algorithms.push_back(7); // voronoi exploration + + // coordinate system definition: x points in forward direction of robot and camera, y points to the left side of the robot and z points upwards. x and y span the ground plane. + // measures in [m] + std::vector fov_points(4); +// fov_points[0].x = 0.04035; // this field of view represents the off-center iMop floor wiping device +// fov_points[0].y = -0.136; +// fov_points[1].x = 0.04035; +// fov_points[1].y = 0.364; +// fov_points[2].x = 0.54035; // todo: this definition is mirrored on x (y-coordinates are inverted) to work properly --> check why, make it work the intuitive way +// fov_points[2].y = 0.364; +// fov_points[3].x = 0.54035; +// fov_points[3].y = -0.136; +// int planning_mode = 2; // viewpoint planning +// fov_points[0].x = 0.15; // this field of view fits a Asus Xtion sensor mounted at 0.63m height (camera center) pointing downwards to the ground in a respective angle +// fov_points[0].y = 0.35; +// fov_points[1].x = 0.15; +// fov_points[1].y = -0.35; +// fov_points[2].x = 1.15; +// fov_points[2].y = -0.65; +// fov_points[3].x = 1.15; +// fov_points[3].y = 0.65; +// int planning_mode = 2; // viewpoint planning + fov_points[0].x = -0.3; // this is the working area of a vacuum cleaner with 60 cm width + fov_points[0].y = 0.3; + fov_points[1].x = -0.3; + fov_points[1].y = -0.3; + fov_points[2].x = 0.3; + fov_points[2].y = -0.3; + fov_points[3].x = 0.3; + fov_points[3].y = 0.3; + int planning_mode = 2; // footprint planning + geometry_msgs::Point32 fov_origin; + fov_origin.x = 0.; + fov_origin.y = 0.; + + const double robot_radius = 0.3; // [m] + const double coverage_radius = 0.3; // [m] + const double robot_speed = 0.3; // [m/s] + const double robot_rotation_speed = 0.52; // [rad/s] + const float map_resolution = 0.05; // [m/cell] + + ExplorationEvaluation ev(nh, test_map_path, map_names, map_resolution, data_storage_path, robot_radius, coverage_radius, fov_points, fov_origin, + planning_mode, exploration_algorithms, robot_speed, robot_rotation_speed, true, true); + ros::shutdown(); + + //exit + return 0; +} diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/src/sub.cpp b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/src/sub.cpp new file mode 100644 index 0000000..bca1b05 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/src/sub.cpp @@ -0,0 +1,45 @@ +#include "ros/ros.h" +#include "ipa_building_msgs/dis_info.h" +#include "ipa_building_msgs/dis_info_array.h" + + +void doMsg(const ipa_building_msgs::dis_info_array::ConstPtr& ceju_msg){ + + for (const auto& obstacle_info : ceju_msg->dis) + { + + float distance = obstacle_info.distance; + float width = obstacle_info.width; + float height = obstacle_info.height; + ROS_INFO("distance: %.2f ", distance); + + // // 执行避障逻辑 + // if (distance < obstacle_distance_threshold) + // { + // ROS_INFO("Obstacle detected at distance: %.2f meters. Avoiding obstacle.", distance); + + // // 在这里执行避障动作，例如停止机器人 + // geometry_msgs::Twist cmd_vel; + // cmd_vel.linear.x = 0.0; + // cmd_vel.angular.z = 0.0; + // cmd_vel_pub.publish(cmd_vel); + + // // 这里可以添加更复杂的避障逻辑，例如避开障碍物或调整方向 + // } + // else + // { + // ROS_INFO("No obstacle detected at distance: %.2f meters. Continuing.", distance); + // } + } +} +int main(int argc, char *argv[]) +{ + setlocale(LC_ALL,""); + ros::init(argc,argv,"sub_dis"); + ros::NodeHandle nh; + ros::Subscriber sub = nh.subscribe("ceju_info",10,doMsg); + + ros::spin();//循环读取接收的数据，并调用回调函数处理 + + return 0; +} \ No newline at end of file diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/test/room_exploration_client.launch b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/test/room_exploration_client.launch new file mode 100644 index 0000000..6cd31f8 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/test/room_exploration_client.launch @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/test/room_exploration_evaluation.launch b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/test/room_exploration_evaluation.launch new file mode 100644 index 0000000..cd5b643 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/test/room_exploration_evaluation.launch @@ -0,0 +1,7 @@ + + + + + + + diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/worlds/sim.world b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/worlds/sim.world new file mode 100644 index 0000000..85f8712 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/worlds/sim.world @@ -0,0 +1,1578 @@ + + + + 1 + 0 0 10 0 -0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + 0 + 0 + 0 + + + + 1 + + + + + 0 0 1 + 100 100 + + + + + + 100 + 50 + + + + + + + + + + + 10 + + + 0 + + + 0 0 1 + 100 100 + + + + + + + 0 + 0 + 0 + + + 0 0 -9.8 + 6e-06 2.3e-05 -4.2e-05 + + + 0.001 + 1 + 1000 + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + 1 + + + + EARTH_WGS84 + 0 + 0 + 0 + 0 + + + 1 + + 0 0 0.01 0 -0 0 + + + + 0 0 1 + 30 20 + + + + + + 100 + 50 + + + + + + + + + + + 10 + + + 0 11 0.001 0 -0 0 + 0 + + + 0 0 1 + 34 2 + + + + + + + + 0 -11 0.001 0 -0 0 + 0 + + + 0 0 1 + 34 2 + + + + + + + + 16 0 0.001 0 -0 0 + 0 + + + 0 0 1 + 2 20 + + + + + + + + -16 0 0.001 0 -0 0 + 0 + + + 0 0 1 + 2 20 + + + + + + + + 0 + + + 0 0 1 + 30 20 + + + + + + + 0 + 0 + 0 + + -0.121937 -0.503301 0 0 -0 0 + + + 1 + + + 0 0 1 0 -0 0 + + + 1.5 0.8 0.03 + + + + + + 0.6 + 0.6 + + + + + + + + + + + 10 + + + 0 0 1 0 -0 0 + + + 1.5 0.8 0.03 + + + + + + + + 0.68 0.38 0.5 0 -0 0 + + + 0.02 + 1 + + + 10 + + + + + + + + + + + + + + + 0.68 0.38 0.5 0 -0 0 + + + 0.02 + 1 + + + + + + + + 0.68 -0.38 0.5 0 -0 0 + + + 0.02 + 1 + + + 10 + + + + + + + + + + + + + + + 0.68 -0.38 0.5 0 -0 0 + + + 0.02 + 1 + + + + + + + + -0.68 -0.38 0.5 0 -0 0 + + + 0.02 + 1 + + + 10 + + + + + + + + + + + + + + + -0.68 -0.38 0.5 0 -0 0 + + + 0.02 + 1 + + + + + + + + -0.68 0.38 0.5 0 -0 0 + + + 0.02 + 1 + + + 10 + + + + + + + + + + + + + + + -0.68 0.38 0.5 0 -0 0 + + + 0.02 + 1 + + + + + + + 0 + 0 + 0 + + 2.22926 -4.67258 0 0 -0 0 + + + 0.282311 -0.242367 0 0 -0 0 + + + + + 32 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 32 0.15 2.5 + + + + + 0.960784 0.47451 0 1 + + + 0 + + + 1e-06 -11.675 0 0 -0 3.14159 + 0 + 0 + 0 + + + + + + 23.5 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 23.5 0.15 2.5 + + + + + 0.960784 0.47451 0 1 + + + 0 + + + -15.925 -0 0 0 -0 1.5708 + 0 + 0 + 0 + + + + + + 32 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 32 0.15 2.5 + + + + + 0.960784 0.47451 0 1 + + + 0 + + + 1e-06 11.675 0 0 -0 0 + 0 + 0 + 0 + + + + + + 23.5 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 23.5 0.15 2.5 + + + + + 0.960784 0.47451 0 1 + + + 0 + + + 15.925 -0 0 0 -0 -1.5708 + 0 + 0 + 0 + + 1 + + + 1 + + + + + model://pine_tree/meshes/pine_tree.dae + + + 10 + + + + + + + + + + + + + + + + + model://pine_tree/meshes/pine_tree.dae + + Branch + + + + + + + + + + + model://pine_tree/meshes/pine_tree.dae + + Bark + + + + + + + + 0 + 0 + 0 + + 11.8949 7.1898 0 0 -0 0 + + + 1 + + + + + model://pine_tree/meshes/pine_tree.dae + + + 10 + + + + + + + + + + + + + + + + + model://pine_tree/meshes/pine_tree.dae + + Branch + + + + + + + + + + + model://pine_tree/meshes/pine_tree.dae + + Bark + + + + + + + + 0 + 0 + 0 + + 9.134 8.49007 0 0 -0 0 + + + 1 + + + + + model://pine_tree/meshes/pine_tree.dae + + + 10 + + + + + + + + + + + + + + + + + model://pine_tree/meshes/pine_tree.dae + + Branch + + + + + + + + + + + model://pine_tree/meshes/pine_tree.dae + + Bark + + + + + + + + 0 + 0 + 0 + + 6.26329 6.89864 0 0 -0 0 + + + 1 + + + + + model://pine_tree/meshes/pine_tree.dae + + + 10 + + + + + + + + + + + + + + + + + model://pine_tree/meshes/pine_tree.dae + + Branch + + + + + + + + + + + model://pine_tree/meshes/pine_tree.dae + + Bark + + + + + + + + 0 + 0 + 0 + + 10.6895 3.78098 0 0 -0 0 + + + 1 + + + + + model://pine_tree/meshes/pine_tree.dae + + + 10 + + + + + + + + + + + + + + + + + model://pine_tree/meshes/pine_tree.dae + + Branch + + + + + + + + + + + model://pine_tree/meshes/pine_tree.dae + + Bark + + + + + + + + 0 + 0 + 0 + + 2.64018 7.98293 0 0 -0 0 + + + 1 + + + + + model://pine_tree/meshes/pine_tree.dae + + + 10 + + + + + + + + + + + + + + + + + model://pine_tree/meshes/pine_tree.dae + + Branch + + + + + + + + + + + model://pine_tree/meshes/pine_tree.dae + + Bark + + + + + + + + 0 + 0 + 0 + + 6.55847 2.56696 0 0 -0 0 + + + 1 + + + + + model://pine_tree/meshes/pine_tree.dae + + + 10 + + + + + + + + + + + + + + + + + model://pine_tree/meshes/pine_tree.dae + + Branch + + + + + + + + + + + model://pine_tree/meshes/pine_tree.dae + + Bark + + + + + + + + 0 + 0 + 0 + + 9.60411 -0.376221 0 0 -0 0 + + + 1 + + + + + model://pine_tree/meshes/pine_tree.dae + + + 10 + + + + + + + + + + + + + + + + + model://pine_tree/meshes/pine_tree.dae + + Branch + + + + + + + + + + + model://pine_tree/meshes/pine_tree.dae + + Bark + + + + + + + + 0 + 0 + 0 + + -3.21533 6.79176 0 0 -0 0 + + + 1 + + + + + model://pine_tree/meshes/pine_tree.dae + + + 10 + + + + + + + + + + + + + + + + + model://pine_tree/meshes/pine_tree.dae + + Branch + + + + + + + + + + + model://pine_tree/meshes/pine_tree.dae + + Bark + + + + + + + + 0 + 0 + 0 + + 0.441623 3.90862 0 0 -0 0 + + + + + + + 1.5 1.5 1.5 + model://dumpster/meshes/dumpster.dae + + + 10 + + + + + + + + + + + + + + + + + 1.5 1.5 1.5 + model://dumpster/meshes/dumpster.dae + + + + + + + 0 + + 0 0 0 0 -0 0 + + 1 + 0 + 0 + 1 + 0 + 1 + + 1 + + 0 + 0 + + 7.93379 -5.44785 0 0 -0 0 + + + 1 + + + 0 0 0.755 0 -0 0 + + + 0.913 0.913 0.04 + + + 10 + + + + + + + + + + + + + + + 0 0 0.37 0 -0 0 + + + 0.042 0.042 0.74 + + + 10 + + + + + + + + + + + + + + + 0 0 0.02 0 -0 0 + + + 0.56 0.56 0.04 + + + 10 + + + + + + + + + + + + + + + + + model://cafe_table/meshes/cafe_table.dae + + + + 0 + 0 + 0 + + 5.00619 -7.7671 0 0 -0 0 + + + 1 + + + + + model://fire_hydrant/meshes/fire_hydrant.dae + + + 10 + + + + + + + + + + + + + + + + + model://fire_hydrant/meshes/fire_hydrant.dae + + + + 0 + 0 + 0 + + 13.6242 -4.18511 0 0 -0 0 + + + 1 + + + + + model://house_2/meshes/house_2.dae + 1.5 1.5 1.5 + + + 10 + + + + + + + + + + + + + + + + + model://house_2/meshes/house_2.dae + 1.5 1.5 1.5 + + + + + + model://house_1/materials/textures/House_1_Normal.png + + + + 0 + 0 + 0 + + -9.17281 -5.59908 0 0 -0 0 + + + 397 424000000 + 766 142269694 + 1679845835 169511959 + 397424 + + 11.9119 -9.12195 0.011376 2e-06 -0 1e-06 + 1 1 1 + + 11.9119 -9.12195 0.011376 2e-06 -0 1e-06 + 0 0 0 0 -0 0 + 0 0 -9.8 0 -0 0 + 0 0 -9.8 0 -0 0 + + + + -9.1045 -2.14717 0 0 0 -1.55967 + 1 1 1 + + -9.1045 -2.14717 0 0 0 -1.55967 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0.282311 -0.242367 0 0 -0 0 + 1 1 1 + + 0.282312 -11.9174 0 0 -0 3.14159 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -15.6427 -0.242367 0 0 -0 1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 0.282312 11.4326 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 16.2073 -0.242367 0 0 0 -1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 5.00619 -7.7671 0 0 -0 0 + 1 1 1 + + 5.00619 -7.7671 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 13.6242 -4.18511 0 0 -0 0 + 1 1 1 + + 13.6242 -4.18511 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0 0 0 0 -0 0 + 1 1 1 + + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 11.8949 7.1898 0 0 -0 0 + 1 1 1 + + 11.8949 7.1898 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 9.134 8.49007 0 0 -0 0 + 1 1 1 + + 9.134 8.49007 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 6.26329 6.89864 0 0 -0 0 + 1 1 1 + + 6.26329 6.89864 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.6895 3.78098 0 0 -0 0 + 1 1 1 + + 10.6895 3.78098 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 2.64018 7.98293 0 0 -0 0 + 1 1 1 + + 2.64018 7.98293 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 6.55847 2.56696 0 0 -0 0 + 1 1 1 + + 6.55847 2.56696 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 9.60411 -0.376221 0 0 -0 0 + 1 1 1 + + 9.60411 -0.376221 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -3.21533 6.79176 0 0 -0 0 + 1 1 1 + + -3.21533 6.79176 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0.441623 3.90862 0 0 -0 0 + 1 1 1 + + 0.441623 3.90862 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -0.121937 -0.503301 0 0 -0 0 + 1 1 1 + + -0.121937 -0.503301 0.01 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 2.22926 -4.67258 0 0 -0 0 + 1 1 1 + + 2.22926 -4.67258 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0 0 10 0 -0 0 + + + + + 17.9734 -7.30956 65.8679 0 1.24021 2.19755 + orbit + perspective + + + + diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/CMakeLists.txt b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/CMakeLists.txt new file mode 100644 index 0000000..a453b9c --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/CMakeLists.txt @@ -0,0 +1,183 @@ +cmake_minimum_required(VERSION 2.8.3) +project(ipa_room_segmentation) + +# build with c++11 +add_compile_options(-std=c++11) + +set(catkin_RUN_PACKAGES + actionlib + cv_bridge + ipa_building_msgs + libdlib + nav_msgs + opengm + roscpp + roslib + sensor_msgs +) + +set(catkin_BUILD_PACKAGES + ${catkin_RUN_PACKAGES} + dynamic_reconfigure +) + +## Find catkin macros and libraries +find_package(catkin REQUIRED COMPONENTS + ${catkin_BUILD_PACKAGES} +) + +generate_dynamic_reconfigure_options( + cfg/RoomSegmentation.cfg +) + +find_package(OpenCV REQUIRED) +find_package(Boost REQUIRED COMPONENTS system thread filesystem) + +find_package(OpenMP) +if(OPENMP_FOUND) + message(STATUS "OPENMP FOUND") + set(OpenMP_FLAGS ${OpenMP_CXX_FLAGS}) #${OpenMP_C_FLAGS} + set(OpenMP_LIBS gomp) +endif() + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if you package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +# important: common/include needs to be here if you have such a directory +catkin_package( +INCLUDE_DIRS + common/include + ros/include +LIBRARIES + ${PROJECT_NAME} +CATKIN_DEPENDS + ${catkin_RUN_PACKAGES} +DEPENDS + OpenCV + Boost + OpenMP +) + + +########### +## Build ## +########### +## Specify additional locations of header files +## Your package locations should be listed before other locations +# include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) +# important: common/include also needs to be here if you have it +include_directories( + common/include + ros/include + ${catkin_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} +) + +# library of the created algorithms +add_library(${PROJECT_NAME} STATIC + common/src/distance_segmentation.cpp + common/src/morphological_segmentation.cpp + common/src/abstract_voronoi_segmentation.cpp + common/src/voronoi_segmentation.cpp + common/src/adaboost_classifier.cpp + common/src/wavefront_region_growing.cpp + common/src/contains.cpp common/src/features.cpp + common/src/raycasting.cpp + common/src/meanshift2d.cpp + common/src/room_class.cpp + common/src/voronoi_random_field_segmentation.cpp + common/src/clique_class.cpp + common/src/cv_boost_loader.cpp + common/src/voronoi_random_field_features.cpp +) +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} + ${OpenCV_LIBRARIES} + ${Boost_LIBRARIES} +) +add_dependencies(${PROJECT_NAME} + ${catkin_EXPORTED_TARGETS} + ${${PROJECT_NAME}_EXPORTED_TARGETS} +) + +### segmentation action server: see room_segmentation_action_server_params.yaml to change the used method +add_executable(room_segmentation_server + ros/src/room_segmentation_server.cpp + ) +target_compile_options(room_segmentation_server PRIVATE ${OpenMP_FLAGS}) +add_dependencies(room_segmentation_server + ${catkin_EXPORTED_TARGETS} + ${PROJECT_NAME} + ${${PROJECT_NAME}_EXPORTED_TARGETS} +) +target_link_libraries(room_segmentation_server + ${catkin_LIBRARIES} + ${Boost_LIBRARIES} + ${OpenMP_LIBS} + ${OpenCV_LIBRARIES} + ${PROJECT_NAME}) + +### client for testing purpose +add_executable(room_segmentation_client ros/src/room_segmentation_client.cpp) +target_link_libraries(room_segmentation_client + ${catkin_LIBRARIES} + ${OpenCV_LIBRARIES}) +add_dependencies(room_segmentation_client + ${catkin_EXPORTED_TARGETS} + ${${PROJECT_NAME}_EXPORTED_TARGETS} +) + +### evaluation function for numeric properties and comparison to a given ground truth segmentation +add_executable(evaluation + ros/src/evaluation_numeric_properties.cpp + common/src/evaluation_segmentation.cpp) +target_link_libraries(evaluation + ${catkin_LIBRARIES} + ${OpenCV_LIBRARIES}) +add_dependencies(evaluation + ${catkin_EXPORTED_TARGETS} + ${${PROJECT_NAME}_EXPORTED_TARGETS} +) + + +############# +## Install ## +############# +## Mark executables and/or libraries for installation +install(TARGETS ${PROJECT_NAME} room_segmentation_server room_segmentation_client evaluation + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +##uncomment this if you have a common-directory with header-files +install(DIRECTORY common/include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) + +##uncomment this if you have header-files in your project +install(DIRECTORY ros/include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) + +##uncomment this if you use launch and yaml files +install(DIRECTORY ros/launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/ros + PATTERN ".svn" EXCLUDE +) + +#install(DIRECTORY common/files +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/common +# PATTERN ".svn" EXCLUDE +#) diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/cfg/RoomSegmentation.cfg b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/cfg/RoomSegmentation.cfg new file mode 100644 index 0000000..b66e224 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/cfg/RoomSegmentation.cfg @@ -0,0 +1,59 @@ +#!/usr/bin/env python +PACKAGE = "ipa_room_segmentation" + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + +method_enum = gen.enum([ gen.const("MorphologicalSegmentation", int_t, 1, "Use the morphological segmentation algorithm."), + gen.const("DistanceSegmentation", int_t, 2, "Use the distance segmentation algorithm."), + gen.const("VoronoiSegmentation", int_t, 3, "Use the Voronoi segmentation algorithm."), + gen.const("SemanticSegmentation", int_t, 4, "Use the semantic/feature-based segmentation algorithm."), + gen.const("RandomFieldSegmentation", int_t, 5, "Use the voronoi-random-field segmentation algorithm."), + gen.const("PassThroughSegmentation", int_t, 99, "Use the pass through segmentation algorithm.")], + "Segmentation algorithm") +gen.add("room_segmentation_algorithm", int_t, 0, "Segmentation method", 3, 1, 99, edit_method=method_enum) + +gen.add("display_segmented_map", bool_t, 0, "Show the resulting segmented map directly", False) +gen.add("publish_segmented_map", bool_t, 0, "Publish the resulting segmented map as grid map", False) + +# room area factor-> Set the limitation of area of the room -------> in [m^2] +#morphological segmentation: 47.0 - 0.8 (means the room area after eroding/shrinking s.t. too small/big contours are not treated as rooms) +gen.add("room_area_factor_upper_limit_morphological", double_t, 0, "Upper room limit for morphological segmentation", 47.0, 0.0) +gen.add("room_area_factor_lower_limit_morphological", double_t, 0, "Lower room limit for morphological segmentation", 0.8, 0.0) + +# distance segmentation: 163.0 - 0.35 (means the room area after decreasing the boundary for the binary filter s.t. too small/big contours are not treated as rooms) +gen.add("room_area_factor_upper_limit_distance", double_t, 0, "Upper room limit for distance segmentation", 163.0, 0.0) +gen.add("room_area_factor_lower_limit_distance", double_t, 0, "Lower room limit for distance segmentation", 0.35, 0.0) + +# Voronoi segmentation: 120.0 - 1.53 (means the max/min area that an area separated by critical lines is allowed to have) +gen.add("room_area_factor_upper_limit_voronoi", double_t, 0, "Upper room limit for Voronoi segmentation", 1000000.0, 0.0) +gen.add("room_area_factor_lower_limit_voronoi", double_t, 0, "Lower room limit for Voronoi segmentation", 0.1, 0.0) + +# Semantic Segmentation: 23.0 - 1.0 (means the max/min area a connected classified region is allowed to have) +gen.add("room_area_factor_upper_limit_semantic", double_t, 0, "Upper room limit for semantic/feature-based segmentation", 1000000.0, 0.0) # if you choose this value small (i.e 23.0) then too big hallway contours are randomly separated into smaller regions using a watershed algorithm, which can look bad +gen.add("room_area_factor_lower_limit_semantic", double_t, 0, "Lower room limit for semantic/feature-based segmentation", 1.0, 0.0) + +# Voronoi random field segmentation: 1000000.0 - 1.53 (means the max/min area a connected classified region is allowed to have) +gen.add("room_area_upper_limit_voronoi_random", double_t, 0, "Upper room limit for Voronoi-random-field segmentation", 1000000.0, 0.0) +gen.add("room_area_lower_limit_voronoi_random", double_t, 0, "Lower room limit for Voronoi-random-field segmentation", 1.53, 0.0) + +# Passthrough segmentation: 1000000.0 - 1.53 (means the max/min area a connected classified region is allowed to have) +gen.add("room_area_upper_limit_passthrough", double_t, 0, "Upper room limit for Voronoi-random-field segmentation", 1000000.0, 0.0) +gen.add("room_area_lower_limit_passthrough", double_t, 0, "Lower room limit for Voronoi-random-field segmentation", 1.0, 0.0) + +# parameters for the voronoi segmentation that specify the neighborhood for critical Point extraction and the distance between critical Points. +gen.add("voronoi_neighborhood_index", int_t, 0, "Size of neighborhood on graph for Voronoi segmentation, larger value sets a larger neighborhood for searching critical points", 280, 0) +gen.add("max_iterations", int_t, 0, "Max number of Iterations for search of neighbors, also used for the vrf segmentation", 150, 0) +gen.add("min_critical_point_distance_factor", double_t, 0, "Minimal distance factor between two critical points before one of it gets eliminated", 0.5, 0.0) +gen.add("max_area_for_merging", double_t, 0, "Maximal area [m^2] of a room that should be merged with its surrounding rooms, also used for the voronoi random field segmentation", 12.5, 0.0) + +# parameters for the voronoi random field segmentation that specify the size of the neighborhood generated on the Voronoi graph, the minimal +# size this neighborhood can have, how far base nodes for each node on the graph need to be apart and how many iterations the inference +# max. should do +gen.add("voronoi_random_field_epsilon_for_neighborhood", int_t, 0, "Larger value sets larger neighborhood, concentrated in a node of the conditional random field", 5, 0) +gen.add("min_neighborhood_size", int_t, 0, "Min. size of the above mentioned neighborhood", 4, 0) +gen.add("min_voronoi_random_field_node_distance", double_t, 0, "Min distance the base nodes for each crf node need to be apart", 7.0, 0.0) +gen.add("max_voronoi_random_field_inference_iterations", int_t, 0, "Max number of iterations the inference algorithm should do", 9000, 0) + +exit(gen.generate(PACKAGE, "room_segmentation_server", "RoomSegmentation")) \ No newline at end of file diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/classifier_models/semantic_hallway_SVM.xml b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/classifier_models/semantic_hallway_SVM.xml new file mode 100644 index 0000000..0cf657b --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/classifier_models/semantic_hallway_SVM.xml @@ -0,0 +1,26 @@ + + + + ONE_CLASS + LINEAR + 5.9999999999999998e-01 + 2.2204460492503131e-16 + 100 + 23 + 23 + 1 + 1 + + <_> + 2.83671656e+05 7.78090750e+06 2.06722500e+04 4.50947109e+04 + 5.79335150e+06 5300250. 19302716. 5.71475812e+05 1567863. + 1.29049977e+05 7.88329650e+06 6738720. 509354208. 4.15090725e+06 + 115733672. 4.59749316e+03 6.03456750e+06 4511485. 28201040. + 7.34039550e+06 4.96453250e+06 4.85331211e+04 3.32311797e+04 + + <_> + 1 + 4.7882008646924377e+11 + + 1. + diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/classifier_models/semantic_hallway_boost.xml b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/classifier_models/semantic_hallway_boost.xml new file mode 100644 index 0000000..538572f --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/classifier_models/semantic_hallway_boost.xml @@ -0,0 +1,32240 @@ + + + + DiscreteAdaboost + 3 + 350 + 1. + 1 + 23 + 23 + 23 + 0 + + 0 + 10 + 2 + 10 + 0 + + 1 + 2 +
d
+ + 1. 1. + + 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 + + 1 + 1 +
i
+ + 2 + + 1 + 2 +
i
+ + -1 1 + + <_> + -1 + + <_> + 0 + 882720 + -2.5000247678744505e+00 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 8.5281175374984741e-01 + 4.9016128540039062e+01 + <_> + 1 + 602347 + -2.5000247678744505e+00 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 6.4516609907150269e-01 + 5.1874628067016602e+00 + <_> + 2 + 574973 + -2.5000247678744505e+00 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 27374 + 2.5000247678744505e+00 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 280373 + 2.5000247678744505e+00 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 2.7897748351097107e-01 + 9.7361612319946289e-01 + <_> + 2 + 211281 + 2.5000247678744505e+00 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 69092 + -2.5000247678744505e+00 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.0561948056756905e+00 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 7.0789510011672974e-01 + 3.3711798191070557e+00 + <_> + 1 + 659370 + -1.0561948056756905e+00 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 4.9510058760643005e-01 + 1.1866049468517303e-01 + <_> + 2 + 16249 + 1.0561948056756905e+00 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 643121 + -1.0561948056756905e+00 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 223350 + 1.0561948056756905e+00 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 2.4686211347579956e-01 + 3.7584754943847656e+01 + <_> + 2 + 41021 + -1.0561948056756905e+00 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 182329 + 1.0561948056756905e+00 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.0835391258083200e+00 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 6.8653523921966553e-01 + 4.9311373382806778e-02 + <_> + 1 + 597709 + 1.0835391258083200e+00 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.2165627479553223e-01 + 2.7373552322387695e+01 + <_> + 2 + 202717 + -1.0835391258083200e+00 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 394992 + 1.0835391258083200e+00 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 285011 + -1.0835391258083200e+00 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 2.2550687193870544e-01 + 5.7749996185302734e+00 + <_> + 2 + 271207 + -1.0835391258083200e+00 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 13804 + 1.0835391258083200e+00 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -7.5052965116975667e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 6.2174135446548462e-01 + 5.8754093170166016e+01 + <_> + 1 + 451918 + 7.5052965116975667e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 3.3912891149520874e-01 + 1.3885684967041016e+01 + <_> + 2 + 407085 + 7.5052965116975667e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 44833 + -7.5052965116975667e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 430802 + -7.5052965116975667e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 3.4016519784927368e-01 + 7.3667076110839844e+01 + <_> + 2 + 376333 + -7.5052965116975667e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 54469 + 7.5052965116975667e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -7.0255471055884633e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 6.1124324798583984e-01 + 1.7473866045475006e-01 + <_> + 1 + 383200 + -7.0255471055884633e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 3.3070948719978333e-01 + 8.8179000854492188e+01 + <_> + 2 + 381680 + -7.0255471055884633e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 1520 + 7.0255471055884633e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 499520 + 7.0255471055884633e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 3.3804446458816528e-01 + 3.9916137695312500e+01 + <_> + 2 + 317052 + -7.0255471055884633e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 182468 + 7.0255471055884633e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 5.5757275271874029e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.8539623022079468e-01 + 6.6094345092773438e+01 + <_> + 1 + 563853 + 5.5757275271874029e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 4.2348077893257141e-01 + 5.1872992515563965e+00 + <_> + 2 + 472915 + 5.5757275271874029e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 90938 + -5.5757275271874029e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 318867 + -5.5757275271874029e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 2.1240997314453125e-01 + 5.8216087341308594e+01 + <_> + 2 + 16908 + 5.5757275271874029e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 301959 + -5.5757275271874029e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -7.3718438427964017e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.9774291515350342e-01 + 3.9099571228027344e+01 + <_> + 1 + 484375 + -7.3718438427964017e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 2.9912680387496948e-01 + 2.3193679809570312e+01 + <_> + 2 + 20399 + 7.3718438427964017e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 463976 + -7.3718438427964017e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 398345 + 7.3718438427964017e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 3.7725305557250977e-01 + 5.5976442992687225e-02 + <_> + 2 + 309373 + 7.3718438427964017e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 88972 + -7.3718438427964017e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -5.7711237363036894e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 5.9095835685729980e-01 + 7.8611063957214355e+00 + <_> + 1 + 167129 + 5.7711237363036894e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 1.5990914404392242e-01 + 1.9507030487060547e+01 + <_> + 2 + 102470 + -5.7711237363036894e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 64659 + 5.7711237363036894e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 715591 + -5.7711237363036894e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 4.8049354553222656e-01 + 3.6017631530761719e+01 + <_> + 2 + 58754 + 5.7711237363036894e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 656837 + -5.7711237363036894e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 6.6379357710435383e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 6.1264258623123169e-01 + 4.6442675781250000e+03 + <_> + 1 + 688948 + 6.6379357710435383e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 4.7940942645072937e-01 + 9.5261204242706299e-01 + <_> + 2 + 59542 + -6.6379357710435383e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 629406 + 6.6379357710435383e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 193772 + -6.6379357710435383e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 1.8070261180400848e-01 + 9.6825778484344482e-01 + <_> + 2 + 50309 + 6.6379357710435383e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 143463 + -6.6379357710435383e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -4.9806891380818058e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 5.6908506155014038e-01 + 9.7688269615173340e-01 + <_> + 1 + 453264 + 4.9806891380818058e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 4.0892013907432556e-01 + 1.4821989746093750e+03 + <_> + 2 + 230631 + 4.9806891380818058e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 222633 + -4.9806891380818058e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 429456 + -4.9806891380818058e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 2.1308527886867523e-01 + 3.2999923706054688e+01 + <_> + 2 + 419671 + -4.9806891380818058e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 9785 + 4.9806891380818058e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -5.0646726701474654e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 5.8226627111434937e-01 + 1.7250000000000000e+02 + <_> + 1 + 506329 + -5.0646726701474654e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 3.6317276954650879e-01 + 2.2534292221069336e+01 + <_> + 2 + 41922 + 5.0646726701474654e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 464407 + -5.0646726701474654e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 376391 + 5.0646726701474654e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 2.6080518960952759e-01 + 5.8332994580268860e-02 + <_> + 2 + 279776 + 5.0646726701474654e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 96615 + -5.0646726701474654e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 4.0498477250255488e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 5.5375719070434570e-01 + 9.7888779640197754e-01 + <_> + 1 + 528807 + 4.0498477250255488e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 4.6711561083793640e-01 + 1.4249422073364258e+01 + <_> + 2 + 404206 + 4.0498477250255488e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 124601 + -4.0498477250255488e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 353913 + -4.0498477250255488e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 1.3276910781860352e-01 + 3.6847179412841797e+01 + <_> + 2 + 73290 + 4.0498477250255488e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 280623 + -4.0498477250255488e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -3.8490637503640324e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 5.6859707832336426e-01 + 8.7510818481445312e+01 + <_> + 1 + 657030 + -3.8490637503640324e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 3.8780328631401062e-01 + 5.8185371398925781e+01 + <_> + 2 + 574652 + -3.8490637503640324e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 82378 + 3.8490637503640324e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 225690 + 3.8490637503640324e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 2.0725263655185699e-01 + 1.3191735744476318e+00 + <_> + 2 + 95330 + -3.8490637503640324e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 130360 + 3.8490637503640324e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 4.3547130396010858e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.5804246664047241e-01 + 5.0668243408203125e+01 + <_> + 1 + 302622 + 4.3547130396010858e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 2.4361383914947510e-01 + 1.9793853759765625e+01 + <_> + 2 + 109160 + -4.3547130396010858e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 193462 + 4.3547130396010858e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 580098 + -4.3547130396010858e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 3.6356556415557861e-01 + 7.0139106750488281e+01 + <_> + 2 + 501553 + -4.3547130396010858e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 78545 + 4.3547130396010858e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 3.9508589159378837e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 5.6446391344070435e-01 + 1.5353633463382721e-01 + <_> + 1 + 121061 + -3.9508589159378837e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 1.5201655030250549e-01 + 6.5945739746093750e+00 + <_> + 2 + 90682 + -3.9508589159378837e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 30379 + 3.9508589159378837e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 761659 + 3.9508589159378837e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 4.4548988342285156e-01 + 1.1396166992187500e+02 + <_> + 2 + 232129 + -3.9508589159378837e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 529530 + 3.9508589159378837e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -4.2963459221342304e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 5.5056160688400269e-01 + 9.7362148761749268e-01 + <_> + 1 + 363049 + -4.2963459221342304e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 3.3263361454010010e-01 + 4.9014308929443359e+01 + <_> + 2 + 151703 + 4.2963459221342304e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 211346 + -4.2963459221342304e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 519671 + 4.2963459221342304e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 2.7315279841423035e-01 + 1.2722101807594299e-01 + <_> + 2 + 45197 + -4.2963459221342304e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 474474 + 4.2963459221342304e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -3.9238680475218229e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 5.7303798198699951e-01 + 8.9154830932617188e+01 + <_> + 1 + 666441 + -3.9238680475218229e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 3.9737287163734436e-01 + 4.9769868850708008e+00 + <_> + 2 + 97277 + 3.9238680475218229e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 569164 + -3.9238680475218229e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 216279 + 3.9238680475218229e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 1.9948427379131317e-01 + 3.3712952136993408e+00 + <_> + 2 + 186542 + 3.9238680475218229e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 29737 + -3.9238680475218229e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -3.8445973133888361e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.5484569072723389e-01 + 3.5730915069580078e+01 + <_> + 1 + 434384 + -3.8445973133888361e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 1.9449812173843384e-01 + 9.8893880844116211e-01 + <_> + 2 + 430281 + -3.8445973133888361e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 4103 + 3.8445973133888361e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 448336 + 3.8445973133888361e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 4.0045017004013062e-01 + 7.3852386474609375e+01 + <_> + 2 + 259008 + 3.8445973133888361e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 189328 + -3.8445973133888361e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -3.1898207249877786e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.5290323495864868e-01 + 5.8774444580078125e+01 + <_> + 1 + 718234 + -3.1898207249877786e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 4.1853344440460205e-01 + 3.3898910522460938e+01 + <_> + 2 + 220036 + 3.1898207249877786e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 498198 + -3.1898207249877786e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 164486 + 3.1898207249877786e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 1.6054271161556244e-01 + 6.0868024826049805e-02 + <_> + 2 + 147144 + 3.1898207249877786e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 17342 + -3.1898207249877786e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -2.7638361411288459e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 5.4218429327011108e-01 + 2.3582912981510162e-02 + <_> + 1 + 121797 + -2.7638361411288459e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 9.1429881751537323e-02 + 3.6944442987442017e-01 + <_> + 2 + 2185 + 2.7638361411288459e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 119612 + -2.7638361411288459e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 760923 + 2.7638361411288459e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 4.7722950577735901e-01 + 9.7989165782928467e-01 + <_> + 2 + 467048 + 2.7638361411288459e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 293875 + -2.7638361411288459e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -3.8936288401883468e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 5.5028510093688965e-01 + 3.3781882375478745e-02 + <_> + 1 + 299408 + 3.8936288401883468e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 2.3490308225154877e-01 + 5.1872992515563965e+00 + <_> + 2 + 247438 + 3.8936288401883468e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 51970 + -3.8936288401883468e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 583312 + -3.8936288401883468e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 3.6122623085975647e-01 + 1.4901842117309570e+01 + <_> + 2 + 542480 + -3.8936288401883468e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 40832 + 3.8936288401883468e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -3.7859194457551471e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 5.4297500848770142e-01 + 1.0696094512939453e+01 + <_> + 1 + 519303 + 3.7859194457551471e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 2.8631174564361572e-01 + 1.5550000000000000e+02 + <_> + 2 + 292353 + -3.7859194457551471e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 226950 + 3.7859194457551471e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 363417 + -3.7859194457551471e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 3.0722171068191528e-01 + 2.4954649806022644e-01 + <_> + 2 + 328706 + -3.7859194457551471e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 34711 + 3.7859194457551471e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 3.8806981941850882e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 5.6464052200317383e-01 + 2.6087051630020142e-01 + <_> + 1 + 514877 + 3.8806981941850882e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 4.0939474105834961e-01 + 1.6938510894775391e+01 + <_> + 2 + 472171 + 3.8806981941850882e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 42706 + -3.8806981941850882e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 367843 + -3.8806981941850882e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 1.8642322719097137e-01 + 4.5934448242187500e+01 + <_> + 2 + 89925 + 3.8806981941850882e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 277918 + -3.8806981941850882e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -2.5601663827535109e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 5.4288333654403687e-01 + 2.0450000000000000e+02 + <_> + 1 + 724586 + -2.5601663827535109e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 4.4759979844093323e-01 + 7.9199924468994141e+00 + <_> + 2 + 541486 + -2.5601663827535109e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 183100 + 2.5601663827535109e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 158134 + 2.5601663827535109e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 1.1605703085660934e-01 + 9.6780145168304443e-01 + <_> + 2 + 63784 + 2.5601663827535109e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 94350 + -2.5601663827535109e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 2.4067785600455416e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.3569650650024414e-01 + 2.1868953704833984e+01 + <_> + 1 + 169192 + -2.4067785600455416e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 6.4721196889877319e-02 + 3.8999923706054688e+01 + <_> + 2 + 168384 + -2.4067785600455416e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 808 + 2.4067785600455416e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 713528 + 2.4067785600455416e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 4.9515947699546814e-01 + 1.8298877716064453e+01 + <_> + 2 + 655994 + 2.4067785600455416e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 57534 + -2.4067785600455416e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -3.4200244768870741e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 5.5117404460906982e-01 + 4.1146377563476562e+01 + <_> + 1 + 189582 + 3.4200244768870741e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>13 + 1.4584004878997803e-01 + 1.1058124542236328e+01 + <_> + 2 + 65761 + -3.4200244768870741e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 123821 + 3.4200244768870741e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 693138 + -3.4200244768870741e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 4.3883681297302246e-01 + 3.2507812500000000e+01 + <_> + 2 + 680385 + -3.4200244768870741e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 12753 + 3.4200244768870741e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 2.8520754813896321e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 5.4125010967254639e-01 + 3.2449951171875000e+03 + <_> + 1 + 574444 + 2.8520754813896321e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 3.4096714854240417e-01 + 9.5860791206359863e-01 + <_> + 2 + 60757 + -2.8520754813896321e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 513687 + 2.8520754813896321e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 308276 + -2.8520754813896321e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 2.2985531389713287e-01 + 9.7581613063812256e-01 + <_> + 2 + 178810 + 2.8520754813896321e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 129466 + -2.8520754813896321e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -2.2982056488671518e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 5.4501110315322876e-01 + 2.0850000000000000e+02 + <_> + 1 + 745585 + -2.2982056488671518e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>1 + 4.5085111260414124e-01 + 2.6444540023803711e+01 + <_> + 2 + 12686 + 2.2982056488671518e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 732899 + -2.2982056488671518e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 137135 + 2.2982056488671518e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 1.0635246336460114e-01 + 4.6487405896186829e-02 + <_> + 2 + 41844 + 2.2982056488671518e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 95291 + -2.2982056488671518e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 2.4580184093415508e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 5.3684824705123901e-01 + 1.0054676532745361e+00 + <_> + 1 + 87233 + -2.4580184093415508e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 8.9667581021785736e-02 + 3.1854756176471710e-02 + <_> + 2 + 35334 + 2.4580184093415508e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 51899 + -2.4580184093415508e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 795487 + 2.4580184093415508e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 4.7147533297538757e-01 + 3.3532081604003906e+01 + <_> + 2 + 346815 + -2.4580184093415508e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 448672 + 2.4580184093415508e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -2.8135021871546456e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.4266953468322754e-01 + 1.2662216796875000e+03 + <_> + 1 + 528422 + 2.8135021871546456e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 2.9836127161979675e-01 + 5.1877889633178711e+00 + <_> + 2 + 465528 + 2.8135021871546456e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 62894 + -2.8135021871546456e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 354298 + -2.8135021871546456e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 2.7151593565940857e-01 + 1.0398562011718750e+03 + <_> + 2 + 24471 + 2.8135021871546456e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 329827 + -2.8135021871546456e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -2.3879996448776486e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 5.3198719024658203e-01 + 2.0673083496093750e+02 + <_> + 1 + 563897 + -2.3879996448776486e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 2.9127633571624756e-01 + 2.1284889221191406e+01 + <_> + 2 + 493911 + -2.3879996448776486e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 69986 + 2.3879996448776486e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 318823 + 2.3879996448776486e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 2.6814156770706177e-01 + 1.2748368084430695e-01 + <_> + 2 + 27121 + -2.3879996448776486e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 291702 + 2.3879996448776486e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -3.2072387369203748e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.4793244600296021e-01 + 6.3041107177734375e+01 + <_> + 1 + 518900 + 3.2072387369203748e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 3.4303501248359680e-01 + 1.2224289894104004e+01 + <_> + 2 + 448812 + 3.2072387369203748e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 70088 + -3.2072387369203748e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 363820 + -3.2072387369203748e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 2.3646563291549683e-01 + 9.6474623680114746e-01 + <_> + 2 + 69743 + 3.2072387369203748e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 294077 + -3.2072387369203748e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -2.5388568804675321e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 5.3625667095184326e-01 + 7.5241134643554688e+01 + <_> + 1 + 550226 + -2.5388568804675321e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 3.1047654151916504e-01 + 1.8750000000000000e+02 + <_> + 2 + 494944 + -2.5388568804675321e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 55282 + 2.5388568804675321e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 332494 + 2.5388568804675321e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>8 + 2.5265613198280334e-01 + 3.5000002384185791e+00 + <_> + 2 + 247473 + 2.5388568804675321e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 85021 + -2.5388568804675321e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -2.8432079365336993e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.3741228580474854e-01 + 4.7506725311279297e+01 + <_> + 1 + 254983 + 2.8432079365336993e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 1.9746458530426025e-01 + 1.2050000000000000e+02 + <_> + 2 + 108797 + -2.8432079365336993e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 146186 + 2.8432079365336993e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 627737 + -2.8432079365336993e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 3.7314060330390930e-01 + 9.7361600399017334e-01 + <_> + 2 + 273881 + -2.8432079365336993e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 353856 + 2.8432079365336993e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 2.7807586135889539e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 5.3048604726791382e-01 + 3.9837718009948730e-02 + <_> + 1 + 396766 + 2.7807586135889539e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 2.9367303848266602e-01 + 1.5735460281372070e+01 + <_> + 2 + 333714 + 2.7807586135889539e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 63052 + -2.7807586135889539e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 485954 + -2.7807586135889539e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 2.7540138363838196e-01 + 1.2582890510559082e+01 + <_> + 2 + 384791 + -2.7807586135889539e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 101163 + 2.7807586135889539e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -2.5257428818013500e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.3876495361328125e-01 + 6.5172973632812500e+01 + <_> + 1 + 774061 + -2.5257428818013500e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 4.4251585006713867e-01 + 5.6907775878906250e+02 + <_> + 2 + 155718 + 2.5257428818013500e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 618343 + -2.5257428818013500e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 108659 + 2.5257428818013500e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 1.2029416114091873e-01 + 2.8483349084854126e-01 + <_> + 2 + 96999 + 2.5257428818013500e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 11660 + -2.5257428818013500e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -2.6888200258282402e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.3376609086990356e-01 + 3.2374977111816406e+01 + <_> + 1 + 367112 + -2.6888200258282402e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 1.4384716749191284e-01 + 3.1992652416229248e+00 + <_> + 2 + 331941 + -2.6888200258282402e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 35171 + 2.6888200258282402e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 515608 + 2.6888200258282402e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 4.2297124862670898e-01 + 2.8283744812011719e+01 + <_> + 2 + 355009 + 2.6888200258282402e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 160599 + -2.6888200258282402e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -2.6557449847167958e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 5.4306346178054810e-01 + 4.9310810863971710e-02 + <_> + 1 + 597702 + -2.6557449847167958e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 4.3830740451812744e-01 + 2.2350000000000000e+02 + <_> + 2 + 571125 + -2.6557449847167958e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 26577 + 2.6557449847167958e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 285018 + 2.6557449847167958e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 1.2769871950149536e-01 + 1.8904611468315125e-01 + <_> + 2 + 119410 + -2.6557449847167958e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 165608 + 2.6557449847167958e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 2.1957614635785158e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.3768950700759888e-01 + 3.3249731445312500e+02 + <_> + 1 + 41096 + -2.1957614635785158e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 3.0376721173524857e-02 + 1.2922230362892151e-01 + <_> + 2 + 1151 + 2.1957614635785158e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 39945 + -2.1957614635785158e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 841624 + 2.1957614635785158e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 5.2429783344268799e-01 + 5.7744584977626801e-02 + <_> + 2 + 691440 + 2.1957614635785158e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 150184 + -2.1957614635785158e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.5621912673964305e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>13 + 5.2971059083938599e-01 + 1.1778937530517578e+02 + <_> + 1 + 849067 + -1.5621912673964305e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 5.0207209587097168e-01 + 3.9004119873046875e+01 + <_> + 2 + 839501 + -1.5621912673964305e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 9566 + 1.5621912673964305e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 33653 + 1.5621912673964305e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 3.6903481930494308e-02 + 2.6576604003906250e+03 + <_> + 2 + 16706 + 1.5621912673964305e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 16947 + -1.5621912673964305e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 2.5984527176661293e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 5.2983206510543823e-01 + 3.3699297904968262e+00 + <_> + 1 + 659234 + 2.5984527176661293e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 3.4077429771423340e-01 + 2.2761417388916016e+01 + <_> + 2 + 168907 + -2.5984527176661293e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 490327 + 2.5984527176661293e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 223486 + -2.5984527176661293e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 2.2382394969463348e-01 + 4.9548980712890625e+01 + <_> + 2 + 75712 + -2.5984527176661293e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 147774 + 2.5984527176661293e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -2.8589414297244886e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.5083858966827393e-01 + 4.5184902191162109e+01 + <_> + 1 + 225027 + 2.8589414297244886e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 1.7789599299430847e-01 + 5.1924037933349609e+00 + <_> + 2 + 179559 + 2.8589414297244886e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 45468 + -2.8589414297244886e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 657693 + -2.8589414297244886e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 3.9309465885162354e-01 + 4.0533802032470703e+01 + <_> + 2 + 37224 + 2.8589414297244886e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 620469 + -2.8589414297244886e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 2.5890222762878878e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 5.3204536437988281e-01 + 3.0226737260818481e-01 + <_> + 1 + 635135 + 2.5890222762878878e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 4.6253535151481628e-01 + 1.1937193572521210e-01 + <_> + 2 + 41365 + -2.5890222762878878e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 593770 + 2.5890222762878878e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 247585 + -2.5890222762878878e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 1.0183106362819672e-01 + 9.5969329833984375e+01 + <_> + 2 + 214121 + -2.5890222762878878e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 33464 + 2.5890222762878878e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.6087276350002813e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 5.2966541051864624e-01 + 1.2195181640625000e+04 + <_> + 1 + 865649 + -1.6087276350002813e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 5.1756471395492554e-01 + 4.5140047073364258e+00 + <_> + 2 + 85899 + 1.6087276350002813e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 779750 + -1.6087276350002813e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 17071 + 1.6087276350002813e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 2.2566951811313629e-02 + 2.2305555343627930e+00 + <_> + 2 + 3322 + -1.6087276350002813e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 13749 + 1.6087276350002813e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 2.5162349612340584e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.3231668472290039e-01 + 4.1311653137207031e+01 + <_> + 1 + 511470 + -2.5162349612340584e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 2.2732007503509521e-01 + 1.8950000000000000e+02 + <_> + 2 + 439893 + -2.5162349612340584e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 71577 + 2.5162349612340584e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 371250 + 2.5162349612340584e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>4 + 3.3525598049163818e-01 + 7.6380165100097656e+01 + <_> + 2 + 237881 + 2.5162349612340584e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 133369 + -2.5162349612340584e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.9680235984700412e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 5.3176385164260864e-01 + 1.2164132118225098e+01 + <_> + 1 + 767723 + 1.9680235984700412e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 4.7934114933013916e-01 + 2.9931622743606567e-01 + <_> + 2 + 726628 + 1.9680235984700412e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 41095 + -1.9680235984700412e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 114997 + -1.9680235984700412e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>10 + 6.9701254367828369e-02 + 5.9036102294921875e+00 + <_> + 2 + 8029 + 1.9680235984700412e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 106968 + -1.9680235984700412e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -2.1085934556631833e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.3102642297744751e-01 + 6.8379714965820312e+01 + <_> + 1 + 791915 + -2.1085934556631833e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 4.5863428711891174e-01 + 2.7736222743988037e-01 + <_> + 2 + 488246 + -2.1085934556631833e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 303669 + 2.1085934556631833e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 90805 + 2.1085934556631833e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 9.3886099755764008e-02 + 1.5468671917915344e-01 + <_> + 2 + 14078 + -2.1085934556631833e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 76727 + 2.1085934556631833e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -2.3927106695682640e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.3679347038269043e-01 + 6.3816757202148438e+01 + <_> + 1 + 530787 + 2.3927106695682640e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 3.4056138992309570e-01 + 2.4999579787254333e-01 + <_> + 2 + 423022 + 2.3927106695682640e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 107765 + -2.3927106695682640e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 351933 + -2.3927106695682640e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 2.1897262334823608e-01 + 3.2547447204589844e+01 + <_> + 2 + 344551 + -2.3927106695682640e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 7382 + 2.3927106695682640e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -2.4655053899517229e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 5.3666996955871582e-01 + 1.6050000000000000e+02 + <_> + 1 + 399660 + -2.4655053899517229e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 2.9678973555564880e-01 + 2.6728283691406250e+03 + <_> + 2 + 245275 + 2.4655053899517229e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 154385 + -2.4655053899517229e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 483060 + 2.4655053899517229e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 2.6453757286071777e-01 + 2.2190344333648682e-01 + <_> + 2 + 193758 + 2.4655053899517229e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 289302 + -2.4655053899517229e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -2.5498001638289297e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 5.4181605577468872e-01 + 1.6050000000000000e+02 + <_> + 1 + 399660 + -2.5498001638289297e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 3.0098134279251099e-01 + 4.9605865478515625e+01 + <_> + 2 + 337015 + -2.5498001638289297e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 62645 + 2.5498001638289297e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 483060 + 2.5498001638289297e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 2.6242053508758545e-01 + 1.3052967071533203e+01 + <_> + 2 + 417934 + 2.5498001638289297e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 65126 + -2.5498001638289297e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -2.7456508200215562e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.4151570796966553e-01 + 4.7608787536621094e+01 + <_> + 1 + 256299 + 2.7456508200215562e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 1.9770053029060364e-01 + 1.6914348602294922e+01 + <_> + 2 + 79379 + -2.7456508200215562e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 176920 + 2.7456508200215562e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 626421 + -2.7456508200215562e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>8 + 3.7051275372505188e-01 + 1.0000002384185791e+00 + <_> + 2 + 496938 + -2.7456508200215562e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 129483 + 2.7456508200215562e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -2.0004802945700365e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 5.2579295635223389e-01 + 4.9303025007247925e-02 + <_> + 1 + 597556 + -2.0004802945700365e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 4.2179220914840698e-01 + 2.1679308265447617e-02 + <_> + 2 + 95981 + 2.0004802945700365e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 501575 + -2.0004802945700365e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 285164 + 2.0004802945700365e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 1.2805368006229401e-01 + 1.2209074020385742e+01 + <_> + 2 + 215563 + -2.0004802945700365e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 69601 + 2.0004802945700365e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 2.1766179722445614e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 5.3841793537139893e-01 + 1.9424425125122070e+01 + <_> + 1 + 834878 + 2.1766179722445614e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 5.3460085391998291e-01 + 7.2649765014648438e+01 + <_> + 2 + 719975 + 2.1766179722445614e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 114903 + -2.1766179722445614e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 47842 + -2.1766179722445614e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>1 + 1.9600756466388702e-02 + 1.2279651641845703e+02 + <_> + 2 + 45030 + -2.1766179722445614e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 2812 + 2.1766179722445614e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -2.5286557649926933e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.4081672430038452e-01 + 6.3223449707031250e+01 + <_> + 1 + 760071 + -2.5286557649926933e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 4.2534014582633972e-01 + 1.4329477539062500e+03 + <_> + 2 + 271128 + 2.5286557649926933e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 488943 + -2.5286557649926933e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 122649 + 2.5286557649926933e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 1.3754153251647949e-01 + 3.0271297693252563e-01 + <_> + 2 + 115372 + 2.5286557649926933e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 7277 + -2.5286557649926933e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 2.0555075165763376e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 5.3161782026290894e-01 + 2.1139262616634369e-01 + <_> + 1 + 347753 + 2.0555075165763376e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 2.7911043167114258e-01 + 9.7840070724487305e-01 + <_> + 2 + 263596 + 2.0555075165763376e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 84157 + -2.0555075165763376e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 534967 + -2.0555075165763376e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 2.7209708094596863e-01 + 7.2531059265136719e+01 + <_> + 2 + 507329 + -2.0555075165763376e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 27638 + 2.0555075165763376e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 2.6318671841471969e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 5.3100723028182983e-01 + 3.0554033203125000e+03 + <_> + 1 + 555074 + 2.6318671841471969e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 3.3232513070106506e-01 + 1.1882034301757812e+02 + <_> + 2 + 231033 + -2.6318671841471969e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 324041 + 2.6318671841471969e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 327646 + -2.6318671841471969e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 2.3309436440467834e-01 + 1.0087275505065918e+01 + <_> + 2 + 154505 + 2.6318671841471969e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 173141 + -2.6318671841471969e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.7886966412152602e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.2818858623504639e-01 + 5.7150909423828125e+02 + <_> + 1 + 157310 + 1.7886966412152602e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>3 + 8.5386924445629120e-02 + 3.7840070724487305e+00 + <_> + 2 + 65160 + -1.7886966412152602e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 92150 + 1.7886966412152602e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 725410 + -1.7886966412152602e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 4.5921164751052856e-01 + 2.0250000000000000e+02 + <_> + 2 + 556290 + -1.7886966412152602e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 169120 + 1.7886966412152602e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 2.4412491736279468e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.3618264198303223e-01 + 6.7966049194335938e+01 + <_> + 1 + 588744 + 2.4412491736279468e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 3.7384462356567383e-01 + 9.1840713500976562e+01 + <_> + 2 + 544871 + 2.4412491736279468e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 43873 + -2.4412491736279468e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 293976 + -2.4412491736279468e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 1.8688529729843140e-01 + 1.7250000000000000e+02 + <_> + 2 + 31347 + 2.4412491736279468e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 262629 + -2.4412491736279468e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -2.5168356891603977e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 5.3682410717010498e-01 + 1.4950000000000000e+02 + <_> + 1 + 301559 + -2.5168356891603977e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 2.5741899013519287e-01 + 1.9172586500644684e-02 + <_> + 2 + 38284 + 2.5168356891603977e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 263275 + -2.5168356891603977e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 581161 + 2.5168356891603977e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 3.0517184734344482e-01 + 5.2177287638187408e-02 + <_> + 2 + 386885 + 2.5168356891603977e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 194276 + -2.5168356891603977e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.6274388301285150e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 5.2321952581405640e-01 + 4.9310810863971710e-02 + <_> + 1 + 597702 + -1.6274388301285150e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 4.2153576016426086e-01 + 1.7465135455131531e-01 + <_> + 2 + 287908 + 1.6274388301285150e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 309794 + -1.6274388301285150e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 285018 + 1.6274388301285150e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 1.1906065791845322e-01 + 3.3731169700622559e+00 + <_> + 2 + 254827 + 1.6274388301285150e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 30191 + -1.6274388301285150e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -2.3305996235932100e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 5.2847641706466675e-01 + 1.9722145795822144e-01 + <_> + 1 + 519616 + -2.3305996235932100e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 3.0912411212921143e-01 + 5.3183586120605469e+01 + <_> + 2 + 355831 + -2.3305996235932100e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 163785 + 2.3305996235932100e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 363104 + 2.3305996235932100e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 2.4887856841087341e-01 + 9.6279144287109375e-01 + <_> + 2 + 40121 + -2.3305996235932100e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 322983 + 2.3305996235932100e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -2.3036952403919270e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.3912377357482910e-01 + 5.3898361206054688e+01 + <_> + 1 + 365071 + 2.3036952403919270e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>3 + 2.3922763764858246e-01 + 3.7840070724487305e+00 + <_> + 2 + 67640 + -2.3036952403919270e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 297431 + 2.3036952403919270e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 517649 + -2.3036952403919270e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 3.1811138987541199e-01 + 4.0031188964843750e+01 + <_> + 2 + 510270 + -2.3036952403919270e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 7379 + 2.3036952403919270e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -2.0377221557767070e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 5.2814871072769165e-01 + 1.8350000000000000e+02 + <_> + 1 + 596228 + -2.0377221557767070e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>3 + 3.7580871582031250e-01 + 4.0621910095214844e+00 + <_> + 2 + 76371 + 2.0377221557767070e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 519857 + -2.0377221557767070e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 286492 + 2.0377221557767070e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 1.7495878040790558e-01 + 2.6395947265625000e+03 + <_> + 2 + 252916 + 2.0377221557767070e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 33576 + -2.0377221557767070e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.8553929539802766e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 5.2320522069931030e-01 + 2.9549052734375000e+03 + <_> + 1 + 544006 + 1.8553929539802766e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 3.1955578923225403e-01 + 2.6475187301635742e+01 + <_> + 2 + 211300 + -1.8553929539802766e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 332706 + 1.8553929539802766e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 338714 + -1.8553929539802766e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 2.2669644653797150e-01 + 6.0698143005371094e+01 + <_> + 2 + 259895 + -1.8553929539802766e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 78819 + 1.8553929539802766e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -2.3932159263591954e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 5.3755301237106323e-01 + 1.7373134613037109e+01 + <_> + 1 + 538378 + 2.3932159263591954e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 3.0823731422424316e-01 + 1.5216379394531250e+03 + <_> + 2 + 218668 + -2.3932159263591954e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 319710 + 2.3932159263591954e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 344342 + -2.3932159263591954e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 2.5130915641784668e-01 + 2.6667720079421997e-01 + <_> + 2 + 321778 + -2.3932159263591954e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 22564 + 2.3932159263591954e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.8502400513672809e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.1913762092590332e-01 + 4.4779052734375000e+01 + <_> + 1 + 558733 + -1.8502400513672809e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 2.7459961175918579e-01 + 1.0752773437500000e+03 + <_> + 2 + 171202 + 1.8502400513672809e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 387531 + -1.8502400513672809e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 323987 + 1.8502400513672809e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 2.7152487635612488e-01 + 2.1282728016376495e-01 + <_> + 2 + 214857 + 1.8502400513672809e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 109130 + -1.8502400513672809e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.9179242298010485e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 5.2680104970932007e-01 + 3.2482216796875000e+03 + <_> + 1 + 574784 + 1.9179242298010485e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 3.3918642997741699e-01 + 3.7387070059776306e-01 + <_> + 2 + 468901 + 1.9179242298010485e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 105883 + -1.9179242298010485e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 307936 + -1.9179242298010485e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 2.0861522853374481e-01 + 2.8730431571602821e-02 + <_> + 2 + 75080 + 1.9179242298010485e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 232856 + -1.9179242298010485e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.7677682637851502e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 5.1921409368515015e-01 + 7.3645393371582031e+01 + <_> + 1 + 534334 + -1.7677682637851502e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 2.9001012444496155e-01 + 4.0442691802978516e+01 + <_> + 2 + 161707 + 1.7677682637851502e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 372627 + -1.7677682637851502e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 348386 + 1.7677682637851502e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>8 + 2.5406932830810547e-01 + 2.5000000000000000e+00 + <_> + 2 + 257611 + 1.7677682637851502e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 90775 + -1.7677682637851502e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.2944927495056188e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 5.2261495590209961e-01 + 2.7957454681396484e+01 + <_> + 1 + 863896 + -1.2944927495056188e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.0150543451309204e-01 + 7.6779220581054688e+01 + <_> + 2 + 828421 + -1.2944927495056188e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 35475 + 1.2944927495056188e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 18824 + 1.2944927495056188e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 3.0811768025159836e-02 + 8.7500000000000000e+01 + <_> + 2 + 1526 + -1.2944927495056188e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 17298 + 1.2944927495056188e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 2.2805138788649618e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 5.2759498357772827e-01 + 1.2750000000000000e+02 + <_> + 1 + 154218 + -2.2805138788649618e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 1.6031777858734131e-01 + 9.7954940795898438e-01 + <_> + 2 + 69686 + -2.2805138788649618e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 84532 + 2.2805138788649618e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 728502 + 2.2805138788649618e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 3.9644926786422729e-01 + 1.0653674316406250e+03 + <_> + 2 + 144239 + -2.2805138788649618e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 584263 + 2.2805138788649618e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.1223244744301501e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>1 + 5.2056831121444702e-01 + 2.7092132568359375e+01 + <_> + 1 + 14184 + 1.1223244744301501e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 2.0670806989073753e-02 + 1.3561982154846191e+01 + <_> + 2 + 2597 + -1.1223244744301501e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 11587 + 1.1223244744301501e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 868536 + -1.1223244744301501e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 5.0735789537429810e-01 + 7.3869981765747070e+00 + <_> + 2 + 793621 + -1.1223244744301501e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 74915 + 1.1223244744301501e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.5530507131952809e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 5.2601444721221924e-01 + 5.2472219467163086e+00 + <_> + 1 + 785383 + 1.5530507131952809e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 4.7027212381362915e-01 + 3.3307580566406250e+02 + <_> + 2 + 41479 + -1.5530507131952809e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 743904 + 1.5530507131952809e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 97337 + -1.5530507131952809e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 6.8476296961307526e-02 + 2.5150000000000000e+02 + <_> + 2 + 86815 + -1.5530507131952809e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 10522 + 1.5530507131952809e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.2688637710805309e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>1 + 5.2681589126586914e-01 + 2.6944393157958984e+01 + <_> + 1 + 13767 + 1.2688637710805309e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 2.0236637443304062e-02 + 9.7680604457855225e-01 + <_> + 2 + 2455 + -1.2688637710805309e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 11312 + 1.2688637710805309e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 868953 + -1.2688637710805309e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 5.1144248247146606e-01 + 4.1006408691406250e+01 + <_> + 2 + 858830 + -1.2688637710805309e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 10123 + 1.2688637710805309e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.8286569125336186e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 5.1992076635360718e-01 + 1.2523591613769531e+02 + <_> + 1 + 809078 + 1.8286569125336186e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 4.4946891069412231e-01 + 6.3247621059417725e-02 + <_> + 2 + 710354 + 1.8286569125336186e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 98724 + -1.8286569125336186e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 73642 + -1.8286569125336186e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 9.6120528876781464e-02 + 1.2654811859130859e+01 + <_> + 2 + 28910 + 1.8286569125336186e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 44732 + -1.8286569125336186e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.6664812093165204e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 5.3545141220092773e-01 + 2.2550000000000000e+02 + <_> + 1 + 814284 + -1.6664812093165204e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 4.8810854554176331e-01 + 3.5277777910232544e-01 + <_> + 2 + 2224 + 1.6664812093165204e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 812060 + -1.6664812093165204e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 68436 + 1.6664812093165204e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 5.3457342088222504e-02 + 5.2621202468872070e+00 + <_> + 2 + 58922 + 1.6664812093165204e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 9514 + -1.6664812093165204e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.7054016986871479e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 5.2422934770584106e-01 + 3.4956390380859375e+01 + <_> + 1 + 140866 + -1.7054016986871479e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 9.7741618752479553e-02 + 4.9074954986572266e+01 + <_> + 2 + 99632 + -1.7054016986871479e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 41234 + 1.7054016986871479e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 741854 + 1.7054016986871479e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 4.4479039311408997e-01 + 1.2406488507986069e-01 + <_> + 2 + 44089 + -1.7054016986871479e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 697765 + 1.7054016986871479e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.4708091350926242e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 5.2632671594619751e-01 + 2.9207530975341797e+01 + <_> + 1 + 155246 + 1.4708091350926242e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 7.2520099580287933e-02 + 1.6195281982421875e+01 + <_> + 2 + 78729 + -1.4708091350926242e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 76517 + 1.4708091350926242e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 727474 + -1.4708091350926242e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 4.6418398618698120e-01 + 8.6062240600585938e+01 + <_> + 2 + 13616 + 1.4708091350926242e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 713858 + -1.4708091350926242e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.8537181928091362e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.2009099721908569e-01 + 7.6652450561523438e+01 + <_> + 1 + 682492 + 1.8537181928091362e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 4.1290074586868286e-01 + 1.6710216522216797e+01 + <_> + 2 + 627187 + 1.8537181928091362e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 55305 + -1.8537181928091362e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 200228 + -1.8537181928091362e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 1.3330994546413422e-01 + 1.2432705078125000e+03 + <_> + 2 + 38762 + 1.8537181928091362e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 161466 + -1.8537181928091362e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -2.1321615139911135e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.3494381904602051e-01 + 6.5299362182617188e+01 + <_> + 1 + 774888 + -2.1321615139911135e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 4.3464040756225586e-01 + 1.5307974815368652e-01 + <_> + 2 + 201052 + 2.1321615139911135e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 573836 + -2.1321615139911135e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 107832 + 2.1321615139911135e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 1.1846260726451874e-01 + 6.7380859375000000e+03 + <_> + 2 + 98330 + 2.1321615139911135e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 9502 + -2.1321615139911135e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.7835422273965343e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 5.2141159772872925e-01 + 2.6457395553588867e+00 + <_> + 1 + 543502 + 1.7835422273965343e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 2.5564342737197876e-01 + 3.1950057983398438e+01 + <_> + 2 + 295667 + -1.7835422273965343e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 247835 + 1.7835422273965343e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 339218 + -1.7835422273965343e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 2.8882730007171631e-01 + 1.2249999046325684e+00 + <_> + 2 + 32471 + 1.7835422273965343e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 306747 + -1.7835422273965343e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.6421246789221766e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 5.2026802301406860e-01 + 1.4970359206199646e-01 + <_> + 1 + 194491 + -1.6421246789221766e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 1.2315726280212402e-01 + 9.6512353420257568e-01 + <_> + 2 + 29978 + 1.6421246789221766e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 164513 + -1.6421246789221766e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 688229 + 1.6421246789221766e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 4.1780385375022888e-01 + 1.6746243286132812e+02 + <_> + 2 + 382442 + -1.6421246789221766e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 305787 + 1.6421246789221766e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 2.1349945196507980e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.3403520584106445e-01 + 6.3926227569580078e+01 + <_> + 1 + 532387 + 2.1349945196507980e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 3.4311762452125549e-01 + 8.8874023437500000e+01 + <_> + 2 + 492657 + 2.1349945196507980e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 39730 + -2.1349945196507980e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 350333 + -2.1349945196507980e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 2.1005542576313019e-01 + 4.9007560729980469e+01 + <_> + 2 + 173563 + 2.1349945196507980e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 176770 + -2.1349945196507980e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -2.1157485399239337e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.2946388721466064e-01 + 5.0247917175292969e+01 + <_> + 1 + 616448 + -2.1157485399239337e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 3.3478778600692749e-01 + 1.4483706054687500e+03 + <_> + 2 + 264101 + 2.1157485399239337e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 352347 + -2.1157485399239337e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 266272 + 2.1157485399239337e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 2.1790950000286102e-01 + 2.0245698094367981e-01 + <_> + 2 + 167887 + 2.1157485399239337e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 98385 + -2.1157485399239337e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.4987596391113447e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 5.1824301481246948e-01 + 1.9450000000000000e+02 + <_> + 1 + 666719 + -1.4987596391113447e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 3.9597755670547485e-01 + 4.7238201141357422e+01 + <_> + 2 + 251380 + 1.4987596391113447e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 415339 + -1.4987596391113447e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 216001 + 1.4987596391113447e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 1.4142143726348877e-01 + 6.9206595420837402e-02 + <_> + 2 + 187863 + 1.4987596391113447e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 28138 + -1.4987596391113447e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.6945746191775046e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 5.2560162544250488e-01 + 1.3750000000000000e+02 + <_> + 1 + 217397 + -1.6945746191775046e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 1.9838671386241913e-01 + 9.2698411941528320e+00 + <_> + 2 + 173324 + -1.6945746191775046e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 44073 + 1.6945746191775046e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 665323 + 1.6945746191775046e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 3.4387657046318054e-01 + 1.6638052368164062e+02 + <_> + 2 + 272338 + -1.6945746191775046e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 392985 + 1.6945746191775046e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.8303376278591144e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 5.2342832088470459e-01 + 1.6974687576293945e+01 + <_> + 1 + 526638 + 1.8303376278591144e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 2.9907098412513733e-01 + 5.3737506866455078e+00 + <_> + 2 + 456304 + 1.8303376278591144e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 70334 + -1.8303376278591144e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 356082 + -1.8303376278591144e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 2.4656014144420624e-01 + 4.0000801086425781e+01 + <_> + 2 + 349628 + -1.8303376278591144e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 6454 + 1.8303376278591144e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.9837200424471124e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.3148043155670166e-01 + 4.5435165405273438e+01 + <_> + 1 + 566916 + -1.9837200424471124e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 2.8911086916923523e-01 + 1.9850000000000000e+02 + <_> + 2 + 497959 + -1.9837200424471124e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 68957 + 1.9837200424471124e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 315804 + 1.9837200424471124e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 2.6032015681266785e-01 + 1.2580104172229767e-01 + <_> + 2 + 19943 + -1.9837200424471124e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 295861 + 1.9837200424471124e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.8274765362971820e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.2690309286117554e-01 + 1.0715937500000000e+03 + <_> + 1 + 415837 + 1.8274765362971820e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 2.3095072805881500e-01 + 1.0696158409118652e+01 + <_> + 2 + 376178 + 1.8274765362971820e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 39659 + -1.8274765362971820e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 466883 + -1.8274765362971820e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 3.1460943818092346e-01 + 5.5210243225097656e+01 + <_> + 2 + 126974 + 1.8274765362971820e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 339909 + -1.8274765362971820e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.7981433608797842e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 5.2889794111251831e-01 + 1.3750000000000000e+02 + <_> + 1 + 217397 + -1.7981433608797842e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>10 + 2.0291399955749512e-01 + 7.0379514694213867e+00 + <_> + 2 + 211530 + -1.7981433608797842e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 5867 + 1.7981433608797842e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 665323 + 1.7981433608797842e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 3.4191885590553284e-01 + 9.8043918609619141e-01 + <_> + 2 + 474461 + 1.7981433608797842e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 190862 + -1.7981433608797842e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.6787892706410881e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.2736580371856689e-01 + 4.2897491455078125e+01 + <_> + 1 + 199585 + 1.6787892706410881e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 1.5381197631359100e-01 + 5.5714025497436523e+00 + <_> + 2 + 159918 + 1.6787892706410881e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 39667 + -1.6787892706410881e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 683135 + -1.6787892706410881e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 3.8805946707725525e-01 + 4.4086141586303711e+00 + <_> + 2 + 581177 + -1.6787892706410881e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 101958 + 1.6787892706410881e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.7317557622810609e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 5.2000737190246582e-01 + 2.4731689453125000e+03 + <_> + 1 + 478156 + 1.7317557622810609e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 2.8003850579261780e-01 + 2.7021043777465820e+01 + <_> + 2 + 207413 + -1.7317557622810609e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 270743 + 1.7317557622810609e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 404564 + -1.7317557622810609e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 2.6314750313758850e-01 + 6.4897674560546875e+01 + <_> + 2 + 333565 + -1.7317557622810609e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 70999 + 1.7317557622810609e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.8871309968597677e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 5.2945041656494141e-01 + 3.9570122957229614e-02 + <_> + 1 + 392264 + 1.8871309968597677e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 2.8795385360717773e-01 + 1.2070747070312500e+03 + <_> + 2 + 77576 + -1.8871309968597677e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 314688 + 1.8871309968597677e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 490456 + -1.8871309968597677e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 2.5908491015434265e-01 + 4.9794895172119141e+01 + <_> + 2 + 61265 + 1.8871309968597677e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 429191 + -1.8871309968597677e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.5101704383245079e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 5.2044838666915894e-01 + 6.9030746459960938e+01 + <_> + 1 + 484168 + -1.5101704383245079e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 2.6727783679962158e-01 + 1.9172497093677521e-02 + <_> + 2 + 31689 + 1.5101704383245079e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 452479 + -1.5101704383245079e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 398552 + 1.5101704383245079e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 2.7040484547615051e-01 + 1.0260999679565430e+01 + <_> + 2 + 106632 + -1.5101704383245079e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 291920 + 1.5101704383245079e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.8635702791409478e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 5.2642911672592163e-01 + 9.7360563278198242e-01 + <_> + 1 + 362649 + -1.8635702791409478e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 2.9384776949882507e-01 + 1.1438926696777344e+01 + <_> + 2 + 153154 + 1.8635702791409478e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 209495 + -1.8635702791409478e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 520071 + 1.8635702791409478e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 2.5260710716247559e-01 + 4.9030090332031250e+01 + <_> + 2 + 450934 + -1.8635702791409478e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 69137 + 1.8635702791409478e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.9765326927722926e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 5.3124678134918213e-01 + 9.7360563278198242e-01 + <_> + 1 + 362649 + -1.9765326927722926e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 2.8489610552787781e-01 + 1.4350000000000000e+02 + <_> + 2 + 64160 + -1.9765326927722926e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 298489 + 1.9765326927722926e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 520071 + 1.9765326927722926e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 2.6435697078704834e-01 + 7.0211486816406250e+01 + <_> + 2 + 370858 + 1.9765326927722926e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 149213 + -1.9765326927722926e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.9008562947159074e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 5.2817881107330322e-01 + 9.7688269615173340e-01 + <_> + 1 + 453264 + -1.9008562947159074e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 3.8661244511604309e-01 + 1.3652859628200531e-01 + <_> + 2 + 44955 + 1.9008562947159074e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 408309 + -1.9008562947159074e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 429456 + 1.9008562947159074e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 1.6076639294624329e-01 + 1.4200860595703125e+02 + <_> + 2 + 239336 + 1.9008562947159074e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 190120 + -1.9008562947159074e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.6488461611685662e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 5.2262514829635620e-01 + 1.3924482464790344e-01 + <_> + 1 + 121797 + -1.6488461611685662e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 8.2732662558555603e-02 + 1.2755950927734375e+01 + <_> + 2 + 56802 + 1.6488461611685662e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 64995 + -1.6488461611685662e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 760923 + 1.6488461611685662e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 4.5839536190032959e-01 + 1.1973855590820312e+02 + <_> + 2 + 690430 + 1.6488461611685662e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 70493 + -1.6488461611685662e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -2.0696553564999931e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 5.2718508243560791e-01 + 1.5144380569458008e+01 + <_> + 1 + 471388 + -2.0696553564999931e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 2.6793250441551208e-01 + 4.9755523681640625e+01 + <_> + 2 + 433903 + -2.0696553564999931e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 37485 + 2.0696553564999931e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 411332 + 2.0696553564999931e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 2.8362497687339783e-01 + 1.5036141872406006e-01 + <_> + 2 + 94777 + -2.0696553564999931e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 316555 + 2.0696553564999931e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.7442798053884997e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.2825641632080078e-01 + 1.0713525390625000e+03 + <_> + 1 + 415724 + 1.7442798053884997e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 2.2957545518875122e-01 + 3.3307580566406250e+02 + <_> + 2 + 41479 + -1.7442798053884997e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 374245 + 1.7442798053884997e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 466996 + -1.7442798053884997e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 3.1392130255699158e-01 + 4.1006095886230469e+01 + <_> + 2 + 460665 + -1.7442798053884997e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 6331 + 1.7442798053884997e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.6894820709831398e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 5.2284801006317139e-01 + 3.0361111164093018e+00 + <_> + 1 + 554473 + -1.6894820709831398e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 2.9346308112144470e-01 + 1.7843583226203918e-01 + <_> + 2 + 214178 + 1.6894820709831398e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 340295 + -1.6894820709831398e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 328247 + 1.6894820709831398e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 2.4867379665374756e-01 + 4.2562915802001953e+01 + <_> + 2 + 75934 + -1.6894820709831398e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 252313 + 1.6894820709831398e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.7674119203356484e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 5.1899164915084839e-01 + 1.2321180105209351e+00 + <_> + 1 + 372782 + 1.7674119203356484e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 2.2046257555484772e-01 + 5.1326644897460938e+01 + <_> + 2 + 131116 + -1.7674119203356484e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 241666 + 1.7674119203356484e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 509938 + -1.7674119203356484e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 3.2360807061195374e-01 + 1.2249999046325684e+00 + <_> + 2 + 102254 + 1.7674119203356484e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 407684 + -1.7674119203356484e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.4608854021190626e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 5.2037280797958374e-01 + 7.0782981872558594e+01 + <_> + 1 + 747806 + 1.4608854021190626e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 4.0612849593162537e-01 + 1.9494859695434570e+01 + <_> + 2 + 703201 + 1.4608854021190626e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 44605 + -1.4608854021190626e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 134914 + -1.4608854021190626e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>13 + 1.3032881915569305e-01 + 1.1943687438964844e+02 + <_> + 2 + 105632 + -1.4608854021190626e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 29282 + 1.4608854021190626e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.6290662080802179e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.2730250358581543e-01 + 6.7461097717285156e+01 + <_> + 1 + 786712 + -1.6290662080802179e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>1 + 4.3959954380989075e-01 + 2.5377178192138672e+01 + <_> + 2 + 10670 + 1.6290662080802179e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 776042 + -1.6290662080802179e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 96008 + 1.6290662080802179e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 1.0103728622198105e-01 + 2.5565913085937500e+03 + <_> + 2 + 72779 + 1.6290662080802179e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 23229 + -1.6290662080802179e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.4416610781529565e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.2142697572708130e-01 + 2.2786550521850586e+01 + <_> + 1 + 183963 + -1.4416610781529565e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 6.8228527903556824e-02 + 2.4811942875385284e-01 + <_> + 2 + 129761 + -1.4416610781529565e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 54202 + 1.4416610781529565e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 698757 + 1.4416610781529565e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 4.6775069832801819e-01 + 2.9504753112792969e+01 + <_> + 2 + 558623 + 1.4416610781529565e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 140134 + -1.4416610781529565e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.3452192081534106e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 5.1857942342758179e-01 + 1.1083720397949219e+02 + <_> + 1 + 775712 + -1.3452192081534106e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 4.1056188941001892e-01 + 1.3630136489868164e+01 + <_> + 2 + 677782 + -1.3452192081534106e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 97930 + 1.3452192081534106e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 107008 + 1.3452192081534106e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>8 + 1.2301797419786453e-01 + 1.7500000000000000e+01 + <_> + 2 + 88911 + 1.3452192081534106e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 18097 + -1.3452192081534106e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.7298411620927254e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 5.2151030302047729e-01 + 1.3457941055297852e+01 + <_> + 1 + 728960 + 1.7298411620927254e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 3.9567282795906067e-01 + 7.1139678955078125e+01 + <_> + 2 + 556234 + 1.7298411620927254e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 172726 + -1.7298411620927254e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 153760 + -1.7298411620927254e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 1.4746569097042084e-01 + 9.6817898750305176e-01 + <_> + 2 + 118375 + -1.7298411620927254e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 35385 + 1.7298411620927254e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.8077361312586679e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.2921539545059204e-01 + 6.3411556243896484e+01 + <_> + 1 + 761421 + -1.8077361312586679e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 4.1398248076438904e-01 + 1.6275157928466797e+01 + <_> + 2 + 698556 + -1.8077361312586679e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 62865 + 1.8077361312586679e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 121299 + 1.8077361312586679e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 1.3108824193477631e-01 + 1.5295208930969238e+01 + <_> + 2 + 93223 + 1.8077361312586679e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 28076 + -1.8077361312586679e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.8090017551510282e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 5.2529436349868774e-01 + 1.0182608366012573e+00 + <_> + 1 + 162001 + -1.8090017551510282e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 1.4433114230632782e-01 + 3.3587098121643066e-02 + <_> + 2 + 71177 + 1.8090017551510282e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 90824 + -1.8090017551510282e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 720719 + 1.8090017551510282e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 4.0077096223831177e-01 + 1.8091466903686523e+01 + <_> + 2 + 681251 + 1.8090017551510282e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 39468 + -1.8090017551510282e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.3113544658253162e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.1797407865524292e-01 + 3.5487310791015625e+02 + <_> + 1 + 52318 + 1.3113544658253162e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>3 + 4.3862845748662949e-02 + 3.7840070724487305e+00 + <_> + 2 + 42754 + -1.3113544658253162e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 9564 + 1.3113544658253162e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 830402 + -1.3113544658253162e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 4.8887410759925842e-01 + 2.3927772521972656e+01 + <_> + 2 + 807320 + -1.3113544658253162e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 23082 + 1.3113544658253162e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.5050401463649776e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 5.2547717094421387e-01 + 3.8817194104194641e-01 + <_> + 1 + 791036 + 1.5050401463649776e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 5.1546055078506470e-01 + 9.6774436533451080e-02 + <_> + 2 + 11781 + -1.5050401463649776e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 779255 + 1.5050401463649776e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 91684 + -1.5050401463649776e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 2.2094590589404106e-02 + 1.0142603302001953e+02 + <_> + 2 + 85734 + -1.5050401463649776e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 5950 + 1.5050401463649776e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.1782278029411612e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.2232253551483154e-01 + 8.0389335632324219e+01 + <_> + 1 + 859440 + -1.1782278029411612e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>1 + 5.0747865438461304e-01 + 2.8053895950317383e+01 + <_> + 2 + 16815 + 1.1782278029411612e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 842625 + -1.1782278029411612e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 23280 + 1.1782278029411612e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 2.1942995488643646e-02 + 9.6181493997573853e-01 + <_> + 2 + 9987 + 1.1782278029411612e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 13293 + -1.1782278029411612e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.3723020357718160e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>13 + 5.1898813247680664e-01 + 1.1058124542236328e+01 + <_> + 1 + 68239 + -1.3723020357718160e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 4.6264912933111191e-02 + 3.1845542907714844e+01 + <_> + 2 + 55708 + -1.3723020357718160e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 12531 + 1.3723020357718160e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 814481 + 1.3723020357718160e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 4.8798888921737671e-01 + 2.0290888845920563e-02 + <_> + 2 + 56036 + -1.3723020357718160e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 758445 + 1.3723020357718160e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.3496464949818757e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>3 + 5.2348971366882324e-01 + 4.0621910095214844e+00 + <_> + 1 + 76371 + 1.3496464949818757e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 5.8088716119527817e-02 + 3.8128118515014648e+00 + <_> + 2 + 68099 + -1.3496464949818757e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 8272 + 1.3496464949818757e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 806349 + -1.3496464949818757e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 4.7560131549835205e-01 + 8.0716880798339844e+01 + <_> + 2 + 784073 + -1.3496464949818757e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 22276 + 1.3496464949818757e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.3937218642837171e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 5.2214956283569336e-01 + 4.4297332763671875e+02 + <_> + 1 + 826754 + 1.3937218642837171e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 4.7053712606430054e-01 + 6.3277542591094971e-02 + <_> + 2 + 723962 + 1.3937218642837171e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 102792 + -1.3937218642837171e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 55966 + -1.3937218642837171e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 6.4249634742736816e-02 + 1.2626149177551270e+01 + <_> + 2 + 34554 + 1.3937218642837171e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 21412 + -1.3937218642837171e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.1688314619837324e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 5.2122378349304199e-01 + 1.0006089210510254e+00 + <_> + 1 + 19990 + 1.1688314619837324e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 3.0160043388605118e-02 + 3.1165347099304199e+00 + <_> + 2 + 13134 + 1.1688314619837324e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 6856 + -1.1688314619837324e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 862730 + -1.1688314619837324e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>13 + 4.9902752041816711e-01 + 3.8793749809265137e+00 + <_> + 2 + 3311 + 1.1688314619837324e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 859419 + -1.1688314619837324e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.3462910389821445e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 5.1732021570205688e-01 + 8.2670087814331055e+00 + <_> + 1 + 231846 + -1.3462910389821445e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 1.0497253388166428e-01 + 3.6026550292968750e+01 + <_> + 2 + 91783 + 1.3462910389821445e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 140063 + -1.3462910389821445e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 650874 + 1.3462910389821445e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>8 + 4.2863398790359497e-01 + 5.0000000000000000e-01 + <_> + 2 + 89552 + -1.3462910389821445e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 561322 + 1.3462910389821445e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.5253509332660217e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 5.2270543575286865e-01 + 1.0634729862213135e+00 + <_> + 1 + 268025 + 1.5253509332660217e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 1.8296697735786438e-01 + 1.7006107177734375e+03 + <_> + 2 + 230186 + 1.5253509332660217e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 37839 + -1.5253509332660217e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 614695 + -1.5253509332660217e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 3.5509303212165833e-01 + 1.0406154785156250e+03 + <_> + 2 + 113608 + 1.5253509332660217e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 501087 + -1.5253509332660217e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.5108034041044591e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 5.1858007907867432e-01 + 1.9277192354202271e+00 + <_> + 1 + 461877 + -1.5108034041044591e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 2.7026772499084473e-01 + 1.7296025156974792e-01 + <_> + 2 + 94134 + 1.5108034041044591e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 367743 + -1.5108034041044591e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 420843 + 1.5108034041044591e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 2.6743069291114807e-01 + 1.0547151565551758e+01 + <_> + 2 + 231010 + -1.5108034041044591e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 189833 + 1.5108034041044591e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.4485323597660088e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.1651149988174438e-01 + 1.2618762207031250e+03 + <_> + 1 + 525663 + 1.4485323597660088e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 2.8805953264236450e-01 + 3.3219573974609375e+02 + <_> + 2 + 40686 + -1.4485323597660088e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 484977 + 1.4485323597660088e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 357057 + -1.4485323597660088e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 2.4809059500694275e-01 + 3.1549996948242188e+02 + <_> + 2 + 251960 + -1.4485323597660088e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 105097 + 1.4485323597660088e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.2389695219422896e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 5.1903158426284790e-01 + 2.1150000000000000e+02 + <_> + 1 + 760119 + -1.2389695219422896e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 4.3817356228828430e-01 + 4.9272228032350540e-02 + <_> + 2 + 549005 + -1.2389695219422896e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 211114 + 1.2389695219422896e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 122601 + 1.2389695219422896e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 9.2761114239692688e-02 + 2.9519119858741760e-01 + <_> + 2 + 79801 + 1.2389695219422896e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 42800 + -1.2389695219422896e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.5021747154370152e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 5.2080684900283813e-01 + 5.1715463399887085e-02 + <_> + 1 + 643881 + 1.5021747154370152e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 4.4966846704483032e-01 + 1.9361829757690430e+01 + <_> + 2 + 598014 + 1.5021747154370152e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 45867 + -1.5021747154370152e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 238839 + -1.5021747154370152e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 8.7815448641777039e-02 + 1.6223345947265625e+03 + <_> + 2 + 136323 + 1.5021747154370152e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 102516 + -1.5021747154370152e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.5104266043261949e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.2864831686019897e-01 + 3.5403479003906250e+02 + <_> + 1 + 51950 + 1.5104266043261949e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>13 + 3.7973597645759583e-02 + 1.1057499885559082e+01 + <_> + 2 + 43052 + -1.5104266043261949e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 8898 + 1.5104266043261949e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 830770 + -1.5104266043261949e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 4.9971544742584229e-01 + 9.6596515178680420e-01 + <_> + 2 + 172326 + 1.5104266043261949e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 658444 + -1.5104266043261949e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.9557942998780994e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 5.1880055665969849e-01 + 9.7360563278198242e-01 + <_> + 1 + 362649 + -1.9557942998780994e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 2.9879355430603027e-01 + 4.9010505676269531e+01 + <_> + 2 + 151435 + 1.9557942998780994e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 211214 + -1.9557942998780994e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 520071 + 1.9557942998780994e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 2.4994602799415588e-01 + 8.2670726776123047e+00 + <_> + 2 + 187551 + -1.9557942998780994e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 332520 + 1.9557942998780994e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.7531063423995971e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.2391844987869263e-01 + 5.5836666107177734e+01 + <_> + 1 + 684317 + -1.7531063423995971e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 3.6422514915466309e-01 + 2.7048809051513672e+01 + <_> + 2 + 64490 + 1.7531063423995971e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 619827 + -1.7531063423995971e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 198403 + 1.7531063423995971e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 1.7949061095714569e-01 + 2.1929091215133667e-01 + <_> + 2 + 165551 + 1.7531063423995971e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 32852 + -1.7531063423995971e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.6987000913257311e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 5.2429395914077759e-01 + 5.1878147125244141e+00 + <_> + 1 + 761081 + 1.6987000913257311e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 4.5103564858436584e-01 + 2.0904876708984375e+01 + <_> + 2 + 723553 + 1.6987000913257311e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 37528 + -1.6987000913257311e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 121639 + -1.6987000913257311e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 9.1330043971538544e-02 + 4.9014778137207031e+01 + <_> + 2 + 27368 + -1.6987000913257311e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 94271 + 1.6987000913257311e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.5508147831230867e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 5.3036290407180786e-01 + 9.0298099517822266e+00 + <_> + 1 + 323142 + 1.5508147831230867e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 1.4453001320362091e-01 + 1.0054676532745361e+00 + <_> + 2 + 52249 + -1.5508147831230867e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 270893 + 1.5508147831230867e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 559578 + -1.5508147831230867e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 3.9416283369064331e-01 + 1.5760597656250000e+04 + <_> + 2 + 556495 + -1.5508147831230867e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 3083 + 1.5508147831230867e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.5342202611876526e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 5.1971518993377686e-01 + 2.7653417968750000e+03 + <_> + 1 + 520367 + 1.5342202611876526e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 3.0594521760940552e-01 + 1.2412986755371094e+01 + <_> + 2 + 468698 + 1.5342202611876526e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 51669 + -1.5342202611876526e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 362353 + -1.5342202611876526e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 2.3233523964881897e-01 + 1.1594191551208496e+01 + <_> + 2 + 300667 + -1.5342202611876526e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 61686 + 1.5342202611876526e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.5504460978397924e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 5.2174305915832520e-01 + 2.2924989461898804e-01 + <_> + 1 + 412765 + 1.5504460978397924e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 3.0794218182563782e-01 + 3.3801021575927734e+01 + <_> + 2 + 94379 + -1.5504460978397924e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 318386 + 1.5504460978397924e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 469955 + -1.5504460978397924e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 2.3074150085449219e-01 + 8.2617813110351562e+01 + <_> + 2 + 332617 + -1.5504460978397924e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 137338 + 1.5504460978397924e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.7663468622642209e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 5.2982634305953979e-01 + 3.0481584370136261e-02 + <_> + 1 + 232769 + 1.7663468622642209e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 1.6375517845153809e-01 + 1.5765869140625000e+01 + <_> + 2 + 170567 + 1.7663468622642209e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 62202 + -1.7663468622642209e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 649951 + -1.7663468622642209e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 3.8028904795646667e-01 + 3.8203027343750000e+02 + <_> + 2 + 26190 + 1.7663468622642209e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 623761 + -1.7663468622642209e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.6564221985873614e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 5.1796710491180420e-01 + 1.6050000000000000e+02 + <_> + 1 + 399660 + -1.6564221985873614e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 2.9315069317817688e-01 + 1.1290136337280273e+01 + <_> + 2 + 341249 + -1.6564221985873614e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 58411 + 1.6564221985873614e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 483060 + 1.6564221985873614e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 2.4816544353961945e-01 + 2.5353164062500000e+03 + <_> + 2 + 434885 + 1.6564221985873614e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 48175 + -1.6564221985873614e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.7215929309747219e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 5.2232277393341064e-01 + 1.0233835220336914e+01 + <_> + 1 + 474781 + 1.7215929309747219e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 2.2924977540969849e-01 + 1.8042167663574219e+01 + <_> + 2 + 400178 + 1.7215929309747219e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 74603 + -1.7215929309747219e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 407939 + -1.7215929309747219e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 3.1368407607078552e-01 + 1.2954711914062500e+03 + <_> + 2 + 74434 + 1.7215929309747219e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 333505 + -1.7215929309747219e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.5008166544717419e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.2053868770599365e-01 + 5.0605857849121094e+01 + <_> + 1 + 621100 + -1.5008166544717419e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 3.2591822743415833e-01 + 1.4217727661132812e+01 + <_> + 2 + 543576 + -1.5008166544717419e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 77524 + 1.5008166544717419e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 261620 + 1.5008166544717419e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 2.1153192222118378e-01 + 2.2933530807495117e-01 + <_> + 2 + 202550 + 1.5008166544717419e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 59070 + -1.5008166544717419e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.6115713578469781e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 5.2568131685256958e-01 + 8.4856582031250000e+03 + <_> + 1 + 828116 + 1.6115713578469781e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 4.8999029397964478e-01 + 3.8817194104194641e-01 + <_> + 2 + 736549 + 1.6115713578469781e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 91567 + -1.6115713578469781e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 54604 + -1.6115713578469781e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 5.0212003290653229e-02 + 2.4363305568695068e+00 + <_> + 2 + 5163 + 1.6115713578469781e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 49441 + -1.6115713578469781e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.2217388706835304e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>13 + 5.2113318443298340e-01 + 1.2291749572753906e+02 + <_> + 1 + 854787 + -1.2217388706835304e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 4.9575480818748474e-01 + 4.3668647766113281e+01 + <_> + 2 + 208002 + 1.2217388706835304e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 646785 + -1.2217388706835304e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 27933 + 1.2217388706835304e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 3.4750718623399734e-02 + 1.7827874422073364e-01 + <_> + 2 + 11111 + -1.2217388706835304e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 16822 + 1.2217388706835304e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.4389490851931711e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.1943367719650269e-01 + 3.2689224243164062e+01 + <_> + 1 + 373640 + -1.4389490851931711e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>8 + 1.4826011657714844e-01 + 5.0000000000000000e-01 + <_> + 2 + 75914 + 1.4389490851931711e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 297726 + -1.4389490851931711e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 509080 + 1.4389490851931711e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>13 + 3.8765168190002441e-01 + 2.3785625457763672e+01 + <_> + 2 + 8976 + -1.4389490851931711e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 500104 + 1.4389490851931711e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.3113257461308978e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>13 + 5.2870148420333862e-01 + 6.5018749237060547e+00 + <_> + 1 + 17596 + 1.3113257461308978e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 2.3623729124665260e-02 + 5.4762855529785156e+01 + <_> + 2 + 12772 + -1.3113257461308978e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 4824 + 1.3113257461308978e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 865124 + -1.3113257461308978e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 5.0911253690719604e-01 + 4.8412844848632812e+02 + <_> + 2 + 2391 + 1.3113257461308978e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 862733 + -1.3113257461308978e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.5395090726599980e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.2046531438827515e-01 + 6.3925266265869141e+01 + <_> + 1 + 532376 + 1.5395090726599980e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 3.3379149436950684e-01 + 1.2478563308715820e+01 + <_> + 2 + 464708 + 1.5395090726599980e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 67668 + -1.5395090726599980e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 350344 + -1.5395090726599980e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 2.0462039113044739e-01 + 1.9115633544921875e+03 + <_> + 2 + 246407 + -1.5395090726599980e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 103937 + 1.5395090726599980e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -7.0891233867616102e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 5.1380199193954468e-01 + 3.9166665077209473e-01 + <_> + 1 + 3675 + 7.0891233867616102e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 9.4717629253864288e-03 + 2.3396150209009647e-03 + <_> + 2 + 932 + -7.0891233867616102e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 2743 + 7.0891233867616102e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 879045 + -7.0891233867616102e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 5.0824362039566040e-01 + 4.9310810863971710e-02 + <_> + 2 + 594027 + -7.0891233867616102e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 285018 + 7.0891233867616102e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.2738899646449692e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 5.1406884193420410e-01 + 7.4177337646484375e+01 + <_> + 1 + 778419 + 1.2738899646449692e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 4.3520861864089966e-01 + 2.3214962005615234e+01 + <_> + 2 + 190874 + -1.2738899646449692e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 587545 + 1.2738899646449692e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 104301 + -1.2738899646449692e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 9.6595622599124908e-02 + 9.5797806978225708e-01 + <_> + 2 + 7126 + 1.2738899646449692e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 97175 + -1.2738899646449692e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -9.2374693751139270e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>13 + 5.1922696828842163e-01 + 6.5018749237060547e+00 + <_> + 1 + 17596 + 9.2374693751139270e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 2.3020882159471512e-02 + 1.3835926055908203e+01 + <_> + 2 + 2720 + -9.2374693751139270e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 14876 + 9.2374693751139270e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 865124 + -9.2374693751139270e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.0005638599395752e-01 + 6.8381072998046875e+01 + <_> + 2 + 774329 + -9.2374693751139270e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 90795 + 9.2374693751139270e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.3764077253002516e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 5.1601397991180420e-01 + 4.5694446563720703e+00 + <_> + 1 + 733593 + 1.3764077253002516e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 4.2000633478164673e-01 + 3.6223334960937500e+03 + <_> + 2 + 506640 + 1.3764077253002516e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 226953 + -1.3764077253002516e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 149127 + -1.3764077253002516e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 1.1434962600469589e-01 + 3.7323164062500000e+03 + <_> + 2 + 105133 + -1.3764077253002516e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 43994 + 1.3764077253002516e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.9031496828101474e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 5.2017998695373535e-01 + 4.6393759765625000e+03 + <_> + 1 + 688631 + -1.9031496828101474e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 4.0408775210380554e-01 + 3.5138888359069824e+00 + <_> + 2 + 501828 + -1.9031496828101474e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 186803 + 1.9031496828101474e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 194089 + 1.9031496828101474e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 1.4334791898727417e-01 + 1.4732261962890625e+03 + <_> + 2 + 131864 + 1.9031496828101474e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 62225 + -1.9031496828101474e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.7159442229572505e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 5.2571183443069458e-01 + 4.5694446563720703e+00 + <_> + 1 + 733593 + 1.7159442229572505e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 4.2676064372062683e-01 + 9.1846817016601562e+01 + <_> + 2 + 612494 + 1.7159442229572505e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 121099 + -1.7159442229572505e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 149127 + -1.7159442229572505e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 1.1603302508592606e-01 + 3.7601918945312500e+03 + <_> + 2 + 105921 + -1.7159442229572505e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 43206 + 1.7159442229572505e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.5491306279280093e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 5.1714891195297241e-01 + 3.0361111164093018e+00 + <_> + 1 + 554473 + -1.5491306279280093e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 2.9821380972862244e-01 + 1.9750000000000000e+02 + <_> + 2 + 496728 + -1.5491306279280093e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 57745 + 1.5491306279280093e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 328247 + 1.5491306279280093e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 2.4043717980384827e-01 + 4.5694446563720703e+00 + <_> + 2 + 179120 + 1.5491306279280093e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 149127 + -1.5491306279280093e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.5112870264636089e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 5.1591855287551880e-01 + 9.7360563278198242e-01 + <_> + 1 + 362649 + -1.5112870264636089e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 2.8606006503105164e-01 + 5.5948234558105469e+01 + <_> + 2 + 159316 + 1.5112870264636089e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 203333 + -1.5112870264636089e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 520071 + 1.5112870264636089e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 2.5165036320686340e-01 + 1.3924482464790344e-01 + <_> + 2 + 81406 + -1.5112870264636089e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 438665 + 1.5112870264636089e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.5848753713481872e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.2460360527038574e-01 + 5.6597629547119141e+01 + <_> + 1 + 690867 + -1.5848753713481872e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 3.6396765708923340e-01 + 1.4598178863525391e-01 + <_> + 2 + 132696 + 1.5848753713481872e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 558171 + -1.5848753713481872e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 191853 + 1.5848753713481872e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 1.7557150125503540e-01 + 1.2472306191921234e-01 + <_> + 2 + 7765 + -1.5848753713481872e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 184088 + 1.5848753713481872e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.8467330920249878e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 5.2402979135513306e-01 + 3.3699297904968262e+00 + <_> + 1 + 659234 + 1.8467330920249878e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 3.4882396459579468e-01 + 6.9690351486206055e+00 + <_> + 2 + 84499 + -1.8467330920249878e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 574735 + 1.8467330920249878e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 223486 + -1.8467330920249878e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 1.9721359014511108e-01 + 3.7550712585449219e+01 + <_> + 2 + 41022 + 1.8467330920249878e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 182464 + -1.8467330920249878e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -9.1590318110742600e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 5.1799583435058594e-01 + 2.0639503002166748e+00 + <_> + 1 + 13904 + 9.1590318110742600e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 2.1610239520668983e-02 + 9.7588074207305908e-01 + <_> + 2 + 1771 + -9.1590318110742600e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 12133 + 9.1590318110742600e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 868816 + -9.1590318110742600e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 5.0127136707305908e-01 + 7.4249625205993652e+00 + <_> + 2 + 794334 + -9.1590318110742600e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 74482 + 9.1590318110742600e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.4192656595626946e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 5.1756715774536133e-01 + 1.4337041974067688e-01 + <_> + 1 + 91501 + -1.4192656595626946e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 1.0054870694875717e-01 + 2.1980529651045799e-02 + <_> + 2 + 18145 + 1.4192656595626946e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 73356 + -1.4192656595626946e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 791219 + 1.4192656595626946e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 4.3487349152565002e-01 + 2.9338939666748047e+01 + <_> + 2 + 299075 + -1.4192656595626946e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 492144 + 1.4192656595626946e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.3184794344957612e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 5.2383834123611450e-01 + 1.6361110210418701e+00 + <_> + 1 + 262336 + 1.3184794344957612e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 1.1473449319601059e-01 + 1.7661753296852112e-01 + <_> + 2 + 63630 + -1.3184794344957612e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 198706 + 1.3184794344957612e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 620384 + -1.3184794344957612e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 4.1817983984947205e-01 + 4.0149372100830078e+01 + <_> + 2 + 69170 + 1.3184794344957612e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 551214 + -1.3184794344957612e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.3593186978629782e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 5.1870042085647583e-01 + 5.3859569132328033e-02 + <_> + 1 + 682504 + 1.3593186978629782e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 4.6931943297386169e-01 + 1.3940798118710518e-02 + <_> + 2 + 32627 + -1.3593186978629782e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 649877 + 1.3593186978629782e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 200216 + -1.3593186978629782e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 6.4611308276653290e-02 + 1.6939497070312500e+03 + <_> + 2 + 121003 + 1.3593186978629782e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 79213 + -1.3593186978629782e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.0712582612787516e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.2380466461181641e-01 + 1.0780788421630859e+01 + <_> + 1 + 9261 + 1.0712582612787516e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 1.0825651697814465e-02 + 2.3396150209009647e-03 + <_> + 2 + 1022 + -1.0712582612787516e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 8239 + 1.0712582612787516e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 873459 + -1.0712582612787516e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 5.1593023538589478e-01 + 9.8968684673309326e-01 + <_> + 2 + 872981 + -1.0712582612787516e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 478 + 1.0712582612787516e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.5436056649768831e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.1724588871002197e-01 + 4.2640373229980469e+01 + <_> + 1 + 530446 + -1.5436056649768831e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 2.5062859058380127e-01 + 9.7688281536102295e-01 + <_> + 2 + 174383 + -1.5436056649768831e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 356063 + 1.5436056649768831e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 352274 + 1.5436056649768831e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 2.8788509964942932e-01 + 9.7624023437500000e+02 + <_> + 2 + 16775 + -1.5436056649768831e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 335499 + 1.5436056649768831e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.6182873443285012e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 5.1497900485992432e-01 + 1.9831608886718750e+03 + <_> + 1 + 398344 + 1.6182873443285012e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 2.3368765413761139e-01 + 3.4386888146400452e-02 + <_> + 2 + 120906 + 1.6182873443285012e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 277438 + -1.6182873443285012e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 484376 + -1.6182873443285012e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 3.0668148398399353e-01 + 2.0055413246154785e+00 + <_> + 2 + 97194 + 1.6182873443285012e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 387182 + -1.6182873443285012e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.3253656848585513e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 5.2019423246383667e-01 + 3.5954517364501953e+01 + <_> + 1 + 148866 + -1.3253656848585513e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 9.7039222717285156e-02 + 4.9074954986572266e+01 + <_> + 2 + 106107 + -1.3253656848585513e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 42759 + 1.3253656848585513e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 733854 + 1.3253656848585513e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 4.3604651093482971e-01 + 1.3226324462890625e+02 + <_> + 2 + 671228 + 1.3253656848585513e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 62626 + -1.3253656848585513e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.3521379496087457e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 5.1417887210845947e-01 + 3.0403369140625000e+02 + <_> + 1 + 741348 + -1.3521379496087457e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 4.0201160311698914e-01 + 2.3680853843688965e-01 + <_> + 2 + 561460 + -1.3521379496087457e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 179888 + 1.3521379496087457e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 141372 + 1.3521379496087457e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 1.3174042105674744e-01 + 2.4532456696033478e-01 + <_> + 2 + 135143 + 1.3521379496087457e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 6229 + -1.3521379496087457e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -2.1527973441807904e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 5.1608854532241821e-01 + 2.2939157485961914e-01 + <_> + 1 + 665909 + 2.1527973441807904e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 3.8956171274185181e-01 + 6.7966049194335938e+01 + <_> + 2 + 425863 + 2.1527973441807904e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 240046 + -2.1527973441807904e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 216811 + -2.1527973441807904e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 1.6405132412910461e-01 + 1.1480089187622070e+01 + <_> + 2 + 170833 + -2.1527973441807904e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 45978 + 2.1527973441807904e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.6449789813279850e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.2677273750305176e-01 + 1.8037824707031250e+03 + <_> + 1 + 721329 + -1.6449789813279850e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 4.0812149643898010e-01 + 1.6156013488769531e+01 + <_> + 2 + 708561 + -1.6449789813279850e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 12768 + 1.6449789813279850e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 161391 + 1.6449789813279850e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 1.3291050493717194e-01 + 4.5441482543945312e+01 + <_> + 2 + 51233 + -1.6449789813279850e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 110158 + 1.6449789813279850e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.6821935026031784e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 5.2279496192932129e-01 + 1.3457941055297852e+01 + <_> + 1 + 728960 + 1.6821935026031784e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 3.9806309342384338e-01 + 1.0701467285156250e+03 + <_> + 2 + 171076 + -1.6821935026031784e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 557884 + 1.6821935026031784e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 153760 + -1.6821935026031784e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 1.4389283955097198e-01 + 4.8999481201171875e+01 + <_> + 2 + 86468 + 1.6821935026031784e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 67292 + -1.6821935026031784e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.2774872355953595e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.1970934867858887e-01 + 5.9401330566406250e+02 + <_> + 1 + 169669 + 1.2774872355953595e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 8.2486808300018311e-02 + 1.4642375946044922e+01 + <_> + 2 + 56415 + -1.2774872355953595e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 113254 + 1.2774872355953595e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 713051 + -1.2774872355953595e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 4.4940701127052307e-01 + 2.1150000000000000e+02 + <_> + 2 + 590450 + -1.2774872355953595e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 122601 + 1.2774872355953595e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.5043409796995610e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 5.1557129621505737e-01 + 9.6412289142608643e-01 + <_> + 1 + 144080 + -1.5043409796995610e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>13 + 7.3164306581020355e-02 + 1.2217437744140625e+02 + <_> + 2 + 125656 + -1.5043409796995610e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 18424 + 1.5043409796995610e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 738640 + 1.5043409796995610e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 4.6437343955039978e-01 + 2.5102270507812500e+03 + <_> + 2 + 732688 + 1.5043409796995610e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 5952 + -1.5043409796995610e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.2213979474255111e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.2743208408355713e-01 + 1.8529287719726562e+02 + <_> + 1 + 2298 + 1.2213979474255111e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 9.1448528692126274e-03 + 2.3396150209009647e-03 + <_> + 2 + 977 + -1.2213979474255111e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 1321 + 1.2213979474255111e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 880422 + -1.2213979474255111e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 5.2135217189788818e-01 + 3.1814361572265625e+02 + <_> + 2 + 872300 + -1.2213979474255111e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 8122 + 1.2213979474255111e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.5391970011548017e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 5.1817291975021362e-01 + 9.7515201568603516e-01 + <_> + 1 + 401473 + 1.5391970011548017e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>8 + 3.3433124423027039e-01 + 4.8500000000000000e+01 + <_> + 2 + 373199 + 1.5391970011548017e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 28274 + -1.5391970011548017e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 481247 + -1.5391970011548017e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 2.0407290756702423e-01 + 1.9958956298828125e+03 + <_> + 2 + 264659 + 1.5391970011548017e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 216588 + -1.5391970011548017e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -9.8528864883682371e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 5.1679217815399170e-01 + 4.0760154724121094e+00 + <_> + 1 + 76447 + 9.8528864883682371e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.7451516389846802e-02 + 3.3225021362304688e+02 + <_> + 2 + 35849 + -9.8528864883682371e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 40598 + 9.8528864883682371e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 806273 + -9.8528864883682371e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 4.6716079115867615e-01 + 9.8881262540817261e-01 + <_> + 2 + 802242 + -9.8528864883682371e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 4031 + 9.8528864883682371e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.1973032377040303e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 5.1663309335708618e-01 + 8.4094589233398438e+01 + <_> + 1 + 851120 + 1.1973032377040303e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 4.9671879410743713e-01 + 9.6090728044509888e-01 + <_> + 2 + 96778 + -1.1973032377040303e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 754342 + 1.1973032377040303e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 31600 + -1.1973032377040303e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 3.3178083598613739e-02 + 9.6181493997573853e-01 + <_> + 2 + 9952 + 1.1973032377040303e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 21648 + -1.1973032377040303e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.2789717888703350e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 5.2081155776977539e-01 + 1.1083720397949219e+02 + <_> + 1 + 775712 + -1.2789717888703350e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 4.1415563225746155e-01 + 1.3004655838012695e+01 + <_> + 2 + 659827 + -1.2789717888703350e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 115885 + 1.2789717888703350e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 107008 + 1.2789717888703350e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 1.1777514964342117e-01 + 1.7850000000000000e+02 + <_> + 2 + 18432 + -1.2789717888703350e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 88576 + 1.2789717888703350e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.3912029000207526e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 5.1905900239944458e-01 + 2.3968553543090820e+01 + <_> + 1 + 652037 + 1.3912029000207526e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 3.7184661626815796e-01 + 3.4303757548332214e-01 + <_> + 2 + 516542 + 1.3912029000207526e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 135495 + -1.3912029000207526e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 230683 + -1.3912029000207526e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 1.6287745535373688e-01 + 1.6569257736206055e+01 + <_> + 2 + 213834 + -1.3912029000207526e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 16849 + 1.3912029000207526e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.5635177482848622e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.2096682786941528e-01 + 6.0706512451171875e+01 + <_> + 1 + 740256 + -1.5635177482848622e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 3.9196369051933289e-01 + 1.3255786895751953e-01 + <_> + 2 + 75647 + 1.5635177482848622e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 664609 + -1.5635177482848622e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 142464 + 1.5635177482848622e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>4 + 1.4704482257366180e-01 + 8.3879852294921875e+01 + <_> + 2 + 94282 + 1.5635177482848622e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 48182 + -1.5635177482848622e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.6203780793528536e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 5.2000653743743896e-01 + 6.4397480468750000e+03 + <_> + 1 + 775340 + 1.6203780793528536e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 4.5413655042648315e-01 + 8.1181564331054688e+01 + <_> + 2 + 623990 + 1.6203780793528536e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 151350 + -1.6203780793528536e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 107380 + -1.6203780793528536e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 8.6284510791301727e-02 + 2.4346966743469238e+00 + <_> + 2 + 18653 + 1.6203780793528536e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 88727 + -1.6203780793528536e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.5747635455102088e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>13 + 5.2527719736099243e-01 + 8.2628128051757812e+01 + <_> + 1 + 720163 + -1.5747635455102088e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 4.1845187544822693e-01 + 2.1850000000000000e+02 + <_> + 2 + 700964 + -1.5747635455102088e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 19199 + 1.5747635455102088e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 162557 + 1.5747635455102088e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 1.2083605676889420e-01 + 9.7091972827911377e-01 + <_> + 2 + 79673 + 1.5747635455102088e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 82884 + -1.5747635455102088e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.5927162030193687e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 5.1800841093063354e-01 + 9.6816873550415039e-01 + <_> + 1 + 220280 + -1.5927162030193687e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 1.3576315343379974e-01 + 3.0310668945312500e+00 + <_> + 2 + 131140 + -1.5927162030193687e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 89140 + 1.5927162030193687e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 662440 + 1.5927162030193687e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 4.0397080779075623e-01 + 2.3610116577148438e+02 + <_> + 2 + 517791 + 1.5927162030193687e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 144649 + -1.5927162030193687e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.6327065147740613e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 5.2699422836303711e-01 + 1.9700955200195312e+02 + <_> + 1 + 539232 + -1.6327065147740613e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 2.7215531468391418e-01 + 1.0239037322998047e+02 + <_> + 2 + 523511 + -1.6327065147740613e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 15721 + 1.6327065147740613e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 343488 + 1.6327065147740613e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>13 + 2.6857191324234009e-01 + 2.9673126220703125e+01 + <_> + 2 + 15391 + -1.6327065147740613e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 328097 + 1.6327065147740613e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.9862895000969566e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.2308297157287598e-01 + 4.8618629455566406e+01 + <_> + 1 + 598028 + 1.9862895000969566e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 3.1783524155616760e-01 + 5.1877889633178711e+00 + <_> + 2 + 571996 + 1.9862895000969566e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 26032 + -1.9862895000969566e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 284692 + -1.9862895000969566e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 2.3165938258171082e-01 + 9.7361612319946289e-01 + <_> + 2 + 213508 + -1.9862895000969566e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 71184 + 1.9862895000969566e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.7501120242802398e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 5.2644926309585571e-01 + 2.1370663452148438e+02 + <_> + 1 + 580176 + -1.7501120242802398e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 3.0134236812591553e-01 + 4.8414001464843750e+00 + <_> + 2 + 562352 + -1.7501120242802398e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 17824 + 1.7501120242802398e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 302544 + 1.7501120242802398e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 2.4229909479618073e-01 + 2.5264859199523926e-01 + <_> + 2 + 288408 + 1.7501120242802398e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 14136 + -1.7501120242802398e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.6379811197624464e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.1758897304534912e-01 + 3.9146591186523438e+01 + <_> + 1 + 484940 + 1.6379811197624464e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 2.1544224023818970e-01 + 9.6972543001174927e-01 + <_> + 2 + 73329 + -1.6379811197624464e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 411611 + 1.6379811197624464e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 397780 + -1.6379811197624464e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 3.2541596889495850e-01 + 9.7100257873535156e-01 + <_> + 2 + 219167 + 1.6379811197624464e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 178613 + -1.6379811197624464e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -7.7730586636507809e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>13 + 5.1660019159317017e-01 + 3.7518749237060547e+00 + <_> + 1 + 4360 + 7.7730586636507809e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 1.2169615365564823e-02 + 2.3396150209009647e-03 + <_> + 2 + 1025 + -7.7730586636507809e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 3335 + 7.7730586636507809e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 878360 + -7.7730586636507809e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>8 + 5.0725322961807251e-01 + 1.5850000000000000e+02 + <_> + 2 + 868881 + -7.7730586636507809e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 9479 + 7.7730586636507809e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.4548049627782231e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 5.1823103427886963e-01 + 9.6816873550415039e-01 + <_> + 1 + 220280 + -1.4548049627782231e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 1.3301922380924225e-01 + 1.3513422012329102e+01 + <_> + 2 + 102920 + 1.4548049627782231e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 117360 + -1.4548049627782231e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 662440 + 1.4548049627782231e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 4.0328687429428101e-01 + 7.8073083496093750e+02 + <_> + 2 + 258718 + -1.4548049627782231e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 403722 + 1.4548049627782231e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.3286394621028511e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.2052193880081177e-01 + 4.2509910583496094e+01 + <_> + 1 + 195514 + 1.3286394621028511e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 1.5595395863056183e-01 + 1.2140546798706055e+01 + <_> + 2 + 177817 + 1.3286394621028511e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 17697 + -1.3286394621028511e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 687206 + -1.3286394621028511e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>8 + 3.7721323966979980e-01 + 1.0000002384185791e+00 + <_> + 2 + 547505 + -1.3286394621028511e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 139701 + 1.3286394621028511e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.0141842709523226e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>8 + 5.1050430536270142e-01 + 9.5000000000000000e+00 + <_> + 1 + 741694 + 1.0141842709523226e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 4.1334989666938782e-01 + 1.6119709610939026e-01 + <_> + 2 + 237391 + -1.0141842709523226e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 504303 + 1.0141842709523226e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 141026 + -1.0141842709523226e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 1.1198298633098602e-01 + 1.5681928396224976e-01 + <_> + 2 + 40956 + 1.0141842709523226e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 100070 + -1.0141842709523226e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.0204254714042733e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 5.1165634393692017e-01 + 4.9310810863971710e-02 + <_> + 1 + 597702 + -1.0204254714042733e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 4.2001527547836304e-01 + 2.2750000000000000e+02 + <_> + 2 + 576248 + -1.0204254714042733e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 21454 + 1.0204254714042733e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 285018 + 1.0204254714042733e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 1.0547325760126114e-01 + 1.2068505287170410e+01 + <_> + 2 + 210136 + -1.0204254714042733e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 74882 + 1.0204254714042733e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.3206404367127791e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.1923865079879761e-01 + 7.7107513427734375e+01 + <_> + 1 + 686673 + 1.3206404367127791e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 4.0693727135658264e-01 + 2.0618534088134766e+01 + <_> + 2 + 147467 + -1.3206404367127791e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 539206 + 1.3206404367127791e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 196047 + -1.3206404367127791e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 1.2603084743022919e-01 + 1.1269699707031250e+03 + <_> + 2 + 32141 + 1.3206404367127791e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 163906 + -1.3206404367127791e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.2522801186209614e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.1282906532287598e-01 + 1.9481311035156250e+03 + <_> + 1 + 748890 + -1.2522801186209614e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 4.2607733607292175e-01 + 2.2262081503868103e-02 + <_> + 2 + 60771 + 1.2522801186209614e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 688119 + -1.2522801186209614e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 133830 + 1.2522801186209614e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 1.0518882423639297e-01 + 8.5385124206542969e+01 + <_> + 2 + 51615 + -1.2522801186209614e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 82215 + 1.2522801186209614e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.1394421949935540e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 5.1818764209747314e-01 + 5.2861108779907227e+00 + <_> + 1 + 787552 + 1.1394421949935540e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 4.6491500735282898e-01 + 1.2412986755371094e+01 + <_> + 2 + 694928 + 1.1394421949935540e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 92624 + -1.1394421949935540e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 95168 + -1.1394421949935540e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 6.3540279865264893e-02 + 9.6232366561889648e-01 + <_> + 2 + 71571 + 1.1394421949935540e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 23597 + -1.1394421949935540e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.0826579693739027e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 5.1471543312072754e-01 + 2.2348790168762207e+00 + <_> + 1 + 480732 + -1.0826579693739027e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 2.7823588252067566e-01 + 7.9505096435546875e+01 + <_> + 2 + 451112 + -1.0826579693739027e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 29620 + 1.0826579693739027e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 401988 + 1.0826579693739027e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 2.4880415201187134e-01 + 9.7053229808807373e-01 + <_> + 2 + 128425 + -1.0826579693739027e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 273563 + 1.0826579693739027e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.3144537343157656e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 5.1298958063125610e-01 + 1.6050000000000000e+02 + <_> + 1 + 399660 + -1.3144537343157656e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>13 + 2.9288473725318909e-01 + 4.3612503051757812e+01 + <_> + 2 + 340268 + -1.3144537343157656e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 59392 + 1.3144537343157656e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 483060 + 1.3144537343157656e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 2.3992937803268433e-01 + 2.4841389060020447e-01 + <_> + 2 + 246913 + 1.3144537343157656e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 236147 + -1.3144537343157656e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.2882403429229958e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 5.1944392919540405e-01 + 6.4103988647460938e+01 + <_> + 1 + 680149 + 1.2882403429229958e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 3.5578826069831848e-01 + 4.3049764633178711e+00 + <_> + 2 + 421525 + 1.2882403429229958e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 258624 + -1.2882403429229958e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 202571 + -1.2882403429229958e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 1.7637327313423157e-01 + 9.7361671924591064e-01 + <_> + 2 + 134355 + -1.2882403429229958e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 68216 + 1.2882403429229958e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.0659118283176185e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 5.1539218425750732e-01 + 1.0216948986053467e+00 + <_> + 1 + 174242 + -1.0659118283176185e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 1.3959318399429321e-01 + 3.0075774192810059e+00 + <_> + 2 + 111579 + 1.0659118283176185e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 62663 + -1.0659118283176185e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 708478 + 1.0659118283176185e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 3.8702940940856934e-01 + 5.3617730736732483e-02 + <_> + 2 + 533869 + 1.0659118283176185e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 174609 + -1.0659118283176185e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.4069933258304967e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 5.1755863428115845e-01 + 1.4383237838745117e+01 + <_> + 1 + 761907 + -1.4069933258304967e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 4.3396136164665222e-01 + 9.6425104141235352e-01 + <_> + 2 + 56236 + 1.4069933258304967e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 705671 + -1.4069933258304967e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 120813 + 1.4069933258304967e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 1.0115556418895721e-01 + 2.3710642009973526e-02 + <_> + 2 + 62480 + -1.4069933258304967e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 58333 + 1.4069933258304967e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.3823153035302269e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.1895242929458618e-01 + 1.2654792480468750e+03 + <_> + 1 + 527911 + 1.3823153035302269e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 2.9878714680671692e-01 + 6.4664505004882812e+01 + <_> + 2 + 422761 + 1.3823153035302269e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 105150 + -1.3823153035302269e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 354809 + -1.3823153035302269e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 2.3571582138538361e-01 + 1.8355369567871094e-01 + <_> + 2 + 221289 + -1.3823153035302269e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 133520 + 1.3823153035302269e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.7249636334057181e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 5.1692783832550049e-01 + 1.7464916408061981e-01 + <_> + 1 + 382615 + 1.7249636334057181e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 2.2168469429016113e-01 + 3.4354557037353516e+01 + <_> + 2 + 136889 + -1.7249636334057181e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 245726 + 1.7249636334057181e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 500105 + -1.7249636334057181e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 3.2133278250694275e-01 + 7.2052200317382812e+01 + <_> + 2 + 456403 + -1.7249636334057181e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 43702 + 1.7249636334057181e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.5834237357117847e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.2223962545394897e-01 + 3.9147827148437500e+01 + <_> + 1 + 484958 + 1.5834237357117847e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 2.1953789889812469e-01 + 9.6796119213104248e-01 + <_> + 2 + 65939 + -1.5834237357117847e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 419019 + 1.5834237357117847e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 397762 + -1.5834237357117847e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 3.1996521353721619e-01 + 9.0676116943359375e+00 + <_> + 2 + 75124 + 1.5834237357117847e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 322638 + -1.5834237357117847e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.3945437861514770e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 5.2157562971115112e-01 + 1.1918860673904419e-01 + <_> + 1 + 41045 + -1.3945437861514770e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 5.4044932126998901e-02 + 2.2783178836107254e-02 + <_> + 2 + 9257 + 1.3945437861514770e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 31788 + -1.3945437861514770e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 841675 + 1.3945437861514770e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 4.8076227307319641e-01 + 4.3226741790771484e+01 + <_> + 2 + 366611 + -1.3945437861514770e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 475064 + 1.3945437861514770e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.2048864826994479e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 5.1537650823593140e-01 + 3.6076293945312500e+01 + <_> + 1 + 254261 + 1.2048864826994479e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 1.3336037099361420e-01 + 3.4386821091175079e-02 + <_> + 2 + 145753 + 1.2048864826994479e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 108508 + -1.2048864826994479e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 628459 + -1.2048864826994479e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 3.9672541618347168e-01 + 4.9310810863971710e-02 + <_> + 2 + 384511 + -1.2048864826994479e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 243948 + 1.2048864826994479e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.4186187406301717e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 5.1801306009292603e-01 + 5.1113564521074295e-02 + <_> + 1 + 632343 + 1.4186187406301717e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 4.4642397761344910e-01 + 1.2350000000000000e+02 + <_> + 2 + 123775 + -1.4186187406301717e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 508568 + 1.4186187406301717e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 250377 + -1.4186187406301717e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 8.8982142508029938e-02 + 2.8121381998062134e-01 + <_> + 2 + 210086 + -1.4186187406301717e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 40291 + 1.4186187406301717e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.2434725271574262e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.1820492744445801e-01 + 4.8178554534912109e+01 + <_> + 1 + 264011 + 1.2434725271574262e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 2.0129428803920746e-01 + 1.2525884628295898e+01 + <_> + 2 + 235163 + 1.2434725271574262e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 28848 + -1.2434725271574262e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 618709 + -1.2434725271574262e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 3.2975253462791443e-01 + 1.2650000000000000e+02 + <_> + 2 + 10805 + 1.2434725271574262e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 607904 + -1.2434725271574262e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.6547437538510612e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 5.1948291063308716e-01 + 2.1972222328186035e+00 + <_> + 1 + 383085 + -1.6547437538510612e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 1.9222603738307953e-01 + 2.1615540981292725e-01 + <_> + 2 + 203432 + -1.6547437538510612e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 179653 + 1.6547437538510612e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 499635 + 1.6547437538510612e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 3.4904840588569641e-01 + 1.5528797912597656e+02 + <_> + 2 + 82677 + -1.6547437538510612e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 416958 + 1.6547437538510612e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -9.8896280736352746e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>13 + 5.1965743303298950e-01 + 6.4293746948242188e+00 + <_> + 1 + 17091 + 9.8896280736352746e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 2.4607667699456215e-02 + 5.4762855529785156e+01 + <_> + 2 + 12531 + -9.8896280736352746e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 4560 + 9.8896280736352746e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 865629 + -9.8896280736352746e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 5.0009626150131226e-01 + 1.0243181228637695e+01 + <_> + 2 + 710778 + -9.8896280736352746e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 154851 + 9.8896280736352746e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 9.9199441656269274e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 5.1254796981811523e-01 + 2.3613871765136719e+02 + <_> + 1 + 625587 + 9.9199441656269274e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 3.1762367486953735e-01 + 6.0726833343505859e+01 + <_> + 2 + 402724 + 9.9199441656269274e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 222863 + -9.9199441656269274e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 257133 + -9.9199441656269274e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 2.0715585350990295e-01 + 4.3361110687255859e+00 + <_> + 2 + 155581 + -9.9199441656269274e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 101552 + 9.9199441656269274e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.1442218518604311e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 5.1460164785385132e-01 + 1.0711774826049805e+00 + <_> + 1 + 277502 + 1.1442218518604311e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 1.8278945982456207e-01 + 4.1967071533203125e+01 + <_> + 2 + 169931 + -1.1442218518604311e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 107571 + 1.1442218518604311e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 605218 + -1.1442218518604311e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 3.4578493237495422e-01 + 1.0406154785156250e+03 + <_> + 2 + 111686 + 1.1442218518604311e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 493532 + -1.1442218518604311e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.6530445102620753e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 5.1629894971847534e-01 + 2.2140160202980042e-01 + <_> + 1 + 633777 + 1.6530445102620753e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 3.6336201429367065e-01 + 8.2161560058593750e+01 + <_> + 2 + 602232 + 1.6530445102620753e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 31545 + -1.6530445102620753e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 248943 + -1.6530445102620753e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 1.7787025868892670e-01 + 1.1409013748168945e+01 + <_> + 2 + 191862 + -1.6530445102620753e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 57081 + 1.6530445102620753e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.2533726342779244e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.2247959375381470e-01 + 3.5487310791015625e+02 + <_> + 1 + 52318 + 1.2533726342779244e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>3 + 4.0325526148080826e-02 + 3.7840070724487305e+00 + <_> + 2 + 42754 + -1.2533726342779244e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 9564 + 1.2533726342779244e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 830402 + -1.2533726342779244e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 4.9096783995628357e-01 + 8.6163131713867188e+01 + <_> + 2 + 53683 + 1.2533726342779244e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 776719 + -1.2533726342779244e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.3342783486076615e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 5.1667976379394531e-01 + 1.3747453689575195e-01 + <_> + 1 + 111178 + -1.3342783486076615e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 7.3850348591804504e-02 + 1.2986158370971680e+01 + <_> + 2 + 94569 + -1.3342783486076615e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 16609 + 1.3342783486076615e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 771542 + 1.3342783486076615e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 4.5945721864700317e-01 + 9.6585571289062500e+01 + <_> + 2 + 633155 + 1.3342783486076615e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 138387 + -1.3342783486076615e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.3932167099759948e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 5.1923316717147827e-01 + 1.6650000000000000e+02 + <_> + 1 + 454525 + -1.3932167099759948e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 3.1775084137916565e-01 + 1.0295486450195312e+01 + <_> + 2 + 354294 + -1.3932167099759948e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 100231 + 1.3932167099759948e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 428195 + 1.3932167099759948e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 2.1702334284782410e-01 + 2.8572428226470947e-01 + <_> + 2 + 281597 + 1.3932167099759948e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 146598 + -1.3932167099759948e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.2699821808608344e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 5.1649564504623413e-01 + 1.0375097274780273e+01 + <_> + 1 + 489332 + 1.2699821808608344e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 2.3156504333019257e-01 + 7.2211818695068359e+00 + <_> + 2 + 350175 + 1.2699821808608344e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 139157 + -1.2699821808608344e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 393388 + -1.2699821808608344e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 3.0014190077781677e-01 + 3.1498894691467285e+00 + <_> + 2 + 234478 + -1.2699821808608344e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 158910 + 1.2699821808608344e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.2906877916375525e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 5.1586192846298218e-01 + 1.7850000000000000e+02 + <_> + 1 + 558651 + -1.2906877916375525e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 3.5671445727348328e-01 + 1.5682178735733032e-01 + <_> + 2 + 109675 + 1.2906877916375525e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 448976 + -1.2906877916375525e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 324069 + 1.2906877916375525e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 1.7550800740718842e-01 + 1.0139340209960938e+02 + <_> + 2 + 208285 + -1.2906877916375525e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 115784 + 1.2906877916375525e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.3514393109725334e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 5.2081906795501709e-01 + 1.7144098877906799e-01 + <_> + 1 + 360361 + -1.3514393109725334e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 2.0496743917465210e-01 + 9.6172618865966797e-01 + <_> + 2 + 52202 + 1.3514393109725334e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 308159 + -1.3514393109725334e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 522359 + 1.3514393109725334e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 3.2876721024513245e-01 + 9.6365535259246826e-01 + <_> + 2 + 74583 + -1.3514393109725334e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 447776 + 1.3514393109725334e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.2172172264139647e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 5.1405769586563110e-01 + 1.6650000000000000e+02 + <_> + 1 + 454525 + -1.2172172264139647e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 3.1545063853263855e-01 + 4.6819731445312500e+03 + <_> + 2 + 352225 + -1.2172172264139647e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 102300 + 1.2172172264139647e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 428195 + 1.2172172264139647e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 2.1494227647781372e-01 + 1.1416563034057617e+01 + <_> + 2 + 200313 + 1.2172172264139647e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 227882 + -1.2172172264139647e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.2600796274905465e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 5.2045398950576782e-01 + 1.2309650331735611e-01 + <_> + 1 + 47736 + -1.2600796274905465e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 5.8143828064203262e-02 + 3.3083333969116211e+00 + <_> + 2 + 27022 + 1.2600796274905465e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 20714 + -1.2600796274905465e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 834984 + 1.2600796274905465e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 4.7331655025482178e-01 + 3.1864631175994873e-01 + <_> + 2 + 812720 + 1.2600796274905465e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 22264 + -1.2600796274905465e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.3582731676039014e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 5.2070939540863037e-01 + 3.0843707919120789e-01 + <_> + 1 + 650530 + -1.3582731676039014e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 4.6237590909004211e-01 + 6.5716323852539062e+01 + <_> + 2 + 550266 + -1.3582731676039014e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 100264 + 1.3582731676039014e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 232190 + 1.3582731676039014e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 7.1528807282447815e-02 + 9.3056854248046875e+01 + <_> + 2 + 194278 + -1.3582731676039014e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 37912 + 1.3582731676039014e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.2445679874803793e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 5.2153694629669189e-01 + 7.1541847229003906e+01 + <_> + 1 + 754979 + 1.2445679874803793e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 4.1397175192832947e-01 + 1.9902000427246094e+01 + <_> + 2 + 712462 + 1.2445679874803793e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 42517 + -1.2445679874803793e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 127741 + -1.2445679874803793e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 1.1710235476493835e-01 + 2.4950000000000000e+02 + <_> + 2 + 116484 + -1.2445679874803793e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 11257 + 1.2445679874803793e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -8.9580309535675029e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.1865381002426147e-01 + 1.1197845458984375e+01 + <_> + 1 + 11995 + 8.9580309535675029e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 1.1759687215089798e-02 + 3.5040259361267090e-01 + <_> + 2 + 4044 + 8.9580309535675029e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 7951 + -8.9580309535675029e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 870725 + -8.9580309535675029e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 5.1062041521072388e-01 + 3.0947694182395935e-01 + <_> + 2 + 649788 + -8.9580309535675029e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 220937 + 8.9580309535675029e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.1465877153533668e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.0886815786361694e-01 + 5.5216079711914062e+01 + <_> + 1 + 679431 + -1.1465877153533668e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 3.5245060920715332e-01 + 8.4106079101562500e+02 + <_> + 2 + 291103 + 1.1465877153533668e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 388328 + -1.1465877153533668e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 203289 + 1.1465877153533668e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 1.7618271708488464e-01 + 1.9848366081714630e-01 + <_> + 2 + 118241 + 1.1465877153533668e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 85048 + -1.1465877153533668e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.3405075039626771e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 5.2102065086364746e-01 + 1.9638888835906982e+00 + <_> + 1 + 333415 + -1.3405075039626771e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 1.5986835956573486e-01 + 7.6529556274414062e+01 + <_> + 2 + 309625 + -1.3405075039626771e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 23790 + 1.3405075039626771e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 549305 + 1.3405075039626771e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 3.7359422445297241e-01 + 3.0250942230224609e+01 + <_> + 2 + 56683 + -1.3405075039626771e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 492622 + 1.3405075039626771e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.3356169959232195e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.2158385515213013e-01 + 3.9147827148437500e+01 + <_> + 1 + 484958 + 1.3356169959232195e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 2.0946808159351349e-01 + 4.0066700428724289e-02 + <_> + 2 + 255407 + 1.3356169959232195e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 229551 + -1.3356169959232195e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 397762 + -1.3356169959232195e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 3.2387277483940125e-01 + 9.6250331401824951e-01 + <_> + 2 + 67228 + 1.3356169959232195e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 330534 + -1.3356169959232195e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.2080363330393365e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 5.1479589939117432e-01 + 4.6513631939888000e-02 + <_> + 1 + 540095 + -1.2080363330393365e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 3.8127714395523071e-01 + 1.8750000000000000e+02 + <_> + 2 + 438424 + -1.2080363330393365e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 101671 + 1.2080363330393365e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 342625 + 1.2080363330393365e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 1.4888709783554077e-01 + 1.6247725486755371e-01 + <_> + 2 + 89629 + -1.2080363330393365e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 252996 + 1.2080363330393365e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.1292790823387461e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 5.1889324188232422e-01 + 4.5916666984558105e+00 + <_> + 1 + 734914 + 1.1292790823387461e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 4.1940683126449585e-01 + 7.8092810058593750e+02 + <_> + 2 + 83484 + -1.1292790823387461e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 651430 + 1.1292790823387461e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 147806 + -1.1292790823387461e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 1.0879517346620560e-01 + 1.2507208824157715e+01 + <_> + 2 + 30425 + 1.1292790823387461e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 117381 + -1.1292790823387461e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -6.6348746565019420e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 5.1353782415390015e-01 + 3.5277777910232544e-01 + <_> + 1 + 2224 + 6.6348746565019420e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 7.9937055706977844e-03 + 2.3396150209009647e-03 + <_> + 2 + 897 + -6.6348746565019420e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 1327 + 6.6348746565019420e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 880496 + -6.6348746565019420e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.0858741998672485e-01 + 7.9081420898437500e+01 + <_> + 2 + 852992 + -6.6348746565019420e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 27504 + 6.6348746565019420e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.0380346754995637e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.1207786798477173e-01 + 2.0558071136474609e+01 + <_> + 1 + 146798 + -1.0380346754995637e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 5.3757350891828537e-02 + 1.7893083393573761e-02 + <_> + 2 + 14082 + 1.0380346754995637e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 132716 + -1.0380346754995637e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 735922 + 1.0380346754995637e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 4.7217023372650146e-01 + 8.0500000000000000e+01 + <_> + 2 + 2198 + -1.0380346754995637e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 733724 + 1.0380346754995637e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.2402129775755640e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.2029514312744141e-01 + 3.5403479003906250e+02 + <_> + 1 + 51950 + 1.2402129775755640e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>3 + 4.0265109390020370e-02 + 3.7840070724487305e+00 + <_> + 2 + 42525 + -1.2402129775755640e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 9425 + 1.2402129775755640e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 830770 + -1.2402129775755640e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 4.9070054292678833e-01 + 9.8776817321777344e-01 + <_> + 2 + 818469 + -1.2402129775755640e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 12301 + 1.2402129775755640e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.1174451569061232e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>8 + 5.1771777868270874e-01 + 1.0950000000000000e+02 + <_> + 1 + 861404 + 1.1174451569061232e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 4.9417373538017273e-01 + 3.3225021362304688e+02 + <_> + 2 + 39399 + -1.1174451569061232e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 822005 + 1.1174451569061232e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 21316 + -1.1174451569061232e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 3.3733375370502472e-02 + 9.7636735439300537e-01 + <_> + 2 + 14433 + -1.1174451569061232e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 6883 + 1.1174451569061232e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -9.2008834215581078e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.1710635423660278e-01 + 3.5403479003906250e+02 + <_> + 1 + 51950 + 9.2008834215581078e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>3 + 3.6475311964750290e-02 + 3.7840070724487305e+00 + <_> + 2 + 42525 + -9.2008834215581078e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 9425 + 9.2008834215581078e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 830770 + -9.2008834215581078e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 4.8651069402694702e-01 + 5.3055932617187500e+02 + <_> + 2 + 5664 + 9.2008834215581078e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 825106 + -9.2008834215581078e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.1206244302541971e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 5.1573443412780762e-01 + 1.7063159942626953e+01 + <_> + 1 + 813488 + 1.1206244302541971e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 4.9491870403289795e-01 + 9.8200012207031250e+01 + <_> + 2 + 777577 + 1.1206244302541971e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 35911 + -1.1206244302541971e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 69232 + -1.1206244302541971e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 3.3067610114812851e-02 + 9.6073269844055176e-01 + <_> + 2 + 62879 + -1.1206244302541971e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 6353 + 1.1206244302541971e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -9.0745143780190440e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>1 + 5.1739364862442017e-01 + 2.6975215911865234e+01 + <_> + 1 + 13849 + 9.0745143780190440e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 2.1915556862950325e-02 + 9.7495067119598389e-01 + <_> + 2 + 1233 + -9.0745143780190440e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 12616 + 9.0745143780190440e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 868871 + -9.0745143780190440e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>13 + 5.0075519084930420e-01 + 1.1753937530517578e+02 + <_> + 2 + 834898 + -9.0745143780190440e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 33973 + 9.0745143780190440e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 8.7684481634476105e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 5.1117843389511108e-01 + 9.6944439411163330e-01 + <_> + 1 + 135836 + -8.7684481634476105e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>10 + 5.1922481507062912e-02 + 3.9914073944091797e+00 + <_> + 2 + 8769 + 8.7684481634476105e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 127067 + -8.7684481634476105e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 746884 + 8.7684481634476105e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 4.6998462080955505e-01 + 7.1500000000000000e+01 + <_> + 2 + 1265 + -8.7684481634476105e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 745619 + 8.7684481634476105e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.1418394962738621e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.1731228828430176e-01 + 3.8520984649658203e+01 + <_> + 1 + 146123 + 1.1418394962738621e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 1.1987941712141037e-01 + 9.8408565521240234e+00 + <_> + 2 + 128451 + 1.1418394962738621e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 17672 + -1.1418394962738621e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 736597 + -1.1418394962738621e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 4.0863558650016785e-01 + 8.3221206665039062e+00 + <_> + 2 + 58072 + 1.1418394962738621e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 678525 + -1.1418394962738621e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.3642321939419808e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.1853173971176147e-01 + 3.3794467926025391e+01 + <_> + 1 + 396536 + -1.3642321939419808e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 1.6062475740909576e-01 + 1.1050000000000000e+02 + <_> + 2 + 62779 + 1.3642321939419808e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 333757 + -1.3642321939419808e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 486184 + 1.3642321939419808e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 3.7342825531959534e-01 + 9.2590942382812500e+00 + <_> + 2 + 23187 + -1.3642321939419808e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 462997 + 1.3642321939419808e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -8.8934439666397608e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 5.2081781625747681e-01 + 5.3055932617187500e+02 + <_> + 1 + 6899 + 8.8934439666397608e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 2.3901364766061306e-03 + 3.6409476399421692e-01 + <_> + 2 + 597 + 8.8934439666397608e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 6302 + -8.8934439666397608e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 875821 + -8.8934439666397608e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 5.1982885599136353e-01 + 7.4083333015441895e+00 + <_> + 2 + 851176 + -8.8934439666397608e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 24645 + 8.8934439666397608e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 9.5080199393547421e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>13 + 5.1209837198257446e-01 + 1.1065624237060547e+01 + <_> + 1 + 68321 + -9.5080199393547421e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 4.8200789839029312e-02 + 9.7500002384185791e-01 + <_> + 2 + 61259 + -9.5080199393547421e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 7062 + 9.5080199393547421e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 814399 + 9.5080199393547421e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 4.7555136680603027e-01 + 3.5518403320312500e+03 + <_> + 2 + 543109 + 9.5080199393547421e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 271290 + -9.5080199393547421e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.3670565874284332e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 5.1976078748703003e-01 + 1.5997242927551270e-01 + <_> + 1 + 274643 + 1.3670565874284332e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 1.6347302496433258e-01 + 7.3014312744140625e+01 + <_> + 2 + 264903 + 1.3670565874284332e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 9740 + -1.3670565874284332e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 608077 + -1.3670565874284332e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 3.7065026164054871e-01 + 5.6768994140625000e+03 + <_> + 2 + 596819 + -1.3670565874284332e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 11258 + 1.3670565874284332e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.2696884783743767e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 5.1723891496658325e-01 + 7.1441132812500000e+03 + <_> + 1 + 797886 + 1.2696884783743767e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 4.6492782235145569e-01 + 3.4368904113769531e+01 + <_> + 2 + 730767 + 1.2696884783743767e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 67119 + -1.2696884783743767e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 84834 + -1.2696884783743767e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 6.6771805286407471e-02 + 2.4362087249755859e+00 + <_> + 2 + 12567 + 1.2696884783743767e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 72267 + -1.2696884783743767e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.1929965025037552e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>13 + 5.1753205060958862e-01 + 9.6618125915527344e+01 + <_> + 1 + 794801 + -1.1929965025037552e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 4.4954901933670044e-01 + 3.0843707919120789e-01 + <_> + 2 + 579663 + -1.1929965025037552e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 215138 + 1.1929965025037552e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 87919 + 1.1929965025037552e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 8.0240584909915924e-02 + 2.2746036946773529e-01 + <_> + 2 + 73757 + 1.1929965025037552e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 14162 + -1.1929965025037552e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.0860662705279479e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.1622432470321655e-01 + 2.3423248291015625e+01 + <_> + 1 + 194257 + -1.0860662705279479e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 7.1898259222507477e-02 + 2.3958118438720703e+01 + <_> + 2 + 21625 + 1.0860662705279479e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 172632 + -1.0860662705279479e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 688463 + 1.0860662705279479e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 4.5522674918174744e-01 + 7.1500000000000000e+01 + <_> + 2 + 945 + -1.0860662705279479e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 687518 + 1.0860662705279479e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.1725763723154785e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.1875513792037964e-01 + 4.4602760314941406e+01 + <_> + 1 + 218350 + 1.1725763723154785e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 1.7011776566505432e-01 + 1.4642375946044922e+01 + <_> + 2 + 53499 + -1.1725763723154785e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 164851 + 1.1725763723154785e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 664370 + -1.1725763723154785e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 3.5916310548782349e-01 + 1.2150000000000000e+02 + <_> + 2 + 11407 + 1.1725763723154785e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 652963 + -1.1725763723154785e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.1167648010132951e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 5.1820802688598633e-01 + 1.3650000000000000e+02 + <_> + 1 + 210910 + -1.1167648010132951e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 1.9387055933475494e-01 + 3.5577264404296875e+02 + <_> + 2 + 52544 + 1.1167648010132951e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 158366 + -1.1167648010132951e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 671810 + 1.1167648010132951e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 3.3401957154273987e-01 + 4.0920783996582031e+01 + <_> + 2 + 329307 + -1.1167648010132951e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 342503 + 1.1167648010132951e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.2505078408336831e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 5.1260077953338623e-01 + 1.6905261993408203e+01 + <_> + 1 + 524449 + 1.2505078408336831e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>13 + 2.9245281219482422e-01 + 1.9444374084472656e+01 + <_> + 2 + 135667 + -1.2505078408336831e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 388782 + 1.2505078408336831e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 358271 + -1.2505078408336831e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 2.3876920342445374e-01 + 4.2080000042915344e-02 + <_> + 2 + 75985 + 1.2505078408336831e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 282286 + -1.2505078408336831e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.1146531669964642e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.1469743251800537e-01 + 3.8520984649658203e+01 + <_> + 1 + 146123 + 1.1146531669964642e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 1.2118757516145706e-01 + 3.3219573974609375e+02 + <_> + 2 + 40282 + -1.1146531669964642e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 105841 + 1.1146531669964642e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 736597 + -1.1146531669964642e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 4.0664994716644287e-01 + 6.0706512451171875e+01 + <_> + 2 + 594161 + -1.1146531669964642e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 142436 + 1.1146531669964642e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.4722421523437013e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.1222920417785645e-01 + 4.8905628204345703e+01 + <_> + 1 + 600969 + 1.4722421523437013e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 3.1731200218200684e-01 + 1.9359523057937622e-01 + <_> + 2 + 114683 + -1.4722421523437013e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 486286 + 1.4722421523437013e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 281751 + -1.4722421523437013e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 2.1942771971225739e-01 + 4.8183624267578125e+01 + <_> + 2 + 40157 + 1.4722421523437013e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 241594 + -1.4722421523437013e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.1909982435381317e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 5.1740634441375732e-01 + 1.1911777496337891e+02 + <_> + 1 + 260790 + -1.1909982435381317e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 9.8409734666347504e-02 + 1.0450000000000000e+02 + <_> + 2 + 44866 + 1.1909982435381317e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 215924 + -1.1909982435381317e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 621930 + 1.1909982435381317e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 4.3133008480072021e-01 + 9.1517004394531250e+02 + <_> + 2 + 41408 + -1.1909982435381317e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 580522 + 1.1909982435381317e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.1359715151968533e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 5.1759630441665649e-01 + 4.6553656458854675e-02 + <_> + 1 + 541029 + -1.1359715151968533e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 3.8337594270706177e-01 + 2.2750000000000000e+02 + <_> + 2 + 526042 + -1.1359715151968533e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 14987 + 1.1359715151968533e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 341691 + 1.1359715151968533e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 1.4499284327030182e-01 + 1.3962946777343750e+03 + <_> + 2 + 148893 + 1.1359715151968533e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 192798 + -1.1359715151968533e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.2322219910166020e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 5.1454466581344604e-01 + 6.3750000000000000e+00 + <_> + 1 + 829136 + 1.2322219910166020e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 4.9534726142883301e-01 + 9.6182703971862793e-01 + <_> + 2 + 68783 + -1.2322219910166020e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 760353 + 1.2322219910166020e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 53584 + -1.2322219910166020e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 3.5419374704360962e-02 + 9.6181493997573853e-01 + <_> + 2 + 45768 + 1.2322219910166020e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 7816 + -1.2322219910166020e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.1781178830356834e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>13 + 5.2268415689468384e-01 + 1.2721937561035156e+02 + <_> + 1 + 858625 + -1.1781178830356834e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 4.9915239214897156e-01 + 1.0780788421630859e+01 + <_> + 2 + 9261 + 1.1781178830356834e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 849364 + -1.1781178830356834e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 24095 + 1.1781178830356834e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 3.0266525223851204e-02 + 2.6119586825370789e-01 + <_> + 2 + 13317 + -1.1781178830356834e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 10778 + 1.1781178830356834e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.2950340294746163e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 5.1489472389221191e-01 + 1.9693386554718018e-01 + <_> + 1 + 286193 + 1.2950340294746163e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 2.3451860249042511e-01 + 8.4089599609375000e+01 + <_> + 2 + 266079 + 1.2950340294746163e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 20114 + -1.2950340294746163e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 596527 + -1.2950340294746163e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 2.9781207442283630e-01 + 3.5527777671813965e+00 + <_> + 2 + 466581 + -1.2950340294746163e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 129946 + 1.2950340294746163e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.3382719657511438e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.1340484619140625e-01 + 3.9127510070800781e+01 + <_> + 1 + 484708 + 1.3382719657511438e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 2.1581776440143585e-01 + 2.3972038574218750e+03 + <_> + 2 + 338458 + 1.3382719657511438e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 146250 + -1.3382719657511438e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 398012 + -1.3382719657511438e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 3.1758919358253479e-01 + 9.9669523239135742e+00 + <_> + 2 + 119203 + 1.3382719657511438e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 278809 + -1.3382719657511438e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.3052820589276917e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 5.1961916685104370e-01 + 1.2550000000000000e+02 + <_> + 1 + 142648 + -1.3052820589276917e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 1.4402635395526886e-01 + 3.3239997923374176e-02 + <_> + 2 + 99101 + -1.3052820589276917e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 43547 + 1.3052820589276917e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 740072 + 1.3052820589276917e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 3.8855946063995361e-01 + 7.7380010986328125e+02 + <_> + 2 + 65023 + -1.3052820589276917e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 675049 + 1.3052820589276917e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.1143963889842658e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.1930880546569824e-01 + 3.7608242034912109e+01 + <_> + 1 + 135299 + 1.1143963889842658e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 1.1370519548654556e-01 + 1.4642375946044922e+01 + <_> + 2 + 48444 + -1.1143963889842658e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 86855 + 1.1143963889842658e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 747421 + -1.1143963889842658e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 4.1412591934204102e-01 + 2.7229607105255127e-02 + <_> + 2 + 95577 + 1.1143963889842658e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 651844 + -1.1143963889842658e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.5217408284279793e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 5.1494979858398438e-01 + 9.6397743225097656e+00 + <_> + 1 + 399063 + -1.5217408284279793e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 1.8638405203819275e-01 + 7.0999298095703125e+00 + <_> + 2 + 126262 + 1.5217408284279793e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 272801 + -1.5217408284279793e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 483657 + 1.5217408284279793e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 3.5158622264862061e-01 + 2.9056789398193359e+01 + <_> + 2 + 67660 + -1.5217408284279793e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 415997 + 1.5217408284279793e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -9.4635032346753689e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.2099221944808960e-01 + 1.1370868682861328e+01 + <_> + 1 + 13294 + 9.4635032346753689e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 1.1681082658469677e-02 + 2.3396150209009647e-03 + <_> + 2 + 1022 + -9.4635032346753689e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 12272 + 9.4635032346753689e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 869426 + -9.4635032346753689e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.1196002960205078e-01 + 7.9061347961425781e+01 + <_> + 2 + 841854 + -9.4635032346753689e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 27572 + 9.4635032346753689e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 9.4711133362356342e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 5.1325416564941406e-01 + 5.2972221374511719e+00 + <_> + 1 + 788170 + 9.4711133362356342e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 4.6157640218734741e-01 + 5.0363531494140625e+02 + <_> + 2 + 129946 + -9.4711133362356342e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 658224 + 9.4711133362356342e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 94550 + -9.4711133362356342e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 6.2083713710308075e-02 + 2.5150000000000000e+02 + <_> + 2 + 84156 + -9.4711133362356342e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 10394 + 9.4711133362356342e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -9.4493641747558574e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 5.1660895347595215e-01 + 2.4264328002929688e+01 + <_> + 1 + 99637 + 9.4493641747558574e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 4.4669870287179947e-02 + 1.5394971847534180e+01 + <_> + 2 + 63376 + -9.4493641747558574e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 36261 + 9.4493641747558574e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 783083 + -9.4493641747558574e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 4.7893598675727844e-01 + 3.8409677124023438e+02 + <_> + 2 + 13669 + 9.4493641747558574e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 769414 + -9.4493641747558574e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.0268576004758100e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 5.1545894145965576e-01 + 9.5613986253738403e-01 + <_> + 1 + 73738 + -1.0268576004758100e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 2.4481346830725670e-02 + 4.5859130859375000e+01 + <_> + 2 + 60302 + -1.0268576004758100e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 13436 + 1.0268576004758100e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 808982 + 1.0268576004758100e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 5.0116753578186035e-01 + 6.4861106872558594e+00 + <_> + 2 + 793030 + 1.0268576004758100e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 15952 + -1.0268576004758100e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -8.7415687815332127e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 5.1738774776458740e-01 + 2.0639503002166748e+00 + <_> + 1 + 13904 + 8.7415687815332127e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 2.1813781931996346e-02 + 1.9439224243164062e+01 + <_> + 2 + 5661 + -8.7415687815332127e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 8243 + 8.7415687815332127e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 868816 + -8.7415687815332127e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.0002622604370117e-01 + 7.1283096313476562e+01 + <_> + 2 + 797538 + -8.7415687815332127e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 71278 + 8.7415687815332127e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 9.0927238798277743e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 5.0975197553634644e-01 + 1.2518388032913208e+00 + <_> + 1 + 377084 + 9.0927238798277743e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 2.0931947231292725e-01 + 4.1524017333984375e+01 + <_> + 2 + 93330 + -9.0927238798277743e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 283754 + 9.0927238798277743e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 505636 + -9.0927238798277743e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 3.1339669227600098e-01 + 4.4432319641113281e+01 + <_> + 2 + 112515 + 9.0927238798277743e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 393121 + -9.0927238798277743e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 9.5052653794233957e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 5.1104712486267090e-01 + 3.2332382202148438e+01 + <_> + 1 + 120080 + -9.5052653794233957e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 7.5593367218971252e-02 + 6.7714500427246094e+00 + <_> + 2 + 53841 + 9.5052653794233957e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 66239 + -9.5052653794233957e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 762640 + 9.5052653794233957e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 4.4815191626548767e-01 + 7.5972385406494141e+00 + <_> + 2 + 87136 + -9.5052653794233957e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 675504 + 9.5052653794233957e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -7.5071076731170652e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 5.1666319370269775e-01 + 3.5277777910232544e-01 + <_> + 1 + 2224 + 7.5071076731170652e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 8.0808280035853386e-03 + 2.6486915349960327e-01 + <_> + 2 + 1298 + 7.5071076731170652e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 926 + -7.5071076731170652e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 880496 + -7.5071076731170652e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.1067811250686646e-01 + 1.0946619033813477e+01 + <_> + 2 + 8232 + 7.5071076731170652e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 872264 + -7.5071076731170652e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 9.4623998779569909e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 5.1043331623077393e-01 + 5.3959779441356659e-02 + <_> + 1 + 683722 + 9.4623998779569909e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 4.6497312188148499e-01 + 1.4772460937500000e+03 + <_> + 2 + 185379 + -9.4623998779569909e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 498343 + 9.4623998779569909e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 198998 + -9.4623998779569909e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 5.8665242046117783e-02 + 2.4351483583450317e-01 + <_> + 2 + 130152 + -9.4623998779569909e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 68846 + 9.4623998779569909e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.2714328334082659e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 5.1731991767883301e-01 + 1.6297939453125000e+03 + <_> + 1 + 330856 + 1.2714328334082659e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 1.8274462223052979e-01 + 1.2698571777343750e+02 + <_> + 2 + 323734 + 1.2714328334082659e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 7122 + -1.2714328334082659e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 551864 + -1.2714328334082659e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 3.4899845719337463e-01 + 1.9487727880477905e+00 + <_> + 2 + 110726 + 1.2714328334082659e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 441138 + -1.2714328334082659e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.0029983928741941e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 5.1090341806411743e-01 + 1.9007104635238647e-01 + <_> + 1 + 257192 + 1.0029983928741941e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 2.1419817209243774e-01 + 3.8416666984558105e+00 + <_> + 2 + 170984 + 1.0029983928741941e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 86208 + -1.0029983928741941e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 625528 + -1.0029983928741941e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 3.1085580587387085e-01 + 9.4207134246826172e+00 + <_> + 2 + 267693 + 1.0029983928741941e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 357835 + -1.0029983928741941e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.2812338943322610e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 5.1928746700286865e-01 + 3.0527777671813965e+00 + <_> + 1 + 558604 + -1.2812338943322610e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 2.9665496945381165e-01 + 1.8450000000000000e+02 + <_> + 2 + 461479 + -1.2812338943322610e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 97125 + 1.2812338943322610e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 324116 + 1.2812338943322610e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 2.3533213138580322e-01 + 9.7587108612060547e-01 + <_> + 2 + 275955 + 1.2812338943322610e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 48161 + -1.2812338943322610e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.2207526287606150e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.1623684167861938e-01 + 1.3427116699218750e+03 + <_> + 1 + 572221 + 1.2207526287606150e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>1 + 3.1281876564025879e-01 + 8.1077964782714844e+01 + <_> + 2 + 470399 + 1.2207526287606150e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 101822 + -1.2207526287606150e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 310499 + -1.2207526287606150e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 2.1766221523284912e-01 + 2.0246419310569763e-01 + <_> + 2 + 233657 + -1.2207526287606150e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 76842 + 1.2207526287606150e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.4989269913710268e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.1366740465164185e-01 + 5.5216079711914062e+01 + <_> + 1 + 679431 + -1.4989269913710268e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 3.5601037740707397e-01 + 4.4606324285268784e-02 + <_> + 2 + 404899 + -1.4989269913710268e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 274532 + 1.4989269913710268e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 203289 + 1.4989269913710268e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 1.8139278888702393e-01 + 2.0816545188426971e-01 + <_> + 2 + 159252 + 1.4989269913710268e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 44037 + -1.4989269913710268e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.5342495351359042e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 5.1776731014251709e-01 + 4.0041230618953705e-02 + <_> + 1 + 399948 + 1.5342495351359042e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 3.0101346969604492e-01 + 6.6715942382812500e+01 + <_> + 2 + 381041 + 1.5342495351359042e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 18907 + -1.5342495351359042e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 482772 + -1.5342495351359042e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 2.3726768791675568e-01 + 3.0918169021606445e+00 + <_> + 2 + 364549 + -1.5342495351359042e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 118223 + 1.5342495351359042e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.4579421952377511e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 5.1463216543197632e-01 + 2.7181453704833984e+00 + <_> + 1 + 558384 + 1.4579421952377511e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 2.6393714547157288e-01 + 9.7053241729736328e-01 + <_> + 2 + 139368 + -1.4579421952377511e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 419016 + 1.4579421952377511e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 324336 + -1.4579421952377511e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 2.7244696021080017e-01 + 9.6973073482513428e-01 + <_> + 2 + 132600 + 1.4579421952377511e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 191736 + -1.4579421952377511e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -8.3675549513667263e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 5.1156282424926758e-01 + 2.3050000000000000e+02 + <_> + 1 + 827850 + -8.3675549513667263e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 4.7785559296607971e-01 + 5.6930437088012695e+00 + <_> + 2 + 552849 + -8.3675549513667263e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 275001 + 8.3675549513667263e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 54870 + 8.3675549513667263e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 4.3051108717918396e-02 + 9.6928584575653076e-01 + <_> + 2 + 30078 + 8.3675549513667263e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 24792 + -8.3675549513667263e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 9.3153440467758086e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 5.1211380958557129e-01 + 6.0000886917114258e+00 + <_> + 1 + 605670 + 9.3153440467758086e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 3.8052120804786682e-01 + 9.6944439411163330e-01 + <_> + 2 + 94691 + -9.3153440467758086e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 510979 + 9.3153440467758086e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 277050 + -9.3153440467758086e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 1.4275032281875610e-01 + 2.6701806640625000e+03 + <_> + 2 + 156682 + -9.3153440467758086e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 120368 + 9.3153440467758086e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -9.5184707513219904e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 5.1580941677093506e-01 + 1.0013697147369385e+00 + <_> + 1 + 38274 + 9.5184707513219904e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.0499700009822845e-02 + 4.9278511047363281e+01 + <_> + 2 + 29569 + 9.5184707513219904e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 8705 + -9.5184707513219904e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 844446 + -9.5184707513219904e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 4.7327852249145508e-01 + 2.7905413508415222e-01 + <_> + 2 + 777316 + -9.5184707513219904e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 67130 + 9.5184707513219904e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 9.1681486528081030e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 5.1230579614639282e-01 + 2.9147505760192871e-01 + <_> + 1 + 827055 + 9.1681486528081030e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 4.8605522513389587e-01 + 5.7274185121059418e-02 + <_> + 2 + 696762 + 9.1681486528081030e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 130293 + -9.1681486528081030e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 55665 + -9.1681486528081030e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 3.6849107593297958e-02 + 7.9624938964843750e+01 + <_> + 2 + 47805 + -9.1681486528081030e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 7860 + 9.1681486528081030e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.0179268045699244e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 5.1810514926910400e-01 + 3.9401649475097656e+01 + <_> + 1 + 847989 + -1.0179268045699244e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 4.8632714152336121e-01 + 8.0004062652587891e+00 + <_> + 2 + 780457 + -1.0179268045699244e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 67532 + 1.0179268045699244e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 34731 + 1.0179268045699244e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 3.9099089801311493e-02 + 1.7113932967185974e-01 + <_> + 2 + 13738 + -1.0179268045699244e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 20993 + 1.0179268045699244e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 9.9199627152149528e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 5.1473385095596313e-01 + 4.0854901123046875e+02 + <_> + 1 + 814521 + 9.9199627152149528e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 4.5232692360877991e-01 + 9.5607256889343262e-01 + <_> + 2 + 70215 + -9.9199627152149528e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 744306 + 9.9199627152149528e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 68199 + -9.9199627152149528e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 7.2452656924724579e-02 + 1.1904067039489746e+01 + <_> + 2 + 34953 + 9.9199627152149528e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 33246 + -9.9199627152149528e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.2045684120765200e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.1715165376663208e-01 + 1.9483010253906250e+03 + <_> + 1 + 748926 + -1.2045684120765200e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 4.2893403768539429e-01 + 1.6140855789184570e+01 + <_> + 2 + 731917 + -1.2045684120765200e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 17009 + 1.2045684120765200e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 133794 + 1.2045684120765200e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 1.0114382207393646e-01 + 2.2938682138919830e-01 + <_> + 2 + 119960 + 1.2045684120765200e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 13834 + -1.2045684120765200e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 9.8600859335020066e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.1442885398864746e-01 + 1.7187983398437500e+03 + <_> + 1 + 698956 + 9.8600859335020066e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 3.8903987407684326e-01 + 5.1110818982124329e-02 + <_> + 2 + 483455 + 9.8600859335020066e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 215501 + -9.8600859335020066e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 183764 + -9.8600859335020066e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 1.3559038937091827e-01 + 1.0698510742187500e+02 + <_> + 2 + 110229 + -9.8600859335020066e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 73535 + 9.8600859335020066e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.0013113568673283e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 5.1187586784362793e-01 + 1.1251853027343750e+03 + <_> + 1 + 203274 + 1.0013113568673283e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 8.9294776320457458e-02 + 7.5003746032714844e+01 + <_> + 2 + 153384 + -1.0013113568673283e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 49890 + 1.0013113568673283e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 679446 + -1.0013113568673283e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 4.3571710586547852e-01 + 3.5114007568359375e+02 + <_> + 2 + 30772 + 1.0013113568673283e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 648674 + -1.0013113568673283e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.0479083482852962e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 5.1722937822341919e-01 + 7.3068008422851562e+01 + <_> + 1 + 69470 + -1.0479083482852962e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 3.0421104282140732e-02 + 4.1849542236328125e+02 + <_> + 2 + 64489 + -1.0479083482852962e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 4981 + 1.0479083482852962e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 813250 + 1.0479083482852962e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 4.9575266242027283e-01 + 2.5513930664062500e+03 + <_> + 2 + 760523 + 1.0479083482852962e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 52727 + -1.0479083482852962e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -6.9944597027767702e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.1338863372802734e-01 + 8.1779647827148438e+01 + <_> + 1 + 863763 + -6.9944597027767702e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 5.0226563215255737e-01 + 3.5277777910232544e-01 + <_> + 2 + 2224 + 6.9944597027767702e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 861539 + -6.9944597027767702e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 18957 + 6.9944597027767702e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 1.5213403850793839e-02 + 1.1604920959472656e+02 + <_> + 2 + 16762 + 6.9944597027767702e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 2195 + -6.9944597027767702e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.0049008429189323e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 5.1192611455917358e-01 + 1.9285855293273926e+00 + <_> + 1 + 300532 + -1.0049008429189323e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 1.1127478629350662e-01 + 3.1663694977760315e-01 + <_> + 2 + 176414 + -1.0049008429189323e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 124118 + 1.0049008429189323e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 582188 + 1.0049008429189323e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 4.1382661461830139e-01 + 3.0425888299942017e-01 + <_> + 2 + 477731 + 1.0049008429189323e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 104457 + -1.0049008429189323e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.2855730349878802e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 5.1117056608200073e-01 + 2.6046544313430786e-01 + <_> + 1 + 513635 + -1.2855730349878802e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 3.6986562609672546e-01 + 9.6437197923660278e-01 + <_> + 2 + 109023 + 1.2855730349878802e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 404612 + -1.2855730349878802e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 369085 + 1.2855730349878802e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 1.6222952306270599e-01 + 2.5710353851318359e+00 + <_> + 2 + 292480 + 1.2855730349878802e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 76605 + -1.2855730349878802e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 9.5465057419031996e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 5.1730257272720337e-01 + 1.3940798118710518e-02 + <_> + 1 + 32627 + -9.5465057419031996e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 1.5295770950615406e-02 + 5.1016855239868164e+00 + <_> + 2 + 1597 + 9.5465057419031996e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 31030 + -9.5465057419031996e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 850093 + 9.5465057419031996e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 5.0855237245559692e-01 + 1.2303461879491806e-01 + <_> + 2 + 44974 + -9.5465057419031996e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 805119 + 9.5465057419031996e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -9.6415003328645679e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 5.1114720106124878e-01 + 3.7143341064453125e+01 + <_> + 1 + 158181 + 9.6415003328645679e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>13 + 1.0440933704376221e-01 + 1.9092498779296875e+01 + <_> + 2 + 91397 + -9.6415003328645679e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 66784 + 9.6415003328645679e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 724539 + -9.6415003328645679e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>1 + 4.1967576742172241e-01 + 4.5582500457763672e+01 + <_> + 2 + 28001 + 9.6415003328645679e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 696538 + -9.6415003328645679e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.0653500199646342e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 5.1474612951278687e-01 + 5.3066043853759766e+00 + <_> + 1 + 764408 + 1.0653500199646342e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 4.4804531335830688e-01 + 5.0363531494140625e+02 + <_> + 2 + 127459 + -1.0653500199646342e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 636949 + 1.0653500199646342e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 118312 + -1.0653500199646342e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 7.8563265502452850e-02 + 4.9014778137207031e+01 + <_> + 2 + 26161 + -1.0653500199646342e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 92151 + 1.0653500199646342e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -8.7227327944372962e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 5.1673740148544312e-01 + 4.0570430755615234e+00 + <_> + 1 + 76100 + 8.7227327944372962e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 5.7999830693006516e-02 + 8.1099443435668945e+00 + <_> + 2 + 66844 + 8.7227327944372962e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 9256 + -8.7227327944372962e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 806620 + -8.7227327944372962e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 4.6379318833351135e-01 + 2.3313360214233398e+01 + <_> + 2 + 29593 + 8.7227327944372962e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 777027 + -8.7227327944372962e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.0744457647570353e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 5.1406890153884888e-01 + 1.5870826721191406e+01 + <_> + 1 + 796112 + 1.0744457647570353e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 4.7454094886779785e-01 + 9.6592041015625000e+01 + <_> + 2 + 758653 + 1.0744457647570353e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 37459 + -1.0744457647570353e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 86608 + -1.0744457647570353e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.2294373512268066e-02 + 9.3383010864257812e+01 + <_> + 2 + 68632 + -1.0744457647570353e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 17976 + 1.0744457647570353e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -8.7475577555735706e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 5.1489800214767456e-01 + 2.3750000000000000e+02 + <_> + 1 + 842740 + -8.7475577555735706e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 4.9156761169433594e-01 + 4.9310840666294098e-02 + <_> + 2 + 585767 + -8.7475577555735706e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 256973 + 8.7475577555735706e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 39980 + 8.7475577555735706e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 3.0287345871329308e-02 + 2.1982169151306152e+00 + <_> + 2 + 16033 + -8.7475577555735706e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 23947 + 8.7475577555735706e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.0012120742820195e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 5.1017475128173828e-01 + 5.1415245980024338e-02 + <_> + 1 + 638031 + 1.0012120742820195e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 4.4468110799789429e-01 + 1.3216240234375000e+03 + <_> + 2 + 138468 + -1.0012120742820195e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 499563 + 1.0012120742820195e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 244689 + -1.0012120742820195e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 8.0328300595283508e-02 + 4.9735156250000000e+03 + <_> + 2 + 216089 + -1.0012120742820195e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 28600 + 1.0012120742820195e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.1139629904554423e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 5.1527249813079834e-01 + 1.3501220703125000e+03 + <_> + 1 + 263212 + 1.1139629904554423e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 1.2927815318107605e-01 + 1.1971948623657227e+01 + <_> + 2 + 216509 + -1.1139629904554423e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 46703 + 1.1139629904554423e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 619508 + -1.1139629904554423e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 3.9854216575622559e-01 + 6.7810668945312500e+01 + <_> + 2 + 533114 + -1.1139629904554423e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 86394 + 1.1139629904554423e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.2939202182366219e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 5.2019113302230835e-01 + 2.2457817077636719e+01 + <_> + 1 + 632209 + 1.2939202182366219e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 3.6560800671577454e-01 + 1.6281558990478516e+01 + <_> + 2 + 572511 + 1.2939202182366219e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 59698 + -1.2939202182366219e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 250511 + -1.2939202182366219e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 1.6669492423534393e-01 + 2.4156896770000458e-01 + <_> + 2 + 216330 + -1.2939202182366219e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 34181 + 1.2939202182366219e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.2250088306848443e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.1482892036437988e-01 + 1.6151179199218750e+03 + <_> + 1 + 669253 + -1.2250088306848443e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 3.7292891740798950e-01 + 1.4953508377075195e+01 + <_> + 2 + 654604 + -1.2250088306848443e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 14649 + 1.2250088306848443e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 213467 + 1.2250088306848443e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 1.5765807032585144e-01 + 1.5687054395675659e-01 + <_> + 2 + 35542 + -1.2250088306848443e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 177925 + 1.2250088306848443e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.1012913581323687e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>10 + 5.1693290472030640e-01 + 8.7880306243896484e+00 + <_> + 1 + 724584 + 1.1012913581323687e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 4.2123246192932129e-01 + 7.8584746093750000e+03 + <_> + 2 + 660016 + 1.1012913581323687e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 64568 + -1.1012913581323687e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 158136 + -1.1012913581323687e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 1.0627202689647675e-01 + 8.0757070312500000e+03 + <_> + 2 + 156262 + -1.1012913581323687e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 1874 + 1.1012913581323687e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -8.6335966027004080e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 5.1213514804840088e-01 + 8.7217148437500000e+03 + <_> + 1 + 832388 + -8.6335966027004080e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 4.8214641213417053e-01 + 6.3046508789062500e+01 + <_> + 2 + 714739 + -8.6335966027004080e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 117649 + 8.6335966027004080e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 50332 + 8.6335966027004080e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 3.9424180984497070e-02 + 1.2924060058593750e+03 + <_> + 2 + 31721 + 8.6335966027004080e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 18611 + -8.6335966027004080e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 9.5053473052294321e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 5.1496863365173340e-01 + 7.3706222534179688e+01 + <_> + 1 + 774473 + 9.5053473052294321e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 4.3096181750297546e-01 + 1.1797039031982422e+01 + <_> + 2 + 574908 + 9.5053473052294321e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 199565 + -9.5053473052294321e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 108247 + -9.5053473052294321e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 9.2783667147159576e-02 + 9.7360563278198242e-01 + <_> + 2 + 82765 + -9.5053473052294321e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 25482 + 9.5053473052294321e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.2486305892425398e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 5.1188904047012329e-01 + 1.2129903793334961e+01 + <_> + 1 + 651287 + -1.2486305892425398e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 3.3008763194084167e-01 + 1.2595489501953125e+02 + <_> + 2 + 630194 + -1.2486305892425398e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 21093 + 1.2486305892425398e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 231433 + 1.2486305892425398e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 2.0108763873577118e-01 + 4.2796148681640625e+02 + <_> + 2 + 204164 + 1.2486305892425398e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 27269 + -1.2486305892425398e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.4881791536970340e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 5.1729077100753784e-01 + 1.2525152206420898e+01 + <_> + 1 + 681291 + 1.4881791536970340e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 3.5587441921234131e-01 + 6.7825866699218750e+01 + <_> + 2 + 508813 + 1.4881791536970340e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 172478 + -1.4881791536970340e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 201429 + -1.4881791536970340e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 1.8126153945922852e-01 + 9.7179555892944336e-01 + <_> + 2 + 157846 + -1.4881791536970340e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 43583 + 1.4881791536970340e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.3984958175915035e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 5.2143442630767822e-01 + 3.5027778148651123e+00 + <_> + 1 + 632167 + -1.3984958175915035e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 3.5061496496200562e-01 + 6.6573921203613281e+01 + <_> + 2 + 583053 + -1.3984958175915035e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 49114 + 1.3984958175915035e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 250553 + 1.3984958175915035e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 1.8429054319858551e-01 + 1.2200248241424561e-01 + <_> + 2 + 16834 + -1.3984958175915035e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 233719 + 1.3984958175915035e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.1351189242547870e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>1 + 5.1673692464828491e-01 + 6.3547519683837891e+01 + <_> + 1 + 310597 + 1.1351189242547870e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 2.0332904160022736e-01 + 1.2332736968994141e+01 + <_> + 2 + 265157 + 1.1351189242547870e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 45440 + -1.1351189242547870e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 572123 + -1.1351189242547870e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>8 + 3.2501849532127380e-01 + 1.5000002384185791e+00 + <_> + 2 + 455827 + -1.1351189242547870e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 116296 + 1.1351189242547870e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.1620001547569191e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>8 + 5.1641446352005005e-01 + 2.5000002384185791e+00 + <_> + 1 + 709059 + 1.1620001547569191e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 4.0091326832771301e-01 + 9.1666994094848633e+00 + <_> + 2 + 291562 + -1.1620001547569191e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 417497 + 1.1620001547569191e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 173661 + -1.1620001547569191e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 1.2810410559177399e-01 + 9.7694289684295654e-01 + <_> + 2 + 98821 + -1.1620001547569191e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 74840 + 1.1620001547569191e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.0324061878595918e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 5.0883173942565918e-01 + 9.8828277587890625e+00 + <_> + 1 + 430648 + 1.0324061878595918e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 1.9811230897903442e-01 + 1.6950000000000000e+02 + <_> + 2 + 349264 + -1.0324061878595918e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 81384 + 1.0324061878595918e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 452072 + -1.0324061878595918e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 3.2767495512962341e-01 + 1.7250000000000000e+02 + <_> + 2 + 144198 + 1.0324061878595918e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 307874 + -1.0324061878595918e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.2464332048406863e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 5.1626908779144287e-01 + 9.8828277587890625e+00 + <_> + 1 + 430648 + 1.2464332048406863e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 2.0228776335716248e-01 + 1.9277486801147461e+00 + <_> + 2 + 155164 + -1.2464332048406863e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 275484 + 1.2464332048406863e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 452072 + -1.2464332048406863e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 3.2883277535438538e-01 + 7.3423736572265625e+01 + <_> + 2 + 250061 + -1.2464332048406863e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 202011 + 1.2464332048406863e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 8.3196641780681660e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 5.1272737979888916e-01 + 7.3706222534179688e+01 + <_> + 1 + 774473 + 8.3196641780681660e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 4.2957296967506409e-01 + 5.0050497055053711e-02 + <_> + 2 + 558698 + 8.3196641780681660e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 215775 + -8.3196641780681660e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 108247 + -8.3196641780681660e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 9.1214209794998169e-02 + 2.4950000000000000e+02 + <_> + 2 + 98447 + -8.3196641780681660e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 9800 + 8.3196641780681660e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -8.8709603077637519e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 5.1288670301437378e-01 + 1.1416666507720947e+00 + <_> + 1 + 171901 + 8.8709603077637519e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>13 + 6.7043520510196686e-02 + 1.1058124542236328e+01 + <_> + 2 + 64029 + -8.8709603077637519e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 107872 + 8.8709603077637519e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 710819 + -8.8709603077637519e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 4.5511934161186218e-01 + 7.3527774810791016e+00 + <_> + 2 + 684764 + -8.8709603077637519e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 26055 + 8.8709603077637519e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.0200757197105535e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>8 + 5.1430970430374146e-01 + 4.1500000000000000e+01 + <_> + 1 + 820638 + 1.0200757197105535e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 4.5283472537994385e-01 + 2.6412561416625977e+01 + <_> + 2 + 232669 + -1.0200757197105535e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 587969 + 1.0200757197105535e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 62082 + -1.0200757197105535e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 7.2645075619220734e-02 + 1.9568501710891724e+00 + <_> + 2 + 23617 + 1.0200757197105535e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 38465 + -1.0200757197105535e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -7.4146309958005197e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>1 + 5.1343142986297607e-01 + 3.9485031127929688e+01 + <_> + 1 + 82635 + 7.4146309958005197e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 6.5225981175899506e-02 + 8.2040596008300781e+00 + <_> + 2 + 72894 + 7.4146309958005197e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 9741 + -7.4146309958005197e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 800085 + -7.4146309958005197e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 4.5330211520195007e-01 + 1.2738451361656189e-01 + <_> + 2 + 58254 + 7.4146309958005197e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 741831 + -7.4146309958005197e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 8.0941260705440082e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 5.1312166452407837e-01 + 3.1864631175994873e-01 + <_> + 1 + 860456 + 8.0941260705440082e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 5.0408554077148438e-01 + 9.8062193393707275e-01 + <_> + 2 + 585265 + 8.0941260705440082e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 275191 + -8.0941260705440082e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 22264 + -8.0941260705440082e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 1.6138739883899689e-02 + 4.5250743865966797e+01 + <_> + 2 + 10751 + 8.0941260705440082e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 11513 + -8.0941260705440082e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -8.2499662417127792e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 5.1441407203674316e-01 + 1.6361110210418701e+00 + <_> + 1 + 262336 + 8.2499662417127792e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 1.1587233096361160e-01 + 2.8954421997070312e+01 + <_> + 2 + 230335 + 8.2499662417127792e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 32001 + -8.2499662417127792e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 620384 + -8.2499662417127792e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 4.0474089980125427e-01 + 5.8885707855224609e+00 + <_> + 2 + 1846 + 8.2499662417127792e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 618538 + -8.2499662417127792e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.1169926362945755e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>13 + 5.1363658905029297e-01 + 1.9444374084472656e+01 + <_> + 1 + 135667 + -1.1169926362945755e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 1.0936907678842545e-01 + 2.4883413314819336e-01 + <_> + 2 + 100537 + -1.1169926362945755e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 35130 + 1.1169926362945755e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 747053 + 1.1169926362945755e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 4.1852673888206482e-01 + 2.7778682708740234e+01 + <_> + 2 + 151031 + -1.1169926362945755e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 596022 + 1.1169926362945755e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -9.4600501300894715e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 5.1694715023040771e-01 + 2.9028301239013672e+01 + <_> + 1 + 153015 + 9.4600501300894715e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 7.4835188686847687e-02 + 9.2753009796142578e+00 + <_> + 2 + 122898 + 9.4600501300894715e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 30117 + -9.4600501300894715e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 729705 + -9.4600501300894715e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 4.4879731535911560e-01 + 7.4722218513488770e-01 + <_> + 2 + 8288 + 9.4600501300894715e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 721417 + -9.4600501300894715e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 9.5313568452163239e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.1234507560729980e-01 + 2.2868448257446289e+01 + <_> + 1 + 185346 + -9.5313568452163239e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 6.7804135382175446e-02 + 1.9476972520351410e-02 + <_> + 2 + 18767 + 9.5313568452163239e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 166579 + -9.5313568452163239e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 697374 + 9.5313568452163239e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 4.5600622892379761e-01 + 7.1500000000000000e+01 + <_> + 2 + 987 + -9.5313568452163239e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 696387 + 9.5313568452163239e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -6.8046149620195201e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 5.1556253433227539e-01 + 8.2806862890720367e-02 + <_> + 1 + 873894 + -6.8046149620195201e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 5.1484334468841553e-01 + 2.6995529174804688e+01 + <_> + 2 + 854266 + -6.8046149620195201e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 19628 + 6.8046149620195201e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 8826 + 6.8046149620195201e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 2.1616152953356504e-03 + 1.9699844360351562e+02 + <_> + 2 + 8772 + -6.8046149620195201e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 54 + 6.8046149620195201e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 8.1989988520674420e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.0836217403411865e-01 + 1.4196281738281250e+03 + <_> + 1 + 607419 + 8.1989988520674420e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 3.2993209362030029e-01 + 9.2327545166015625e+01 + <_> + 2 + 552735 + 8.1989988520674420e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 54684 + -8.1989988520674420e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 275301 + -8.1989988520674420e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 1.9055393338203430e-01 + 1.3535617828369141e+01 + <_> + 2 + 140629 + 8.1989988520674420e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 134672 + -8.1989988520674420e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.3612178894590560e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 5.1438689231872559e-01 + 9.7220790386199951e-01 + <_> + 1 + 329213 + 1.3612178894590560e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 2.3397213220596313e-01 + 4.1265846252441406e+01 + <_> + 2 + 98672 + -1.3612178894590560e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 230541 + 1.3612178894590560e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 553507 + -1.3612178894590560e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 3.0000588297843933e-01 + 1.3004627227783203e+01 + <_> + 2 + 522001 + -1.3612178894590560e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 31506 + 1.3612178894590560e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.4174359815885956e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.2310168743133545e-01 + 1.0694110107421875e+03 + <_> + 1 + 414849 + 1.4174359815885956e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 2.3964780569076538e-01 + 1.1876630783081055e+01 + <_> + 2 + 401949 + 1.4174359815885956e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 12900 + -1.4174359815885956e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 467871 + -1.4174359815885956e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 2.9572889208793640e-01 + 5.2797199249267578e+01 + <_> + 2 + 107182 + 1.4174359815885956e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 360689 + -1.4174359815885956e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.3976332347742679e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>10 + 5.1770704984664917e-01 + 7.1512346267700195e+00 + <_> + 1 + 382532 + -1.3976332347742679e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 2.7232521772384644e-01 + 1.5850000000000000e+02 + <_> + 2 + 333940 + -1.3976332347742679e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 48592 + 1.3976332347742679e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 500188 + 1.3976332347742679e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 2.6255884766578674e-01 + 5.1605654296875000e+03 + <_> + 2 + 416279 + 1.3976332347742679e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 83909 + -1.3976332347742679e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 9.2690956547639522e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.1453536748886108e-01 + 1.4193212890625000e+03 + <_> + 1 + 607300 + 9.2690956547639522e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>10 + 3.3013632893562317e-01 + 7.5242357254028320e+00 + <_> + 2 + 411486 + 9.2690956547639522e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 195814 + -9.2690956547639522e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 275420 + -9.2690956547639522e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 1.9301982223987579e-01 + 3.3393999934196472e-01 + <_> + 2 + 258051 + -9.2690956547639522e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 17369 + 9.2690956547639522e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.5512883816047315e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.1569175720214844e-01 + 5.1933715820312500e+01 + <_> + 1 + 639142 + -1.5512883816047315e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 3.4072896838188171e-01 + 2.2359392046928406e-01 + <_> + 2 + 432480 + -1.5512883816047315e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 206662 + 1.5512883816047315e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 243578 + 1.5512883816047315e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 1.9797566533088684e-01 + 2.0227992534637451e-01 + <_> + 2 + 152373 + 1.5512883816047315e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 91205 + -1.5512883816047315e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.4311757368526096e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 5.2287214994430542e-01 + 2.2418455779552460e-01 + <_> + 1 + 645245 + 1.4311757368526096e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 3.6794313788414001e-01 + 9.8246645927429199e-01 + <_> + 2 + 502389 + 1.4311757368526096e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 142856 + -1.4311757368526096e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 237475 + -1.4311757368526096e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 1.6777531802654266e-01 + 1.4800925292968750e+03 + <_> + 2 + 203522 + -1.4311757368526096e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 33953 + 1.4311757368526096e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.0740744894310113e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.1679742336273193e-01 + 8.4028808593750000e+02 + <_> + 1 + 290992 + 1.0740744894310113e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 1.5549302101135254e-01 + 9.7689026594161987e-01 + <_> + 2 + 39284 + -1.0740744894310113e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 251708 + 1.0740744894310113e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 591728 + -1.0740744894310113e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 3.7133306264877319e-01 + 6.9164237976074219e+00 + <_> + 2 + 13270 + 1.0740744894310113e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 578458 + -1.0740744894310113e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.1958057418300316e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 5.1416927576065063e-01 + 2.1694445610046387e+00 + <_> + 1 + 378470 + -1.1958057418300316e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 1.8806050717830658e-01 + 3.0148733407258987e-02 + <_> + 2 + 89410 + 1.1958057418300316e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 289060 + -1.1958057418300316e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 504250 + 1.1958057418300316e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 3.4179905056953430e-01 + 3.2689224243164062e+01 + <_> + 2 + 60260 + -1.1958057418300316e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 443990 + 1.1958057418300316e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -6.3881119860105612e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 5.1491260528564453e-01 + 4.8412844848632812e+02 + <_> + 1 + 2462 + 6.3881119860105612e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 1.3176739448681474e-03 + 3.6422818899154663e-01 + <_> + 2 + 121 + 6.3881119860105612e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 2341 + -6.3881119860105612e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 880258 + -6.3881119860105612e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 5.1464718580245972e-01 + 8.2806862890720367e-02 + <_> + 2 + 871475 + -6.3881119860105612e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 8783 + 6.3881119860105612e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 9.6650544007345948e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.1182425022125244e-01 + 1.4196281738281250e+03 + <_> + 1 + 607419 + 9.6650544007345948e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 3.3048462867736816e-01 + 3.3225021362304688e+02 + <_> + 2 + 40788 + -9.6650544007345948e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 566631 + 9.6650544007345948e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 275301 + -9.6650544007345948e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 1.9365923106670380e-01 + 1.8742108345031738e-01 + <_> + 2 + 179502 + -9.6650544007345948e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 95799 + 9.6650544007345948e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -9.5842179055065591e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 5.1347541809082031e-01 + 4.6553656458854675e-02 + <_> + 1 + 541029 + -9.5842179055065591e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 3.8547986745834351e-01 + 2.1850000000000000e+02 + <_> + 2 + 516230 + -9.5842179055065591e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 24799 + 9.5842179055065591e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 341691 + 9.5842179055065591e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 1.3846234977245331e-01 + 4.1255149841308594e+00 + <_> + 2 + 311353 + 9.5842179055065591e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 30338 + -9.5842179055065591e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 9.8390354061838686e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.1343107223510742e-01 + 7.6292663574218750e+01 + <_> + 1 + 679164 + 9.8390354061838686e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 3.9866673946380615e-01 + 2.4469029903411865e-01 + <_> + 2 + 540847 + 9.8390354061838686e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 138317 + -9.8390354061838686e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 203556 + -9.8390354061838686e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 1.2591102719306946e-01 + 4.2645394802093506e-02 + <_> + 2 + 14080 + 9.8390354061838686e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 189476 + -9.8390354061838686e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.1642999119343789e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.1199835538864136e-01 + 5.6772323608398438e+01 + <_> + 1 + 692799 + -1.1642999119343789e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 3.6462584137916565e-01 + 2.3033350706100464e-01 + <_> + 2 + 502937 + -1.1642999119343789e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 189862 + 1.1642999119343789e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 189921 + 1.1642999119343789e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 1.6444881260395050e-01 + 1.5871065139770508e+01 + <_> + 2 + 167686 + 1.1642999119343789e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 22235 + -1.1642999119343789e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.0780331574833411e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 5.1321882009506226e-01 + 1.1083757400512695e+01 + <_> + 1 + 302791 + 1.0780331574833411e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>8 + 1.9705520570278168e-01 + 1.5000002384185791e+00 + <_> + 2 + 243728 + 1.0780331574833411e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 59063 + -1.0780331574833411e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 579929 + -1.0780331574833411e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>8 + 3.2986953854560852e-01 + 1.5000002384185791e+00 + <_> + 2 + 462754 + -1.0780331574833411e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 117175 + 1.0780331574833411e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.1146339728890808e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 5.1113092899322510e-01 + 2.1796804666519165e-01 + <_> + 1 + 619026 + 1.1146339728890808e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 3.4683200716972351e-01 + 3.1105361938476562e+01 + <_> + 2 + 187806 + -1.1146339728890808e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 431220 + 1.1146339728890808e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 263694 + -1.1146339728890808e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 1.8100503087043762e-01 + 1.1480089187622070e+01 + <_> + 2 + 203577 + -1.1146339728890808e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 60117 + 1.1146339728890808e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.2156583882395897e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.2060949802398682e-01 + 3.9127510070800781e+01 + <_> + 1 + 484708 + 1.2156583882395897e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 2.1059408783912659e-01 + 6.0722084045410156e+01 + <_> + 2 + 351307 + 1.2156583882395897e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 133401 + -1.2156583882395897e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 398012 + -1.2156583882395897e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 3.1975999474525452e-01 + 9.0933103561401367e+00 + <_> + 2 + 76341 + 1.2156583882395897e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 321671 + -1.2156583882395897e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.3607432731232069e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.1743149757385254e-01 + 5.8760490417480469e+01 + <_> + 1 + 452036 + -1.3607432731232069e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 2.9705420136451721e-01 + 1.6450000000000000e+02 + <_> + 2 + 382144 + -1.3607432731232069e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 69892 + 1.3607432731232069e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 430684 + 1.3607432731232069e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 2.3691199719905853e-01 + 1.5460202097892761e-01 + <_> + 2 + 37389 + -1.3607432731232069e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 393295 + 1.3607432731232069e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.2939426290034436e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.1484709978103638e-01 + 5.6885215759277344e+01 + <_> + 1 + 420110 + 1.2939426290034436e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 2.7478271722793579e-01 + 2.1961538696289062e+02 + <_> + 2 + 338210 + 1.2939426290034436e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 81900 + -1.2939426290034436e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 462610 + -1.2939426290034436e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 2.5752079486846924e-01 + 6.0146926879882812e+01 + <_> + 2 + 60774 + 1.2939426290034436e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 401836 + -1.2939426290034436e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.3788163218417238e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 5.1868355274200439e-01 + 5.6418434143066406e+01 + <_> + 1 + 332384 + -1.3788163218417238e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 2.0634472370147705e-01 + 4.3539289385080338e-02 + <_> + 2 + 265088 + -1.3788163218417238e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 67296 + 1.3788163218417238e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 550336 + 1.3788163218417238e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 3.2807117700576782e-01 + 3.0332836151123047e+01 + <_> + 2 + 125563 + -1.3788163218417238e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 424773 + 1.3788163218417238e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.2288828615627784e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.2186959981918335e-01 + 4.4005065917968750e+01 + <_> + 1 + 211822 + 1.2288828615627784e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 1.7441318929195404e-01 + 3.4386754035949707e-02 + <_> + 2 + 176098 + 1.2288828615627784e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 35724 + -1.2288828615627784e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 670898 + -1.2288828615627784e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 3.5627028346061707e-01 + 2.7465816587209702e-02 + <_> + 2 + 74544 + 1.2288828615627784e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 596354 + -1.2288828615627784e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.2373330185222492e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.1379692554473877e-01 + 4.3858562469482422e+01 + <_> + 1 + 546821 + -1.2373330185222492e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>8 + 2.6456314325332642e-01 + 2.7500000000000000e+01 + <_> + 2 + 493613 + -1.2373330185222492e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 53208 + 1.2373330185222492e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 335899 + 1.2373330185222492e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>8 + 2.6633077859878540e-01 + 2.5000000000000000e+00 + <_> + 2 + 261115 + 1.2373330185222492e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 74784 + -1.2373330185222492e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 8.5837123603982324e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.1273876428604126e-01 + 1.4212055664062500e+03 + <_> + 1 + 608036 + 8.5837123603982324e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 3.2932490110397339e-01 + 7.5288684082031250e+02 + <_> + 2 + 67677 + -8.5837123603982324e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 540359 + 8.5837123603982324e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 274684 + -8.5837123603982324e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 1.9212122261524200e-01 + 3.9687362670898438e+01 + <_> + 2 + 269277 + -8.5837123603982324e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 5407 + 8.5837123603982324e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -9.1399751621124209e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 5.1287460327148438e-01 + 3.6527776718139648e+00 + <_> + 1 + 651124 + -9.1399751621124209e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 3.4884500503540039e-01 + 1.4822725296020508e+01 + <_> + 2 + 638581 + -9.1399751621124209e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 12543 + 9.1399751621124209e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 231596 + 9.1399751621124209e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 1.7398902773857117e-01 + 1.1858220398426056e-01 + <_> + 2 + 13413 + -9.1399751621124209e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 218183 + 9.1399751621124209e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.3267822326464726e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 5.1505303382873535e-01 + 1.3376935958862305e+01 + <_> + 1 + 725181 + 1.3267822326464726e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 3.9233598113059998e-01 + 7.6486793518066406e+01 + <_> + 2 + 597607 + 1.3267822326464726e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 127574 + -1.3267822326464726e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 157539 + -1.3267822326464726e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 1.4078499376773834e-01 + 1.5540316700935364e-01 + <_> + 2 + 40589 + 1.3267822326464726e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 116950 + -1.3267822326464726e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.0811608353915657e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>13 + 5.1618635654449463e-01 + 7.6270622253417969e+01 + <_> + 1 + 684355 + -1.0811608353915657e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 3.8952401280403137e-01 + 5.2729541778564453e+01 + <_> + 2 + 592063 + -1.0811608353915657e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 92292 + 1.0811608353915657e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 198365 + 1.0811608353915657e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 1.3747870922088623e-01 + 5.2044520378112793e+00 + <_> + 2 + 167105 + 1.0811608353915657e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 31260 + -1.0811608353915657e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.3715338460647430e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.1717972755432129e-01 + 4.8905628204345703e+01 + <_> + 1 + 600969 + 1.3715338460647430e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 3.1890875101089478e-01 + 9.6019530296325684e-01 + <_> + 2 + 70559 + -1.3715338460647430e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 530410 + 1.3715338460647430e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 281751 + -1.3715338460647430e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 2.1532593667507172e-01 + 9.7361612319946289e-01 + <_> + 2 + 212081 + -1.3715338460647430e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 69670 + 1.3715338460647430e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.3254547985093981e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 5.2258175611495972e-01 + 3.5027778148651123e+00 + <_> + 1 + 632167 + -1.3254547985093981e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 3.4998008608818054e-01 + 1.4569084167480469e+01 + <_> + 2 + 619962 + -1.3254547985093981e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 12205 + 1.3254547985093981e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 250553 + 1.3254547985093981e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 1.8310786783695221e-01 + 1.1858220398426056e-01 + <_> + 2 + 14570 + -1.3254547985093981e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 235983 + 1.3254547985093981e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 8.4454813764816852e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.1257169246673584e-01 + 1.4574616699218750e+03 + <_> + 1 + 619868 + 8.4454813764816852e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 3.3629661798477173e-01 + 9.6474754333496094e+01 + <_> + 2 + 582652 + 8.4454813764816852e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 37216 + -8.4454813764816852e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 262852 + -8.4454813764816852e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 1.8480452895164490e-01 + 1.0024337768554688e+00 + <_> + 2 + 9444 + 8.4454813764816852e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 253408 + -8.4454813764816852e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.0493421493039290e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 5.1370531320571899e-01 + 7.9077102661132812e+01 + <_> + 1 + 585326 + -1.0493421493039290e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>8 + 3.0146947503089905e-01 + 5.0000000000000000e-01 + <_> + 2 + 108706 + 1.0493421493039290e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 476620 + -1.0493421493039290e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 297394 + 1.0493421493039290e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>8 + 2.2474002838134766e-01 + 5.0000000000000000e-01 + <_> + 2 + 29156 + -1.0493421493039290e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 268238 + 1.0493421493039290e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.1406296476672259e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 5.1054799556732178e-01 + 3.7489395141601562e+00 + <_> + 1 + 691547 + -1.1406296476672259e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 3.6840578913688660e-01 + 6.9189804077148438e+01 + <_> + 2 + 653491 + -1.1406296476672259e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 38056 + 1.1406296476672259e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 191173 + 1.1406296476672259e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 1.6007909178733826e-01 + 7.0338334960937500e+03 + <_> + 2 + 147485 + 1.1406296476672259e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 43688 + -1.1406296476672259e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.1464179753103011e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>1 + 5.1817089319229126e-01 + 9.0308670043945312e+01 + <_> + 1 + 634530 + 1.1464179753103011e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 3.6807507276535034e-01 + 1.6281558990478516e+01 + <_> + 2 + 574806 + 1.1464179753103011e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 59724 + -1.1464179753103011e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 248190 + -1.1464179753103011e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 1.6055402159690857e-01 + 1.8537008666992188e+02 + <_> + 2 + 34270 + 1.1464179753103011e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 213920 + -1.1464179753103011e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.0583016574305738e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.1528364419937134e-01 + 5.8760490417480469e+01 + <_> + 1 + 452036 + -1.0583016574305738e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 2.9451945424079895e-01 + 1.6950000000000000e+02 + <_> + 2 + 403075 + -1.0583016574305738e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 48961 + 1.0583016574305738e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 430684 + 1.0583016574305738e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 2.3191341757774353e-01 + 3.6025100708007812e+01 + <_> + 2 + 125667 + -1.0583016574305738e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 305017 + 1.0583016574305738e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.0786114121936161e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.1500076055526733e-01 + 5.5453094482421875e+01 + <_> + 1 + 394915 + 1.0786114121936161e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 2.6535040140151978e-01 + 3.7104926109313965e+00 + <_> + 2 + 291008 + 1.0786114121936161e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 103907 + -1.0786114121936161e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 487805 + -1.0786114121936161e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 2.6158878207206726e-01 + 4.8507652282714844e+01 + <_> + 2 + 27530 + 1.0786114121936161e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 460275 + -1.0786114121936161e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 8.3061166609416481e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>8 + 5.1316148042678833e-01 + 1.1750000000000000e+02 + <_> + 1 + 863242 + 8.3061166609416481e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 4.8929327726364136e-01 + 4.8402999877929688e+01 + <_> + 2 + 854756 + 8.3061166609416481e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 8486 + -8.3061166609416481e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 19478 + -8.3061166609416481e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 3.1460084021091461e-02 + 3.8995925903320312e+01 + <_> + 2 + 16526 + -8.3061166609416481e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 2952 + 8.3061166609416481e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -8.8300218715455306e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.1393413543701172e-01 + 3.5577264404296875e+02 + <_> + 1 + 52695 + 8.8300218715455306e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>13 + 4.0122959762811661e-02 + 1.1057499885559082e+01 + <_> + 2 + 43547 + -8.8300218715455306e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 9148 + 8.8300218715455306e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 830025 + -8.8300218715455306e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>1 + 4.8193776607513428e-01 + 3.6836692810058594e+01 + <_> + 2 + 24520 + 8.8300218715455306e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 805505 + -8.8300218715455306e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 8.2057540433533471e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 5.1392060518264771e-01 + 7.2993118286132812e+01 + <_> + 1 + 69349 + -8.2057540433533471e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 3.1042398884892464e-02 + 6.8500000000000000e+01 + <_> + 2 + 6848 + 8.2057540433533471e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 62501 + -8.2057540433533471e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 813371 + 8.2057540433533471e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 4.8946046829223633e-01 + 7.1500000000000000e+01 + <_> + 2 + 2047 + -8.2057540433533471e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 811324 + 8.2057540433533471e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -8.0671486902307146e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.1318776607513428e-01 + 3.5577264404296875e+02 + <_> + 1 + 52695 + 8.0671486902307146e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>13 + 3.8424737751483917e-02 + 1.1057499885559082e+01 + <_> + 2 + 43547 + -8.0671486902307146e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 9148 + 8.0671486902307146e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 830025 + -8.0671486902307146e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 4.8173221945762634e-01 + 1.3090585708618164e+01 + <_> + 2 + 658289 + -8.0671486902307146e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 171736 + 8.0671486902307146e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.0655615320169901e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 5.1230406761169434e-01 + 1.2525152206420898e+01 + <_> + 1 + 681291 + 1.0655615320169901e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 3.4392166137695312e-01 + 3.2598178863525391e+01 + <_> + 2 + 670749 + 1.0655615320169901e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 10542 + -1.0655615320169901e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 201429 + -1.0655615320169901e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 1.8269221484661102e-01 + 4.8998966217041016e+01 + <_> + 2 + 105268 + 1.0655615320169901e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 96161 + -1.0655615320169901e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.2948516575761299e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.1818317174911499e-01 + 5.9467582702636719e+01 + <_> + 1 + 726898 + -1.2948516575761299e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 3.8248854875564575e-01 + 1.3105225563049316e-01 + <_> + 2 + 68516 + 1.2948516575761299e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 658382 + -1.2948516575761299e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 155822 + 1.2948516575761299e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 1.4983759820461273e-01 + 2.0996171236038208e-01 + <_> + 2 + 122180 + 1.2948516575761299e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 33642 + -1.2948516575761299e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 1.0700409275441951e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 5.1549249887466431e-01 + 3.5372036132812500e+03 + <_> + 1 + 601219 + 1.0700409275441951e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 3.5022792220115662e-01 + 7.5344039916992188e+01 + <_> + 2 + 541756 + 1.0700409275441951e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 59463 + -1.0700409275441951e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 281501 + -1.0700409275441951e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 1.7649760842323303e-01 + 4.9647010862827301e-02 + <_> + 2 + 228343 + -1.0700409275441951e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 53158 + 1.0700409275441951e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.0474920535281734e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.1142102479934692e-01 + 4.6025096893310547e+01 + <_> + 1 + 573955 + -1.0474920535281734e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 2.9203769564628601e-01 + 1.0725485839843750e+03 + <_> + 2 + 172723 + 1.0474920535281734e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 401232 + -1.0474920535281734e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 308765 + 1.0474920535281734e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 2.3412567377090454e-01 + 9.8169006347656250e+02 + <_> + 2 + 11367 + -1.0474920535281734e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 297398 + 1.0474920535281734e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -1.2487974796495811e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.1834857463836670e-01 + 1.0657727050781250e+03 + <_> + 1 + 413180 + 1.2487974796495811e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 2.4111241102218628e-01 + 2.3733158409595490e-01 + <_> + 2 + 132397 + 1.2487974796495811e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 280783 + -1.2487974796495811e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 469540 + -1.2487974796495811e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 2.9006701707839966e-01 + 2.7945224761962891e+01 + <_> + 2 + 458156 + -1.2487974796495811e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 11384 + 1.2487974796495811e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + 7.6957062285353720e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 5.1330202817916870e-01 + 3.7387070059776306e-01 + <_> + 1 + 772364 + 7.6957062285353720e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 5.0283682346343994e-01 + 1.6083779335021973e+00 + <_> + 2 + 72592 + -7.6957062285353720e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 699772 + 7.6957062285353720e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 110356 + -7.6957062285353720e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 1.6392931342124939e-02 + 5.2843241691589355e+00 + <_> + 2 + 72743 + -7.6957062285353720e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 37613 + 7.6957062285353720e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 882720 + -7.5763809755519787e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.1155614852905273e-01 + 3.5136163330078125e+02 + <_> + 1 + 50792 + 7.5763809755519787e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>3 + 3.6862608045339584e-02 + 3.7840070724487305e+00 + <_> + 2 + 41774 + -7.5763809755519787e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 9018 + 7.5763809755519787e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 831928 + -7.5763809755519787e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 4.8206928372383118e-01 + 2.5892502069473267e-01 + <_> + 2 + 490194 + -7.5763809755519787e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 341734 + 7.5763809755519787e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/classifier_models/semantic_room_SVM.xml b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/classifier_models/semantic_room_SVM.xml new file mode 100644 index 0000000..0cf657b --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/classifier_models/semantic_room_SVM.xml @@ -0,0 +1,26 @@ + + + + ONE_CLASS + LINEAR + 5.9999999999999998e-01 + 2.2204460492503131e-16 + 100 + 23 + 23 + 1 + 1 + + <_> + 2.83671656e+05 7.78090750e+06 2.06722500e+04 4.50947109e+04 + 5.79335150e+06 5300250. 19302716. 5.71475812e+05 1567863. + 1.29049977e+05 7.88329650e+06 6738720. 509354208. 4.15090725e+06 + 115733672. 4.59749316e+03 6.03456750e+06 4511485. 28201040. + 7.34039550e+06 4.96453250e+06 4.85331211e+04 3.32311797e+04 + + <_> + 1 + 4.7882008646924377e+11 + + 1. + diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/classifier_models/semantic_room_boost.xml b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/classifier_models/semantic_room_boost.xml new file mode 100644 index 0000000..537f8a7 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/classifier_models/semantic_room_boost.xml @@ -0,0 +1,32240 @@ + + + + DiscreteAdaboost + 3 + 350 + 1. + 1 + 23 + 23 + 23 + 0 + + 0 + 10 + 2 + 10 + 0 + + 1 + 2 +
d
+ + 1. 1. + + 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 + + 1 + 1 +
i
+ + 2 + + 1 + 2 +
i
+ + -1 1 + + <_> + -1 + + <_> + 0 + 939949 + 2.0226456578642606e+00 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 8.5827314853668213e-01 + 3.9443078041076660e+00 + <_> + 1 + 794668 + 2.0226456578642606e+00 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 7.5376427173614502e-01 + 3.6514926147460938e+02 + <_> + 2 + 763423 + 2.0226456578642606e+00 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 31245 + -2.0226456578642606e+00 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 145281 + -2.0226456578642606e+00 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 1.2939000129699707e-01 + 3.5909347534179688e+01 + <_> + 2 + 24368 + 2.0226456578642606e+00 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 120913 + -2.0226456578642606e+00 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.4251557105945021e+00 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 7.0150953531265259e-01 + 4.1433166503906250e+01 + <_> + 1 + 532558 + 1.4251557105945021e+00 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 3.5428416728973389e-01 + 2.9447725296020508e+01 + <_> + 2 + 30378 + -1.4251557105945021e+00 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 502180 + 1.4251557105945021e+00 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 407391 + -1.4251557105945021e+00 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 4.5186123251914978e-01 + 5.4849568754434586e-02 + <_> + 2 + 283768 + -1.4251557105945021e+00 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 123623 + 1.4251557105945021e+00 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 7.3144894913893577e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 6.1336368322372437e-01 + 6.0232398986816406e+01 + <_> + 1 + 756347 + 7.3144894913893577e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 4.7422397136688232e-01 + 3.5520416259765625e+01 + <_> + 2 + 73329 + -7.3144894913893577e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 683018 + 7.3144894913893577e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 183602 + -7.3144894913893577e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 2.0089919865131378e-01 + 1.4493373036384583e-01 + <_> + 2 + 22549 + 7.3144894913893577e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 161053 + -7.3144894913893577e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 9.1345010705023710e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 6.2904185056686401e-01 + 1.7058036804199219e+01 + <_> + 1 + 748219 + -9.1345010705023710e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 5.3601086139678955e-01 + 4.9688607454299927e-02 + <_> + 2 + 471299 + -9.1345010705023710e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 276920 + 9.1345010705023710e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 191730 + 9.1345010705023710e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>4 + 1.7769478261470795e-01 + 9.6141662597656250e+01 + <_> + 2 + 179433 + 9.1345010705023710e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 12297 + -9.1345010705023710e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 6.6897989435159666e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 6.0922271013259888e-01 + 2.3336733877658844e-01 + <_> + 1 + 723866 + 6.6897989435159666e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 4.8616808652877808e-01 + 8.0151657104492188e+01 + <_> + 2 + 706971 + 6.6897989435159666e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 16895 + -6.6897989435159666e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 216083 + -6.6897989435159666e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 1.7510661482810974e-01 + 3.4879661560058594e+01 + <_> + 2 + 124023 + 6.6897989435159666e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 92060 + -6.6897989435159666e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -7.1466370849675187e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 6.0272848606109619e-01 + 2.2764015942811966e-02 + <_> + 1 + 227734 + 7.1466370849675187e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>1 + 1.3911896944046021e-01 + 6.2335443496704102e+00 + <_> + 2 + 975 + -7.1466370849675187e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 226759 + 7.1466370849675187e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 712215 + -7.1466370849675187e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 5.3231185674667358e-01 + 9.7940850257873535e-01 + <_> + 2 + 479991 + -7.1466370849675187e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 232224 + 7.1466370849675187e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 3.2188265261940818e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 5.6916862726211548e-01 + 8.5121059417724609e+00 + <_> + 1 + 892143 + 3.2188265261940818e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>8 + 5.4261237382888794e-01 + 1.4850000000000000e+02 + <_> + 2 + 875578 + 3.2188265261940818e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 16565 + -3.2188265261940818e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 47806 + -3.2188265261940818e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 3.7170626223087311e-02 + 1.3600394129753113e-01 + <_> + 2 + 7914 + 3.2188265261940818e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 39892 + -3.2188265261940818e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -5.2607187711191294e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 5.8416277170181274e-01 + 3.6514898681640625e+02 + <_> + 1 + 852826 + -5.2607187711191294e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.1332676410675049e-01 + 3.2797714233398438e+01 + <_> + 2 + 362160 + 5.2607187711191294e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 490666 + -5.2607187711191294e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 87123 + 5.2607187711191294e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 1.1523968726396561e-01 + 2.4305555820465088e+00 + <_> + 2 + 5475 + -5.2607187711191294e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 81648 + 5.2607187711191294e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 4.9275705130157632e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 5.9013003110885620e-01 + 1.0480075836181641e+01 + <_> + 1 + 364232 + -4.9275705130157632e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 2.4214275181293488e-01 + 1.2566794433593750e+03 + <_> + 2 + 148544 + 4.9275705130157632e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 215688 + -4.9275705130157632e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 575717 + 4.9275705130157632e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 3.7861296534538269e-01 + 7.6526794433593750e+02 + <_> + 2 + 30911 + -4.9275705130157632e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 544806 + 4.9275705130157632e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -5.5180950907530646e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 5.7895511388778687e-01 + 4.6034218750000000e+03 + <_> + 1 + 766677 + -5.5180950907530646e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 4.4710671901702881e-01 + 6.5082028508186340e-02 + <_> + 2 + 607342 + -5.5180950907530646e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 159335 + 5.5180950907530646e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 173272 + 5.5180950907530646e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 1.8744860589504242e-01 + 2.3180081844329834e+00 + <_> + 2 + 45689 + -5.5180950907530646e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 127583 + 5.5180950907530646e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 3.2315952813058696e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.6513071060180664e-01 + 8.3768737792968750e+01 + <_> + 1 + 928622 + 3.2315952813058696e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 5.6154537200927734e-01 + 2.2150000000000000e+02 + <_> + 2 + 784747 + 3.2315952813058696e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 143875 + -3.2315952813058696e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 11327 + -3.2315952813058696e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 1.8548663705587387e-02 + 1.3647222518920898e+01 + <_> + 2 + 10964 + -3.2315952813058696e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 363 + 3.2315952813058696e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -4.6178414175178550e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 5.5798757076263428e-01 + 3.9368353784084320e-02 + <_> + 1 + 495592 + -4.6178414175178550e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 4.0544465184211731e-01 + 2.0206743240356445e+01 + <_> + 2 + 346458 + -4.6178414175178550e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 149134 + 4.6178414175178550e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 444357 + 4.6178414175178550e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 2.0799270272254944e-01 + 5.6933601379394531e+01 + <_> + 2 + 86886 + -4.6178414175178550e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 357471 + 4.6178414175178550e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 4.7894053863509611e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.7199865579605103e-01 + 5.0703388214111328e+01 + <_> + 1 + 652142 + 4.7894053863509611e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 3.6117437481880188e-01 + 3.2997757196426392e-01 + <_> + 2 + 643555 + 4.7894053863509611e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 8587 + -4.7894053863509611e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 287807 + -4.7894053863509611e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 2.5632330775260925e-01 + 3.9445405006408691e+00 + <_> + 2 + 194010 + -4.7894053863509611e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 93797 + 4.7894053863509611e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -3.8366454704631753e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 5.6729328632354736e-01 + 7.3379653930664062e+01 + <_> + 1 + 794147 + -3.8366454704631753e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 4.7110486030578613e-01 + 2.2246288299560547e+01 + <_> + 2 + 678744 + -3.8366454704631753e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 115403 + 3.8366454704631753e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 145802 + 3.8366454704631753e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 1.2365179508924484e-01 + 8.4130043029785156e+01 + <_> + 2 + 135373 + 3.8366454704631753e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 10429 + -3.8366454704631753e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 3.4246713448914645e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.5762475728988647e-01 + 6.7228729248046875e+01 + <_> + 1 + 825059 + 3.4246713448914645e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 4.7398668527603149e-01 + 2.3507771492004395e+00 + <_> + 2 + 7116 + -3.4246713448914645e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 817943 + 3.4246713448914645e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 114890 + -3.4246713448914645e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 1.1080300807952881e-01 + 6.7799672851562500e+03 + <_> + 2 + 105727 + -3.4246713448914645e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 9163 + 3.4246713448914645e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -3.7130784746187950e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 5.5459982156753540e-01 + 7.5639572143554688e+01 + <_> + 1 + 816853 + -3.7130784746187950e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 4.9659723043441772e-01 + 1.0443525695800781e+02 + <_> + 2 + 155095 + 3.7130784746187950e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 661758 + -3.7130784746187950e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 123096 + 3.7130784746187950e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 9.5177732408046722e-02 + 1.0122606754302979e+00 + <_> + 2 + 12893 + -3.7130784746187950e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 110203 + 3.7130784746187950e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 2.9916910272209457e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.5125379562377930e-01 + 6.4852972412109375e+02 + <_> + 1 + 108056 + -2.9916910272209457e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 9.5989674329757690e-02 + 3.3307434082031250e+02 + <_> + 2 + 11464 + 2.9916910272209457e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 96592 + -2.9916910272209457e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 831893 + 2.9916910272209457e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 4.7824969887733459e-01 + 7.0398056030273438e+01 + <_> + 2 + 748928 + 2.9916910272209457e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 82965 + -2.9916910272209457e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -3.4348224147131201e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 5.4840040206909180e-01 + 3.6522686767578125e+02 + <_> + 1 + 852886 + -3.4348224147131201e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 4.9598258733749390e-01 + 3.9441034793853760e+00 + <_> + 2 + 763452 + -3.4348224147131201e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 89434 + 3.4348224147131201e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 87063 + 3.4348224147131201e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 8.9053578674793243e-02 + 8.3949218750000000e+01 + <_> + 2 + 33343 + -3.4348224147131201e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 53720 + 3.4348224147131201e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 2.8541880095404548e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 5.5140954256057739e-01 + 5.1020393371582031e+00 + <_> + 1 + 844055 + 2.8541880095404548e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 4.7138437628746033e-01 + 1.0122606754302979e+00 + <_> + 2 + 106688 + -2.8541880095404548e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 737367 + 2.8541880095404548e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 95894 + -2.8541880095404548e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 9.9489822983741760e-02 + 3.5927295684814453e+01 + <_> + 2 + 11146 + -2.8541880095404548e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 84748 + 2.8541880095404548e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -2.8084537709206142e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.4304260015487671e-01 + 1.0930314636230469e+02 + <_> + 1 + 856315 + -2.8084537709206142e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.3605252504348755e-01 + 2.7683769226074219e+01 + <_> + 2 + 246638 + 2.8084537709206142e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 609677 + -2.8084537709206142e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 83634 + 2.8084537709206142e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>4 + 3.3700939267873764e-02 + 9.5415611267089844e+01 + <_> + 2 + 730 + -2.8084537709206142e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 82904 + 2.8084537709206142e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 3.1152356942667836e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 5.5724275112152100e-01 + 4.1268825531005859e+01 + <_> + 1 + 110956 + -3.1152356942667836e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 1.3897058367729187e-01 + 1.0122606754302979e+00 + <_> + 2 + 26558 + 3.1152356942667836e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 84398 + -3.1152356942667836e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 828993 + 3.1152356942667836e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>13 + 4.3828651309013367e-01 + 1.8369375228881836e+01 + <_> + 2 + 37030 + -3.1152356942667836e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 791963 + 3.1152356942667836e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -2.5573182698729352e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 5.4431748390197754e-01 + 9.1470205783843994e-01 + <_> + 1 + 67119 + 2.5573182698729352e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 2.4935822933912277e-02 + 1.1680285644531250e+02 + <_> + 2 + 66951 + 2.5573182698729352e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 168 + -2.5573182698729352e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 872830 + -2.5573182698729352e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.3865098953247070e-01 + 1.1615284729003906e+02 + <_> + 2 + 797862 + -2.5573182698729352e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 74968 + 2.5573182698729352e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 3.0793041085087958e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.5126595497131348e-01 + 5.6819297790527344e+01 + <_> + 1 + 718531 + 3.0793041085087958e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 4.0906214714050293e-01 + 1.8980936050415039e+01 + <_> + 2 + 691749 + 3.0793041085087958e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 26782 + -3.0793041085087958e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 221418 + -3.0793041085087958e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 1.6731788218021393e-01 + 1.4008244872093201e-01 + <_> + 2 + 32632 + 3.0793041085087958e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 188786 + -3.0793041085087958e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -3.4947794838469348e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 5.5449664592742920e-01 + 3.3788502216339111e-02 + <_> + 1 + 414113 + -3.4947794838469348e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 3.2785657048225403e-01 + 9.5386415719985962e-01 + <_> + 2 + 202553 + 3.4947794838469348e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 211560 + -3.4947794838469348e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 525836 + 3.4947794838469348e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 2.5863438844680786e-01 + 4.9185619354248047e+00 + <_> + 2 + 470182 + 3.4947794838469348e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 55654 + -3.4947794838469348e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -3.4190774290229742e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 5.5470842123031616e-01 + 2.8553571777343750e+03 + <_> + 1 + 617064 + -3.4190774290229742e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 3.2767680287361145e-01 + 1.0375065612792969e+02 + <_> + 2 + 134043 + 3.4190774290229742e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 483021 + -3.4190774290229742e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 322885 + 3.4190774290229742e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 2.5697705149650574e-01 + 2.0250000000000000e+02 + <_> + 2 + 250118 + 3.4190774290229742e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 72767 + -3.4190774290229742e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -3.1139308996722537e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.3828305006027222e-01 + 6.1068405151367188e+01 + <_> + 1 + 478604 + -3.1139308996722537e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 3.5351324081420898e-01 + 1.3953120231628418e+01 + <_> + 2 + 331104 + -3.1139308996722537e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 147500 + 3.1139308996722537e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 461345 + 3.1139308996722537e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 2.2371201217174530e-01 + 9.6479928493499756e-01 + <_> + 2 + 144612 + -3.1139308996722537e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 316733 + 3.1139308996722537e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 3.3246089252838135e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.4506015777587891e-01 + 4.7473869323730469e+01 + <_> + 1 + 615556 + 3.3246089252838135e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 3.4663394093513489e-01 + 1.9650000000000000e+02 + <_> + 2 + 502296 + 3.3246089252838135e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 113260 + -3.3246089252838135e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 324393 + -3.3246089252838135e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 2.3572407662868500e-01 + 7.1488784790039062e+01 + <_> + 2 + 160167 + -3.3246089252838135e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 164226 + 3.3246089252838135e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -2.0086876060077061e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 5.3347140550613403e-01 + 3.6453039550781250e+02 + <_> + 1 + 852357 + -2.0086876060077061e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 4.6865060925483704e-01 + 9.5974552631378174e-01 + <_> + 2 + 222683 + 2.0086876060077061e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 629674 + -2.0086876060077061e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 87592 + 2.0086876060077061e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 8.1398412585258484e-02 + 2.5950000000000000e+02 + <_> + 2 + 81163 + 2.0086876060077061e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 6429 + -2.0086876060077061e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 2.9953262198223746e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.4157960414886475e-01 + 6.5227142333984375e+01 + <_> + 1 + 806044 + 2.9953262198223746e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 4.6954536437988281e-01 + 2.2762157022953033e-02 + <_> + 2 + 224771 + -2.9953262198223746e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 581273 + 2.9953262198223746e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 133905 + -2.9953262198223746e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>10 + 1.0478290915489197e-01 + 9.7655277252197266e+00 + <_> + 2 + 93787 + -2.9953262198223746e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 40118 + 2.9953262198223746e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -2.4759011298447267e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 5.2982527017593384e-01 + 4.6672798156738281e+01 + <_> + 1 + 439798 + 2.4759011298447267e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 2.1273081004619598e-01 + 1.5047150850296021e-01 + <_> + 2 + 101101 + -2.4759011298447267e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 338697 + 2.4759011298447267e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 500151 + -2.4759011298447267e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 3.4885242581367493e-01 + 9.1137039661407471e-01 + <_> + 2 + 32850 + 2.4759011298447267e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 467301 + -2.4759011298447267e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 2.9671761826596560e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.5251622200012207e-01 + 4.1433364868164062e+01 + <_> + 1 + 532563 + -2.9671761826596560e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 2.5726059079170227e-01 + 1.9505516052246094e+01 + <_> + 2 + 64605 + 2.9671761826596560e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 467958 + -2.9671761826596560e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 407386 + 2.9671761826596560e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 3.1637933850288391e-01 + 6.5512292480468750e+02 + <_> + 2 + 5633 + -2.9671761826596560e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 401753 + 2.9671761826596560e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 3.9510793074347778e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.3196793794631958e-01 + 3.8175857543945312e+01 + <_> + 1 + 480695 + 3.9510793074347778e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 2.1420837938785553e-01 + 6.0202746582031250e+02 + <_> + 2 + 89107 + -3.9510793074347778e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 391588 + 3.9510793074347778e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 459254 + -3.9510793074347778e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 3.8330334424972534e-01 + 3.9462192058563232e+00 + <_> + 2 + 343589 + -3.9510793074347778e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 115665 + 3.9510793074347778e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 2.8982653401679226e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 5.4727774858474731e-01 + 3.2548737525939941e+00 + <_> + 1 + 735526 + 2.8982653401679226e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 3.5362020134925842e-01 + 6.4791790771484375e+02 + <_> + 2 + 31272 + -2.8982653401679226e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 704254 + 2.8982653401679226e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 204423 + -2.8982653401679226e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 2.1833348274230957e-01 + 1.1850000000000000e+02 + <_> + 2 + 23558 + 2.8982653401679226e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 180865 + -2.8982653401679226e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -2.4330133871242746e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 5.3345614671707153e-01 + 1.6454334259033203e+01 + <_> + 1 + 485942 + -2.4330133871242746e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 3.1326773762702942e-01 + 2.9456104278564453e+01 + <_> + 2 + 43132 + 2.4330133871242746e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 442810 + -2.4330133871242746e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 454007 + 2.4330133871242746e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 2.4725930392742157e-01 + 7.2192153930664062e+01 + <_> + 2 + 387984 + 2.4330133871242746e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 66023 + -2.4330133871242746e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -2.5618613697765935e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 5.2949368953704834e-01 + 2.5537453613281250e+03 + <_> + 1 + 574641 + -2.5618613697765935e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 2.8042337298393250e-01 + 1.1056037902832031e+01 + <_> + 2 + 273173 + 2.5618613697765935e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 301468 + -2.5618613697765935e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 365308 + 2.5618613697765935e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 2.8327515721321106e-01 + 7.6469225883483887e+00 + <_> + 2 + 321097 + 2.5618613697765935e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 44211 + -2.5618613697765935e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -2.5676963887145088e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 5.4714041948318481e-01 + 7.9059158325195312e+01 + <_> + 1 + 843043 + -2.5676963887145088e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 5.0245326757431030e-01 + 9.1810214519500732e-01 + <_> + 2 + 71570 + 2.5676963887145088e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 771473 + -2.5676963887145088e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 96906 + 2.5676963887145088e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 6.1388768255710602e-02 + 7.8031951904296875e+01 + <_> + 2 + 66989 + 2.5676963887145088e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 29917 + -2.5676963887145088e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 2.1600088535735781e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.4289114475250244e-01 + 3.6072143554687500e+02 + <_> + 1 + 16348 + -2.1600088535735781e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>3 + 4.0688611567020416e-02 + 3.7838444709777832e+00 + <_> + 2 + 13358 + 2.1600088535735781e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 2990 + -2.1600088535735781e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 923601 + 2.1600088535735781e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.1310265064239502e-01 + 5.5818759918212891e+01 + <_> + 2 + 691437 + 2.1600088535735781e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 232164 + -2.1600088535735781e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 2.5964254171437146e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 5.4372173547744751e-01 + 2.2417547607421875e+02 + <_> + 1 + 610116 + -2.5964254171437146e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 3.1904256343841553e-01 + 9.6664911508560181e-01 + <_> + 2 + 206359 + 2.5964254171437146e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 403757 + -2.5964254171437146e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 329833 + 2.5964254171437146e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 2.4550586938858032e-01 + 3.6004463195800781e+01 + <_> + 2 + 8482 + -2.5964254171437146e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 321351 + 2.5964254171437146e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 2.4695705612579522e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 5.3235769271850586e-01 + 3.4919373691082001e-02 + <_> + 1 + 430223 + -2.4695705612579522e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>8 + 3.2390600442886353e-01 + 4.5000000000000000e+00 + <_> + 2 + 296292 + -2.4695705612579522e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 133931 + 2.4695705612579522e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 509726 + 2.4695705612579522e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 2.3752139508724213e-01 + 2.0504289865493774e-01 + <_> + 2 + 112178 + -2.4695705612579522e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 397548 + 2.4695705612579522e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 2.9196653330769035e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 5.4162555932998657e-01 + 1.7450000000000000e+02 + <_> + 1 + 481764 + 2.9196653330769035e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 3.6287128925323486e-01 + 5.3934939205646515e-02 + <_> + 2 + 425260 + 2.9196653330769035e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 56504 + -2.9196653330769035e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 458185 + -2.9196653330769035e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 2.0960621535778046e-01 + 6.6537246108055115e-02 + <_> + 2 + 325798 + -2.9196653330769035e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 132387 + 2.9196653330769035e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 2.2953812396051076e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.4063379764556885e-01 + 6.0077247619628906e+01 + <_> + 1 + 464839 + -2.2953812396051076e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 3.3281683921813965e-01 + 1.5374826431274414e+01 + <_> + 2 + 334877 + -2.2953812396051076e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 129962 + 2.2953812396051076e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 475110 + 2.2953812396051076e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 2.2431707382202148e-01 + 5.3167217254638672e+01 + <_> + 2 + 26575 + -2.2953812396051076e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 448535 + 2.2953812396051076e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 2.4363841170508058e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 5.3623861074447632e-01 + 2.9583332538604736e+00 + <_> + 1 + 441193 + 2.4363841170508058e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>8 + 2.8272402286529541e-01 + 1.7500000000000000e+01 + <_> + 2 + 381614 + 2.4363841170508058e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 59579 + -2.4363841170508058e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 498756 + -2.4363841170508058e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 2.7788606286048889e-01 + 9.0812724828720093e-01 + <_> + 2 + 55786 + 2.4363841170508058e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 442970 + -2.4363841170508058e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 2.5497291522360283e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 5.3967899084091187e-01 + 1.7085388183593750e+03 + <_> + 1 + 423647 + -2.5497291522360283e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 1.7904870212078094e-01 + 6.8777061998844147e-02 + <_> + 2 + 299309 + -2.5497291522360283e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 124338 + 2.5497291522360283e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 516302 + 2.5497291522360283e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 3.8435143232345581e-01 + 1.2912152099609375e+02 + <_> + 2 + 86740 + -2.5497291522360283e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 429562 + 2.5497291522360283e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 2.1670166953156289e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 5.2932906150817871e-01 + 1.9639355468750000e+02 + <_> + 1 + 545226 + 2.1670166953156289e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 2.6945090293884277e-01 + 8.5003405761718750e+02 + <_> + 2 + 92561 + -2.1670166953156289e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 452665 + 2.1670166953156289e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 394723 + -2.1670166953156289e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 2.8451350331306458e-01 + 4.6782046556472778e-02 + <_> + 2 + 245317 + -2.1670166953156289e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 149406 + 2.1670166953156289e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.9867698478456372e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.2459204196929932e-01 + 4.6659034729003906e+01 + <_> + 1 + 215529 + -1.9867698478456372e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 1.9870659708976746e-01 + 9.9500000000000000e+01 + <_> + 2 + 26084 + 1.9867698478456372e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 189445 + -1.9867698478456372e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 724420 + 1.9867698478456372e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 3.5079991817474365e-01 + 2.3951523437500000e+03 + <_> + 2 + 439875 + -1.9867698478456372e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 284545 + 1.9867698478456372e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 2.7481371638895929e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 5.3700256347656250e-01 + 2.3375824093818665e-01 + <_> + 1 + 430255 + -2.7481371638895929e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 3.5576564073562622e-01 + 9.5551097393035889e-01 + <_> + 2 + 137163 + 2.7481371638895929e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 293092 + -2.7481371638895929e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 509694 + 2.7481371638895929e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 2.1250864863395691e-01 + 2.8410644531250000e+01 + <_> + 2 + 7338 + -2.7481371638895929e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 502356 + 2.7481371638895929e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 2.5990437379519565e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 5.4344940185546875e-01 + 1.9050000000000000e+02 + <_> + 1 + 603590 + 2.5990437379519565e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 4.2830768227577209e-01 + 6.8611109256744385e-01 + <_> + 2 + 15701 + -2.5990437379519565e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 587889 + 2.5990437379519565e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 336359 + -2.5990437379519565e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 1.3630510866641998e-01 + 6.5881326794624329e-02 + <_> + 2 + 216221 + -2.5990437379519565e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 120138 + 2.5990437379519565e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.8847031236620435e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.2467125654220581e-01 + 2.2579105377197266e+01 + <_> + 1 + 124713 + 1.8847031236620435e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>3 + 7.7892690896987915e-02 + 2.6636819839477539e+00 + <_> + 2 + 9771 + -1.8847031236620435e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 114942 + 1.8847031236620435e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 815236 + -1.8847031236620435e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>13 + 4.6908590197563171e-01 + 6.0606872558593750e+01 + <_> + 2 + 486342 + -1.8847031236620435e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 328894 + 1.8847031236620435e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 2.0111406417186814e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.3266733884811401e-01 + 5.8463272094726562e+01 + <_> + 1 + 737406 + 2.0111406417186814e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 4.0138897299766541e-01 + 1.3870344238281250e+03 + <_> + 2 + 287251 + -2.0111406417186814e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 450155 + 2.0111406417186814e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 202543 + -2.0111406417186814e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 1.4872075617313385e-01 + 2.7583332061767578e+00 + <_> + 2 + 4139 + 2.0111406417186814e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 198404 + -2.0111406417186814e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -2.2015334495825503e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 5.2287429571151733e-01 + 2.3300969600677490e-01 + <_> + 1 + 722230 + -2.2015334495825503e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 4.0365332365036011e-01 + 6.7302330017089844e+01 + <_> + 2 + 425238 + -2.2015334495825503e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 296992 + 2.2015334495825503e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 217719 + 2.2015334495825503e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 1.5116378664970398e-01 + 1.3221721649169922e+01 + <_> + 2 + 166666 + 2.2015334495825503e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 51053 + -2.2015334495825503e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 2.1334032322058097e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 5.2919000387191772e-01 + 3.2303017578125000e+03 + <_> + 1 + 657889 + -2.1334032322058097e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 3.2652023434638977e-01 + 1.2837186813354492e+01 + <_> + 2 + 424008 + 2.1334032322058097e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 233881 + -2.1334032322058097e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 282060 + 2.1334032322058097e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 2.2661346197128296e-01 + 2.2450000000000000e+02 + <_> + 2 + 255130 + 2.1334032322058097e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 26930 + -2.1334032322058097e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -2.5941580031289818e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 5.3171366453170776e-01 + 9.6487987041473389e-01 + <_> + 1 + 320289 + 2.5941580031289818e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 1.5992198884487152e-01 + 8.3526809692382812e+01 + <_> + 2 + 190522 + 2.5941580031289818e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 129767 + -2.5941580031289818e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 619660 + -2.5941580031289818e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>10 + 4.0457069873809814e-01 + 8.9043960571289062e+00 + <_> + 2 + 472566 + -2.5941580031289818e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 147094 + 2.5941580031289818e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 2.1092436669383541e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 5.3618997335433960e-01 + 5.4829515516757965e-02 + <_> + 1 + 711654 + 2.1092436669383541e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 4.9277240037918091e-01 + 2.1750000000000000e+02 + <_> + 2 + 637159 + 2.1092436669383541e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 74495 + -2.1092436669383541e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 228295 + -2.1092436669383541e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>10 + 5.9764046221971512e-02 + 1.0166900634765625e+01 + <_> + 2 + 149832 + -2.1092436669383541e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 78463 + 2.1092436669383541e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.8112584311580154e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 5.2681690454483032e-01 + 3.6526010131835938e+02 + <_> + 1 + 852907 + -1.8112584311580154e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 4.6716079115867615e-01 + 2.8757917881011963e-01 + <_> + 2 + 532627 + -1.8112584311580154e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 320280 + 1.8112584311580154e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 87042 + 1.8112584311580154e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 7.7997267246246338e-02 + 3.9442620277404785e+00 + <_> + 2 + 31198 + 1.8112584311580154e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 55844 + -1.8112584311580154e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.9304498250030885e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 5.3133988380432129e-01 + 5.5652875900268555e+00 + <_> + 1 + 664441 + 1.9304498250030885e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 4.0501934289932251e-01 + 4.5049853324890137e+00 + <_> + 2 + 31587 + -1.9304498250030885e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 632854 + 1.9304498250030885e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 275508 + -1.9304498250030885e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 1.4309258759021759e-01 + 1.4984306693077087e-01 + <_> + 2 + 59141 + 1.9304498250030885e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 216367 + -1.9304498250030885e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.6541370481310153e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>8 + 5.2147001028060913e-01 + 5.5000000000000000e+00 + <_> + 1 + 709653 + -1.6541370481310153e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 4.0957421064376831e-01 + 2.2454063415527344e+01 + <_> + 2 + 101247 + 1.6541370481310153e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 608406 + -1.6541370481310153e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 230296 + 1.6541370481310153e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 1.3168516755104065e-01 + 2.5083332061767578e+00 + <_> + 2 + 65751 + -1.6541370481310153e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 164545 + 1.6541370481310153e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.6352544659856905e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 5.2623450756072998e-01 + 6.9722223281860352e-01 + <_> + 1 + 17201 + -1.6352544659856905e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>3 + 3.6550253629684448e-02 + 3.7851691246032715e+00 + <_> + 2 + 10891 + 1.6352544659856905e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 6310 + -1.6352544659856905e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 922748 + 1.6352544659856905e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 5.0424027442932129e-01 + 6.3734143066406250e+02 + <_> + 2 + 27606 + -1.6352544659856905e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 895142 + 1.6352544659856905e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.7509322078942291e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 5.3266936540603638e-01 + 9.2378002405166626e-01 + <_> + 1 + 85577 + 1.7509322078942291e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 2.4161590263247490e-02 + 1.7522876739501953e+01 + <_> + 2 + 83307 + 1.7509322078942291e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 2270 + -1.7509322078942291e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 854372 + -1.7509322078942291e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 5.1950019598007202e-01 + 9.5500000000000000e+01 + <_> + 2 + 21323 + 1.7509322078942291e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 833049 + -1.7509322078942291e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.5419982266619525e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 5.2714896202087402e-01 + 4.5119943618774414e+00 + <_> + 1 + 44490 + -1.5419982266619525e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 7.2316348552703857e-02 + 3.0070510253906250e+03 + <_> + 2 + 30653 + -1.5419982266619525e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 13837 + 1.5419982266619525e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 895459 + 1.5419982266619525e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>8 + 4.6615740656852722e-01 + 8.9500000000000000e+01 + <_> + 2 + 851929 + 1.5419982266619525e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 43530 + -1.5419982266619525e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.7927364907126292e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 5.2412807941436768e-01 + 2.9443140029907227e+01 + <_> + 1 + 50293 + 1.7927364907126292e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 7.8219868242740631e-02 + 1.2250000000000000e+02 + <_> + 2 + 23436 + 1.7927364907126292e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 26857 + -1.7927364907126292e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 889656 + -1.7927364907126292e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 4.6647888422012329e-01 + 9.0769916772842407e-01 + <_> + 2 + 55021 + 1.7927364907126292e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 834635 + -1.7927364907126292e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.7005233492098371e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>13 + 5.3408318758010864e-01 + 1.1840625762939453e+01 + <_> + 1 + 35660 + -1.7005233492098371e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 6.4576245844364166e-02 + 3.3248651123046875e+02 + <_> + 2 + 11094 + 1.7005233492098371e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 24566 + -1.7005233492098371e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 904289 + 1.7005233492098371e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 4.7783467173576355e-01 + 8.0095996856689453e+00 + <_> + 2 + 854038 + 1.7005233492098371e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 50251 + -1.7005233492098371e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.5766479967775321e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 5.2259242534637451e-01 + 3.6526010131835938e+02 + <_> + 1 + 852907 + -1.5766479967775321e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 4.6372973918914795e-01 + 2.8347057342529297e+01 + <_> + 2 + 259588 + 1.5766479967775321e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 593319 + -1.5766479967775321e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 87042 + 1.5766479967775321e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 7.5605012476444244e-02 + 4.2391284179687500e+03 + <_> + 2 + 56009 + 1.5766479967775321e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 31033 + -1.5766479967775321e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 2.0345531067045106e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>3 + 5.3263503313064575e-01 + 7.6928968429565430e+00 + <_> + 1 + 153509 + -2.0345531067045106e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 1.3095039129257202e-01 + 9.7286391258239746e-01 + <_> + 2 + 96137 + 2.0345531067045106e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 57372 + -2.0345531067045106e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 786440 + 2.0345531067045106e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 4.1973870992660522e-01 + 6.5749998092651367e+00 + <_> + 2 + 665002 + 2.0345531067045106e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 121438 + -2.0345531067045106e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.7944784876261041e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 5.3015738725662231e-01 + 9.4611334800720215e-01 + <_> + 1 + 172035 + 1.7944784876261041e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 5.6955724954605103e-02 + 2.5150000000000000e+02 + <_> + 2 + 159136 + 1.7944784876261041e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 12899 + -1.7944784876261041e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 767914 + -1.7944784876261041e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 4.8778623342514038e-01 + 9.8044538497924805e-01 + <_> + 2 + 569009 + -1.7944784876261041e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 198905 + 1.7944784876261041e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.4321518446515757e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.2754551172256470e-01 + 3.5654925537109375e+02 + <_> + 1 + 15966 + -1.4321518446515757e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>3 + 3.8986083120107651e-02 + 3.7838444709777832e+00 + <_> + 2 + 13121 + 1.4321518446515757e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 2845 + -1.4321518446515757e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 923983 + 1.4321518446515757e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 4.9675664305686951e-01 + 6.2573333740234375e+02 + <_> + 2 + 25632 + -1.4321518446515757e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 898351 + 1.4321518446515757e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.4797855418420455e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 5.2789056301116943e-01 + 9.0533632040023804e-01 + <_> + 1 + 51102 + 1.4797855418420455e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 1.1116954497992992e-02 + 1.1682679748535156e+02 + <_> + 2 + 51001 + 1.4797855418420455e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 101 + -1.4797855418420455e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 888847 + -1.4797855418420455e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 5.2581030130386353e-01 + 9.4659570312500000e+03 + <_> + 2 + 859000 + -1.4797855418420455e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 29847 + 1.4797855418420455e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.3808474979353280e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.2476662397384644e-01 + 3.5654925537109375e+02 + <_> + 1 + 15966 + -1.3808474979353280e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>3 + 3.7089947611093521e-02 + 3.7838444709777832e+00 + <_> + 2 + 13121 + 1.3808474979353280e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 2845 + -1.3808474979353280e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 923983 + 1.3808474979353280e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 4.9737650156021118e-01 + 1.4504259824752808e-01 + <_> + 2 + 151920 + -1.3808474979353280e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 772063 + 1.3808474979353280e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -2.0996825118360757e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 5.2747422456741333e-01 + 3.3963266601562500e+03 + <_> + 1 + 672392 + -2.0996825118360757e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 3.3741167187690735e-01 + 2.6613320922851562e+02 + <_> + 2 + 549459 + -2.0996825118360757e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 122933 + 2.0996825118360757e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 267557 + 2.0996825118360757e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 2.1488840878009796e-01 + 1.0634729862213135e+00 + <_> + 2 + 64492 + -2.0996825118360757e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 203065 + 2.0996825118360757e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 2.2154819413187568e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.3745418787002563e-01 + 5.2733337402343750e+01 + <_> + 1 + 673737 + 2.2154819413187568e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>3 + 3.6880999803543091e-01 + 2.1995224952697754e+00 + <_> + 2 + 6441 + -2.2154819413187568e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 667296 + 2.2154819413187568e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 266212 + -2.2154819413187568e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 1.8635159730911255e-01 + 2.9811453819274902e-01 + <_> + 2 + 259380 + -2.2154819413187568e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 6832 + 2.2154819413187568e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -2.0966977975364728e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 5.3306114673614502e-01 + 5.7893180847167969e+01 + <_> + 1 + 611818 + -2.0966977975364728e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 3.4385809302330017e-01 + 2.2045988082885742e+01 + <_> + 2 + 113768 + 2.0966977975364728e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 498050 + -2.0966977975364728e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 328131 + 2.0966977975364728e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>10 + 2.0836815237998962e-01 + 7.7264251708984375e+00 + <_> + 2 + 82712 + 2.0966977975364728e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 245419 + -2.0966977975364728e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.5014962810470026e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.2724194526672363e-01 + 6.1111895751953125e+02 + <_> + 1 + 93252 + -1.5014962810470026e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 8.9419081807136536e-02 + 3.0537776947021484e+01 + <_> + 2 + 90161 + -1.5014962810470026e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 3091 + 1.5014962810470026e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 846697 + 1.5014962810470026e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 4.4804796576499939e-01 + 1.1910075694322586e-01 + <_> + 2 + 45200 + -1.5014962810470026e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 801497 + 1.5014962810470026e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.8442427962608809e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 5.2161502838134766e-01 + 6.6441131591796875e+01 + <_> + 1 + 410908 + 1.8442427962608809e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 2.5781446695327759e-01 + 9.7253596782684326e-01 + <_> + 2 + 202305 + 1.8442427962608809e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 208603 + -1.8442427962608809e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 529041 + -1.8442427962608809e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 2.8816136717796326e-01 + 9.3519508838653564e-01 + <_> + 2 + 94933 + 1.8442427962608809e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 434108 + -1.8442427962608809e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.6971946250957687e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 5.2426284551620483e-01 + 5.1751437187194824e+00 + <_> + 1 + 846526 + 1.6971946250957687e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 4.5070362091064453e-01 + 1.8711274489760399e-02 + <_> + 2 + 169058 + -1.6971946250957687e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 677468 + 1.6971946250957687e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 93423 + -1.6971946250957687e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 9.1624699532985687e-02 + 1.0563714981079102e+01 + <_> + 2 + 17329 + 1.6971946250957687e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 76094 + -1.6971946250957687e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -2.1727208506559886e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.2861344814300537e-01 + 2.9034343719482422e+01 + <_> + 1 + 276366 + 2.1727208506559886e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 1.1707996577024460e-01 + 3.9455897808074951e+00 + <_> + 2 + 261525 + 2.1727208506559886e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 14841 + -2.1727208506559886e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 663583 + -2.1727208506559886e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 4.3702536821365356e-01 + 3.5542198181152344e+01 + <_> + 2 + 15913 + 2.1727208506559886e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 647670 + -2.1727208506559886e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 2.0684197195656920e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>3 + 5.3254318237304688e-01 + 7.8003687858581543e+00 + <_> + 1 + 157905 + -2.0684197195656920e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 1.2717868387699127e-01 + 2.9402675628662109e+01 + <_> + 2 + 29009 + 2.0684197195656920e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 128896 + -2.0684197195656920e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 782044 + 2.0684197195656920e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 4.2434823513031006e-01 + 5.9840869140625000e+02 + <_> + 2 + 16106 + -2.0684197195656920e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 765938 + 2.0684197195656920e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.4850088785745186e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 5.2558284997940063e-01 + 3.7336519360542297e-01 + <_> + 1 + 808198 + -1.4850088785745186e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 5.0735270977020264e-01 + 1.8960458755493164e+01 + <_> + 2 + 498995 + -1.4850088785745186e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 309203 + 1.4850088785745186e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 131751 + 1.4850088785745186e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 2.9704423621296883e-02 + 2.1447370529174805e+01 + <_> + 2 + 88473 + 1.4850088785745186e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 43278 + -1.4850088785745186e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -2.7919352867831865e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.2836149930953979e-01 + 3.7081085205078125e+01 + <_> + 1 + 461617 + 2.7919352867831865e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 1.8942002952098846e-01 + 4.3669052124023438e+00 + <_> + 2 + 441471 + 2.7919352867831865e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 20146 + -2.7919352867831865e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 478332 + -2.7919352867831865e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 3.7992846965789795e-01 + 3.9426755905151367e+00 + <_> + 2 + 360116 + -2.7919352867831865e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 118216 + 2.7919352867831865e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.6013950970421839e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.2049374580383301e-01 + 6.3209609985351562e+01 + <_> + 1 + 786153 + 1.6013950970421839e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 4.3022614717483521e-01 + 1.2938302612304688e+02 + <_> + 2 + 270724 + -1.6013950970421839e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 515429 + 1.6013950970421839e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 153796 + -1.6013950970421839e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 1.0972338169813156e-01 + 2.3527720570564270e-01 + <_> + 2 + 122903 + -1.6013950970421839e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 30893 + 1.6013950970421839e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.7737709217655662e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 5.2222496271133423e-01 + 9.5088806152343750e+01 + <_> + 1 + 681902 + -1.7737709217655662e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 3.9270564913749695e-01 + 9.6021175384521484e-01 + <_> + 2 + 179417 + 1.7737709217655662e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 502485 + -1.7737709217655662e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 258047 + 1.7737709217655662e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 1.5152271091938019e-01 + 9.6046769618988037e-01 + <_> + 2 + 79947 + -1.7737709217655662e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 178100 + 1.7737709217655662e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 2.1806181652265189e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 5.2576917409896851e-01 + 1.6350000000000000e+02 + <_> + 1 + 384399 + 2.1806181652265189e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 3.1430721282958984e-01 + 2.6305556297302246e+00 + <_> + 2 + 223480 + 2.1806181652265189e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 160919 + -2.1806181652265189e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 555550 + -2.1806181652265189e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 2.3999322950839996e-01 + 5.1471831054687500e+03 + <_> + 2 + 485049 + -2.1806181652265189e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 70501 + 2.1806181652265189e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.8521138212693108e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 5.2972471714019775e-01 + 5.2022891044616699e+00 + <_> + 1 + 847311 + 1.8521138212693108e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>1 + 4.5997777581214905e-01 + 3.1454174041748047e+01 + <_> + 2 + 9259 + -1.8521138212693108e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 838052 + 1.8521138212693108e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 92638 + -1.8521138212693108e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 8.6193166673183441e-02 + 1.0563714981079102e+01 + <_> + 2 + 17122 + 1.8521138212693108e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 75516 + -1.8521138212693108e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.4638727039226834e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 5.2762317657470703e-01 + 9.0940940380096436e-01 + <_> + 1 + 57887 + 1.4638727039226834e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 1.3349481858313084e-02 + 1.1680285644531250e+02 + <_> + 2 + 57746 + 1.4638727039226834e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 141 + -1.4638727039226834e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 882062 + -1.4638727039226834e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 5.2318209409713745e-01 + 8.8682480156421661e-02 + <_> + 2 + 9315 + 1.4638727039226834e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 872747 + -1.4638727039226834e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 2.2951140356447819e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.2372467517852783e-01 + 1.0487126464843750e+03 + <_> + 1 + 330941 + -2.2951140356447819e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 2.1193994581699371e-01 + 3.5090618133544922e+01 + <_> + 2 + 46001 + 2.2951140356447819e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 284940 + -2.2951140356447819e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 609008 + 2.2951140356447819e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 3.4518736600875854e-01 + 5.1502326965332031e+01 + <_> + 2 + 73365 + -2.2951140356447819e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 535643 + 2.2951140356447819e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.8454164097795372e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 5.2249705791473389e-01 + 2.6138887405395508e+00 + <_> + 1 + 382622 + 1.8454164097795372e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 2.3500546813011169e-01 + 7.0575776100158691e+00 + <_> + 2 + 78502 + -1.8454164097795372e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 304120 + 1.8454164097795372e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 557327 + -1.8454164097795372e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 3.1099945306777954e-01 + 9.0427994728088379e-01 + <_> + 2 + 49346 + 1.8454164097795372e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 507981 + -1.8454164097795372e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.2892082822737772e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.2249181270599365e-01 + 7.7215759277343750e+01 + <_> + 1 + 906033 + 1.2892082822737772e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 5.0599133968353271e-01 + 7.0277774333953857e-01 + <_> + 2 + 17903 + -1.2892082822737772e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 888130 + 1.2892082822737772e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 33916 + -1.2892082822737772e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>1 + 2.6194272562861443e-02 + 1.6466418457031250e+02 + <_> + 2 + 30502 + -1.2892082822737772e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 3414 + 1.2892082822737772e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.6751074352116604e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.2797514200210571e-01 + 2.4551137924194336e+01 + <_> + 1 + 168426 + 1.6751074352116604e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>3 + 8.3729520440101624e-02 + 6.9994598627090454e-01 + <_> + 2 + 1265 + -1.6751074352116604e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 167161 + 1.6751074352116604e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 771523 + -1.6751074352116604e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>10 + 4.5805051922798157e-01 + 9.7318449020385742e+00 + <_> + 2 + 662516 + -1.6751074352116604e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 109007 + 1.6751074352116604e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.5364987958804799e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 5.2594870328903198e-01 + 1.1250000000000000e+00 + <_> + 1 + 82406 + -1.5364987958804799e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>3 + 6.8763710558414459e-02 + 3.7839164733886719e+00 + <_> + 2 + 20439 + 1.5364987958804799e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 61967 + -1.5364987958804799e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 857543 + 1.5364987958804799e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 4.6957337856292725e-01 + 1.0024337768554688e+00 + <_> + 2 + 45638 + -1.5364987958804799e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 811905 + 1.5364987958804799e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.2347176411268729e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 5.2103263139724731e-01 + 8.8440839843750000e+03 + <_> + 1 + 899242 + -1.2347176411268729e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 4.9286371469497681e-01 + 3.7516337633132935e-01 + <_> + 2 + 770350 + -1.2347176411268729e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 128892 + 1.2347176411268729e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 40707 + 1.2347176411268729e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 3.7965066730976105e-02 + 9.5329582691192627e-03 + <_> + 2 + 4405 + -1.2347176411268729e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 36302 + 1.2347176411268729e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.2215436688987001e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.1995581388473511e-01 + 3.6072143554687500e+02 + <_> + 1 + 16348 + -1.2215436688987001e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>3 + 3.6753557622432709e-02 + 3.7838444709777832e+00 + <_> + 2 + 13358 + 1.2215436688987001e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 2990 + -1.2215436688987001e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 923601 + 1.2215436688987001e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 4.9374711513519287e-01 + 8.1318756103515625e+02 + <_> + 2 + 100202 + -1.2215436688987001e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 823399 + 1.2215436688987001e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.0965676087322185e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 5.2008521556854248e-01 + 9.0427994728088379e-01 + <_> + 1 + 49346 + 1.0965676087322185e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 8.6742490530014038e-03 + 1.1682679748535156e+02 + <_> + 2 + 49259 + 1.0965676087322185e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 87 + -1.0965676087322185e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 890603 + -1.0965676087322185e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>4 + 5.1871252059936523e-01 + 1.1238658142089844e+02 + <_> + 2 + 822802 + -1.0965676087322185e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 67801 + 1.0965676087322185e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.1474082076758624e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.1933848857879639e-01 + 3.6072143554687500e+02 + <_> + 1 + 16348 + -1.1474082076758624e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>13 + 3.5487469285726547e-02 + 1.1056249618530273e+01 + <_> + 2 + 13705 + 1.1474082076758624e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 2643 + -1.1474082076758624e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 923601 + 1.1474082076758624e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 4.9316629767417908e-01 + 3.0309746093750000e+03 + <_> + 2 + 831922 + 1.1474082076758624e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 91679 + -1.1474082076758624e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.5055644268914459e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 5.2269959449768066e-01 + 9.4933962821960449e-01 + <_> + 1 + 188872 + 1.5055644268914459e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>4 + 6.0675408691167831e-02 + 8.8097274780273438e+01 + <_> + 2 + 168010 + 1.5055644268914459e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 20862 + -1.5055644268914459e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 751077 + -1.5055644268914459e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>10 + 4.7689276933670044e-01 + 9.1141080856323242e+00 + <_> + 2 + 605013 + -1.5055644268914459e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 146064 + 1.5055644268914459e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.8948479037314492e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.2623099088668823e-01 + 1.8762277832031250e+03 + <_> + 1 + 650544 + 1.8948479037314492e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 4.2303678393363953e-01 + 1.9950000000000000e+02 + <_> + 2 + 517230 + 1.8948479037314492e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 133314 + -1.8948479037314492e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 289405 + -1.8948479037314492e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 1.2419318407773972e-01 + 6.5745878906250000e+03 + <_> + 2 + 255626 + -1.8948479037314492e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 33779 + 1.8948479037314492e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.7051521423440452e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 5.2642256021499634e-01 + 9.6477150917053223e-01 + <_> + 1 + 318578 + 1.7051521423440452e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 1.4038184285163879e-01 + 4.4807361602783203e+01 + <_> + 2 + 20941 + -1.7051521423440452e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 297637 + 1.7051521423440452e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 621371 + -1.7051521423440452e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 4.0214395523071289e-01 + 7.9086059570312500e+01 + <_> + 2 + 448865 + -1.7051521423440452e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 172506 + 1.7051521423440452e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.9134869356844561e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 5.2203911542892456e-01 + 1.6350000000000000e+02 + <_> + 1 + 384399 + 1.9134869356844561e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 3.0663001537322998e-01 + 2.9068121686577797e-02 + <_> + 2 + 196438 + -1.9134869356844561e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 187961 + 1.9134869356844561e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 555550 + -1.9134869356844561e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 2.4106173217296600e-01 + 3.6864425659179688e+01 + <_> + 2 + 185514 + 1.9134869356844561e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 370036 + -1.9134869356844561e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.3975961300400136e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.2771258354187012e-01 + 7.8731391906738281e+01 + <_> + 1 + 912492 + 1.3975961300400136e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 5.1268196105957031e-01 + 5.1758052825927734e+01 + <_> + 2 + 3859 + -1.3975961300400136e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 908633 + 1.3975961300400136e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 27457 + -1.3975961300400136e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 2.2201210260391235e-02 + 5.3361110687255859e+00 + <_> + 2 + 8345 + -1.3975961300400136e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 19112 + 1.3975961300400136e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.5625159603632241e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.2351695299148560e-01 + 2.8027894973754883e+01 + <_> + 1 + 254285 + 1.5625159603632241e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 1.0120427608489990e-01 + 9.7941130399703979e-01 + <_> + 2 + 138018 + 1.5625159603632241e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 116267 + -1.5625159603632241e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 685664 + -1.5625159603632241e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 4.3777933716773987e-01 + 3.5512935638427734e+01 + <_> + 2 + 18106 + 1.5625159603632241e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 667558 + -1.5625159603632241e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.7669245099739400e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 5.2532440423965454e-01 + 2.4805555343627930e+00 + <_> + 1 + 361118 + -1.7669245099739400e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 2.1014948189258575e-01 + 2.1776126861572266e+01 + <_> + 2 + 102634 + 1.7669245099739400e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 258484 + -1.7669245099739400e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 578831 + 1.7669245099739400e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 3.3390906453132629e-01 + 4.4582412719726562e+01 + <_> + 2 + 46516 + -1.7669245099739400e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 532315 + 1.7669245099739400e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.5995032971129580e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.2373433113098145e-01 + 4.6447605133056641e+01 + <_> + 1 + 602267 + 1.5995032971129580e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 3.1128957867622375e-01 + 1.1250000000000000e+00 + <_> + 2 + 82406 + -1.5995032971129580e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 519861 + 1.5995032971129580e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 337682 + -1.5995032971129580e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>8 + 2.2861297428607941e-01 + 1.0000002384185791e+00 + <_> + 2 + 240038 + -1.5995032971129580e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 97644 + 1.5995032971129580e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.6020979207732525e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 5.2021014690399170e-01 + 6.0987548828125000e+01 + <_> + 1 + 325825 + 1.6020979207732525e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 2.0665526390075684e-01 + 1.6650000000000000e+02 + <_> + 2 + 250160 + 1.6020979207732525e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 75665 + -1.6020979207732525e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 614124 + -1.6020979207732525e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 3.3331173658370972e-01 + 1.9050000000000000e+02 + <_> + 2 + 299844 + -1.6020979207732525e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 314280 + 1.6020979207732525e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.6906426314122777e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 5.2427899837493896e-01 + 5.5260574340820312e+01 + <_> + 1 + 573951 + -1.6906426314122777e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 2.9843395948410034e-01 + 9.8145365715026855e-01 + <_> + 2 + 436014 + -1.6906426314122777e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 137937 + 1.6906426314122777e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 365998 + 1.6906426314122777e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 2.4373172223567963e-01 + 6.1781429290771484e+01 + <_> + 2 + 198147 + 1.6906426314122777e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 167851 + -1.6906426314122777e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.5463692631694240e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 5.2273249626159668e-01 + 2.2507629394531250e+00 + <_> + 1 + 491873 + 1.5463692631694240e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 2.9699823260307312e-01 + 1.8376313447952271e+00 + <_> + 2 + 160553 + -1.5463692631694240e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 331320 + 1.5463692631694240e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 448076 + -1.5463692631694240e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 2.4158415198326111e-01 + 9.4933933019638062e-01 + <_> + 2 + 107266 + 1.5463692631694240e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 340810 + -1.5463692631694240e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.4687987693291313e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 5.1975530385971069e-01 + 4.6527776718139648e+00 + <_> + 1 + 667876 + 1.4687987693291313e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 4.1499182581901550e-01 + 1.0481826066970825e+00 + <_> + 2 + 170028 + -1.4687987693291313e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 497848 + 1.4687987693291313e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 272073 + -1.4687987693291313e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 1.2166227400302887e-01 + 6.8635722656250000e+03 + <_> + 2 + 239670 + -1.4687987693291313e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 32403 + 1.4687987693291313e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.9384694115623977e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 5.2034705877304077e-01 + 1.8602773547172546e-01 + <_> + 1 + 457925 + -1.9384694115623977e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 2.6380142569541931e-01 + 2.0797789096832275e-01 + <_> + 2 + 262074 + -1.9384694115623977e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 195851 + 1.9384694115623977e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 482024 + 1.9384694115623977e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 2.8450912237167358e-01 + 9.7941803932189941e-01 + <_> + 2 + 361010 + 1.9384694115623977e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 121014 + -1.9384694115623977e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 2.0283655343433965e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 5.3963851928710938e-01 + 3.1497065429687500e+03 + <_> + 1 + 650159 + -2.0283655343433965e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 3.2241764664649963e-01 + 1.6369568109512329e+00 + <_> + 2 + 121279 + 2.0283655343433965e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 528880 + -2.0283655343433965e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 289790 + 2.0283655343433965e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 2.2811833024024963e-01 + 1.0424986839294434e+01 + <_> + 2 + 263120 + 2.0283655343433965e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 26670 + -2.0283655343433965e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.8252863409197770e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 5.1803791522979736e-01 + 1.5967869758605957e-01 + <_> + 1 + 257512 + -1.8252863409197770e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 1.6228677332401276e-01 + 1.3131362915039062e+01 + <_> + 2 + 144396 + -1.8252863409197770e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 113116 + 1.8252863409197770e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 682437 + 1.8252863409197770e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 3.8321912288665771e-01 + 1.3707794189453125e+01 + <_> + 2 + 501887 + 1.8252863409197770e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 180550 + -1.8252863409197770e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.8687429669279840e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 5.1495540142059326e-01 + 3.4712675781250000e+03 + <_> + 1 + 678816 + -1.8687429669279840e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 3.4068343043327332e-01 + 6.6205863952636719e+01 + <_> + 2 + 519594 + -1.8687429669279840e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 159222 + 1.8687429669279840e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 261133 + 1.8687429669279840e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 2.0589964091777802e-01 + 5.8298278808593750e+01 + <_> + 2 + 183219 + 1.8687429669279840e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 77914 + -1.8687429669279840e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.4547844282869651e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.2396368980407715e-01 + 5.3576210021972656e+01 + <_> + 1 + 683091 + 1.4547844282869651e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>13 + 3.6376655101776123e-01 + 9.2831249237060547e+00 + <_> + 2 + 16688 + -1.4547844282869651e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 666403 + 1.4547844282869651e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 256858 + -1.4547844282869651e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 1.7253904044628143e-01 + 7.4679649353027344e+01 + <_> + 2 + 131084 + -1.4547844282869651e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 125774 + 1.4547844282869651e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.3680631128171461e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.2054405212402344e-01 + 2.9034343719482422e+01 + <_> + 1 + 276366 + 1.3680631128171461e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 1.0614043474197388e-01 + 1.3064670562744141e-01 + <_> + 2 + 5663 + -1.3680631128171461e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 270703 + 1.3680631128171461e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 663583 + -1.3680631128171461e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 4.2800790071487427e-01 + 9.4342005252838135e-01 + <_> + 2 + 130928 + 1.3680631128171461e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 532655 + -1.3680631128171461e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.3166429193015805e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.2033382654190063e-01 + 8.4185607910156250e+02 + <_> + 1 + 212441 + -1.3166429193015805e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 1.3941790163516998e-01 + 2.0698678970336914e+01 + <_> + 2 + 6459 + 1.3166429193015805e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 205982 + -1.3166429193015805e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 727508 + 1.3166429193015805e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 3.9345070719718933e-01 + 7.4694442749023438e+00 + <_> + 2 + 622385 + 1.3166429193015805e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 105123 + -1.3166429193015805e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.4080156439319078e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 5.1611179113388062e-01 + 2.3027777671813965e+00 + <_> + 1 + 323748 + 1.4080156439319078e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>13 + 1.8306370079517365e-01 + 1.9970624923706055e+01 + <_> + 2 + 70717 + -1.4080156439319078e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 253031 + 1.4080156439319078e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 616201 + -1.4080156439319078e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 3.5207864642143250e-01 + 9.2954289913177490e-01 + <_> + 2 + 102196 + 1.4080156439319078e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 514005 + -1.4080156439319078e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.4410234755613913e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 5.2150040864944458e-01 + 5.4829515516757965e-02 + <_> + 1 + 711654 + 1.4410234755613913e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 4.7860175371170044e-01 + 2.3050000000000000e+02 + <_> + 2 + 667351 + 1.4410234755613913e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 44303 + -1.4410234755613913e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 228295 + -1.4410234755613913e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 5.7361621409654617e-02 + 1.2693084716796875e+01 + <_> + 2 + 168338 + 1.4410234755613913e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 59957 + -1.4410234755613913e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.9311494705331517e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.2397710084915161e-01 + 1.4861379394531250e+03 + <_> + 1 + 508421 + -1.9311494705331517e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 3.4595966339111328e-01 + 2.9456104278564453e+01 + <_> + 2 + 42394 + 1.9311494705331517e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 466027 + -1.9311494705331517e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 431528 + 1.9311494705331517e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 2.0216959714889526e-01 + 2.0420529785156250e+03 + <_> + 2 + 188549 + -1.9311494705331517e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 242979 + 1.9311494705331517e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.8394724303274260e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 5.2324557304382324e-01 + 1.9303430616855621e-01 + <_> + 1 + 505212 + -1.8394724303274260e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 2.8579345345497131e-01 + 2.2880256175994873e-01 + <_> + 2 + 318141 + -1.8394724303274260e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 187071 + 1.8394724303274260e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 434737 + 1.8394724303274260e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 2.6006412506103516e-01 + 8.8881176757812500e+02 + <_> + 2 + 147501 + -1.8394724303274260e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 287236 + 1.8394724303274260e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.7301285855085843e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>10 + 5.2127557992935181e-01 + 7.2154712677001953e+00 + <_> + 1 + 389038 + 1.7301285855085843e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 3.0573955178260803e-01 + 1.4413627982139587e-01 + <_> + 2 + 70106 + -1.7301285855085843e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 318932 + 1.7301285855085843e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 550911 + -1.7301285855085843e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 2.3740607500076294e-01 + 5.3200410156250000e+03 + <_> + 2 + 487696 + -1.7301285855085843e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 63215 + 1.7301285855085843e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.0268447686943041e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.1760959625244141e-01 + 3.6080279541015625e+02 + <_> + 1 + 16356 + -1.0268447686943041e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>3 + 3.7909720093011856e-02 + 3.7838444709777832e+00 + <_> + 2 + 13361 + 1.0268447686943041e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 2995 + -1.0268447686943041e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 923593 + 1.0268447686943041e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 4.8773887753486633e-01 + 8.9629760742187500e+01 + <_> + 2 + 920140 + 1.0268447686943041e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 3453 + -1.0268447686943041e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.3590493486121011e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 5.2433151006698608e-01 + 1.1377232421875000e+04 + <_> + 1 + 920580 + -1.3590493486121011e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.0934237241744995e-01 + 1.2009352111816406e+02 + <_> + 2 + 848869 + -1.3590493486121011e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 71711 + 1.3590493486121011e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 19369 + 1.3590493486121011e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 2.4581661447882652e-02 + 1.0071091353893280e-02 + <_> + 2 + 2357 + -1.3590493486121011e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 17012 + 1.3590493486121011e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.2084423292202352e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.2157765626907349e-01 + 3.6080279541015625e+02 + <_> + 1 + 16356 + -1.2084423292202352e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>13 + 3.7027254700660706e-02 + 1.1056249618530273e+01 + <_> + 2 + 13708 + 1.2084423292202352e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 2648 + -1.2084423292202352e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 923593 + 1.2084423292202352e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 4.9314710497856140e-01 + 5.9145904541015625e+02 + <_> + 2 + 17720 + -1.2084423292202352e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 905873 + 1.2084423292202352e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.5956087586991674e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.2135306596755981e-01 + 6.2226818084716797e+01 + <_> + 1 + 494857 + -1.5956087586991674e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>8 + 3.5762667655944824e-01 + 5.0000000000000000e-01 + <_> + 2 + 76347 + 1.5956087586991674e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 418510 + -1.5956087586991674e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 445092 + 1.5956087586991674e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 1.8217913806438446e-01 + 4.7987724304199219e+01 + <_> + 2 + 206839 + 1.5956087586991674e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 238253 + -1.5956087586991674e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.2693220142775646e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 5.1857244968414307e-01 + 1.8103691339492798e+00 + <_> + 1 + 288895 + -1.2693220142775646e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 9.4141520559787750e-02 + 1.0433176994323730e+01 + <_> + 2 + 118613 + 1.2693220142775646e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 170282 + -1.2693220142775646e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 651054 + 1.2693220142775646e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 4.3754899501800537e-01 + 6.8305559158325195e+00 + <_> + 2 + 551750 + 1.2693220142775646e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 99304 + -1.2693220142775646e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.5389044359898910e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 5.2371990680694580e-01 + 6.1050945281982422e+01 + <_> + 1 + 652576 + -1.5389044359898910e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 3.6046865582466125e-01 + 7.8545087890625000e+03 + <_> + 2 + 617832 + -1.5389044359898910e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 34744 + 1.5389044359898910e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 287373 + 1.5389044359898910e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 1.7792819440364838e-01 + 3.1845269203186035e+00 + <_> + 2 + 181563 + 1.5389044359898910e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 105810 + -1.5389044359898910e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 8.3870080679042142e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 5.1708382368087769e-01 + 1.0006089210510254e+00 + <_> + 1 + 19467 + -8.3870080679042142e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 2.6617852970957756e-02 + 8.1239227294921875e+02 + <_> + 2 + 2979 + 8.3870080679042142e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 16488 + -8.3870080679042142e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 920482 + 8.3870080679042142e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 4.9433737993240356e-01 + 7.9277893066406250e+01 + <_> + 2 + 895551 + 8.3870080679042142e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 24931 + -8.3870080679042142e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.2289714425892372e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 5.1786142587661743e-01 + 9.4596225023269653e-01 + <_> + 1 + 171277 + 1.2289714425892372e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 5.3401883691549301e-02 + 2.5250000000000000e+02 + <_> + 2 + 159026 + 1.2289714425892372e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 12251 + -1.2289714425892372e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 768672 + -1.2289714425892372e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 4.7728380560874939e-01 + 2.9338821411132812e+01 + <_> + 2 + 251096 + 1.2289714425892372e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 517576 + -1.2289714425892372e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.4276985606023079e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 5.2062207460403442e-01 + 4.0246734619140625e+01 + <_> + 1 + 104202 + -1.4276985606023079e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 1.2249957025051117e-01 + 3.5908348083496094e+01 + <_> + 2 + 62844 + -1.4276985606023079e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 41358 + 1.4276985606023079e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 835747 + 1.4276985606023079e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 4.1313239932060242e-01 + 3.3934539556503296e-01 + <_> + 2 + 831645 + 1.4276985606023079e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 4102 + -1.4276985606023079e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.4237213254602935e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 5.2481311559677124e-01 + 3.0167543888092041e-01 + <_> + 1 + 905329 + -1.4237213254602935e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 5.0417786836624146e-01 + 9.0433025360107422e-01 + <_> + 2 + 49396 + 1.4237213254602935e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 855933 + -1.4237213254602935e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 34620 + 1.4237213254602935e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 3.1355135142803192e-02 + 5.7891723632812500e+02 + <_> + 2 + 7494 + -1.4237213254602935e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 27126 + 1.4237213254602935e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.1258989768911457e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 5.2006214857101440e-01 + 1.2199418991804123e-01 + <_> + 1 + 49259 + -1.1258989768911457e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 3.1972102820873260e-02 + 1.4835440429687500e+04 + <_> + 2 + 40547 + -1.1258989768911457e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 8712 + 1.1258989768911457e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 890690 + 1.1258989768911457e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 4.9614566564559937e-01 + 2.5550000000000000e+02 + <_> + 2 + 852773 + 1.1258989768911457e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 37917 + -1.1258989768911457e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.2920328520692814e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 5.1880699396133423e-01 + 1.0550000000000000e+02 + <_> + 1 + 39396 + 1.2920328520692814e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 7.9890109598636627e-02 + 2.4086183309555054e-01 + <_> + 2 + 31075 + 1.2920328520692814e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 8321 + -1.2920328520692814e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 900553 + -1.2920328520692814e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 4.5236584544181824e-01 + 2.5679212808609009e-01 + <_> + 2 + 478197 + -1.2920328520692814e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 422356 + 1.2920328520692814e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.4485991530951922e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 5.1966798305511475e-01 + 2.3416333007812500e+03 + <_> + 1 + 542428 + -1.4485991530951922e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 2.4750557541847229e-01 + 2.1095451712608337e-01 + <_> + 2 + 68162 + 1.4485991530951922e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 474266 + -1.4485991530951922e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 397521 + 1.4485991530951922e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 2.8864619135856628e-01 + 4.9547256469726562e+01 + <_> + 2 + 131064 + -1.4485991530951922e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 266457 + 1.4485991530951922e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.3373559171674387e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 5.2121621370315552e-01 + 7.4969604492187500e+01 + <_> + 1 + 512906 + 1.3373559171674387e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>3 + 2.9279240965843201e-01 + 6.9994598627090454e-01 + <_> + 2 + 1263 + -1.3373559171674387e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 511643 + 1.3373559171674387e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 427043 + -1.3373559171674387e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 2.4059174954891205e-01 + 1.0375577926635742e+01 + <_> + 2 + 76775 + 1.3373559171674387e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 350268 + -1.3373559171674387e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.5164341257408578e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 5.1784980297088623e-01 + 1.2489745140075684e+01 + <_> + 1 + 553861 + -1.5164341257408578e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>10 + 3.6461788415908813e-01 + 8.4897260665893555e+00 + <_> + 2 + 442286 + -1.5164341257408578e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 111575 + 1.5164341257408578e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 386088 + 1.5164341257408578e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 1.7322050034999847e-01 + 2.5436753034591675e-01 + <_> + 2 + 349558 + 1.5164341257408578e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 36530 + -1.5164341257408578e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.7327241134309210e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 5.2641940116882324e-01 + 5.2638893127441406e+00 + <_> + 1 + 718053 + 1.7327241134309210e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 4.4531148672103882e-01 + 7.8703811645507812e+01 + <_> + 2 + 710143 + 1.7327241134309210e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 7910 + -1.7327241134309210e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 221896 + -1.7327241134309210e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 9.7898557782173157e-02 + 8.1936653137207031e+01 + <_> + 2 + 193562 + -1.7327241134309210e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 28334 + 1.7327241134309210e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.3521308827747153e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 5.2151274681091309e-01 + 3.6544204711914062e+02 + <_> + 1 + 853045 + -1.3521308827747153e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 4.6195203065872192e-01 + 9.4341659545898438e-01 + <_> + 2 + 141548 + 1.3521308827747153e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 711497 + -1.3521308827747153e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 86904 + 1.3521308827747153e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 7.1799837052822113e-02 + 4.5963447265625000e+03 + <_> + 2 + 60409 + 1.3521308827747153e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 26495 + -1.3521308827747153e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.0633895788839619e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 5.1699042320251465e-01 + 7.4694442749023438e+00 + <_> + 1 + 834826 + 1.0633895788839619e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 4.8435679078102112e-01 + 8.9855255126953125e+01 + <_> + 2 + 833482 + 1.0633895788839619e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 1344 + -1.0633895788839619e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 105123 + -1.0633895788839619e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 4.2202927172183990e-02 + 9.0213543176651001e-01 + <_> + 2 + 43885 + 1.0633895788839619e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 61238 + -1.0633895788839619e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.3425997128059089e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>10 + 5.1892316341400146e-01 + 9.6124658584594727e+00 + <_> + 1 + 821257 + -1.3425997128059089e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 5.0282526016235352e-01 + 9.5285648107528687e-01 + <_> + 2 + 190053 + 1.3425997128059089e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 631204 + -1.3425997128059089e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 118692 + 1.3425997128059089e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 3.0689425766468048e-02 + 1.4613538742065430e+01 + <_> + 2 + 96472 + 1.3425997128059089e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 22220 + -1.3425997128059089e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.2084796930501578e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 5.2112770080566406e-01 + 6.3972225189208984e+00 + <_> + 1 + 790326 + 1.2084796930501578e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 4.7172585129737854e-01 + 1.8601902008056641e+01 + <_> + 2 + 777627 + 1.2084796930501578e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 12699 + -1.2084796930501578e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 149623 + -1.2084796930501578e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 5.8449432253837585e-02 + 9.0213537216186523e-01 + <_> + 2 + 45641 + 1.2084796930501578e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 103982 + -1.2084796930501578e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.5604750785430646e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 5.1841139793395996e-01 + 9.8416671752929688e+00 + <_> + 1 + 271208 + 1.5604750785430646e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 1.6956667602062225e-01 + 2.3599453270435333e-01 + <_> + 2 + 129472 + 1.5604750785430646e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 141736 + -1.5604750785430646e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 668741 + -1.5604750785430646e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 3.6936622858047485e-01 + 3.6364509582519531e+01 + <_> + 2 + 198935 + 1.5604750785430646e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 469806 + -1.5604750785430646e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.6264306971587122e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.2288913726806641e-01 + 1.0529658203125000e+03 + <_> + 1 + 333942 + -1.6264306971587122e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 2.1534834802150726e-01 + 9.6991807222366333e-01 + <_> + 2 + 31568 + 1.6264306971587122e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 302374 + -1.6264306971587122e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 606007 + 1.6264306971587122e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 3.2522302865982056e-01 + 5.3277183532714844e+01 + <_> + 2 + 80663 + -1.6264306971587122e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 525344 + 1.6264306971587122e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.4208959999182846e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 5.1991397142410278e-01 + 6.0987548828125000e+01 + <_> + 1 + 325825 + 1.4208959999182846e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 2.1276226639747620e-01 + 1.0893228530883789e+01 + <_> + 2 + 221101 + 1.4208959999182846e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 104724 + -1.4208959999182846e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 614124 + -1.4208959999182846e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 3.2270050048828125e-01 + 3.5419574737548828e+01 + <_> + 2 + 199875 + 1.4208959999182846e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 414249 + -1.4208959999182846e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.4667734861774334e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>13 + 5.2283686399459839e-01 + 2.4676876068115234e+01 + <_> + 1 + 179104 + -1.4667734861774334e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 1.4106029272079468e-01 + 3.5950222015380859e+01 + <_> + 2 + 168435 + -1.4667734861774334e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 10669 + 1.4667734861774334e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 760845 + 1.4667734861774334e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 3.9554345607757568e-01 + 9.1226123046875000e+02 + <_> + 2 + 116213 + -1.4667734861774334e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 644632 + 1.4667734861774334e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.6234018299859460e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 5.2172207832336426e-01 + 2.3297649621963501e-01 + <_> + 1 + 722088 + -1.6234018299859460e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 3.9708167314529419e-01 + 3.4960517883300781e+01 + <_> + 2 + 291573 + 1.6234018299859460e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 430515 + -1.6234018299859460e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 217861 + 1.6234018299859460e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 1.4341448247432709e-01 + 1.8008959293365479e+00 + <_> + 2 + 101310 + -1.6234018299859460e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 116551 + 1.6234018299859460e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.8243375701631986e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.2128857374191284e-01 + 4.1433364868164062e+01 + <_> + 1 + 532563 + -1.8243375701631986e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 2.5012785196304321e-01 + 3.1776718795299530e-02 + <_> + 2 + 251105 + -1.8243375701631986e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 281458 + 1.8243375701631986e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 407386 + 1.8243375701631986e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 2.9535451531410217e-01 + 9.1712036132812500e+00 + <_> + 2 + 31830 + -1.8243375701631986e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 375556 + 1.8243375701631986e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.4814374665074947e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.2290439605712891e-01 + 4.1884216308593750e+01 + <_> + 1 + 143859 + 1.4814374665074947e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>8 + 1.5557292103767395e-01 + 5.0000000000000000e-01 + <_> + 2 + 21425 + -1.4814374665074947e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 122434 + 1.4814374665074947e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 796090 + -1.4814374665074947e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 3.8139542937278748e-01 + 3.5223068237304688e+01 + <_> + 2 + 136223 + 1.4814374665074947e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 659867 + -1.4814374665074947e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.2247785767563799e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>10 + 5.1862311363220215e-01 + 5.9579987525939941e+00 + <_> + 1 + 115200 + -1.2247785767563799e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 1.3730363547801971e-01 + 4.0930297851562500e+03 + <_> + 2 + 84001 + -1.2247785767563799e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 31199 + 1.2247785767563799e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 824749 + 1.2247785767563799e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 3.9327761530876160e-01 + 1.0054676532745361e+00 + <_> + 2 + 54240 + -1.2247785767563799e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 770509 + 1.2247785767563799e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.2486536270025078e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 5.1463896036148071e-01 + 1.5817348957061768e+00 + <_> + 1 + 427806 + 1.2486536270025078e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 2.5202721357345581e-01 + 2.0350000000000000e+02 + <_> + 2 + 313843 + 1.2486536270025078e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 113963 + -1.2486536270025078e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 512143 + -1.2486536270025078e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 2.7914863824844360e-01 + 9.5588791370391846e-01 + <_> + 2 + 143698 + 1.2486536270025078e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 368445 + -1.2486536270025078e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.0171605590349064e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.1451677083969116e-01 + 2.1128759384155273e+01 + <_> + 1 + 95306 + 1.0171605590349064e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 6.0613598674535751e-02 + 5.1695160865783691e+00 + <_> + 2 + 92134 + 1.0171605590349064e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 3172 + -1.0171605590349064e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 844643 + -1.0171605590349064e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>10 + 4.6479350328445435e-01 + 9.6125583648681641e+00 + <_> + 2 + 726022 + -1.0171605590349064e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 118621 + 1.0171605590349064e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.3231017257577496e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.2201497554779053e-01 + 6.9068542480468750e+01 + <_> + 1 + 843967 + 1.3231017257577496e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 4.6740609407424927e-01 + 2.5941801071166992e+00 + <_> + 2 + 9338 + -1.3231017257577496e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 834629 + 1.3231017257577496e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 95982 + -1.3231017257577496e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 6.5623268485069275e-02 + 7.1581406250000000e+03 + <_> + 2 + 90445 + -1.3231017257577496e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 5537 + 1.3231017257577496e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.1356070758383358e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 5.1551055908203125e-01 + 7.7179107666015625e+01 + <_> + 1 + 829492 + -1.1356070758383358e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 4.6923360228538513e-01 + 2.0744129180908203e+01 + <_> + 2 + 87817 + 1.1356070758383358e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 741675 + -1.1356070758383358e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 110457 + 1.1356070758383358e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>8 + 5.9126112610101700e-02 + 7.2500000000000000e+01 + <_> + 2 + 106361 + 1.1356070758383358e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 4096 + -1.1356070758383358e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.0221608748266943e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 5.1641017198562622e-01 + 4.0583753585815430e+00 + <_> + 1 + 36568 + -1.0221608748266943e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 6.5990567207336426e-02 + 3.3244024658203125e+02 + <_> + 2 + 11030 + 1.0221608748266943e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 25538 + -1.0221608748266943e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 903381 + 1.0221608748266943e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>8 + 4.5954123139381409e-01 + 8.8500000000000000e+01 + <_> + 2 + 856331 + 1.0221608748266943e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 47050 + -1.0221608748266943e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.3467197639084488e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 5.1407432556152344e-01 + 3.5572050781250000e+03 + <_> + 1 + 686172 + -1.3467197639084488e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 3.4165552258491516e-01 + 3.0067000538110733e-02 + <_> + 2 + 217207 + 1.3467197639084488e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 468965 + -1.3467197639084488e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 253777 + 1.3467197639084488e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 1.9196166098117828e-01 + 2.9605170711874962e-02 + <_> + 2 + 129310 + -1.3467197639084488e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 124467 + 1.3467197639084488e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.6468979172033224e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 5.1478171348571777e-01 + 3.7368781864643097e-02 + <_> + 1 + 467331 + -1.6468979172033224e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 3.5363423824310303e-01 + 1.0444276809692383e+01 + <_> + 2 + 133007 + 1.6468979172033224e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 334324 + -1.6468979172033224e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 472618 + 1.6468979172033224e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 1.8744541704654694e-01 + 1.8723413348197937e-01 + <_> + 2 + 190752 + -1.6468979172033224e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 281866 + 1.6468979172033224e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.5859764780610144e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 5.2198135852813721e-01 + 1.0909145355224609e+01 + <_> + 1 + 400579 + -1.5859764780610144e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 2.6805472373962402e-01 + 2.6204687356948853e-01 + <_> + 2 + 331060 + -1.5859764780610144e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 69519 + 1.5859764780610144e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 539370 + 1.5859764780610144e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 2.7151179313659668e-01 + 2.2793330078125000e+03 + <_> + 2 + 283326 + -1.5859764780610144e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 256044 + 1.5859764780610144e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.4508596285957101e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.1847738027572632e-01 + 5.3962562561035156e+01 + <_> + 1 + 687292 + 1.4508596285957101e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 3.7063494324684143e-01 + 1.2416666746139526e+00 + <_> + 2 + 103570 + -1.4508596285957101e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 583722 + 1.4508596285957101e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 252657 + -1.4508596285957101e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>10 + 1.6557306051254272e-01 + 7.0749759674072266e+00 + <_> + 2 + 24861 + 1.4508596285957101e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 227796 + -1.4508596285957101e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.4180929097925615e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.2262759208679199e-01 + 6.3897567749023438e+01 + <_> + 1 + 516984 + -1.4180929097925615e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 3.6749491095542908e-01 + 7.3569531440734863e+00 + <_> + 2 + 67561 + 1.4180929097925615e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 449423 + -1.4180929097925615e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 422965 + 1.4180929097925615e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>8 + 1.6789813339710236e-01 + 5.0000000000000000e-01 + <_> + 2 + 49139 + -1.4180929097925615e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 373826 + 1.4180929097925615e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.2361131053733582e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 5.1455682516098022e-01 + 1.3583366699218750e+03 + <_> + 1 + 324954 + -1.2361131053733582e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 1.2238895148038864e-01 + 1.9750000000000000e+02 + <_> + 2 + 213276 + -1.2361131053733582e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 111678 + 1.2361131053733582e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 614995 + 1.2361131053733582e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 4.0847459435462952e-01 + 2.0350000000000000e+02 + <_> + 2 + 467670 + 1.2361131053733582e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 147325 + -1.2361131053733582e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.4792039486798814e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.1682090759277344e-01 + 7.5420074462890625e+01 + <_> + 1 + 667538 + -1.4792039486798814e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 4.4248783588409424e-01 + 2.4599638581275940e-01 + <_> + 2 + 543365 + -1.4792039486798814e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 124173 + 1.4792039486798814e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 272411 + 1.4792039486798814e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 9.4424985349178314e-02 + 2.5103190541267395e-01 + <_> + 2 + 238322 + 1.4792039486798814e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 34089 + -1.4792039486798814e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 9.7277636957457184e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.1722711324691772e-01 + 3.5654925537109375e+02 + <_> + 1 + 15966 + -9.7277636957457184e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>3 + 3.6826536059379578e-02 + 3.7838444709777832e+00 + <_> + 2 + 13121 + 9.7277636957457184e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 2845 + -9.7277636957457184e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 923983 + 9.7277636957457184e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>13 + 4.8747372627258301e-01 + 7.9001876831054688e+01 + <_> + 2 + 706141 + 9.7277636957457184e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 217842 + -9.7277636957457184e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.3144272861567094e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.1552879810333252e-01 + 8.1480682373046875e+01 + <_> + 1 + 733147 + -1.3144272861567094e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 4.5887055993080139e-01 + 1.4556673049926758e+01 + <_> + 2 + 530667 + -1.3144272861567094e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 202480 + 1.3144272861567094e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 206802 + 1.3144272861567094e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 7.3942884802818298e-02 + 1.3909103393554688e+01 + <_> + 2 + 147067 + 1.3144272861567094e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 59735 + -1.3144272861567094e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.2226850761556346e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.2325713634490967e-01 + 3.5654925537109375e+02 + <_> + 1 + 15966 + -1.2226850761556346e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 3.6128357052803040e-02 + 3.8128118515014648e+00 + <_> + 2 + 13159 + 1.2226850761556346e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 2807 + -1.2226850761556346e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 923983 + 1.2226850761556346e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 4.9440073966979980e-01 + 5.9719335937500000e+02 + <_> + 2 + 19012 + -1.2226850761556346e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 904971 + 1.2226850761556346e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -9.8349571520979923e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.1762080192565918e-01 + 1.1996318054199219e+02 + <_> + 1 + 868110 + -9.8349571520979923e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 5.1397484540939331e-01 + 9.2957198619842529e-01 + <_> + 2 + 102230 + 9.8349571520979923e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 765880 + -9.8349571520979923e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 71839 + 9.8349571520979923e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>8 + 1.0592775419354439e-02 + 1.6350000000000000e+02 + <_> + 2 + 71762 + 9.8349571520979923e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 77 + -9.8349571520979923e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.3987604208494389e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 5.1908642053604126e-01 + 4.7694444656372070e+00 + <_> + 1 + 676721 + 1.3987604208494389e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 4.2447528243064880e-01 + 1.9250000000000000e+02 + <_> + 2 + 486768 + 1.3987604208494389e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 189953 + -1.3987604208494389e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 263228 + -1.3987604208494389e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 1.1043683439493179e-01 + 6.8635722656250000e+03 + <_> + 2 + 231851 + -1.3987604208494389e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 31377 + 1.3987604208494389e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.1951059624486883e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.2114754915237427e-01 + 7.2257553100585938e+01 + <_> + 1 + 626697 + -1.1951059624486883e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 4.2197787761688232e-01 + 9.5526528358459473e-01 + <_> + 2 + 160084 + 1.1951059624486883e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 466613 + -1.1951059624486883e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 313252 + 1.1951059624486883e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 1.0786426812410355e-01 + 1.7019056701660156e+02 + <_> + 2 + 59640 + -1.1951059624486883e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 253612 + 1.1951059624486883e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.4474907604567777e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 5.1662522554397583e-01 + 1.1965702056884766e+01 + <_> + 1 + 500134 + 1.4474907604567777e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>10 + 3.3947950601577759e-01 + 5.9742970466613770e+00 + <_> + 2 + 76699 + -1.4474907604567777e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 423435 + 1.4474907604567777e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 439815 + -1.4474907604567777e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 1.9664470851421356e-01 + 1.5965084731578827e-01 + <_> + 2 + 75240 + 1.4474907604567777e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 364575 + -1.4474907604567777e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.3154829226993328e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.1839280128479004e-01 + 4.5136322021484375e+01 + <_> + 1 + 586520 + 1.3154829226993328e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>3 + 2.9840365052223206e-01 + 6.9994598627090454e-01 + <_> + 2 + 1265 + -1.3154829226993328e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 585255 + 1.3154829226993328e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 353429 + -1.3154829226993328e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>10 + 2.3443609476089478e-01 + 9.0865497589111328e+00 + <_> + 2 + 209530 + -1.3154829226993328e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 143899 + 1.3154829226993328e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.4100987143215393e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.1774358749389648e-01 + 4.1433364868164062e+01 + <_> + 1 + 532563 + -1.4100987143215393e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 2.4812401831150055e-01 + 2.4100942611694336e+01 + <_> + 2 + 157586 + 1.4100987143215393e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 374977 + -1.4100987143215393e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 407386 + 1.4100987143215393e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 2.8707015514373779e-01 + 9.9245328903198242e+00 + <_> + 2 + 52587 + -1.4100987143215393e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 354799 + 1.4100987143215393e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.3452405231804127e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 5.1579374074935913e-01 + 1.5109452605247498e-01 + <_> + 1 + 116461 + 1.3452405231804127e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 1.4258930087089539e-01 + 4.1246064007282257e-02 + <_> + 2 + 104559 + 1.3452405231804127e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 11902 + -1.3452405231804127e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 823488 + -1.3452405231804127e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 3.9099109172821045e-01 + 4.1604399681091309e-02 + <_> + 2 + 427581 + -1.3452405231804127e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 395907 + 1.3452405231804127e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.3037264899262682e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.1930791139602661e-01 + 4.4155387878417969e+01 + <_> + 1 + 574521 + 1.3037264899262682e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 2.8360489010810852e-01 + 3.6072143554687500e+02 + <_> + 2 + 16348 + -1.3037264899262682e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 558173 + 1.3037264899262682e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 365428 + -1.3037264899262682e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>13 + 2.4894219636917114e-01 + 4.7503124237060547e+01 + <_> + 2 + 47521 + 1.3037264899262682e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 317907 + -1.3037264899262682e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.2324102120564007e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>13 + 5.1704293489456177e-01 + 6.1890625000000000e+01 + <_> + 1 + 620875 + -1.2324102120564007e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>8 + 3.8868343830108643e-01 + 5.9500000000000000e+01 + <_> + 2 + 564940 + -1.2324102120564007e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 55935 + 1.2324102120564007e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 319074 + 1.2324102120564007e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 1.4208786189556122e-01 + 2.2295611572265625e+02 + <_> + 2 + 77580 + -1.2324102120564007e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 241494 + 1.2324102120564007e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.2492877877159057e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 5.1573222875595093e-01 + 4.4779390096664429e-02 + <_> + 1 + 594460 + 1.2492877877159057e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 4.1906371712684631e-01 + 5.2443717956542969e+01 + <_> + 2 + 431442 + 1.2492877877159057e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 163018 + -1.2492877877159057e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 345489 + -1.2492877877159057e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>10 + 1.1212792247533798e-01 + 1.0354104995727539e+01 + <_> + 2 + 272248 + -1.2492877877159057e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 73241 + 1.2492877877159057e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.4873554760957200e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 5.2108299732208252e-01 + 5.4158996582031250e+01 + <_> + 1 + 556828 + -1.4873554760957200e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 2.9137802124023438e-01 + 6.6263771057128906e+01 + <_> + 2 + 432326 + -1.4873554760957200e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 124502 + 1.4873554760957200e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 383121 + 1.4873554760957200e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 2.4573744833469391e-01 + 1.2470302734375000e+03 + <_> + 2 + 101857 + -1.4873554760957200e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 281264 + 1.4873554760957200e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.2486195147091961e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 5.1829880475997925e-01 + 7.0155502319335938e+01 + <_> + 1 + 452822 + 1.2486195147091961e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 2.6847642660140991e-01 + 1.1205345392227173e-01 + <_> + 2 + 17667 + -1.2486195147091961e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 435155 + 1.2486195147091961e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 487127 + -1.2486195147091961e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 2.6269856095314026e-01 + 1.0453325271606445e+01 + <_> + 2 + 106002 + 1.2486195147091961e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 381125 + -1.2486195147091961e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.1091855283682912e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.1336604356765747e-01 + 5.7762901306152344e+01 + <_> + 1 + 429016 + -1.1091855283682912e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 3.1351360678672791e-01 + 3.0278734862804413e-02 + <_> + 2 + 258854 + -1.1091855283682912e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 170162 + 1.1091855283682912e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 510933 + 1.1091855283682912e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 2.1418765187263489e-01 + 1.1039464950561523e+01 + <_> + 2 + 171790 + -1.1091855283682912e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 339143 + 1.1091855283682912e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.1807985698104093e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.1687234640121460e-01 + 4.0402381896972656e+01 + <_> + 1 + 126588 + 1.1807985698104093e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 1.4471909403800964e-01 + 2.5544345378875732e-01 + <_> + 2 + 90855 + 1.1807985698104093e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 35733 + -1.1807985698104093e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 813361 + -1.1807985698104093e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 3.8476660847663879e-01 + 2.6123559474945068e-01 + <_> + 2 + 433620 + -1.1807985698104093e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 379741 + 1.1807985698104093e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.1395974899913031e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 5.1238042116165161e-01 + 2.9089414062500000e+03 + <_> + 1 + 623774 + -1.1395974899913031e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 2.9501268267631531e-01 + 9.7030556201934814e-01 + <_> + 2 + 234921 + 1.1395974899913031e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 388853 + -1.1395974899913031e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 316175 + 1.1395974899913031e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 2.3344647884368896e-01 + 6.3548358917236328e+01 + <_> + 2 + 254415 + 1.1395974899913031e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 61760 + -1.1395974899913031e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.0265049289042262e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.1278525590896606e-01 + 2.3064628601074219e+01 + <_> + 1 + 135102 + 1.0265049289042262e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 7.4561521410942078e-02 + 1.7552718520164490e-02 + <_> + 2 + 24419 + -1.0265049289042262e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 110683 + 1.0265049289042262e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 804847 + -1.0265049289042262e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 4.5107859373092651e-01 + 6.9332962036132812e+01 + <_> + 2 + 611078 + -1.0265049289042262e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 193769 + 1.0265049289042262e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.2384311121328677e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.2009183168411255e-01 + 6.4267272949218750e+01 + <_> + 1 + 796750 + 1.2384311121328677e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 4.3617489933967590e-01 + 9.7039550781250000e+02 + <_> + 2 + 158526 + -1.2384311121328677e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 638224 + 1.2384311121328677e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 143199 + -1.2384311121328677e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 9.4746373593807220e-02 + 1.6750000000000000e+02 + <_> + 2 + 19845 + 1.2384311121328677e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 123354 + -1.2384311121328677e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.5532466387272550e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 5.1680976152420044e-01 + 1.9740542769432068e-01 + <_> + 1 + 289800 + -1.5532466387272550e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 2.7329006791114807e-01 + 1.3451740264892578e+01 + <_> + 2 + 169289 + -1.5532466387272550e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 120511 + 1.5532466387272550e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 650149 + 1.5532466387272550e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 2.6546320319175720e-01 + 1.3502442359924316e+01 + <_> + 2 + 473309 + 1.5532466387272550e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 176840 + -1.5532466387272550e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.1368624350818753e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 5.1478189229965210e-01 + 3.5411977539062500e+03 + <_> + 1 + 684785 + -1.1368624350818753e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 3.4158617258071899e-01 + 9.1186157226562500e+01 + <_> + 2 + 477467 + -1.1368624350818753e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 207318 + 1.1368624350818753e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 255164 + 1.1368624350818753e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 1.8680480122566223e-01 + 4.0912404656410217e-02 + <_> + 2 + 192822 + 1.1368624350818753e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 62342 + -1.1368624350818753e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.3728200346062053e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 5.1521629095077515e-01 + 2.0072413635253906e+02 + <_> + 1 + 554701 + 1.3728200346062053e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 2.7609831094741821e-01 + 4.6104877471923828e+01 + <_> + 2 + 171849 + -1.3728200346062053e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 382852 + 1.3728200346062053e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 385248 + -1.3728200346062053e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>10 + 2.5816839933395386e-01 + 6.6138110160827637e+00 + <_> + 2 + 40460 + 1.3728200346062053e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 344788 + -1.3728200346062053e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.0611612458645657e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 5.1484346389770508e-01 + 4.6146593093872070e+00 + <_> + 1 + 827512 + 1.0611612458645657e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 4.2756190896034241e-01 + 1.2464975565671921e-01 + <_> + 2 + 46489 + -1.0611612458645657e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 781023 + 1.0611612458645657e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 112437 + -1.0611612458645657e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 9.8942257463932037e-02 + 2.5423841476440430e+01 + <_> + 2 + 3281 + 1.0611612458645657e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 109156 + -1.0611612458645657e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.1894718739014246e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.1923310756683350e-01 + 2.7271972656250000e+01 + <_> + 1 + 236965 + 1.1894718739014246e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 9.9588334560394287e-02 + 1.4395534992218018e-01 + <_> + 2 + 6948 + -1.1894718739014246e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 230017 + 1.1894718739014246e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 702984 + -1.1894718739014246e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 4.3011343479156494e-01 + 9.3014585971832275e-01 + <_> + 2 + 92184 + 1.1894718739014246e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 610800 + -1.1894718739014246e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.2385256588843900e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 5.1746886968612671e-01 + 2.0555319786071777e+00 + <_> + 1 + 437293 + -1.2385256588843900e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 1.4870209991931915e-01 + 3.1608514487743378e-02 + <_> + 2 + 163992 + 1.2385256588843900e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 273301 + -1.2385256588843900e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 502656 + 1.2385256588843900e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 3.8222151994705200e-01 + 7.4361114501953125e+00 + <_> + 2 + 435779 + 1.2385256588843900e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 66877 + -1.2385256588843900e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.1881115590063121e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 5.1958966255187988e-01 + 6.0846992492675781e+01 + <_> + 1 + 650125 + -1.1881115590063121e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 3.6181405186653137e-01 + 1.7267564535140991e+00 + <_> + 2 + 168602 + 1.1881115590063121e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 481523 + -1.1881115590063121e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 289824 + 1.1881115590063121e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 1.6785383224487305e-01 + 9.8166351318359375e+01 + <_> + 2 + 132726 + 1.1881115590063121e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 157098 + -1.1881115590063121e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.0867374934912219e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 5.1105690002441406e-01 + 2.7210922911763191e-02 + <_> + 1 + 301164 + -1.0867374934912219e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 1.8637789785861969e-01 + 5.1214477539062500e+01 + <_> + 2 + 189626 + -1.0867374934912219e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 111538 + 1.0867374934912219e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 638785 + 1.0867374934912219e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>8 + 3.4076383709907532e-01 + 1.0000002384185791e+00 + <_> + 2 + 490984 + 1.0867374934912219e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 147801 + -1.0867374934912219e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.1372033780795540e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 5.1558148860931396e-01 + 9.9168682098388672e+00 + <_> + 1 + 274797 + 1.1372033780795540e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 1.7489457130432129e-01 + 5.3135444641113281e+01 + <_> + 2 + 148743 + 1.1372033780795540e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 126054 + -1.1372033780795540e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 665152 + -1.1372033780795540e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 3.5350492596626282e-01 + 9.7479581832885742e-01 + <_> + 2 + 374649 + -1.1372033780795540e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 290503 + 1.1372033780795540e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.2368963188103065e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.1900184154510498e-01 + 9.9439648437500000e+02 + <_> + 1 + 296518 + -1.2368963188103065e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 1.9117398560047150e-01 + 1.9879512488842010e-01 + <_> + 2 + 50830 + 1.2368963188103065e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 245688 + -1.2368963188103065e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 643431 + 1.2368963188103065e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 3.3970904350280762e-01 + 3.7326507568359375e+01 + <_> + 2 + 32531 + -1.2368963188103065e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 610900 + 1.2368963188103065e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.0241542372264607e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 5.1623177528381348e-01 + 3.6025115966796875e+02 + <_> + 1 + 848885 + -1.0241542372264607e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 4.5200258493423462e-01 + 1.4941427612304688e+02 + <_> + 2 + 375054 + 1.0241542372264607e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 473831 + -1.0241542372264607e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 91064 + 1.0241542372264607e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 7.3578923940658569e-02 + 7.0114410400390625e+01 + <_> + 2 + 45792 + 1.0241542372264607e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 45272 + -1.0241542372264607e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.2411295974804377e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 5.1660376787185669e-01 + 2.4158200073242188e+02 + <_> + 1 + 643579 + -1.2411295974804377e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 3.4869930148124695e-01 + 1.7450000000000000e+02 + <_> + 2 + 406721 + -1.2411295974804377e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 236858 + 1.2411295974804377e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 296370 + 1.2411295974804377e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 1.8228915333747864e-01 + 2.4527778625488281e+00 + <_> + 2 + 14448 + -1.2411295974804377e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 281922 + 1.2411295974804377e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.1564170880364812e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.1835513114929199e-01 + 4.2798751831054688e+01 + <_> + 1 + 555136 + 1.1564170880364812e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 2.6613765954971313e-01 + 3.1125984191894531e+01 + <_> + 2 + 42378 + -1.1564170880364812e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 512758 + 1.1564170880364812e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 384813 + -1.1564170880364812e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 2.6274058222770691e-01 + 4.7268836975097656e+01 + <_> + 2 + 24399 + 1.1564170880364812e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 360414 + -1.1564170880364812e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.0917740150682652e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 5.1400333642959595e-01 + 4.9472222328186035e+00 + <_> + 1 + 691145 + 1.0917740150682652e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 4.2282542586326599e-01 + 8.0568740844726562e+01 + <_> + 2 + 687955 + 1.0917740150682652e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 3190 + -1.0917740150682652e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 248804 + -1.0917740150682652e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 1.0444184392690659e-01 + 8.1723884582519531e+01 + <_> + 2 + 212769 + -1.0917740150682652e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 36035 + 1.0917740150682652e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.2537516836552562e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.1971763372421265e-01 + 8.1480682373046875e+01 + <_> + 1 + 733147 + -1.2537516836552562e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>4 + 4.6260526776313782e-01 + 2.1177604675292969e+01 + <_> + 2 + 11092 + 1.2537516836552562e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 722055 + -1.2537516836552562e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 206802 + 1.2537516836552562e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 6.8697519600391388e-02 + 1.9150000000000000e+02 + <_> + 2 + 15252 + -1.2537516836552562e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 191550 + 1.2537516836552562e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.2659728036665654e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 5.1381570100784302e-01 + 8.3111286163330078e+00 + <_> + 1 + 133431 + -1.2659728036665654e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 1.1659813672304153e-01 + 3.8756088256835938e+01 + <_> + 2 + 73915 + -1.2659728036665654e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 59516 + 1.2659728036665654e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 806518 + 1.2659728036665654e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 4.1500899195671082e-01 + 1.7086648559570312e+02 + <_> + 2 + 360595 + -1.2659728036665654e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 445923 + 1.2659728036665654e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.2211355649801489e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 5.2027511596679688e-01 + 1.6386668395996094e+02 + <_> + 1 + 446048 + 1.2211355649801489e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 2.0335604250431061e-01 + 1.2683263421058655e-01 + <_> + 2 + 22796 + -1.2211355649801489e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 423252 + 1.2211355649801489e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 493901 + -1.2211355649801489e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 3.2713446021080017e-01 + 2.1524451673030853e-02 + <_> + 2 + 69865 + 1.2211355649801489e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 424036 + -1.2211355649801489e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.0219958308394529e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 5.1336395740509033e-01 + 8.3111286163330078e+00 + <_> + 1 + 133431 + -1.0219958308394529e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 1.1275272816419601e-01 + 3.3606372773647308e-02 + <_> + 2 + 52190 + -1.0219958308394529e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 81241 + 1.0219958308394529e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 806518 + 1.0219958308394529e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 4.1277495026588440e-01 + 1.0038001537322998e+00 + <_> + 2 + 47585 + -1.0219958308394529e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 758933 + 1.0219958308394529e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.0760306532657286e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 5.1740473508834839e-01 + 9.4369812011718750e+00 + <_> + 1 + 249885 + 1.0760306532657286e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>8 + 1.5981103479862213e-01 + 5.0000000000000000e-01 + <_> + 2 + 37729 + -1.0760306532657286e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 212156 + 1.0760306532657286e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 690064 + -1.0760306532657286e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>8 + 3.6706382036209106e-01 + 5.0000000000000000e-01 + <_> + 2 + 90893 + 1.0760306532657286e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 599171 + -1.0760306532657286e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.3823163809371145e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 5.1601469516754150e-01 + 5.3538734436035156e+01 + <_> + 1 + 546805 + -1.3823163809371145e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 2.7973932027816772e-01 + 3.2443816661834717e+00 + <_> + 2 + 467112 + -1.3823163809371145e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 79693 + 1.3823163809371145e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 393144 + 1.3823163809371145e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 2.5476366281509399e-01 + 3.0026710033416748e+00 + <_> + 2 + 249301 + 1.3823163809371145e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 143843 + -1.3823163809371145e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.0474471773266424e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.1146388053894043e-01 + 1.0009554443359375e+03 + <_> + 1 + 300350 + -1.0474471773266424e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>3 + 1.9282492995262146e-01 + 3.7830076217651367e+00 + <_> + 2 + 27909 + 1.0474471773266424e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 272441 + -1.0474471773266424e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 639599 + 1.0474471773266424e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 3.3333733677864075e-01 + 1.4991036987304688e+02 + <_> + 2 + 569472 + 1.0474471773266424e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 70127 + -1.0474471773266424e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.2249949324652215e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 5.1367110013961792e-01 + 2.1334065496921539e-01 + <_> + 1 + 623921 + -1.2249949324652215e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 3.3855026960372925e-01 + 8.1723884582519531e+01 + <_> + 2 + 586489 + -1.2249949324652215e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 37432 + 1.2249949324652215e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 316028 + 1.2249949324652215e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 1.9203636050224304e-01 + 7.3123031616210938e+01 + <_> + 2 + 179816 + 1.2249949324652215e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 136212 + -1.2249949324652215e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.0188552564308727e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.1783776283264160e-01 + 3.6473352050781250e+02 + <_> + 1 + 16709 + -1.0188552564308727e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>3 + 3.8332480937242508e-02 + 3.7838444709777832e+00 + <_> + 2 + 13543 + 1.0188552564308727e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 3166 + -1.0188552564308727e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 923240 + 1.0188552564308727e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 4.8711690306663513e-01 + 3.0449208984375000e+03 + <_> + 2 + 832614 + 1.0188552564308727e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 90626 + -1.0188552564308727e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -9.7906638029029602e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 5.1632207632064819e-01 + 9.3393045663833618e-01 + <_> + 1 + 117895 + 9.7906638029029602e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 3.1006336212158203e-02 + 2.9295075684785843e-02 + <_> + 2 + 110365 + 9.7906638029029602e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 7530 + -9.7906638029029602e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 822054 + -9.7906638029029602e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 4.9345079064369202e-01 + 1.1786351013183594e+02 + <_> + 2 + 748402 + -9.7906638029029602e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 73652 + 9.7906638029029602e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 9.4271281492322459e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.1653790473937988e-01 + 3.5654925537109375e+02 + <_> + 1 + 15966 + -9.4271281492322459e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>13 + 3.6129284650087357e-02 + 1.1056249618530273e+01 + <_> + 2 + 13465 + 9.4271281492322459e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 2501 + -9.4271281492322459e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 923983 + 9.4271281492322459e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 4.8742109537124634e-01 + 1.9024143218994141e+01 + <_> + 2 + 890885 + 9.4271281492322459e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 33098 + -9.4271281492322459e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -9.6775493256729628e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>8 + 5.1508307456970215e-01 + 1.4500000000000000e+01 + <_> + 1 + 763187 + -9.6775493256729628e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 4.1590178012847900e-01 + 8.3762603759765625e+01 + <_> + 2 + 706819 + -9.6775493256729628e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 56368 + 9.6775493256729628e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 176762 + 9.6775493256729628e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 1.0827323049306870e-01 + 4.2593774795532227e+00 + <_> + 2 + 157020 + 9.6775493256729628e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 19742 + -9.6775493256729628e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.1503800523615740e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 5.1214671134948730e-01 + 2.3439809570312500e+03 + <_> + 1 + 542820 + -1.1503800523615740e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 2.4338784813880920e-01 + 1.0790184020996094e+01 + <_> + 2 + 185676 + 1.1503800523615740e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 357144 + -1.1503800523615740e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 397129 + 1.1503800523615740e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 2.8533998131752014e-01 + 4.6162094116210938e+01 + <_> + 2 + 103032 + -1.1503800523615740e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 294097 + 1.1503800523615740e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 8.2044196370148079e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 5.1220917701721191e-01 + 2.8282489776611328e+00 + <_> + 1 + 11716 + -8.2044196370148079e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 3.7428431212902069e-02 + 2.1724937438964844e+01 + <_> + 2 + 5292 + 8.2044196370148079e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 6424 + -8.2044196370148079e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 928233 + 8.2044196370148079e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 4.8307111859321594e-01 + 5.2861108779907227e+00 + <_> + 2 + 708263 + 8.2044196370148079e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 219970 + -8.2044196370148079e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.0796067380229943e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 5.1348280906677246e-01 + 9.6329951286315918e-01 + <_> + 1 + 295869 + 1.0796067380229943e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 1.1721687018871307e-01 + 8.0062957763671875e+01 + <_> + 2 + 162444 + 1.0796067380229943e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 133425 + -1.0796067380229943e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 644080 + -1.0796067380229943e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 4.0974712371826172e-01 + 8.3062034606933594e+01 + <_> + 2 + 414475 + -1.0796067380229943e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 229605 + 1.0796067380229943e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.0417018751059616e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 5.1413953304290771e-01 + 7.8658462524414062e+01 + <_> + 1 + 549829 + 1.0417018751059616e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 3.0912581086158752e-01 + 2.2896571159362793e+00 + <_> + 2 + 261673 + -1.0417018751059616e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 288156 + 1.0417018751059616e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 390120 + -1.0417018751059616e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 2.1689319610595703e-01 + 9.2003417015075684e-01 + <_> + 2 + 44338 + 1.0417018751059616e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 345782 + -1.0417018751059616e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.2691173345409468e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 5.1491510868072510e-01 + 2.8232684135437012e+00 + <_> + 1 + 670000 + 1.2691173345409468e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 2.8502017259597778e-01 + 1.0254147052764893e+00 + <_> + 2 + 112203 + -1.2691173345409468e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 557797 + 1.2691173345409468e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 269949 + -1.2691173345409468e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 2.4666523933410645e-01 + 1.0182608366012573e+00 + <_> + 2 + 52668 + 1.2691173345409468e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 217281 + -1.2691173345409468e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 7.0446876779555689e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.1036065816879272e-01 + 3.5654925537109375e+02 + <_> + 1 + 15966 + -7.0446876779555689e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>13 + 3.5542223602533340e-02 + 1.1056249618530273e+01 + <_> + 2 + 13465 + 7.0446876779555689e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 2501 + -7.0446876779555689e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 923983 + 7.0446876779555689e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 4.8206222057342529e-01 + 3.0305554866790771e+00 + <_> + 2 + 439544 + 7.0446876779555689e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 484439 + -7.0446876779555689e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.3260981113436809e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 5.1856428384780884e-01 + 9.6857696771621704e-01 + <_> + 1 + 390368 + 1.3260981113436809e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 2.1224506199359894e-01 + 3.1626525878906250e+01 + <_> + 2 + 13760 + -1.3260981113436809e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 376608 + 1.3260981113436809e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 549581 + -1.3260981113436809e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 3.2085889577865601e-01 + 6.7078651428222656e+01 + <_> + 2 + 307994 + -1.3260981113436809e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 241587 + 1.3260981113436809e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.1302082434307391e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 5.1571887731552124e-01 + 2.3027777671813965e+00 + <_> + 1 + 323748 + 1.1302082434307391e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 1.9133907556533813e-01 + 1.4779894828796387e+01 + <_> + 2 + 5497 + -1.1302082434307391e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 318251 + 1.1302082434307391e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 616201 + -1.1302082434307391e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 3.3688610792160034e-01 + 1.0265763671875000e+04 + <_> + 2 + 594697 + -1.1302082434307391e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 21504 + 1.1302082434307391e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.1478588045320895e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 5.1961421966552734e-01 + 1.1194443702697754e+00 + <_> + 1 + 81288 + -1.1478588045320895e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>3 + 6.7029125988483429e-02 + 3.7839164733886719e+00 + <_> + 2 + 20326 + 1.1478588045320895e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 60962 + -1.1478588045320895e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 858661 + 1.1478588045320895e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>3 + 4.6163588762283325e-01 + 3.5610752105712891e+00 + <_> + 2 + 8175 + -1.1478588045320895e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 850486 + 1.1478588045320895e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -9.2154603087352516e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 5.1540750265121460e-01 + 3.9386529541015625e+02 + <_> + 1 + 869595 + -9.2154603087352516e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 4.6392014622688293e-01 + 3.9249157714843750e+03 + <_> + 2 + 675440 + -9.2154603087352516e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 194155 + 9.2154603087352516e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 70354 + 9.2154603087352516e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.9102222323417664e-02 + 7.2660476684570312e+01 + <_> + 2 + 41016 + 9.2154603087352516e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 29338 + -9.2154603087352516e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.4404947299507564e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 5.1464146375656128e-01 + 1.9950734078884125e-01 + <_> + 1 + 544507 + -1.4404947299507564e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 2.9966062307357788e-01 + 3.8930503845214844e+01 + <_> + 2 + 254651 + 1.4404947299507564e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 289856 + -1.4404947299507564e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 395442 + 1.4404947299507564e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 2.3628960549831390e-01 + 1.2029939651489258e+01 + <_> + 2 + 165493 + -1.4404947299507564e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 229949 + 1.4404947299507564e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 9.5787362051583683e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 5.1071768999099731e-01 + 4.9669306725263596e-02 + <_> + 1 + 662177 + 9.5787362051583683e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 4.5105466246604919e-01 + 4.1431861877441406e+01 + <_> + 2 + 389725 + -9.5787362051583683e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 272452 + 9.5787362051583683e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 277772 + -9.5787362051583683e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>10 + 7.2873875498771667e-02 + 1.0166900634765625e+01 + <_> + 2 + 199264 + -9.5787362051583683e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 78508 + 9.5787362051583683e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.3179937781298462e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.1794672012329102e-01 + 4.0751495361328125e+01 + <_> + 1 + 521780 + 1.3179937781298462e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 2.3504495620727539e-01 + 5.0337509765625000e+03 + <_> + 2 + 468251 + 1.3179937781298462e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 53529 + -1.3179937781298462e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 418169 + -1.3179937781298462e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 2.9785728454589844e-01 + 4.5600231170654297e+01 + <_> + 2 + 23329 + 1.3179937781298462e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 394840 + -1.3179937781298462e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.2406194387703730e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 5.1589226722717285e-01 + 1.2832164764404297e+01 + <_> + 1 + 384316 + -1.2406194387703730e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 2.3038609325885773e-01 + 1.2211467742919922e+01 + <_> + 2 + 238773 + -1.2406194387703730e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 145543 + 1.2406194387703730e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 555633 + 1.2406194387703730e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 3.0058968067169189e-01 + 9.6565132141113281e+00 + <_> + 2 + 99938 + -1.2406194387703730e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 455695 + 1.2406194387703730e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.4576661867569382e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 5.1913428306579590e-01 + 1.1477750778198242e+01 + <_> + 1 + 453559 + 1.4576661867569382e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 3.0843305587768555e-01 + 3.9746753871440887e-02 + <_> + 2 + 190227 + 1.4576661867569382e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 263332 + -1.4576661867569382e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 486390 + -1.4576661867569382e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 2.2794422507286072e-01 + 6.6322349548339844e+01 + <_> + 2 + 321520 + -1.4576661867569382e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 164870 + 1.4576661867569382e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.0473212032211107e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 5.1375508308410645e-01 + 2.1069567871093750e+02 + <_> + 1 + 577809 + 1.0473212032211107e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 2.9258355498313904e-01 + 2.0474296063184738e-02 + <_> + 2 + 166361 + -1.0473212032211107e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 411448 + 1.0473212032211107e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 362140 + -1.0473212032211107e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 2.3357556760311127e-01 + 2.4204508960247040e-01 + <_> + 2 + 318183 + -1.0473212032211107e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 43957 + 1.0473212032211107e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -8.8484693549611887e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 5.1163947582244873e-01 + 9.4814705848693848e-01 + <_> + 1 + 182656 + 8.8484693549611887e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.4984942078590393e-02 + 7.1276817321777344e+01 + <_> + 2 + 161799 + 8.8484693549611887e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 20857 + -8.8484693549611887e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 757293 + -8.8484693549611887e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 4.6712180972099304e-01 + 8.9122436523437500e+01 + <_> + 2 + 59810 + 8.8484693549611887e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 697483 + -8.8484693549611887e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 9.2694329415126342e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.1684498786926270e-01 + 3.6473352050781250e+02 + <_> + 1 + 16709 + -9.2694329415126342e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>13 + 3.5039000213146210e-02 + 1.1056249618530273e+01 + <_> + 2 + 13892 + 9.2694329415126342e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 2817 + -9.2694329415126342e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 923240 + 9.2694329415126342e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 4.8811799287796021e-01 + 3.8020561218261719e+01 + <_> + 2 + 813077 + 9.2694329415126342e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 110163 + -9.2694329415126342e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.0288841677036091e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 5.1483708620071411e-01 + 3.7499490356445312e+02 + <_> + 1 + 859328 + -1.0288841677036091e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 4.6046206355094910e-01 + 3.7403319031000137e-02 + <_> + 2 + 428756 + -1.0288841677036091e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 430572 + 1.0288841677036091e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 80621 + 1.0288841677036091e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 6.5237380564212799e-02 + 3.9438748359680176e+00 + <_> + 2 + 28400 + 1.0288841677036091e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 52221 + -1.0288841677036091e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.1941188064491802e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 5.1235097646713257e-01 + 2.3439809570312500e+03 + <_> + 1 + 542820 + -1.1941188064491802e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>8 + 2.4645562469959259e-01 + 5.0000000000000000e-01 + <_> + 2 + 83385 + 1.1941188064491802e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 459435 + -1.1941188064491802e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 397129 + 1.1941188064491802e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 2.8336191177368164e-01 + 2.0504441261291504e+00 + <_> + 2 + 97962 + -1.1941188064491802e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 299167 + 1.1941188064491802e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 9.4549468049911295e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 5.1340091228485107e-01 + 2.2395410537719727e+00 + <_> + 1 + 513799 + 9.4549468049911295e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 1.8521597981452942e-01 + 2.0108352050781250e+03 + <_> + 2 + 352169 + 9.4549468049911295e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 161630 + -9.4549468049911295e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 426150 + -9.4549468049911295e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 3.3840379118919373e-01 + 3.7188237905502319e-01 + <_> + 2 + 408593 + -9.4549468049911295e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 17557 + 9.4549468049911295e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.1020093476539640e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 5.1864713430404663e-01 + 5.4867222905158997e-02 + <_> + 1 + 711999 + 1.1020093476539640e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 4.7321084141731262e-01 + 3.2862496948242188e+02 + <_> + 2 + 6770 + -1.1020093476539640e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 705229 + 1.1020093476539640e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 227950 + -1.1020093476539640e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 5.4311551153659821e-02 + 1.2693084716796875e+01 + <_> + 2 + 168048 + 1.1020093476539640e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 59902 + -1.1020093476539640e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -9.2193133949371384e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.1473861932754517e-01 + 2.0698678970336914e+01 + <_> + 1 + 6985 + 9.2193133949371384e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 2.6909025385975838e-02 + 2.0887202117592096e-03 + <_> + 2 + 843 + -9.2193133949371384e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 6142 + 9.2193133949371384e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 932964 + -9.2193133949371384e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>10 + 4.9612295627593994e-01 + 9.6125583648681641e+00 + <_> + 2 + 814279 + -9.2193133949371384e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 118685 + 9.2193133949371384e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 9.7866499745687521e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 5.1452964544296265e-01 + 1.1132866144180298e-01 + <_> + 1 + 34942 + -9.7866499745687521e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 4.7213044017553329e-02 + 1.1150000000000000e+02 + <_> + 2 + 7882 + 9.7866499745687521e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 27060 + -9.7866499745687521e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 905007 + 9.7866499745687521e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 4.7723406553268433e-01 + 7.1007156372070312e+01 + <_> + 2 + 830259 + 9.7866499745687521e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 74748 + -9.7866499745687521e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.0249582909718888e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 5.1586729288101196e-01 + 9.6326553821563721e-01 + <_> + 1 + 295381 + 1.0249582909718888e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 1.1409680545330048e-01 + 1.0866064453125000e+03 + <_> + 2 + 29502 + -1.0249582909718888e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 265879 + 1.0249582909718888e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 644568 + -1.0249582909718888e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 4.1150474548339844e-01 + 2.2446777343750000e+02 + <_> + 2 + 424692 + -1.0249582909718888e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 219876 + 1.0249582909718888e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.0652606811626956e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 5.1579689979553223e-01 + 1.5524687194824219e+02 + <_> + 1 + 408027 + 1.0652606811626956e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 1.7519327998161316e-01 + 3.6744121551513672e+01 + <_> + 2 + 377784 + 1.0652606811626956e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 30243 + -1.0652606811626956e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 531922 + -1.0652606811626956e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 3.5141307115554810e-01 + 3.0161577463150024e-01 + <_> + 2 + 522876 + -1.0652606811626956e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 9046 + 1.0652606811626956e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.1185457094423212e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.1340693235397339e-01 + 3.5810890197753906e+01 + <_> + 1 + 436022 + -1.1185457094423212e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 1.6652441024780273e-01 + 3.4024901390075684e+00 + <_> + 2 + 397573 + 1.1185457094423212e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 38449 + -1.1185457094423212e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 503927 + 1.1185457094423212e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 3.6141011118888855e-01 + 1.2651381835937500e+03 + <_> + 2 + 110502 + -1.1185457094423212e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 393425 + 1.1185457094423212e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -9.7932827215946913e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 5.1449972391128540e-01 + 9.4600832462310791e-01 + <_> + 1 + 171520 + 9.7932827215946913e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 5.0194162875413895e-02 + 2.1649919509887695e+01 + <_> + 2 + 136726 + 9.7932827215946913e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 34794 + -9.7932827215946913e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 768429 + -9.7932827215946913e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>10 + 4.7426947951316833e-01 + 9.7234477996826172e+00 + <_> + 2 + 672701 + -9.7932827215946913e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 95728 + 9.7932827215946913e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 8.9959268783834520e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>3 + 5.1604413986206055e-01 + 2.2577753067016602e+00 + <_> + 1 + 6692 + -8.9959268783834520e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 2.5705030187964439e-02 + 1.8914577484130859e+01 + <_> + 2 + 3700 + 8.9959268783834520e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 2992 + -8.9959268783834520e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 933257 + 8.9959268783834520e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 4.9676963686943054e-01 + 1.4926003456115723e+01 + <_> + 2 + 914269 + 8.9959268783834520e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 18988 + -8.9959268783834520e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -7.3220287722891714e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 5.1328206062316895e-01 + 9.0213537216186523e-01 + <_> + 1 + 45679 + 7.3220287722891714e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.8892276138067245e-03 + 1.1682679748535156e+02 + <_> + 2 + 45619 + 7.3220287722891714e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 60 + -7.3220287722891714e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 894270 + -7.3220287722891714e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>10 + 5.1240766048431396e-01 + 1.0657048225402832e+01 + <_> + 2 + 829120 + -7.3220287722891714e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 65150 + 7.3220287722891714e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 9.9750461990075853e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.1521730422973633e-01 + 5.5185436248779297e+01 + <_> + 1 + 700867 + 9.9750461990075853e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 3.7367221713066101e-01 + 9.0728530883789062e+01 + <_> + 2 + 645328 + 9.9750461990075853e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 55539 + -9.9750461990075853e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 239082 + -9.9750461990075853e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 1.5124475955963135e-01 + 2.7916667461395264e+00 + <_> + 2 + 7855 + 9.9750461990075853e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 231227 + -9.9750461990075853e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.1134577094666968e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 5.1574254035949707e-01 + 1.8550380706787109e+01 + <_> + 1 + 561962 + -1.1134577094666968e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 3.5081800818443298e-01 + 8.2423019409179688e+01 + <_> + 2 + 478035 + -1.1134577094666968e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 83927 + 1.1134577094666968e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 377987 + 1.1134577094666968e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>8 + 1.7698970437049866e-01 + 5.0000000000000000e-01 + <_> + 2 + 46159 + -1.1134577094666968e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 331828 + 1.1134577094666968e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 9.9965563503050386e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 5.1274710893630981e-01 + 7.9649353027343750e+01 + <_> + 1 + 558281 + 9.9965563503050386e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 3.1131926178932190e-01 + 2.3037192821502686e+00 + <_> + 2 + 269975 + -9.9965563503050386e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 288306 + 9.9965563503050386e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 381668 + -9.9965563503050386e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 2.1365134418010712e-01 + 1.2518388032913208e+00 + <_> + 2 + 129390 + 9.9965563503050386e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 252278 + -9.9965563503050386e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -8.7027974257729637e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.1138991117477417e-01 + 4.2606941223144531e+01 + <_> + 1 + 552189 + 8.7027974257729637e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>8 + 2.5669866800308228e-01 + 1.0000002384185791e+00 + <_> + 2 + 404291 + 8.7027974257729637e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 147898 + -8.7027974257729637e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 387760 + -8.7027974257729637e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 2.6504459977149963e-01 + 8.2769302368164062e+01 + <_> + 2 + 223962 + -8.7027974257729637e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 163798 + 8.7027974257729637e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -8.3982844180968783e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 5.0998622179031372e-01 + 6.5771188735961914e+00 + <_> + 1 + 694832 + -8.3982844180968783e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 4.1123777627944946e-01 + 1.5279832482337952e-01 + <_> + 2 + 88851 + 8.3982844180968783e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 605981 + -8.3982844180968783e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 245117 + 8.3982844180968783e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 1.0974559932947159e-01 + 4.5084171295166016e+01 + <_> + 2 + 163864 + 8.3982844180968783e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 81253 + -8.3982844180968783e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.1916632279413154e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 5.1257252693176270e-01 + 1.7084568738937378e-01 + <_> + 1 + 344073 + -1.1916632279413154e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 1.9984720647335052e-01 + 6.1914779663085938e+01 + <_> + 2 + 194370 + -1.1916632279413154e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 149703 + 1.1916632279413154e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 595876 + 1.1916632279413154e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 3.2990917563438416e-01 + 9.7737467288970947e-01 + <_> + 2 + 387985 + 1.1916632279413154e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 207891 + -1.1916632279413154e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.1287854870653116e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>4 + 5.1627111434936523e-01 + 5.0960041046142578e+01 + <_> + 1 + 352212 + 1.1287854870653116e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 2.8430205583572388e-01 + 1.9669380187988281e+01 + <_> + 2 + 344496 + 1.1287854870653116e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 7716 + -1.1287854870653116e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 587737 + -1.1287854870653116e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 2.4388766288757324e-01 + 6.5050810546875000e+03 + <_> + 2 + 545360 + -1.1287854870653116e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 42377 + 1.1287854870653116e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -9.8053807332656423e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.1429033279418945e-01 + 5.7745811462402344e+01 + <_> + 1 + 428726 + -9.8053807332656423e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 3.1620678305625916e-01 + 2.7441722154617310e-01 + <_> + 2 + 384916 + -9.8053807332656423e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 43810 + 9.8053807332656423e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 511223 + 9.8053807332656423e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 2.0828703045845032e-01 + 2.5942230224609375e-01 + <_> + 2 + 455485 + 9.8053807332656423e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 55738 + -9.8053807332656423e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 9.1795950625598699e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.1402187347412109e-01 + 1.7400307617187500e+03 + <_> + 1 + 608464 + 9.1795950625598699e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 3.9332646131515503e-01 + 1.8950000000000000e+02 + <_> + 2 + 453872 + 9.1795950625598699e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 154592 + -9.1795950625598699e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 331485 + -9.1795950625598699e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 1.2960642576217651e-01 + 7.0761157226562500e+03 + <_> + 2 + 298751 + -9.1795950625598699e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 32734 + 9.1795950625598699e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.0262002980224427e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.1447802782058716e-01 + 7.0742095947265625e+01 + <_> + 1 + 606257 + -1.0262002980224427e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 4.1028589010238647e-01 + 9.5564103126525879e-01 + <_> + 2 + 156064 + 1.0262002980224427e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 450193 + -1.0262002980224427e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 333692 + 1.0262002980224427e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 1.1534661799669266e-01 + 6.3913696289062500e+01 + <_> + 2 + 227185 + 1.0262002980224427e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 106507 + -1.0262002980224427e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 9.5269309255806275e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 5.1336473226547241e-01 + 1.6194852828979492e+01 + <_> + 1 + 733652 + 9.5269309255806275e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 4.6268984675407410e-01 + 2.0750000000000000e+02 + <_> + 2 + 563780 + 9.5269309255806275e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 169872 + -9.5269309255806275e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 206297 + -9.5269309255806275e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 6.1109475791454315e-02 + 2.0961490631103516e+01 + <_> + 2 + 166793 + 9.5269309255806275e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 39504 + -9.5269309255806275e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.0221759831464151e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.1706039905548096e-01 + 7.5304916381835938e+01 + <_> + 1 + 666173 + -1.0221759831464151e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 4.3650162220001221e-01 + 9.8162543773651123e-01 + <_> + 2 + 565311 + -1.0221759831464151e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 100862 + 1.0221759831464151e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 273776 + 1.0221759831464151e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>8 + 8.9030563831329346e-02 + 5.0000000000000000e-01 + <_> + 2 + 30593 + -1.0221759831464151e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 243183 + 1.0221759831464151e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.0871136824213226e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>13 + 5.1150315999984741e-01 + 2.5200626373291016e+01 + <_> + 1 + 189520 + -1.0871136824213226e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 1.4954216778278351e-01 + 3.6143554687500000e+01 + <_> + 2 + 178324 + -1.0871136824213226e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 11196 + 1.0871136824213226e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 750429 + 1.0871136824213226e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 3.7760895490646362e-01 + 7.8792163085937500e+02 + <_> + 2 + 69891 + -1.0871136824213226e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 680538 + 1.0871136824213226e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.1040360760653388e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 5.2000057697296143e-01 + 3.3417877197265625e+01 + <_> + 1 + 187205 + 1.1040360760653388e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 9.9817611277103424e-02 + 1.5752697753906250e+02 + <_> + 2 + 1279 + -1.1040360760653388e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 185926 + 1.1040360760653388e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 752744 + -1.1040360760653388e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 4.2775529623031616e-01 + 7.8034830093383789e+00 + <_> + 2 + 50950 + 1.1040360760653388e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 701794 + -1.1040360760653388e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.0652427639061973e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 5.1562213897705078e-01 + 8.1050701141357422e+00 + <_> + 1 + 114162 + -1.0652427639061973e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 9.8385035991668701e-02 + 3.3612042665481567e-02 + <_> + 2 + 47018 + -1.0652427639061973e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 67144 + 1.0652427639061973e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 825787 + 1.0652427639061973e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 4.2822086811065674e-01 + 1.2153674662113190e-01 + <_> + 2 + 44259 + -1.0652427639061973e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 781528 + 1.0652427639061973e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.0040612460695243e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.1709353923797607e-01 + 3.5551582336425781e+01 + <_> + 1 + 73798 + 1.0040612460695243e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 1.0942199081182480e-01 + 1.2250000000000000e+02 + <_> + 2 + 54191 + 1.0040612460695243e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 19607 + -1.0040612460695243e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 866151 + -1.0040612460695243e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 4.1565847396850586e-01 + 2.7271972656250000e+01 + <_> + 2 + 183377 + 1.0040612460695243e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 682774 + -1.0040612460695243e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 9.5484531187294427e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 5.1515024900436401e-01 + 1.0745413208007812e+02 + <_> + 1 + 169272 + -9.5484531187294427e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 8.3884537220001221e-02 + 1.6368326187133789e+01 + <_> + 2 + 23639 + 9.5484531187294427e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 145633 + -9.5484531187294427e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 770677 + 9.5484531187294427e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 4.3996846675872803e-01 + 5.9278814697265625e+02 + <_> + 2 + 9499 + -9.5484531187294427e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 761178 + 9.5484531187294427e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -9.3549929003984206e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 5.1499819755554199e-01 + 3.0203989148139954e-01 + <_> + 1 + 905771 + -9.3549929003984206e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>4 + 4.9240612983703613e-01 + 1.1240586853027344e+02 + <_> + 2 + 838378 + -9.3549929003984206e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 67393 + 9.3549929003984206e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 34178 + 9.3549929003984206e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 3.0964301899075508e-02 + 9.7940278053283691e-01 + <_> + 2 + 22324 + 9.3549929003984206e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 11854 + -9.3549929003984206e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 7.0700068797801635e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.1467943191528320e-01 + 8.9687347412109375e+01 + <_> + 1 + 936547 + 7.0700068797801635e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 5.1276344060897827e-01 + 1.0006089210510254e+00 + <_> + 2 + 19411 + -7.0700068797801635e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 917136 + 7.0700068797801635e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 3402 + -7.0700068797801635e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 4.9042003229260445e-03 + 1.2773427581787109e+02 + <_> + 2 + 3178 + -7.0700068797801635e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 224 + 7.0700068797801635e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -8.4540796843124849e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>13 + 5.1058536767959595e-01 + 4.5061874389648438e+01 + <_> + 1 + 447485 + 8.4540796843124849e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 2.8131595253944397e-01 + 2.2580709308385849e-02 + <_> + 2 + 180345 + -8.4540796843124849e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 267140 + 8.4540796843124849e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 492464 + -8.4540796843124849e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 2.3980668187141418e-01 + 9.0213525295257568e-01 + <_> + 2 + 20842 + 8.4540796843124849e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 471622 + -8.4540796843124849e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.0802084869110026e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 5.1117241382598877e-01 + 1.2718648910522461e+01 + <_> + 1 + 381466 + -1.0802084869110026e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 2.3209235072135925e-01 + 3.9108800888061523e+00 + <_> + 2 + 335896 + -1.0802084869110026e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 45570 + 1.0802084869110026e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 558483 + 1.0802084869110026e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 2.9488661885261536e-01 + 1.0866591930389404e+00 + <_> + 2 + 140551 + -1.0802084869110026e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 417932 + 1.0802084869110026e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.0409681855586407e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 5.1406484842300415e-01 + 4.3159341812133789e+00 + <_> + 1 + 602738 + 1.0409681855586407e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 3.5198828577995300e-01 + 2.0850000000000000e+02 + <_> + 2 + 440991 + 1.0409681855586407e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 161747 + -1.0409681855586407e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 337211 + -1.0409681855586407e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 1.7401243746280670e-01 + 2.7646877288818359e+01 + <_> + 2 + 285630 + -1.0409681855586407e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 51581 + 1.0409681855586407e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -9.2676985333180570e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 5.1281064748764038e-01 + 3.3262111663818359e+01 + <_> + 1 + 184280 + 9.2676985333180570e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 9.7014181315898895e-02 + 8.4402847290039062e+00 + <_> + 2 + 56836 + -9.2676985333180570e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 127444 + 9.2676985333180570e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 755669 + -9.2676985333180570e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 4.2613849043846130e-01 + 6.5771188735961914e+00 + <_> + 2 + 557083 + -9.2676985333180570e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 198586 + 9.2676985333180570e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 9.0680856772967028e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 5.1107567548751831e-01 + 4.3188662528991699e+00 + <_> + 1 + 815040 + 9.0680856772967028e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 4.1403180360794067e-01 + 1.9024143218994141e+01 + <_> + 2 + 784965 + 9.0680856772967028e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 30075 + -9.0680856772967028e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 124909 + -9.0680856772967028e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 1.0862288624048233e-01 + 3.5908348083496094e+01 + <_> + 2 + 18385 + -9.0680856772967028e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 106524 + 9.0680856772967028e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.1662341508940306e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>13 + 5.1799350976943970e-01 + 1.1056875228881836e+01 + <_> + 1 + 29964 + 1.1662341508940306e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 6.4243853092193604e-02 + 2.9545368194580078e+01 + <_> + 2 + 17222 + 1.1662341508940306e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 12742 + -1.1662341508940306e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 909985 + -1.1662341508940306e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 4.6487900614738464e-01 + 2.0119082182645798e-02 + <_> + 2 + 174194 + 1.1662341508940306e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 735791 + -1.1662341508940306e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 9.8432717541454995e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 5.1453846693038940e-01 + 1.6504567116498947e-02 + <_> + 1 + 152505 + -9.8432717541454995e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 4.9194905906915665e-02 + 1.9939944744110107e+00 + <_> + 2 + 82530 + 9.8432717541454995e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 69975 + -9.8432717541454995e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 787444 + 9.8432717541454995e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 4.7539341449737549e-01 + 1.6343698501586914e+01 + <_> + 2 + 7929 + -9.8432717541454995e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 779515 + 9.8432717541454995e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -9.1733427480770291e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.1604121923446655e-01 + 2.0691448211669922e+01 + <_> + 1 + 6982 + 9.1733427480770291e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 2.6812275871634483e-02 + 2.0887202117592096e-03 + <_> + 2 + 843 + -9.1733427480770291e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 6139 + 9.1733427480770291e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 932967 + -9.1733427480770291e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 4.9610501527786255e-01 + 3.3226409912109375e+02 + <_> + 2 + 7098 + 9.1733427480770291e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 925869 + -9.1733427480770291e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 7.3024640252932996e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.1365131139755249e-01 + 3.2883883666992188e+02 + <_> + 1 + 6786 + -7.3024640252932996e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 2.0481588318943977e-02 + 9.6784992218017578e+00 + <_> + 2 + 1953 + 7.3024640252932996e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 4833 + -7.3024640252932996e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 933163 + 7.3024640252932996e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 4.9776646494865417e-01 + 3.3934539556503296e-01 + <_> + 2 + 927688 + 7.3024640252932996e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 5475 + -7.3024640252932996e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -7.5774151188967645e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 5.1113587617874146e-01 + 2.0463119506835938e+01 + <_> + 1 + 910568 + -7.5774151188967645e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 4.9280345439910889e-01 + 7.0456588745117188e+01 + <_> + 2 + 735219 + -7.5774151188967645e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 175349 + 7.5774151188967645e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 29381 + 7.5774151188967645e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 2.6131037622690201e-02 + 1.9509376585483551e-01 + <_> + 2 + 8569 + -7.5774151188967645e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 20812 + 7.5774151188967645e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 9.8167903644580265e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.1437771320343018e-01 + 6.1977767944335938e+01 + <_> + 1 + 774008 + 9.8167903644580265e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 4.1632673144340515e-01 + 6.6416664123535156e+00 + <_> + 2 + 682302 + 9.8167903644580265e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 91706 + -9.8167903644580265e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 165941 + -9.8167903644580265e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 1.0819554328918457e-01 + 1.0740039062500000e+03 + <_> + 2 + 19226 + 9.8167903644580265e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 146715 + -9.8167903644580265e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -9.3797428572422945e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 5.1542824506759644e-01 + 9.4423305988311768e-01 + <_> + 1 + 162533 + 9.3797428572422945e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 4.7023918479681015e-02 + 1.9750000000000000e+02 + <_> + 2 + 100683 + -9.3797428572422945e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 61850 + 9.3797428572422945e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 777416 + -9.3797428572422945e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>4 + 4.7640827298164368e-01 + 1.1442896270751953e+02 + <_> + 2 + 712609 + -9.3797428572422945e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 64807 + 9.3797428572422945e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 6.9855733689422550e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.1463878154754639e-01 + 8.9687347412109375e+01 + <_> + 1 + 936547 + 6.9855733689422550e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 5.1279735565185547e-01 + 2.5450000000000000e+02 + <_> + 2 + 896690 + 6.9855733689422550e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 39857 + -6.9855733689422550e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 3402 + -6.9855733689422550e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 4.6594655141234398e-03 + 1.2773427581787109e+02 + <_> + 2 + 3178 + -6.9855733689422550e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 224 + 6.9855733689422550e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -9.8618008032255344e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 5.1052582263946533e-01 + 6.0961181640625000e+01 + <_> + 1 + 651488 + -9.8618008032255344e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 3.5672342777252197e-01 + 6.0163281250000000e+03 + <_> + 2 + 586036 + -9.8618008032255344e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 65452 + 9.8618008032255344e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 288461 + 9.8618008032255344e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 1.6791111230850220e-01 + 4.3159341812133789e+00 + <_> + 2 + 197944 + 9.8618008032255344e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 90517 + -9.8618008032255344e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.1300555227614685e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>13 + 5.1087939739227295e-01 + 7.7558120727539062e+01 + <_> + 1 + 712698 + 1.1300555227614685e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 4.3538451194763184e-01 + 4.6403505859375000e+03 + <_> + 2 + 568017 + 1.1300555227614685e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 144681 + -1.1300555227614685e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 227251 + -1.1300555227614685e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 9.2836841940879822e-02 + 5.5609873046875000e+03 + <_> + 2 + 212046 + -1.1300555227614685e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 15205 + 1.1300555227614685e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.2123096838445586e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 5.1544207334518433e-01 + 4.1111455078125000e+03 + <_> + 1 + 731820 + -1.2123096838445586e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 3.7174120545387268e-01 + 2.4556973266601562e+02 + <_> + 2 + 535134 + -1.2123096838445586e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 196686 + 1.2123096838445586e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 208129 + 1.2123096838445586e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 1.5852946043014526e-01 + 2.4582362365722656e+02 + <_> + 2 + 119603 + 1.2123096838445586e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 88526 + -1.2123096838445586e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 9.3351414050983267e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 5.0907236337661743e-01 + 2.1084733581542969e+02 + <_> + 1 + 578193 + 9.3351414050983267e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 2.9459452629089355e-01 + 1.5375535190105438e-01 + <_> + 2 + 53653 + -9.3351414050983267e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 524540 + 9.3351414050983267e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 361756 + -9.3351414050983267e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 2.2872638702392578e-01 + 1.5067446231842041e-01 + <_> + 2 + 82993 + 9.3351414050983267e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 278763 + -9.3351414050983267e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 8.8365365456057388e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 5.0892072916030884e-01 + 3.3963266601562500e+03 + <_> + 1 + 672392 + -8.8365365456057388e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 3.2809808850288391e-01 + 4.1605226695537567e-02 + <_> + 2 + 326731 + -8.8365365456057388e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 345661 + 8.8365365456057388e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 267557 + 8.8365365456057388e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 1.9397889077663422e-01 + 1.0013697147369385e+00 + <_> + 2 + 8759 + -8.8365365456057388e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 258798 + 8.8365365456057388e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 9.6320378514866870e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 5.0943094491958618e-01 + 3.9328619837760925e-02 + <_> + 1 + 495043 + 9.6320378514866870e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 3.6762815713882446e-01 + 1.4560797810554504e-01 + <_> + 2 + 115729 + -9.6320378514866870e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 379314 + 9.6320378514866870e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 444906 + -9.6320378514866870e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 1.5643332898616791e-01 + 3.9041641235351562e+01 + <_> + 2 + 210994 + 9.6320378514866870e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 233912 + -9.6320378514866870e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.0020149185666212e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 5.1178598403930664e-01 + 1.0164431762695312e+02 + <_> + 1 + 720173 + -1.0020149185666212e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 3.9980518817901611e-01 + 1.1795907974243164e+01 + <_> + 2 + 638834 + -1.0020149185666212e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 81339 + 1.0020149185666212e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 219776 + 1.0020149185666212e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 1.2522424757480621e-01 + 1.3854326171875000e+03 + <_> + 2 + 18266 + -1.0020149185666212e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 201510 + 1.0020149185666212e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 8.0142561699724210e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 5.1266598701477051e-01 + 7.4361114501953125e+00 + <_> + 1 + 833749 + 8.0142561699724210e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 4.8009333014488220e-01 + 8.3695732116699219e+01 + <_> + 2 + 828878 + 8.0142561699724210e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 4871 + -8.0142561699724210e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 106200 + -8.0142561699724210e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 3.9931599050760269e-02 + 9.0433025360107422e-01 + <_> + 2 + 47128 + 8.0142561699724210e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 59072 + -8.0142561699724210e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -8.8531581195675044e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.1393693685531616e-01 + 2.1005577087402344e+01 + <_> + 1 + 92870 + 8.8531581195675044e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 5.5871695280075073e-02 + 9.7751682996749878e-01 + <_> + 2 + 31360 + -8.8531581195675044e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 61510 + 8.8531581195675044e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 847079 + -8.8531581195675044e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 4.6624675393104553e-01 + 9.5650970935821533e-02 + <_> + 2 + 15205 + 8.8531581195675044e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 831874 + -8.8531581195675044e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 8.0643603190497029e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 5.1349449157714844e-01 + 1.0527777671813965e+00 + <_> + 1 + 67213 + -8.0643603190497029e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 5.6659560650587082e-02 + 1.5906517207622528e-01 + <_> + 2 + 8074 + 8.0643603190497029e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 59139 + -8.0643603190497029e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 872736 + 8.0643603190497029e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 4.6349042654037476e-01 + 3.1296426773071289e+01 + <_> + 2 + 32152 + -8.0643603190497029e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 840584 + 8.0643603190497029e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -8.3408887312940355e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.1333922147750854e-01 + 3.5551582336425781e+01 + <_> + 1 + 73798 + 8.3408887312940355e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 1.0716832429170609e-01 + 9.7878766059875488e-01 + <_> + 2 + 50078 + 8.3408887312940355e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 23720 + -8.3408887312940355e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 866151 + -8.3408887312940355e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 4.1367182135581970e-01 + 2.4118915557861328e+01 + <_> + 2 + 113583 + 8.3408887312940355e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 752568 + -8.3408887312940355e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.0728917495552040e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 5.1237511634826660e-01 + 2.1249774932861328e+01 + <_> + 1 + 28915 + -1.0728917495552040e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 3.9859049022197723e-02 + 9.7912311553955078e-01 + <_> + 2 + 10718 + -1.0728917495552040e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 18197 + 1.0728917495552040e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 911034 + 1.0728917495552040e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 4.8693755269050598e-01 + 9.7940850257873535e-01 + <_> + 2 + 690465 + 1.0728917495552040e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 220569 + -1.0728917495552040e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.1701912983474262e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 5.1750779151916504e-01 + 9.7890961170196533e-01 + <_> + 1 + 682198 + -1.1701912983474262e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 4.5462617278099060e-01 + 3.0072689056396484e-01 + <_> + 2 + 659971 + -1.1701912983474262e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 22227 + 1.1701912983474262e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 257751 + 1.1701912983474262e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 7.4595265090465546e-02 + 1.3050000000000000e+02 + <_> + 2 + 40134 + -1.1701912983474262e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 217617 + 1.1701912983474262e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 8.5613730797816381e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 5.1744431257247925e-01 + 1.2247219085693359e+02 + <_> + 1 + 936426 + 8.5613730797816381e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 5.1352334022521973e-01 + 2.5650000000000000e+02 + <_> + 2 + 900441 + 8.5613730797816381e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 35985 + -8.5613730797816381e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 3523 + -8.5613730797816381e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 7.8670568764209747e-03 + 2.6592832803726196e-01 + <_> + 2 + 2054 + 8.5613730797816381e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 1469 + -8.5613730797816381e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -8.6735312322957378e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 5.1344555616378784e-01 + 1.4027777910232544e+00 + <_> + 1 + 130320 + 8.6735312322957378e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 9.0417973697185516e-02 + 2.4074706435203552e-01 + <_> + 2 + 25447 + 8.6735312322957378e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 104873 + -8.6735312322957378e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 809629 + -8.6735312322957378e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 4.3125227093696594e-01 + 9.3393045663833618e-01 + <_> + 2 + 117895 + 8.6735312322957378e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 691734 + -8.6735312322957378e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 8.0242236444842213e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.1339691877365112e-01 + 3.0695959472656250e+03 + <_> + 1 + 851129 + 8.0242236444842213e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 4.8779577016830444e-01 + 2.1493236541748047e+01 + <_> + 2 + 786756 + 8.0242236444842213e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 64373 + -8.0242236444842213e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 88820 + -8.0242236444842213e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 3.2254021614789963e-02 + 9.0213543176651001e-01 + <_> + 2 + 40813 + 8.0242236444842213e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 48007 + -8.0242236444842213e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -8.1468966820003819e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 5.1222765445709229e-01 + 9.4596225023269653e-01 + <_> + 1 + 171277 + 8.1468966820003819e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>8 + 4.7770999372005463e-02 + 8.9500000000000000e+01 + <_> + 2 + 154357 + 8.1468966820003819e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 16920 + -8.1468966820003819e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 768672 + -8.1468966820003819e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>4 + 4.7258499264717102e-01 + 9.2105995178222656e+01 + <_> + 2 + 661302 + -8.1468966820003819e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 107370 + 8.1468966820003819e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 9.1451705022931221e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.1278561353683472e-01 + 1.8597670898437500e+03 + <_> + 1 + 645524 + 9.1451705022931221e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 4.1259974241256714e-01 + 8.0559066772460938e+01 + <_> + 2 + 643966 + 9.1451705022931221e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 1558 + -9.1451705022931221e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 294425 + -9.1451705022931221e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 1.1024726927280426e-01 + 7.1166640625000000e+03 + <_> + 2 + 264982 + -9.1451705022931221e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 29443 + 9.1451705022931221e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -9.9869671996668505e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 5.1335608959197998e-01 + 9.6258354187011719e-01 + <_> + 1 + 285758 + 9.9869671996668505e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 1.0445322841405869e-01 + 8.0962127685546875e+01 + <_> + 2 + 159601 + 9.9869671996668505e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 126157 + -9.9869671996668505e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 654191 + -9.9869671996668505e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 4.2049345374107361e-01 + 8.2862075805664062e+01 + <_> + 2 + 501793 + -9.9869671996668505e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 152398 + 9.9869671996668505e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 7.8995393404377695e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>3 + 5.1490581035614014e-01 + 2.1488533020019531e+00 + <_> + 1 + 6188 + -7.8995393404377695e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 2.3196497932076454e-02 + 1.8914577484130859e+01 + <_> + 2 + 3651 + 7.8995393404377695e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 2537 + -7.8995393404377695e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 933761 + 7.8995393404377695e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 4.9654209613800049e-01 + 5.9278698730468750e+02 + <_> + 2 + 18534 + -7.8995393404377695e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 915227 + 7.8995393404377695e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -7.3069802982608956e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.1076966524124146e-01 + 2.1129188537597656e+01 + <_> + 1 + 95313 + 7.3069802982608956e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.6325383484363556e-02 + 2.7788288116455078e+01 + <_> + 2 + 14399 + -7.3069802982608956e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 80914 + 7.3069802982608956e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 844636 + -7.3069802982608956e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 4.6193394064903259e-01 + 9.4224190711975098e-01 + <_> + 2 + 151943 + 7.3069802982608956e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 692693 + -7.3069802982608956e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 7.5284014119540471e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 5.1227766275405884e-01 + 7.2583332061767578e+00 + <_> + 1 + 827599 + 7.5284014119540471e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 4.7852370142936707e-01 + 1.1127196252346039e-01 + <_> + 2 + 26527 + -7.5284014119540471e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 801072 + 7.5284014119540471e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 112350 + -7.5284014119540471e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 4.0288418531417847e-02 + 6.5754504203796387e+00 + <_> + 2 + 85827 + -7.5284014119540471e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 26523 + 7.5284014119540471e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -6.7239043695726808e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 5.1022368669509888e-01 + 8.2500000000000000e+01 + <_> + 1 + 10592 + 6.7239043695726808e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 3.5598151385784149e-02 + 2.9276496887207031e+01 + <_> + 2 + 1126 + -6.7239043695726808e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 9466 + 6.7239043695726808e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 929357 + -6.7239043695726808e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 4.8120528459548950e-01 + 1.2259896484375000e+04 + <_> + 2 + 914623 + -6.7239043695726808e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 14734 + 6.7239043695726808e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 7.8109874412406910e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 5.1145505905151367e-01 + 2.1249774932861328e+01 + <_> + 1 + 28915 + -7.8109874412406910e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 3.7226170301437378e-02 + 9.7948110103607178e-01 + <_> + 2 + 11023 + -7.8109874412406910e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 17892 + 7.8109874412406910e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 911034 + 7.8109874412406910e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 4.8229137063026428e-01 + 2.1740995407104492e+01 + <_> + 2 + 17562 + -7.8109874412406910e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 893472 + 7.8109874412406910e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -7.3889439851449801e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 5.1225376129150391e-01 + 3.9744672179222107e-01 + <_> + 1 + 841247 + -7.3889439851449801e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 5.0090205669403076e-01 + 7.5189315795898438e+01 + <_> + 2 + 825971 + -7.3889439851449801e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 15276 + 7.3889439851449801e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 98702 + 7.3889439851449801e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 1.7561897635459900e-02 + 9.1909484863281250e+01 + <_> + 2 + 67207 + 7.3889439851449801e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 31495 + -7.3889439851449801e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 7.2464617387381705e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>3 + 5.1211899518966675e-01 + 3.5976135253906250e+01 + <_> + 1 + 822391 + 7.2464617387381705e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 4.8770624399185181e-01 + 2.2657241821289062e+01 + <_> + 2 + 707093 + 7.2464617387381705e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 115298 + -7.2464617387381705e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 117558 + -7.2464617387381705e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>10 + 3.0401989817619324e-02 + 1.0657048225402832e+01 + <_> + 2 + 52342 + -7.2464617387381705e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 65216 + 7.2464617387381705e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -7.5468331774474012e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.1163858175277710e-01 + 2.3131701660156250e+03 + <_> + 1 + 759460 + -7.5468331774474012e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 4.5115607976913452e-01 + 3.8149261474609375e+00 + <_> + 2 + 30564 + 7.5468331774474012e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 728896 + -7.5468331774474012e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 180489 + 7.5468331774474012e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 6.7702054977416992e-02 + 2.2213289141654968e-01 + <_> + 2 + 105285 + -7.5468331774474012e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 75204 + 7.5468331774474012e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 7.5340115140718977e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>13 + 5.1288437843322754e-01 + 1.0873125076293945e+01 + <_> + 1 + 24839 + -7.5340115140718977e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 5.3778979927301407e-02 + 8.1874121093750000e+02 + <_> + 2 + 2897 + 7.5340115140718977e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 21942 + -7.5340115140718977e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 915110 + 7.5340115140718977e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 4.6504715085029602e-01 + 8.3843841552734375e+02 + <_> + 2 + 110256 + -7.5340115140718977e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 804854 + 7.5340115140718977e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -7.2961739232204420e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 5.1062929630279541e-01 + 7.7204910278320312e+01 + <_> + 1 + 829688 + -7.2961739232204420e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 4.6198007464408875e-01 + 1.4616707031250000e+04 + <_> + 2 + 820148 + -7.2961739232204420e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 9540 + 7.2961739232204420e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 110261 + 7.2961739232204420e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 5.6252285838127136e-02 + 1.6002917289733887e-01 + <_> + 2 + 11571 + 7.2961739232204420e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 98690 + -7.2961739232204420e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 6.9085031075190817e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.1334100961685181e-01 + 3.2862496948242188e+02 + <_> + 1 + 6770 + -6.9085031075190817e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 1.9802030175924301e-02 + 2.3638277053833008e+01 + <_> + 2 + 4586 + -6.9085031075190817e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 2184 + 6.9085031075190817e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 933179 + 6.9085031075190817e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 4.9746236205101013e-01 + 1.2247219085693359e+02 + <_> + 2 + 929656 + 6.9085031075190817e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 3523 + -6.9085031075190817e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -6.5135799717405254e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.0997900962829590e-01 + 1.6958400726318359e+01 + <_> + 1 + 30499 + 6.5135799717405254e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 3.2121390104293823e-02 + 1.5752697753906250e+02 + <_> + 2 + 1279 + -6.5135799717405254e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 29220 + 6.5135799717405254e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 909450 + -6.5135799717405254e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 4.8415681719779968e-01 + 9.2957198619842529e-01 + <_> + 2 + 102287 + 6.5135799717405254e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 807163 + -6.5135799717405254e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 6.7121523411906470e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 5.1070320606231689e-01 + 6.8305559158325195e+00 + <_> + 1 + 810376 + 6.7121523411906470e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 4.7155928611755371e-01 + 2.2669214755296707e-02 + <_> + 2 + 141986 + -6.7121523411906470e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 668390 + 6.7121523411906470e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 129573 + -6.7121523411906470e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 4.5214787125587463e-02 + 9.1136169433593750e-01 + <_> + 2 + 59038 + 6.7121523411906470e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 70535 + -6.7121523411906470e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 9.2258588204175915e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 5.0979942083358765e-01 + 2.3646846413612366e-02 + <_> + 1 + 242637 + 9.2258588204175915e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 1.2459499388933182e-01 + 4.8942436218261719e+01 + <_> + 2 + 216987 + 9.2258588204175915e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 25650 + -9.2258588204175915e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 697312 + -9.2258588204175915e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 3.9845332503318787e-01 + 8.8724197387695312e+01 + <_> + 2 + 32833 + 9.2258588204175915e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 664479 + -9.2258588204175915e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 8.2414446790697690e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.1558572053909302e-01 + 3.6480801391601562e+02 + <_> + 1 + 16714 + -8.2414446790697690e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>3 + 3.3692657947540283e-02 + 3.7838444709777832e+00 + <_> + 2 + 13546 + 8.2414446790697690e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 3168 + -8.2414446790697690e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 923235 + 8.2414446790697690e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 4.8689928650856018e-01 + 2.0956485748291016e+01 + <_> + 2 + 18896 + -8.2414446790697690e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 904339 + 8.2414446790697690e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -8.0159142314968707e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 5.1253300905227661e-01 + 7.7489433288574219e+01 + <_> + 1 + 831903 + -8.0159142314968707e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 4.6546125411987305e-01 + 6.1990652233362198e-02 + <_> + 2 + 714401 + -8.0159142314968707e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 117502 + 8.0159142314968707e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 108046 + 8.0159142314968707e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 5.4567810148000717e-02 + 2.9249999523162842e+00 + <_> + 2 + 5140 + -8.0159142314968707e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 102906 + 8.0159142314968707e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 8.6435336318017544e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>13 + 5.1143181324005127e-01 + 7.6274375915527344e+01 + <_> + 1 + 704230 + 8.6435336318017544e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 4.2848521471023560e-01 + 6.0689468383789062e+00 + <_> + 2 + 506340 + 8.6435336318017544e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 197890 + -8.6435336318017544e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 235719 + -8.6435336318017544e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 9.3110173940658569e-02 + 5.2815126953125000e+03 + <_> + 2 + 216187 + -8.6435336318017544e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 19532 + 8.6435336318017544e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 9.6299361674348424e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 5.1346254348754883e-01 + 4.3611497879028320e+00 + <_> + 1 + 617236 + -9.6299361674348424e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 3.6838540434837341e-01 + 1.0912328720092773e+01 + <_> + 2 + 263504 + -9.6299361674348424e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 353732 + 9.6299361674348424e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 322713 + 9.6299361674348424e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 1.5567083656787872e-01 + 2.2957393646240234e+01 + <_> + 2 + 273893 + 9.6299361674348424e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 48820 + -9.6299361674348424e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.2800181074087835e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 5.1375198364257812e-01 + 1.0303613662719727e+01 + <_> + 1 + 347177 + 1.2800181074087835e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 2.2165440022945404e-01 + 1.9226037597656250e+02 + <_> + 2 + 272232 + 1.2800181074087835e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 74945 + -1.2800181074087835e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 592772 + -1.2800181074087835e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 3.1030243635177612e-01 + 3.0082355499267578e+01 + <_> + 2 + 114246 + 1.2800181074087835e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 478526 + -1.2800181074087835e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.2758327088320612e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 5.2071052789688110e-01 + 1.8625845336914062e+02 + <_> + 1 + 519844 + -1.2758327088320612e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 2.4517355859279633e-01 + 5.2453964233398438e+01 + <_> + 2 + 449058 + -1.2758327088320612e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 70786 + 1.2758327088320612e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 420105 + 1.2758327088320612e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 2.8667905926704407e-01 + 8.0124511718750000e+00 + <_> + 2 + 15931 + -1.2758327088320612e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 404174 + 1.2758327088320612e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.1556975128644291e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 5.1384389400482178e-01 + 3.0361111164093018e+00 + <_> + 1 + 456431 + 1.1556975128644291e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 2.7896773815155029e-01 + 2.7141433209180832e-02 + <_> + 2 + 82542 + -1.1556975128644291e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 373889 + 1.1556975128644291e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 483518 + -1.1556975128644291e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 2.4989259243011475e-01 + 1.4688331604003906e+02 + <_> + 2 + 90994 + 1.1556975128644291e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 392524 + -1.1556975128644291e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.1387202331095281e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 5.1328349113464355e-01 + 3.0473126098513603e-02 + <_> + 1 + 355953 + 1.1387202331095281e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 2.5208243727684021e-01 + 2.0887202117592096e-03 + <_> + 2 + 843 + -1.1387202331095281e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 355110 + 1.1387202331095281e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 583996 + -1.1387202331095281e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 2.7635484933853149e-01 + 5.2909439086914062e+01 + <_> + 2 + 109291 + 1.1387202331095281e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 474705 + -1.1387202331095281e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.0990809231734344e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.1957875490188599e-01 + 5.7762901306152344e+01 + <_> + 1 + 429016 + -1.0990809231734344e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 3.1701892614364624e-01 + 2.1533111572265625e+01 + <_> + 2 + 7767 + 1.0990809231734344e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 421249 + -1.0990809231734344e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 510933 + 1.0990809231734344e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 2.1043047308921814e-01 + 1.8651330471038818e+00 + <_> + 2 + 185707 + -1.0990809231734344e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 325226 + 1.0990809231734344e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 7.1302797002674709e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.1348185539245605e-01 + 3.6480801391601562e+02 + <_> + 1 + 16714 + -7.1302797002674709e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 3.3132173120975494e-02 + 9.6784992218017578e+00 + <_> + 2 + 1966 + 7.1302797002674709e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 14748 + -7.1302797002674709e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 923235 + 7.1302797002674709e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 4.8468598723411560e-01 + 5.2694444656372070e+00 + <_> + 2 + 701817 + 7.1302797002674709e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 221418 + -7.1302797002674709e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -8.1355971249010228e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.1226305961608887e-01 + 7.4459777832031250e+01 + <_> + 1 + 655746 + -8.1355971249010228e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 4.2784062027931213e-01 + 1.4689365386962891e+01 + <_> + 2 + 474179 + -8.1355971249010228e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 181567 + 8.1355971249010228e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 284203 + 8.1355971249010228e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 9.2487148940563202e-02 + 2.6068437099456787e-01 + <_> + 2 + 257260 + 8.1355971249010228e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 26943 + -8.1355971249010228e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 9.1873296930504139e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 5.1383638381958008e-01 + 1.3917081832885742e+01 + <_> + 1 + 666300 + 9.1873296930504139e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 4.1398775577545166e-01 + 3.3934539556503296e-01 + <_> + 2 + 661024 + 9.1873296930504139e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 5276 + -9.1873296930504139e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 273649 + -9.1873296930504139e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 1.0896442085504532e-01 + 1.7041602730751038e-01 + <_> + 2 + 73384 + 9.1873296930504139e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 200265 + -9.1873296930504139e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -8.2419900827061882e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.1255029439926147e-01 + 7.6530525207519531e+01 + <_> + 1 + 680531 + -8.2419900827061882e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 4.3775960803031921e-01 + 3.6212229728698730e-01 + <_> + 2 + 595452 + -8.2419900827061882e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 85079 + 8.2419900827061882e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 259418 + 8.2419900827061882e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 8.2833699882030487e-02 + 1.4494836330413818e-01 + <_> + 2 + 32312 + -8.2419900827061882e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 227106 + 8.2419900827061882e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 5.8195910720962994e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.1076304912567139e-01 + 3.2862496948242188e+02 + <_> + 1 + 6770 + -5.8195910720962994e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 1.9483391195535660e-02 + 2.3545037955045700e-02 + <_> + 2 + 4271 + -5.8195910720962994e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 2499 + 5.8195910720962994e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 933179 + 5.8195910720962994e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 4.9506148695945740e-01 + 5.7734259033203125e+02 + <_> + 2 + 14723 + -5.8195910720962994e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 918456 + 5.8195910720962994e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -9.0868419482808410e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 5.1056963205337524e-01 + 2.4845570325851440e-02 + <_> + 1 + 262591 + 9.0868419482808410e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 1.4444598555564880e-01 + 4.8942436218261719e+01 + <_> + 2 + 232758 + 9.0868419482808410e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 29833 + -9.0868419482808410e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 677358 + -9.0868419482808410e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 3.7825548648834229e-01 + 3.3225021362304688e+02 + <_> + 2 + 6090 + 9.0868419482808410e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 671268 + -9.0868419482808410e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 9.3879513095381270e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>13 + 5.1341784000396729e-01 + 2.4794376373291016e+01 + <_> + 1 + 181554 + -9.3879513095381270e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 1.4344008266925812e-01 + 7.8456152343750000e+02 + <_> + 2 + 18636 + 9.3879513095381270e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 162918 + -9.3879513095381270e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 758395 + 9.3879513095381270e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 3.8001257181167603e-01 + 6.6320623779296875e+02 + <_> + 2 + 28007 + -9.3879513095381270e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 730388 + 9.3879513095381270e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -9.9845847805733259e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 5.1359522342681885e-01 + 2.0583333969116211e+00 + <_> + 1 + 273068 + 9.9845847805733259e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 1.5204519033432007e-01 + 5.3405029296875000e+01 + <_> + 2 + 148524 + 9.9845847805733259e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 124544 + -9.9845847805733259e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 666881 + -9.9845847805733259e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 3.7289556860923767e-01 + 3.1982746124267578e+01 + <_> + 2 + 128311 + 9.9845847805733259e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 538570 + -9.9845847805733259e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 9.3919512814304065e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.1630872488021851e-01 + 8.3809625244140625e+02 + <_> + 1 + 210529 + -9.3919512814304065e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 1.4106523990631104e-01 + 3.3075317382812500e+01 + <_> + 2 + 186666 + -9.3919512814304065e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 23863 + 9.3919512814304065e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 729420 + 9.3919512814304065e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 3.8239741325378418e-01 + 8.1059589385986328e+00 + <_> + 2 + 20642 + -9.3919512814304065e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 708778 + 9.3919512814304065e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -8.3286147255598963e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.1171511411666870e-01 + 3.5518180847167969e+01 + <_> + 1 + 73499 + 8.3286147255598963e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 1.0630647838115692e-01 + 9.6968495845794678e-01 + <_> + 2 + 30252 + -8.3286147255598963e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 43247 + 8.3286147255598963e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 866450 + -8.3286147255598963e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 4.1450303792953491e-01 + 2.9694503784179688e+01 + <_> + 2 + 232618 + 8.3286147255598963e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 633832 + -8.3286147255598963e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 9.9451920948802550e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>1 + 5.1334905624389648e-01 + 6.7580406188964844e+01 + <_> + 1 + 381093 + -9.9451920948802550e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 2.2956003248691559e-01 + 3.4562306404113770e+00 + <_> + 2 + 320644 + -9.9451920948802550e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 60449 + 9.9451920948802550e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 558856 + 9.9451920948802550e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 2.9528248310089111e-01 + 5.4131675720214844e+01 + <_> + 2 + 55603 + -9.9451920948802550e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 503253 + 9.9451920948802550e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -9.7484575846720475e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 5.0921845436096191e-01 + 1.1477750778198242e+01 + <_> + 1 + 453559 + 9.7484575846720475e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 2.9782015085220337e-01 + 4.6675849914550781e+01 + <_> + 2 + 124874 + -9.7484575846720475e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 328685 + 9.7484575846720475e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 486390 + -9.7484575846720475e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 2.2653171420097351e-01 + 2.5138888359069824e+00 + <_> + 2 + 40222 + 9.7484575846720475e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 446168 + -9.7484575846720475e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.0489068721059110e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 5.1210880279541016e-01 + 4.3557140976190567e-02 + <_> + 1 + 569543 + 1.0489068721059110e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 4.0671634674072266e-01 + 6.6202972412109375e+01 + <_> + 2 + 492654 + 1.0489068721059110e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 76889 + -1.0489068721059110e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 370406 + -1.0489068721059110e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 1.1948232352733612e-01 + 1.0475438117980957e+01 + <_> + 2 + 175368 + -1.0489068721059110e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 195038 + 1.0489068721059110e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -9.3640786361259234e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 5.1513731479644775e-01 + 7.1042518615722656e+01 + <_> + 1 + 766440 + -9.3640786361259234e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 4.3155348300933838e-01 + 2.0463119506835938e+01 + <_> + 2 + 742247 + -9.3640786361259234e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 24193 + 9.3640786361259234e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 173509 + 9.3640786361259234e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 9.1839611530303955e-02 + 1.1991396484375000e+03 + <_> + 2 + 6411 + -9.3640786361259234e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 167098 + 9.3640786361259234e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.2110063967193492e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.1329803466796875e-01 + 5.2015617370605469e+01 + <_> + 1 + 321342 + 1.2110063967193492e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 2.5662544369697571e-01 + 3.9916667938232422e+00 + <_> + 2 + 229848 + 1.2110063967193492e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 91494 + -1.2110063967193492e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 618607 + -1.2110063967193492e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 2.7361276745796204e-01 + 3.0552150681614876e-02 + <_> + 2 + 133924 + 1.2110063967193492e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 484683 + -1.2110063967193492e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.1883732064945969e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 5.1413631439208984e-01 + 3.5184942185878754e-02 + <_> + 1 + 434288 + -1.1883732064945969e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 3.3258283138275146e-01 + 1.8326249122619629e+00 + <_> + 2 + 120222 + 1.1883732064945969e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 314066 + -1.1883732064945969e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 505661 + 1.1883732064945969e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 1.9709157943725586e-01 + 1.2805453491210938e+02 + <_> + 2 + 103597 + -1.1883732064945969e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 402064 + 1.1883732064945969e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 9.5209721526612168e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>4 + 5.1127165555953979e-01 + 5.7878948211669922e+01 + <_> + 1 + 474844 + 9.5209721526612168e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 3.4760969877243042e-01 + 4.4236723333597183e-02 + <_> + 2 + 399453 + 9.5209721526612168e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 75391 + -9.5209721526612168e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 465105 + -9.5209721526612168e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 1.7617475986480713e-01 + 5.1848793945312500e+03 + <_> + 2 + 416818 + -9.5209721526612168e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 48287 + 9.5209721526612168e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.0623421539921347e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 5.1427578926086426e-01 + 2.1393588185310364e-01 + <_> + 1 + 357893 + -1.0623421539921347e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 3.0796292424201965e-01 + 9.5831578969955444e-01 + <_> + 2 + 128441 + 1.0623421539921347e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 229452 + -1.0623421539921347e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 582056 + 1.0623421539921347e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 2.1857069432735443e-01 + 9.1728906631469727e+00 + <_> + 2 + 154778 + -1.0623421539921347e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 427278 + 1.0623421539921347e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 9.8074675364156094e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 5.1148182153701782e-01 + 1.0465257644653320e+01 + <_> + 1 + 362757 + 9.8074675364156094e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 2.3092493414878845e-01 + 3.3313110470771790e-02 + <_> + 2 + 101489 + 9.8074675364156094e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 261268 + -9.8074675364156094e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 577192 + -9.8074675364156094e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 2.9357409477233887e-01 + 3.7736026763916016e+01 + <_> + 2 + 124775 + 9.8074675364156094e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 452417 + -9.8074675364156094e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.0359002693278439e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>1 + 5.1835864782333374e-01 + 4.8388854980468750e+01 + <_> + 1 + 107186 + -1.0359002693278439e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 1.0784414410591125e-01 + 5.5886039733886719e+01 + <_> + 2 + 83462 + -1.0359002693278439e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 23724 + 1.0359002693278439e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 832763 + 1.0359002693278439e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 4.1803023219108582e-01 + 2.5985252380371094e+01 + <_> + 2 + 17842 + -1.0359002693278439e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 814921 + 1.0359002693278439e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -8.8198803525203415e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 5.1493400335311890e-01 + 1.7083332538604736e+00 + <_> + 1 + 194453 + 8.8198803525203415e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 1.1511732637882233e-01 + 2.7744205668568611e-02 + <_> + 2 + 33358 + -8.8198803525203415e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 161095 + 8.8198803525203415e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 745496 + -8.8198803525203415e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 4.0691810846328735e-01 + 3.2193367004394531e+01 + <_> + 2 + 176096 + 8.8198803525203415e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 569400 + -8.8198803525203415e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 8.5319567469694435e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 5.1347285509109497e-01 + 1.7780647277832031e+00 + <_> + 1 + 262405 + -8.5319567469694435e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 7.9224467277526855e-02 + 2.3705536127090454e-01 + <_> + 2 + 173592 + 8.5319567469694435e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 88813 + -8.5319567469694435e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 677544 + 8.5319567469694435e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 4.4209250807762146e-01 + 2.1491771697998047e+01 + <_> + 2 + 584008 + 8.5319567469694435e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 93536 + -8.5319567469694435e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -7.5582321207290984e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.1142048835754395e-01 + 2.0529514312744141e+01 + <_> + 1 + 83892 + 7.5582321207290984e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 5.0806883722543716e-02 + 5.0637893676757812e+00 + <_> + 2 + 79822 + 7.5582321207290984e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 4070 + -7.5582321207290984e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 856057 + -7.5582321207290984e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 4.6807971596717834e-01 + 6.1299880981445312e+01 + <_> + 2 + 571635 + -7.5582321207290984e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 284422 + 7.5582321207290984e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -7.0557212230028296e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 5.0872755050659180e-01 + 1.7083332538604736e+00 + <_> + 1 + 194453 + 7.0557212230028296e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 1.1494127660989761e-01 + 2.5290709733963013e-01 + <_> + 2 + 53183 + 7.0557212230028296e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 141270 + -7.0557212230028296e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 745496 + -7.0557212230028296e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 4.0269070863723755e-01 + 2.6766842603683472e-01 + <_> + 2 + 485595 + -7.0557212230028296e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 259901 + 7.0557212230028296e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 8.5915839530178353e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>13 + 5.1210051774978638e-01 + 7.9001876831054688e+01 + <_> + 1 + 722107 + 8.5915839530178353e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 4.3606504797935486e-01 + 1.0130571289062500e+03 + <_> + 2 + 143780 + -8.5915839530178353e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 578327 + 8.5915839530178353e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 217842 + -8.5915839530178353e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 8.5400700569152832e-02 + 7.7776007652282715e+00 + <_> + 2 + 178634 + -8.5915839530178353e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 39208 + 8.5915839530178353e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.1871690515561377e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 5.1166379451751709e-01 + 1.7350000000000000e+02 + <_> + 1 + 472830 + -1.1871690515561377e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 3.6101397871971130e-01 + 4.1575707495212555e-02 + <_> + 2 + 337772 + -1.1871690515561377e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 135058 + 1.1871690515561377e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 467119 + 1.1871690515561377e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 1.6863043606281281e-01 + 6.2376068115234375e+01 + <_> + 2 + 336697 + 1.1871690515561377e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 130422 + -1.1871690515561377e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 7.9747021423423631e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>13 + 5.1039820909500122e-01 + 4.5376876831054688e+01 + <_> + 1 + 450760 + 7.9747021423423631e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 2.8535002470016479e-01 + 4.5416669845581055e+00 + <_> + 2 + 350887 + 7.9747021423423631e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 99873 + -7.9747021423423631e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 489189 + -7.9747021423423631e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 2.3457618057727814e-01 + 1.7607159423828125e+03 + <_> + 2 + 237195 + -7.9747021423423631e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 251994 + 7.9747021423423631e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -1.0458336468928661e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 5.1354891061782837e-01 + 2.3132567107677460e-01 + <_> + 1 + 714357 + -1.0458336468928661e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 3.8229107856750488e-01 + 1.1761998046875000e+04 + <_> + 2 + 696679 + -1.0458336468928661e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 17678 + 1.0458336468928661e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 225592 + 1.0458336468928661e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 1.4383094012737274e-01 + 3.7486991882324219e+01 + <_> + 2 + 140318 + -1.0458336468928661e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 85274 + 1.0458336468928661e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.0116176671351323e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 5.1337283849716187e-01 + 4.1362483054399490e-02 + <_> + 1 + 528160 + 1.0116176671351323e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 3.8520646095275879e-01 + 2.3988038301467896e-01 + <_> + 2 + 456793 + 1.0116176671351323e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 71367 + -1.0116176671351323e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 411789 + -1.0116176671351323e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 1.4006242156028748e-01 + 3.9607601165771484e+01 + <_> + 2 + 200827 + 1.0116176671351323e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 210962 + -1.0116176671351323e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -9.8587283797664668e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 5.1170921325683594e-01 + 2.2789442539215088e-01 + <_> + 1 + 698319 + -9.8587283797664668e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 3.7315154075622559e-01 + 7.7017005920410156e+01 + <_> + 2 + 501613 + -9.8587283797664668e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 196706 + 9.8587283797664668e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 241630 + 9.8587283797664668e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 1.5147534012794495e-01 + 7.9469665527343750e+01 + <_> + 2 + 155563 + 9.8587283797664668e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 86067 + -9.8587283797664668e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 7.2388925867946902e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 5.1272624731063843e-01 + 2.4836845397949219e+00 + <_> + 1 + 8336 + -7.2388925867946902e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 2.8353475034236908e-02 + 2.1864805221557617e+01 + <_> + 2 + 4716 + 7.2388925867946902e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 3620 + -7.2388925867946902e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 931613 + 7.2388925867946902e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 4.8973587155342102e-01 + 1.9532299041748047e+01 + <_> + 2 + 899770 + 7.2388925867946902e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 31843 + -7.2388925867946902e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -6.9927448485946411e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 5.1072293519973755e-01 + 8.3762603759765625e+01 + <_> + 1 + 870511 + -6.9927448485946411e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 4.8741468787193298e-01 + 2.1392341613769531e+01 + <_> + 2 + 100383 + 6.9927448485946411e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 770128 + -6.9927448485946411e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 69438 + 6.9927448485946411e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 3.0060056596994400e-02 + 2.4718548583984375e+02 + <_> + 2 + 5962 + -6.9927448485946411e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 63476 + 6.9927448485946411e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 7.2269446935734366e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>1 + 5.1198285818099976e-01 + 4.1848121643066406e+01 + <_> + 1 + 55631 + -7.2269446935734366e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 7.9731270670890808e-02 + 8.2703430175781250e+02 + <_> + 2 + 5002 + 7.2269446935734366e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 50629 + -7.2269446935734366e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 884318 + 7.2269446935734366e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 4.3832823634147644e-01 + 5.5659802246093750e+02 + <_> + 2 + 31695 + -7.2269446935734366e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 852623 + 7.2269446935734366e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -7.1999307597837292e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 5.1122826337814331e-01 + 1.4027777910232544e+00 + <_> + 1 + 130320 + 7.1999307597837292e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 8.8670313358306885e-02 + 2.0237146377563477e+01 + <_> + 2 + 19142 + -7.1999307597837292e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 111178 + 7.1999307597837292e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 809629 + -7.1999307597837292e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 4.2932173609733582e-01 + 7.3771652221679688e+01 + <_> + 2 + 780723 + -7.1999307597837292e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 28906 + 7.1999307597837292e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 6.6314611340506965e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.1069760322570801e-01 + 3.6350097656250000e+02 + <_> + 1 + 16619 + -6.6314611340506965e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>3 + 3.4099999815225601e-02 + 3.7838444709777832e+00 + <_> + 2 + 13487 + 6.6314611340506965e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 3132 + -6.6314611340506965e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 923330 + 6.6314611340506965e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 4.8247256875038147e-01 + 6.7861108779907227e+00 + <_> + 2 + 791820 + 6.6314611340506965e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 131510 + -6.6314611340506965e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -7.8905796369653489e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 5.1127386093139648e-01 + 9.3714785575866699e-01 + <_> + 1 + 130472 + 7.8905796369653489e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 3.6708597093820572e-02 + 1.4108461141586304e-01 + <_> + 2 + 31678 + -7.8905796369653489e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 98794 + 7.8905796369653489e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 809477 + -7.8905796369653489e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 4.8300760984420776e-01 + 4.2025224609375000e+03 + <_> + 2 + 646531 + -7.8905796369653489e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 162946 + 7.8905796369653489e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 7.8072262911372578e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 5.1048618555068970e-01 + 1.3338859379291534e-01 + <_> + 1 + 72512 + -7.8072262911372578e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 9.2181146144866943e-02 + 2.2968001365661621e+00 + <_> + 2 + 17394 + 7.8072262911372578e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 55118 + -7.8072262911372578e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 867437 + 7.8072262911372578e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 4.2732700705528259e-01 + 1.9246902465820312e+00 + <_> + 2 + 362527 + -7.8072262911372578e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 504910 + 7.8072262911372578e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -9.2238364194507275e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 5.1418399810791016e-01 + 2.2395410537719727e+00 + <_> + 1 + 513799 + 9.2238364194507275e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 1.8378044664859772e-01 + 1.2741674423217773e+01 + <_> + 2 + 303902 + -9.2238364194507275e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 209897 + 9.2238364194507275e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 426150 + -9.2238364194507275e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 3.3926281332969666e-01 + 3.8725262880325317e-01 + <_> + 2 + 412951 + -9.2238364194507275e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 13199 + 9.2238364194507275e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 6.8149442290896436e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.1459604501724243e-01 + 1.1534797668457031e+01 + <_> + 1 + 1694 + -6.8149442290896436e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 7.3195197619497776e-03 + 3.0557768554687500e+03 + <_> + 2 + 550 + 6.8149442290896436e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 1144 + -6.8149442290896436e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 938255 + 6.8149442290896436e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.0971126556396484e-01 + 9.0099792480468750e+01 + <_> + 2 + 935151 + 6.8149442290896436e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 3104 + -6.8149442290896436e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -9.3064027551599024e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 5.0871545076370239e-01 + 1.4050000000000000e+02 + <_> + 1 + 190472 + 9.3064027551599024e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 2.1024987101554871e-01 + 9.7942411899566650e-01 + <_> + 2 + 141929 + 9.3064027551599024e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 48543 + -9.3064027551599024e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 749477 + -9.3064027551599024e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 3.1299936771392822e-01 + 9.7865974903106689e-01 + <_> + 2 + 537369 + -9.3064027551599024e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 212108 + 9.3064027551599024e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 8.3429426841054810e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.1083725690841675e-01 + 9.8939685058593750e+02 + <_> + 1 + 293459 + -8.3429426841054810e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 1.8333186209201813e-01 + 9.8017108440399170e-01 + <_> + 2 + 148283 + -8.3429426841054810e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 145176 + 8.3429426841054810e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 646490 + 8.3429426841054810e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 3.3751341700553894e-01 + 4.4099445343017578e+01 + <_> + 2 + 49053 + -8.3429426841054810e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 597437 + 8.3429426841054810e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -9.1370924959347538e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 5.1122385263442993e-01 + 2.6027777194976807e+00 + <_> + 1 + 380777 + 9.1370924959347538e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 2.2420799732208252e-01 + 1.5382666885852814e-01 + <_> + 2 + 20466 + -9.1370924959347538e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 360311 + 9.1370924959347538e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 559172 + -9.1370924959347538e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 2.9861885309219360e-01 + 3.1233376264572144e-01 + <_> + 2 + 555373 + -9.1370924959347538e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 3799 + 9.1370924959347538e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 1.0525025290225019e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.1245540380477905e-01 + 1.3024299316406250e+03 + <_> + 1 + 444184 + -1.0525025290225019e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 2.8796845674514771e-01 + 4.8693733215332031e+01 + <_> + 2 + 414164 + -1.0525025290225019e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 30020 + 1.0525025290225019e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 495765 + 1.0525025290225019e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 2.3831984400749207e-01 + 3.5366456508636475e+00 + <_> + 2 + 395233 + 1.0525025290225019e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 100532 + -1.0525025290225019e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + 9.9200526586580143e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.1484316587448120e-01 + 4.9804302215576172e+01 + <_> + 1 + 642680 + 9.9200526586580143e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>10 + 3.4121102094650269e-01 + 9.1308612823486328e+00 + <_> + 2 + 596472 + 9.9200526586580143e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 46208 + -9.9200526586580143e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 297269 + -9.9200526586580143e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>13 + 1.8356879055500031e-01 + 4.7991874694824219e+01 + <_> + 2 + 28000 + 9.9200526586580143e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 269269 + -9.9200526586580143e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 939949 + -8.1278822395898273e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 5.1284188032150269e-01 + 1.8550380706787109e+01 + <_> + 1 + 561962 + -8.1278822395898273e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 3.4744775295257568e-01 + 1.3132913038134575e-02 + <_> + 2 + 111699 + 8.1278822395898273e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 450263 + -8.1278822395898273e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 377987 + 8.1278822395898273e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 1.7286077141761780e-01 + 1.2966735839843750e+03 + <_> + 2 + 88788 + -8.1278822395898273e-02 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 289199 + 8.1278822395898273e-02 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/classifier_models/vrf_conditional_field_weights.txt b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/classifier_models/vrf_conditional_field_weights.txt new file mode 100644 index 0000000..bf28f59 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/classifier_models/vrf_conditional_field_weights.txt @@ -0,0 +1,35 @@ + 0.0315632 + 0.130265 + 0.230182 + 0.120392 + 0.235773 + 0.111072 + 0.153368 + 0.0664334 + 0.254045 + 0.116998 + 0.1993 + 0.091067 + 0.0397418 + 0.414929 + 0.0781084 + 0.0429856 +-0.0506238 + 0.252552 + 0.249195 + 0.157152 + 0.409335 + 0.28834 + 0.284801 + 0.0958252 + 0.139567 + 0.305824 + 0.33268 + 0.148952 + 0.287684 + 0.378066 + 0.499984 + 0.39717 + 0.282108 + 0.548368 + 0.537415 diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/classifier_models/vrf_doorway_boost.xml b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/classifier_models/vrf_doorway_boost.xml new file mode 100644 index 0000000..bc1c3b7 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/classifier_models/vrf_doorway_boost.xml @@ -0,0 +1,3260 @@ + + + + DiscreteAdaboost + 3 + 35 + 1. + 1 + 28 + 28 + 28 + 0 + + 0 + 10 + 2 + 10 + 0 + + 1 + 2 +
d
+ + 1. 1. + + 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 + + 1 + 1 +
i
+ + 2 + + 1 + 2 +
i
+ + -1 1 + + <_> + -1 + + <_> + 0 + 9107 + -3.2714055917456113e+00 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>24 + 9.6310532093048096e-01 + 55. + <_> + 1 + 38 + 3.2714055917456113e+00 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>1 + 3.9530033245682716e-03 + 4.3359134674072266e+01 + <_> + 2 + 2 + -3.2714055917456113e+00 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 36 + 3.2714055917456113e+00 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 9069 + -3.2714055917456113e+00 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>1 + 9.5948171615600586e-01 + 2.4549925231933594e+02 + <_> + 2 + 9066 + -3.2714055917456113e+00 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 3 + 3.2714055917456113e+00 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + -2.3152649157339633e+00 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>24 + 8.7555474042892456e-01 + 170. + <_> + 1 + 1161 + 2.3152649157339633e+00 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>24 + 4.5387151837348938e-01 + 110. + <_> + 2 + 625 + -2.3152649157339633e+00 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 536 + 2.3152649157339633e+00 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 7946 + -2.3152649157339633e+00 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>27 + 4.5626187324523926e-01 + 5.0481063842773438e+01 + <_> + 2 + 7781 + -2.3152649157339633e+00 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 165 + 2.3152649157339633e+00 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + 1.6100350400745878e+00 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>3 + 7.2100114822387695e-01 + 7.9208981990814209e-01 + <_> + 1 + 4332 + -1.6100350400745878e+00 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>26 + 2.1469165384769440e-01 + 8.9369656250000000e+05 + <_> + 2 + 4281 + -1.6100350400745878e+00 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 51 + 1.6100350400745878e+00 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 4775 + 1.6100350400745878e+00 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 6.1872458457946777e-01 + 4.1944444179534912e-01 + <_> + 2 + 2452 + 1.6100350400745878e+00 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 2323 + -1.6100350400745878e+00 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + -9.2897773716780951e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 6.5099722146987915e-01 + 3.5741989135742188e+01 + <_> + 1 + 5324 + -9.2897773716780951e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 2.8559100627899170e-01 + 7.7840313720703125e+02 + <_> + 2 + 264 + 9.2897773716780951e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 5060 + -9.2897773716780951e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 3783 + 9.2897773716780951e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>3 + 4.3127682805061340e-01 + 1.1550946235656738e+00 + <_> + 2 + 2882 + -9.2897773716780951e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 901 + 9.2897773716780951e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + 8.4761073934510067e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 6.4322030544281006e-01 + 6.6717926025390625e+01 + <_> + 1 + 6279 + -8.4761073934510067e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>23 + 3.7461647391319275e-01 + 8.8262379169464111e-02 + <_> + 2 + 911 + 8.4761073934510067e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 5368 + -8.4761073934510067e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 2828 + 8.4761073934510067e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 3.2544922828674316e-01 + 2.5000000372529030e-02 + <_> + 2 + 1623 + -8.4761073934510067e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 1205 + 8.4761073934510067e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + -6.8203305172393069e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>24 + 6.0584831237792969e-01 + 270. + <_> + 1 + 5464 + 6.8203305172393069e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>25 + 5.3175848722457886e-01 + 9.9500000000000000e+01 + <_> + 2 + 2965 + -6.8203305172393069e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 2499 + 6.8203305172393069e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 3643 + -6.8203305172393069e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 1.3242444396018982e-01 + 3.9999997615814209e-01 + <_> + 2 + 1 + -6.8203305172393069e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 3642 + -6.8203305172393069e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + -6.8717287466109556e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 5.9576237201690674e-01 + 6.3829338073730469e+01 + <_> + 1 + 4957 + -6.8717287466109556e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 2.2716684639453888e-01 + 7.2365472412109375e+02 + <_> + 2 + 198 + 6.8717287466109556e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 4759 + -6.8717287466109556e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 4150 + 6.8717287466109556e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>25 + 4.3817088007926941e-01 + 1.7850000000000000e+02 + <_> + 2 + 3634 + 6.8717287466109556e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 516 + -6.8717287466109556e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + -6.6399674013829491e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>8 + 6.1477029323577881e-01 + 1.2550000000000000e+02 + <_> + 1 + 7371 + -6.6399674013829491e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>26 + 5.1602345705032349e-01 + 8.1892893750000000e+05 + <_> + 2 + 7338 + -6.6399674013829491e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 33 + 6.6399674013829491e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 1736 + 6.6399674013829491e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 1.4413419365882874e-01 + 1.8449199199676514e-01 + <_> + 2 + 703 + -6.6399674013829491e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 1033 + 6.6399674013829491e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + 6.7159186942512283e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>8 + 6.0821497440338135e-01 + 3.7500000000000000e+01 + <_> + 1 + 4219 + 6.7159186942512283e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>24 + 4.0776279568672180e-01 + 270. + <_> + 2 + 2183 + 6.7159186942512283e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 2036 + -6.7159186942512283e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 4888 + -6.7159186942512283e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>3 + 2.5409671664237976e-01 + 1.5013561248779297e+00 + <_> + 2 + 3390 + -6.7159186942512283e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 1498 + 6.7159186942512283e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + -6.2809361212419890e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 5.8938223123550415e-01 + 1.5500000000000000e+01 + <_> + 1 + 2964 + 6.2809361212419890e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>3 + 1.8443775177001953e-01 + 1.0236372947692871e+00 + <_> + 2 + 2227 + -6.2809361212419890e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 737 + 6.2809361212419890e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 6143 + -6.2809361212419890e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>3 + 4.6761932969093323e-01 + 8.2522457838058472e-01 + <_> + 2 + 2341 + 6.2809361212419890e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 3802 + -6.2809361212419890e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + 7.0723932494989528e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.8280897140502930e-01 + 5.2326019287109375e+01 + <_> + 1 + 6982 + 7.0723932494989528e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 4.1291543841362000e-01 + 2.7679210662841797e+01 + <_> + 2 + 3720 + -7.0723932494989528e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 3262 + 7.0723932494989528e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 2125 + -7.0723932494989528e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 2.5687542557716370e-01 + 1.8152867257595062e-01 + <_> + 2 + 1253 + 7.0723932494989528e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 872 + -7.0723932494989528e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + -6.2584596498195966e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 5.9209883213043213e-01 + 4.1250659942626953e+01 + <_> + 1 + 5070 + 6.2584596498195966e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>8 + 2.1918424963951111e-01 + 1.3500000000000000e+01 + <_> + 2 + 1887 + 6.2584596498195966e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 3183 + -6.2584596498195966e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 4037 + -6.2584596498195966e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>8 + 4.3236270546913147e-01 + 1.2050000000000000e+02 + <_> + 2 + 3165 + -6.2584596498195966e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 872 + 6.2584596498195966e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + 6.1904643227130030e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>24 + 5.7970148324966431e-01 + 170. + <_> + 1 + 1161 + 6.1904643227130030e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>23 + 3.3826807141304016e-01 + 8.4383964538574219e-02 + <_> + 2 + 138 + -6.1904643227130030e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 1023 + 6.1904643227130030e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 7946 + -6.1904643227130030e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 3.1173357367515564e-01 + 2.1469295024871826e-01 + <_> + 2 + 6329 + -6.1904643227130030e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 1617 + 6.1904643227130030e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + -6.3131072039408431e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>27 + 5.7840973138809204e-01 + 3.0634849548339844e+01 + <_> + 1 + 7789 + -6.3131072039408431e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>3 + 3.7280678749084473e-01 + 3.2754845619201660e+00 + <_> + 2 + 7573 + -6.3131072039408431e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 216 + 6.3131072039408431e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 1318 + 6.3131072039408431e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 2.7997982501983643e-01 + 1.3653913140296936e-01 + <_> + 2 + 555 + -6.3131072039408431e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 763 + 6.3131072039408431e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + 6.5502294805462713e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>23 + 6.1450338363647461e-01 + 1.2136831134557724e-01 + <_> + 1 + 5667 + 6.5502294805462713e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>27 + 5.2657282352447510e-01 + 5.4250793457031250e+01 + <_> + 2 + 5532 + 6.5502294805462713e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 135 + -6.5502294805462713e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 3440 + -6.5502294805462713e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>8 + 1.3156862556934357e-01 + 5.0000000000000000e-01 + <_> + 2 + 42 + 6.5502294805462713e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 3398 + -6.5502294805462713e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + -5.0226863579218839e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 5.8985453844070435e-01 + 9.5000000000000000e+00 + <_> + 1 + 2414 + 5.0226863579218839e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 1.0689283907413483e-01 + 1.8055555224418640e-01 + <_> + 2 + 2211 + -5.0226863579218839e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 203 + 5.0226863579218839e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 6693 + -5.0226863579218839e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 5.1609945297241211e-01 + 1.4911785125732422e+00 + <_> + 2 + 123 + 5.0226863579218839e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 6570 + -5.0226863579218839e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + 4.7991872916504247e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>24 + 5.8454781770706177e-01 + 270. + <_> + 1 + 5464 + 4.7991872916504247e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>25 + 5.5896860361099243e-01 + 1.8550000000000000e+02 + <_> + 2 + 4425 + 4.7991872916504247e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 1039 + -4.7991872916504247e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 3643 + -4.7991872916504247e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>23 + 5.8760087937116623e-02 + 1.4157611131668091e-01 + <_> + 2 + 3424 + -4.7991872916504247e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 219 + 4.7991872916504247e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + -5.1678353149475698e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>3 + 5.8672708272933960e-01 + 1.5460885763168335e+00 + <_> + 1 + 6012 + -5.1678353149475698e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 4.2891278862953186e-01 + 3.8852580566406250e+03 + <_> + 2 + 5942 + -5.1678353149475698e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 70 + 5.1678353149475698e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 3095 + 5.1678353149475698e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>26 + 1.9748255610466003e-01 + 2.9461697265625000e+04 + <_> + 2 + 1710 + -5.1678353149475698e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 1385 + 5.1678353149475698e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + 4.0363490149754094e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.5959206819534302e-01 + 2.7556386718750000e+03 + <_> + 1 + 7882 + 4.0363490149754094e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>3 + 4.4465914368629456e-01 + 5.2763319015502930e-01 + <_> + 2 + 2963 + -4.0363490149754094e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 4919 + 4.0363490149754094e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 1225 + -4.0363490149754094e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 1.5490151941776276e-01 + 6.4314804077148438e+01 + <_> + 2 + 225 + 4.0363490149754094e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 1000 + -4.0363490149754094e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + -4.9095934231959842e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>13 + 5.8047777414321899e-01 + 8.6264999389648438e+01 + <_> + 1 + 7440 + -4.9095934231959842e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 4.4410687685012817e-01 + 1.0182608366012573e+00 + <_> + 2 + 742 + 4.9095934231959842e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 6698 + -4.9095934231959842e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 1667 + 4.9095934231959842e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>13 + 1.7622554302215576e-01 + 1.1985499572753906e+02 + <_> + 2 + 463 + 4.9095934231959842e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 1204 + -4.9095934231959842e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + 5.0451759294742304e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 5.6870746612548828e-01 + 1.5613188743591309e+00 + <_> + 1 + 4478 + -5.0451759294742304e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 2.6170817017555237e-01 + 8.5175827026367188e+01 + <_> + 2 + 4180 + -5.0451759294742304e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 298 + 5.0451759294742304e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 4629 + 5.0451759294742304e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>25 + 3.6181223392486572e-01 + 1.7850000000000000e+02 + <_> + 2 + 3738 + 5.0451759294742304e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 891 + -5.0451759294742304e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + -4.8549094627092687e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 5.6624644994735718e-01 + 4.1566818237304688e+01 + <_> + 1 + 5120 + 4.8549094627092687e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>24 + 2.1347649395465851e-01 + 170. + <_> + 2 + 444 + 4.8549094627092687e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 4676 + -4.8549094627092687e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 3987 + -4.8549094627092687e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>25 + 4.0556713938713074e-01 + 8.3500000000000000e+01 + <_> + 2 + 2284 + 4.8549094627092687e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 1703 + -4.8549094627092687e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + -4.4357375720496384e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 5.6585323810577393e-01 + 1.0664761066436768e+00 + <_> + 1 + 1768 + 4.4357375720496384e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 1.4249736070632935e-01 + 4.8953857421875000e+03 + <_> + 2 + 1438 + 4.4357375720496384e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 330 + -4.4357375720496384e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 7339 + -4.4357375720496384e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 4.6661290526390076e-01 + 4.1567359924316406e+01 + <_> + 2 + 4129 + 4.4357375720496384e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 3210 + -4.4357375720496384e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + 5.1670651941009904e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 5.7469356060028076e-01 + 5.1958709716796875e+01 + <_> + 1 + 6391 + -5.1670651941009904e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>24 + 2.8974708914756775e-01 + 205. + <_> + 2 + 3431 + -5.1670651941009904e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 2960 + 5.1670651941009904e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 2716 + 5.1670651941009904e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 3.3663022518157959e-01 + 1.3901821899414062e+02 + <_> + 2 + 2505 + 5.1670651941009904e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 211 + -5.1670651941009904e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + -3.9175005119569001e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 5.5667036771774292e-01 + 2.3750000000000000e+00 + <_> + 1 + 2527 + 3.9175005119569001e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 1.1789477616548538e-01 + 1.9780056476593018e+00 + <_> + 2 + 955 + -3.9175005119569001e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 1572 + 3.9175005119569001e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 6580 + -3.9175005119569001e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 4.7880914807319641e-01 + 2.9500000000000000e+01 + <_> + 2 + 4174 + -3.9175005119569001e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 2406 + 3.9175005119569001e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + -5.1470802376187574e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>24 + 5.5831867456436157e-01 + 170. + <_> + 1 + 1161 + 5.1470802376187574e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 3.9778375625610352e-01 + 6.7979972839355469e+01 + <_> + 2 + 765 + 5.1470802376187574e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 396 + -5.1470802376187574e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 7946 + -5.1470802376187574e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 2.2812573611736298e-01 + 9.2190345764160156e+01 + <_> + 2 + 7313 + -5.1470802376187574e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 633 + 5.1470802376187574e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + 4.2337279493316338e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 5.6844371557235718e-01 + 1.1582202148437500e+02 + <_> + 1 + 7561 + -4.2337279493316338e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 3.7152063846588135e-01 + 1.1693493652343750e+03 + <_> + 2 + 3161 + 4.2337279493316338e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 4400 + -4.2337279493316338e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 1546 + 4.2337279493316338e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 2.3276942968368530e-01 + 6.9638891220092773e+00 + <_> + 2 + 1219 + 4.2337279493316338e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 327 + -4.2337279493316338e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + 4.5821621806657103e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 5.8577603101730347e-01 + 5.7022949218750000e+01 + <_> + 1 + 3774 + -4.5821621806657103e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>24 + 1.4161908626556396e-01 + 205. + <_> + 2 + 1937 + -4.5821621806657103e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 1837 + 4.5821621806657103e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 5333 + 4.5821621806657103e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>24 + 4.7097182273864746e-01 + 270. + <_> + 2 + 3496 + 4.5821621806657103e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 1837 + -4.5821621806657103e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + -3.7843490616237097e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>4 + 5.6317996978759766e-01 + 1.0272638702392578e+02 + <_> + 1 + 8130 + -3.7843490616237097e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 5.0669294595718384e-01 + 1.4659145474433899e-01 + <_> + 2 + 2015 + 3.7843490616237097e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 6115 + -3.7843490616237097e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 977 + 3.7843490616237097e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 8.6802639067173004e-02 + 3.0500000000000000e+01 + <_> + 2 + 472 + 3.7843490616237097e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 505 + -3.7843490616237097e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + 4.4811816939793903e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 5.7101595401763916e-01 + 2.0504987239837646e-01 + <_> + 1 + 3615 + -4.4811816939793903e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 2.5422197580337524e-01 + 6.0896649956703186e-02 + <_> + 2 + 3062 + -4.4811816939793903e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 553 + 4.4811816939793903e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 5492 + 4.4811816939793903e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>27 + 3.5596972703933716e-01 + 1.4530952453613281e+01 + <_> + 2 + 3831 + -4.4811816939793903e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 1661 + 4.4811816939793903e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + 4.1148184795391379e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>3 + 5.6035870313644409e-01 + 9.9717175960540771e-01 + <_> + 1 + 4861 + -4.1148184795391379e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 1.9316196441650391e-01 + 9.5117878913879395e-01 + <_> + 2 + 494 + 4.1148184795391379e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 4367 + -4.1148184795391379e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 4246 + 4.1148184795391379e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 4.0828117728233337e-01 + 1.6942491754889488e-02 + <_> + 2 + 2732 + -4.1148184795391379e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 1514 + 4.1148184795391379e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + -4.3029155546966441e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 5.6485289335250854e-01 + 2.3122344166040421e-02 + <_> + 1 + 4543 + 4.3029155546966441e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>24 + 2.2718144953250885e-01 + 360. + <_> + 2 + 2882 + 4.3029155546966441e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 1661 + -4.3029155546966441e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 4564 + -4.3029155546966441e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 3.7876182794570923e-01 + 9.5000000000000000e+00 + <_> + 2 + 2356 + 4.3029155546966441e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 2208 + -4.3029155546966441e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + -4.1773668811624759e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 5.6275844573974609e-01 + 2.7182479858398438e+02 + <_> + 1 + 7297 + -4.1773668811624759e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 3.6964702606201172e-01 + 3.7593054771423340e-01 + <_> + 2 + 6400 + -4.1773668811624759e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 897 + 4.1773668811624759e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 1810 + 4.1773668811624759e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 2.3329451680183411e-01 + 5.7824768066406250e+02 + <_> + 2 + 1337 + 4.1773668811624759e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 473 + -4.1773668811624759e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + 3.7409012181142703e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.5900222063064575e-01 + 2.5404727935791016e+01 + <_> + 1 + 3041 + -3.7409012181142703e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>23 + 6.6533848643302917e-02 + 8.0045714974403381e-02 + <_> + 2 + 183 + 3.7409012181142703e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 2858 + -3.7409012181142703e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 6066 + 3.7409012181142703e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 5.2591305971145630e-01 + 5.0168984375000000e+03 + <_> + 2 + 4591 + 3.7409012181142703e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 1475 + -3.7409012181142703e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + -4.0148831215433622e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 5.6105279922485352e-01 + 1.4739143848419189e-01 + <_> + 1 + 2280 + 4.0148831215433622e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>23 + 1.3512298464775085e-01 + 1.3335093855857849e-01 + <_> + 2 + 1782 + 4.0148831215433622e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 498 + -4.0148831215433622e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 6827 + -4.0148831215433622e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 4.6392220258712769e-01 + 1.4722222089767456e-01 + <_> + 2 + 2657 + 4.0148831215433622e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 4170 + -4.0148831215433622e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/classifier_models/vrf_hallway_boost.xml b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/classifier_models/vrf_hallway_boost.xml new file mode 100644 index 0000000..1971177 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/classifier_models/vrf_hallway_boost.xml @@ -0,0 +1,3260 @@ + + + + DiscreteAdaboost + 3 + 35 + 1. + 1 + 28 + 28 + 28 + 0 + + 0 + 10 + 2 + 10 + 0 + + 1 + 2 +
d
+ + 1. 1. + + 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 + + 1 + 1 +
i
+ + 2 + + 1 + 2 +
i
+ + -1 1 + + <_> + -1 + + <_> + 0 + 9107 + -2.3498728981776389e+00 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 8.9634346961975098e-01 + 6.6852409362792969e+01 + <_> + 1 + 7681 + -2.3498728981776389e+00 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 7.8093773126602173e-01 + 7.5708389282226562e+00 + <_> + 2 + 7532 + -2.3498728981776389e+00 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 149 + 2.3498728981776389e+00 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 1426 + 2.3498728981776389e+00 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>3 + 1.3198637962341309e-01 + 9.3578100204467773e-01 + <_> + 2 + 1238 + 2.3498728981776389e+00 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 188 + -2.3498728981776389e+00 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + -1.6132361966763005e+00 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 7.6537060737609863e-01 + 4.2279670715332031e+01 + <_> + 1 + 6116 + -1.6132361966763005e+00 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 3.8815322518348694e-01 + 3.4980114746093750e+02 + <_> + 2 + 68 + 1.6132361966763005e+00 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 6048 + -1.6132361966763005e+00 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 2991 + 1.6132361966763005e+00 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 4.4570696353912354e-01 + 1.3932722806930542e-01 + <_> + 2 + 549 + -1.6132361966763005e+00 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 2442 + 1.6132361966763005e+00 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + -8.6550204224557181e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 6.5861845016479492e-01 + 4.0388727188110352e+00 + <_> + 1 + 7734 + -8.6550204224557181e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>8 + 5.2588081359863281e-01 + 1.6050000000000000e+02 + <_> + 2 + 7122 + -8.6550204224557181e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 612 + 8.6550204224557181e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 1373 + 8.6550204224557181e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 1.7792810499668121e-01 + 2.3789729923009872e-02 + <_> + 2 + 367 + -8.6550204224557181e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 1006 + 8.6550204224557181e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + 1.7418811673013059e+00 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 6.9076508283615112e-01 + 4.9995690584182739e-02 + <_> + 1 + 6736 + 1.7418811673013059e+00 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 6.0081732273101807e-01 + 2.2865723818540573e-02 + <_> + 2 + 4506 + -1.7418811673013059e+00 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 2230 + 1.7418811673013059e+00 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 2371 + -1.7418811673013059e+00 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>1 + 2.5010851025581360e-01 + 2.2531497192382812e+02 + <_> + 2 + 2003 + -1.7418811673013059e+00 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 368 + 1.7418811673013059e+00 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + -6.1239962291525207e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>23 + 6.1669647693634033e-01 + 8.4107868373394012e-02 + <_> + 1 + 1193 + 6.1239962291525207e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 1.1767710745334625e-01 + 1.6722180175781250e+02 + <_> + 2 + 450 + -6.1239962291525207e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 743 + 6.1239962291525207e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 7914 + -6.1239962291525207e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 5.3081089258193970e-01 + 1.4006979370117188e+02 + <_> + 2 + 7625 + -6.1239962291525207e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 289 + 6.1239962291525207e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + 9.1579372703108464e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>3 + 6.5173763036727905e-01 + 1.2015671730041504e+00 + <_> + 1 + 5305 + 9.1579372703108464e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 5.5489611625671387e-01 + 9.7820323705673218e-01 + <_> + 2 + 3417 + 9.1579372703108464e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 1888 + -9.1579372703108464e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 3802 + -9.1579372703108464e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 1.5928816795349121e-01 + 1.8055555224418640e-01 + <_> + 2 + 68 + 9.1579372703108464e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 3734 + -9.1579372703108464e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + -8.6551874205548596e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 6.1528581380844116e-01 + 6.6963409423828125e+01 + <_> + 1 + 7686 + 8.6551874205548596e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 4.4570353627204895e-01 + 4.1311408996582031e+01 + <_> + 2 + 5079 + -8.6551874205548596e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 2607 + 8.6551874205548596e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 1421 + -8.6551874205548596e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 2.5810885429382324e-01 + 1.0591673851013184e+00 + <_> + 2 + 145 + 8.6551874205548596e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 1276 + -8.6551874205548596e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + -6.2614995057232792e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 5.9996426105499268e-01 + 9.7818785905838013e-01 + <_> + 1 + 7185 + -6.2614995057232792e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>26 + 5.0314968824386597e-01 + 2.3851454687500000e+05 + <_> + 2 + 6646 + -6.2614995057232792e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 539 + 6.2614995057232792e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 1922 + 6.2614995057232792e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 1.4846630394458771e-01 + 9.8264855146408081e-01 + <_> + 2 + 777 + 6.2614995057232792e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 1145 + -6.2614995057232792e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + 6.2991355645422531e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 5.9118473529815674e-01 + 3.3339038085937500e+03 + <_> + 1 + 6044 + 6.2991355645422531e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 3.9555683732032776e-01 + 6.2690764665603638e-02 + <_> + 2 + 4856 + 6.2991355645422531e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 1188 + -6.2991355645422531e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 3063 + -6.2991355645422531e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 2.5691303610801697e-01 + 1.1739651858806610e-01 + <_> + 2 + 621 + 6.2991355645422531e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 2442 + -6.2991355645422531e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + -5.3693540876437640e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>3 + 5.8123505115509033e-01 + 5.0875937938690186e-01 + <_> + 1 + 3368 + 5.3693540876437640e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>23 + 2.7925282716751099e-01 + 1.3729497790336609e-01 + <_> + 2 + 2283 + 5.3693540876437640e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 1085 + -5.3693540876437640e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 5739 + -5.3693540876437640e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>25 + 3.5184639692306519e-01 + 1.6750000000000000e+02 + <_> + 2 + 4035 + -5.3693540876437640e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 1704 + 5.3693540876437640e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + -4.8439268669317326e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 5.6430369615554810e-01 + 2.5947868824005127e+00 + <_> + 1 + 5606 + -4.8439268669317326e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 2.9663595557212830e-01 + 3.9991921386718750e+03 + <_> + 2 + 4874 + -4.8439268669317326e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 732 + 4.8439268669317326e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 3501 + 4.8439268669317326e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 3.2214865088462830e-01 + 1.1504754424095154e-01 + <_> + 2 + 316 + -4.8439268669317326e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 3185 + 4.8439268669317326e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + -5.5437128649510958e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 5.7781493663787842e-01 + 2.2847714843750000e+03 + <_> + 1 + 4479 + 5.5437128649510958e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 2.8067153692245483e-01 + 1.2577197265625000e+02 + <_> + 2 + 2399 + -5.5437128649510958e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 2080 + 5.5437128649510958e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 4628 + -5.5437128649510958e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 3.5447761416435242e-01 + 1.3909022521972656e+02 + <_> + 2 + 4345 + -5.5437128649510958e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 283 + 5.5437128649510958e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + 5.1617004625401719e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>3 + 5.7309782505035400e-01 + 1.2401015758514404e+00 + <_> + 1 + 5400 + 5.1617004625401719e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 5.1140254735946655e-01 + 4.0250364691019058e-02 + <_> + 2 + 2528 + 5.1617004625401719e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 2872 + -5.1617004625401719e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 3707 + -5.1617004625401719e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>26 + 1.1484922468662262e-01 + 1.3003096875000000e+05 + <_> + 2 + 3570 + -5.1617004625401719e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 137 + 5.1617004625401719e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + 5.1095995142958284e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 5.9661746025085449e-01 + 2.8615169227123260e-02 + <_> + 1 + 5241 + -5.1095995142958284e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>23 + 2.1537163853645325e-01 + 7.1873486042022705e-02 + <_> + 2 + 118 + 5.1095995142958284e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 5123 + -5.1095995142958284e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 3866 + 5.1095995142958284e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>24 + 4.0965983271598816e-01 + 170. + <_> + 2 + 573 + -5.1095995142958284e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 3293 + 5.1095995142958284e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + 5.2964126402070311e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>25 + 5.6208497285842896e-01 + 5.3500000000000000e+01 + <_> + 1 + 3278 + -5.2964126402070311e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 2.6643174886703491e-01 + 9.9117996215820312e+01 + <_> + 2 + 2709 + -5.2964126402070311e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 569 + 5.2964126402070311e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 5829 + 5.2964126402070311e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 3.6296769976615906e-01 + 2.4722221493721008e-01 + <_> + 2 + 2507 + 5.2964126402070311e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 3322 + -5.2964126402070311e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + -5.1827892565724454e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>26 + 5.6495547294616699e-01 + 7.6652656250000000e+04 + <_> + 1 + 6548 + -5.1827892565724454e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 2.6618286967277527e-01 + 1.0029506683349609e+02 + <_> + 2 + 6381 + -5.1827892565724454e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 167 + 5.1827892565724454e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 2559 + 5.1827892565724454e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 3.6056235432624817e-01 + 7.5181976318359375e+01 + <_> + 2 + 1238 + 5.1827892565724454e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 1321 + -5.1827892565724454e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + 4.5883209311338768e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 5.7222849130630493e-01 + 1.7901263236999512e+00 + <_> + 1 + 2055 + -4.5883209311338768e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 1.0671431571245193e-01 + 2.0302377641201019e-01 + <_> + 2 + 369 + 4.5883209311338768e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 1686 + -4.5883209311338768e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 7052 + 4.5883209311338768e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 5.0602275133132935e-01 + 2.7500000000000000e+01 + <_> + 2 + 4921 + 4.5883209311338768e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 2131 + -4.5883209311338768e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + -4.3320922136911183e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 5.7254517078399658e-01 + 2.4733764648437500e+03 + <_> + 1 + 7396 + -4.3320922136911183e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>8 + 4.4614291191101074e-01 + 1.5950000000000000e+02 + <_> + 2 + 6660 + -4.3320922136911183e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 736 + 4.3320922136911183e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 1711 + 4.3320922136911183e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 1.6049681603908539e-01 + 9.4378352165222168e-01 + <_> + 2 + 896 + -4.3320922136911183e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 815 + 4.3320922136911183e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + 4.7336203324773962e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>26 + 5.7051217555999756e-01 + 3.3268468750000000e+04 + <_> + 1 + 3973 + -4.7336203324773962e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 1.0071597993373871e-01 + 1.4042949676513672e-01 + <_> + 2 + 525 + 4.7336203324773962e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 3448 + -4.7336203324773962e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 5134 + 4.7336203324773962e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.1546323299407959e-01 + 1.1625519561767578e+02 + <_> + 2 + 4341 + 4.7336203324773962e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 793 + -4.7336203324773962e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + -5.0796412912997546e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.7402479648590088e-01 + 4.1616348266601562e+01 + <_> + 1 + 6033 + 5.0796412912997546e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 1.7215172946453094e-01 + 2.1717445373535156e+01 + <_> + 2 + 1642 + -5.0796412912997546e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 4391 + 5.0796412912997546e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 3074 + -5.0796412912997546e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 4.5217737555503845e-01 + 7.8341827392578125e+01 + <_> + 2 + 2129 + -5.0796412912997546e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 945 + 5.0796412912997546e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + -3.8899738965688907e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>8 + 5.5270242691040039e-01 + 2.4500000000000000e+01 + <_> + 1 + 3924 + 3.8899738965688907e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 2.6205247640609741e-01 + 1.2210161983966827e-01 + <_> + 2 + 386 + -3.8899738965688907e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 3538 + 3.8899738965688907e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 5183 + -3.8899738965688907e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>23 + 3.3398884534835815e-01 + 1.3739262521266937e-01 + <_> + 2 + 4429 + -3.8899738965688907e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 754 + 3.8899738965688907e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + -4.0197101477146691e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>27 + 5.5070811510086060e-01 + 2.2617143630981445e+01 + <_> + 1 + 6669 + 4.0197101477146691e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>26 + 3.6117196083068848e-01 + 3.9646132812500000e+04 + <_> + 2 + 3784 + -4.0197101477146691e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 2885 + 4.0197101477146691e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 2438 + -4.0197101477146691e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 2.3798917233943939e-01 + 1.3255763053894043e-01 + <_> + 2 + 796 + 4.0197101477146691e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 1642 + -4.0197101477146691e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + -3.3726200931165862e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.5675232410430908e-01 + 4.6216423034667969e+01 + <_> + 1 + 3585 + 3.3726200931165862e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 1.0723788291215897e-01 + 7.4369716644287109e+00 + <_> + 2 + 3479 + 3.3726200931165862e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 106 + -3.3726200931165862e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 5522 + -3.3726200931165862e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 4.7628739476203918e-01 + 2.7282537841796875e+02 + <_> + 2 + 5408 + -3.3726200931165862e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 114 + 3.3726200931165862e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + 5.0462966898521189e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 5.6343489885330200e-01 + 9.7466319799423218e-01 + <_> + 1 + 6634 + 5.0462966898521189e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>24 + 4.2315843701362610e-01 + 360. + <_> + 2 + 4627 + 5.0462966898521189e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 2007 + -5.0462966898521189e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 2473 + -5.0462966898521189e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>24 + 2.0038825273513794e-01 + 360. + <_> + 2 + 2127 + -5.0462966898521189e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 346 + 5.0462966898521189e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + -4.3392332539242695e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 5.6129997968673706e-01 + 1.1564830017089844e+02 + <_> + 1 + 7553 + 4.3392332539242695e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 3.8729444146156311e-01 + 2.5300484150648117e-02 + <_> + 2 + 4667 + -4.3392332539242695e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 2886 + 4.3392332539242695e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 1554 + -4.3392332539242695e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 2.1951568126678467e-01 + 5.5000000000000000e+00 + <_> + 2 + 270 + 4.3392332539242695e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 1284 + -4.3392332539242695e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + -5.3902185392370994e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>26 + 5.5517673492431641e-01 + 1.1114417968750000e+05 + <_> + 1 + 7301 + -5.3902185392370994e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 3.7256091833114624e-01 + 2.0056053996086121e-01 + <_> + 2 + 4944 + -5.3902185392370994e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 2357 + 5.3902185392370994e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 1806 + 5.3902185392370994e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>1 + 2.5902390480041504e-01 + 1.0350888061523438e+02 + <_> + 2 + 522 + 5.3902185392370994e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 1284 + -5.3902185392370994e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + -5.1612895044485407e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 5.7063508033752441e-01 + 1.7287626862525940e-01 + <_> + 1 + 4351 + 5.1612895044485407e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 2.7733165025711060e-01 + 1.1576941609382629e-01 + <_> + 2 + 623 + -5.1612895044485407e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 3728 + 5.1612895044485407e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 4756 + -5.1612895044485407e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>3 + 3.4891048073768616e-01 + 2.9192537069320679e-01 + <_> + 2 + 965 + 5.1612895044485407e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 3791 + -5.1612895044485407e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + -3.8710311692021926e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 5.5621719360351562e-01 + 4.8389980468750000e+03 + <_> + 1 + 7300 + 3.8710311692021926e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>3 + 4.3817147612571716e-01 + 1.3809230327606201e+00 + <_> + 2 + 4483 + 3.8710311692021926e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 2817 + -3.8710311692021926e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 1807 + -3.8710311692021926e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>26 + 1.5741367638111115e-01 + 2.3230450000000000e+05 + <_> + 2 + 1656 + -3.8710311692021926e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 151 + 3.8710311692021926e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + -4.5836902616031433e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 5.7203811407089233e-01 + 1.6857043457031250e+02 + <_> + 1 + 8715 + -4.5836902616031433e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 5.2488130331039429e-01 + 3.6255528926849365e+00 + <_> + 2 + 6956 + -4.5836902616031433e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 1759 + 4.5836902616031433e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 392 + 4.5836902616031433e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>4 + 8.7745875120162964e-02 + 1.4793472290039062e+02 + <_> + 2 + 336 + 4.5836902616031433e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 56 + -4.5836902616031433e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + -4.5206466576034049e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 5.4853254556655884e-01 + 3.2135329246520996e+00 + <_> + 1 + 6738 + 4.5206466576034049e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>3 + 3.7530311942100525e-01 + 3.8451132178306580e-01 + <_> + 2 + 1987 + -4.5206466576034049e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 4751 + 4.5206466576034049e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 2369 + -4.5206466576034049e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 2.3582689464092255e-01 + 2.6840999603271484e+01 + <_> + 2 + 328 + 4.5206466576034049e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 2041 + -4.5206466576034049e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + -4.0477377718435631e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>24 + 5.5303543806076050e-01 + 170. + <_> + 1 + 1161 + -4.0477377718435631e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 2.1656392514705658e-01 + 5.6737922668457031e+01 + <_> + 2 + 393 + 4.0477377718435631e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 768 + -4.0477377718435631e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 7946 + 4.0477377718435631e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 3.8327014446258545e-01 + 3.7380573272705078e+01 + <_> + 2 + 5031 + -4.0477377718435631e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 2915 + 4.0477377718435631e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + -3.8163551115998778e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>27 + 5.4923915863037109e-01 + 9.6087503433227539e+00 + <_> + 1 + 2590 + 3.8163551115998778e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 1.1972328275442123e-01 + 3.5117156803607941e-02 + <_> + 2 + 1311 + 3.8163551115998778e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 1279 + -3.8163551115998778e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 6517 + -3.8163551115998778e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 4.7454422712326050e-01 + 3.2089103013277054e-02 + <_> + 2 + 4369 + -3.8163551115998778e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 2148 + 3.8163551115998778e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + -4.0369302121759543e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 5.6569349765777588e-01 + 3.6500000000000000e+01 + <_> + 1 + 7171 + -4.0369302121759543e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>27 + 4.6061098575592041e-01 + 9.6087503433227539e+00 + <_> + 2 + 2260 + 4.0369302121759543e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 4911 + -4.0369302121759543e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 1936 + 4.0369302121759543e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 1.3896363973617554e-01 + 3.6500000000000000e+01 + <_> + 2 + 662 + 4.0369302121759543e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 1274 + -4.0369302121759543e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + 4.0541326166669733e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 5.6680959463119507e-01 + 4.7733578830957413e-02 + <_> + 1 + 6606 + 4.0541326166669733e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 4.0074354410171509e-01 + 1.7575678825378418e+00 + <_> + 2 + 1331 + -4.0541326166669733e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 5275 + 4.0541326166669733e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 2501 + -4.0541326166669733e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 1.9924402236938477e-01 + 1.4006979370117188e+02 + <_> + 2 + 2148 + -4.0541326166669733e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 353 + 4.0541326166669733e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + -4.5271880514002444e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 5.5249530076980591e-01 + 1.7500000000000000e+01 + <_> + 1 + 3366 + -4.5271880514002444e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 2.5954529643058777e-01 + 2.4601559340953827e-01 + <_> + 2 + 2809 + -4.5271880514002444e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 557 + 4.5271880514002444e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 5741 + 4.5271880514002444e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 3.5174015164375305e-01 + 2.1500000000000000e+01 + <_> + 2 + 1800 + 4.5271880514002444e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 3941 + -4.5271880514002444e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/classifier_models/vrf_room_boost.xml b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/classifier_models/vrf_room_boost.xml new file mode 100644 index 0000000..6ab40df --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/classifier_models/vrf_room_boost.xml @@ -0,0 +1,3260 @@ + + + + DiscreteAdaboost + 3 + 35 + 1. + 1 + 28 + 28 + 28 + 0 + + 0 + 10 + 2 + 10 + 0 + + 1 + 2 +
d
+ + 1. 1. + + 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 + + 1 + 1 +
i
+ + 2 + + 1 + 2 +
i
+ + -1 1 + + <_> + -1 + + <_> + 0 + 9107 + 2.1786954368674230e+00 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 8.8448446989059448e-01 + 6.1037555694580078e+01 + <_> + 1 + 7437 + 2.1786954368674230e+00 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 7.4107831716537476e-01 + 7.5708389282226562e+00 + <_> + 2 + 7296 + 2.1786954368674230e+00 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 141 + -2.1786954368674230e+00 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 1670 + -2.1786954368674230e+00 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 1.5724168717861176e-01 + 9.7995698451995850e-01 + <_> + 2 + 1573 + -2.1786954368674230e+00 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 97 + 2.1786954368674230e+00 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + 1.3794070830405805e+00 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 7.2231972217559814e-01 + 4.0672660827636719e+01 + <_> + 1 + 5924 + 1.3794070830405805e+00 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>24 + 3.9307719469070435e-01 + 170. + <_> + 2 + 599 + -1.3794070830405805e+00 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 5325 + 1.3794070830405805e+00 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 3183 + -1.3794070830405805e+00 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 4.0581855177879333e-01 + 1.3932722806930542e-01 + <_> + 2 + 595 + 1.3794070830405805e+00 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 2588 + -1.3794070830405805e+00 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + 9.4378708439698156e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>20 + 6.7319673299789429e-01 + 3.6255528926849365e+00 + <_> + 1 + 7256 + 9.4378708439698156e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>24 + 5.4163652658462524e-01 + 55. + <_> + 2 + 29 + -9.4378708439698156e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 7227 + 9.4378708439698156e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 1851 + -9.4378708439698156e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 1.7822745442390442e-01 + 2.3789729923009872e-02 + <_> + 2 + 569 + 9.4378708439698156e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 1282 + -9.4378708439698156e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + -1.1860507438617369e+00 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 6.5405559539794922e-01 + 4.9995690584182739e-02 + <_> + 1 + 6736 + -1.1860507438617369e+00 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 5.4635477066040039e-01 + 3.1944444775581360e-01 + <_> + 2 + 3744 + -1.1860507438617369e+00 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 2992 + 1.1860507438617369e+00 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 2371 + 1.1860507438617369e+00 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 2.1967922151088715e-01 + 1.0152906799316406e+02 + <_> + 2 + 1797 + 1.1860507438617369e+00 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 574 + -1.1860507438617369e+00 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + 7.1466725424286937e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>23 + 6.3218086957931519e-01 + 1.0788087546825409e-01 + <_> + 1 + 4004 + -7.1466725424286937e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 3.1435400247573853e-01 + 3.1702899932861328e+01 + <_> + 2 + 1767 + 7.1466725424286937e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 2237 + -7.1466725424286937e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 5103 + 7.1466725424286937e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>8 + 3.5707762837409973e-01 + 1.6250000000000000e+02 + <_> + 2 + 4846 + 7.1466725424286937e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 257 + -7.1466725424286937e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + 8.6260353600845341e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 6.0615360736846924e-01 + 6.0900188446044922e+01 + <_> + 1 + 7432 + -8.6260353600845341e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>26 + 5.0314950942993164e-01 + 3.6261449218750000e+04 + <_> + 2 + 4360 + 8.6260353600845341e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 3072 + -8.6260353600845341e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 1675 + 8.6260353600845341e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 2.0005480945110321e-01 + 1.4006979370117188e+02 + <_> + 2 + 1322 + 8.6260353600845341e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 353 + -8.6260353600845341e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + 6.6088695298394085e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>25 + 5.9356105327606201e-01 + 6.7500000000000000e+01 + <_> + 1 + 3775 + 6.6088695298394085e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>8 + 2.5638625025749207e-01 + 1.5850000000000000e+02 + <_> + 2 + 3368 + 6.6088695298394085e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 407 + -6.6088695298394085e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 5332 + -6.6088695298394085e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>24 + 4.0307337045669556e-01 + 270. + <_> + 2 + 3116 + -6.6088695298394085e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 2216 + 6.6088695298394085e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + 6.0116260359509455e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>8 + 6.1124479770660400e-01 + 1.5500000000000000e+01 + <_> + 1 + 3596 + -6.0116260359509455e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 2.9448843002319336e-01 + 1.8027573823928833e-02 + <_> + 2 + 1489 + 6.0116260359509455e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 2107 + -6.0116260359509455e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 5511 + 6.0116260359509455e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>26 + 3.5143381357192993e-01 + 2.3851454687500000e+05 + <_> + 2 + 5265 + 6.0116260359509455e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 246 + -6.0116260359509455e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + -7.4746039217199778e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 5.9637230634689331e-01 + 2.5116939097642899e-02 + <_> + 1 + 4815 + 7.4746039217199778e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>3 + 2.5227269530296326e-01 + 1.5344743728637695e+00 + <_> + 2 + 1881 + 7.4746039217199778e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 2934 + -7.4746039217199778e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 4292 + -7.4746039217199778e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 4.2635238170623779e-01 + 5.7995446026325226e-02 + <_> + 2 + 2334 + -7.4746039217199778e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 1958 + 7.4746039217199778e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + 5.8438586566167305e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>21 + 5.9664434194564819e-01 + 2.8839975595474243e-01 + <_> + 1 + 6563 + 5.8438586566167305e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 4.7678318619728088e-01 + 9.9117996215820312e+01 + <_> + 2 + 5993 + 5.8438586566167305e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 570 + -5.8438586566167305e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 2544 + -5.8438586566167305e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>27 + 1.6529276967048645e-01 + 6.4519348144531250e+00 + <_> + 2 + 997 + 5.8438586566167305e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 1547 + -5.8438586566167305e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + -5.6900831390036832e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 5.9064292907714844e-01 + 2.7500000000000000e+01 + <_> + 1 + 6292 + -5.6900831390036832e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 4.4827651977539062e-01 + 9.8225486278533936e-01 + <_> + 2 + 5073 + -5.6900831390036832e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 1219 + 5.6900831390036832e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 2815 + 5.6900831390036832e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>24 + 1.9025780260562897e-01 + 170. + <_> + 2 + 378 + -5.6900831390036832e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 2437 + 5.6900831390036832e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + 4.9992880761827374e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>8 + 5.8090418577194214e-01 + 1.3350000000000000e+02 + <_> + 1 + 7620 + 4.9992880761827374e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 4.9585211277008057e-01 + 3.4901428222656250e+01 + <_> + 2 + 599 + -4.9992880761827374e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 7021 + 4.9992880761827374e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 1487 + -4.9992880761827374e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 1.2659050524234772e-01 + 2.4166667461395264e-01 + <_> + 2 + 823 + -4.9992880761827374e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 664 + 4.9992880761827374e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + -5.5304997807149114e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.8426886796951294e-01 + 2.7679210662841797e+01 + <_> + 1 + 3720 + 5.5304997807149114e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 1.1992295086383820e-01 + 8.8076839447021484e+00 + <_> + 2 + 2963 + 5.5304997807149114e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 757 + -5.5304997807149114e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 5387 + -5.5304997807149114e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>9 + 5.1491999626159668e-01 + 9.7762262821197510e-01 + <_> + 2 + 4181 + -5.5304997807149114e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 1206 + 5.5304997807149114e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + 3.6291999263461128e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 5.6129926443099976e-01 + 8.9882922363281250e+02 + <_> + 1 + 748 + -3.6291999263461128e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 7.2228454053401947e-02 + 1.8500000000000000e+01 + <_> + 2 + 459 + -3.6291999263461128e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 289 + 3.6291999263461128e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 8359 + 3.6291999263461128e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>13 + 5.1751863956451416e-01 + 3.4893188476562500e+02 + <_> + 2 + 7983 + 3.6291999263461128e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 376 + -3.6291999263461128e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + -4.5221083737375550e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>24 + 5.7584840059280396e-01 + 110. + <_> + 1 + 625 + 4.5221083737375550e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>24 + 8.6570978164672852e-02 + 55. + <_> + 2 + 38 + -4.5221083737375550e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 587 + 4.5221083737375550e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 8482 + -4.5221083737375550e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>5 + 5.2459377050399780e-01 + 7.5181976318359375e+01 + <_> + 2 + 7174 + -4.5221083737375550e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 1308 + 4.5221083737375550e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + 4.4606259888224020e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.7565021514892578e-01 + 4.4157386779785156e+01 + <_> + 1 + 6274 + 4.4606259888224020e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>27 + 2.9201084375381470e-01 + 1.9632499694824219e+01 + <_> + 2 + 4454 + 4.4606259888224020e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 1820 + -4.4606259888224020e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 2833 + -4.4606259888224020e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 3.1769183278083801e-01 + 1.1500696092844009e-01 + <_> + 2 + 180 + 4.4606259888224020e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 2653 + -4.4606259888224020e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + 4.1269271599030560e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 5.7579904794692993e-01 + 8.0012268066406250e+01 + <_> + 1 + 8192 + 4.1269271599030560e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 5.3666657209396362e-01 + 2.5583333969116211e+00 + <_> + 2 + 2881 + -4.1269271599030560e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 5311 + 4.1269271599030560e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 915 + -4.1269271599030560e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>24 + 6.5066799521446228e-02 + 140. + <_> + 2 + 106 + 4.1269271599030560e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 809 + -4.1269271599030560e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + 4.3096056583144365e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>0 + 5.6360632181167603e-01 + 4.2027778625488281e+00 + <_> + 1 + 5429 + 4.3096056583144365e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 3.2183092832565308e-01 + 8.0210857391357422e+00 + <_> + 2 + 4322 + 4.3096056583144365e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 1107 + -4.3096056583144365e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 3678 + -4.3096056583144365e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 2.8427210450172424e-01 + 2.9854658203125000e+03 + <_> + 2 + 2934 + -4.3096056583144365e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 744 + 4.3096056583144365e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + 5.1244591858428068e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 5.4743778705596924e-01 + 2.2856015712022781e-02 + <_> + 1 + 4504 + 5.1244591858428068e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>24 + 1.9601167738437653e-01 + 170. + <_> + 2 + 440 + -5.1244591858428068e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 4064 + 5.1244591858428068e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 4603 + -5.1244591858428068e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 4.2936801910400391e-01 + 4.2881276458501816e-02 + <_> + 2 + 1755 + -5.1244591858428068e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 2848 + 5.1244591858428068e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + 4.3789005694068295e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 5.7462888956069946e-01 + 2.8502143919467926e-02 + <_> + 1 + 5227 + 4.3789005694068295e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>27 + 2.9981964826583862e-01 + 9.9598407745361328e+00 + <_> + 2 + 1259 + -4.3789005694068295e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 3968 + 4.3789005694068295e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 3880 + -4.3789005694068295e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>27 + 3.0793648958206177e-01 + 8.2142152786254883e+00 + <_> + 2 + 1150 + 4.3789005694068295e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 2730 + -4.3789005694068295e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + 3.9517989176067042e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 5.6575673818588257e-01 + 4.2500000000000000e+01 + <_> + 1 + 8073 + 3.9517989176067042e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>26 + 4.9922975897789001e-01 + 2.3148275000000000e+05 + <_> + 2 + 7442 + 3.9517989176067042e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 631 + -3.9517989176067042e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 1034 + -3.9517989176067042e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>7 + 9.8299264907836914e-02 + 3.6081323623657227e+00 + <_> + 2 + 530 + -3.9517989176067042e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 504 + 3.9517989176067042e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + -5.1636398057703703e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 5.6804054975509644e-01 + 2.4194677734375000e+03 + <_> + 1 + 4732 + -5.1636398057703703e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>10 + 3.5615742206573486e-01 + 1.1989330291748047e+02 + <_> + 2 + 4266 + -5.1636398057703703e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 466 + 5.1636398057703703e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 4375 + 5.1636398057703703e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>26 + 2.7013972401618958e-01 + 1.1097114843750000e+05 + <_> + 2 + 3174 + 5.1636398057703703e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 1201 + -5.1636398057703703e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + -4.1723969575603886e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 5.5497741699218750e-01 + 1.6999731957912445e-01 + <_> + 1 + 4082 + -4.1723969575603886e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>25 + 2.4794057011604309e-01 + 9.5000000000000000e+00 + <_> + 2 + 1399 + 4.1723969575603886e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 2683 + -4.1723969575603886e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 5025 + 4.1723969575603886e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>11 + 3.5488197207450867e-01 + 1.1500000000000000e+01 + <_> + 2 + 1566 + -4.1723969575603886e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 3459 + 4.1723969575603886e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + -3.9419468697033788e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 5.5248409509658813e-01 + 1.1833110809326172e+02 + <_> + 1 + 3146 + 3.9419468697033788e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>23 + 9.8889589309692383e-02 + 8.0045714974403381e-02 + <_> + 2 + 151 + -3.9419468697033788e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 2995 + 3.9419468697033788e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 5961 + -3.9419468697033788e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 4.9840247631072998e-01 + 1.1940277862548828e+02 + <_> + 2 + 4554 + -3.9419468697033788e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 1407 + 3.9419468697033788e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + 3.8693352640484802e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>26 + 5.6319475173950195e-01 + 7.7233914062500000e+04 + <_> + 1 + 6570 + 3.8693352640484802e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 3.5922673344612122e-01 + 9.8053543090820312e+01 + <_> + 2 + 6381 + 3.8693352640484802e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 189 + -3.8693352640484802e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 2537 + -3.8693352640484802e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>27 + 2.3631756007671356e-01 + 1.8602401733398438e+01 + <_> + 2 + 1450 + -3.8693352640484802e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 1087 + 3.8693352640484802e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + -3.9186399829511376e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>6 + 5.5295193195343018e-01 + 1.2500000000000000e+01 + <_> + 1 + 2442 + 3.9186399829511376e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>3 + 1.8892322480678558e-01 + 9.8895144462585449e-01 + <_> + 2 + 1968 + 3.9186399829511376e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 474 + -3.9186399829511376e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 6665 + -3.9186399829511376e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>22 + 4.0780812501907349e-01 + 1.1576941609382629e-01 + <_> + 2 + 433 + 3.9186399829511376e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 6232 + -3.9186399829511376e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + 4.3512462423498988e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.6591624021530151e-01 + 5.1285041809082031e+01 + <_> + 1 + 4499 + -4.3512462423498988e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>4 + 2.2599273920059204e-01 + 3.4398612976074219e+01 + <_> + 2 + 1371 + 4.3512462423498988e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 3128 + -4.3512462423498988e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 4608 + 4.3512462423498988e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>8 + 3.8110399246215820e-01 + 1.1450000000000000e+02 + <_> + 2 + 3665 + 4.3512462423498988e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 943 + -4.3512462423498988e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + 3.8309151695615390e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 5.5419266223907471e-01 + 3.0633434653282166e-02 + <_> + 1 + 5499 + 3.8309151695615390e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>2 + 3.1679347157478333e-01 + 1.4166668057441711e-01 + <_> + 2 + 689 + -3.8309151695615390e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 4810 + 3.8309151695615390e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 3608 + -3.8309151695615390e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>15 + 2.7782505750656128e-01 + 6.6393353044986725e-02 + <_> + 2 + 2048 + -3.8309151695615390e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 1560 + 3.8309151695615390e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + -3.6656982842834041e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>8 + 5.4765290021896362e-01 + 9.0500000000000000e+01 + <_> + 1 + 6456 + -3.6656982842834041e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>23 + 3.9640560746192932e-01 + 1.5261673927307129e-01 + <_> + 2 + 5846 + -3.6656982842834041e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 610 + 3.6656982842834041e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 2651 + 3.6656982842834041e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>27 + 1.9422426819801331e-01 + 3.0891586303710938e+01 + <_> + 2 + 2230 + 3.6656982842834041e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 421 + -3.6656982842834041e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + 4.2523581015947221e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>16 + 5.6275755167007446e-01 + 4.2253437042236328e+01 + <_> + 1 + 2733 + -4.2523581015947221e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>17 + 1.2971240282058716e-01 + 1.8989261627197266e+01 + <_> + 2 + 510 + 4.2523581015947221e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 2223 + -4.2523581015947221e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 6374 + 4.2523581015947221e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 4.7502306103706360e-01 + 3.3739532470703125e+02 + <_> + 2 + 5052 + 4.2523581015947221e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 1322 + -4.2523581015947221e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + -4.4396796029142338e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 5.6383717060089111e-01 + 2.1553173828125000e+03 + <_> + 1 + 4230 + -4.4396796029142338e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>18 + 3.0161643028259277e-01 + 1.1237963867187500e+02 + <_> + 2 + 1955 + 4.4396796029142338e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 2275 + -4.4396796029142338e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 4877 + 4.4396796029142338e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>14 + 3.0758768320083618e-01 + 1.0778822021484375e+03 + <_> + 2 + 1178 + -4.4396796029142338e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 3699 + 4.4396796029142338e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + 3.2703041076196399e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>26 + 5.4366081953048706e-01 + 4.1280046875000000e+04 + <_> + 1 + 5095 + 3.2703041076196399e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 2.0655247569084167e-01 + 7.2365472412109375e+02 + <_> + 2 + 227 + -3.2703041076196399e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 4868 + 3.2703041076196399e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 4012 + -3.2703041076196399e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>23 + 3.7448418140411377e-01 + 9.8871871829032898e-02 + <_> + 2 + 1578 + 3.2703041076196399e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 2434 + -3.2703041076196399e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + -3.2463155736563098e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>23 + 5.5335444211959839e-01 + 1.1453289538621902e-01 + <_> + 1 + 4868 + -3.2463155736563098e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>19 + 3.5848903656005859e-01 + 1.4433314514160156e+02 + <_> + 2 + 4406 + -3.2463155736563098e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 462 + 3.2463155736563098e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 4239 + 3.2463155736563098e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>26 + 2.2196353971958160e-01 + 2.3820234375000000e+05 + <_> + 2 + 4005 + 3.2463155736563098e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 234 + -3.2463155736563098e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + 3.1793392019096822e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>27 + 5.4508721828460693e-01 + 2.9112878799438477e+01 + <_> + 1 + 7626 + 3.1793392019096822e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>25 + 4.1716161370277405e-01 + 1.4650000000000000e+02 + <_> + 2 + 5636 + 3.1793392019096822e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 1990 + -3.1793392019096822e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 1481 + -3.1793392019096822e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>24 + 1.6165904700756073e-01 + 170. + <_> + 2 + 548 + -3.1793392019096822e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 933 + 3.1793392019096822e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + -1 + + <_> + 0 + 9107 + -3.4141136896530616e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>25 + 5.5981808900833130e-01 + 1.7950000000000000e+02 + <_> + 1 + 7241 + -3.4141136896530616e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>12 + 4.6778613328933716e-01 + 1.5557489257812500e+04 + <_> + 2 + 7168 + -3.4141136896530616e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 73 + 3.4141136896530616e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 1 + 1866 + 3.4141136896530616e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + + <_>26 + 1.1674719303846359e-01 + 7.4591191406250000e+03 + <_> + 2 + 157 + -3.4141136896530616e-01 + 0 + 0 + 0 + 0. + 0. + 0. + 0. + <_> + 2 + 1709 + 3.4141136896530616e-01 + 1 + 0 + 0 + 0. + 0. + 0. + 0. + diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/test_maps/160_160.png b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/test_maps/160_160.png new file mode 100644 index 0000000..4c1dfbe Binary files /dev/null and b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/test_maps/160_160.png differ diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/test_maps/250_250.png b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/test_maps/250_250.png new file mode 100644 index 0000000..5dbb712 Binary files /dev/null and b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/test_maps/250_250.png differ diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/test_maps/300_300.png b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/test_maps/300_300.png new file mode 100644 index 0000000..9116421 Binary files /dev/null and b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/test_maps/300_300.png differ diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/test_maps/400_400.png b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/test_maps/400_400.png new file mode 100644 index 0000000..dd6b454 Binary files /dev/null and b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/test_maps/400_400.png differ diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/test_maps/500_500.png b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/test_maps/500_500.png new file mode 100644 index 0000000..7dff090 Binary files /dev/null and b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/test_maps/500_500.png differ diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/test_maps/a.png b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/test_maps/a.png new file mode 100644 index 0000000..9f5d9e0 Binary files /dev/null and b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/test_maps/a.png differ diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/test_maps/blank.png b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/test_maps/blank.png new file mode 100644 index 0000000..e78f392 Binary files /dev/null and b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/test_maps/blank.png differ diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/test_maps/quellen.txt b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/test_maps/quellen.txt new file mode 100644 index 0000000..b6ec354 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/test_maps/quellen.txt @@ -0,0 +1,8 @@ +radish + +http://www.google.de/imgres?imgurl=http%3A%2F%2Frobotang.co.nz%2Fassets%2FUploads%2Frobots%2Fplayer-plugins%2F_resampled%2Fgmapping.png&imgrefurl=http%3A%2F%2Frobotang.co.nz%2Fprojects%2Frobotics%2Fcustom-player-plugins%2F&h=433&w=541&tbnid=VFJNeto0bLSuqM%3A&zoom=1&docid=XBYXusUqjIUDAM&ei=HZZ1VbeoJaTLyAPNk4CYBg&tbm=isch&client=ubuntu&iact=rc&uact=3&dur=221&page=1&start=0&ndsp=38&ved=0CDAQrQMwBQ + +http://www.google.de/imgres?imgurl=http%3A%2F%2Fanswers.opencv.org%2Fupfiles%2F14146035044346141.jpg&imgrefurl=http%3A%2F%2Fanswers.opencv.org%2Fquestion%2F45891%2Fkeypoint-matching-on-occupancy-grid-maps%2F&h=317&w=875&tbnid=T5zLu9yaL0R2-M%3A&zoom=1&docid=iTnCfNd-Jk67dM&ei=n5Z1Vcz0K-GtygO6koL4Dw&tbm=isch&client=ubuntu&iact=rc&uact=3&dur=232&page=7&start=304&ndsp=51&ved=0CDUQrQMwEDisAg + + + diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/test_maps/test1.png b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/test_maps/test1.png new file mode 100644 index 0000000..96325bb Binary files /dev/null and b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/test_maps/test1.png differ diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/Freiburg52_scan_furnitures_hallway_training.png b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/Freiburg52_scan_furnitures_hallway_training.png new file mode 100644 index 0000000..cf2e2c9 Binary files /dev/null and b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/Freiburg52_scan_furnitures_hallway_training.png differ diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/Freiburg52_scan_furnitures_room_training.png b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/Freiburg52_scan_furnitures_room_training.png new file mode 100644 index 0000000..45e6b40 Binary files /dev/null and b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/Freiburg52_scan_furnitures_room_training.png differ diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/Freiburg52_scan_hallway_training.png b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/Freiburg52_scan_hallway_training.png new file mode 100644 index 0000000..4414126 Binary files /dev/null and b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/Freiburg52_scan_hallway_training.png differ diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/Freiburg52_scan_room_training.png b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/Freiburg52_scan_room_training.png new file mode 100644 index 0000000..0c67c1e Binary files /dev/null and b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/Freiburg52_scan_room_training.png differ diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/lab_a_hallway_training_map.png b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/lab_a_hallway_training_map.png new file mode 100644 index 0000000..4ea800d Binary files /dev/null and b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/lab_a_hallway_training_map.png differ diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/lab_d_room_training_map.png b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/lab_d_room_training_map.png new file mode 100644 index 0000000..307421c Binary files /dev/null and b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/lab_d_room_training_map.png differ diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/lab_intel_furnitures_room_training_map.png b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/lab_intel_furnitures_room_training_map.png new file mode 100644 index 0000000..0b5f4e6 Binary files /dev/null and b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/lab_intel_furnitures_room_training_map.png differ diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/lab_intel_hallway_training_map.png b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/lab_intel_hallway_training_map.png new file mode 100644 index 0000000..27b2b95 Binary files /dev/null and b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/lab_intel_hallway_training_map.png differ diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/lab_ipa_hallway_training_map.png b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/lab_ipa_hallway_training_map.png new file mode 100644 index 0000000..eb1ac68 Binary files /dev/null and b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/lab_ipa_hallway_training_map.png differ diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/lab_ipa_room_training_map.png b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/lab_ipa_room_training_map.png new file mode 100644 index 0000000..111f95b Binary files /dev/null and b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/lab_ipa_room_training_map.png differ diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/original_maps/Fr101_original.png b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/original_maps/Fr101_original.png new file mode 100644 index 0000000..fd57558 Binary files /dev/null and b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/original_maps/Fr101_original.png differ diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/original_maps/Fr52_original.png b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/original_maps/Fr52_original.png new file mode 100644 index 0000000..dd3570d Binary files /dev/null and b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/original_maps/Fr52_original.png differ diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/original_maps/NLB_original.png b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/original_maps/NLB_original.png new file mode 100644 index 0000000..eb16707 Binary files /dev/null and b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/original_maps/NLB_original.png differ diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/original_maps/lab_c_furnitures_original.png b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/original_maps/lab_c_furnitures_original.png new file mode 100644 index 0000000..42af732 Binary files /dev/null and b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/original_maps/lab_c_furnitures_original.png differ diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/original_maps/lab_d_furnitures_original.png b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/original_maps/lab_d_furnitures_original.png new file mode 100644 index 0000000..22828b4 Binary files /dev/null and b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/original_maps/lab_d_furnitures_original.png differ diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/original_maps/lab_intel_original.png b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/original_maps/lab_intel_original.png new file mode 100644 index 0000000..0df7cc0 Binary files /dev/null and b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/original_maps/lab_intel_original.png differ diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/original_maps/lab_ipa_original.png b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/original_maps/lab_ipa_original.png new file mode 100644 index 0000000..00835a1 Binary files /dev/null and b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/original_maps/lab_ipa_original.png differ diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/original_maps/office_e_original.png b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/original_maps/office_e_original.png new file mode 100644 index 0000000..7d811c1 Binary files /dev/null and b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/original_maps/office_e_original.png differ diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/original_maps/office_h_original.png b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/original_maps/office_h_original.png new file mode 100644 index 0000000..a4e5fed Binary files /dev/null and b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/original_maps/office_h_original.png differ diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/training_maps/training_Fr101.png b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/training_maps/training_Fr101.png new file mode 100644 index 0000000..5463c1b Binary files /dev/null and b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/training_maps/training_Fr101.png differ diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/training_maps/training_Fr52.png b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/training_maps/training_Fr52.png new file mode 100644 index 0000000..e8b55e4 Binary files /dev/null and b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/training_maps/training_Fr52.png differ diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/training_maps/training_NLB_furniture.png b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/training_maps/training_NLB_furniture.png new file mode 100644 index 0000000..fb33120 Binary files /dev/null and b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/training_maps/training_NLB_furniture.png differ diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/training_maps/training_intel.png b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/training_maps/training_intel.png new file mode 100644 index 0000000..b6f0152 Binary files /dev/null and b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/training_maps/training_intel.png differ diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/training_maps/training_lab_c_furnitures.png b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/training_maps/training_lab_c_furnitures.png new file mode 100644 index 0000000..ca347e7 Binary files /dev/null and b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/training_maps/training_lab_c_furnitures.png differ diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/training_maps/training_lab_d_furniture.png b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/training_maps/training_lab_d_furniture.png new file mode 100644 index 0000000..e27ed8f Binary files /dev/null and b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/training_maps/training_lab_d_furniture.png differ diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/training_maps/training_lab_ipa.png b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/training_maps/training_lab_ipa.png new file mode 100644 index 0000000..5d21450 Binary files /dev/null and b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/training_maps/training_lab_ipa.png differ diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/training_maps/training_office_e.png b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/training_maps/training_office_e.png new file mode 100644 index 0000000..237a947 Binary files /dev/null and b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/training_maps/training_office_e.png differ diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/training_maps/training_office_h.png b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/training_maps/training_office_h.png new file mode 100644 index 0000000..36636b1 Binary files /dev/null and b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/training_maps/training_office_h.png differ diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/voronoi_maps/Fr101_voronoi.png b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/voronoi_maps/Fr101_voronoi.png new file mode 100644 index 0000000..6771c9f Binary files /dev/null and b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/voronoi_maps/Fr101_voronoi.png differ diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/voronoi_maps/Fr52_voronoi.png b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/voronoi_maps/Fr52_voronoi.png new file mode 100644 index 0000000..d60ef49 Binary files /dev/null and b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/voronoi_maps/Fr52_voronoi.png differ diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/voronoi_maps/NLB_voronoi.png b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/voronoi_maps/NLB_voronoi.png new file mode 100644 index 0000000..5522fe5 Binary files /dev/null and b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/voronoi_maps/NLB_voronoi.png differ diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/voronoi_maps/lab_c_furnitures_voronoi.png b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/voronoi_maps/lab_c_furnitures_voronoi.png new file mode 100644 index 0000000..74411cc Binary files /dev/null and b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/voronoi_maps/lab_c_furnitures_voronoi.png differ diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/voronoi_maps/lab_d_furnitures_voronoi.png b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/voronoi_maps/lab_d_furnitures_voronoi.png new file mode 100644 index 0000000..4b36fdc Binary files /dev/null and b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/voronoi_maps/lab_d_furnitures_voronoi.png differ diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/voronoi_maps/lab_intel_voronoi.png b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/voronoi_maps/lab_intel_voronoi.png new file mode 100644 index 0000000..9095d83 Binary files /dev/null and b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/voronoi_maps/lab_intel_voronoi.png differ diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/voronoi_maps/lab_ipa_voronoi.png b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/voronoi_maps/lab_ipa_voronoi.png new file mode 100644 index 0000000..bd84baf Binary files /dev/null and b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/voronoi_maps/lab_ipa_voronoi.png differ diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/voronoi_maps/office_e_voronoi.png b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/voronoi_maps/office_e_voronoi.png new file mode 100644 index 0000000..ad58cd9 Binary files /dev/null and b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/voronoi_maps/office_e_voronoi.png differ diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/voronoi_maps/office_h_voronoi.png b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/voronoi_maps/office_h_voronoi.png new file mode 100644 index 0000000..40fc288 Binary files /dev/null and b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/voronoi_maps/office_h_voronoi.png differ diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/voronoi_node_maps/Fr101_voronoi_nodes.png b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/voronoi_node_maps/Fr101_voronoi_nodes.png new file mode 100644 index 0000000..45370f0 Binary files /dev/null and b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/voronoi_node_maps/Fr101_voronoi_nodes.png differ diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/voronoi_node_maps/Fr52_voronoi_nodes.png b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/voronoi_node_maps/Fr52_voronoi_nodes.png new file mode 100644 index 0000000..c58ad9e Binary files /dev/null and b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/voronoi_node_maps/Fr52_voronoi_nodes.png differ diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/voronoi_node_maps/NLB_voronoi_nodes.png b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/voronoi_node_maps/NLB_voronoi_nodes.png new file mode 100644 index 0000000..3c24f1f Binary files /dev/null and b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/voronoi_node_maps/NLB_voronoi_nodes.png differ diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/voronoi_node_maps/lab_c_furnitures_voronoi_nodes.png b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/voronoi_node_maps/lab_c_furnitures_voronoi_nodes.png new file mode 100644 index 0000000..74dc5b4 Binary files /dev/null and b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/voronoi_node_maps/lab_c_furnitures_voronoi_nodes.png differ diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/voronoi_node_maps/lab_d_furnitures_voronoi_nodes.png b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/voronoi_node_maps/lab_d_furnitures_voronoi_nodes.png new file mode 100644 index 0000000..f2231d9 Binary files /dev/null and b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/voronoi_node_maps/lab_d_furnitures_voronoi_nodes.png differ diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/voronoi_node_maps/lab_intel_voronoi_nodes.png b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/voronoi_node_maps/lab_intel_voronoi_nodes.png new file mode 100644 index 0000000..ae2e974 Binary files /dev/null and b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/voronoi_node_maps/lab_intel_voronoi_nodes.png differ diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/voronoi_node_maps/lab_ipa_voronoi_nodes.png b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/voronoi_node_maps/lab_ipa_voronoi_nodes.png new file mode 100644 index 0000000..a48ddf2 Binary files /dev/null and b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/voronoi_node_maps/lab_ipa_voronoi_nodes.png differ diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/voronoi_node_maps/office_e_voronoi_nodes.png b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/voronoi_node_maps/office_e_voronoi_nodes.png new file mode 100644 index 0000000..55e3e63 Binary files /dev/null and b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/voronoi_node_maps/office_e_voronoi_nodes.png differ diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/voronoi_node_maps/office_h_voronoi_nodes.png b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/voronoi_node_maps/office_h_voronoi_nodes.png new file mode 100644 index 0000000..2599b31 Binary files /dev/null and b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/files/training_maps/voronoi_random_field_training/voronoi_node_maps/office_h_voronoi_nodes.png differ diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/include/ipa_room_segmentation/abstract_voronoi_segmentation.h b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/include/ipa_room_segmentation/abstract_voronoi_segmentation.h new file mode 100644 index 0000000..0365094 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/include/ipa_room_segmentation/abstract_voronoi_segmentation.h @@ -0,0 +1,84 @@ +#pragma once + +#include "ros/ros.h" +#include +#include +#include +#include +#include +#include + +#include + +#include + +#define PI 3.14159265 + +// Struct that compares two given points and returns if the y-coordinate of the first is smaller or if they are equal if the +// x-coordinate of the first is smaller. This is used for sets to easily store cv::Point objects and search for specific objects. +struct cv_Point_comp +{ + bool operator()(const cv::Point& lhs, const cv::Point& rhs) const + { + return ((lhs.y < rhs.y) || (lhs.y == rhs.y && lhs.x < rhs.x)); + } +}; + +class AbstractVoronoiSegmentation +{ +protected: + + // Function to get the ID of a room, when given an index in the storing vector. + // This function takes a vector of rooms and an index in this vector and returns the ID of this room, meaning the color it has + // been drawn in the map. + bool determineRoomIndexFromRoomID(const std::vector& rooms, const int room_id, size_t& room_index); + + // Function to merge two rooms together on the given already segmented map. + // This function is used to merge two rooms together in the given already segmented map. It takes two indexes, showing which + // room is the bigger one and which is the smaller one. The smaller one should be merged together with the bigger one, what is + // done by changing the color of this room in the map to the color of the bigger room and inserting the members into the bigger + // room. + void mergeRoomPair(std::vector& rooms, const int target_index, const int room_to_merge_index, cv::Mat& segmented_map, const double map_resolution); + + // Function to draw the generalized voronoi-diagram into a given map, not drawing lines that start or end at black pixels + // This function draws the Voronoi-diagram into a given map. It needs the facets as vector of Points, the contour of the + // map and the contours of the holes. It checks if the endpoints of the facets are both inside the map-contour and not + // inside a hole-contour and doesn't draw the lines that are not. + // Function to draw the approximated voronoi graph into a given map. It doesn't draw lines of the graph that start or end + // in a black region. This is necessary because the voronoi graph gets approximated by diskretizing the maps contour and + // using these points as centers for the graph. It gets wrong lines, that are eliminated in this function. See the .cpp + // files for further information. + void drawVoronoi(cv::Mat &img, const std::vector >& facets_of_voronoi, const cv::Scalar voronoi_color, const cv::Mat& eroded_map); + + // Function to get the voronoi-diagram drawn into the map + // This function is here to create the generalized voronoi-graph in the given map. It does following steps: + // 1. It finds every discretized contour in the given map (they are saved as vector). Then it takes these + // contour-Points and adds them to the OpenCV Delaunay generator from which the voronoi-cells can be generated. + // 2. Finally it gets the boundary-Points of the voronoi-cells with getVoronoiFacetList. It takes these facets + // and draws them using the drawVoronoi function. This function draws the facets that only have Points inside + // the map-contour (other lines go to not-reachable places and are not necessary to be looked at). + // 3. It returns the map that has the generalized voronoi-graph drawn in. + void createVoronoiGraph(cv::Mat& map_for_voronoi_generation); + + // This function prunes the generalized Voronoi-graph in the given map. + // It reduces the graph down to the nodes in the graph. A node is a point on the Voronoi graph, that has at least 3 + // neighbors. This deletes errors from the approximate generation of the graph that hasn't been eliminated from + // the drawVoronoi function. the resulting graph is the pruned generalized voronoi graph. + // It does following steps: + // 1. Extract node-points of the Voronoi-Diagram, which have at least 3 neighbors. + // 2. Reduce the leave-nodes (Point on graph with only one neighbor) of the graph until the reduction + // hits a node-Point. This is done to reduce the lines along the real voronoi-graph, coming from the discretisation + // of the contour. + // 3. It returns the map that has the pruned generalized voronoi-graph drawn in. + void pruneVoronoiGraph(cv::Mat& voronoi_map, std::set& node_points); + + // Function to merge rooms together + // Function that goes trough each given room and checks if it should be merged together wit another bigger room, if it is too small. + // This function takes the segmented Map from the original Voronoi-segmentation-algorithm and merges rooms together, + // that are small enough and have only two or one neighbor. + void mergeRooms(cv::Mat& map_to_merge_rooms, std::vector& rooms, double map_resolution_from_subscription, double max_area_for_merging, bool display_map); + +public: + + AbstractVoronoiSegmentation(); +}; diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/include/ipa_room_segmentation/adaboost_classifier.h b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/include/ipa_room_segmentation/adaboost_classifier.h new file mode 100644 index 0000000..c9bb640 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/include/ipa_room_segmentation/adaboost_classifier.h @@ -0,0 +1,49 @@ +#pragma once +#include "ros/ros.h" +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +class AdaboostClassifier +{ +protected: + + bool trained_; // variable that shows if the classifiers has already been trained + + std::vector angles_for_simulation_; // angle-vector used to calculate the features for this algorithm + +#if CV_MAJOR_VERSION == 2 + CvBoostParams params_; // Parameters for the classifiers + CvBoost hallway_boost_, room_boost_; // the AdaBoost-classifiers for rooms and hallways +#else + cv::Ptr hallway_boost_, room_boost_; // the AdaBoost-classifiers for rooms and hallways +#endif + + LaserScannerRaycasting raycasting_; + +public: + + + AdaboostClassifier(); + + + //training-method for the classifier + void trainClassifiers(const std::vector& room_training_maps, const std::vector& hallway_training_maps, + const std::string& classifier_storage_path, bool load_features=false); + + + //labeling-algorithm after the training + void segmentMap(const cv::Mat& map_to_be_labeled, cv::Mat& segmented_map, double map_resolution_from_subscription, + double room_area_factor_lower_limit, double room_area_factor_upper_limit, + const std::string& classifier_storage_path, const std::string& classifier_default_path, bool display_results=false); +}; diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/include/ipa_room_segmentation/clique_class.h b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/include/ipa_room_segmentation/clique_class.h new file mode 100644 index 0000000..e7ea5ff --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/include/ipa_room_segmentation/clique_class.h @@ -0,0 +1,113 @@ +/*! + ***************************************************************** + * \file + * + * \note + * Copyright (c) 2015 \n + * Fraunhofer Institute for Manufacturing Engineering + * and Automation (IPA) \n\n + * + ***************************************************************** + * + * \note + * Project name: Care-O-bot + * \note + * ROS stack name: autopnp + * \note + * ROS package name: ipa_room_segmentation + * + * \author + * Author: Florian Jordan + * \author + * Supervised by: Richard Bormann + * + * \date Date of creation: 11.2015 + * + * \brief + * + * + ***************************************************************** + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * - Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. \n + * - Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. \n + * - Neither the name of the Fraunhofer Institute for Manufacturing + * Engineering and Automation (IPA) nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. \n + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License LGPL as + * published by the Free Software Foundation, either version 3 of the + * License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License LGPL for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License LGPL along with this program. + * If not, see . + * + ****************************************************************/ +// +// This class is used to easily define cliques from a graph. A Clique is a subgraph of a given graph, in which all Points are +// connected to each other. So this class has a element member_Points that stores all Points that are part of this clique. +// Also it has different functions: +// 1. getMemberPoints(): This function returns a vector with all members in it. +// 2. insertMember(cv::Point/vector labels, vector boost_classifiers): +// This function is used for the Voronoi Random Field Segmentation. It gets a list of AdaBoost classifiers +// ( from OpenCV ) and calculates the feature for this clique, depending on the given labels for each Point. +// The labels show to which class each Point belongs and are describing the position of the AdaBoost classifier +// in the given vector. It then calculates Phi(y_k, x) = exp(w_k^T * f_k(y_k, x)). Phi is the clique potential +// needed for the conditional random field, w_k is the weight-vector, defined for each classifier and put together +// for each point here and f_k is the feature vector, that calculates individual features for each point to label +// them as a specific class. y_k are the member points of a clique and x is the hidden node of the conditional +// random field, in this case the given gridmap. +// + +#include +#include +#include + +#pragma once + +class Clique +{ +protected: + std::vector member_points_; // vector that stores the member points of the clique + + std::vector< std::vector > beams_for_members_; // vector that stores the simulated beams for each member (simulated using raycasting) + +public: + + Clique(); // default constructor + + Clique(cv::Point first_member); // constructor if one node is known + + Clique(std::vector members); // constructor if more than one member point is known + + std::vector getMemberPoints(); // function that returns a vector with all member points stored in + + void insertMember(cv::Point& new_member); // function that inserts a member if it isn't already a member + + void insertMember(std::vector& new_members); // function that inserts multiple members, if they are not already members + + bool containsMember(const cv::Point& point); // function that checks if a given point is part of this clique + + unsigned int getNumberOfMembers(); // function that returns the number of members stored in this clique + + void setBeamsForMembers(const std::vector< std::vector > beams); // function that stores the given beams in the class parameter + + std::vector< std::vector > getBeams(); // function that returns the stored laser-beams for the member points + +}; diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/include/ipa_room_segmentation/contains.h b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/include/ipa_room_segmentation/contains.h new file mode 100644 index 0000000..5f6468c --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/include/ipa_room_segmentation/contains.h @@ -0,0 +1,13 @@ +#include "ros/ros.h" +#include +#include +#include +#include +#include +#include +#include + +bool contains(std::vector vector, cv::Scalar element); +bool contains(std::vector vector, cv::Point element); +bool contains(std::vector vector, int element); +bool contains(std::vector > vector, std::vector element); diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/include/ipa_room_segmentation/cv_boost_loader.h b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/include/ipa_room_segmentation/cv_boost_loader.h new file mode 100644 index 0000000..64d3b95 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/include/ipa_room_segmentation/cv_boost_loader.h @@ -0,0 +1,8 @@ +#pragma once +#include + +#if CV_MAJOR_VERSION == 2 +void loadBoost(CvBoost& boost, std::string const& filename); +#else +void loadBoost(cv::Ptr& boost, std::string const& filename); +#endif diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/include/ipa_room_segmentation/distance_segmentation.h b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/include/ipa_room_segmentation/distance_segmentation.h new file mode 100644 index 0000000..c99fc12 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/include/ipa_room_segmentation/distance_segmentation.h @@ -0,0 +1,19 @@ +#include "ros/ros.h" +#include +#include +#include +#include +#include +#include + +#include + +class DistanceSegmentation +{ +public: + + DistanceSegmentation(); + + //algorithm to segment the map + void segmentMap(const cv::Mat& map_to_be_labeled, cv::Mat& segmented_map, double map_resolution_from_subscription, double room_area_factor_lower_limit, double room_area_factor_upper_limit); +}; diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/include/ipa_room_segmentation/evaluation_segmentation.h b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/include/ipa_room_segmentation/evaluation_segmentation.h new file mode 100644 index 0000000..6568413 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/include/ipa_room_segmentation/evaluation_segmentation.h @@ -0,0 +1,40 @@ + +#include +#include +#include +#include +#include +#include + +#include + + +class EvaluationSegmentation +{ +public: + + struct cv_Point_comp + { + bool operator()(const cv::Point& lhs, const cv::Point& rhs) const + { + return ((lhs.y < rhs.y) || (lhs.y == rhs.y && lhs.x < rhs.x)); + } + }; + + typedef std::set PointSet; + typedef std::vector< PointSet > VectorOfPointSets; + + // gt_map = ground truth gray scale map (>250 = free space), each room is a closed contour (rooms are separated by drawn in borders), no colors + // gt_map_color = colored ground truth map (maybe computed by or provided to the function depending on value of compute_gt_map_color) + // segmented_map in CV_32SC1 + void computePrecisionRecall(const cv::Mat& gt_map, cv::Mat& gt_map_color, const cv::Mat& segmented_map, + double& precision_micro, double& precision_macro, double& recall_micro, double& recall_macro, bool compute_gt_map_color=true); + +private: + // fills each closed area (= ground truth room) with a unique id and collects all room points in gt[room id][pixel index] + void groundTruthVectorCalculation(const cv::Mat &bw_map, VectorOfPointSets& gt); + +// void Segmentation_Vector_calculation(const cv::Mat &segmented_map, std::map &segmented_room_mapping); +// void Recall_Precision_Calculation(std::map &results, std::vector gt_mat_vector, std::vector segmentation_mat_vector); + +}; diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/include/ipa_room_segmentation/fast_math.h b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/include/ipa_room_segmentation/fast_math.h new file mode 100644 index 0000000..61cfc9b --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/include/ipa_room_segmentation/fast_math.h @@ -0,0 +1,1608 @@ +/*=====================================================================* + * Copyright (C) 2012 Paul Mineiro * + * All rights reserved. * + * * + * Redistribution and use in source and binary forms, with * + * or without modification, are permitted provided that the * + * following conditions are met: * + * * + * * Redistributions of source code must retain the * + * above copyright notice, this list of conditions and * + * the following disclaimer. * + * * + * * Redistributions in binary form must reproduce the * + * above copyright notice, this list of conditions and * + * the following disclaimer in the documentation and/or * + * other materials provided with the distribution. * + * * + * * Neither the name of Paul Mineiro nor the names * + * of other contributors may be used to endorse or promote * + * products derived from this software without specific * + * prior written permission. * + * * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND * + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, * + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES * + * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER * + * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE * + * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR * + * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF * + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * + * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * + * POSSIBILITY OF SUCH DAMAGE. * + * * + * Contact: Paul Mineiro * + *=====================================================================*/ + +#ifndef __CAST_H_ + +#ifdef __cplusplus +#define cast_uint32_t static_cast +#else +#define cast_uint32_t (uint32_t) +#endif + +#endif // __CAST_H_ +/*=====================================================================* + * Copyright (C) 2011 Paul Mineiro * + * All rights reserved. * + * * + * Redistribution and use in source and binary forms, with * + * or without modification, are permitted provided that the * + * following conditions are met: * + * * + * * Redistributions of source code must retain the * + * above copyright notice, this list of conditions and * + * the following disclaimer. * + * * + * * Redistributions in binary form must reproduce the * + * above copyright notice, this list of conditions and * + * the following disclaimer in the documentation and/or * + * other materials provided with the distribution. * + * * + * * Neither the name of Paul Mineiro nor the names * + * of other contributors may be used to endorse or promote * + * products derived from this software without specific * + * prior written permission. * + * * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND * + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, * + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES * + * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER * + * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE * + * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR * + * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF * + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * + * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * + * POSSIBILITY OF SUCH DAMAGE. * + * * + * Contact: Paul Mineiro * + *=====================================================================*/ + +#ifndef __SSE_H_ +#define __SSE_H_ + +#ifdef __SSE2__ + +#include + +#ifdef __cplusplus +namespace { +#endif // __cplusplus + +typedef __m128 v4sf; +typedef __m128i v4si; + +#define v4si_to_v4sf _mm_cvtepi32_ps +#define v4sf_to_v4si _mm_cvttps_epi32 + +#define v4sfl(x) ((const v4sf) { (x), (x), (x), (x) }) +#define v2dil(x) ((const v4si) { (x), (x) }) +#define v4sil(x) v2dil((((long long) (x)) << 32) | (x)) + +typedef union { v4sf f; float array[4]; } v4sfindexer; +#define v4sf_index(_findx, _findi) \ + ({ \ + v4sfindexer _findvx = { _findx } ; \ + _findvx.array[_findi]; \ + }) +typedef union { v4si i; int array[4]; } v4siindexer; +#define v4si_index(_iindx, _iindi) \ + ({ \ + v4siindexer _iindvx = { _iindx } ; \ + _iindvx.array[_iindi]; \ + }) + +typedef union { v4sf f; v4si i; } v4sfv4sipun; +#define v4sf_fabs(x) \ + ({ \ + v4sfv4sipun vx; \ + vx.f = x; \ + vx.i &= v4sil (0x7FFFFFFF); \ + vx.f; \ + }) + +#ifdef __cplusplus +} // end namespace +#endif // __cplusplus + +#endif // __SSE2__ + +#endif // __SSE_H_ +/*=====================================================================* + * Copyright (C) 2011 Paul Mineiro * + * All rights reserved. * + * * + * Redistribution and use in source and binary forms, with * + * or without modification, are permitted provided that the * + * following conditions are met: * + * * + * * Redistributions of source code must retain the * + * above copyright notice, this list of conditions and * + * the following disclaimer. * + * * + * * Redistributions in binary form must reproduce the * + * above copyright notice, this list of conditions and * + * the following disclaimer in the documentation and/or * + * other materials provided with the distribution. * + * * + * * Neither the name of Paul Mineiro nor the names * + * of other contributors may be used to endorse or promote * + * products derived from this software without specific * + * prior written permission. * + * * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND * + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, * + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES * + * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER * + * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE * + * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR * + * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF * + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * + * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * + * POSSIBILITY OF SUCH DAMAGE. * + * * + * Contact: Paul Mineiro * + *=====================================================================*/ + +#ifndef __FAST_EXP_H_ +#define __FAST_EXP_H_ + +#include + +// Underflow of exponential is common practice in numerical routines, +// so handle it here. + +static inline float +fastpow2 (float p) +{ + float offset = (p < 0) ? 1.0f : 0.0f; + float clipp = (p < -126) ? -126.0f : p; + int w = clipp; + float z = clipp - w + offset; + union { uint32_t i; float f; } v = { cast_uint32_t ( (1 << 23) * (clipp + 121.2740575f + 27.7280233f / (4.84252568f - z) - 1.49012907f * z) ) }; + + return v.f; +} + +static inline float +fastexp (float p) +{ + return fastpow2 (1.442695040f * p); +} + +static inline float +fasterpow2 (float p) +{ + float clipp = (p < -126) ? -126.0f : p; + union { uint32_t i; float f; } v = { cast_uint32_t ( (1 << 23) * (clipp + 126.94269504f) ) }; + return v.f; +} + +static inline float +fasterexp (float p) +{ + return fasterpow2 (1.442695040f * p); +} + +#ifdef __SSE2__ + +static inline v4sf +vfastpow2 (const v4sf p) +{ + v4sf ltzero = _mm_cmplt_ps (p, v4sfl (0.0f)); + v4sf offset = _mm_and_ps (ltzero, v4sfl (1.0f)); + v4sf lt126 = _mm_cmplt_ps (p, v4sfl (-126.0f)); + v4sf clipp = _mm_or_ps (_mm_andnot_ps (lt126, p), _mm_and_ps (lt126, v4sfl (-126.0f))); + v4si w = v4sf_to_v4si (clipp); + v4sf z = clipp - v4si_to_v4sf (w) + offset; + + const v4sf c_121_2740838 = v4sfl (121.2740575f); + const v4sf c_27_7280233 = v4sfl (27.7280233f); + const v4sf c_4_84252568 = v4sfl (4.84252568f); + const v4sf c_1_49012907 = v4sfl (1.49012907f); + union { v4si i; v4sf f; } v = { + v4sf_to_v4si ( + v4sfl (1 << 23) * + (clipp + c_121_2740838 + c_27_7280233 / (c_4_84252568 - z) - c_1_49012907 * z) + ) + }; + + return v.f; +} + +static inline v4sf +vfastexp (const v4sf p) +{ + const v4sf c_invlog_2 = v4sfl (1.442695040f); + + return vfastpow2 (c_invlog_2 * p); +} + +static inline v4sf +vfasterpow2 (const v4sf p) +{ + const v4sf c_126_94269504 = v4sfl (126.94269504f); + v4sf lt126 = _mm_cmplt_ps (p, v4sfl (-126.0f)); + v4sf clipp = _mm_or_ps (_mm_andnot_ps (lt126, p), _mm_and_ps (lt126, v4sfl (-126.0f))); + union { v4si i; v4sf f; } v = { v4sf_to_v4si (v4sfl (1 << 23) * (clipp + c_126_94269504)) }; + return v.f; +} + +static inline v4sf +vfasterexp (const v4sf p) +{ + const v4sf c_invlog_2 = v4sfl (1.442695040f); + + return vfasterpow2 (c_invlog_2 * p); +} + +#endif //__SSE2__ + +#endif // __FAST_EXP_H_ +/*=====================================================================* + * Copyright (C) 2011 Paul Mineiro * + * All rights reserved. * + * * + * Redistribution and use in source and binary forms, with * + * or without modification, are permitted provided that the * + * following conditions are met: * + * * + * * Redistributions of source code must retain the * + * above copyright notice, this list of conditions and * + * the following disclaimer. * + * * + * * Redistributions in binary form must reproduce the * + * above copyright notice, this list of conditions and * + * the following disclaimer in the documentation and/or * + * other materials provided with the distribution. * + * * + * * Neither the name of Paul Mineiro nor the names * + * of other contributors may be used to endorse or promote * + * products derived from this software without specific * + * prior written permission. * + * * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND * + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, * + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES * + * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER * + * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE * + * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR * + * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF * + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * + * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * + * POSSIBILITY OF SUCH DAMAGE. * + * * + * Contact: Paul Mineiro * + *=====================================================================*/ + +#ifndef __FAST_LOG_H_ +#define __FAST_LOG_H_ + +#include + +static inline float +fastlog2 (float x) +{ + union { float f; uint32_t i; } vx = { x }; + union { uint32_t i; float f; } mx = { (vx.i & 0x007FFFFF) | 0x3f000000 }; + float y = vx.i; + y *= 1.1920928955078125e-7f; + + return y - 124.22551499f + - 1.498030302f * mx.f + - 1.72587999f / (0.3520887068f + mx.f); +} + +static inline float +fastlog (float x) +{ + return 0.69314718f * fastlog2 (x); +} + +static inline float +fasterlog2 (float x) +{ + union { float f; uint32_t i; } vx = { x }; + float y = vx.i; + y *= 1.1920928955078125e-7f; + return y - 126.94269504f; +} + +static inline float +fasterlog (float x) +{ +// return 0.69314718f * fasterlog2 (x); + + union { float f; uint32_t i; } vx = { x }; + float y = vx.i; + y *= 8.2629582881927490e-8f; + return y - 87.989971088f; +} + +#ifdef __SSE2__ + +static inline v4sf +vfastlog2 (v4sf x) +{ + union { v4sf f; v4si i; } vx = { x }; + union { v4si i; v4sf f; } mx; mx.i = (vx.i & v4sil (0x007FFFFF)) | v4sil (0x3f000000); + v4sf y = v4si_to_v4sf (vx.i); + y *= v4sfl (1.1920928955078125e-7f); + + const v4sf c_124_22551499 = v4sfl (124.22551499f); + const v4sf c_1_498030302 = v4sfl (1.498030302f); + const v4sf c_1_725877999 = v4sfl (1.72587999f); + const v4sf c_0_3520087068 = v4sfl (0.3520887068f); + + return y - c_124_22551499 + - c_1_498030302 * mx.f + - c_1_725877999 / (c_0_3520087068 + mx.f); +} + +static inline v4sf +vfastlog (v4sf x) +{ + const v4sf c_0_69314718 = v4sfl (0.69314718f); + + return c_0_69314718 * vfastlog2 (x); +} + +static inline v4sf +vfasterlog2 (v4sf x) +{ + union { v4sf f; v4si i; } vx = { x }; + v4sf y = v4si_to_v4sf (vx.i); + y *= v4sfl (1.1920928955078125e-7f); + + const v4sf c_126_94269504 = v4sfl (126.94269504f); + + return y - c_126_94269504; +} + +static inline v4sf +vfasterlog (v4sf x) +{ +// const v4sf c_0_69314718 = v4sfl (0.69314718f); +// +// return c_0_69314718 * vfasterlog2 (x); + + union { v4sf f; v4si i; } vx = { x }; + v4sf y = v4si_to_v4sf (vx.i); + y *= v4sfl (8.2629582881927490e-8f); + + const v4sf c_87_989971088 = v4sfl (87.989971088f); + + return y - c_87_989971088; +} + +#endif // __SSE2__ + +#endif // __FAST_LOG_H_ +/*=====================================================================* + * Copyright (C) 2011 Paul Mineiro * + * All rights reserved. * + * * + * Redistribution and use in source and binary forms, with * + * or without modification, are permitted provided that the * + * following conditions are met: * + * * + * * Redistributions of source code must retain the * + * above copyright notice, this list of conditions and * + * the following disclaimer. * + * * + * * Redistributions in binary form must reproduce the * + * above copyright notice, this list of conditions and * + * the following disclaimer in the documentation and/or * + * other materials provided with the distribution. * + * * + * * Neither the name of Paul Mineiro nor the names * + * of other contributors may be used to endorse or promote * + * products derived from this software without specific * + * prior written permission. * + * * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND * + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, * + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES * + * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER * + * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE * + * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR * + * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF * + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * + * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * + * POSSIBILITY OF SUCH DAMAGE. * + * * + * Contact: Paul Mineiro * + *=====================================================================*/ + +#ifndef __FAST_ERF_H_ +#define __FAST_ERF_H_ + +#include +#include + +// fasterfc: not actually faster than erfcf(3) on newer machines! +// ... although vectorized version is interesting +// and fastererfc is very fast + +static inline float +fasterfc (float x) +{ + static const float k = 3.3509633149424609f; + static const float a = 0.07219054755431126f; + static const float b = 15.418191568719577f; + static const float c = 5.609846028328545f; + + union { float f; uint32_t i; } vc = { c * x }; + float xsq = x * x; + float xquad = xsq * xsq; + + vc.i |= 0x80000000; + + return 2.0f / (1.0f + fastpow2 (k * x)) - a * x * (b * xquad - 1.0f) * fasterpow2 (vc.f); +} + +static inline float +fastererfc (float x) +{ + static const float k = 3.3509633149424609f; + + return 2.0f / (1.0f + fasterpow2 (k * x)); +} + +// fasterf: not actually faster than erff(3) on newer machines! +// ... although vectorized version is interesting +// and fastererf is very fast + +static inline float +fasterf (float x) +{ + return 1.0f - fasterfc (x); +} + +static inline float +fastererf (float x) +{ + return 1.0f - fastererfc (x); +} + +static inline float +fastinverseerf (float x) +{ + static const float invk = 0.30004578719350504f; + static const float a = 0.020287853348211326f; + static const float b = 0.07236892874789555f; + static const float c = 0.9913030456864257f; + static const float d = 0.8059775923760193f; + + float xsq = x * x; + + return invk * fastlog2 ((1.0f + x) / (1.0f - x)) + + x * (a - b * xsq) / (c - d * xsq); +} + +static inline float +fasterinverseerf (float x) +{ + static const float invk = 0.30004578719350504f; + + return invk * fasterlog2 ((1.0f + x) / (1.0f - x)); +} + +#ifdef __SSE2__ + +static inline v4sf +vfasterfc (v4sf x) +{ + const v4sf k = v4sfl (3.3509633149424609f); + const v4sf a = v4sfl (0.07219054755431126f); + const v4sf b = v4sfl (15.418191568719577f); + const v4sf c = v4sfl (5.609846028328545f); + + union { v4sf f; v4si i; } vc; vc.f = c * x; + vc.i |= v4sil (0x80000000); + + v4sf xsq = x * x; + v4sf xquad = xsq * xsq; + + return v4sfl (2.0f) / (v4sfl (1.0f) + vfastpow2 (k * x)) - a * x * (b * xquad - v4sfl (1.0f)) * vfasterpow2 (vc.f); +} + +static inline v4sf +vfastererfc (const v4sf x) +{ + const v4sf k = v4sfl (3.3509633149424609f); + + return v4sfl (2.0f) / (v4sfl (1.0f) + vfasterpow2 (k * x)); +} + +static inline v4sf +vfasterf (v4sf x) +{ + return v4sfl (1.0f) - vfasterfc (x); +} + +static inline v4sf +vfastererf (const v4sf x) +{ + return v4sfl (1.0f) - vfastererfc (x); +} + +static inline v4sf +vfastinverseerf (v4sf x) +{ + const v4sf invk = v4sfl (0.30004578719350504f); + const v4sf a = v4sfl (0.020287853348211326f); + const v4sf b = v4sfl (0.07236892874789555f); + const v4sf c = v4sfl (0.9913030456864257f); + const v4sf d = v4sfl (0.8059775923760193f); + + v4sf xsq = x * x; + + return invk * vfastlog2 ((v4sfl (1.0f) + x) / (v4sfl (1.0f) - x)) + + x * (a - b * xsq) / (c - d * xsq); +} + +static inline v4sf +vfasterinverseerf (v4sf x) +{ + const v4sf invk = v4sfl (0.30004578719350504f); + + return invk * vfasterlog2 ((v4sfl (1.0f) + x) / (v4sfl (1.0f) - x)); +} + +#endif //__SSE2__ + +#endif // __FAST_ERF_H_ +/*=====================================================================* + * Copyright (C) 2011 Paul Mineiro * + * All rights reserved. * + * * + * Redistribution and use in source and binary forms, with * + * or without modification, are permitted provided that the * + * following conditions are met: * + * * + * * Redistributions of source code must retain the * + * above copyright notice, this list of conditions and * + * the following disclaimer. * + * * + * * Redistributions in binary form must reproduce the * + * above copyright notice, this list of conditions and * + * the following disclaimer in the documentation and/or * + * other materials provided with the distribution. * + * * + * * Neither the name of Paul Mineiro nor the names * + * of other contributors may be used to endorse or promote * + * products derived from this software without specific * + * prior written permission. * + * * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND * + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, * + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES * + * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER * + * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE * + * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR * + * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF * + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * + * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * + * POSSIBILITY OF SUCH DAMAGE. * + * * + * Contact: Paul Mineiro * + *=====================================================================*/ + +#ifndef __FAST_GAMMA_H_ +#define __FAST_GAMMA_H_ + +#include + +/* gamma/digamma functions only work for positive inputs */ + +static inline float +fastlgamma (float x) +{ + float logterm = fastlog (x * (1.0f + x) * (2.0f + x)); + float xp3 = 3.0f + x; + + return - 2.081061466f + - x + + 0.0833333f / xp3 + - logterm + + (2.5f + x) * fastlog (xp3); +} + +static inline float +fasterlgamma (float x) +{ + return - 0.0810614667f + - x + - fasterlog (x) + + (0.5f + x) * fasterlog (1.0f + x); +} + +static inline float +fastdigamma (float x) +{ + float twopx = 2.0f + x; + float logterm = fastlog (twopx); + + return (-48.0f + x * (-157.0f + x * (-127.0f - 30.0f * x))) / + (12.0f * x * (1.0f + x) * twopx * twopx) + + logterm; +} + +static inline float +fasterdigamma (float x) +{ + float onepx = 1.0f + x; + + return -1.0f / x - 1.0f / (2 * onepx) + fasterlog (onepx); +} + +#ifdef __SSE2__ + +static inline v4sf +vfastlgamma (v4sf x) +{ + const v4sf c_1_0 = v4sfl (1.0f); + const v4sf c_2_0 = v4sfl (2.0f); + const v4sf c_3_0 = v4sfl (3.0f); + const v4sf c_2_081061466 = v4sfl (2.081061466f); + const v4sf c_0_0833333 = v4sfl (0.0833333f); + const v4sf c_2_5 = v4sfl (2.5f); + + v4sf logterm = vfastlog (x * (c_1_0 + x) * (c_2_0 + x)); + v4sf xp3 = c_3_0 + x; + + return - c_2_081061466 + - x + + c_0_0833333 / xp3 + - logterm + + (c_2_5 + x) * vfastlog (xp3); +} + +static inline v4sf +vfasterlgamma (v4sf x) +{ + const v4sf c_0_0810614667 = v4sfl (0.0810614667f); + const v4sf c_0_5 = v4sfl (0.5f); + const v4sf c_1 = v4sfl (1.0f); + + return - c_0_0810614667 + - x + - vfasterlog (x) + + (c_0_5 + x) * vfasterlog (c_1 + x); +} + +static inline v4sf +vfastdigamma (v4sf x) +{ + v4sf twopx = v4sfl (2.0f) + x; + v4sf logterm = vfastlog (twopx); + + return (v4sfl (-48.0f) + x * (v4sfl (-157.0f) + x * (v4sfl (-127.0f) - v4sfl (30.0f) * x))) / + (v4sfl (12.0f) * x * (v4sfl (1.0f) + x) * twopx * twopx) + + logterm; +} + +static inline v4sf +vfasterdigamma (v4sf x) +{ + const v4sf c_1_0 = v4sfl (1.0f); + const v4sf c_2_0 = v4sfl (2.0f); + v4sf onepx = c_1_0 + x; + + return -c_1_0 / x - c_1_0 / (c_2_0 * onepx) + vfasterlog (onepx); +} + +#endif //__SSE2__ + +#endif // __FAST_GAMMA_H_ +/*=====================================================================* + * Copyright (C) 2011 Paul Mineiro * + * All rights reserved. * + * * + * Redistribution and use in source and binary forms, with * + * or without modification, are permitted provided that the * + * following conditions are met: * + * * + * * Redistributions of source code must retain the * + * above copyright notice, this list of conditions and * + * the following disclaimer. * + * * + * * Redistributions in binary form must reproduce the * + * above copyright notice, this list of conditions and * + * the following disclaimer in the documentation and/or * + * other materials provided with the distribution. * + * * + * * Neither the name of Paul Mineiro nor the names * + * of other contributors may be used to endorse or promote * + * products derived from this software without specific * + * prior written permission. * + * * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND * + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, * + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES * + * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER * + * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE * + * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR * + * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF * + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * + * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * + * POSSIBILITY OF SUCH DAMAGE. * + * * + * Contact: Paul Mineiro * + *=====================================================================*/ + +#ifndef __FAST_HYPERBOLIC_H_ +#define __FAST_HYPERBOLIC_H_ + +#include + +static inline float +fastsinh (float p) +{ + return 0.5f * (fastexp (p) - fastexp (-p)); +} + +static inline float +fastersinh (float p) +{ + return 0.5f * (fasterexp (p) - fasterexp (-p)); +} + +static inline float +fastcosh (float p) +{ + return 0.5f * (fastexp (p) + fastexp (-p)); +} + +static inline float +fastercosh (float p) +{ + return 0.5f * (fasterexp (p) + fasterexp (-p)); +} + +static inline float +fasttanh (float p) +{ + return -1.0f + 2.0f / (1.0f + fastexp (-2.0f * p)); +} + +static inline float +fastertanh (float p) +{ + return -1.0f + 2.0f / (1.0f + fasterexp (-2.0f * p)); +} + +#ifdef __SSE2__ + +static inline v4sf +vfastsinh (const v4sf p) +{ + const v4sf c_0_5 = v4sfl (0.5f); + + return c_0_5 * (vfastexp (p) - vfastexp (-p)); +} + +static inline v4sf +vfastersinh (const v4sf p) +{ + const v4sf c_0_5 = v4sfl (0.5f); + + return c_0_5 * (vfasterexp (p) - vfasterexp (-p)); +} + +static inline v4sf +vfastcosh (const v4sf p) +{ + const v4sf c_0_5 = v4sfl (0.5f); + + return c_0_5 * (vfastexp (p) + vfastexp (-p)); +} + +static inline v4sf +vfastercosh (const v4sf p) +{ + const v4sf c_0_5 = v4sfl (0.5f); + + return c_0_5 * (vfasterexp (p) + vfasterexp (-p)); +} + +static inline v4sf +vfasttanh (const v4sf p) +{ + const v4sf c_1 = v4sfl (1.0f); + const v4sf c_2 = v4sfl (2.0f); + + return -c_1 + c_2 / (c_1 + vfastexp (-c_2 * p)); +} + +static inline v4sf +vfastertanh (const v4sf p) +{ + const v4sf c_1 = v4sfl (1.0f); + const v4sf c_2 = v4sfl (2.0f); + + return -c_1 + c_2 / (c_1 + vfasterexp (-c_2 * p)); +} + +#endif //__SSE2__ + +#endif // __FAST_HYPERBOLIC_H_ +/*=====================================================================* + * Copyright (C) 2011 Paul Mineiro * + * All rights reserved. * + * * + * Redistribution and use in source and binary forms, with * + * or without modification, are permitted provided that the * + * following conditions are met: * + * * + * * Redistributions of source code must retain the * + * above copyright notice, this list of conditions and * + * the following disclaimer. * + * * + * * Redistributions in binary form must reproduce the * + * above copyright notice, this list of conditions and * + * the following disclaimer in the documentation and/or * + * other materials provided with the distribution. * + * * + * * Neither the name of Paul Mineiro nor the names * + * of other contributors may be used to endorse or promote * + * products derived from this software without specific * + * prior written permission. * + * * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND * + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, * + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES * + * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER * + * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE * + * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR * + * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF * + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * + * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * + * POSSIBILITY OF SUCH DAMAGE. * + * * + * Contact: Paul Mineiro * + *=====================================================================*/ + +#ifndef __FAST_LAMBERT_W_H_ +#define __FAST_LAMBERT_W_H_ + +#include + +// these functions compute the upper branch aka W_0 + +static inline float +fastlambertw (float x) +{ + static const float threshold = 2.26445f; + + float c = (x < threshold) ? 1.546865557f : 1.0f; + float d = (x < threshold) ? 2.250366841f : 0.0f; + float a = (x < threshold) ? -0.737769969f : 0.0f; + + float logterm = fastlog (c * x + d); + float loglogterm = fastlog (logterm); + + float minusw = -a - logterm + loglogterm - loglogterm / logterm; + float expminusw = fastexp (minusw); + float xexpminusw = x * expminusw; + float pexpminusw = xexpminusw - minusw; + + return (2.0f * xexpminusw - minusw * (4.0f * xexpminusw - minusw * pexpminusw)) / + (2.0f + pexpminusw * (2.0f - minusw)); +} + +static inline float +fasterlambertw (float x) +{ + static const float threshold = 2.26445f; + + float c = (x < threshold) ? 1.546865557f : 1.0f; + float d = (x < threshold) ? 2.250366841f : 0.0f; + float a = (x < threshold) ? -0.737769969f : 0.0f; + + float logterm = fasterlog (c * x + d); + float loglogterm = fasterlog (logterm); + + float w = a + logterm - loglogterm + loglogterm / logterm; + float expw = fasterexp (-w); + + return (w * w + expw * x) / (1.0f + w); +} + +static inline float +fastlambertwexpx (float x) +{ + static const float k = 1.1765631309f; + static const float a = 0.94537622168f; + + float logarg = fmaxf (x, k); + float powarg = (x < k) ? a * (x - k) : 0; + + float logterm = fastlog (logarg); + float powterm = fasterpow2 (powarg); // don't need accuracy here + + float w = powterm * (logarg - logterm + logterm / logarg); + float logw = fastlog (w); + float p = x - logw; + + return w * (2.0f + p + w * (3.0f + 2.0f * p)) / + (2.0f - p + w * (5.0f + 2.0f * w)); +} + +static inline float +fasterlambertwexpx (float x) +{ + static const float k = 1.1765631309f; + static const float a = 0.94537622168f; + + float logarg = fmaxf (x, k); + float powarg = (x < k) ? a * (x - k) : 0; + + float logterm = fasterlog (logarg); + float powterm = fasterpow2 (powarg); + + float w = powterm * (logarg - logterm + logterm / logarg); + float logw = fasterlog (w); + + return w * (1.0f + x - logw) / (1.0f + w); +} + +#ifdef __SSE2__ + +static inline v4sf +vfastlambertw (v4sf x) +{ + const v4sf threshold = v4sfl (2.26445f); + + v4sf under = _mm_cmplt_ps (x, threshold); + v4sf c = _mm_or_ps (_mm_and_ps (under, v4sfl (1.546865557f)), + _mm_andnot_ps (under, v4sfl (1.0f))); + v4sf d = _mm_and_ps (under, v4sfl (2.250366841f)); + v4sf a = _mm_and_ps (under, v4sfl (-0.737769969f)); + + v4sf logterm = vfastlog (c * x + d); + v4sf loglogterm = vfastlog (logterm); + + v4sf minusw = -a - logterm + loglogterm - loglogterm / logterm; + v4sf expminusw = vfastexp (minusw); + v4sf xexpminusw = x * expminusw; + v4sf pexpminusw = xexpminusw - minusw; + + return (v4sfl (2.0f) * xexpminusw - minusw * (v4sfl (4.0f) * xexpminusw - minusw * pexpminusw)) / + (v4sfl (2.0f) + pexpminusw * (v4sfl (2.0f) - minusw)); +} + +static inline v4sf +vfasterlambertw (v4sf x) +{ + const v4sf threshold = v4sfl (2.26445f); + + v4sf under = _mm_cmplt_ps (x, threshold); + v4sf c = _mm_or_ps (_mm_and_ps (under, v4sfl (1.546865557f)), + _mm_andnot_ps (under, v4sfl (1.0f))); + v4sf d = _mm_and_ps (under, v4sfl (2.250366841f)); + v4sf a = _mm_and_ps (under, v4sfl (-0.737769969f)); + + v4sf logterm = vfasterlog (c * x + d); + v4sf loglogterm = vfasterlog (logterm); + + v4sf w = a + logterm - loglogterm + loglogterm / logterm; + v4sf expw = vfasterexp (-w); + + return (w * w + expw * x) / (v4sfl (1.0f) + w); +} + +static inline v4sf +vfastlambertwexpx (v4sf x) +{ + const v4sf k = v4sfl (1.1765631309f); + const v4sf a = v4sfl (0.94537622168f); + const v4sf two = v4sfl (2.0f); + const v4sf three = v4sfl (3.0f); + const v4sf five = v4sfl (5.0f); + + v4sf logarg = _mm_max_ps (x, k); + v4sf powarg = _mm_and_ps (_mm_cmplt_ps (x, k), a * (x - k)); + + v4sf logterm = vfastlog (logarg); + v4sf powterm = vfasterpow2 (powarg); // don't need accuracy here + + v4sf w = powterm * (logarg - logterm + logterm / logarg); + v4sf logw = vfastlog (w); + v4sf p = x - logw; + + return w * (two + p + w * (three + two * p)) / + (two - p + w * (five + two * w)); +} + +static inline v4sf +vfasterlambertwexpx (v4sf x) +{ + const v4sf k = v4sfl (1.1765631309f); + const v4sf a = v4sfl (0.94537622168f); + + v4sf logarg = _mm_max_ps (x, k); + v4sf powarg = _mm_and_ps (_mm_cmplt_ps (x, k), a * (x - k)); + + v4sf logterm = vfasterlog (logarg); + v4sf powterm = vfasterpow2 (powarg); + + v4sf w = powterm * (logarg - logterm + logterm / logarg); + v4sf logw = vfasterlog (w); + + return w * (v4sfl (1.0f) + x - logw) / (v4sfl (1.0f) + w); +} + +#endif // __SSE2__ + +#endif // __FAST_LAMBERT_W_H_ + +/*=====================================================================* + * Copyright (C) 2011 Paul Mineiro * + * All rights reserved. * + * * + * Redistribution and use in source and binary forms, with * + * or without modification, are permitted provided that the * + * following conditions are met: * + * * + * * Redistributions of source code must retain the * + * above copyright notice, this list of conditions and * + * the following disclaimer. * + * * + * * Redistributions in binary form must reproduce the * + * above copyright notice, this list of conditions and * + * the following disclaimer in the documentation and/or * + * other materials provided with the distribution. * + * * + * * Neither the name of Paul Mineiro nor the names * + * of other contributors may be used to endorse or promote * + * products derived from this software without specific * + * prior written permission. * + * * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND * + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, * + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES * + * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER * + * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE * + * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR * + * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF * + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * + * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * + * POSSIBILITY OF SUCH DAMAGE. * + * * + * Contact: Paul Mineiro * + *=====================================================================*/ + +#ifndef __FAST_POW_H_ +#define __FAST_POW_H_ + +#include + +static inline float +fastpow (float x, + float p) +{ + return fastpow2 (p * fastlog2 (x)); +} + +static inline float +fasterpow (float x, + float p) +{ + return fasterpow2 (p * fasterlog2 (x)); +} + +#ifdef __SSE2__ + +static inline v4sf +vfastpow (const v4sf x, + const v4sf p) +{ + return vfastpow2 (p * vfastlog2 (x)); +} + +static inline v4sf +vfasterpow (const v4sf x, + const v4sf p) +{ + return vfasterpow2 (p * vfasterlog2 (x)); +} + +#endif //__SSE2__ + +#endif // __FAST_POW_H_ +/*=====================================================================* + * Copyright (C) 2011 Paul Mineiro * + * All rights reserved. * + * * + * Redistribution and use in source and binary forms, with * + * or without modification, are permitted provided that the * + * following conditions are met: * + * * + * * Redistributions of source code must retain the * + * above copyright notice, this list of conditions and * + * the following disclaimer. * + * * + * * Redistributions in binary form must reproduce the * + * above copyright notice, this list of conditions and * + * the following disclaimer in the documentation and/or * + * other materials provided with the distribution. * + * * + * * Neither the name of Paul Mineiro nor the names * + * of other contributors may be used to endorse or promote * + * products derived from this software without specific * + * prior written permission. * + * * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND * + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, * + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES * + * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER * + * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE * + * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR * + * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF * + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * + * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * + * POSSIBILITY OF SUCH DAMAGE. * + * * + * Contact: Paul Mineiro * + *=====================================================================*/ + +#ifndef __FAST_SIGMOID_H_ +#define __FAST_SIGMOID_H_ + +#include + +static inline float +fastsigmoid (float x) +{ + return 1.0f / (1.0f + fastexp (-x)); +} + +static inline float +fastersigmoid (float x) +{ + return 1.0f / (1.0f + fasterexp (-x)); +} + +#ifdef __SSE2__ + +static inline v4sf +vfastsigmoid (const v4sf x) +{ + const v4sf c_1 = v4sfl (1.0f); + + return c_1 / (c_1 + vfastexp (-x)); +} + +static inline v4sf +vfastersigmoid (const v4sf x) +{ + const v4sf c_1 = v4sfl (1.0f); + + return c_1 / (c_1 + vfasterexp (-x)); +} + +#endif //__SSE2__ + +#endif // __FAST_SIGMOID_H_ +/*=====================================================================* + * Copyright (C) 2011 Paul Mineiro * + * All rights reserved. * + * * + * Redistribution and use in source and binary forms, with * + * or without modification, are permitted provided that the * + * following conditions are met: * + * * + * * Redistributions of source code must retain the * + * above copyright notice, this list of conditions and * + * the following disclaimer. * + * * + * * Redistributions in binary form must reproduce the * + * above copyright notice, this list of conditions and * + * the following disclaimer in the documentation and/or * + * other materials provided with the distribution. * + * * + * * Neither the name of Paul Mineiro nor the names * + * of other contributors may be used to endorse or promote * + * products derived from this software without specific * + * prior written permission. * + * * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND * + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, * + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES * + * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER * + * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE * + * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR * + * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF * + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * + * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * + * POSSIBILITY OF SUCH DAMAGE. * + * * + * Contact: Paul Mineiro * + *=====================================================================*/ + +#ifndef __FAST_TRIG_H_ +#define __FAST_TRIG_H_ + +#include + +// http://www.devmaster.net/forums/showthread.php?t=5784 +// fast sine variants are for x \in [ -\pi, pi ] +// fast cosine variants are for x \in [ -\pi, pi ] +// fast tangent variants are for x \in [ -\pi / 2, pi / 2 ] +// "full" versions of functions handle the entire range of inputs +// although the range reduction technique used here will be hopelessly +// inaccurate for |x| >> 1000 +// +// WARNING: fastsinfull, fastcosfull, and fasttanfull can be slower than +// libc calls on older machines (!) and on newer machines are only +// slighly faster. however: +// * vectorized versions are competitive +// * faster full versions are competitive + +static inline float +fastsin (float x) +{ + static const float fouroverpi = 1.2732395447351627f; + static const float fouroverpisq = 0.40528473456935109f; + static const float q = 0.78444488374548933f; + union { float f; uint32_t i; } p = { 0.20363937680730309f }; + union { float f; uint32_t i; } r = { 0.015124940802184233f }; + union { float f; uint32_t i; } s = { -0.0032225901625579573f }; + + union { float f; uint32_t i; } vx = { x }; + uint32_t sign = vx.i & 0x80000000; + vx.i = vx.i & 0x7FFFFFFF; + + float qpprox = fouroverpi * x - fouroverpisq * x * vx.f; + float qpproxsq = qpprox * qpprox; + + p.i |= sign; + r.i |= sign; + s.i ^= sign; + + return q * qpprox + qpproxsq * (p.f + qpproxsq * (r.f + qpproxsq * s.f)); +} + +static inline float +fastersin (float x) +{ + static const float fouroverpi = 1.2732395447351627f; + static const float fouroverpisq = 0.40528473456935109f; + static const float q = 0.77633023248007499f; + union { float f; uint32_t i; } p = { 0.22308510060189463f }; + + union { float f; uint32_t i; } vx = { x }; + uint32_t sign = vx.i & 0x80000000; + vx.i &= 0x7FFFFFFF; + + float qpprox = fouroverpi * x - fouroverpisq * x * vx.f; + + p.i |= sign; + + return qpprox * (q + p.f * qpprox); +} + +static inline float +fastsinfull (float x) +{ + static const float twopi = 6.2831853071795865f; + static const float invtwopi = 0.15915494309189534f; + + int k = x * invtwopi; + float half = (x < 0) ? -0.5f : 0.5f; + return fastsin ((half + k) * twopi - x); +} + +static inline float +fastersinfull (float x) +{ + static const float twopi = 6.2831853071795865f; + static const float invtwopi = 0.15915494309189534f; + + int k = x * invtwopi; + float half = (x < 0) ? -0.5f : 0.5f; + return fastersin ((half + k) * twopi - x); +} + +static inline float +fastcos (float x) +{ + static const float halfpi = 1.5707963267948966f; + static const float halfpiminustwopi = -4.7123889803846899f; + float offset = (x > halfpi) ? halfpiminustwopi : halfpi; + return fastsin (x + offset); +} + +static inline float +fastercos (float x) +{ + static const float twooverpi = 0.63661977236758134f; + static const float p = 0.54641335845679634f; + + union { float f; uint32_t i; } vx = { x }; + vx.i &= 0x7FFFFFFF; + + float qpprox = 1.0f - twooverpi * vx.f; + + return qpprox + p * qpprox * (1.0f - qpprox * qpprox); +} + +static inline float +fastcosfull (float x) +{ + static const float halfpi = 1.5707963267948966f; + return fastsinfull (x + halfpi); +} + +static inline float +fastercosfull (float x) +{ + static const float halfpi = 1.5707963267948966f; + return fastersinfull (x + halfpi); +} + +static inline float +fasttan (float x) +{ + static const float halfpi = 1.5707963267948966f; + return fastsin (x) / fastsin (x + halfpi); +} + +static inline float +fastertan (float x) +{ + return fastersin (x) / fastercos (x); +} + +static inline float +fasttanfull (float x) +{ + static const float twopi = 6.2831853071795865f; + static const float invtwopi = 0.15915494309189534f; + + int k = x * invtwopi; + float half = (x < 0) ? -0.5f : 0.5f; + float xnew = x - (half + k) * twopi; + + return fastsin (xnew) / fastcos (xnew); +} + +static inline float +fastertanfull (float x) +{ + static const float twopi = 6.2831853071795865f; + static const float invtwopi = 0.15915494309189534f; + + int k = x * invtwopi; + float half = (x < 0) ? -0.5f : 0.5f; + float xnew = x - (half + k) * twopi; + + return fastersin (xnew) / fastercos (xnew); +} + +#ifdef __SSE2__ + +static inline v4sf +vfastsin (const v4sf x) +{ + const v4sf fouroverpi = v4sfl (1.2732395447351627f); + const v4sf fouroverpisq = v4sfl (0.40528473456935109f); + const v4sf q = v4sfl (0.78444488374548933f); + const v4sf p = v4sfl (0.20363937680730309f); + const v4sf r = v4sfl (0.015124940802184233f); + const v4sf s = v4sfl (-0.0032225901625579573f); + + union { v4sf f; v4si i; } vx = { x }; + v4si sign = vx.i & v4sil (0x80000000); + vx.i &= v4sil (0x7FFFFFFF); + + v4sf qpprox = fouroverpi * x - fouroverpisq * x * vx.f; + v4sf qpproxsq = qpprox * qpprox; + union { v4sf f; v4si i; } vy; vy.f = qpproxsq * (p + qpproxsq * (r + qpproxsq * s)); + vy.i ^= sign; + + return q * qpprox + vy.f; +} + +static inline v4sf +vfastersin (const v4sf x) +{ + const v4sf fouroverpi = v4sfl (1.2732395447351627f); + const v4sf fouroverpisq = v4sfl (0.40528473456935109f); + const v4sf q = v4sfl (0.77633023248007499f); + const v4sf plit = v4sfl (0.22308510060189463f); + union { v4sf f; v4si i; } p = { plit }; + + union { v4sf f; v4si i; } vx = { x }; + v4si sign = vx.i & v4sil (0x80000000); + vx.i &= v4sil (0x7FFFFFFF); + + v4sf qpprox = fouroverpi * x - fouroverpisq * x * vx.f; + + p.i |= sign; + + return qpprox * (q + p.f * qpprox); +} + +static inline v4sf +vfastsinfull (const v4sf x) +{ + const v4sf twopi = v4sfl (6.2831853071795865f); + const v4sf invtwopi = v4sfl (0.15915494309189534f); + + v4si k = v4sf_to_v4si (x * invtwopi); + + v4sf ltzero = _mm_cmplt_ps (x, v4sfl (0.0f)); + v4sf half = _mm_or_ps (_mm_and_ps (ltzero, v4sfl (-0.5f)), + _mm_andnot_ps (ltzero, v4sfl (0.5f))); + + return vfastsin ((half + v4si_to_v4sf (k)) * twopi - x); +} + +static inline v4sf +vfastersinfull (const v4sf x) +{ + const v4sf twopi = v4sfl (6.2831853071795865f); + const v4sf invtwopi = v4sfl (0.15915494309189534f); + + v4si k = v4sf_to_v4si (x * invtwopi); + + v4sf ltzero = _mm_cmplt_ps (x, v4sfl (0.0f)); + v4sf half = _mm_or_ps (_mm_and_ps (ltzero, v4sfl (-0.5f)), + _mm_andnot_ps (ltzero, v4sfl (0.5f))); + + return vfastersin ((half + v4si_to_v4sf (k)) * twopi - x); +} + +static inline v4sf +vfastcos (const v4sf x) +{ + const v4sf halfpi = v4sfl (1.5707963267948966f); + const v4sf halfpiminustwopi = v4sfl (-4.7123889803846899f); + v4sf lthalfpi = _mm_cmpnlt_ps (x, halfpi); + v4sf offset = _mm_or_ps (_mm_and_ps (lthalfpi, halfpiminustwopi), + _mm_andnot_ps (lthalfpi, halfpi)); + return vfastsin (x + offset); +} + +static inline v4sf +vfastercos (v4sf x) +{ + const v4sf twooverpi = v4sfl (0.63661977236758134f); + const v4sf p = v4sfl (0.54641335845679634); + + v4sf vx = v4sf_fabs (x); + v4sf qpprox = v4sfl (1.0f) - twooverpi * vx; + + return qpprox + p * qpprox * (v4sfl (1.0f) - qpprox * qpprox); +} + +static inline v4sf +vfastcosfull (const v4sf x) +{ + const v4sf halfpi = v4sfl (1.5707963267948966f); + return vfastsinfull (x + halfpi); +} + +static inline v4sf +vfastercosfull (const v4sf x) +{ + const v4sf halfpi = v4sfl (1.5707963267948966f); + return vfastersinfull (x + halfpi); +} + +static inline v4sf +vfasttan (const v4sf x) +{ + const v4sf halfpi = v4sfl (1.5707963267948966f); + return vfastsin (x) / vfastsin (x + halfpi); +} + +static inline v4sf +vfastertan (const v4sf x) +{ + return vfastersin (x) / vfastercos (x); +} + +static inline v4sf +vfasttanfull (const v4sf x) +{ + const v4sf twopi = v4sfl (6.2831853071795865f); + const v4sf invtwopi = v4sfl (0.15915494309189534f); + + v4si k = v4sf_to_v4si (x * invtwopi); + + v4sf ltzero = _mm_cmplt_ps (x, v4sfl (0.0f)); + v4sf half = _mm_or_ps (_mm_and_ps (ltzero, v4sfl (-0.5f)), + _mm_andnot_ps (ltzero, v4sfl (0.5f))); + v4sf xnew = x - (half + v4si_to_v4sf (k)) * twopi; + + return vfastsin (xnew) / vfastcos (xnew); +} + +static inline v4sf +vfastertanfull (const v4sf x) +{ + const v4sf twopi = v4sfl (6.2831853071795865f); + const v4sf invtwopi = v4sfl (0.15915494309189534f); + + v4si k = v4sf_to_v4si (x * invtwopi); + + v4sf ltzero = _mm_cmplt_ps (x, v4sfl (0.0f)); + v4sf half = _mm_or_ps (_mm_and_ps (ltzero, v4sfl (-0.5f)), + _mm_andnot_ps (ltzero, v4sfl (0.5f))); + v4sf xnew = x - (half + v4si_to_v4sf (k)) * twopi; + + return vfastersin (xnew) / vfastercos (xnew); +} + +#endif //__SSE2__ + +#endif // __FAST_TRIG_H_ diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/include/ipa_room_segmentation/features.h b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/include/ipa_room_segmentation/features.h new file mode 100644 index 0000000..9b43051 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/include/ipa_room_segmentation/features.h @@ -0,0 +1,81 @@ +//Header for featurecalculation +//number of Features that can be calculated: 23 +#pragma once + +#include + + +class LaserScannerFeatures +{ +public: + //function to get the number of the features implemented + int get_feature_count(); + // reset cached data + void resetCachedData(); + //function for calculating the feature + double get_feature(const std::vector& beams, const std::vector& angles, cv::Point point, int feature); + void get_features(const std::vector& beams, const std::vector& angles, cv::Point point, cv::Mat& features); + //feature 1: average difference between beamlenghts + double calc_feature1(const std::vector& beams); + //feature 2: standard deviation of difference between beamlengths + double calc_feature2(const std::vector& beams); + //feature 3: average difference between beamlengths, but cutting of too long beams + double calc_feature3(const std::vector& beams, double maxlength); + //feature 4: standard deviation of difference between limited beamlenghts + double calc_feature4(const std::vector& beams, double maxval); + //feature 5: average beamlength + double calc_feature5(const std::vector& beams); + //feature 6: standard deviation of the beamlengths + double calc_feature6(const std::vector& beams); + //feature 7: number of gaps between the beams (a gap is when the difference between the beamlenghts is larger than a threshold) + double calc_feature7(const std::vector& beams); + //feature 8: distance between two Points corresponding to local minima of beamlengths + double calc_feature8(const std::vector& beams, const std::vector& angles); + //feature 9: angle between two Points corresponding to local minima of beamlenghts + double calc_feature9(const std::vector& beams, const std::vector& angles); + //feature 10: average of relation between two neighboring beams + double calc_feature10(const std::vector& beams); + //feature 11: standard deviation of relation between two neighboring beams + double calc_feature11(const std::vector& beams); + //feature 12: number of relative gaps, a relativ gap is when the relation of two negihboring beams is larger than a threshold + double calc_feature12(const std::vector& beams); + //feature 13: Kurtosis = (Sum((x - mean)^4))/sigma^4) - 3, where mean is the mean-value and sigma is the standard deviation + double calc_feature13(const std::vector& beams); + //feature 22: average of beam lengths / max. length + double calc_feature22(const std::vector& beams); + //feature 23: standard deviation of the beam lengths divided by the maximal value + double calc_feature23(const std::vector& beams); + //****************from now on Features that need a polygonal approximation of the beams***************************** + //calculate the polygonal approximation + std::vector calc_polygonal_approx(const std::vector&, const std::vector&, cv::Point location); + //calcilate the centroid for each polygonal approximation + cv::Point calc_centroid(const std::vector& beams, const std::vector& angles, cv::Point location); + //feature 14: area of the polygonal approximation + double calc_feature14(const std::vector& beams, const std::vector& angles, cv::Point location); + //feature 15: perimeter of the polygonal approximation + double calc_feature15(const std::vector& beams, const std::vector& angles, cv::Point location); + //feature 16: area divided by perimeter of the polygonal approximation + double calc_feature16(const std::vector& beams, const std::vector& angles, cv::Point location); + //feature 17: average distance between centroid and the boundary of the polygonal approach + double calc_feature17(const std::vector& beams, const std::vector& angles, cv::Point location); + //feature 18: standard deviation of distance between centroid and the boundary of the polygonal approach + double calc_feature18(const std::vector& beams, const std::vector& angles, cv::Point location); + //feature 19: half the major axis of the ellipse that surrounds the polygon, calculated with openCV + double calc_feature19(const std::vector& beams, const std::vector& angles, cv::Point location); + //feature 20: half the minor axis of the ellipse that surrounds the polygon, calculated with openCV + double calc_feature20(const std::vector& beams, const std::vector& angles, cv::Point location); + //feature 21: major axis/minor axis + double calc_feature21(const std::vector& beams, const std::vector& angles, cv::Point location); + +private: + + std::vector features_; + std::vector features_computed_; + + std::vector polygon_; + bool polygon_computed_; + + cv::Point centroid_; + bool centroid_computed_; + +}; diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/include/ipa_room_segmentation/meanshift2d.h b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/include/ipa_room_segmentation/meanshift2d.h new file mode 100644 index 0000000..50b4ba6 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/include/ipa_room_segmentation/meanshift2d.h @@ -0,0 +1,76 @@ +/*! + ***************************************************************** + * \file + * + * \note + * Copyright (c) 2015 \n + * Fraunhofer Institute for Manufacturing Engineering + * and Automation (IPA) \n\n + * + ***************************************************************** + * + * \note + * Project name: Care-O-bot + * \note + * ROS stack name: autopnp + * \note + * ROS package name: ipa_room_segmentation + * + * \author + * Author: Richard Bormann + * \author + * Supervised by: + * + * \date Date of creation: 22.07.2015 + * + * \brief + * + * + ***************************************************************** + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * - Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. \n + * - Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. \n + * - Neither the name of the Fraunhofer Institute for Manufacturing + * Engineering and Automation (IPA) nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. \n + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License LGPL as + * published by the Free Software Foundation, either version 3 of the + * License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License LGPL for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License LGPL along with this program. + * If not, see . + * + ****************************************************************/ + +#include + +#include +#include + +class MeanShift2D +{ +public: + MeanShift2D(void) {}; + + void filter(const std::vector& data, std::vector& filtered_data, const double bandwidth, const int maximumIterations=100); + + void computeConvergencePoints(const std::vector& filtered_data, std::vector& convergence_points, std::vector< std::vector >& convergence_sets, const double sensitivity); + + // map_resolution in [m/cell] + cv::Vec2d findRoomCenter(const cv::Mat& room_image, const std::vector& room_cells, double map_resolution); +}; diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/include/ipa_room_segmentation/morphological_segmentation.h b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/include/ipa_room_segmentation/morphological_segmentation.h new file mode 100644 index 0000000..a10d0a7 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/include/ipa_room_segmentation/morphological_segmentation.h @@ -0,0 +1,14 @@ +#include +#include +#include +#include +#include + + +class MorphologicalSegmentation +{ +public: + MorphologicalSegmentation(); + //algorithm to segment the map + void segmentMap(const cv::Mat& map_to_be_labeled, cv::Mat& segmented_map, double map_resolution_from_subscription, double room_area_factor_lower_limit, double room_area_factor_upper_limit); +}; diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/include/ipa_room_segmentation/raycasting.h b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/include/ipa_room_segmentation/raycasting.h new file mode 100644 index 0000000..de5184f --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/include/ipa_room_segmentation/raycasting.h @@ -0,0 +1,27 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +#define PI 3.14159265 + +class LaserScannerRaycasting +{ +public: + LaserScannerRaycasting(); + + //raycasting function using the simple method that tracks a ray until its end + void raycasting(const cv::Mat& map, const cv::Point& location, std::vector& distances); + + //raycasting function based on the bresenham algorithm + void bresenham_raycasting(const cv::Mat& map, const cv::Point& location, std::vector& distances); + +private: + + std::vector precomputed_cos_; + std::vector precomputed_sin_; +}; diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/include/ipa_room_segmentation/room_class.h b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/include/ipa_room_segmentation/room_class.h new file mode 100644 index 0000000..bdc8de0 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/include/ipa_room_segmentation/room_class.h @@ -0,0 +1,94 @@ +#ifndef __ROOM_CLASS_H__ +#define __ROOM_CLASS_H__ + +#include +#include + +#include +#include +#include +#include +#include + +#include + +//This is the class that represents a room. It has a ID-number, Points that belong to it and a list of neighbors. +class Room +{ +public: + + // Struct that compares two given points and returns if the y-coordinate of the first is smaller or if they are equal if the + // x-coordinate of the first is smaller. This is used for sets to easily store cv::Point objects and search for specific objects. + struct cv_Point_comp + { + bool operator()(const cv::Point& lhs, const cv::Point& rhs) const + { + return ((lhs.y < rhs.y) || (lhs.y == rhs.y && lhs.x < rhs.x)); + } + }; + + typedef std::set PointSet; + + + Room(int id_of_room); + + // merges the provided room into this room + void mergeRoom(Room& room_to_merge, double map_resolution); + + int insertMemberPoint(cv::Point new_member, double map_resolution); + + int insertMemberPoints(const std::vector& new_members, double map_resolution); + + void addNeighbor(int new_neighbor_id); + + int addNeighborID(int new_neighbor_id); + + int getNeighborCount(); + + std::map& getNeighborStatistics(); + + void getNeighborStatisticsInverse(std::map< int,int,std::greater >& neighbor_room_statistics_inverse); + + int getNeighborWithLargestCommonBorder(bool exclude_wall=true); + + double getPerimeterRatioOfXLargestRooms(const int number_rooms); + + double getWallToPerimeterRatio(); + + std::vector& getNeighborIDs(); + + double getArea(); + + double getPerimeter(); + + int getID() const; + + cv::Point getCenter(); + + const std::vector& getMembers(); + + int setRoomId(int new_value, cv::Mat& map); + + int setArea(double room_area); + + int setPerimeter(double room_perimeter); + + +protected: + int id_number_; + + std::vector member_points_; + + std::vector neighbor_room_ids_; + + std::map neighbor_room_statistics_; // maps from room labels of neighboring rooms to number of touching pixels of the respective neighboring room + + double room_area_; + + double room_perimeter_; +}; + + +bool sortRoomsAscending(Room a, Room b); + +#endif diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/include/ipa_room_segmentation/timer.h b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/include/ipa_room_segmentation/timer.h new file mode 100644 index 0000000..797978c --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/include/ipa_room_segmentation/timer.h @@ -0,0 +1,184 @@ +/* + * timer.h + * + * Created on: May 13, 2013 + * Author: rmb-ce + */ + +// from: http://snipplr.com/view/40650/timer-class-for-both-unixlinuxmac-and-windows-system/ + +////////////////// +// How to Use //// +////////////////// + +//#include +//#include "timer.h" +//using namespace std; + +//int main() +//{ +// Timer timer; +// +// // start timer +// timer.start(); +// +// // do something +// ... +// +// // stop timer +// timer.stop(); +// +// // print the elapsed time in millisec +// cout << timer.getElapsedTimeInMilliSec() << " ms.\n"; +// +// return 0; +//} + + +////////////////////////////////////////////////////////////////////////////// +// Timer.h +// ======= +// High Resolution Timer. +// This timer is able to measure the elapsed time with 1 micro-second accuracy +// in both Windows, Linux and Unix system +// +// AUTHOR: Song Ho Ahn (song.ahn@gmail.com) +// CREATED: 2003-01-13 +// UPDATED: 2006-01-13 +// +// Copyright (c) 2003 Song Ho Ahn +////////////////////////////////////////////////////////////////////////////// + +#ifndef TIMER_H_DEF +#define TIMER_H_DEF + +#ifndef __LINUX__ // Windows system specific +//#include +#else // Unix based system specific +#include +#endif + +#include + + +class Timer +{ +public: +// default constructor + Timer() + { + #ifdef WIN32 + QueryPerformanceFrequency(&frequency); + startCount.QuadPart = 0; + endCount.QuadPart = 0; + #else + startCount.tv_sec = startCount.tv_usec = 0; + endCount.tv_sec = endCount.tv_usec = 0; + #endif + + stopped = 0; + startTimeInMicroSec = 0; + endTimeInMicroSec = 0; + + start(); + } + + // default destructor + ~Timer() + {} + + /////////////////////////////////////////////////////////////////////////////// + // start timer. + // startCount will be set at this point. + /////////////////////////////////////////////////////////////////////////////// + void start() + { + stopped = 0; // reset stop flag + #ifdef WIN32 + QueryPerformanceCounter(&startCount); + #else + gettimeofday(&startCount, NULL); + #endif + } + + /////////////////////////////////////////////////////////////////////////////// + // stop the timer. + // endCount will be set at this point. + /////////////////////////////////////////////////////////////////////////////// + void stop() + { + stopped = 1; // set timer stopped flag + + #ifdef WIN32 + QueryPerformanceCounter(&endCount); + #else + gettimeofday(&endCount, NULL); + #endif + } + + /////////////////////////////////////////////////////////////////////////////// + // same as getElapsedTimeInSec() + /////////////////////////////////////////////////////////////////////////////// + double getElapsedTime() + { + return this->getElapsedTimeInSec(); + } + + /////////////////////////////////////////////////////////////////////////////// + // divide elapsedTimeInMicroSec by 1000000 + /////////////////////////////////////////////////////////////////////////////// + double getElapsedTimeInSec() + { + return this->getElapsedTimeInMicroSec() * 0.000001; + } + + /////////////////////////////////////////////////////////////////////////////// + // divide elapsedTimeInMicroSec by 1000 + /////////////////////////////////////////////////////////////////////////////// + double getElapsedTimeInMilliSec() + { + return this->getElapsedTimeInMicroSec() * 0.001; + } + + /////////////////////////////////////////////////////////////////////////////// + // compute elapsed time in micro-second resolution. + // other getElapsedTime will call this first, then convert to correspond resolution. + /////////////////////////////////////////////////////////////////////////////// + double getElapsedTimeInMicroSec() + { + #ifdef WIN32 + if(!stopped) + QueryPerformanceCounter(&endCount); + + startTimeInMicroSec = startCount.QuadPart * (1000000.0 / frequency.QuadPart); + endTimeInMicroSec = endCount.QuadPart * (1000000.0 / frequency.QuadPart); + #else + if(!stopped) + gettimeofday(&endCount, NULL); + + startTimeInMicroSec = (startCount.tv_sec * 1000000.0) + startCount.tv_usec; + endTimeInMicroSec = (endCount.tv_sec * 1000000.0) + endCount.tv_usec; + #endif + + return endTimeInMicroSec - startTimeInMicroSec; + } + + +protected: + + +private: + double startTimeInMicroSec; // starting time in micro-second + double endTimeInMicroSec; // ending time in micro-second + int stopped; // stop flag +#ifdef WIN32 + LARGE_INTEGER frequency; // ticks per second + LARGE_INTEGER startCount; // + LARGE_INTEGER endCount; // +#else + timeval startCount; // + timeval endCount; // +#endif +}; + +#endif // TIMER_H_DEF diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/include/ipa_room_segmentation/voronoi_random_field_features.h b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/include/ipa_room_segmentation/voronoi_random_field_features.h new file mode 100644 index 0000000..920c122 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/include/ipa_room_segmentation/voronoi_random_field_features.h @@ -0,0 +1,101 @@ +//Header for featurecalculation +//number of Features that can be calculated: 25 +#pragma once +#include +#include +#include +#include +#include +#include +#include + +#include + +class voronoiRandomFieldFeatures +{ + +public: + //function to get the number of the features implemented + int getFeatureCount(); + // reset cached data + void resetCachedData(); + //function for calculating the wanted feature + double getFeature(const std::vector& beams, const std::vector& angles, + const std::vector& clique_points, std::vector& labels_for_clique_points, + std::vector& possible_labels, cv::Point point, const int feature); + void getFeatures(const std::vector& beams, const std::vector& angles, const std::vector& clique_points, std::vector& labels_for_clique_points, + std::vector& possible_labels, cv::Point point, std::vector& features); + //feature 1: average difference between beamlengths + double calcFeature1(const std::vector& beams); + //feature 2: standard deviation of difference between beamlengths + double calcFeature2(const std::vector& beams); + //feature 3: average difference between beamlengths, but cutting of too long beams + double calcFeature3(const std::vector& beams, double maxlength); + //feature 4: standard deviation of difference between limited beamlenghts + double calcFeature4(const std::vector& beams, double maxval); + //feature 5: average beamlength + double calcFeature5(const std::vector& beams); + //feature 6: standard deviation of the beamlengths + double calcFeature6(const std::vector& beams); + //feature 7: number of gaps between the beams (a gap is when the difference between the beamlenghts is larger than a threshold) + double calcFeature7(const std::vector& beams); + //feature 8: distance between two Points corresponding to local minima of beamlengths + double calcFeature8(const std::vector& beams, const std::vector& angles); + //feature 9: angle between two Points corresponding to local minima of beamlenghts + double calcFeature9(const std::vector& beams, const std::vector& angles); + //feature 10: average of relation between two neighboring beams + double calcFeature10(const std::vector& beams); + //feature 11: standard deviation of relation between two neighboring beams + double calcFeature11(const std::vector& beams); + //feature 12: number of relative gaps, a relativ gap is when the relation of two negihboring beams is larger than a threshold + double calcFeature12(const std::vector& beams); + //feature 13: Kurtosis = (Sum((x - mean)^4))/sigma^4) - 3, where mean is the mean-value and sigma is the standard deviation + double calcFeature13(const std::vector& beams); + //feature 22: average of beam lengths / max. length + double calcFeature22(const std::vector& beams); + //feature 23: standard deviation of the beam lengths divided by the maximal value + double calcFeature23(const std::vector& beams); + //****************from now on Features that need a polygonal approximation of the beams***************************** + //calculate the polygonal approximation + std::vector calcPolygonalApprox(const std::vector&, const std::vector&, cv::Point location); + //calculate the centroid for each polygonal approximation + cv::Point calcCentroid(const std::vector& beams, const std::vector& angles, cv::Point location); + //feature 14: area of the polygonal approximation + double calcFeature14(const std::vector& beams, const std::vector& angles, cv::Point location); + //feature 15: perimeter of the polygonal approximation + double calcFeature15(const std::vector& beams, const std::vector& angles, cv::Point location); + //feature 16: area divided by perimeter of the polygonal approximation + double calcFeature16(const std::vector& beams, const std::vector& angles, cv::Point location); + //feature 17: average distance between centroid and the boundary of the polygonal approach + double calcFeature17(const std::vector& beams, const std::vector& angles, cv::Point location); + //feature 18: standard deviation of distance between centroid and the boundary of the polygonal approach + double calcFeature18(const std::vector& beams, const std::vector& angles, cv::Point location); + //feature 19: half the major axis of the ellipse that surrounds the polygon, calculated with openCV + double calcFeature19(const std::vector& beams, const std::vector& angles, cv::Point location); + //feature 20: half the minor axis of the ellipse that surrounds the polygon, calculated with openCV + double calcFeature20(const std::vector& beams, const std::vector& angles, cv::Point location); + //feature 21: major axis/minor axis + double calcFeature21(const std::vector& beams, const std::vector& angles, cv::Point location); + // feature 24: the curvature for a given clique + double calcFeature24(std::vector clique_points); + // feature 25: the relation between the labels of Points from the central point to the other points in the clique + double calcFeature25(std::vector& possible_labels, std::vector& labels_for_points); + // feature 26: the number of beams that are shorter than a defined maxval --> for door detection, maxval = 25 + double calcFeature26(const std::vector& beams, double maxval); + // feature 27: the area of the bounding box for beams that are smaller than the shortest beam in a defined epsilon neighborhood + double calcFeature27(const std::vector& beams, const std::vector& angles, double maxval, cv::Point location); + // feature 28: the ratio of the average beamlengths of the n longest and shortest beams + double calcFeature28(const std::vector& beams, double number_of_beams); + +protected: + + std::vector features_; + std::vector features_computed_; + + std::vector polygon_; + bool polygon_computed_; + + cv::Point centroid_; + bool centroid_computed_; +}; + diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/include/ipa_room_segmentation/voronoi_random_field_segmentation.h b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/include/ipa_room_segmentation/voronoi_random_field_segmentation.h new file mode 100644 index 0000000..5d4d225 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/include/ipa_room_segmentation/voronoi_random_field_segmentation.h @@ -0,0 +1,255 @@ +/*! + ***************************************************************** + * \file + * + * \note + * Copyright (c) 2015 \n + * Fraunhofer Institute for Manufacturing Engineering + * and Automation (IPA) \n\n + * + ***************************************************************** + * + * \note + * Project name: Care-O-bot + * \note + * ROS stack name: autopnp + * \note + * ROS package name: ipa_room_segmentation + * + * \author + * Author: Florian Jordan + * \author + * Supervised by: Richard Bormann + * + * \date Date of creation: 10.2015 + * + * \brief + * + * + ***************************************************************** + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * - Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. \n + * - Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. \n + * - Neither the name of the Fraunhofer Institute for Manufacturing + * Engineering and Automation (IPA) nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. \n + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License LGPL as + * published by the Free Software Foundation, either version 3 of the + * License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License LGPL for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License LGPL along with this program. + * If not, see . + * + ****************************************************************/ +#pragma once +#include "ros/ros.h" + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +// OpenGM-library headers +#include +#include +#include +#include +#include +#include +#include + +#include + +#include // some useful functions defined for all segmentations +#include +#include +#include +#include +#include + +#pragma once + + +// Typedef used for the dlib optimization. This Type stores n times 1 elements in a matirx --> represents a column-vector. +typedef dlib::matrix column_vector; + +// Typedefs for the OpenGM library. This library is a template-library for discrete factor-graphs (https://en.wikipedia.org/wiki/Factor_graph) +// and for operations on these, used in this algorithm to do loopy-belief-propagation for the conditional random field. +// +// Typedef for the Label-Space. This stores n variables that each can have m labels. +typedef opengm::SimpleDiscreteSpace LabelSpace; + +// Typedef for a factor-graph that stores doubles as results, adds the factors and has discrete labels for each variable. +typedef opengm::GraphicalModel , LabelSpace > FactorGraph; + +// Typedef for the update rule of messages in a factor graph, when using message propagation. +// Second Typedef is the Belief-Propagation used in this algorithm. It can be used on the defined FactorGraph, maximizes the defined +// functions and uses the Update-Rule for the messages that maximize the overall Graph. MaxDistance is a metric used to measure +// the distance of two messages in the graph. +typedef opengm::BeliefPropagationUpdateRules UpdateRules; +typedef opengm::MessagePassing LoopyBeliefPropagation; + +// Overload of the + operator for vectors: +// Given vectors a and b it returns a+b. If the vectors are not the same size a -1 vector gets returned to show an error. +template +std::vector operator+(const std::vector& a, const std::vector& b) +{ +// assert(a.size() == b.size()); + + if(a.empty()) // if a doesn't store any element it is the zero-vector, so return b + return b; + else if(b.empty()) // if b doesn't store any element it is the zero-vector, so return a + return a; + else if(a.size() != b.size()) // if the vectors are not the same size return a vector with -1 as entries as failure + return std::vector(100, -1); + + std::vector result; // create temporary new vector + result.reserve(a.size()); + + // add each element of the vectors and store them at the corresponding temporary vector position + std::transform(a.begin(), a.end(), b.begin(), + std::back_inserter(result), std::plus()); + + return result; +} + +// Overload of the += operator for vectors: +// Given vector a and b from arbitrary sizes this operator takes vector b and expands a by these elements. +template +std::vector& operator+=(std::vector& a, const std::vector& b) +{ + a.insert(a.end(), b.begin(), b.end()); + return a; +} + +class VoronoiRandomFieldSegmentation : public AbstractVoronoiSegmentation +{ +protected: + + std::vector angles_for_simulation_; // Vector that saves the angles, used to simulate the laser measurements + // for the AdaBoost classifier. + + int number_of_classifiers_; // Number of weak classifiers used from the OpenCV AdaBoost function. + + int number_of_classes_; // Number of classes this algorithm can detect. + + bool trained_boost_, trained_conditional_field_; // Variable that shows if the classifiers has already been trained. + +#if CV_MAJOR_VERSION == 2 + CvBoostParams params_; // Parameters for the classifiers + CvBoost room_boost_, hallway_boost_, doorway_boost_; // The AdaBoost-Classifier to induct the features needed in the conditional random field. +#else + cv::Ptr room_boost_, hallway_boost_, doorway_boost_; // The AdaBoost-Classifier to induct the features needed in the conditional random field. +#endif + + std::vector trained_conditional_weights_; // The weights that are needed for the feature-induction in the conditional random field. + + // Function to check if the given point is more far away from each point in the given set than the min_distance. + bool pointMoreFarAway(const std::set& points, const cv::Point& point, const double min_distance); + + std::vector raycasting(const cv::Mat& map, const cv::Point& location); + + // Function to get all possible configurations for n variables that each can have m labels. E.g. with 2 variables and 3 possible + // labels for each variable there are 9 different configurations. + void getPossibleConfigurations(std::vector >& possible_configurations, const std::vector& possible_labels, + const uint number_of_variables); + + // Function that swaps the label-configurations of CRF-nodes in a way s.t. the nodes are sorted in increasing order. Needed + // to use OpenGM for inference later. + void swapConfigsRegardingNodeIndices(std::vector >& configurations, size_t point_indices[]); + + // Function to calculate the feature vector for a given clique, using the trained AdaBoost classifiers. + void getAdaBoostFeatureVector(std::vector& feature_vector, Clique& clique, + std::vector& given_labels, std::vector& possible_labels); + + // Function that takes a map and draws a pruned voronoi graph in it. + void createPrunedVoronoiGraph(cv::Mat& map_for_voronoi_generation, std::set& node_points); + + // Function to find the Nodes for the conditional random field, given a voronoi-graph. + void findConditonalNodes(std::set& conditional_nodes, const cv::Mat& voronoi_map, + const cv::Mat& distance_map, const std::set& voronoi_nodes, + const int epsilon_for_neighborhood, const int max_iterations, const int min_neighborhood_size, + const double min_node_distance); + + // Function to create a conditional random field out of given points. It needs + // the voronoi-map extracted from the original map to find the neighbors for each point + // and the voronoi-node-points to add the right points as nodes. + void createConditionalField(const cv::Mat& voronoi_map, const std::set& node_points, + std::vector& conditional_random_field_cliques, const std::set& voronoi_node_points, + const cv::Mat& original_map); + + // Function that takes all given training maps and calculates the AdaBoost-Classifiers for them to best label a + // room, hallway and doorway. + void trainBoostClassifiers(const std::vector& training_maps, + std::vector< std::vector >& cliques_of_training_maps, const std::vector possible_labels, + const std::string& classifier_storage_path); // Function to train the AdaBoost classifiers, used for feature induction of the conditional + // random field. + + // Function to find the weights used to calculate the clique potentials. + void findConditionalWeights(std::vector< std::vector >& conditional_random_field_cliques, + std::vector >& random_field_node_points, const std::vector& training_maps, + std::vector& possible_labels, const std::string weights_filepath); + + +public: + // Constructor + VoronoiRandomFieldSegmentation(); + + // This function is used to train the algorithm. The above defined functions separately train the AdaBoost-classifiers and + // the conditional random field. By calling this function the training is done in the right order, because the AdaBoost-classifiers + // need to be trained to calculate features for the conditional random field. + void trainAlgorithms(const std::vector& original_maps, const std::vector& training_maps, + std::vector& voronoi_maps, const std::vector& voronoi_node_maps, + std::vector& possible_labels, const std::string storage_path, + const int epsilon_for_neighborhood, const int max_iterations, const int min_neighborhood_size, + const double min_node_distance); + + // This function is called to find minimal values of a defined log-likelihood-function using the library Dlib. + // This log-likelihood-function is made over all training data to get a likelihood-estimation linear in the weights. + // By minimizing this function the best weights are chosen, what is done here. See voronoi_random_field_segmentation.cpp at + // the beginning for detailed information. + // !!!!Important: The more training maps you have, the more factors appear in the log-likelihood over all maps. Be sure not to + // use too much training-maps, because then the log-likelihood-function easily produces values that are out of the + // double range, which Dlib can't handle. + column_vector findMinValue(unsigned int number_of_weights, double sigma, + const std::vector >& likelihood_parameters, const std::vector& starting_weights); // Function to find the minimal value of a function. Used to find the optimal weights for + // the conditional random field. + + // Function to segment a given map into different regions. It uses the above trained AdaBoost-classifiers and conditional-random-field. + // Also it uses OpenGM to do a inference in the created crf, so it uses the above defined typedefs. + void segmentMap(const cv::Mat& original_map, cv::Mat& segmented_map, const int epsilon_for_neighborhood, + const int max_iterations, const int min_neighborhood_size, std::vector& possible_labels, + const double min_node_distance, bool show_results, + const std::string classifier_storage_path, const std::string classifier_default_path, const int max_inference_iterations, + double map_resolution_from_subscription, double room_area_factor_lower_limit, double room_area_factor_upper_limit, + double max_area_for_merging, std::vector* door_points = NULL); + + // Function used to test several features separately. Not relevant. + void testFunc(const cv::Mat& original_map); + +}; diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/include/ipa_room_segmentation/voronoi_segmentation.h b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/include/ipa_room_segmentation/voronoi_segmentation.h new file mode 100644 index 0000000..635b18c --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/include/ipa_room_segmentation/voronoi_segmentation.h @@ -0,0 +1,27 @@ +#include "ros/ros.h" +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + + +class VoronoiSegmentation : public AbstractVoronoiSegmentation +{ +protected: + +public: + + VoronoiSegmentation(); + + //the segmentation-algorithm + void segmentMap(const cv::Mat& map_to_be_labeled, cv::Mat& segmented_map, double map_resolution_from_subscription, + double room_area_factor_lower_limit, double room_area_factor_upper_limit, int neighborhood_index, int max_iterations, + double min_critical_point_distance_factor, double max_area_for_merging, bool display_map=false); +}; diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/include/ipa_room_segmentation/wavefront_region_growing.h b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/include/ipa_room_segmentation/wavefront_region_growing.h new file mode 100644 index 0000000..b1a8c20 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/include/ipa_room_segmentation/wavefront_region_growing.h @@ -0,0 +1,8 @@ +#include +#include +#include +#include +#include +#include + +void wavefrontRegionGrowing(cv::Mat& image); diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/src/abstract_voronoi_segmentation.cpp b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/src/abstract_voronoi_segmentation.cpp new file mode 100644 index 0000000..d1cfc42 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/src/abstract_voronoi_segmentation.cpp @@ -0,0 +1,463 @@ +#include + +#include +#include + +#include +#include + + + +AbstractVoronoiSegmentation::AbstractVoronoiSegmentation() +{ + +} + +bool AbstractVoronoiSegmentation::determineRoomIndexFromRoomID(const std::vector& rooms, const int room_id, size_t& room_index) +{ + bool found_id = false; + for (size_t r = 0; r < rooms.size(); r++) + { + if (rooms[r].getID() == room_id) + { + room_index = r; + found_id = true; + break; + } + } + + return found_id; +} + +void AbstractVoronoiSegmentation::mergeRoomPair(std::vector& rooms, const int target_index, const int room_to_merge_index, cv::Mat& segmented_map, const double map_resolution) +{ + // integrate room to merge into target and delete merged room + const int target_id = rooms[target_index].getID(); + const int room_to_merge_id = rooms[room_to_merge_index].getID(); + rooms[target_index].mergeRoom(rooms[room_to_merge_index], map_resolution); + rooms[room_to_merge_index].setRoomId(rooms[target_index].getID(), segmented_map); + rooms.erase(rooms.begin()+room_to_merge_index); + std::sort(rooms.begin(), rooms.end(), sortRoomsAscending); + + // update neighborhood statistics for remaining rooms + for (size_t i=0; i& neighbor_ids = rooms[i].getNeighborIDs(); + std::vector::iterator it = std::find(neighbor_ids.begin(), neighbor_ids.end(), room_to_merge_id); + if (it != neighbor_ids.end()) + { + std::vector::iterator it2 = std::find(neighbor_ids.begin(), neighbor_ids.end(), target_id); + if (it2 != neighbor_ids.end()) + neighbor_ids.erase(it); + else + *it = target_id; + } + + std::map& neighbor_statistics = rooms[i].getNeighborStatistics(); + std::map::iterator it3 = neighbor_statistics.find(room_to_merge_id); + if (it3 != neighbor_statistics.end()) + { + std::map::iterator it4 = neighbor_statistics.find(target_id); + if (it4 != neighbor_statistics.end()) + it4->second += it3->second; + else + neighbor_statistics[target_id] = it3->second; + neighbor_statistics.erase(it3); + } + } +} + +void AbstractVoronoiSegmentation::drawVoronoi(cv::Mat &img, const std::vector >& facets_of_voronoi, const cv::Scalar voronoi_color, const cv::Mat& eroded_map) +{ + // go trough each facet of the calculated Voronoi-graph and check if it should be drawn. + for (std::vector >::const_iterator current_contour = facets_of_voronoi.begin(); current_contour != facets_of_voronoi.end(); ++current_contour) + { + // saving-variable for the last Point that has been looked at + cv::Point2f last_point = current_contour->back(); + // draw each line of the voronoi-cell + for (size_t c = 0; c < current_contour->size(); ++c) + { + // variable to check, whether a Point is inside a white area or not + bool inside = true; + cv::Point2f current_point = current_contour->at(c); + // only draw lines that are inside the map-contour + if (((int)current_point.x<0) || ((int)current_point.x >= eroded_map.cols) || + ((int)current_point.y<0) || ((int)current_point.y >= eroded_map.rows) || + eroded_map.at((int)current_point.y, (int)current_point.x) == 0 || + ((int)last_point.x<0) || ((int)last_point.x >= eroded_map.cols) || + ((int)last_point.y<0) || ((int)last_point.y >= eroded_map.rows) || + eroded_map.at((int)last_point.y, (int)last_point.x) == 0) + inside = false; + if (inside) + { + cv::line(img, last_point, current_point, voronoi_color, 1); + } + last_point = current_point; + } + } +} + +//****************Create the Generalized Voronoi-Diagram********************** +// This function is here to create the generalized voronoi-graph in the given map. It does following steps: +// 1. It finds every discretized contour in the given map (they are saved as vector). Then it takes these +// contour-Points and adds them to the OpenCV Delaunay generator from which the voronoi-cells can be generated. +// 2. Finally it gets the boundary-Points of the voronoi-cells with getVoronoiFacetList. It takes these facets +// and draws them using the drawVoronoi function. This function draws the facets that only have Points inside +// the map-contour (other lines go to not-reachable places and are not necessary to be looked at). +// 3. It returns the map that has the generalized voronoi-graph drawn in. +void AbstractVoronoiSegmentation::createVoronoiGraph(cv::Mat& map_for_voronoi_generation) +{ + cv::Mat map_to_draw_voronoi_in = map_for_voronoi_generation.clone(); //variable to save the given map for drawing in the voronoi-diagram + + cv::Mat temporary_map_to_calculate_voronoi = map_for_voronoi_generation.clone(); //variable to save the given map in the createVoronoiGraph-function + + //apply a closing-operator on the map so bad parts are neglected + cv::erode(temporary_map_to_calculate_voronoi, temporary_map_to_calculate_voronoi, cv::Mat()); + cv::dilate(temporary_map_to_calculate_voronoi, temporary_map_to_calculate_voronoi, cv::Mat()); + + //********************1. Get OpenCV delaunay-traingulation****************************** + cv::Rect rect(0, 0, map_to_draw_voronoi_in.cols, map_to_draw_voronoi_in.rows); //variables to generate the voronoi-diagram, using OpenCVs delaunay-triangulation + cv::Subdiv2D subdiv(rect); + std::vector > hole_contours; //variable to save the hole-contours (= black holes inside the white map) + std::vector > contours; //variables for contour extraction and discretisation + //hierarchy saves if the contours are hole-contours: + //hierarchy[{0,1,2,3}]={next contour (same level), previous contour (same level), child contour, parent contour} + //child-contour = 1 if it has one, = -1 if not, same for parent_contour + std::vector hierarchy; + + //get contours of the map + cv::Mat temp = map_to_draw_voronoi_in.clone(); +#if CV_MAJOR_VERSION<=3 + cv::findContours(temp, contours, hierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_NONE); + cv::drawContours(map_to_draw_voronoi_in, contours, -1, cv::Scalar(255), CV_FILLED); +#else + cv::findContours(temp, contours, hierarchy, cv::RETR_CCOMP, cv::CHAIN_APPROX_NONE); + cv::drawContours(map_to_draw_voronoi_in, contours, -1, cv::Scalar(255), cv::FILLED); +#endif + + //put every point of the map-contours into the Delaunay-generator of OpenCV + for (int current_contour = 0; current_contour < contours.size(); current_contour++) + { + for (int current_point = 0; current_point < contours[current_contour].size(); current_point++) + { + cv::Point fp = contours[current_contour][current_point]; + subdiv.insert(fp); + } + //get the contours of the black holes --> it is necessary to check if points are inside these in drawVoronoi + if (hierarchy[current_contour][2] == -1 && hierarchy[current_contour][3] != -1) + { + hole_contours.push_back(contours[current_contour]); + } + } + + //********************2. Get facets and draw voronoi-Graph****************************** + //erode the map so that points near the boundary are not drawn later (see drawVoronoi) + cv::Mat eroded_map; + cv::Point anchor(-1, -1); + cv::erode(temporary_map_to_calculate_voronoi, eroded_map, cv::Mat(), anchor, 2); + + //get the Voronoi regions from the delaunay-subdivision graph + const cv::Scalar voronoi_color(127); //define the voronoi-drawing colour + std::vector > voronoi_facets; // variables to find the facets and centers of the voronoi-cells + std::vector voronoi_centers; + subdiv.getVoronoiFacetList(std::vector(), voronoi_facets, voronoi_centers); + + //draw the voronoi-regions into the map + drawVoronoi(map_to_draw_voronoi_in, voronoi_facets, voronoi_color, eroded_map); + + //make pixels black, which were black before and were colored by the voronoi-regions + for (int v = 0; v < map_to_draw_voronoi_in.rows; v++) + { + for (int u = 0; u < map_to_draw_voronoi_in.cols; u++) + { + if (map_for_voronoi_generation.at(v, u) == 0) + { + map_to_draw_voronoi_in.at(v, u) = 0; + } + } + } + map_for_voronoi_generation = map_to_draw_voronoi_in; +} + +void AbstractVoronoiSegmentation::pruneVoronoiGraph(cv::Mat& voronoi_map, std::set& node_points) +{ + // 1.extract the node-points that have at least three neighbors on the voronoi diagram + // node-points are points on the voronoi-graph that have at least 3 neighbors + for (int v = 1; v < voronoi_map.rows-1; v++) + { + for (int u = 1; u < voronoi_map.cols-1; u++) + { + if (voronoi_map.at(v, u) == 127) + { + int neighbor_count = 0; // variable to save the number of neighbors for each point + // check 3x3 region around current pixel + for (int row_counter = -1; row_counter <= 1; row_counter++) + { + for (int column_counter = -1; column_counter <= 1; column_counter++) + { + // don't check the point itself + if (row_counter == 0 && column_counter == 0) + continue; + + //check if neighbors are colored with the voronoi-color + if (voronoi_map.at(v + row_counter, u + column_counter) == 127) + { + neighbor_count++; + } + } + } + if (neighbor_count > 2) + { + node_points.insert(cv::Point(u,v)); + } + } + } + } + + // 2.reduce the side-lines along the voronoi-graph by checking if it has only one neighbor until a node-point is reached + // --> make it white + // repeat a large enough number of times so the graph converges + for (int step = 0; step < 100; step++) + { + for (int v = 0; v < voronoi_map.rows; v++) + { + for (int u = 0; u < voronoi_map.cols; u++) + { + // set that the point is a point along the graph and not a side-line + if (voronoi_map.at(v, u) == 127) + { + int neighbor_count = 0; //variable to save the number of neighbors for each point + for (int row_counter = -1; row_counter <= 1; row_counter++) + { + for (int column_counter = -1; column_counter <= 1; column_counter++) + { + // don't check the point itself + if (row_counter == 0 && column_counter == 0) + continue; + + // check the surrounding points + const int nv = v + row_counter; + const int nu = u + column_counter; + if (nv >= 0 && nu >= 0 && nv < voronoi_map.rows && nu < voronoi_map.cols && voronoi_map.at(nv, nu) == 127) + { + neighbor_count++; + } + } + } + //if the current point is a node point found in the previous step, it belongs to the voronoi-graph + if (neighbor_count <= 1 && node_points.find(cv::Point(u,v)) == node_points.end()) + { + //if the Point isn't on the voronoi-graph make it white + voronoi_map.at(v, u) = 255; + } + } + } + } + } +} + +void AbstractVoronoiSegmentation::mergeRooms(cv::Mat& map_to_merge_rooms, std::vector& rooms, double map_resolution_from_subscription, double max_area_for_merging, bool display_map) +{ + // This function takes the segmented Map from the original Voronoi-segmentation-algorithm and merges rooms together, + // that are small enough and have only two or one neighbor. + + // 1. go trough every pixel and add points to the rooms with the same ID + for (int y = 0; y < map_to_merge_rooms.rows; y++) + { + for (int x = 0; x < map_to_merge_rooms.cols; x++) + { + int current_id = map_to_merge_rooms.at(y, x); + if (current_id != 0) + { + for (size_t current_room = 0; current_room < rooms.size(); current_room++) //add the Points with the same Id as a room to it + { + if (rooms[current_room].getID() == current_id) //insert the current point into the corresponding room + { + rooms[current_room].insertMemberPoint(cv::Point(x, y), map_resolution_from_subscription); + break; + } + } + } + } + } + + // 2. add the neighbor IDs for every point + for (int current_room = 0; current_room < rooms.size(); current_room++) + { + const int current_id = rooms[current_room].getID(); + std::vector considered_neighbors; // storage for already counted neighborhood points + const std::vector& current_points = rooms[current_room].getMembers(); + for (int current_point = 0; current_point < current_points.size(); current_point++) + { + for (int row_counter = -1; row_counter <= 1; row_counter++) + { + for (int col_counter = -1; col_counter <= 1; col_counter++) + { + const int label = map_to_merge_rooms.at(current_points[current_point].y + row_counter, current_points[current_point].x + col_counter); + + // collect neighbor IDs + if (label != 0 && label != current_id) + rooms[current_room].addNeighborID(label); + + // neighborhood statistics + cv::Point neighbor_point(current_points[current_point].x + col_counter, current_points[current_point].y + row_counter); + if (!contains(considered_neighbors, neighbor_point) && label != current_id) + { + rooms[current_room].addNeighbor(label); + considered_neighbors.push_back(neighbor_point); + } + } + } + } + } + + // 3. merge criteria + // sort rooms ascending by area + std::sort(rooms.begin(), rooms.end(), sortRoomsAscending); + // a) rooms with one neighbor and max. 75% walls around + for (int current_room_index = 0; current_room_index < rooms.size(); ) + { + Room& current_room = rooms[current_room_index]; + bool merge_rooms = false; + size_t merge_index = 0; + + if (current_room.getNeighborCount() == 1 && current_room.getArea() < max_area_for_merging && current_room.getWallToPerimeterRatio() <= 0.75) + { + // check every room if it should be merged with its neighbor that it shares the most boundary with + merge_rooms = determineRoomIndexFromRoomID(rooms, current_room.getNeighborWithLargestCommonBorder(), merge_index); + } + + if (merge_rooms == true) + { + //std::cout << "merge " << current_room.getCenter() << ", id=" << current_room.getID() << " into " << rooms[merge_index].getCenter() << ", id=" << rooms[merge_index].getID() << std::endl; + mergeRoomPair(rooms, merge_index, current_room_index, map_to_merge_rooms, map_resolution_from_subscription); + current_room_index = 0; + } + else + current_room_index++; + } + if (display_map == true) + cv::imshow("a", map_to_merge_rooms); + + // b) small rooms + for (int current_room_index = 0; current_room_index < rooms.size(); ) + { + Room& current_room = rooms[current_room_index]; + bool merge_rooms = false; + size_t merge_index = 0; + + const int max_border_neighbor_id = current_room.getNeighborWithLargestCommonBorder(); + if (current_room.getArea() < 2.0 && (double)current_room.getNeighborStatistics()[max_border_neighbor_id]/current_room.getPerimeter() > 0.2) + { + // merge with that neighbor that shares the most neighboring pixels + merge_rooms = determineRoomIndexFromRoomID(rooms, max_border_neighbor_id, merge_index); + if ((double)rooms[merge_index].getWallToPerimeterRatio() > 0.8) //0.8 + merge_rooms = false; + } + + if (merge_rooms == true) + { + //std::cout << "merge " << current_room.getCenter() << ", id=" << current_room.getID() << " into " << rooms[merge_index].getCenter() << ", id=" << rooms[merge_index].getID() << std::endl; + mergeRoomPair(rooms, merge_index, current_room_index, map_to_merge_rooms, map_resolution_from_subscription); + current_room_index = 0; + } + else + current_room_index++; + } + if (display_map == true) + cv::imshow("b", map_to_merge_rooms); + + // c) merge a room with one neighbor that has max. 2 neighbors and sufficient wall ratio (connect parts inside a room) + for (int current_room_index = 0; current_room_index < rooms.size(); ) + { + Room& current_room = rooms[current_room_index]; + bool merge_rooms = false; + size_t merge_index = 0; + + // merge a room with one neighbor that has max. 2 neighbors and sufficient wall ratio (connect parts inside a room) + const int max_border_neighbor_id = current_room.getNeighborWithLargestCommonBorder(); + if ((current_room.getNeighborCount()==1 || current_room.getPerimeterRatioOfXLargestRooms(1)>0.98) && current_room.getWallToPerimeterRatio() > 0.5 && + (double)current_room.getNeighborStatistics()[max_border_neighbor_id]/current_room.getPerimeter() > 0.15) + { + // merge with that neighbor that shares the most neighboring pixels + merge_rooms = determineRoomIndexFromRoomID(rooms, max_border_neighbor_id, merge_index); + if (rooms[merge_index].getNeighborCount() > 2 && rooms[merge_index].getPerimeterRatioOfXLargestRooms(2)<0.95) // || rooms[merge_index].getWallToPerimeterRatio() < 0.4) + merge_rooms = false; + } + + if (merge_rooms == true) + { + //std::cout << "merge " << current_room.getCenter() << ", id=" << current_room.getID() << " into " << rooms[merge_index].getCenter() << ", id=" << rooms[merge_index].getID() << std::endl; + mergeRoomPair(rooms, merge_index, current_room_index, map_to_merge_rooms, map_resolution_from_subscription); + current_room_index = 0; + } + else + current_room_index++; + } + if (display_map == true) + cv::imshow("c", map_to_merge_rooms); + + // d) merge rooms that share a significant part of their perimeter + for (int current_room_index = 0; current_room_index < rooms.size(); ) + { + Room& current_room = rooms[current_room_index]; + bool merge_rooms = false; + size_t merge_index = 0; + + std::map< int,int,std::greater > neighbor_room_statistics_inverse; // common border length, room_id + current_room.getNeighborStatisticsInverse(neighbor_room_statistics_inverse); +// std::vector& neighbor_ids = current_room.getNeighborIDs(); +// for (size_t n=0; n >::iterator it=neighbor_room_statistics_inverse.begin(); it!=neighbor_room_statistics_inverse.end(); ++it) + { + if (it->second==0) + continue; // skip wall + + const double neighbor_border_ratio = (double)current_room.getNeighborStatistics()[it->second]/current_room.getPerimeter(); + if (neighbor_border_ratio > 0.2 || (neighbor_border_ratio > 0.1 && current_room.getWallToPerimeterRatio() > (1-2*neighbor_border_ratio-0.05) && current_room.getWallToPerimeterRatio() < (1-neighbor_border_ratio))) + { + // merge with that neighbor that shares the most neighboring pixels + merge_rooms = determineRoomIndexFromRoomID(rooms, it->second, merge_index); + if ((double)rooms[merge_index].getNeighborStatistics()[current_room.getID()]/rooms[merge_index].getPerimeter() <= 0.1) + merge_rooms = false; + if (merge_rooms == true) + break; + } + } + + if (merge_rooms == true) + { + //std::cout << "merge " << current_room.getCenter() << ", id=" << current_room.getID() << " into " << rooms[merge_index].getCenter() << ", id=" << rooms[merge_index].getID() << std::endl; + mergeRoomPair(rooms, merge_index, current_room_index, map_to_merge_rooms, map_resolution_from_subscription); + current_room_index = 0; + } + else + current_room_index++; + } + if (display_map == true) + cv::imshow("d", map_to_merge_rooms); + + // e) largest room neighbor touches > 0.5 perimeter (happens often with furniture) + for (int current_room_index = 0; current_room_index < rooms.size(); ) + { + Room& current_room = rooms[current_room_index]; + bool merge_rooms = false; + size_t merge_index = 0; + + const int max_border_neighbor_id = current_room.getNeighborWithLargestCommonBorder(); + if ((double)current_room.getNeighborStatistics()[max_border_neighbor_id]/current_room.getPerimeter() > 0.4) + { + // merge with that neighbor that shares the most neighboring pixels + merge_rooms = determineRoomIndexFromRoomID(rooms, max_border_neighbor_id, merge_index); + } + + if (merge_rooms == true) + { + //std::cout << "merge " << current_room.getCenter() << ", id=" << current_room.getID() << " into " << rooms[merge_index].getCenter() << ", id=" << rooms[merge_index].getID() << std::endl; + mergeRoomPair(rooms, merge_index, current_room_index, map_to_merge_rooms, map_resolution_from_subscription); + current_room_index = 0; + } + else + current_room_index++; + } +} diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/src/adaboost_classifier.cpp b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/src/adaboost_classifier.cpp new file mode 100644 index 0000000..94071f3 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/src/adaboost_classifier.cpp @@ -0,0 +1,512 @@ +#include + +#include +#include +#include + +#include + +#include + +AdaboostClassifier::AdaboostClassifier() +{ + //save the angles between the simulated beams, used in the following algorithm + for (double angle = 0; angle < 360; angle++) + { + angles_for_simulation_.push_back(angle); + } +#if CV_MAJOR_VERSION == 2 + // Set up boosting parameters + CvBoostParams params(CvBoost::DISCRETE, 350, 0, 2, false, 0); + params_ = params; +#endif + trained_ = false; +} + +void AdaboostClassifier::trainClassifiers(const std::vector& room_training_maps, const std::vector& hallway_training_maps, + const std::string& classifier_storage_path, bool load_features) +{ + //**************************Training-Algorithm for the AdaBoost-classifiers***************************** + //This Alogrithm trains two AdaBoost-classifiers from OpenCV. It takes the given training maps and finds the Points + //that are labeled as a room/hallway and calculates the features defined in ipa_room_segmentation/features.h. + //Then these vectors are put in a format that OpenCV expects for the classifiers and then they are trained. + std::vector labels_for_hallways, labels_for_rooms; + std::vector > hallway_features, room_features; + std::vector temporary_beams; + std::vector temporary_features; + cv::Mat hallway_labels_mat, room_labels_mat; + cv::Mat hallway_features_mat, room_features_mat; + std::cout << "Starting to train the algorithm." << std::endl; + std::cout << "number of room training maps: " << room_training_maps.size() << std::endl; + std::cout << "number of hallway training maps: " << hallway_training_maps.size() << std::endl; + //Get the labels for every training point. 1.0 means it belongs to a room and -1.0 means it belongs to a hallway + // if not loading precomputed features, compute them here + if(load_features==false) + { + LaserScannerFeatures lsf; + for(size_t map = 0; map < room_training_maps.size(); ++map) + { + for (int y = 0; y < room_training_maps[map].rows; y++) + { + for (int x = 0; x < room_training_maps[map].cols; x++) + { + if (room_training_maps[map].at(y, x) != 0) + { + //check for label of each Pixel (if it belongs to rooms the label is 1, otherwise it is -1) + if (room_training_maps[map].at(y, x) > 250) + { + labels_for_rooms.push_back(-1); + } + else + { + labels_for_rooms.push_back(1); + } + //simulate the beams and features for every position and save it + raycasting_.raycasting(room_training_maps[map], cv::Point(x, y), temporary_beams); + cv::Mat features; + lsf.get_features(temporary_beams, angles_for_simulation_, cv::Point(x, y), features); + temporary_features.resize(features.cols); + for (int i=0; i(0,i); + room_features.push_back(temporary_features); + temporary_features.clear(); + } + } + } + std::cout << "done one room map" << std::endl; + } + + for(size_t map = 0; map < hallway_training_maps.size(); ++map) + { + for (int y = 0; y < hallway_training_maps[map].rows; y++) + { + for (int x = 0; x < hallway_training_maps[map].cols; x++) + { + if (hallway_training_maps[map].at(y, x) != 0) + { + //check for label of each Pixel (if it belongs to hallways the label is 1, otherwise it is -1) + if (hallway_training_maps[map].at(y, x) > 250) + { + labels_for_hallways.push_back(-1); + } + else + { + labels_for_hallways.push_back(1); + } + //simulate the beams and features for every position and save it + raycasting_.raycasting(hallway_training_maps[map], cv::Point(x, y), temporary_beams); + cv::Mat features; + lsf.get_features(temporary_beams, angles_for_simulation_, cv::Point(x, y), features); + temporary_features.resize(features.cols); + for (int i=0; i(0,i); + hallway_features.push_back(temporary_features); + temporary_features.clear(); + } + } + } + std::cout << "done one hallway map" << std::endl; + } + + //save the found labels and features in Matrices --> hallway + hallway_labels_mat = cv::Mat(labels_for_hallways); + hallway_features_mat = cv::Mat(hallway_features.size(), lsf.get_feature_count(), CV_32FC1); + for (int i = 0; i < labels_for_hallways.size(); i++) + { + // hallway_labels_mat.at(i, 0) = labels_for_hallways[i]; + for (int f = 0; f < hallway_features[i].size(); f++) + { + hallway_features_mat.at(i, f) = (float) hallway_features[i][f]; + } + } + //save the found labels and features in Matrices --> rooms + room_labels_mat = cv::Mat(labels_for_rooms); + room_features_mat = cv::Mat(room_features.size(), lsf.get_feature_count(), CV_32FC1); + for (int i = 0; i < labels_for_rooms.size(); i++) + { + // room_labels_mat.at(i, 0) = labels_for_rooms[i]; + for (int f = 0; f < room_features[i].size(); f++) + { + room_features_mat.at(i, f) = (float) room_features[i][f]; + } + } + + // // save feature data to file + cv::FileStorage fs(classifier_storage_path+"_features.yml", cv::FileStorage::WRITE); + if (fs.isOpened()) + { + fs << "hallway_features_mat" << hallway_features_mat; + fs << "hallway_labels_mat" << hallway_labels_mat; + fs << "room_features_mat" << room_features_mat; + fs << "room_labels_mat" << room_labels_mat; + } + fs.release(); + } + else + { + // load the feature data from file + cv::FileStorage fs(classifier_storage_path+"_features.yml", cv::FileStorage::READ); + std::cout << "Loading feature data from file: " << classifier_storage_path+"_features.yml" << std::endl; + if (fs.isOpened()) + { + fs["hallway_features_mat"] >> hallway_features_mat; + fs["hallway_labels_mat"] >> hallway_labels_mat; + fs["room_features_mat"] >> room_features_mat; + fs["room_labels_mat"] >> room_labels_mat; + } + std::cout << "Loaded features data" << std::endl; + fs.release(); + } + + // check if path for storing classifier models exists + boost::filesystem::path storage_path(classifier_storage_path); + if (boost::filesystem::exists(storage_path) == false) + { + if (boost::filesystem::create_directories(storage_path) == false && boost::filesystem::exists(storage_path) == false) + { + std::cout << "Error: AdaboostClassifier::trainClassifiers: Could not create directory " << storage_path << std::endl; + return; + } + } + + //*********hallway*************** + std::string filename_hallway = classifier_storage_path + "semantic_hallway_boost.xml"; +#if CV_MAJOR_VERSION == 2 + // Train a boost classifier + hallway_boost_.train(hallway_features_mat, CV_ROW_SAMPLE, hallway_labels_mat, cv::Mat(), cv::Mat(), cv::Mat(), cv::Mat(), params_); + //save the trained booster + hallway_boost_.save(filename_hallway.c_str(), "boost"); +#else + // Train a boost classifier + hallway_boost_ = cv::ml::Boost::create(); + hallway_boost_->setBoostType(cv::ml::Boost::REAL); + hallway_boost_->setWeakCount(350); + hallway_boost_->setWeightTrimRate(0); + hallway_boost_->setMaxDepth(2); + hallway_boost_->setUseSurrogates(false); + hallway_boost_->setPriors(cv::Mat()); + hallway_boost_->train(hallway_features_mat, cv::ml::ROW_SAMPLE, hallway_labels_mat); + //save the trained booster + hallway_boost_->save(filename_hallway.c_str()); +#endif + ROS_INFO("Done hallway classifiers."); + + //*************room*************** + std::string filename_room = classifier_storage_path + "semantic_room_boost.xml"; +#if CV_MAJOR_VERSION == 2 + // Train a boost classifier + room_boost_.train(room_features_mat, CV_ROW_SAMPLE, room_labels_mat, cv::Mat(), cv::Mat(), cv::Mat(), cv::Mat(), params_); + //save the trained booster + room_boost_.save(filename_room.c_str(), "boost"); +#else + // Train a boost classifier + room_boost_ = cv::ml::Boost::create(); + room_boost_->setBoostType(cv::ml::Boost::REAL); + room_boost_->setWeakCount(350); + room_boost_->setWeightTrimRate(0); + room_boost_->setMaxDepth(2); + room_boost_->setUseSurrogates(false); + room_boost_->setPriors(cv::Mat()); + room_boost_->train(room_features_mat, cv::ml::ROW_SAMPLE, room_labels_mat); + //save the trained booster + room_boost_->save(filename_room.c_str()); +#endif + + //set the trained-variabel true, so the labeling-algorithm knows the classifiers have been trained already + trained_ = true; + ROS_INFO("Done room classifiers."); + ROS_INFO("Finished training the algorithm."); +} + +void AdaboostClassifier::segmentMap(const cv::Mat& map_to_be_labeled, cv::Mat& segmented_map, double map_resolution_from_subscription, + double room_area_factor_lower_limit, double room_area_factor_upper_limit, const std::string& classifier_storage_path, + const std::string& classifier_default_path, bool display_results) +{ + //******************Semantic-labeling function based on AdaBoost***************************** + //This function calculates single-valued features for every white Pixel in the given occupancy-gridmap and classifies it + //using the AdaBoost-algorithm from OpenCV. It does the following steps: + // I. If the classifiers hasn't been trained before they should load the training-results saved in the + // classifier_models folder + // II. Go trough each Pixel of the given map. If this Pixel is white simulate the laser-beams for it and calculate each + // of the implemented features. + // III. Apply a median-Filter on the labeled map to smooth the output of it. + // IV. Find the contours of the segments given by III. by thresholding the map. First set the threshold so high that + // only room-areas are shown in the map and find them. Then make these room-areas black and finally set the threshold + // a little lower than the hallway-colour. The function only takes contours that are larger than the minimum value + // and splits too large hallway-areas into smaller areas by putting random Points into the too large hallway contour + // and apply a watershed-algorithm on it. At last the saved room and hallway contours are drawn with a random + // colour into the map that hasn't been used already. + + cv::Mat original_map_to_be_labeled = map_to_be_labeled.clone(); + ROS_INFO("Starting to label the map."); + //***********************I. check if classifiers has already been trained***************************** + if (!trained_) //classifiers hasn't been trained before so they should be loaded + { + // check if path for storing classifier models exists + boost::filesystem::path storage_path(classifier_storage_path); + if (boost::filesystem::exists(storage_path) == false) + { + if (boost::filesystem::create_directories(storage_path) == false && boost::filesystem::exists(storage_path) == false) + { + std::cout << "Error: AdaboostClassifier::segmentMap: Could not create directory " << storage_path << std::endl; + return; + } + } + + std::string filename_room = classifier_storage_path + "semantic_room_boost.xml"; + std::string filename_room_default = classifier_default_path + "semantic_room_boost.xml"; + if (boost::filesystem::exists(boost::filesystem::path(filename_room)) == false) + boost::filesystem::copy_file(filename_room_default, filename_room); + loadBoost(room_boost_, filename_room); + + std::string filename_hallway = classifier_storage_path + "semantic_hallway_boost.xml"; + std::string filename_hallway_default = classifier_default_path + "semantic_hallway_boost.xml"; + if (boost::filesystem::exists(boost::filesystem::path(filename_hallway)) == false) + boost::filesystem::copy_file(filename_hallway_default, filename_hallway); + loadBoost(hallway_boost_,filename_hallway); + + trained_ = true; + ROS_INFO("Loaded training results."); + } + + //*************** II. Go trough each Point and label it as room or hallway.************************** +#pragma omp parallel for + for (int y = 0; y < original_map_to_be_labeled.rows; y++) + { + LaserScannerFeatures lsf; + for (int x = 0; x < original_map_to_be_labeled.cols; x++) + { + if (original_map_to_be_labeled.at(y, x) == 255) + { + std::vector temporary_beams; + raycasting_.raycasting(original_map_to_be_labeled, cv::Point(x, y), temporary_beams); + std::vector temporary_features; + cv::Mat features_mat; //OpenCV expects a 32-floating-point Matrix as feature input + lsf.get_features(temporary_beams, angles_for_simulation_, cv::Point(x, y), features_mat); + //classify each Point +#if CV_MAJOR_VERSION == 2 + float room_sum = room_boost_.predict(features_mat, cv::Mat(), cv::Range::all(), false, true); + float hallway_sum = hallway_boost_.predict(features_mat, cv::Mat(), cv::Range::all(), false, true); +#else + float room_sum = room_boost_->predict(features_mat, cv::noArray(), cv::ml::Boost::PREDICT_SUM); + float hallway_sum = hallway_boost_->predict(features_mat, cv::noArray(), cv::ml::Boost::PREDICT_SUM); +#endif + //get the certanity-values for each class (it shows the probability that it belongs to the given class) + double room_certanity = (std::exp((double) room_sum)) / (std::exp(-1 * (double) room_sum) + std::exp((double) room_sum)); + double hallway_certanity = (std::exp((double) hallway_certanity)) + / (std::exp(-1 * (double) hallway_certanity) + std::exp((double) hallway_certanity)); + //make a decision-list and check which class the Point belongs to + double probability_for_room = room_certanity; + double probability_for_hallway = hallway_certanity * (1.0 - probability_for_room); + if (probability_for_room > probability_for_hallway) + { + original_map_to_be_labeled.at(y, x) = 150; //label it as room + } + else + { + original_map_to_be_labeled.at(y, x) = 100; //label it as hallway + } + } + } + } + std::cout << "labeled all white pixels: " << std::endl; + //******************** III. Apply a median filter over the image to smooth the results.*************************** + cv::Mat temporary_map = original_map_to_be_labeled.clone(); + cv::medianBlur(temporary_map, temporary_map, 3); + std::cout << "blurred image" << std::endl; + + //make regions black, that have been black before + for (int x = 0; x < original_map_to_be_labeled.rows; x++) + { + for (int y = 0; y < original_map_to_be_labeled.cols; y++) + { + if (original_map_to_be_labeled.at(x, y) == 0) + { + temporary_map.at(x, y) = 0; + } + } + } +// cv::imshow("thresholded", temporary_map); +// cv::waitKey(); + if(display_results) + { + cv::imshow("classified", temporary_map); + cv::waitKey(); + } + cv::Mat blured_image_for_thresholding = temporary_map.clone(); + + //*********** IV. Fill the large enough rooms with a random color and split the hallways into smaller regions********* + std::vector > contours, temporary_contours, saved_room_contours, saved_hallway_contours; + //hierarchy saves if the contours are hole-contours: + //hierarchy[{0,1,2,3}]={next contour (same level), previous contour (same level), child contour, parent contour} + //child-contour = 1 if it has one, = -1 if not, same for parent_contour + std::vector < cv::Vec4i > hierarchy; + + std::vector < cv::Scalar > already_used_colors; //saving-vector for the already used coloures + + //find the contours, which are labeled as a room + cv::threshold(temporary_map, temporary_map, 120, 255, cv::THRESH_BINARY); //find rooms (value = 150) +#if CV_MAJOR_VERSION<=3 + cv::findContours(temporary_map, contours, hierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_NONE); + cv::drawContours(blured_image_for_thresholding, contours, -1, cv::Scalar(0), CV_FILLED); //make the found regions at the original map black, because they have been looked at already +#else + cv::findContours(temporary_map, contours, hierarchy, cv::RETR_CCOMP, cv::CHAIN_APPROX_NONE); + cv::drawContours(blured_image_for_thresholding, contours, -1, cv::Scalar(0), cv::FILLED); //make the found regions at the original map black, because they have been looked at already +#endif + + //only take rooms that are large enough and that are not a hole-contour + for (int c = 0; c < contours.size(); c++) + { + if (map_resolution_from_subscription * map_resolution_from_subscription * cv::contourArea(contours[c]) > room_area_factor_lower_limit + && hierarchy[c][3] != 1) + { + saved_room_contours.push_back(contours[c]); + } + } + //find the contours, which are labeled as a hallway + map_to_be_labeled.convertTo(segmented_map, CV_32SC1, 256, 0); // rescale to 32 int, 255 --> 255*256 = 65280 + temporary_map = blured_image_for_thresholding.clone(); + + cv::threshold(temporary_map, temporary_map, 90, 255, cv::THRESH_BINARY); //find hallways (value = 100) +#if CV_MAJOR_VERSION<=3 + cv::findContours(temporary_map, contours, hierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_NONE); +#else + cv::findContours(temporary_map, contours, hierarchy, cv::RETR_CCOMP, cv::CHAIN_APPROX_NONE); +#endif + //if the hallway-contours are too big split them into smaller regions, also don't take too small regions + for (int contour_counter = 0; contour_counter < contours.size(); contour_counter++) + { + if (map_resolution_from_subscription * map_resolution_from_subscription * cv::contourArea(contours[contour_counter]) > room_area_factor_upper_limit) + { + //Generate a black map to draw the hallway-contour in. Then use this map to ckeck if the generated random Points + // are inside the contour. + cv::Mat contour_Map = cv::Mat::zeros(temporary_map.rows, temporary_map.cols, CV_8UC1); +#if CV_MAJOR_VERSION<=3 + cv::drawContours(contour_Map, contours, contour_counter, cv::Scalar(255), CV_FILLED); +#else + cv::drawContours(contour_Map, contours, contour_counter, cv::Scalar(255), cv::FILLED); +#endif + cv::erode(contour_Map, contour_Map, cv::Mat(), cv::Point(-1,-1), 10); + //center-counter so enough centers could be found + int center_counter = 0; + //saving-vector for watershed centers + std::vector < cv::Point > temporary_watershed_centers; + //find enough random watershed centers that are inside the hallway-contour + do + { + int random_x = rand() % temporary_map.rows; + int random_y = rand() % temporary_map.cols; + if (contour_Map.at(random_y, random_x) == 255) + { + temporary_watershed_centers.push_back(cv::Point(random_x, random_y)); + center_counter++; + } + } while (center_counter <= (map_resolution_from_subscription * map_resolution_from_subscription * cv::contourArea(contours[contour_counter])) / 8); + cv::Mat temporary_Map_to_wavefront; + contour_Map.convertTo(temporary_Map_to_wavefront, CV_32SC1, 256, 0); + //draw the centers as white circles into a black map and give the center-map and the contour-map to the opencv watershed-algorithm + for (int current_center = 0; current_center < temporary_watershed_centers.size(); current_center++) + { + bool coloured = false; + do + { + cv::Scalar fill_colour(rand() % 52224 + 13056); + if (!contains(already_used_colors, fill_colour)) + { +#if CV_MAJOR_VERSION<=3 + cv::circle(temporary_Map_to_wavefront, temporary_watershed_centers[current_center], 2, fill_colour, CV_FILLED); +#else + cv::circle(temporary_Map_to_wavefront, temporary_watershed_centers[current_center], 2, fill_colour, cv::FILLED); +#endif + already_used_colors.push_back(fill_colour); + coloured = true; + } + } while (!coloured); + } + //make sure all previously black Pixels are still black + for (int x = 0; x < map_to_be_labeled.rows; x++) + { + for (int y = 0; y < map_to_be_labeled.cols; y++) + { + if (map_to_be_labeled.at(x, y) == 0) + { + temporary_Map_to_wavefront.at(x, y) = 0; + } + } + } + wavefrontRegionGrowing(temporary_Map_to_wavefront); + //draw the seperated contour into the map, which should be labeled + for (int row = 0; row < segmented_map.rows; row++) + { + for (int col = 0; col < segmented_map.cols; col++) + { + if (temporary_Map_to_wavefront.at(row, col) != 0) + { + segmented_map.at(row, col) = temporary_Map_to_wavefront.at(row, col); + } + } + } + } + else if (map_resolution_from_subscription * map_resolution_from_subscription * cv::contourArea(contours[contour_counter]) + > room_area_factor_lower_limit) + { + saved_hallway_contours.push_back(contours[contour_counter]); + } + } + std::cout << "finished too big hallway contours" << std::endl; + //draw every room and lasting hallway contour with a random colour into the map + for (int room = 0; room < saved_room_contours.size(); room++) + { + bool coloured = false; + do + { + cv::Scalar fill_colour(rand() % 52224 + 13056); + if (!contains(already_used_colors, fill_colour)) + { +#if CV_MAJOR_VERSION<=3 + cv::drawContours(segmented_map, saved_room_contours, room, fill_colour, CV_FILLED); +#else + cv::drawContours(segmented_map, saved_room_contours, room, fill_colour, cv::FILLED); +#endif + already_used_colors.push_back(fill_colour); + coloured = true; + } + } while (!coloured); + } + std::cout << "finished room contours" << std::endl; + for (int hallway = 0; hallway < saved_hallway_contours.size(); hallway++) + { + bool coloured = false; + int loop_counter = 0; //loop-counter to exit the loop if it gets a infite loop + do + { + loop_counter++; + cv::Scalar fill_colour(rand() % 52224 + 13056); + if (!contains(already_used_colors, fill_colour) || loop_counter > 250) + { +#if CV_MAJOR_VERSION<=3 + cv::drawContours(segmented_map, saved_hallway_contours, hallway, fill_colour, CV_FILLED); +#else + cv::drawContours(segmented_map, saved_hallway_contours, hallway, fill_colour, cv::FILLED); +#endif + already_used_colors.push_back(fill_colour); + coloured = true; + } + } while (!coloured); + } + std::cout << "finished small hallway contours" << std::endl; + //spread the coloured regions to regions, which were too small and aren't drawn into the map + wavefrontRegionGrowing(segmented_map); + //make sure previously black pixels are still black + for (int v = 0; v < map_to_be_labeled.rows; ++v) + { + for (int u = 0; u < map_to_be_labeled.cols; ++u) + { + if (map_to_be_labeled.at(v, u) == 0) + { + segmented_map.at(v, u) = 0; + } + } + } + ROS_INFO("Finished Labeling the map."); +} diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/src/clique_class.cpp b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/src/clique_class.cpp new file mode 100644 index 0000000..6dba2c5 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/src/clique_class.cpp @@ -0,0 +1,74 @@ +#include + +// +// This class is used to easily create cliques that are subgraphs of a large graph.A Clique is a subgraph of this graph, in which +// all nodes are connected to each other. See the Header for further information. +// + +// default constructor +Clique::Clique() +{ + +} + +// constructor if one member is known +Clique::Clique(cv::Point first_member) +{ + member_points_.push_back(first_member); +} + +// constructor if a couple of members are known +Clique::Clique(std::vector members) +{ + for(size_t member = 0; member < members.size(); ++member) + { + member_points_.push_back(members[member]); + } +} + +// function that returns a vector containing all member points +std::vector Clique::getMemberPoints() +{ + return member_points_; +} + +// function that inserts a single point as a new member +void Clique::insertMember(cv::Point& new_member) +{ + if(contains(member_points_, new_member) == false) + member_points_.push_back(new_member); +} + +// function that inserts multiple points as new members +void Clique::insertMember(std::vector& new_members) +{ + for(std::vector::iterator new_member = new_members.begin(); new_member != new_members.end(); ++new_member) + { + if(contains(member_points_, *new_member) == false) + member_points_.push_back(*new_member); + } +} + +// function to check if a given point is part of the clique +bool Clique::containsMember(const cv::Point& point) +{ + return contains(member_points_, point); +} + +// function that returns the number of members of this clique +unsigned int Clique::getNumberOfMembers() +{ + return member_points_.size(); +} + +// function to save the given beams in the class parameter +void Clique::setBeamsForMembers(const std::vector< std::vector > beams) +{ + beams_for_members_ = beams ; +} + +// function that returns the stored laser-beams +std::vector< std::vector > Clique::getBeams() +{ + return beams_for_members_; +} diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/src/contains.cpp b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/src/contains.cpp new file mode 100644 index 0000000..9f46b8b --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/src/contains.cpp @@ -0,0 +1,53 @@ +#include + +bool contains(std::vector vector, cv::Scalar element) +{ + //this functions checks, if the given element is in the given vector (in this case for cv::Sclar elements) + if (!vector.empty()) + { + return vector.end() != std::find(vector.begin(), vector.end(), element); + } + else + { + return false; + } +} + +bool contains(std::vector vector, cv::Point element) +{ + //this functions checks, if the given element is in the given vector (in this case for cv::Point elements) + if (!vector.empty()) + { + return vector.end() != std::find(vector.begin(), vector.end(), element); + } + else + { + return false; + } +} + +bool contains(std::vector vector, int element) +{ + //this functions checks, if the given element is in the given vector (in this case for int elements) + if (!vector.empty()) + { + return vector.end() != std::find(vector.begin(), vector.end(), element); + } + else + { + return false; + } +} + +bool contains(std::vector > vector, std::vector element) +{ + //this functions checks, if the given element is in the given vector (in this case for vector) + if (!vector.empty()) + { + return vector.end() != std::find(vector.begin(), vector.end(), element); + } + else + { + return false; + } +} diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/src/cv_boost_loader.cpp b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/src/cv_boost_loader.cpp new file mode 100644 index 0000000..63696c4 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/src/cv_boost_loader.cpp @@ -0,0 +1,17 @@ +#include + +#if CV_MAJOR_VERSION == 2 +void loadBoost(CvBoost& boost, std::string const& filename) +{ + boost.load(filename.c_str()); +} +#else +void loadBoost(cv::Ptr& boost, std::string const& filename) +{ +#if CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION<=2 + boost = cv::Algorithm::load(filename.c_str()); +#else + boost = cv::ml::Boost::load(filename.c_str()); +#endif +} +#endif diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/src/distance_segmentation.cpp b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/src/distance_segmentation.cpp new file mode 100644 index 0000000..5928a31 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/src/distance_segmentation.cpp @@ -0,0 +1,123 @@ +#include + +#include +#include + +DistanceSegmentation::DistanceSegmentation() +{ + +} + +void DistanceSegmentation::segmentMap(const cv::Mat& map_to_be_labeled, cv::Mat& segmented_map, double map_resolution_from_subscription, double room_area_factor_lower_limit, double room_area_factor_upper_limit) +{ + //variables for energy maximization + double optimal_room_area = 50; //variable that sets the desired optimal room area + double constant_additional_value = optimal_room_area * optimal_room_area; //variable that sets the energy function higher so that it is 0 for the lower limit + //variables for distance transformation + cv::Mat temporary_map = map_to_be_labeled.clone(); + //variables for thresholding and finding the room-areas + cv::Mat thresh_map; + std::vector < std::vector > contours; + //hierarchy saves if the contours are hole-contours: + //hierarchy[{0,1,2,3}]={next contour (same level), previous contour (same level), child contour, parent contour} + //child-contour = 1 if it has one, = -1 if not, same for parent_contour + std::vector < cv::Vec4i > hierarchy, hierarchy_saver; + std::vector < std::vector > temporary_contours; + // + //Segmentation of a gridmap into roomlike areas based on the distance-transformation of the map + // + + //1. Get the distance-transformed map and make it an 8-bit single-channel image + cv::erode(temporary_map, temporary_map, cv::Mat()); + cv::Mat distance_map; //variable for the distance-transformed map, type: CV_32FC1 +#if CV_MAJOR_VERSION<=3 + cv::distanceTransform(temporary_map, distance_map, CV_DIST_L2, 5); +#else + cv::distanceTransform(temporary_map, distance_map, cv::DIST_L2, 5); +#endif + cv::convertScaleAbs(distance_map, distance_map); // conversion to 8 bit image + + //2. Threshold the map and find the contours of the rooms. Change the threshold and repeat steps until last possible threshold. + //Then take the contours from the threshold with the most contours between the roomfactors and draw it in the map with a random color. + std::vector > saved_contours, hole_contour_saver; //saving-vector for the found contours + double saved_energy = 0; + for (int current_threshold = 255; current_threshold > 0; current_threshold--) + { //change the threshold for the grayscale-image from largest possible value to smallest + //reset number of rooms + temporary_contours.clear(); + contours.clear(); + hierarchy.clear(); + cv::threshold(distance_map, thresh_map, current_threshold, 255, cv::THRESH_BINARY); +#if CV_MAJOR_VERSION<=3 + cv::findContours(thresh_map, contours, hierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_NONE); +#else + cv::findContours(thresh_map, contours, hierarchy, cv::RETR_CCOMP, cv::CHAIN_APPROX_NONE); +#endif + + //Get the number of large enough regions to be a room. Only check non-holes. + //Energy function: -(x-a)^2 + b, where x is the current area, a is the optimal area and b is a factor to make the function zero at x=0 + for (int c = 0; c < contours.size(); c++) + { + if (hierarchy[c][3] == -1) + { + double room_area = map_resolution_from_subscription * map_resolution_from_subscription * cv::contourArea(contours[c]); + //subtract the area from the hole contours inside the found contour, because the contour area grows extremly large if it is a closed loop + for(int hole = 0; hole < contours.size(); hole++) + { + if(hierarchy[hole][3] == c)//check if the parent of the hole is the current looked at contour + { + room_area -= map_resolution_from_subscription * map_resolution_from_subscription * cv::contourArea(contours[hole]); + } + } + if (room_area >= room_area_factor_lower_limit && room_area <= room_area_factor_upper_limit) + { + temporary_contours.push_back(contours[c]); + //update the energy of the current configuration + } + } + } + //check if current step has a better energy than the saved one + if (temporary_contours.size() >= saved_contours.size())//(current_energy >= saved_energy) + { + saved_contours.clear(); + hole_contour_saver.clear(); + hierarchy_saver.clear(); + saved_contours = temporary_contours; + hole_contour_saver = contours; + hierarchy_saver = hierarchy; + } + } + //Draw the found contours from the step with most areas in the map with a random colour, that hasn't been used yet + std::vector already_used_colors; //saving-variable for already used fill-colours + map_to_be_labeled.convertTo(segmented_map, CV_32SC1, 256, 0); // rescale to 32 int, 255 --> 255*256 = 65280 + for (int current_contour = 0; current_contour < saved_contours.size(); current_contour++) + { + bool drawn = false; //variable to check if contour has been drawn + int loop_counter = 0;//loop counter for ending the loop if it gets into an endless loop + do + { + loop_counter++; + cv::Scalar fill_colour(rand() % 52224 + 13056); + if (!contains(already_used_colors, fill_colour) || loop_counter > 250) + { + cv::drawContours(segmented_map, saved_contours, current_contour, fill_colour, 7); + already_used_colors.push_back(fill_colour); //add used colour to the saving-vector + drawn = true; + } + } while (!drawn); + } + //draw the hole contours black into the new map + for(int current_hole = 0; current_hole < hole_contour_saver.size(); current_hole++) + { + if(hierarchy_saver[current_hole][3] == 1) + { +#if CV_MAJOR_VERSION<=3 + cv::drawContours(segmented_map, hole_contour_saver, current_hole, cv::Scalar(0), CV_FILLED); +#else + cv::drawContours(segmented_map, hole_contour_saver, current_hole, cv::Scalar(0), cv::FILLED); +#endif + } + } + //spread the colors to the white pixels + wavefrontRegionGrowing(segmented_map); +} diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/src/evaluation_segmentation.cpp b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/src/evaluation_segmentation.cpp new file mode 100644 index 0000000..bb2714e --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/src/evaluation_segmentation.cpp @@ -0,0 +1,416 @@ +#include + +#include +#include + +#include +#include +#include + + +//int pixel_number_calculation (cv::Mat matrix) +//{ +// int pixel_number; +// for (unsigned int i=0; i(i,j) == 0) +// { +// ++pixel_number; +// } +// } +// } +// return pixel_number; +//} +//struct byPixelsnumber +//{ +// bool operator () (const cv::Mat & a,const cv::Mat & b) +// { +// int OccupiedPixelsnumber_a,OccupiedPixelsnumber_b = 0; +// for (unsigned int i=0; i(i,j) == 0) +// { +// ++OccupiedPixelsnumber_a; +// } +// } +// } +// +// for (unsigned int i=0; i(i,j) == 0) +// { +// ++OccupiedPixelsnumber_b; +// } +// } +// } +// +// return OccupiedPixelsnumber_a > OccupiedPixelsnumber_b ; +// } +//}; + + +void EvaluationSegmentation::groundTruthVectorCalculation(const cv::Mat &bw_map, VectorOfPointSets& gt) +{ + gt.clear(); + + cv::Mat label_image; + bw_map.convertTo(label_image, CV_32SC1); + + int label_count = 2; // starts at 2 because 0,1 are used already + + for (int y = 0; y < label_image.rows; y++) + { + for (int x = 0; x < label_image.cols; x++) + { + if (bw_map.at(y,x) != 255 || label_image.at(y,x)!=255) + continue; + + // fill each room area with a unique id + cv::Rect rect; + cv::floodFill(label_image, cv::Point(x,y), label_count, &rect, 0, 0, 8); + + // collect all pixels that belong to this room + PointSet blob; + for (int j = rect.y; j < (rect.y + rect.height); j++) + { + int* row = (int*)label_image.ptr(j); + for (int i = rect.x; i < (rect.x + rect.width); i++) + { + if (row[i] != label_count) + continue; + blob.insert(cv::Point(i,j)); + } + } + gt.push_back(blob); + + label_count++; + } + } +} + +void EvaluationSegmentation::computePrecisionRecall(const cv::Mat& gt_map, cv::Mat& gt_map_color, const cv::Mat& segmented_map, + double& precision_micro, double& precision_macro, double& recall_micro, double& recall_macro, bool compute_gt_map_color) +{ + // create vector of rooms that contain a set of the room pixels from the ground truth map + VectorOfPointSets gt_points_vector; // room points: gt[room id][pixel index] + if (compute_gt_map_color == true) + { + cv::Mat bw_map; + cv::threshold(gt_map, bw_map, 250, 255, cv::THRESH_BINARY); + //compute the ground truth matrix + groundTruthVectorCalculation(bw_map, gt_points_vector); + + // generate colored ground truth map + gt_map_color = cv::Mat::zeros(gt_map.size(), CV_8UC3); + for(size_t i=0; i < gt_points_vector.size(); i++) + { + cv::Vec3b color(1 + rand()%255, 1 + rand()%255, 1 + rand()%255); + for(PointSet::iterator it=gt_points_vector[i].begin(); it!=gt_points_vector[i].end(); it++) + gt_map_color.at(*it) = color; + } + } + else + { + // get point sets for segmentation map + std::map gt_points_map; // maps a label key identifier to a vector of points belonging to that room label + const cv::Vec3b black(0,0,0); + for (int v=0; v(v,u); + if (color != black) + { + int key = color.val[0] + color.val[1]<<8 + color.val[2]<<16; + gt_points_map[key].insert(cv::Point(u,v)); + } + } + } + for (std::map::iterator it=gt_points_map.begin(); it!=gt_points_map.end(); ++it) + if (it->second.size() > 100) + gt_points_vector.push_back(it->second); + } + + // remove mini rooms from gt + for (VectorOfPointSets::iterator it=gt_points_vector.begin(); it!=gt_points_vector.end();) + { + if (it->size() <= 100) + gt_points_vector.erase(it); + else + it++; + } + + // get point sets for segmentation map + std::map seg_points_map; // maps a label key identifier to a vector of points belonging to that room label + for (int v=0; v(v,u); + if (label != 0) + seg_points_map[label].insert(cv::Point(u,v)); + } + } + VectorOfPointSets seg_points_vector; + for (std::map::iterator it=seg_points_map.begin(); it!=seg_points_map.end(); ++it) + if (it->second.size() > 100) + seg_points_vector.push_back(it->second); + + // set up matrix of overlaps: rows=seg_points_vector , cols = gt_points_vector + cv::Mat overlap = cv::Mat::zeros(seg_points_vector.size(), gt_points_vector.size(), CV_64FC1); + for (size_t v=0; v(v,u) = overlapping; + } + } + + // precision + precision_micro = 0.; // average of individual precisions + precision_macro = 0.; // pixel count of all overlap areas / pixel count of all found segment areas + double pdenominator_macro = 0.; + for (size_t v=0; v(v,u) > max_overlap) + max_overlap = overlap.at(v,u); + } + precision_micro += max_overlap / (double)seg_points_vector[v].size(); + precision_macro += max_overlap; + pdenominator_macro += (double)seg_points_vector[v].size(); + } + precision_micro /= (double)seg_points_vector.size(); + precision_macro /= pdenominator_macro; + + // recall + recall_micro = 0.; // average of individual recalls + recall_macro = 0.; // pixel count of all overlap areas / pixel count of all gt segment areas + double rdenominator_macro = 0.; + for (size_t u=0; u(v,u) > max_overlap) + max_overlap = overlap.at(v,u); + } + recall_micro += max_overlap / (double)gt_points_vector[u].size(); + recall_macro += max_overlap; + rdenominator_macro += (double)gt_points_vector[u].size(); + } + recall_micro /= (double)gt_points_vector.size(); + recall_macro /= rdenominator_macro; + //std::cout << recall_micro << "\t" << precision_micro << "\t" << recall_macro << "\t" << precision_macro << std::endl; +} + +//void EvaluationSegmentation::Segmentation_Vector_calculation(const cv::Mat &segmented_map, std::map &segmented_room_mapping) +//{ +// for(unsigned int i=0; i (i,j)[0] != 0 && segmented_map.at(i,j)[1] != 0 && segmented_map.at(i,j)[2] != 0) +// { +// segmented_room_mapping[cv::Point2i (i,j)] = segmented_map.at(i,j); +// } +// } +// } +//} +// +//void EvaluationSegmentation::Recall_Precision_Calculation(std::map &results, std::vector gt_mat_vector, std::vector segmentation_mat_vector) +//{ +//// std::sort(gt_mat_vector.begin(), gt_mat_vector.end(), byPixelsnumber()); +//// std::sort(segmentation_mat_vector.begin(), segmentation_mat_vector.end(), byPixelsnumber()); +// +// for (unsigned int i = 0; i < segmentation_mat_vector.size();i++) +// { +// int max_pixel_number = 0; +// int pixel_number = 0; +// int selected_gt_mat_pixel_number = 0; +// for (unsigned int j = 0; j < gt_mat_vector.size();j++) +// { +// cv::Mat overlapped_matrix; +// cv::bitwise_or(gt_mat_vector[i],segmentation_mat_vector[j],overlapped_matrix); +// +// pixel_number = pixel_number_calculation(overlapped_matrix); +// +// if (pixel_number > max_pixel_number) +// { +// max_pixel_number = pixel_number; +// selected_gt_mat_pixel_number = pixel_number_calculation(gt_mat_vector[j]); +// } +// } +// +// float recall = float(max_pixel_number/selected_gt_mat_pixel_number); +// float precision = float(max_pixel_number/ (pixel_number_calculation(segmentation_mat_vector[i]))); +// +// // calculate the first pixel of segmentation matrix +// for (unsigned int m=0; m(m,n) == 0) +// { +// results[cv::Point2i(m,n)] = cv::Point2f (recall,precision); +// break; +// } +// } +// } +// } +//} + +/* +int main(int argc, char** argv) +{ + ros::init(argc, argv, "evaluation_seg"); + ros::NodeHandle n; + +// if (argc < 2) +// { +// std::cout << "error: not enough input parameters!" << std::endl; +// return -1; +// } + + std::vector< std::string > map_names; +// map_names.push_back("lab_ipa"); +// map_names.push_back("lab_c_scan"); +// map_names.push_back("Freiburg52_scan"); +// map_names.push_back("Freiburg79_scan"); +// map_names.push_back("lab_b_scan"); +// map_names.push_back("lab_intel"); +// map_names.push_back("Freiburg101_scan"); +// map_names.push_back("lab_d_scan"); +// map_names.push_back("lab_f_scan"); +// map_names.push_back("lab_a_scan"); +// map_names.push_back("NLB"); +// map_names.push_back("office_a"); +// map_names.push_back("office_b"); +// map_names.push_back("office_c"); +// map_names.push_back("office_d"); +// map_names.push_back("office_e"); +// map_names.push_back("office_f"); +// map_names.push_back("office_g"); +// map_names.push_back("office_h"); +// map_names.push_back("office_i"); + map_names.push_back("lab_ipa_furnitures"); + map_names.push_back("lab_c_scan_furnitures"); + map_names.push_back("Freiburg52_scan_furnitures"); + map_names.push_back("Freiburg79_scan_furnitures"); + map_names.push_back("lab_b_scan_furnitures"); + map_names.push_back("lab_intel_furnitures"); + map_names.push_back("Freiburg101_scan_furnitures"); + map_names.push_back("lab_d_scan_furnitures"); + map_names.push_back("lab_f_scan_furnitures"); + map_names.push_back("lab_a_scan_furnitures"); + map_names.push_back("NLB_furnitures"); + map_names.push_back("office_a_furnitures"); + map_names.push_back("office_b_furnitures"); + map_names.push_back("office_c_furnitures"); + map_names.push_back("office_d_furnitures"); + map_names.push_back("office_e_furnitures"); + map_names.push_back("office_f_furnitures"); + map_names.push_back("office_g_furnitures"); + map_names.push_back("office_h_furnitures"); + map_names.push_back("office_i_furnitures"); + + const std::string segmented_map_path = "room_segmentation/"; //ros::package::getPath("ipa_room_segmentation") + "/common/files/segmented_maps/"; + + for (size_t image_index = 0; image_index(v,u); + segmented_map_int.at(v,u) = color.val[0] + color.val[1]<<8 + color.val[2]<<16; + } + } + + double precision_micro, precision_macro, recall_micro, recall_macro; + cv::Mat gt_map_color; + EvaluationSegmentation es; + es.computePrecisionRecall(gt_map, gt_map_color, segmented_map, precision_micro, precision_macro, recall_micro, recall_macro, true); + std::cout << recall_micro << "\t" << precision_micro << "\t" << recall_macro << "\t" << precision_macro << std::endl; + std::string gt_image_filename_color = segmented_map_path + map_name + "_gt_color_segmentation.png"; //ros::package::getPath("ipa_room_segmentation") + "/common/files/test_maps/" + map_name + "_gt_color_segmentation.png"; + cv::imwrite(gt_image_filename_color.c_str(), gt_map_color); + } +// +// cv::Mat gt_map = cv::imread(argv[1],CV_8U); +// cv::Mat segmented_map = cv::imread(argv[2]); +// +// EvaluationSegmentation es; +// +// cv::Mat bw_map; +// cv::threshold(gt_map, bw_map, 250, 255, cv::THRESH_BINARY); +// //compute the GT matrix +// std::vector < std::vector > gt_points_vector; +// es.GT_Vector_calculation(bw_map, gt_points_vector); +// +// cv::Mat GT_matrix = cv::Mat::zeros(gt_map.size(), CV_8UC3); +// for(size_t i=0; i < gt_points_vector.size(); i++) +// { +// unsigned char r = 255 * (rand()/(1.0 + RAND_MAX)); +// unsigned char g = 255 * (rand()/(1.0 + RAND_MAX)); +// unsigned char b = 255 * (rand()/(1.0 + RAND_MAX)); +// +// for(size_t j=0; j < gt_points_vector[i].size(); j++) +// { +// int x = gt_points_vector[i][j].x; +// int y = gt_points_vector[i][j].y; +// +// GT_matrix.at(y,x)[0] = b; +// GT_matrix.at(y,x)[1] = g; +// GT_matrix.at(y,x)[2] = r; +// } +// } +// cv::imshow("labeled", GT_matrix); +// cv::waitKey(0); +// +// // save ground truth and segmentation into maps +// std::map segmented_room_mapping, GT_room_mapping; +// std::vector gt_mat_vector, segmentation_mat_vector; +// es.Segmentation_Vector_calculation(GT_matrix, GT_room_mapping); +// es.Segmentation_Vector_calculation(segmented_map, segmented_room_mapping); +// +// for(std::map::iterator iterator = GT_room_mapping.begin(); iterator != GT_room_mapping.end(); iterator++) +// { +////todo: convert map to vector +// } +// +// std::map results; +// +//// cv::namedWindow("labeled"); +//// cv::imshow("labeled", GT_matrix); +//// cv::waitKey(0); + + + return 0; +} +*/ diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/src/features.cpp b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/src/features.cpp new file mode 100644 index 0000000..55e7f2d --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/src/features.cpp @@ -0,0 +1,873 @@ +#include +#include +#include +#include +#include + +#include + +#define PI 3.14159265 + +//get the number of implemented features. Needs to be changed to the new value if you change it +int LaserScannerFeatures::get_feature_count() +{ + return 23; +} + +void LaserScannerFeatures::resetCachedData() +{ + features_.clear(); + features_.resize(get_feature_count(), 0.); + features_computed_.clear(); + features_computed_.resize(get_feature_count(), false); + + polygon_.clear(); + polygon_computed_ = false; + + centroid_computed_ = false; +} + +//**********************see features.h for a better overview of what is calculated and needed************************* +//Method for calculating the feature for the classifier +double LaserScannerFeatures::get_feature(const std::vector& beams, const std::vector& angles, cv::Point point, const int feature) +{ + switch (feature) + { + case 1: + return calc_feature1(beams); + case 2: + return calc_feature2(beams); + case 3: + return calc_feature3(beams, 10); + case 4: + return calc_feature4(beams, 10); + case 5: + return calc_feature5(beams); + case 6: + return calc_feature6(beams); + case 7: + return calc_feature7(beams); + case 8: + return calc_feature8(beams, angles); + case 9: + return calc_feature9(beams, angles); + case 10: + return calc_feature10(beams); + case 11: + return calc_feature11(beams); + case 12: + return calc_feature12(beams); + case 13: + return calc_feature13(beams); + case 14: + return calc_feature14(beams, angles, point); + case 15: + return calc_feature15(beams, angles, point); + case 16: + return calc_feature16(beams, angles, point); + case 17: + return calc_feature17(beams, angles, point); + case 18: + return calc_feature18(beams, angles, point); + case 19: + return calc_feature19(beams, angles, point); + case 20: + return calc_feature20(beams, angles, point); + case 21: + return calc_feature21(beams, angles, point); + case 22: + return calc_feature22(beams); + case 23: + return calc_feature23(beams); + } +} + +void LaserScannerFeatures::get_features(const std::vector& beams, const std::vector& angles, cv::Point point, cv::Mat& features) +{ + // reset internal data storage + resetCachedData(); + + // compute features + calc_feature1(beams); + calc_feature2(beams); + calc_feature3(beams, 10); + calc_feature4(beams, 10); + calc_feature5(beams); + calc_feature6(beams); + calc_feature7(beams); + calc_feature8(beams, angles); + calc_feature9(beams, angles); + calc_feature10(beams); + calc_feature11(beams); + calc_feature12(beams); + calc_feature13(beams); + calc_feature14(beams, angles, point); + calc_feature15(beams, angles, point); + calc_feature16(beams, angles, point); + calc_feature17(beams, angles, point); + calc_feature18(beams, angles, point); + calc_feature19(beams, angles, point); + calc_feature20(beams, angles, point); + calc_feature21(beams, angles, point); + calc_feature22(beams); + calc_feature23(beams); + + // write features + features.create(1, get_feature_count(), CV_32FC1); + for (int i=0; i(0,i) = features_[i]; +} + +//Calculation of Feature 1: average difference of the beams +double LaserScannerFeatures::calc_feature1(const std::vector& beams) +{ + if (features_computed_[0]) + return features_[0]; + + double differences_sum; + for (int b = 0; b < beams.size() - 1; b++) + { + differences_sum += abs(beams[b] - beams[b + 1]); + } + //get the difference betweeen the last and the first beam + differences_sum += abs(beams[beams.size() - 1] - beams[0]); + //calculate the average difference and return it + features_computed_[0] = true; + features_[0] = (differences_sum / (double)beams.size()); + + if (features_[0]!=features_[0]) + std::cout << " features_[0]="<& beams) +{ + if (features_computed_[1]) + return features_[1]; + + double mean; //mean-value of the difference, calculated with calc_feature1 + double sum; //helping variable + //initialise + mean = calc_feature1(beams); + sum = 0; + //calculate deviation + for (int b = 0; b < beams.size(); b++) + { + sum += (beams[b] - mean)*(beams[b] - mean);//std::pow((beams[b] - mean), 2.0); + } + sum = sum / (double)(beams.size() - 1); + features_computed_[1] = true; + features_[1] = std::sqrt(sum); + + if (features_[1]!=features_[1]) + std::cout << " features_[1]="<& beams, double maxval) +{ + if (features_computed_[2]) + return features_[2]; + + double differences_sum; + double val1, val2; + for (int b = 0; b < beams.size() - 1; b++) + { + //reset saved beamlenghts + val1 = maxval; + val2 = maxval; + if (beams[b] < maxval) + { + val1 = beams[b]; + } + if (beams[b + 1] < maxval) + { + val2 = beams[b + 1]; + } + differences_sum += abs(val1 - val2); + } + //get the difference betweeen the last and the first beam + val1 = maxval; + val2 = maxval; + if (beams[beams.size()-1] < maxval) + { + val1 = beams[beams.size()-1]; + } + if (beams[0] < maxval) + { + val2 = beams[0]; + } + differences_sum += abs(val1 - val2); + //calculate the average difference and return it + features_computed_[2] = true; + features_[2] = (differences_sum / (double)beams.size()); + + if (features_[2]!=features_[2]) + std::cout << " features_[2]="<& beams, double maxval) +{ + if (features_computed_[3]) + return features_[3]; + + double mean; //mean-value of the difference, calculated with calc_feature1 + double v, w, difference, sum; //helping variables + //initialize + mean = calc_feature3(beams, maxval); + sum = 0; + //calculate deviation + for (int b = 0; b < beams.size() - 1; b++) + { + //reset value of current beam + v = maxval; + w = maxval; + if (beams[b] < maxval) + { + v = beams[b]; + } + if (beams[b + 1] < maxval) + { + w = beams[b + 1]; + } + difference = abs(v - w); + sum += (difference - mean)*(difference - mean); //std::pow((difference - mean), 2.0); + } + //add the difference from last to first point + v = maxval; + w = maxval; + if (beams[beams.size()-1] < maxval) + { + v = beams[beams.size()-1]; + } + if (beams[0] < maxval) + { + w = beams[0]; + } + difference = abs(v - w); + sum += (difference - mean)*(difference - mean); //std::pow((difference - mean), 2.0); + sum = sum / (beams.size() - 1); + features_computed_[3] = true; + features_[3] = std::sqrt(sum); + + if (features_[3]!=features_[3]) + std::cout << " features_[3]="<& beams) +{ + if (features_computed_[4]) + return features_[4]; + + double sum; + //get the sum of the beamlengths + for (int b = 0; b < beams.size(); b++) + { + sum += beams[b]; + } + //divide by number of beams and return value + features_computed_[4] = true; + features_[4] = (sum / (double)beams.size()); + + if (features_[4]!=features_[4]) + std::cout << " features_[4]="<& beams) +{ + if (features_computed_[5]) + return features_[5]; + + double mean; //mean-value of the beamlenghts, calculated with calc_feature5 + double sum; //helping variable + //initialize + mean = calc_feature5(beams); + sum = 0; + //calculate deviation + for (int b = 0; b < beams.size(); b++) + { + sum += (beams[b] - mean)*(beams[b] - mean); //std::pow((beams[b] - mean), 2); + } + sum = sum / (beams.size() - 1); + features_computed_[5] = true; + features_[5] = std::sqrt(sum); + + if (features_[5]!=features_[5]) + std::cout << " features_[5]="<& beams) +{ + if (features_computed_[6]) + return features_[6]; + + double threshold = 0.5; //[m], see "Semantic labeling of places" + double gaps = 0; + for (int b = 0; b < beams.size() - 1; b++) + { + if (abs(beams[b] - beams[b + 1]) > threshold) + { + gaps++; + } + } + if (abs(beams[beams.size() - 1] - beams[0]) > threshold) + { + gaps++; + } + features_computed_[6] = true; + features_[6] = gaps; + return features_[6]; +} + +//Calculation of feature 8: The distance between two Endpoints of local minima of beamlenghts +double LaserScannerFeatures::calc_feature8(const std::vector& beams, const std::vector& angles) +{ + if (features_computed_[7]) + return features_[7]; + + //Remark: angles are relatively to the robot + double length_1 = 10000000; + double length_2 = 10000000; + double angle_1, angle_2; + //get the two Points corresponding to minimal beamlength + for (int b = 0; b < beams.size(); b++) + { + if (beams[b] < length_1 && beams[b] > length_2) + { + length_1 = beams[b]; + angle_1 = angles[b]; + } + else if (beams[b] < length_2) + { + length_2 = beams[b]; + angle_2 = angles[b]; + } + } + //calculate the x/y-values of the Points + double x1_x2 = std::cos(angle_1 * PI / 180) * length_1 - std::cos(angle_2 * PI / 180) * length_2; + double y1_y2 = std::sin(angle_1 * PI / 180) * length_1 - std::sin(angle_2 * PI / 180) * length_2; + //calculate and return the euclidean distance between the Points + features_computed_[7] = true; + features_[7] = std::sqrt(x1_x2*x1_x2 + y1_y2*y1_y2); + + if (features_[7]!=features_[7]) + std::cout << " features_[7]="<& beams, const std::vector& angles) +{ + if (features_computed_[8]) + return features_[8]; + + //Remark: angles are relative to the robot + double length_1 = beams[0]; + double length_2 = beams[1]; + double angle_1 = angles[0]; + double angle_2 = angles[1]; + double x_1, y_1, x_2, y_2; + //get the two Points corresponding to minimal beamlengths + for (int b = 0; b < beams.size(); b++) + { + if (beams[b] < length_1 && beams[b] > length_2) + { + length_1 = beams[b]; + angle_1 = angles[b]; + } + else if (beams[b] <= length_2) + { + length_2 = beams[b]; + angle_2 = angles[b]; + } + } + //calculate the x/y-values of the Points + const double pi_to_degree = PI / 180; + x_1 = std::cos(angle_1 * pi_to_degree) * length_1; + y_1 = std::sin(angle_1 * pi_to_degree) * length_1; + x_2 = std::cos(angle_2 * pi_to_degree) * length_2; + y_2 = std::sin(angle_2 * pi_to_degree) * length_2; + //calculate and return the angle between the Points + const double coordvec = (x_1 * x_2) + (y_1 * y_2); + const double absvec = (length_1 * length_2); + const double quot = std::max(-1., std::min(1., coordvec / absvec)); + features_computed_[8] = true; + features_[8] = std::acos(quot) * 180.0 / PI; + return features_[8]; +} + +//Calculate Feature 10: The average of the relations (b_i/b_(i+1)) between two neighboring beams +double LaserScannerFeatures::calc_feature10(const std::vector& beams) +{ + if (features_computed_[9]) + return features_[9]; + + double length_1, length_2; + double sum_relation = 0; + //calculate the relations and add it to the sum + for (int b = 0; b < beams.size() - 1; b++) + { + length_1 = beams[b]; + length_2 = beams[b + 1]; + if (length_1 < length_2) + { + sum_relation += (length_1 / length_2); + } + else + { + sum_relation += (length_2 / length_1); + } + } + length_1 = beams[beams.size() - 1]; + length_2 = beams[0]; + if (length_1 < length_2) + { + sum_relation += (length_1 / length_2); + } + else + { + sum_relation += (length_2 / length_1); + } + //calculate and return the average of the relations + features_computed_[9] = true; + features_[9] = (sum_relation / beams.size()); + + if (features_[9]!=features_[9]) + std::cout << " features_[9]="<& beams) +{ + if (features_computed_[10]) + return features_[10]; + + //calculate the mean of the relations by using Feature 10 + double mean = calc_feature10(beams); + double sum = 0; + //calculate the standard_deviation + for (int b = 0; b < beams.size(); b++) + { + sum += (beams[b] - mean); //std::pow((beams[b] - mean), 2); + } + sum = sum / (beams.size() - 1); + features_computed_[10] = true; + features_[10] = std::sqrt(sum); + + if (features_[10]!=features_[10]) + std::cout << " features_[10]="<& beams) +{ + if (features_computed_[11]) + return features_[11]; + + double threshold = 0.5; //[m] see "Semantic labeling of places" + double gaps, length_1, length_2; + for (int b = 0; b < beams.size() - 1; b++) + { + length_1 = beams[b]; + length_2 = beams[b + 1]; + if (length_1 < length_2) + { + if ((length_1 / length_2) < threshold) + { + gaps++; + } + } + else + { + if ((length_2 / length_1) < threshold) + { + gaps++; + } + } + } + length_1 = beams[0]; + length_2 = beams[beams.size() - 1]; + if (length_1 < length_2) + { + if ((length_1 / length_2) < threshold) + { + gaps++; + } + } + else + { + if ((length_2 / length_1) < threshold) + { + gaps++; + } + } + features_computed_[11] = true; + features_[11] = gaps; + return features_[11]; +} + +//Calculate Feature 13: The Kurtosis, which is given by: +//(Sum((x - mean)^4))/sigma^4) - 3, where mean is the mean-value and sigma is the standard deviation +double LaserScannerFeatures::calc_feature13(const std::vector& beams) +{ + if (features_computed_[12]) + return features_[12]; + + double sum = 0; + //get the standard deviation and the mean by using previous functions + double sigma = calc_feature6(beams); + double mean = calc_feature5(beams); + //calculate the Kurtosis + for (int b = 0; b < beams.size(); b++) + { + double v=(beams[b] - mean); + sum += v*v*v*v; //std::pow((beams[b] - mean), 4); + } + features_computed_[12] = true; + features_[12] = ((sum / std::pow(sigma, 4)) - 3); + + if (features_[12]!=features_[12]) + std::cout << " features_[12]="<& beams) +{ + if (features_computed_[21]) + return features_[21]; + + double sum; + double maxval = 0.; + //find maximal value of the beams + for (int b = 0; b < beams.size(); b++) + if (beams[b] > maxval) + maxval = beams[b]; + if (maxval == 0.) + maxval = 1.; + //get the average of the beams/maxval + //get the sum of the beamlengths + for (int b = 0; b < beams.size(); b++) + { + sum += (beams[b] / maxval); + } + //divide by number of beams and return value + features_computed_[21] = true; + features_[21] = (sum / (double)beams.size()); + + if (features_[21]!=features_[21]) + std::cout << " features_[21]="<& beams) +{ + if (features_computed_[22]) + return features_[22]; + + double sum = 0; + double mean = calc_feature22(beams); + double maxval = 0; + //find maximal value of the beams + for (int b = 0; b < beams.size(); b++) + if (beams[b] > maxval) + maxval = beams[b]; + if (maxval == 0.) + maxval = 1.; + const double maxvalinv = 1./maxval; + //get the standard deviation + for (int b = 0; b < beams.size(); b++) + { + const double v = (beams[b] * maxvalinv) - mean; + sum += v*v; + } + sum = sum / (beams.size() - 1); + features_computed_[22] = true; + features_[22] = std::sqrt(sum); + + if (features_[22]!=features_[22]) + std::cout << " features_[22]="< LaserScannerFeatures::calc_polygonal_approx(const std::vector& beams, const std::vector& angles, cv::Point location) +{ + if (polygon_computed_ == true) + return polygon_; + + polygon_.clear(); + double x, y; + //calculate the endpoint for every beam and add it to the polygon + for (int b = 0; b < beams.size(); b++) + { //calculate the x/y-values + //Remark: angles in radiant + double pi_to_degree = PI / 180; + x = std::cos(angles[b] * pi_to_degree) * beams[b]; + y = std::sin(angles[b] * pi_to_degree) * beams[b]; + polygon_.push_back(cv::Point(location.x + x, location.y + y)); + } + polygon_computed_ = true; + return polygon_; +} + +//Calculate the centroid of the polygonal approximation +cv::Point LaserScannerFeatures::calc_centroid(const std::vector& beams, const std::vector& angles, cv::Point location) +{ + if (centroid_computed_ == true) + return centroid_; + + double x, y; + double sumX = 0; + double sumY = 0; + //get every Point by using the polygonal approximation + std::vector polygon = calc_polygonal_approx(beams, angles, location); + for (int p = 0; p < polygon.size(); p++) + { + sumX += polygon[p].x; + sumY += polygon[p].y; + } + centroid_.x = sumX / polygon.size(); + centroid_.y = sumY / polygon.size(); + centroid_computed_ = true; + return centroid_; +} + +//Calculate Feature 14: The area of the polygonal approximation of the beams +double LaserScannerFeatures::calc_feature14(const std::vector& beams, const std::vector& angles, cv::Point location) +{ + if (features_computed_[13]) + return features_[13]; + + double map_resolution = 0.05000; + std::vector polygon = calc_polygonal_approx(beams, angles, location); + features_computed_[13] = true; + features_[13] = map_resolution * map_resolution * cv::contourArea(polygon); + + if (features_[13]!=features_[13]) + std::cout << " features_[13]="<& beams, const std::vector& angles, cv::Point location) +{ + if (features_computed_[14]) + return features_[14]; + + std::vector polygon = calc_polygonal_approx(beams, angles, location); + features_computed_[14] = true; + features_[14] = cv::arcLength(polygon, true); + + if (features_[14]!=features_[14]) + std::cout << " features_[14]="<& beams, const std::vector& angles, cv::Point location) +{ + if (features_computed_[15]) + return features_[15]; + + features_computed_[15] = true; + features_[15] = (calc_feature14(beams, angles, location) / calc_feature15(beams, angles, location)); + + if (features_[15]!=features_[15]) + std::cout << " features_[15]="<& beams, const std::vector& angles, cv::Point location) +{ + if (features_computed_[16]) + return features_[16]; + + std::vector < cv::Point > polygon = calc_polygonal_approx(beams, angles, location); + cv::Point centroid = calc_centroid(beams, angles, location); + double sum = 0; + double delta_x, delta_y; + //calculate the distance between the centroid and the boundary and add it to the sum + for (int p = 0; p < polygon.size(); p++) + { + delta_x = polygon[p].x - centroid.x; + delta_y = polygon[p].y - centroid.y; + sum += std::sqrt(delta_x*delta_x + delta_y*delta_y); //std::pow(delta_x, 2) + std::pow(delta_y, 2)); + } + //calculate and return the average of the distances + features_computed_[16] = true; + features_[16] = (sum / polygon.size()); + + if (features_[16]!=features_[16]) + std::cout << " features_[16]="<& beams, const std::vector& angles, cv::Point location) +{ + if (features_computed_[17]) + return features_[17]; + + std::vector < cv::Point > polygon = calc_polygonal_approx(beams, angles, location); + cv::Point centroid = calc_centroid(beams, angles, location); + //get the mean of the distances by using Feature 17 + double mean = calc_feature17(beams, angles, location); + double current_distance; + double sum = 0; + //calculate the standard_deviation + for (int p = 0; p < polygon.size(); p++) + { + double delta_x = polygon[p].x - centroid.x; + double delta_y = polygon[p].y - centroid.y; + current_distance = std::sqrt(delta_x*delta_x + delta_y*delta_y); //std::pow(delta_x, 2) + std::pow(delta_y, 2)); + sum += (current_distance - mean)*(current_distance - mean); //std::pow(current_distance - mean, 2); + } + sum = sum / (beams.size() - 1); + features_computed_[17] = true; + features_[17] = std::sqrt(sum); + + if (features_[17]!=features_[17]) + std::cout << " features_[17]="<& beams, const std::vector& angles, cv::Point location) +{ + if (features_computed_[18]) + return features_[18]; + + std::vector < cv::Point > polygon = calc_polygonal_approx(beams, angles, location); + cv::Point centroid = calc_centroid(beams, angles, location); + cv::Point2f points[4]; + std::vector < cv::Point2f > edge_points; + double distance = 0; + //saving-variable for the Points of the ellipse + cv::RotatedRect ellipse = cv::fitEllipse(cv::Mat(polygon)); + //get the edge-points of the ellipse + ellipse.points(points); + //saving the Points of the ellipse in a vector + for (int i = 0; i < 4; i++) + { + edge_points.push_back(points[i]); + } + //calculate the distance between the Points and take the largest one + for (int p = 0; p < edge_points.size(); p++) + { + for (int np = 0; np < edge_points.size(); np++) + { + //if (std::sqrt(std::pow((edge_points[p].x - edge_points[np].x), 2) + std::pow((edge_points[p].y - edge_points[np].y), 2)) > distance) + const float a = (edge_points[p].x - edge_points[np].x); + const float b = (edge_points[p].y - edge_points[np].y); + const double sqr = a*a + b*b; + if (sqr > distance) + { + //distance = std::sqrt(std::pow((edge_points[p].x - edge_points[np].x), 2) + std::pow((edge_points[p].y - edge_points[np].y), 2)); + distance = sqr; + } + } + } + features_computed_[18] = true; + features_[18] = (std::sqrt(distance) / 2); + + if (features_[18]!=features_[18]) + std::cout << " features_[18]="<& beams, const std::vector& angles, cv::Point location) +{ + if (features_computed_[19]) + return features_[19]; + + std::vector < cv::Point > polygon = calc_polygonal_approx(beams, angles, location); + cv::Point2f points[4]; + std::vector < cv::Point2f > edge_points; + double distance = 1e6*1e6; + //saving-variable for the Points of the ellipse + cv::RotatedRect ellipse = cv::fitEllipse(cv::Mat(polygon)); + //get the edge-points of the ellipse + ellipse.points(points); + //saving the Points of the ellipse in a vector + for (int i = 0; i < 4; i++) + { + edge_points.push_back(points[i]); + } + //calculate the distance between the Points and take the largest one + for (int p = 0; p < edge_points.size(); p++) + { + for (int np = 0; np < edge_points.size(); np++) + { + if (p==np) + continue; +// if (std::sqrt(std::pow((edge_points[p].x - edge_points[np].x), 2) + std::pow((edge_points[p].y - edge_points[np].y), 2)) < distance +// && std::sqrt(std::pow((edge_points[p].x - edge_points[np].x), 2) + std::pow((edge_points[p].y - edge_points[np].y), 2)) > 0 && p != np) + const float a = (edge_points[p].x - edge_points[np].x); + const float b = (edge_points[p].y - edge_points[np].y); + const double sqr = a*a + b*b; + if (sqr < distance) // && sqr > 0) // && p != np) + { + distance = sqr; + } + } + } + features_computed_[19] = true; + features_[19] = (std::sqrt(distance) / 2); + + if (features_[19]!=features_[19]) + std::cout << " features_[19]="<& beams, const std::vector& angles, cv::Point location) +{ + if (features_computed_[20]) + return features_[20]; + + features_computed_[20] = true; + features_[20] = (calc_feature19(beams, angles, location) / (0.0001+calc_feature20(beams, angles, location))); + + if (features_[20]!=features_[20]) + std::cout << " features_[20]="<. + * + ****************************************************************/ + +#include "ipa_room_segmentation/meanshift2d.h" +#include "ipa_room_segmentation/fast_math.h" + +void MeanShift2D::filter(const std::vector& data, std::vector& filtered_data, const double bandwidth, const int maximumIterations) +{ + // prepare mean shift set + std::vector mean_shift_set(data.size()); + filtered_data = data; + + // mean shift iteration + for (int iter=0; iter& filtered_data, std::vector& convergence_points, std::vector< std::vector >& convergence_sets, const double sensitivity) +{ + // cluster data according to convergence points + convergence_sets.resize(1, std::vector(1, 0)); + convergence_points.resize(1, filtered_data[0]); + for (int i=1; i<(int)filtered_data.size(); i++) + { + bool create_new_set = true; + for (int j=0; j<(int)convergence_points.size(); j++) + { + if (cv::norm(filtered_data[i]-convergence_points[j]) < sensitivity) + { + convergence_sets[j].push_back(i); + convergence_points[j] = (convergence_points[j]*(convergence_sets[j].size()-1.) + filtered_data[i]) / (double)convergence_sets[j].size(); // update mean of convergence point + create_new_set = false; + break; + } + } + if (create_new_set == true) + { + convergence_sets.push_back(std::vector(1, i)); + convergence_points.push_back(filtered_data[i]); + } + } +} + +cv::Vec2d MeanShift2D::findRoomCenter(const cv::Mat& room_image, const std::vector& room_cells, double map_resolution) +{ + // downsample data if too big + std::vector room_cells_sampled; + if (room_cells.size() > 1000) + { + const int factor = room_cells.size()/1000; + for (size_t i=0; i filtered_room_cells; + filter(room_cells_sampled, filtered_room_cells, bandwidth, 100); + + // take mean of filtering result in simple rooms, i.e. if no obstacles is located at the computed cell + cv::Scalar mean_coordinates = cv::mean(filtered_room_cells); + cv::Vec2d room_center(mean_coordinates[0], mean_coordinates[1]); + if (room_image.at(room_center[1], room_center[0]) == 255) + return room_center; + + // otherwise compute convergence sets of filtering results and return mean of largest convergence set + // determine convergence points + std::vector convergence_points; + std::vector< std::vector > convergence_sets; + computeConvergencePoints(filtered_room_cells, convergence_points, convergence_sets, 1./bandwidth*0.1); + + // select convergence point with largest point support + size_t max_index = 0; + size_t max_size = 0; + for (size_t i=0; i max_size) + { + max_size = convergence_sets[i].size(); + max_index = i; + } + } + return convergence_points[max_index]; +} diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/src/morphological_segmentation.cpp b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/src/morphological_segmentation.cpp new file mode 100644 index 0000000..41266c1 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/src/morphological_segmentation.cpp @@ -0,0 +1,132 @@ +#include + +#include +#include + +MorphologicalSegmentation::MorphologicalSegmentation() +{ + +} + +void MorphologicalSegmentation::segmentMap(const cv::Mat& map_to_be_labeled, cv::Mat& segmented_map, double map_resolution_from_subscription, + double room_area_factor_lower_limit, double room_area_factor_upper_limit) +{ + /*This segmentation algorithm does: + * 1. collect the map data + * 2. erode the map to extract contours + * 3. find the extracted contures and save them if they fullfill the room-area criterion + * 4. draw and fill the saved contoures in a clone of the map from 1. with a random colour + * 5. get the obstacle information from the original map and draw them in the clone from 4. + * 6. spread the coloured regions to the white Pixels + */ + + //make two map clones to work with + cv::Mat temporary_map_to_find_rooms = map_to_be_labeled.clone(); //map to find the rooms and for eroding + //**************erode temporary_map until last possible room found**************** + //erode map a specified amount of times + std::vector < std::vector > saved_contours; //saving variable for every contour that is between the upper and the lower limit + ROS_INFO("starting eroding"); + for (int counter = 0; counter < 73; counter++) + { + //erode the map one time + cv::Mat eroded_map; + cv::Point anchor(-1, -1); //needed for opencv erode + cv::erode(temporary_map_to_find_rooms, eroded_map, cv::Mat(), anchor, 1); + //save the more eroded map + temporary_map_to_find_rooms = eroded_map; + //Save the eroded map in a second map, which is used to find the contours. This is neccesarry, because + //the function findContours changes the given map and would make it impossible to work any further with it + cv::Mat contour_map = eroded_map.clone(); + //find Contours in the more eroded map + std::vector < std::vector > temporary_contours; //temporary saving-variable + //hierarchy saves if the contours are hole-contours: + //hierarchy[{0,1,2,3}]={next contour (same level), previous contour (same level), child contour, parent contour} + //child-contour = 1 if it has one, = -1 if not, same for parent_contour + std::vector < cv::Vec4i > hierarchy; +#if CV_MAJOR_VERSION<=3 + cv::findContours(contour_map, temporary_contours, hierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE); +#else + cv::findContours(contour_map, temporary_contours, hierarchy, cv::RETR_CCOMP, cv::CHAIN_APPROX_SIMPLE); +#endif + if (temporary_contours.size() != 0) + { + //check every contour if it fullfills the criteria of a room + for (int current_contour = 0; current_contour < temporary_contours.size(); current_contour++) + { //only take first level contours --> second level contours belong to holes and doesn't need to be looked at + if (hierarchy[current_contour][3] == -1) + { + //check if contour is large/small enough for a room + double room_area = map_resolution_from_subscription * map_resolution_from_subscription + * cv::contourArea(temporary_contours[current_contour]); + //subtract the area from the hole contours inside the found contour, because the contour area grows extremly large if it is a closed loop + for (int hole = 0; hole < temporary_contours.size(); hole++) + { + if (hierarchy[hole][3] == current_contour) //check if the parent of the hole is the current looked at contour + { + room_area -= map_resolution_from_subscription * map_resolution_from_subscription + * cv::contourArea(temporary_contours[hole]); + } + } + if (room_area_factor_lower_limit < room_area && room_area < room_area_factor_upper_limit) + { + //save contour for later drawing in map + saved_contours.push_back(temporary_contours[current_contour]); + //make region black if room found --> region doesn't need to be looked at anymore +#if CV_MAJOR_VERSION<=3 + cv::drawContours(temporary_map_to_find_rooms, temporary_contours, current_contour, cv::Scalar(0), CV_FILLED, 8, hierarchy, 2); +#else + cv::drawContours(temporary_map_to_find_rooms, temporary_contours, current_contour, cv::Scalar(0), cv::FILLED, 8, hierarchy, 2); +#endif + } + } + } + } + } + //*******************draw contures in new map*********************** + std::cout << "Segmentation Found " << saved_contours.size() << " rooms." << std::endl; + //draw filled contoures in new_map_to_draw_contours_ with random colour if this colour hasn't been used yet + cv::Mat new_map_to_draw_contours; //map for drawing the found contours + map_to_be_labeled.convertTo(segmented_map, CV_32SC1, 256, 0); + std::vector < cv::Scalar > already_used_coloures; //vector for saving the already used coloures + for (int idx = 0; idx < saved_contours.size(); idx++) + { + bool drawn = false; //checking-variable if contour has been drawn + int draw_counter = 0; //counter to exit loop if it gets into an endless-loop (e.g. when there are more rooms than possible) + do + { + draw_counter++; + cv::Scalar fill_colour(rand() % 52224 + 13056); + if (!contains(already_used_coloures, fill_colour) || draw_counter > 250) + { + //if colour is unique draw Contour in map +#if CV_MAJOR_VERSION<=3 + cv::drawContours(segmented_map, saved_contours, idx, fill_colour, CV_FILLED); +#else + cv::drawContours(segmented_map, saved_contours, idx, fill_colour, cv::FILLED); +#endif + already_used_coloures.push_back(fill_colour); //add colour to used coloures + drawn = true; + } + } while (!drawn); + } + //*************************obstacles*********************** + //get obstacle informations and draw them into the new map + ROS_INFO("starting getting obstacle information"); + for (int row = 0; row < map_to_be_labeled.rows; ++row) + { + for (int col = 0; col < map_to_be_labeled.cols; ++col) + { + //find obstacles = black pixels + if (map_to_be_labeled.at(row, col) == 0) + { + segmented_map.at(row, col) = 0; + } + } + } + ROS_INFO("drawn obstacles in map"); + //**************spread the colored region by making white pixel around a contour their color**************** + //spread the coloured regions to the white Pixels + wavefrontRegionGrowing(segmented_map); + ROS_INFO("filled white pixels in new map"); + +} diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/src/raycasting.cpp b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/src/raycasting.cpp new file mode 100644 index 0000000..74eac84 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/src/raycasting.cpp @@ -0,0 +1,194 @@ +#include + +LaserScannerRaycasting::LaserScannerRaycasting() +: precomputed_cos_(360), precomputed_sin_(360) +{ + double pi_to_rad = PI / 180.; + for (int angle = 0; angle < 360; angle++) + { + precomputed_cos_[angle] = std::cos(angle * pi_to_rad); + precomputed_sin_[angle] = std::sin(angle * pi_to_rad); + } +} + +void LaserScannerRaycasting::raycasting(const cv::Mat& map, const cv::Point& location, std::vector& distances) +{ +// cv::Mat test_map = map.clone(); + //Raycasting Algorithm. It simulates the laser measurment at the given location and returns the lengths + //of the simulated beams + double simulated_y, simulated_x, simulated_cos, simulated_sin; + double temporary_distance; + distances.resize(360, 0); + double delta_y, delta_x; + for (int angle = 0; angle < 360; angle++) + { + simulated_cos = precomputed_cos_[angle]; + simulated_sin = precomputed_sin_[angle]; + temporary_distance = 90000001; + for (double distance = 1; distance < 1000000; ++distance) + { + const int ny = location.y + simulated_sin * distance; + const int nx = location.x + simulated_cos * distance; + //make sure the simulated point isn't out of the boundaries of the map + if (ny < 0 || ny >= map.rows || nx < 0 || nx >= map.cols) + break; + if (map.at(ny, nx) == 0 && distance < temporary_distance) + { + temporary_distance = distance; +// cv::line(test_map, cv::Point(location.x, location.y), cv::Point(nx, ny), cv::Scalar(127), 1); + break; + } + } + if (temporary_distance > 90000000) + { + temporary_distance = 10; + } + distances[angle] = temporary_distance; + } + +// cv::circle(test_map, cv::Point(location.x, location.y), 3, cv::Scalar(50), CV_FILLED); +// cv::imshow("simulated angles", test_map); +// cv::waitKey(); +// return distances; +} + +void LaserScannerRaycasting::bresenham_raycasting(const cv::Mat& map, const cv::Point& location, std::vector& distances) +{ + distances.resize(360); + int y_destination, x_destination; + int double_y, double_x; + int dy, dx, ystep, xstep; + int y_current, x_current; + int y_start = location.y; + int x_start = location.x; + int error, previous_error; + bool hit_black_pixel; + //go trough every angle from 0:1:359 + for (int angle = 0; angle < 360; angle++) + { + //set destination Point and difference of the coordinates + y_destination = y_start + std::sin((double)angle * PI / 180.) * 1000; + x_destination = x_start + std::cos((double)angle * PI / 180.) * 1000; + dy = y_destination - y_start; + dx = x_destination - x_start; + //reset the went distance + hit_black_pixel = false; + y_current = 0; + x_current = 0; + //check for quadrant in which the line goes + if (dx < 0) + { + xstep = -1; + dx = -dx; + } + else + { + xstep = 1; + } + + if (dy < 0) + { + ystep = -1; + dy = -dy; + } + else + { + ystep = 1; + } + //set the doubled differences + double_y = 2 * dy; + double_x = 2 * dx; + if (double_y >= double_x) //first octant (0 <= slope <= 1) --> favour y + { + error = dy; + previous_error = error; + //go in direction of the current angle + do + { + y_current += ystep; + error += double_x; + if (error > double_y) + { + x_current += xstep; + error -= double_y; + //check Pixels that vary from bresenham line --> supercover line + if (error + previous_error < double_y) + { + if (map.at(y_start + y_current, x_start + x_current - xstep) == 0) + { + hit_black_pixel = true; + } + } + else if (error + previous_error > double_y) + { + if (map.at(y_start + y_current - ystep, x_start + x_current) == 0) + { + hit_black_pixel = true; + } + } + else + { + if (map.at(y_start + y_current, x_start + x_current - xstep) == 0 + || map.at(y_start + y_current - ystep, x_start + x_current)) + { + hit_black_pixel = true; + } + } + } + //check if next Pixel is a black Pixel + if (map.at(y_start + y_current, x_start + x_current) == 0) + { + hit_black_pixel = true; + } + previous_error = error; + } while (!hit_black_pixel); + distances[angle] = std::sqrt(std::pow(y_current, 2.0) + std::pow(x_current, 2.0)); + } + else // favour x + { + error = dx; + previous_error = error; + //go in direction of the current angle + do + { + x_current += xstep; + error += double_y; + if (error > double_x) + { + y_current += ystep; + error -= double_x; + //check Pixels that vary from bresenham line --> supercover line + if (error + previous_error < double_x) + { + if (map.at(y_start + y_current - ystep, x_start + x_current) == 0) + { + hit_black_pixel = true; + } + } + else if (error + previous_error > double_x) + { + if (map.at(y_start + y_current, x_start + x_current - xstep) == 0) + { + hit_black_pixel = true; + } + } + else + { + if (map.at(y_start + y_current, x_start + x_current - xstep) == 0 + || map.at(y_start + y_current - ystep, x_start + x_current)) + { + hit_black_pixel = true; + } + } + } + //check if next Pixel is a black Pixel + if (map.at(y_start + y_current, x_start + x_current) == 0) + { + hit_black_pixel = true; + } + previous_error = error; + } while (!hit_black_pixel); + distances[angle] = std::sqrt(std::pow(y_current, 2.0) + std::pow(x_current, 2.0)); + } + } +} diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/src/room_class.cpp b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/src/room_class.cpp new file mode 100644 index 0000000..f48d94c --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/src/room_class.cpp @@ -0,0 +1,257 @@ +#include + +Room::Room(int id_of_room) +{ + id_number_ = id_of_room; + //initial values for the area and perimeter + room_area_ = 0; + room_perimeter_ = 0; +} + +void Room::mergeRoom(Room& room_to_merge, double map_resolution) +{ + // member_points_, room_area_ + insertMemberPoints(room_to_merge.getMembers(), map_resolution); + + // neighbor_room_ids_ + const std::vector& neighbor_ids = room_to_merge.getNeighborIDs(); + +// std::cout << "neighbor_room_ids_:\n"; +// for (size_t i=0; i::iterator it = neighbor_room_ids_.begin(); it != neighbor_room_ids_.end();) + { + if (*it == id_number_ || *it == room_to_merge.getID()) + neighbor_room_ids_.erase(it); + else + ++it; + } + +// std::cout << "neighbor_room_ids_after_merge:\n"; +// for (size_t i=0; i::const_iterator it = room_to_merge.getNeighborStatistics().begin(); it != room_to_merge.getNeighborStatistics().end(); ++it) + { + if (it->first != id_number_) + { + if (neighbor_room_statistics_.find(it->first) != neighbor_room_statistics_.end()) + neighbor_room_statistics_[it->first] += it->second; + else + neighbor_room_statistics_[it->first] = it->second; + } + } + neighbor_room_statistics_.erase(room_to_merge.getID()); +} + +//function to add a Point to the Room +int Room::insertMemberPoint(cv::Point new_member, double map_resolution) +{ + if (!contains(member_points_, new_member)) + { + member_points_.push_back(new_member); + room_area_ += map_resolution * map_resolution; + return 0; + } + return 1; +} + +//function to add a few Points +int Room::insertMemberPoints(const std::vector& new_members, double map_resolution) +{ + for (size_t point = 0; point < new_members.size(); point++) + { + if (!contains(member_points_, new_members[point])) + { + member_points_.push_back(new_members[point]); + } + } + room_area_ += map_resolution * map_resolution * new_members.size(); // todo: theoretically might count too much area if member_points_ and new_members are not disjunct + return 0; +} + +//function to add a neighbor to the room statistics +void Room::addNeighbor(int new_neighbor_id) +{ + if (neighbor_room_statistics_.find(new_neighbor_id) == neighbor_room_statistics_.end()) + neighbor_room_statistics_[new_neighbor_id] = 1; + else + neighbor_room_statistics_[new_neighbor_id]++; +} + +//function to add a neighbor to the Room +int Room::addNeighborID(int new_neighbor_id) +{ + if (!contains(neighbor_room_ids_, new_neighbor_id)) + { + neighbor_room_ids_.push_back(new_neighbor_id); + return 0; + } + return 1; +} + +//function to get how many neighbors this room has +int Room::getNeighborCount() +{ + return neighbor_room_ids_.size(); +} + +std::map& Room::getNeighborStatistics() +{ + return neighbor_room_statistics_; +} + +void Room::getNeighborStatisticsInverse(std::map< int,int,std::greater >& neighbor_room_statistics_inverse) +{ + //std::map< int,int,std::greater > neighbor_room_statistics_inverse; // common border length, room_id + for (std::map::iterator it=neighbor_room_statistics_.begin(); it!=neighbor_room_statistics_.end(); ++it) + neighbor_room_statistics_inverse[it->second] = it->first; +} + +int Room::getNeighborWithLargestCommonBorder(bool exclude_wall) +{ + if (neighbor_room_statistics_.size() == 0) + return 0; + + std::map< int,int,std::greater > neighbor_room_statistics_inverse; // common border length, room_id + for (std::map::iterator it=neighbor_room_statistics_.begin(); it!=neighbor_room_statistics_.end(); ++it) + neighbor_room_statistics_inverse[it->second] = it->first; + + if (exclude_wall == true && neighbor_room_statistics_inverse.begin()->second==0 && neighbor_room_statistics_inverse.size() > 1) + { + std::map::iterator it = neighbor_room_statistics_inverse.begin(); + it++; + return it->second; + } + + return neighbor_room_statistics_inverse.begin()->second; +} + +double Room::getPerimeterRatioOfXLargestRooms(const int number_rooms) +{ + if (neighbor_room_statistics_.size() == 0) + return 0; + + std::map< int,int,std::greater > neighbor_room_statistics_inverse; // common border length, room_id + for (std::map::iterator it=neighbor_room_statistics_.begin(); it!=neighbor_room_statistics_.end(); ++it) + neighbor_room_statistics_inverse[it->second] = it->first; + + int counter = 0; + double value = 0.; + for (std::map::iterator it=neighbor_room_statistics_inverse.begin(); it!=neighbor_room_statistics_inverse.end() && counterfirst; + if (it->second != 0) + counter++; + } + + return value/getPerimeter(); +} + +double Room::getWallToPerimeterRatio() +{ + double value = 0.; + if (neighbor_room_statistics_.find(0) != neighbor_room_statistics_.end()) + value = neighbor_room_statistics_[0]/getPerimeter(); + + return value; +} + +std::vector& Room::getNeighborIDs() +{ + return neighbor_room_ids_; +} + +//function to get the area of this room, which has been set previously +double Room::getArea() +{ + if (room_area_ != 0) + { + return room_area_; + } + std::cout << "Warning: Room Area hasn't been set for this room." << std::endl; + return -1; +} + +//function to get the perimeter of this room, which has been set previously +double Room::getPerimeter() +{ +// if (room_perimeter_ != 0) +// { +// return room_perimeter_; +// } +// std::cout << "Warning: Room Perimeter hasn't been set for this room." << std::endl; + room_perimeter_ = 0.; + for (std::map::iterator it=neighbor_room_statistics_.begin(); it!=neighbor_room_statistics_.end(); ++it) + room_perimeter_ += it->second; + + return room_perimeter_; +} + +//function to get the ID number of this room +int Room::getID() const +{ + return id_number_; +} + +cv::Point Room::getCenter() +{ + cv::Scalar center = cv::mean(member_points_); + return cv::Point(center[0], center[1]); +} + +//function to get the Members of this room +const std::vector& Room::getMembers() +{ + if (member_points_.size() == 0) + { + std::cout << "Warning: This room has no members." << std::endl; + } + return member_points_; +} + +//This function sets the room ID to a different value. This is useful for merging different rooms together. +int Room::setRoomId(int new_value, cv::Mat& map) +{ + for (int y = 0; y < map.rows; y++) + { + for (int x = 0; x < map.cols; x++) + { + if (map.at(y, x) == id_number_) + { + map.at(y, x) = new_value; + } + } + } + id_number_ = new_value; + return 0; +} + +//function to set the area of this room +int Room::setArea(double room_area) +{ + room_area_ = room_area; + return 0; +} + +//function to set the perimeter of this room +int Room::setPerimeter(double room_perimeter) +{ + room_perimeter_ = room_perimeter; + return 0; +} + +bool sortRoomsAscending(Room a, Room b) +{ + return (a.getArea() < b.getArea()); +} diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/src/segmentation_tester.cpp b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/src/segmentation_tester.cpp new file mode 100644 index 0000000..3c5d571 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/src/segmentation_tester.cpp @@ -0,0 +1,208 @@ +#include + +#include + +#include +#include + +#include + + +int main() +{ + std::string package_path = ros::package::getPath("ipa_room_segmentation"); + std::string map_path = package_path + "/common/files/test_maps/"; + + std::vector map_names; +// map_names.push_back("office_b.png"); + map_names.push_back("office_e.png"); +// map_names.push_back("NLB.png"); + map_names.push_back("lab_ipa.png"); + map_names.push_back("NLB_furnitures.png"); +// map_names.push_back("office_e_furnitures.png"); +// map_names.push_back("lab_c_scan_furnitures.png"); + + std::vector maps(map_names.size()); + + for(size_t i = 0; i < map_names.size(); ++i) + { + cv::Mat map = cv::imread(map_path + map_names[i], 0); + + for(unsigned int u = 0; u < map.rows; ++u) + { + for(unsigned int v = 0; v < map.cols; ++v) + { + if(map.at(u,v) < 250) + { + map.at(u,v) = 0; + } + else + { + map.at(u,v) = 255; + } + } + } + +// cv::imshow("test", map); +// cv::waitKey(); + + maps[i] = map.clone(); + } + + std::vector possible_labels(3); // vector that stores the possible labels that are drawn in the training maps. Order: room - hallway - doorway + possible_labels[0] = 77; + possible_labels[1] = 115; + possible_labels[2] = 179; + + // strings that stores the path to the saving files + std::string conditional_weights_path = package_path + "/common/files/classifier_models/conditional_field_weights.txt"; + std::string boost_file_path = package_path + "/common/files/classifier_models/"; + + // optimal result saving path + std::string conditional_weights_optimal_path = package_path + "/common/files/optimal_vrf_res/conditional_field_weights.txt"; + std::string boost_file_optimal_path = package_path + "/common/files/optimal_vrf_res/"; + + // load the training maps + cv::Mat training_map; + std::vector training_maps; + training_map = cv::imread(package_path + "/common/files/training_maps/voronoi_random_field_training/training_maps/training_Fr52.png", 0); + training_maps.push_back(training_map); + training_map = cv::imread(package_path + "/common/files/training_maps/voronoi_random_field_training/training_maps/training_Fr101.png", 0); + training_maps.push_back(training_map); + training_map = cv::imread(package_path + "/common/files/training_maps/voronoi_random_field_training/training_maps/training_intel.png", 0); + training_maps.push_back(training_map); + training_map = cv::imread(package_path + "/common/files/training_maps/voronoi_random_field_training/training_maps/training_lab_d_furniture.png", 0); + training_maps.push_back(training_map); + training_map = cv::imread(package_path + "/common/files/training_maps/voronoi_random_field_training/training_maps/training_lab_ipa.png", 0); + training_maps.push_back(training_map); + training_map = cv::imread(package_path + "/common/files/training_maps/voronoi_random_field_training/training_maps/training_NLB_furniture.png", 0); + training_maps.push_back(training_map); + training_map = cv::imread(package_path + "/common/files/training_maps/voronoi_random_field_training/training_maps/training_office_e.png", 0); + training_maps.push_back(training_map); + training_map = cv::imread(package_path + "/common/files/training_maps/voronoi_random_field_training/training_maps/training_office_h.png", 0); + training_maps.push_back(training_map); + training_map = cv::imread(package_path + "/common/files/training_maps/voronoi_random_field_training/training_maps/training_lab_c_furnitures.png", 0); + training_maps.push_back(training_map); + // load the voronoi maps + std::vector voronoi_maps; + training_map = cv::imread(package_path + "/common/files/training_maps/voronoi_random_field_training/voronoi_maps/Fr52_voronoi.png", 0); + voronoi_maps.push_back(training_map); + training_map = cv::imread(package_path + "/common/files/training_maps/voronoi_random_field_training/voronoi_maps/Fr101_voronoi.png", 0); + voronoi_maps.push_back(training_map); + training_map = cv::imread(package_path + "/common/files/training_maps/voronoi_random_field_training/voronoi_maps/lab_intel_voronoi.png", 0); + voronoi_maps.push_back(training_map); + training_map = cv::imread(package_path + "/common/files/training_maps/voronoi_random_field_training/voronoi_maps/lab_d_furnitures_voronoi.png", 0); + voronoi_maps.push_back(training_map); + training_map = cv::imread(package_path + "/common/files/training_maps/voronoi_random_field_training/voronoi_maps/lab_ipa_voronoi.png", 0); + voronoi_maps.push_back(training_map); + training_map = cv::imread(package_path + "/common/files/training_maps/voronoi_random_field_training/voronoi_maps/NLB_voronoi.png", 0); + voronoi_maps.push_back(training_map); + training_map = cv::imread(package_path + "/common/files/training_maps/voronoi_random_field_training/voronoi_maps/office_e_voronoi.png", 0); + voronoi_maps.push_back(training_map); + training_map = cv::imread(package_path + "/common/files/training_maps/voronoi_random_field_training/voronoi_maps/office_h_voronoi.png", 0); + voronoi_maps.push_back(training_map); + training_map = cv::imread(package_path + "/common/files/training_maps/voronoi_random_field_training/voronoi_maps/lab_c_furnitures_voronoi.png", 0); + voronoi_maps.push_back(training_map); + // load the voronoi-nodes maps + std::vector voronoi_node_maps; + training_map = cv::imread(package_path + "/common/files/training_maps/voronoi_random_field_training/voronoi_node_maps/Fr52_voronoi_nodes.png", 0); + voronoi_node_maps.push_back(training_map); + training_map = cv::imread(package_path + "/common/files/training_maps/voronoi_random_field_training/voronoi_node_maps/Fr101_voronoi_nodes.png", 0); + voronoi_node_maps.push_back(training_map); + training_map = cv::imread(package_path + "/common/files/training_maps/voronoi_random_field_training/voronoi_node_maps/lab_intel_voronoi_nodes.png", 0); + voronoi_node_maps.push_back(training_map); + training_map = cv::imread(package_path + "/common/files/training_maps/voronoi_random_field_training/voronoi_node_maps/lab_d_furnitures_voronoi_nodes.png", 0); + voronoi_node_maps.push_back(training_map); + training_map = cv::imread(package_path + "/common/files/training_maps/voronoi_random_field_training/voronoi_node_maps/lab_ipa_voronoi_nodes.png", 0); + voronoi_node_maps.push_back(training_map); + training_map = cv::imread(package_path + "/common/files/training_maps/voronoi_random_field_training/voronoi_node_maps/NLB_voronoi_nodes.png", 0); + voronoi_node_maps.push_back(training_map); + training_map = cv::imread(package_path + "/common/files/training_maps/voronoi_random_field_training/voronoi_node_maps/office_e_voronoi_nodes.png", 0); + voronoi_node_maps.push_back(training_map); + training_map = cv::imread(package_path + "/common/files/training_maps/voronoi_random_field_training/voronoi_node_maps/office_h_voronoi_nodes.png", 0); + voronoi_node_maps.push_back(training_map); + training_map = cv::imread(package_path + "/common/files/training_maps/voronoi_random_field_training/voronoi_node_maps/lab_c_furnitures_voronoi_nodes.png", 0); + voronoi_node_maps.push_back(training_map); + // load the original maps + std::vector original_maps; + training_map = cv::imread(package_path + "/common/files/training_maps/voronoi_random_field_training/original_maps/Fr52_original.png", 0); + original_maps.push_back(training_map); + training_map = cv::imread(package_path + "/common/files/training_maps/voronoi_random_field_training/original_maps/Fr101_original.png", 0); + original_maps.push_back(training_map); + training_map = cv::imread(package_path + "/common/files/training_maps/voronoi_random_field_training/original_maps/lab_intel_original.png", 0); + original_maps.push_back(training_map); + training_map = cv::imread(package_path + "/common/files/training_maps/voronoi_random_field_training/original_maps/lab_d_furnitures_original.png", 0); + original_maps.push_back(training_map); + training_map = cv::imread(package_path + "/common/files/training_maps/voronoi_random_field_training/original_maps/lab_ipa_original.png", 0); + original_maps.push_back(training_map); + training_map = cv::imread(package_path + "/common/files/training_maps/voronoi_random_field_training/original_maps/NLB_original.png", 0); + original_maps.push_back(training_map); + training_map = cv::imread(package_path + "/common/files/training_maps/voronoi_random_field_training/original_maps/office_e_original.png", 0); + original_maps.push_back(training_map); + training_map = cv::imread(package_path + "/common/files/training_maps/voronoi_random_field_training/original_maps/office_h_original.png", 0); + original_maps.push_back(training_map); + training_map = cv::imread(package_path + "/common/files/training_maps/voronoi_random_field_training/original_maps/lab_c_furnitures_original.png", 0); + original_maps.push_back(training_map); + + std::cout << training_maps.size() << " " << voronoi_maps.size() << " " << voronoi_node_maps.size() << " " << original_maps.size() << std::endl; +// for(size_t i = 0; i < training_maps.size(); ++i) +// { +// cv::imshow("training_map", training_maps[i]); +// cv::imshow("voronoi_map", voronoi_maps[i]); +// cv::imshow("nodes", voronoi_node_maps[i]); +// cv::imshow("original_maps", original_maps[i]); +// cv::waitKey(); +// } + + double map_resolution = 0.05; + double room_lower_limit_voronoi_ = 1.53; //1.53; + double room_upper_limit_voronoi_ = 1000000.; //120.0; + + std::vector door_points; + + VoronoiRandomFieldSegmentation segmenter(false, false); + +// segmenter.trainAlgorithms(training_maps, voronoi_maps, voronoi_node_maps, original_maps, possible_labels, conditional_weights_path, boost_file_path); + + for(size_t i = 0; i < map_names.size(); ++i) + { + segmenter.segmentMap(maps[i], maps[i], 5, 50, 4, possible_labels, 7, true, conditional_weights_path, boost_file_path, 9000, map_resolution, room_lower_limit_voronoi_, room_upper_limit_voronoi_, 12.5, &door_points); + + std::cout << "number of doorpoints: " << door_points.size() << std::endl; + door_points.clear(); +// cv::imshow("res", maps[i]); +// cv::waitKey(); + } + +// segmenter.testFunc(maps[0]); +// +// // Do several training steps and segment different maps to find the best training-result. This is done by checking the complete +// // crf-potentials. +// +// double best_potential = 0; +// +// for(size_t training = 1; training <= 10; ++training) +// { +// if(training != 1) +// segmenter.trainAlgorithms(training_maps, voronoi_maps, voronoi_node_maps, original_maps, possible_labels, conditional_weights_path, boost_file_path); +// +// double current_potential = 0; +// +// for(size_t i = 0; i < 6; ++i) +// { +// cv::Mat map = maps[i].clone(); +// +// current_potential += segmenter.segmentMap(map, map, 7, 50, 5, possible_labels, 7, true, conditional_weights_path, boost_file_path, 9000, map_resolution, room_lower_limit_voronoi_, room_upper_limit_voronoi_); // 7, 50, 4, 5 +// } +// +// std::cout << std::endl << "********** Step: " << training << ". current_potential: " << current_potential << std::endl; +// +// if(current_potential > best_potential) +// { +// best_potential = current_potential; +// segmenter.testFunc(conditional_weights_optimal_path, boost_file_optimal_path); +// } +// } + + return 0; +} diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/src/voronoi_random_field_features.cpp b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/src/voronoi_random_field_features.cpp new file mode 100644 index 0000000..5292cb7 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/src/voronoi_random_field_features.cpp @@ -0,0 +1,1052 @@ +#include + +#define PI 3.14159265 + +// structure to perform breadth-first-search to detect minimal loops + +//get the number of implemented features. Needs to be changed to the new value if you change it +int voronoiRandomFieldFeatures::getFeatureCount() +{ + return 28; +} + +// reset the saved features +void voronoiRandomFieldFeatures::resetCachedData() +{ + features_.clear(); + features_.resize(getFeatureCount(), 0.); + features_computed_.clear(); + features_computed_.resize(getFeatureCount(), false); + + polygon_.clear(); + polygon_computed_ = false; + + centroid_computed_ = false; +} + +//**********************see features.h for a better overview of what is calculated and needed************************* +//Method for calculating the feature for the classifier +double voronoiRandomFieldFeatures::getFeature(const std::vector& beams, const std::vector& angles, + const std::vector& clique_points, std::vector& labels_for_clique_points, + std::vector& possible_labels, cv::Point point, const int feature) +{ + switch (feature) + { + case 1: + return calcFeature1(beams); + case 2: + return calcFeature2(beams); + case 3: + return calcFeature3(beams, 30); + case 4: + return calcFeature4(beams, 30); + case 5: + return calcFeature5(beams); + case 6: + return calcFeature6(beams); + case 7: + return calcFeature7(beams); + case 8: + return calcFeature8(beams, angles); + case 9: + return calcFeature9(beams, angles); + case 10: + return calcFeature10(beams); + case 11: + return calcFeature11(beams); + case 12: + return calcFeature12(beams); + case 13: + return calcFeature13(beams); + case 14: + return calcFeature14(beams, angles, point); + case 15: + return calcFeature15(beams, angles, point); + case 16: + return calcFeature16(beams, angles, point); + case 17: + return calcFeature17(beams, angles, point); + case 18: + return calcFeature18(beams, angles, point); + case 19: + return calcFeature19(beams, angles, point); + case 20: + return calcFeature20(beams, angles, point); + case 21: + return calcFeature21(beams, angles, point); + case 22: + return calcFeature22(beams); + case 23: + return calcFeature23(beams); + case 24: + return calcFeature24(clique_points); + case 25: + return calcFeature25(possible_labels, labels_for_clique_points); + case 26: + return calcFeature26(beams, 22); + case 27: + return calcFeature27(beams, angles, 8, point); + case 28: + return calcFeature28(beams, 5); + default: + return -1; + } +} + +void voronoiRandomFieldFeatures::getFeatures(const std::vector& beams, const std::vector& angles, const std::vector& clique_points, std::vector& labels_for_clique_points, + std::vector& possible_labels, cv::Point point, std::vector& features) +{ + // reset internal data storage + resetCachedData(); + + // compute features + calcFeature1(beams); + calcFeature2(beams); + calcFeature3(beams, 30); + calcFeature4(beams, 30); + calcFeature5(beams); + calcFeature6(beams); + calcFeature7(beams); + calcFeature8(beams, angles); + calcFeature9(beams, angles); + calcFeature10(beams); + calcFeature11(beams); + calcFeature12(beams); + calcFeature13(beams); + calcFeature14(beams, angles, point); + calcFeature15(beams, angles, point); + calcFeature16(beams, angles, point); + calcFeature17(beams, angles, point); + calcFeature18(beams, angles, point); + calcFeature19(beams, angles, point); + calcFeature20(beams, angles, point); + calcFeature21(beams, angles, point); + calcFeature22(beams); + calcFeature23(beams); + calcFeature24(clique_points); + calcFeature25(possible_labels, labels_for_clique_points); + calcFeature26(beams, 22); + calcFeature27(beams, angles, 8, point); + calcFeature28(beams, 5); + + // write features + features.clear(); + features = features_; +} + +//Calculation of Feature 1: average difference of the beams +double voronoiRandomFieldFeatures::calcFeature1(const std::vector& beams) +{ + if (features_computed_[0]) + return features_[0]; + + double differences_sum = 0; + for (int b = 0; b < beams.size() - 1; b++) + { + differences_sum += abs(beams[b] - beams[b + 1]); + } + //get the difference between the last and the first beam + differences_sum += abs(beams[beams.size() - 1] - beams[0]); + + // set the cache of the calculated feature + features_computed_[0] = true; + features_[0] = (differences_sum / (double)beams.size()); + + //calculate the average difference and return it + return features_[0]; +} + +//Calculation of Feature 2: standard deviation of the difference of the beams +double voronoiRandomFieldFeatures::calcFeature2(const std::vector& beams) +{ + if (features_computed_[1]) + return features_[1]; + + double mean; //mean-value of the difference, calculated with calcFeature1 + double sum = 0; //helping variable + //initialize + mean = calcFeature1(beams); + //calculate deviation + for (int b = 0; b < beams.size(); b++) + { + sum += std::pow((beams[b] - mean), 2.0); + } +// std::cout << "f2: (" << sum << ") "; + sum = sum / (double) (beams.size() - 1); + + features_computed_[1] = true; + features_[1] = std::sqrt(sum); + + return features_[1]; +} + +//Calculation of Feature 3: average difference of the to a max_value limited beams +double voronoiRandomFieldFeatures::calcFeature3(const std::vector& beams, double maxval) +{ + if (features_computed_[2]) + return features_[2]; + + double differences_sum = 0; + double val1, val2; + for (int b = 0; b < beams.size()-1; b++) + { + //reset saved beamlenghts + val1 = maxval; + val2 = maxval; + if (beams[b] < maxval) + { + val1 = beams[b]; + } + if (beams[b + 1] < maxval) + { + val2 = beams[b + 1]; + } + differences_sum += abs(val1 - val2); + } + //get the difference between the last and the first beam + val1 = maxval; + val2 = maxval; + if (beams[beams.size()-1] < maxval) + { + val1 = beams[beams.size()-1]; + } + if (beams[0] < maxval) + { + val2 = beams[0]; + } + differences_sum += abs(val1 - val2); + //calculate the average difference and return it + features_computed_[2] = true; + features_[2] = (differences_sum / (double)beams.size()); + + return features_[2]; +} + +//Calculation of Feature 4: The Standard Deviation of the difference of limited beams +double voronoiRandomFieldFeatures::calcFeature4(const std::vector& beams, double maxval) +{ + if (features_computed_[3]) + return features_[3]; + + double mean; //mean-value of the difference, calculated with calcFeature1 + double v, w, difference, sum; //helping variables + //initialise + mean = calcFeature3(beams, maxval); + sum = 0; + //calculate deviation + for (int b = 0; b < beams.size() - 1; b++) + { + //reset value of current beam + v = maxval; + w = maxval; + if (beams[b] < maxval) + { + v = beams[b]; + } + if (beams[b + 1] < maxval) + { + w = beams[b + 1]; + } + difference = abs(v - w); + sum += std::pow((difference - mean), 2.0); + } + //add the difference from last to first point + v = maxval; + w = maxval; + if (beams[beams.size()-1] < maxval) + { + v = beams[beams.size()-1]; + } + if (beams[0] < maxval) + { + w = beams[0]; + } + difference = abs(v - w); + sum += std::pow((difference - mean), 2.0); + sum = sum / (beams.size() - 1); + + features_computed_[3] = true; + features_[3] = std::sqrt(sum); + + return features_[3]; +} + +//Calculation of Feature 5: The average beamlength +double voronoiRandomFieldFeatures::calcFeature5(const std::vector& beams) +{ + if (features_computed_[4]) + return features_[4]; + + double sum = 0; + //get the sum of the beamlengths + for (int b = 0; b < beams.size(); b++) + { + sum += beams[b]; + } + //divide by number of beams and return value + features_computed_[4] = true; + features_[4] = (sum / (double)beams.size()); + + return features_[4]; +} + +//Calculation of Feature 6: The standard deviation of the beamlenghts +double voronoiRandomFieldFeatures::calcFeature6(const std::vector& beams) +{ + if (features_computed_[5]) + return features_[5]; + + double mean; //mean-value of the beamlenghts, calculated with calcFeature5 + double sum, res; //helping variables + //initialize + mean = calcFeature5(beams); + sum = 0; + //calculate deviation + for (int b = 0; b < beams.size(); b++) + { + sum += std::pow((beams[b] - mean), 2.0); + } + res = sum / (double)(beams.size() - 1); +// std::cout << "f6: (" << res << ") "; + + features_computed_[5] = true; + features_[5] = std::sqrt(res); + + return features_[5]; +} + +//Calculation of Feature 7: The number of gaps between the beams, a gap is when the difference of the lenghts is larger +//than a specified threshold +double voronoiRandomFieldFeatures::calcFeature7(const std::vector& beams) +{ + if (features_computed_[6]) + return features_[6]; + + double threshold = 10; //[pixel], see "Semantic labeling of places" + double gaps = 0; + for (int b = 0; b < beams.size() - 1; b++) + { + if (abs(beams[b] - beams[b + 1]) > threshold) + { + gaps++; + } + } + if (abs(beams[beams.size() - 1] - beams[0]) > threshold) + { + gaps++; + } + features_computed_[6] = true; + features_[6] = gaps; + + return gaps; +} + +//Calculation of feature 8: The distance between two Endpoints of local minima of beamlenghts +double voronoiRandomFieldFeatures::calcFeature8(const std::vector& beams, const std::vector& angles) +{ + if (features_computed_[7]) + return features_[7]; + + //Remark: angles are relatively to the robot + double length_1 = 10000000; + double length_2 = 10000000; + double angle_1 = angles[0], angle_2 = angles[1]; + double x_1, y_1, x_2, y_2; + //get the two Points corresponding to minimal beamlength + for (int b = 0; b < beams.size(); b++) + { + if (beams[b] < length_1 && beams[b] > length_2) + { + length_1 = beams[b]; + angle_1 = angles[b]; + } + else if (beams[b] < length_2) + { + length_2 = beams[b]; + angle_2 = angles[b]; + } + } + //calculate the x/y-values of the Points + x_1 = std::cos(angle_1 * PI / 180) * length_1; + y_1 = std::sin(angle_1 * PI / 180) * length_1; + x_2 = std::cos(angle_2 * PI / 180) * length_2; + y_2 = std::sin(angle_2 * PI / 180) * length_2; + + //calculate and return the euclidean distance between the Points + features_computed_[7] = true; + features_[7] = std::sqrt(std::pow((x_1 - x_2), 2) + std::pow((y_1 - y_2), 2)); + + return features_[7]; +} + +//Calculate Feature 9: The Angle between two Endpoints of local minima of beamlengths +double voronoiRandomFieldFeatures::calcFeature9(const std::vector& beams, const std::vector& angles) +{ + if (features_computed_[8]) + return features_[8]; + + //Remark: angles are relatively to the robot + double length_1 = beams[0]; + double length_2 = beams[1]; + double angle_1 = angles[0]; + double angle_2 = angles[1]; + double x_1, y_1, x_2, y_2; + //get the two Points corresponding to minimal beamlengths + for (int b = 0; b < beams.size(); b++) + { + if (beams[b] < length_1 && beams[b] > length_2) + { + length_1 = beams[b]; + angle_1 = angles[b]; + } + else if (beams[b] <= length_2) + { + length_2 = beams[b]; + angle_2 = angles[b]; + } + } + //calculate the x/y-values of the Points + double pi_to_degree = PI / 180; + x_1 = std::cos(angle_1 * pi_to_degree) * length_1; + y_1 = std::sin(angle_1 * pi_to_degree) * length_1; + x_2 = std::cos(angle_2 * pi_to_degree) * length_2; + y_2 = std::sin(angle_2 * pi_to_degree) * length_2; + //calculate and return the angle between the Points + double coordvec = (x_1 * x_2) + (y_1 * y_2); + double absvec = (length_1 * length_2); + const double quot = std::max(-1., std::min(1., coordvec / absvec)); + // if the raycasting-algorithm only found zero-beams return 0 + features_computed_[8] = true; + features_[8] = std::acos(quot) * 180.0 / PI; + + return features_[8]; +} + +//Calculate Feature 10: The average of the relations (b_i/b_(i+1)) between two neighboring beams +double voronoiRandomFieldFeatures::calcFeature10(const std::vector& beams) +{ + if (features_computed_[9]) + return features_[9]; + + double length_1, length_2; + double sum_relation = 0; + //calculate the relations and add it to the sum + for (int b = 0; b < beams.size() - 1; b++) + { + length_1 = beams[b]; + length_2 = beams[b + 1]; + if (length_1 < length_2) + { + sum_relation += (length_1 / length_2); + } + else + { + sum_relation += (length_2 / length_1); + } + } + length_1 = beams[beams.size() - 1]; + length_2 = beams[0]; + if (length_1 < length_2) + { + sum_relation += (length_1 / length_2); + } + else + { + sum_relation += (length_2 / length_1); + } + //calculate and return the average of the relations + features_computed_[9] = true; + features_[9] = (sum_relation / (double)beams.size()); + + return features_[9]; +} + +//Calculate Feature 11: The standard deviation of the relations (b_i/b_(i+1)) between two neighboring beams +double voronoiRandomFieldFeatures::calcFeature11(const std::vector& beams) +{ + if (features_computed_[10]) + return features_[10]; + + //calculate the mean of the relations by using Feature 10 + double mean = calcFeature10(beams); + double sum = 0; + //calculate the standard_deviation + for (int b = 0; b < beams.size(); b++) + { + sum += std::pow((beams[b] - mean), 2); + } + sum = sum / (double)(beams.size() - 1); + features_computed_[10] = true; + features_[10] = std::sqrt(sum); + + return features_[10]; +} + +//Calculate Feature 12: The number of relative gaps. A relative gap is when the relation (b_i/b_(i+1)) is smaller than a +//specified threshold +double voronoiRandomFieldFeatures::calcFeature12(const std::vector& beams) +{ + if (features_computed_[11]) + return features_[11]; + + double threshold = 0.85; + double gaps = 0; + double length_1, length_2; + for (int b = 0; b < beams.size() - 1; b++) + { + length_1 = beams[b]; + length_2 = beams[b + 1]; + if (length_1 < length_2) + { + if ((length_1 / length_2) < threshold) + { + gaps++; + } + } + else + { + if ((length_2 / length_1) < threshold) + { + gaps++; + } + } + } + length_1 = beams[0]; + length_2 = beams[beams.size() - 1]; + if (length_1 < length_2) + { + if ((length_1 / length_2) < threshold) + { + gaps++; + } + } + else + { + if ((length_2 / length_1) < threshold) + { + gaps++; + } + } + features_computed_[11] = true; + features_[11] = gaps; + + return features_[11]; +} + +//Calculate Feature 13: The Kurtosis, which is given by: +//(Sum((x - mean)^4))/sigma^4) - 3, where mean is the mean-value and sigma is the standard deviation +double voronoiRandomFieldFeatures::calcFeature13(const std::vector& beams) +{ + if (features_computed_[12]) + return features_[12]; + + double sum = 0; + //get the standard deviation and the mean by using previous functions + double sigma = calcFeature6(beams); + double mean = calcFeature5(beams); + //calculate the Kurtosis + for (int b = 0; b < beams.size(); b++) + { + sum += std::pow((beams[b] - mean), 4); + } + features_computed_[12] = true; + features_[12] = ((sum / std::pow(sigma, 4)) - 3); + + return features_[12]; +} + +//Calc Feature 22: The average of the beam lengths divided by the maximal length +double voronoiRandomFieldFeatures::calcFeature22(const std::vector& beams) +{ + if (features_computed_[21]) + return features_[21]; + + double sum = 0; + double maxval = 0; + //find maximal value of the beams + for (int b = 0; b < beams.size(); b++) + { + if (beams[b] > maxval) + { + maxval = beams[b]; + } + } + //get the average of the beams/maxval + //get the sum of the beamlengths + for (int b = 0; b < beams.size(); b++) + { + sum += (beams[b] / maxval); + } + //divide by number of beams and return value + features_computed_[21] = true; + features_[21] = (sum / (double)beams.size()); + + return features_[21]; +} + +//Calculate Feature 23: The standard deviation of the beam lengths divided by the maximal length +double voronoiRandomFieldFeatures::calcFeature23(const std::vector& beams) +{ + if (features_computed_[22]) + return features_[22]; + + double sum = 0; + double mean = calcFeature22(beams); + double maxval = 0; + //find maximal value of the beams + for (int b = 0; b < beams.size(); b++) + { + if (beams[b] > maxval) + { + maxval = beams[b]; + } + } + //get the standard deviation + for (int b = 0; b < beams.size(); b++) + { + sum += std::pow(((beams[b] / maxval) - mean), 2); + } + sum = sum / (double) (beams.size() - 1); + + features_computed_[22] = true; + features_[22] = std::sqrt(sum); + + return features_[22]; +} + +//*******************Features based on a polygonal approximation of the beams******************* +//Calculate the polygonal approximation +std::vector voronoiRandomFieldFeatures::calcPolygonalApprox(const std::vector& beams, const std::vector& angles, cv::Point location) +{ + if (polygon_computed_ == true) + return polygon_; + + polygon_.clear(); + double x, y; + //calculate the endpoint for every beam and add it to the polygon + for (int b = 0; b < beams.size(); b++) + { //calculate the x/y-values + //Remark: angles in radian + double pi_to_degree = PI / 180; + x = std::cos(angles[b] * pi_to_degree) * beams[b]; + y = std::sin(angles[b] * pi_to_degree) * beams[b]; + polygon_.push_back(cv::Point(location.x + x, location.y + y)); + } + + polygon_computed_ = true; + return polygon_; +} + +//Calculate the centroid of the polygonal approximation +cv::Point voronoiRandomFieldFeatures::calcCentroid(const std::vector& beams, const std::vector& angles, cv::Point location) +{ + if (centroid_computed_ == true) + return centroid_; + + double x, y; + double sumX = 0; + double sumY = 0; + //get every Point by using the polygonal approximation + std::vector < cv::Point > polygon = calcPolygonalApprox(beams, angles, location); + for (int p = 0; p < polygon.size(); p++) + { + sumX += polygon[p].x; + sumY += polygon[p].y; + } + + centroid_.x = sumX / (double) polygon.size(); + centroid_.y = sumY / (double) polygon.size(); + centroid_computed_ = true; + return centroid_; +} + +//Calculate Feature 14: The area of the polygonal approximation of the beams +double voronoiRandomFieldFeatures::calcFeature14(const std::vector& beams, const std::vector& angles, cv::Point location) +{ + if (features_computed_[13]) + return features_[13]; + + double map_resolution = 0.05000; + std::vector < cv::Point > polygon = calcPolygonalApprox(beams, angles, location); + + features_computed_[13] = true; + features_[13] = map_resolution * map_resolution * cv::contourArea(polygon); + + return features_[13]; +} + +//Calculate Feature 15: The perimeter of the polygonal approximation of the beams +double voronoiRandomFieldFeatures::calcFeature15(const std::vector& beams, const std::vector& angles, cv::Point location) +{ + if (features_computed_[14]) + return features_[14]; + + std::vector < cv::Point > polygon = calcPolygonalApprox(beams, angles, location); + features_computed_[14] = true; + features_[14] = cv::arcLength(polygon, true); + + return features_[14]; +} + +//Calculate Feature 16: The quotient of area divided by perimeter of the polygonal approximation of the beams +double voronoiRandomFieldFeatures::calcFeature16(const std::vector& beams, const std::vector& angles, cv::Point location) +{ + if (features_computed_[15]) + return features_[15]; + + features_computed_[15] = true; + features_[15] = (calcFeature14(beams, angles, location) / calcFeature15(beams, angles, location)); + + return features_[15]; +} + +//Calculate Feature 17: The average of the distance between the centroid and the boundary-Points of the polygonal approximation +double voronoiRandomFieldFeatures::calcFeature17(const std::vector& beams, const std::vector& angles, cv::Point location) +{ + if (features_computed_[16]) + return features_[16]; + + std::vector < cv::Point > polygon = calcPolygonalApprox(beams, angles, location); + cv::Point centroid = calcCentroid(beams, angles, location); + double sum = 0; + double delta_x, delta_y; + //calculate the distance between the centroid and the boundary and add it to the sum + for (int p = 0; p < polygon.size(); p++) + { + delta_x = polygon[p].x - centroid.x; + delta_y = polygon[p].y - centroid.y; + sum += std::sqrt(std::pow(delta_x, 2) + std::pow(delta_y, 2)); + } + //calculate and return the average of the distances + features_computed_[16] = true; + features_[16] = (sum / (double) polygon.size()); + + return features_[16]; +} + +//Calculate Feature 18: The standard deviation of the distance between the centroid and the boundary-Points +double voronoiRandomFieldFeatures::calcFeature18(const std::vector& beams, const std::vector& angles, cv::Point location) +{ + if (features_computed_[17]) + return features_[17]; + + std::vector < cv::Point > polygon = calcPolygonalApprox(beams, angles, location); + cv::Point centroid = calcCentroid(beams, angles, location); + //get the mean of the distances by using Feature 17 + double mean = calcFeature17(beams, angles, location); + double current_distance; + double sum = 0; + //calculate the standard_deviation + for (int p = 0; p < polygon.size(); p++) + { + double delta_x = polygon[p].x - centroid.x; + double delta_y = polygon[p].y - centroid.y; + current_distance = std::sqrt(std::pow(delta_x, 2) + std::pow(delta_y, 2)); + sum += std::pow(current_distance - mean, 2); + } + sum = sum / (double) (beams.size() - 1); + + features_computed_[17] = true; + features_[17] = std::sqrt(sum); + + return features_[17]; +} + +//Calculate Feature 19: The half major axis of the bounding ellipse, calculated with openCV +double voronoiRandomFieldFeatures::calcFeature19(const std::vector& beams, const std::vector& angles, cv::Point location) +{ + if (features_computed_[18]) + return features_[18]; + + std::vector < cv::Point > polygon = calcPolygonalApprox(beams, angles, location); + cv::Point centroid = calcCentroid(beams, angles, location); + cv::Point2f points[4]; + std::vector < cv::Point2f > edge_points; + double distance = 0; + //saving-variable for the Points of the ellipse + cv::RotatedRect ellipse = cv::fitEllipse(cv::Mat(polygon)); + //get the edge-points of the ellipse + ellipse.points(points); + //saving the Points of the ellipse in a vector + for (int i = 0; i < 4; i++) + { + edge_points.push_back(points[i]); + } + //calculate the distance between the Points and take the largest one + for (int p = 0; p < edge_points.size(); p++) + { + for (int np = 0; np < edge_points.size(); np++) + { + if (std::sqrt(std::pow((edge_points[p].x - edge_points[np].x), 2) + std::pow((edge_points[p].y - edge_points[np].y), 2)) > distance) + { + distance = std::sqrt(std::pow((edge_points[p].x - edge_points[np].x), 2) + std::pow((edge_points[p].y - edge_points[np].y), 2)); + } + } + } + + features_computed_[18] = true; + features_[18] = (distance / 2); + + return features_[18]; +} + +//Calculate Feature 20: The half minor axis of the bounding ellipse, calculated with openCV +double voronoiRandomFieldFeatures::calcFeature20(const std::vector& beams, const std::vector& angles, cv::Point location) +{ + if (features_computed_[19]) + return features_[19]; + + std::vector < cv::Point > polygon = calcPolygonalApprox(beams, angles, location); + cv::Point2f points[4]; + std::vector < cv::Point2f > edge_points; + double distance = 1000000; + //saving-variable for the Points of the ellipse + cv::RotatedRect ellipse = cv::fitEllipse(cv::Mat(polygon)); + //get the edge-points of the ellipse + ellipse.points(points); + //saving the Points of the ellipse in a vector + for (int i = 0; i < 4; i++) + { + edge_points.push_back(points[i]); + } + //calculate the distance between the Points and take the largest one + for (int p = 0; p < edge_points.size(); p++) + { + for (int np = 0; np < edge_points.size(); np++) + { + if (std::sqrt(std::pow((edge_points[p].x - edge_points[np].x), 2) + std::pow((edge_points[p].y - edge_points[np].y), 2)) < distance + && std::sqrt(std::pow((edge_points[p].x - edge_points[np].x), 2) + std::pow((edge_points[p].y - edge_points[np].y), 2)) > 0 && p != np) + { + distance = std::sqrt(std::pow((edge_points[p].x - edge_points[np].x), 2) + std::pow((edge_points[p].y - edge_points[np].y), 2)); + } + } + } + + features_computed_[19] = true; + features_[19] = (distance / 2); + + return features_[19]; +} + +//Calculate Feature 21: The Quotient of half the major axis and half the minor axis +double voronoiRandomFieldFeatures::calcFeature21(const std::vector& beams, const std::vector& angles, cv::Point location) +{ + if (features_computed_[20]) + return features_[20]; + + features_computed_[20] = true; + features_[20] = (calcFeature19(beams, angles, location) / calcFeature20(beams, angles, location)); + + return features_[20]; +} + +// Calculate Feature 24: The curvature of the voronoi graph approximated by the points of the clique. The curvature of a graph +// is given by k = 1/r, with r as the radius of the approximate circle at this position. +double voronoiRandomFieldFeatures::calcFeature24(std::vector clique_points) +{ + if (features_computed_[23]) + return features_[23]; + + float radius = 0; + cv::Point2f center; + + // Get the circle that approaches the points of the clique. If the clique has more than three neighbors it is a voronoi-node + // clique, in which case the mean of the curvatures gets calculated. + if(clique_points.size() <= 3) + { + // get the enclosing circle together with the radius of it + cv::minEnclosingCircle(clique_points, center, radius); + } + else + { + // go trough each 3-point combination and calculate the sum of the radius + for(unsigned int curvature = 1; curvature < clique_points.size(); ++curvature) + { + float temporary_radius; + + std::vector temporary_point_vector(3); // vector that stores the current points + + // fill vector + temporary_point_vector[0] = clique_points[0]; + temporary_point_vector[1] = clique_points[curvature]; + // Check if i+1 would be larger than last index. If so take the first point that isn't zero as second point. + if((curvature+1)%clique_points.size() == 0) + temporary_point_vector[2] = clique_points[1]; + else + temporary_point_vector[2] = clique_points[(curvature+1)%clique_points.size()]; + + // calculate the enclosing circle for this combination + cv::minEnclosingCircle(temporary_point_vector, center, temporary_radius); + + // add the current radius to sum + radius += temporary_radius; + } + + // get the mean + radius = radius/((double) clique_points.size()-1); + } + + // return the curvature + features_computed_[23] = true; + features_[23] = 1.0/(double)radius; + + return features_[23]; +} + +// Calculate Feature 25: The relation between the labels of Points from the central point to the other points in the clique. +// If two neighboring points have the labels hallway-hallway this feature gets very high and if they +// are differing from each other it gets small. To do this the possible_labels-vector stores all the +// labels for the different classes that can occur. +// !!!!!! Important: !!!!!! +// The possible_lables-vector stores in this program the labels in the order +// room-hallway-doorway +double voronoiRandomFieldFeatures::calcFeature25(std::vector& possible_labels, std::vector& labels_for_points) +{ + if (features_computed_[24]) + return features_[24]; + + // count how often each possible label occurs in the given labels + int maximal_amount = -1; // integer to save the maximal amount one label occurs + + for(size_t label = 0; label < possible_labels.size(); ++label) + { + // check for one label how often it appears + int label_count = std::count(labels_for_points.begin(), labels_for_points.end(), possible_labels[label]); + + // check if current label appears more often than the previously checked + if(label_count > maximal_amount) + maximal_amount = label_count; + } + + // set the initial value for the feature (40 * maximal_amount, so the feature is always >= 0, even if later a lot of the value gets subtracted) + double feature_value = 40 * maximal_amount; + + // ***Check for possibility that two neighboring labels can occur. For example it is very unlikely that a hallway appears right + // after a room, without a doorway between, but it is very likely that a hallway adds to a hallway. + + // Create a map to get a factor to add or subtract from the inital-value for a feature. The key for this map is the sum of + // two labels, so shows the relation between two points, and the Data for a key is the factor. + std::map label_mapping; + + // create each possible key and data (done by hand because in this case all possible configurations are known before --> room,hallway,doorway) + label_mapping[possible_labels[0] + possible_labels[0]] = 10; // room-room + label_mapping[possible_labels[0] + possible_labels[1]] = -10; // room-hallway + label_mapping[possible_labels[0] + possible_labels[2]] = 5; // room-doorway + label_mapping[possible_labels[1] + possible_labels[1]] = 10; // hallway-hallway + label_mapping[possible_labels[1] + possible_labels[2]] = 5; // hallway-doorway + label_mapping[possible_labels[2] + possible_labels[2]] = 10; // doorway-doorway + + // increase or decrease the feature-value + for(std::vector::iterator current_point = labels_for_points.begin(); current_point != labels_for_points.end(); ++current_point) + { + // check each neighbor that isn't the point itself + for(std::vector::iterator current_neighbor = labels_for_points.begin(); current_neighbor != labels_for_points.end(); ++current_neighbor) + { + // check if the two labels are not from the same point by calculating the distance in the vector between these two + if(std::distance(current_point, current_neighbor) != 0) + { + // get the key for the mapping + unsigned int current_sum = *current_point + *current_neighbor; + + feature_value += label_mapping[current_sum]; + } + } + } + + features_computed_[24] = true; + features_[24] = feature_value; + + return features_[24]; +} + +// Feature 26: number of beams that are shorter than a defined maxval +double voronoiRandomFieldFeatures::calcFeature26(const std::vector& beams, double maxval) +{ + if(features_computed_[25] == true) + return features_[25]; + + int number_of_short_beams = 0; + + for(size_t beam = 0; beam < beams.size(); ++beam) + if(beams[beam] <= maxval) + ++number_of_short_beams; + + features_computed_[25] = true; + features_[25] = (double) number_of_short_beams; + + return features_[25]; +} + +// Feature 27: The are of the bounding box that is calculated for the endpoints of all beams that are limited to a maxval +double voronoiRandomFieldFeatures::calcFeature27(const std::vector& beams, const std::vector& angles, double epsilon, cv::Point location) +{ + if(features_computed_[26] == true) + return features_[26]; + + std::vector short_points; + + // get minimal value of the beamlengths + double min_length = 1e5; + for(size_t beam = 0; beam < beams.size(); ++beam) + if(beams[beam] <= min_length) + min_length = beams[beam]; + + // search for beams which lengths are in the defined epsilon neighborhood + double pi_to_degree = PI / 180; + for(size_t beam = 0; beam < beams.size(); ++beam) + { + if(beams[beam] <= (min_length + epsilon) || beams[beam] > (min_length - epsilon)) + { + double x = std::cos(angles[beam] * pi_to_degree) * beams[beam] + location.x; + double y = std::sin(angles[beam] * pi_to_degree) * beams[beam] + location.y; + short_points.push_back(cv::Point(x, y)); + } + } + + // calculate the bounding box area + cv::RotatedRect bounding_box = cv::minAreaRect(short_points); + + features_computed_[26] = true; + features_[26] = bounding_box.size.area(); + + return features_[26]; +} + +// Feature 28: Ratio of the average lengths of n longest and n smalles beams. +double voronoiRandomFieldFeatures::calcFeature28(const std::vector& beams, double number_of_beams) +{ + if(features_computed_[27] == true) + return features_[27]; + + // find n longest and shortest beams + std::vector longest_beams (number_of_beams); + std::vector shortest_beams (number_of_beams); + std::vector sorted_beams = beams; + + std::sort(sorted_beams.begin(), sorted_beams.end()); + + int index = 0; + for(std::vector::iterator beam = sorted_beams.begin(); beam != sorted_beams.begin()+number_of_beams; ++beam) + { + longest_beams[index] = *beam; + ++index; + } + + index = 0; + for(std::vector::reverse_iterator beam = sorted_beams.rbegin(); beam != sorted_beams.rbegin()+number_of_beams; ++beam) + { + shortest_beams[index] = *beam; + ++index; + } + + // get average beamlengths + double average_longest = std::accumulate(longest_beams.begin(), longest_beams.end(), 0) / (double) longest_beams.size(); + double average_shortest = std::accumulate(shortest_beams.begin(), shortest_beams.end(), 0) / (double) shortest_beams.size(); + + // calculate ratio + features_computed_[27] = true; + features_[27] = average_shortest / average_longest; + + return features_[27]; + +} diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/src/voronoi_random_field_segmentation.cpp b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/src/voronoi_random_field_segmentation.cpp new file mode 100644 index 0000000..40997ca --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/src/voronoi_random_field_segmentation.cpp @@ -0,0 +1,1940 @@ +/*! + ***************************************************************** + * \file + * + * \note + * Copyright (c) 2015 \n + * Fraunhofer Institute for Manufacturing Engineering + * and Automation (IPA) \n\n + * + ***************************************************************** + * + * \note + * Project name: Care-O-bot + * \note + * ROS stack name: autopnp + * \note + * ROS package name: ipa_room_segmentation + * + * \author + * Author: Florian Jordan + * \author + * Supervised by: Richard Bormann + * + * \date Date of creation: 10.2015 + * + * \brief + * + * + ***************************************************************** + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * - Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. \n + * - Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. \n + * - Neither the name of the Fraunhofer Institute for Manufacturing + * Engineering and Automation (IPA) nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. \n + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License LGPL as + * published by the Free Software Foundation, either version 3 of the + * License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License LGPL for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License LGPL along with this program. + * If not, see . + * + ****************************************************************/ + +#include + +#include + +#include +#include + +// This function is the optimization function L(w) = -1 * sum(i)(log(p(y_i|MB(y_i, w), x)) + ((w - w_r)^T (w - w_r)) / 2 * sigma^2) +// to find the optimal weights for the given prelabeled map. to find these the function has to be minimized. +// i indicates the labeled example +// w is the weights vector +// y_i is one node in the build graph +// MB(y_i, w) is the Markov blanket of y_i, in this case the current neighbors of y_i +// w_r is a starting point for w (can be 0 if not known better, so small weights get favorized) +// sigma is the standard deviation, can be chosen freely +// the last part of this function is the gaussian shrinking function to prevent the weights from getting too large +// the local likelihoods are given by the function +// p(y_i|x) = 1/Z(X) * exp(sum(k of K)(w_k^T * f_k(y_k, x))) +// - K are all cliques in the graph +// - w_k is the weightvector, regarding to the calculateable features for this clique +// - f_k is the feature function, calculating a vector of features, that are individual for each clique +// An example for this function, regarding to the voronoi random fields, is: +// L(w) = -(log( (exp(5w_1 + 10w_2)) / (exp(5w_1 + 10w_2) + exp(4w_1 + 7w_2)) ) + log( (exp(7w_1 + 8w_2)) / (exp(7w_1 + 8w_2) + exp(4w_1 + 1w_2)) )) + (w_1-2 w_2-2)^T * (w_1-2 w_2-2) / 2 * 3^2 +// +class pseudoLikelihoodOptimization +{ +public: + // number of weights that have to bee calculated + unsigned int number_of_weights; + + // vector that saves the parameters for each part of the optimization function + std::vector > log_parameters; + + // vector to save the starting_point for the weights + std::vector starting_weights; + + // the sigma used for the gaussian shrinking function + double sigma; + + // initializing constructor + pseudoLikelihoodOptimization() + { + number_of_weights = 0; + sigma = 1.; + } + + // overload of the () operator, which is needed from Dlib + double operator()(const column_vector& weights) const + { + double result = 0; + // go trough each part of the function and calculate the log(.) and add it to the result + for(unsigned int function_part = 0; function_part < log_parameters.size(); ++function_part) + { + double splitting_of_exponent = 20; // because this function often produces exponents around 1000 the calculation of one exponential part needs to be splitted + long double log_numerator = 1., log_denominator = 0.; // numerator and denominator for each log + long double exp_exponent = 0; // helping variable to get each exponent for exp(.) + // get the log_numerator for each function part + for(unsigned int numerator_factor = 0; numerator_factor < number_of_weights; ++numerator_factor) + { + exp_exponent += log_parameters[function_part][numerator_factor] * weights(numerator_factor); + } + exp_exponent = exp_exponent / splitting_of_exponent; + for(size_t split = 0; split < splitting_of_exponent; ++split) + log_numerator = log_numerator * exp(exp_exponent); + + // used for debugging, when parts of the function become too large for double + if(exp_exponent > 250.0) + { + std::cout << "exp exponent: " << exp_exponent << " numerator: " << log_numerator<< std::endl; +// for(int i = 0; i < number_of_weights; ++i) +// std::cout << weights(i) << " "; +// std::cout << std::endl; + } + + // add the numerator to the denominator, because it has to appear here + log_denominator += log_numerator; + // add each clique-value to the denominator + unsigned int vector_position = number_of_weights; // variable to get the current absolute starting position + // for the vector + do + { + exp_exponent = 0; + for(unsigned int relative_position = 0; relative_position < number_of_weights; ++relative_position) + { + exp_exponent += log_parameters[function_part][vector_position + relative_position] * weights(relative_position); + } + exp_exponent = exp_exponent / splitting_of_exponent; + // update the absolute vector position + vector_position += number_of_weights; + // update the denominator + long double denominator_part = 1.0; + for(size_t split = 0; split < splitting_of_exponent; ++split) + denominator_part *= exp(exp_exponent); + + log_denominator += denominator_part; + + }while(vector_position < log_parameters[function_part].size()); + + // update the result to return + result -= log10(log_numerator / log_denominator); + } + + // add the gaussian shrinking function + double gaussian_numerator = 0; + for(unsigned int weight = 0; weight < number_of_weights; ++weight) + { + gaussian_numerator += std::pow(weights(weight) - starting_weights[weight], 2.0); + } + result += gaussian_numerator / (2.0 * sigma * sigma); +// std::cout << "gaussian shrinking: " << gaussian_numerator << "/" << (2.0 * sigma * sigma) << std::endl; +// std::cout << "res: " << result << std::endl; + return result; + } +}; + +// Structs that are used to sort the label configurations in a way s.t. OpenGM can use it properly when defining functions +// and factors. +struct labelWithIndex +{ +public: + uint label; // current label of the configuration configuration + size_t absolute_index; // index of the node in the global CRF-node-saver +}; + +struct compLabelsByIndices +{ + bool operator()(labelWithIndex const &a, labelWithIndex const &b) + { + return (a.absolute_index < b.absolute_index); + } +}; + +// Struct used to sort contours regarding their size +struct compContoursSize +{ + bool operator()(std::vector const &a, std::vector const &b) + { + return (cv::contourArea(a) >= cv::contourArea(b)); + } +} contourComparer; + + +// Constructor +VoronoiRandomFieldSegmentation::VoronoiRandomFieldSegmentation() +{ + //save the angles between the simulated beams, used in the following algorithm + for (double angle = 0; angle < 360; angle++) + { + angles_for_simulation_.push_back(angle); + } + + // set number of classes this algorithm can detect + number_of_classes_ = 3; + + // Set up boosting parameters + number_of_classifiers_ = 35; +#if CV_MAJOR_VERSION == 2 + CvBoostParams params(CvBoost::DISCRETE, number_of_classifiers_, 0, 2, false, 0); + params_ = params; +#endif + trained_boost_ = false; + trained_conditional_field_ = false; +} + +// This Function checks if the given cv::Point is more far away from all the cv::Points in the given set. If one point gets found +// that this Point is nearer than the defined min_distance the function returns false to stop it immediately. +bool VoronoiRandomFieldSegmentation::pointMoreFarAway(const std::set& points, const cv::Point& point, const double min_distance) +{ + double square_distance = min_distance * min_distance; + for(std::set::const_iterator current_point = points.begin(); current_point != points.end(); ++current_point) + { + double dx = current_point->x - point.x; + double dy = current_point->y - point.y; + if( ((dx*dx + dy*dy)) <= square_distance) + return false; + } + return true; +} + +std::vector VoronoiRandomFieldSegmentation::raycasting(const cv::Mat& map, const cv::Point& location) +{ +// cv::Mat test_map (map.rows, map.cols, map.type(), cv::Scalar(255)); + //Raycasting Algorithm. It simulates the laser measurment at the given location and returns the lengths + //of the simulated beams + double simulated_x, simulated_y, simulated_cos, simulated_sin; + double temporary_distance; + std::vector distances(360, 0); + double delta_x, delta_y; + double pi_to_rad = PI / 180; + for (double angle = 0; angle < 360; angle++) + { + simulated_cos = std::cos(angle * pi_to_rad); + simulated_sin = std::sin(angle * pi_to_rad); + temporary_distance = 90000001; + for (double distance = 0; distance < 1000000; ++distance) + { + simulated_x = simulated_cos * distance; + simulated_y = simulated_sin * distance; + //make sure the simulated Point isn't out of the boundaries of the map + if (location.x + simulated_x > 0 && location.x + simulated_x < map.rows && location.y + simulated_y > 0 && location.y + simulated_y < map.cols) + { + if (map.at(location.x + simulated_x, location.y + simulated_y) == 0 && distance < temporary_distance) + { + temporary_distance = distance; + break; + } + } + } + if (temporary_distance > 90000000) + { + temporary_distance = 10; + } + distances[angle] = temporary_distance; + } + + return distances; +} + + +// This function computes all possible configurations for n variables that each can have m labels, e.g. when there are 2 variabels +// with 3 possible label for each there are 9 possible configurations. Important is that this function does compute multiple +// configurations like (1,2) and (2,1). +// This is done using the std::next_permutation function that computes the permutations of a given vector/array. To get all possible +// configurations first a vector is created and filled that stores n*m elements, representing the label for each variable. Then +// after the vector gets sorted all possible permutations are listed and the first n elements in this permutation are one configuration +// and going to be saved, if it hasn't occurred yet. +void VoronoiRandomFieldSegmentation::getPossibleConfigurations(std::vector >& possible_configurations, + const std::vector& possible_labels, const uint number_of_variables) +{ + // check how many possible labels there are and create a vector with number_of_variables*number_of_labels length + uint number_of_labels = possible_labels.size(); + std::vector label_vector(number_of_labels*number_of_variables); + + // fill the created vector with the labels for each variable + for(size_t variable = 0; variable < number_of_variables; ++variable) + for(size_t label = 0; label < number_of_labels; ++label) + label_vector[label + number_of_labels * variable] = possible_labels[label]; + + // sort the vector (expected from std::next_permutation) + std::sort (label_vector.begin(),label_vector.end()); + + // set to save the found configurations --> to easily check if one configuration has already been found + std::set > found_configurations; + + // get all permutations and create one configuration out of each + do + { + // configuration-vector + std::vector current_config(number_of_variables); + + // resave the first number_of_variables elements + for(size_t v = 0; v < number_of_variables; ++v) + current_config[v] = label_vector[v]; + + // check if the current_configuration has occurred yet, if not add it to the possible configurations + if(found_configurations.find(current_config) == found_configurations.end()) + { + found_configurations.insert(current_config); + possible_configurations.push_back(current_config); + } + + }while (std::next_permutation(label_vector.begin(),label_vector.end())); +} + +// +// This function is used to swap label-configurations in a order s.t. the nodes, which the labels are assigned to, are ordered +// with increasing indices. The indices show the position in the global set that stores all nodes. This is necessary, because +// OpenGM (The library used for inference in this graphical model) expects it this way. To do this the above defined structs +// labelWithIndex and compLabelsByIndices and the function std::sort are used. This function then creates a vector that stores +// the configurations as the struct labelWithIndex and applies a sort on all of it. Then it assignes the new found labels into +// the original vector. +void VoronoiRandomFieldSegmentation::swapConfigsRegardingNodeIndices(std::vector >& configurations, + size_t point_indices[]) +{ + for(size_t configuration = 0; configuration < configurations.size(); ++configuration) + { + // go trough configuration and create labels with the corresponding index + std::vector current_vector; + for(size_t current_node = 0; current_node < configurations[configuration].size(); ++current_node) + { + labelWithIndex current_label = {configurations[configuration][current_node], point_indices[current_node]}; + current_vector.push_back(current_label); + } + // sort the current vector + std::sort(current_vector.begin(), current_vector.end(), compLabelsByIndices()); + + // reassign the vector-elements + for(size_t node = 0; node < configurations[configuration].size(); ++node) + configurations[configuration][node] = current_vector[node].label; + } +} + +// +// ******* Function to create a conditional random field out of the given points ******************* +// +// This function constructs the Conditional Random Field out of the given points, by creating the edges of +// this graph from the cliques as described in segment. This is done by: +// 1. Searching the two nearest neighbors for each Point that isn't a voronoi-node and the 4 nearest neighbors for +// points that are voronoi-nodes (if possible to find 4, else 3 is enough, for example when the end of a hallway is reached). +// The nearest nodes are found by going along the pruned Voronoi graph to ensure that the found clique is the wanted clique, +// containing nodes that are connected by the voronoi-graph. +// The searching in a direction on the graph stops, if a new found node is a conditional-random-field-node, +// because in this direction the nearest neighbor has been found, and if the algorithm can't find new +// voronoi-graph-points. The second case occurs, when the current node is a dead end and has only one neighbor. +// 2. The found neighbors are defined as a new clique with the current looked at point. +// 3. For each found clique simulate the laserbeams at this point by using the defined raycasting-function. This step is done +// because the beams only need to be computed once for each clique and doing this at this point saves much time later. +void VoronoiRandomFieldSegmentation::createConditionalField(const cv::Mat& voronoi_map, const std::set& node_points, + std::vector& conditional_random_field_cliques, const std::set& voronoi_node_points, + const cv::Mat& original_map) +{ + // 1. Search for the n neighbors of the each point by going along the voronoi graph until a conditional-field-node gets + // found. + std::map, cv_Point_comp > raycasts; // map that stores the simulated rays for given OpenCV Points --> some points would get raycasted several times, this saves computation-time + + for(std::set::const_iterator current_point = node_points.begin(); current_point != node_points.end(); ++current_point) + { + // check how many neighbors need to be found --> 4 if the current node is a voronoi graph node, 2 else + // ( 4 because e.g. a node in the middle of a cross has four neighbors) + int number_of_neighbors = 2; + if(voronoi_node_points.find(*current_point) != voronoi_node_points.end()) + number_of_neighbors = 4; + + // vector to save the searched points + std::vector searched_points; + searched_points.push_back(*current_point); + + // vector to save the found neighbors + std::set found_neighbors; + + // integer to check if new voronoi nodes could be found + unsigned int previous_size_of_searched_nodes; + + // go along the Voronoi Graph starting from the current node and find the nearest conditional random field nodes + do + { + // save the size of the searched-points vector + previous_size_of_searched_nodes = searched_points.size(); + + // create temporary-vector to save the new found nodes + std::vector temporary_point_vector = searched_points; + + // check each visited point for neighbors that are voronoi nodes + // remark: I check every point again, because it produces better results than when only new points get expanded. + for(std::vector::iterator searching_point = searched_points.begin(); searching_point != searched_points.end(); ++searching_point) + { + bool random_field_node = false; // if the current node is a node of the conditional random field, don't go further in this direction + if(found_neighbors.find(*searching_point) != found_neighbors.end()) + random_field_node = true; + + if(random_field_node == false) + { + // check around a 3x3 region for nodes of the voronoi graph + for(int du = -1; du <= 1; du++) + { + for(int dv = -1; dv <= 1; dv++) // && abs(du) + abs(dv) != 0 + { + // don't check point itself + if(du == 0 && dv == 0) + continue; + + // get point that needs to be expanded + cv::Point point_to_check = cv::Point(searching_point->x + dv, searching_point->y + du); + + // voronoi node is drawn with a value of 127 in the map, don't check already checked points + if(voronoi_map.at(point_to_check) == 127 + && contains(temporary_point_vector, point_to_check) == false) + { + // add found voronoi node to searched-points vector + temporary_point_vector.push_back(point_to_check); + + // Check if point is a conditional random field node. Check on size is to prevent addition of + // points that appear in the same step and would make the clique too large. + if(node_points.find(point_to_check) != node_points.end() + && found_neighbors.size() < number_of_neighbors) + found_neighbors.insert(point_to_check); + } + } + } + } + } + + // assign the temporary-vector as the new reached-points vector + searched_points = temporary_point_vector; + + }while(found_neighbors.size() < number_of_neighbors && previous_size_of_searched_nodes != searched_points.size()); + + // 2. create a clique out of the current node and its found neighbors + conditional_random_field_cliques.push_back(Clique(*current_point)); + std::vector neighbor_vector(found_neighbors.begin(), found_neighbors.end()); // convert set to a vector to easily insert the new members + conditional_random_field_cliques.back().insertMember(neighbor_vector); + + // 3. Simulate the laser-beams at each found member and store them. This step saves a lot of computation time later. + std::vector clique_members = conditional_random_field_cliques.back().getMemberPoints(); + std::vector< std::vector > laser_beams(clique_members.size()); + + for(size_t member = 0; member < clique_members.size(); ++member) + { + laser_beams[member] = raycasting(original_map, cv::Point(clique_members[member].y, clique_members[member].x)); + } + + conditional_random_field_cliques.back().setBeamsForMembers(laser_beams); + } +} + +//**************************Training-Algorithm for the AdaBoost-classifiers***************************** +// This Function trains the AdaBoost-classifiers from OpenCV. It takes the given training maps and finds the Points +// that are labeled as the specified classes and calculates the features defined in +// +// ipa_room_segmentation/voronoi_random_field_features.h +// +// These vectors are put in a format that OpenCV expects for the classifiers and then they are trained. +void VoronoiRandomFieldSegmentation::trainBoostClassifiers(const std::vector& training_maps, + std::vector< std::vector >& cliques_of_training_maps, std::vector possible_labels, + const std::string& classifier_storage_path) +{ + boost::filesystem::path storage_path(classifier_storage_path); + if (boost::filesystem::exists(storage_path) == false) + { + if (boost::filesystem::create_directories(storage_path) == false && boost::filesystem::exists(storage_path) == false) + { + std::cout << "Error: VoronoiRandomFieldSegmentation::trainBoostClassifiers: Could not create directory " << storage_path << std::endl; + return; + } + } + + std::cout << "starting to train the Boost Classifiers." << std::endl; + + // vectors that store the given labels and features for each point (order: room-hallway-doorway) + std::vector< std::vector > labels_for_classes(number_of_classes_); + std::vector< std::vector > features_for_points; + + // go trough each found clique and take the first point of the clique as current point + // --> each possible point is only once the first (central) point of a clique + voronoiRandomFieldFeatures vrf_feature_computer; + for(size_t map = 0; map < training_maps.size(); ++map) + { + cv::Mat current_map = training_maps[map]; + for(std::vector::iterator current_clique = cliques_of_training_maps[map].begin(); current_clique != cliques_of_training_maps[map].end(); ++current_clique) + { + // get all members of the current clique (used later) + std::vector current_clique_members = current_clique->getMemberPoints(); + + // get the central point of the clique + cv::Point current_point = current_clique_members[0]; + + // get the stored labels for these points + std::vector current_labels_for_points(current_clique_members.size()); + + for(size_t point = 0; point < current_clique_members.size(); ++point) + { + current_labels_for_points[point] = current_map.at(current_clique_members[point]); + } + + // get the stored laser-beams for the central point + std::vector current_beams = current_clique->getBeams()[0]; + + // get the feature for the current point and store it in the global vector + std::vector current_features; + + vrf_feature_computer.getFeatures(current_beams, angles_for_simulation_, current_clique_members, current_labels_for_points, possible_labels, current_point, current_features); + features_for_points.push_back(current_features); + + // get the labels-vector for each class + // --> OpenCV expects the labels: +1 if it belongs to the class, -1 if it doesn't + for(size_t current_class = 0; current_class < number_of_classes_; ++current_class) + { + if(current_labels_for_points[0] == possible_labels[current_class]) + labels_for_classes[current_class].push_back(1); + else + labels_for_classes[current_class].push_back(-1); + } + } + } + + std::cout << "found all features and labels." << std::endl; + + // Train each AdaBoost-classifier. + // + //*************room*************** + //save the found labels and features in Matrices + cv::Mat room_labels_Mat(labels_for_classes[0]); + cv::Mat features_Mat(features_for_points.size(), vrf_feature_computer.getFeatureCount(), CV_32FC1); + for (int i = 0; i < labels_for_classes[0].size(); i++) + { + for (int f = 0; f < vrf_feature_computer.getFeatureCount(); f++) + { + features_Mat.at(i, f) = (float) features_for_points[i][f]; + } + } + std::string filename_room = classifier_storage_path + "vrf_room_boost.xml"; +#if CV_MAJOR_VERSION == 2 + // Train a boost classifier + room_boost_.train(features_Mat, CV_ROW_SAMPLE, room_labels_Mat, cv::Mat(), cv::Mat(), cv::Mat(), cv::Mat(), params_); + //save the trained booster + room_boost_.save(filename_room.c_str(), "boost"); +#else + // Train a boost classifier + room_boost_ = cv::ml::Boost::create(); + room_boost_->setBoostType(cv::ml::Boost::REAL); + room_boost_->setWeakCount(number_of_classifiers_); + room_boost_->setWeightTrimRate(0); + room_boost_->setMaxDepth(2); + room_boost_->setUseSurrogates(false); + room_boost_->setPriors(cv::Mat()); + room_boost_->train(features_Mat, cv::ml::ROW_SAMPLE, room_labels_Mat); + //save the trained booster + room_boost_->save(filename_room.c_str()); +#endif + std::cout << "Trained room classifier" << std::endl; + + // + //*************hallway*************** + //save the found labels and features in Matrices + cv::Mat hallway_labels_Mat(labels_for_classes[1]); + std::string filename_hallway = classifier_storage_path + "vrf_hallway_boost.xml"; +#if CV_MAJOR_VERSION == 2 + // Train a boost classifier + hallway_boost_.train(features_Mat, CV_ROW_SAMPLE, hallway_labels_Mat, cv::Mat(), cv::Mat(), cv::Mat(), cv::Mat(), params_); + //save the trained booster + hallway_boost_.save(filename_hallway.c_str(), "boost"); +#else + // Train a boost classifier + hallway_boost_ = cv::ml::Boost::create(); + hallway_boost_->setBoostType(cv::ml::Boost::REAL); + hallway_boost_->setWeakCount(number_of_classifiers_); + hallway_boost_->setWeightTrimRate(0); + hallway_boost_->setMaxDepth(2); + hallway_boost_->setUseSurrogates(false); + hallway_boost_->setPriors(cv::Mat()); + hallway_boost_->train(features_Mat, cv::ml::ROW_SAMPLE, hallway_labels_Mat); + //save the trained booster + hallway_boost_->save(filename_hallway.c_str()); +#endif + std::cout << "Trained hallway classifier" << std::endl; + + // + //*************doorway*************** + //save the found labels and features in Matrices + cv::Mat doorway_labels_Mat(labels_for_classes[2]); + std::string filename_doorway = classifier_storage_path + "vrf_doorway_boost.xml"; +#if CV_MAJOR_VERSION == 2 + // Train a boost classifier + doorway_boost_.train(features_Mat, CV_ROW_SAMPLE, doorway_labels_Mat, cv::Mat(), cv::Mat(), cv::Mat(), cv::Mat(), params_); + //save the trained booster + doorway_boost_.save(filename_doorway.c_str(), "boost"); +#else + // Train a boost classifier + doorway_boost_ = cv::ml::Boost::create(); + doorway_boost_->setBoostType(cv::ml::Boost::REAL); + doorway_boost_->setWeakCount(number_of_classifiers_); + doorway_boost_->setWeightTrimRate(0); + doorway_boost_->setMaxDepth(2); + doorway_boost_->setUseSurrogates(false); + doorway_boost_->setPriors(cv::Mat()); + doorway_boost_->train(features_Mat, cv::ml::ROW_SAMPLE, doorway_labels_Mat); + //save the trained booster + doorway_boost_->save(filename_doorway.c_str()); +#endif + std::cout << "Trained doorway classifier" << std::endl; + + // set the trained Boolean for the AdaBoost-classifiers to true + trained_boost_ = true; + std::cout << "Finished training the Boost algorithm." << std::endl; +} + +// +// ********************* Function to calculate the feature-vector for a given clique. *********************** +// +// This function calculates the feature vector for a given clique, using the trained AdaBoost classifiers. These calculate +// different boost-features specific for the given point and neighbors of it and then multiplies these with a weight-vector +// producing weak hypothesis to specify the label of the current point. These weak hypothesis are used as features for the +// conditional random field. The possible_labels vector stores the possible labels a point in the training map can have in +// the order: +// room, hallway, doorway +void VoronoiRandomFieldSegmentation::getAdaBoostFeatureVector(std::vector& feature_vector, Clique& clique, + std::vector& given_labels, std::vector& possible_labels) +{ + // Get the points that belong to the clique and the stored simulated beams for each one. + std::vector clique_members = clique.getMemberPoints(); + std::vector< std::vector > beams_for_points = clique.getBeams(); + + // vector that is used to sum up the calculated features + std::vector temporary_feature_vector(feature_vector.size(), 0.0); + + // For each member of this clique calculate the weak-hypothesis and add the resulting vectors in the end + for(size_t point = 0; point < clique_members.size(); ++point) + { + // Check which classifier (room, hallway or doorway) needs to be used. + unsigned int classifier; + for(size_t label = 0; label < possible_labels.size(); ++label) + { + if(possible_labels[label] == given_labels[point]) + { + classifier = label; + break; + } + } + + // get the features for the central point of the clique + voronoiRandomFieldFeatures vrf_feature_computer; + cv::Mat featuresMat(1, vrf_feature_computer.getFeatureCount(), CV_32FC1); //OpenCV expects a 32-floating-point Matrix as feature input + std::vector current_features; + vrf_feature_computer.getFeatures(beams_for_points[point], angles_for_simulation_, clique_members, given_labels, possible_labels, clique_members[point], current_features); + + for (int f = 1; f <= vrf_feature_computer.getFeatureCount(); ++f) + { + featuresMat.at(0, f - 1) = (float) current_features[f-1]; + } + + // Calculate the weak hypothesis by using the wanted classifier. +#if CV_MAJOR_VERSION<=3 + CvMat features = featuresMat; + cv::Mat weaker (1, number_of_classifiers_, CV_32F); + CvMat weak_hypothesis = weaker; // Wanted from OpenCV to get the weak hypothesis from the + // separate weak classifiers. +#else + cv::Mat features = featuresMat; + cv::Mat weaker (1, number_of_classifiers_, CV_32F); +#endif + + // For each point the classifier depends on the given label. If the point is labeled as a room the room-boost should be + // used and so on. + switch(classifier) + { +#if CV_MAJOR_VERSION == 2 + case 0: + room_boost_.predict(&features, 0, &weak_hypothesis); + break; + case 1: + hallway_boost_.predict(&features, 0, &weak_hypothesis); + break; + case 2: + doorway_boost_.predict(&features, 0, &weak_hypothesis); + break; +#else + case 0: + room_boost_->predict(featuresMat, weaker); + break; + case 1: + hallway_boost_->predict(featuresMat, weaker); + break; + case 2: + doorway_boost_->predict(featuresMat, weaker); + break; +#endif + } + + + // Write the weak hypothesis in the feature vector. + for(size_t f = 0; f < number_of_classifiers_; ++f) + { +#if CV_MAJOR_VERSION<=3 + temporary_feature_vector[f] = temporary_feature_vector[f] + (double) CV_MAT_ELEM(weak_hypothesis, float, 0, f); +#else + temporary_feature_vector[f] = temporary_feature_vector[f] + (double) weaker.at(0, f); +#endif + } + } + + // copy the summed vector to the given feature-vector + feature_vector = temporary_feature_vector; +} + +// +//********************* Function to find the conditional field weights. **************** +// +// This function is used to find the weights, that are used to compute the clique potentials. The AdaBoost classifier trained +// before gives the vector f(y_k, x) that stores the values for each feature, calculated for a given clique in the +// random field. to get the clique potential this vector gets multiplied by w^T, which is the transposed weight-vector, calculated +// here. These weights define the importance of one single feature in classifying the clique. To calculate the weights the +// following steps are done: +// I. For each given field-node calculate the features of the clique, using the AdaBoost classifiers. The features for the +// conditonal-random-field are the weak hypothesis, produced by the AdaBoost classifers. This hypothesis is the +// predicted label for a given point, multiplied by the weight for the selected classifier. For more details see +// the trainBoostClassifiers() or any document on Boosting. +// II. The above defined features are used to maximize the pseudo-likelihood over all training-data by minimizing a +// feature-function, by using the Dlib-c++-library. The found weights are then saved at the location, which is given +// to this function. +// +void VoronoiRandomFieldSegmentation::findConditionalWeights(std::vector< std::vector >& conditional_random_field_cliques, + std::vector >& random_field_node_points, const std::vector& training_maps, + std::vector& possible_labels, const std::string weights_filepath) +{ + // check if the AdaBoost-classifiers has already been trained yet, if not the conditional field can't be trained + if(trained_boost_ == false) + ROS_ERROR("AdaBoost-classifiers haven't been trained yet. First train the AdaBoost algorithm before training the conditional-random-field"); + + std::cout << "Starting to train the conditional-random-field." << std::endl; + + // ************ I. Go trough each found point and compute the pseudo-likelihood of it to get one big likelihood. **************** + + std::vector > all_point_feature_vectors; // Vector that stores every feature vector calculated for the + // found nodes of the CRF. One vector of it stores all features + // for different labels, in a specified order. The first + // values are for the real training label and the rest of it + // for labels different than the given one. + + for(size_t current_map_index = 0; current_map_index < training_maps.size(); ++current_map_index) + { + for(std::set::iterator current_point = random_field_node_points[current_map_index].begin(); current_point != random_field_node_points[current_map_index].end(); ++current_point) + { + // vector to save the cliques that were found for one point + std::vector cliques_for_point; + + // vector that stores the labels of each clique-member for the current point + std::vector< std::vector > labels_of_cliques; + + // set the given training label for this point + unsigned int real_label = training_maps[current_map_index].at(*current_point); + + // for each point find the cliques that this point belongs to + for(std::vector::iterator current_clique = conditional_random_field_cliques[current_map_index].begin(); current_clique != conditional_random_field_cliques[current_map_index].end(); ++current_clique) + { + if(current_clique->containsMember(*current_point)) + { + cliques_for_point.push_back(*current_clique); + + // search for the labels of the clique-members + std::vector temporary_clique_labels(current_clique->getNumberOfMembers()); + std::vector members = current_clique->getMemberPoints(); + + for(size_t member = 0; member < current_clique->getNumberOfMembers(); ++member) + temporary_clique_labels[member] = training_maps[current_map_index].at(members[member]); + + // save the found labels + labels_of_cliques.push_back(temporary_clique_labels); + } + } + + // For each found clique compute the feature vector for different labels. The first label is the label that was + // given to the algorithm by the training data and the other are the remaining labels, different from the first. + std::vector > feature_vectors(number_of_classes_); // vector to store the found feature-vectors for each class + + // vector to store the feature-vectors computed for the different cliques + std::vector > temporary_feature_vectors(cliques_for_point.size(), std::vector(number_of_classifiers_, 0.0)); + + // get the clique-feature-vectors for the given training label and add them to the first feature-vector for this label + for(size_t clique = 0; clique < cliques_for_point.size(); ++clique) + { + getAdaBoostFeatureVector(temporary_feature_vectors[clique], cliques_for_point[clique], labels_of_cliques[clique], possible_labels); + feature_vectors[0] = feature_vectors[0] + temporary_feature_vectors[clique]; + } + + // assign the first feature-vector to the complete feature-vector + all_point_feature_vectors.push_back(feature_vectors[0]); + + // get the other feature-vectors for the different labels + unsigned int label_index = 1; // variable to access the right places in feature_vectors for each possible label + for(size_t label_position = 0; label_position < possible_labels.size(); ++label_position) + { + // only compute different labels + if(possible_labels[label_position] != real_label) + { + for(size_t clique = 0; clique < cliques_for_point.size(); ++clique) + { + // copy the real-label-vector and change the label for the current point --> see how clique potential changes + std::vector temporary_labels = labels_of_cliques[clique]; + std::vector current_clique_members = cliques_for_point[clique].getMemberPoints(); + + int point_position = std::find(current_clique_members.begin(), current_clique_members.end(), *current_point) - current_clique_members.begin(); + + temporary_labels[point_position] = possible_labels[label_position]; + + // get the AdaBoost-feature-vector + getAdaBoostFeatureVector(temporary_feature_vectors[clique], cliques_for_point[clique], temporary_labels, possible_labels); + feature_vectors[label_index] = feature_vectors[label_index] + temporary_feature_vectors[clique]; + } + // append the last vector in all_point_feature_vector by the calculated vector + all_point_feature_vectors.back() += feature_vectors[label_index]; + // set index for labels one step higher + ++label_index; + } + } + } + } + + // + // ********* II. Find the weights that minimize the total likelihood by using the Dlib-library. *********** + // + // define the mean-weights for the gaussian shrinking function + std::vector mean_weights(number_of_classifiers_, 0); + + voronoiRandomFieldFeatures vrf_features; + cv::Mat featuresMat(1, vrf_features.getFeatureCount(), CV_32FC1); //OpenCV expects a 32-floating-point Matrix as feature input + for (int f = 1; f <= vrf_features.getFeatureCount(); ++f) + featuresMat.at(0, f - 1) = (float) 1; + + // Calculate the weak hypothesis by using the wanted classifier. The weak hypothesis is given by h_i(x) = w_i * f_i(x) +#if CV_MAJOR_VERSION<=3 + CvMat features = featuresMat; + cv::Mat weaker (1, number_of_classifiers_, CV_32F); + CvMat weak_hypothesis = weaker; // Wanted from OpenCV to get the weak hypothesis from the + // separate weak classifiers. +#else + cv::Mat features = featuresMat; + cv::Mat weaker(1, number_of_classifiers_, CV_32F); +#endif + + // Get weights for room, hallway and doorway classifier. +#if CV_MAJOR_VERSION == 2 + room_boost_.predict(&features, 0, &weak_hypothesis); +#else + room_boost_->predict(featuresMat, weaker); +#endif + + for(size_t f = 0; f < number_of_classifiers_; ++f) +#if CV_MAJOR_VERSION<=3 + mean_weights[f] += (double) CV_MAT_ELEM(weak_hypothesis, float, 0, f); +#else + mean_weights[f] += (double) weaker.at(0, f); +#endif + +#if CV_MAJOR_VERSION == 2 + hallway_boost_.predict(&features, 0, &weak_hypothesis); +#else + hallway_boost_->predict(featuresMat, weaker); +#endif + + for(size_t f = 0; f < number_of_classifiers_; ++f) +#if CV_MAJOR_VERSION<=3 + mean_weights[f] *= (double) CV_MAT_ELEM(weak_hypothesis, float, 0, f); +#else + mean_weights[f] *= (double) weaker.at(0, f); +#endif + +#if CV_MAJOR_VERSION == 2 + doorway_boost_.predict(&features, 0, &weak_hypothesis); +#else + doorway_boost_->predict(featuresMat, weaker); +#endif + + for(size_t f = 0; f < number_of_classifiers_; ++f) +#if CV_MAJOR_VERSION<=3 + mean_weights[f] *= (double) CV_MAT_ELEM(weak_hypothesis, float, 0, f); +#else + mean_weights[f] *= (double) weaker.at(0, f); +#endif + + // find the best weights --> minimize the defined function for the pseudo-likelihood + std::cout << "finding weights using Dlib" << std::endl; + column_vector weight_results; + weight_results = findMinValue(number_of_classifiers_, 9.0, all_point_feature_vectors, mean_weights); + + // clear the already found weights, if trained more than one time + trained_conditional_weights_.clear(); + + // save the found weights to a std::vector + for(size_t weight = 0; weight < number_of_classifiers_; ++weight) + trained_conditional_weights_.push_back(weight_results(0, weight)); + + // save the weights to a file + boost::filesystem::path storage_path(weights_filepath); + if (boost::filesystem::exists(storage_path) == false) + { + if (boost::filesystem::create_directories(storage_path) == false && boost::filesystem::exists(storage_path) == false) + { + std::cout << "Error: VoronoiRandomFieldSegmentation::findConditionalWeights: Could not create directory " << storage_path << std::endl; + return; + } + } + std::string filename = weights_filepath + "vrf_conditional_field_weights.txt"; + std::ofstream output_file(filename.c_str(), std::ios::out); + if (output_file.is_open()==true) + output_file << weight_results; + output_file.close(); + + //set the trained-variable true, so the labeling-algorithm knows the classifiers have been trained already + trained_conditional_field_ = true; + std::cout << "Finished training the Conditional Field." << std::endl; + +} + +// +//********************* Function to train the whole algorithm. **************** +// +// I. Go trough each given training map and find the drawn points, that represent the nodes for the conditional random +// field. Also it finds the voronoi-nodes that are drawn in a different color than the other nodes. These points are +// used in the second step of this function to create the cliques in the conditional random field. +// II. For each given training map the found nodes are used to create conditional random fields. The whole algorithm is +// based on this and to train the algorithm a crf for each training map is needed. This step produces cliques, that +// all have a clique-potential, depending on features and the crf-weights, that will get maximized later. +// III. In the next step the AdaBoost-classifiers are trained. This is done by giving the above defined function +// trainBoostClassifiers() the given training maps. This function searches for points labeled as room/hallway/doorway +// and adds them as demonstration-data for AdaBoost-classifiers. +// IV. In the last step the weights for the conditional-random-field are found. As said above the clique-potentials are +// depending on these weights and so they are chosen to maximize the potentials over all training maps. Because this +// would be really hard to do directly, a log-likelihood estimation is applied. +void VoronoiRandomFieldSegmentation::trainAlgorithms(const std::vector& original_maps, const std::vector& training_maps, + std::vector& voronoi_maps, const std::vector& voronoi_node_maps, + std::vector& possible_labels, const std::string storage_path, + const int epsilon_for_neighborhood, const int max_iterations, const int min_neighborhood_size, + const double min_node_distance) +{ + // ********** I. Go trough each map and find the drawn node-points for it and check if it is a voronoi-node. ***************** + std::vector > random_field_node_points, voronoi_node_points; + + std::cout << "Starting to find the conditional-random-field-cliques." << std::endl; + + bool compute_voronoi_maps = false; + if (voronoi_maps.size() != original_maps.size()) + { + compute_voronoi_maps = true; + voronoi_maps.resize(original_maps.size()); + std::cout << "Creating the voronoi graphs before training." << std::endl; + } + for(size_t current_map_index = 0; current_map_index < training_maps.size(); ++current_map_index) + { + // Find conditional field nodes by checking each pixel for its color. + const cv::Mat& current_map = training_maps[current_map_index]; + std::set current_nodes, current_voronoi_nodes; + if (compute_voronoi_maps == false && voronoi_node_maps.size() == original_maps.size()) + { + const cv::Mat& current_voronoi_node_map = voronoi_node_maps[current_map_index]; + for(size_t v = 0; v < current_map.rows; ++v) + { + for(size_t u = 0; u < current_map.cols; ++u) + { + // check if the current point is a voronoi-node by checking the color in the voronoi-node map + if(current_voronoi_node_map.at(v, u) != 255 && current_voronoi_node_map.at(v, u) != 0) + current_voronoi_nodes.insert(cv::Point(u, v)); + } + } + } + else + { + voronoi_maps[current_map_index] = original_maps[current_map_index].clone(); + createPrunedVoronoiGraph(voronoi_maps[current_map_index], current_voronoi_nodes); + } + + // read in a fully labeled map (not only points) and generate current_nodes accordingly + // find the conditional random field nodes for the current map + cv::Mat distance_map; //distance-map of the original-map (used to check the distance of each point to nearest black pixel) +#if CV_MAJOR_VERSION<=3 + cv::distanceTransform(original_maps[current_map_index], distance_map, CV_DIST_L2, 5); +#else + cv::distanceTransform(original_maps[current_map_index], distance_map, cv::DIST_L2, 5); +#endif + cv::convertScaleAbs(distance_map, distance_map); + + // find all nodes for the conditional random field + findConditonalNodes(current_nodes, voronoi_maps[current_map_index], distance_map, current_voronoi_nodes, epsilon_for_neighborhood, max_iterations, min_neighborhood_size, min_node_distance); + + // save the found nodes + random_field_node_points.push_back(current_nodes); + voronoi_node_points.push_back(current_voronoi_nodes); + } + + // ********** II. Create the conditional random fields. ***************** + std::cout << "Creating the conditional-random-field-cliques." << std::endl; + + std::vector > conditional_random_field_cliques; + for(size_t current_map = 0; current_map < training_maps.size(); ++current_map) + { + // create conditional random field + std::vector current_cliques; + createConditionalField(voronoi_maps[current_map], random_field_node_points[current_map], current_cliques, voronoi_node_points[current_map], original_maps[current_map]); + + // save the found cliques + conditional_random_field_cliques.push_back(current_cliques); + } + + // ********** III. Train the AdaBoost-classifiers. ***************** + trainBoostClassifiers(training_maps, conditional_random_field_cliques, possible_labels, storage_path); + + // ********** IV. Find the conditional-random-field weights. ***************** + findConditionalWeights(conditional_random_field_cliques, random_field_node_points, training_maps, possible_labels, storage_path); +} + +// +//****************Create the pruned generalized Voronoi-Graph********************** +// +//This function is here to create the pruned generalized voronoi-graph in the given map. It does following steps: +// 1. Creates a Voronoi Graph +// 2. It reduces the graph until the nodes in the graph. A node is a point on the voronoi graph, that has at least 3 +// neighbors. This deletes errors from the approximate generation of the graph that hasn't been eliminated from +// the drawVoronoi function. the resulting graph is the pruned generalized voronoi graph. +// 3. It returns the map that has the pruned generalized voronoi-graph drawn in. +void VoronoiRandomFieldSegmentation::createPrunedVoronoiGraph(cv::Mat& map_for_voronoi_generation, std::set& node_points) +{ + //********************1. Create the Voronoi graph****************************** + createVoronoiGraph(map_for_voronoi_generation); + + //********************2. Reduce the graph until its nodes****************************** + pruneVoronoiGraph(map_for_voronoi_generation, node_points); +} + +// This function finds nodes for the conditional random field. +// It looks at a given pruned voronoi graph and concentrates a defined region on this graph into one point, that is +// used as a node in a graph. In this graph nodes are connected, that +// i) are right beside each other +// ii) and if a Point in the graph has three or more neighbors all of these four nodes are connected to each other +// so that different cliques occur. This is necessary for using a Conditional Random Filed to label the nodes as a +// defined class. To do so the following steps are done: +// 1. Add the previously found node points on the voronoi graph to the Conditional Random Field graph. The algorithm +// checks if they are too close to each other and only adds one of these close points to the crf. +// 2. Look at an epsilon neighborhood on the graph and choose the point farthest away from the black pixels as +// point for the crf. The farthest point is chosen, because else it would be possible that the chosen point +// is too near at the black pixels. Also the new node has to be more far away than a defined min. distance to +// each already found node s.t. two nodes are not too close to each other, or else the crf would be created wrong. +void VoronoiRandomFieldSegmentation::findConditonalNodes(std::set& conditional_nodes, + const cv::Mat& voronoi_map, const cv::Mat& distance_map, const std::set& voronoi_nodes, + const int epsilon_for_neighborhood, const int max_iterations, const int min_neighborhood_size, + const double min_node_distance) +{ + // add the given voronoi nodes as conditional nodes, if they are far away enough from each other + for(std::set::iterator node = voronoi_nodes.begin(); node != voronoi_nodes.end(); ++node) + { + if(pointMoreFarAway(conditional_nodes, *node, min_node_distance) == true) + conditional_nodes.insert(*node); + } + + // create a copy of the given voronoi map to keep track of which points already have been looked at + cv::Mat voronoi_map_for_node_extraction = voronoi_map.clone(); + + // go trough the copied voronoi map and concentrate neighborhoods into one central point + for (int v = 0; v < voronoi_map_for_node_extraction.rows; v++) + { + for (int u = 0; u < voronoi_map_for_node_extraction.cols; u++) + { + if (voronoi_map_for_node_extraction.at(v, u) == 127) + { + int loopcounter = 0; // if a part of the graph is not connected to the rest this variable helps to stop the loop + std::set neighbor_points; // neighboring-variable, which is different for each point + std::set temporary_points, searching_points; // Sets that are used to save new found points and expand them in the next iteration. + // temporary points to expand the neighborhood in iterative steps and not in one + + int neighbor_count = 0; // variable to save the number of neighbors for each point + + neighbor_points.insert(cv::Point(u, v)); //add the current Point to the neighborhood and to the expanding-queue + searching_points.insert(cv::Point(u, v)); + + do + { + loopcounter++; + // check every point in the neighborhood for other neighbors connected to it + for (std::set::iterator current_neighbor_point = searching_points.begin(); current_neighbor_point != searching_points.end(); ++current_neighbor_point) + { + for (int row_counter = -1; row_counter <= 1; row_counter++) + { + for (int column_counter = -1; column_counter <= 1; column_counter++) + { + // don't check the point itself + if(row_counter == 0 && column_counter == 0) + continue; + + // check the neighboring points + // (if it already is in the neighborhood it doesn't need to be checked again) + const int nu = current_neighbor_point->x + column_counter; + const int nv = current_neighbor_point->y + row_counter; + + if (neighbor_points.find(cv::Point(nu, nv)) == neighbor_points.end() && + nv >= 0 && nu >= 0 && + nv < voronoi_map_for_node_extraction.rows && nu < voronoi_map_for_node_extraction.cols && + voronoi_map_for_node_extraction.at(nv, nu) == 127) + { + neighbor_count++; + temporary_points.insert(cv::Point(nu, nv)); + } + } + } + } + + // go trough every found point after all neighborhood points have been checked and add them to it + for (std::set::iterator temporary_point = temporary_points.begin(); temporary_point != temporary_points.end(); ++temporary_point) + { + neighbor_points.insert(*temporary_point); + + // make the found points white in the voronoi-map (already looked at) + voronoi_map_for_node_extraction.at(*temporary_point) = 255; + } + + // make the current point white --> doesn't need to be looked at anymore + voronoi_map_for_node_extraction.at(v, u) = 255; + + // reassign the new found nodes as new to-be-searched nodes + searching_points = temporary_points; + + // check if enough neighbors have been checked or checked enough times (e.g. at a small segment of the graph) + }while (neighbor_count <= epsilon_for_neighborhood && loopcounter < max_iterations); + + // only check the neighborhood, if it is large enough --> to prevent nodes that are close to each other + if(neighbor_count >= min_neighborhood_size) + { + // check every found point in the neighborhood if it is the local minimum in the distanceMap and if + // this point is far enough away from other node points to prevent too close nodes + cv::Point current_conditional_field_point = cv::Point(u, v); + for (std::set::iterator point = neighbor_points.begin(); point != neighbor_points.end(); ++point) + { + if (distance_map.at(*point) < distance_map.at(current_conditional_field_point) + && pointMoreFarAway(conditional_nodes, *point, min_node_distance) == true) + { + current_conditional_field_point = *point; + } + } + // add the local minimum point to the critical points and check a last time if the node is far enough away + // from other nodes (because if no new node is found the initialized gets added every time, neglecting + // this constraint) + if(pointMoreFarAway(conditional_nodes, current_conditional_field_point, min_node_distance) == true) + conditional_nodes.insert(current_conditional_field_point); + } + } + } + } +} + +// This function is called to find minimal values of a defined log-likelihood-function using the library Dlib. +// This log-likelihood-function is made over all training data to get a likelihood-estimation linear in the weights. +// By minimizing this function the best weights are chosen, what is done here. See beginning of this file for detailed information. +// !!!!Important: Numerical problems might occur --> non finite outputs. This is because the derivative gets approximated. +// Change the starting point and the last entry of find_min_using_approximate_derivatives when this occurs. +column_vector VoronoiRandomFieldSegmentation::findMinValue(unsigned int number_of_weights, double sigma, + const std::vector >& likelihood_parameters, const std::vector& starting_weights) +{ + std::cout << "finding min values" << std::endl; + // create a column vector as starting search point, that is needed from Dlib to find the min. value of a function + column_vector starting_point(number_of_weights); + + // initialize the starting point as zero to favor small weights + starting_point = 1e-1; + + // create a Likelihood-optimizer object to find the weights that maximize the pseudo-likelihood + pseudoLikelihoodOptimization minimizer; + + // set the values for this optimization-object + minimizer.sigma = sigma; + minimizer.number_of_weights = number_of_weights; + + minimizer.log_parameters = likelihood_parameters; + minimizer.starting_weights = starting_weights; + + // find the best weights for the given parameters + dlib::find_min_using_approximate_derivatives(dlib::bfgs_search_strategy(), dlib::objective_delta_stop_strategy(1e-7), minimizer, starting_point, -1, 1e-10); + + return starting_point; +} + + + +// +//****************** Segmentation Function ********************* +// +// This function segments the given original_map into different regions by using the voronoi random field method from +// Stephen Friedman and Dieter Fox ( http://www.cs.washington.edu/robotics/projects/semantic-mapping/abstracts/vrf-place-labeling-ijcai-07.abstract.html ). +// This algorithm has two parts, the training step and the actual segmentation. The training should be finished, see above for details. +// In the segmentation step following actions are made: +// I.) From the given map that should be labeled (original_map) a pruned generalized Voronoi diagram is extracted ( https://www.sthu.org/research/voronoidiagrams/ ). +// This is done using the method from Karimipour and Ghandehari ( A Stable Voronoi-based Algorithm for Medial Axis Extraction through Labeling Sample Points ) +// that samples the building contour to get centerpoints to compute the voronoi graph. This approximated graph has +// some errors in it, so two elimination steps are done: +// i) eliminate lines of the graph that start or end in black regions +// ii) reduce the graph from after the first step until the nodes of the graph +// See the createPrunedVoronoiGraph() function above for better information. OpenCV is used to do this. +// II.) It finds Nodes for the Conditional Random Field, using the above defined function findConditionalNodes(). +// III.) It constructs the Conditional Random Field graph from the previously found points, by using the above defined function. +// IV.) It creates a factor graph out of the defined crf. This is necessary, because OpenGM, the library used for inference, +// needs it this way. To do so the algorithm goes trough each found clique and computes the clique-potential of it +// for all possible label-configuration. The results of this are saved in an opengm::function so OpenGM can use it. +// V.) After the factor graph has been build, an inference algorithm is applied to find the labels that maximize the complete +// value of the graph, meaning the potential of the crf. To do this not the Product of the factors are maximized, but the +// sum of the exponents. The potentials are given by exp(w^T * f) and the graph-potential is the product of these, so +// it is possible to just maximize the sum of all exponents. This is convenient, because the factors will scale very high +// and would go out of the double range. +// VI.) At the last step the algorithm takes the above found best labels and draws it into a copy of the original map. The +// rooms and hallways are drawn with the color of this class and doorways are drawn black. This is done because it +// produces intersections between different segments, most likely between rooms and hallways. These intersections are +// wanted because they create separate segments for each room/hallway and don't put several together as one big. The +// drawing into the map copy is done by finding base points for each crf-node (two black pixels that are closest to this +// node) and drawing lines in the wanted color to both. Then a wavefront-region-growing is applied on the map-copy to +// fill the segments with one color, generating several rooms and hallways. In the last step the contours of the rooms +// and hallways are searched and drawn in the given map with a unique color into the map, if they are not too small or big. +void VoronoiRandomFieldSegmentation::segmentMap(const cv::Mat& original_map, cv::Mat& segmented_map, const int epsilon_for_neighborhood, + const int max_iterations, const int min_neighborhood_size, std::vector& possible_labels, + const double min_node_distance, bool show_results, const std::string classifier_storage_path, const std::string classifier_default_path, + const int max_inference_iterations, double map_resolution_from_subscription, double room_area_factor_lower_limit, + double room_area_factor_upper_limit, double max_area_for_merging, std::vector* door_points) +{ + // check if path for storing classifier models exists + boost::filesystem::path storage_path(classifier_storage_path); + if (boost::filesystem::exists(storage_path) == false) + { + if (boost::filesystem::create_directories(storage_path) == false && boost::filesystem::exists(storage_path) == false) + { + std::cout << "Error: VoronoiRandomFieldSegmentation::segmentMap: Could not create directory " << storage_path << std::endl; + return; + } + } + + // save a copy of the original image + cv::Mat original_image = original_map.clone(); + + // if the training results haven't been loaded or trained before load them + if(trained_boost_ == false) + { + // load the AdaBoost-classifiers + std::string filename_room = classifier_storage_path + "vrf_room_boost.xml"; + std::string filename_room_default = classifier_default_path + "vrf_room_boost.xml"; + if (boost::filesystem::exists(boost::filesystem::path(filename_room)) == false) + boost::filesystem::copy_file(filename_room_default, filename_room); + loadBoost(room_boost_,filename_room); + + std::string filename_hallway = classifier_storage_path + "vrf_hallway_boost.xml"; + std::string filename_hallway_default = classifier_default_path + "vrf_hallway_boost.xml"; + if (boost::filesystem::exists(boost::filesystem::path(filename_hallway)) == false) + boost::filesystem::copy_file(filename_hallway_default, filename_hallway); + loadBoost(hallway_boost_,filename_hallway); + + std::string filename_doorway = classifier_storage_path + "vrf_doorway_boost.xml"; + std::string filename_doorway_default = classifier_default_path + "vrf_doorway_boost.xml"; + if (boost::filesystem::exists(boost::filesystem::path(filename_doorway)) == false) + boost::filesystem::copy_file(filename_doorway_default, filename_doorway); + loadBoost(doorway_boost_,filename_doorway); + + // set the trained-Boolean true to only load parameters once + trained_boost_ = true; + } + + if(trained_conditional_field_ == false) + { + // clear weights that might be standing from before + trained_conditional_weights_.clear(); + + // load the weights out of the file + std::string filename_crf = classifier_storage_path + "vrf_conditional_field_weights.txt"; + std::string filename_crf_default = classifier_default_path + "vrf_conditional_field_weights.txt"; + if (boost::filesystem::exists(boost::filesystem::path(filename_crf)) == false) + boost::filesystem::copy_file(filename_crf_default, filename_crf); + std::ifstream input_file(filename_crf.c_str()); + std::string line; + double value; + if (input_file.is_open()) + { + while (getline(input_file, line)) + { + std::istringstream iss(line); + while (iss >> value) + { + trained_conditional_weights_.push_back(value); + } + } + input_file.close(); + } + + // set the trained-Boolean to true so the weights only get read in once + trained_conditional_field_ = true; + } + + // ************* I. Create the pruned generalized Voronoi graph ************* + cv::Mat voronoi_map = original_map.clone(); + + std::set node_points; //variable for node point extraction + + // use the above defined function to create a pruned Voronoi graph + std::cout << "creating voronoi graph" << std::endl; + Timer timer; // variable to measure computation-time + createPrunedVoronoiGraph(voronoi_map, node_points); + std::cout << "created graph. Time: " << timer.getElapsedTimeInMilliSec() << "ms" << std::endl; + + // ************* II. Extract the nodes used for the conditional random field ************* + // + std::set conditional_field_nodes; + + // get the distance transformed map, which shows the distance of every white pixel to the closest zero-pixel + cv::Mat distance_map; //distance-map of the original-map (used to check the distance of each point to nearest black pixel) +#if CV_MAJOR_VERSION<=3 + cv::distanceTransform(original_map, distance_map, CV_DIST_L2, 5); +#else + cv::distanceTransform(original_map, distance_map, cv::DIST_L2, 5); +#endif + cv::convertScaleAbs(distance_map, distance_map); + + // find all nodes for the conditional random field + timer.start(); + findConditonalNodes(conditional_field_nodes, voronoi_map, distance_map, node_points, epsilon_for_neighborhood, max_iterations, min_neighborhood_size, min_node_distance); + std::cout << "found all conditional field nodes. Time: " << timer.getElapsedTimeInMilliSec() << "ms" << std::endl; + + // show the node points if wanted + cv::Mat node_map = original_map.clone(); + if(show_results == true) + { +#if CV_MAJOR_VERSION<=3 + cv::cvtColor(node_map, node_map, CV_GRAY2BGR); +#else + cv::cvtColor(node_map, node_map, cv::COLOR_GRAY2BGR); +#endif + for(std::set::iterator node = conditional_field_nodes.begin(); node != conditional_field_nodes.end(); ++node) + { +#if CV_MAJOR_VERSION<=3 + cv::circle(node_map, *node, 0, cv::Scalar(250,0,0), CV_FILLED); +#else + cv::circle(node_map, *node, 0, cv::Scalar(250,0,0), cv::FILLED); +#endif + } + +// cv::imshow("nodes of the conditional random field", node_map); +// cv::waitKey(); + } + + // ************* III. Construct the Conditional Random Field from the found nodes ************* + // + // Go along the voronoi graph each point and find the 2 or 3 nearest neighbors and construct a clique out of them. + // If enough neighbors are found or no new voronoi-nodes were found in the last step, the algorithm stops. If no new + // Voronoi-nodes got found, the current node is a dead end and has only one neighbor. + // This is done using the above defined function createConditionalField, so see this function for further information. + cv::Mat neighbor_map = node_map.clone(); // map to show the resulting cliques if wanted + + std::vector conditional_random_field_cliques; // vector to save the found cliques of the conditional random field + + // construct the conditional random field + std::cout << "starting to create conditional field" << std::endl; + timer.start(); + createConditionalField(voronoi_map, conditional_field_nodes, conditional_random_field_cliques, node_points, original_image); + + std::cout << "Created field. Time: " << timer.getElapsedTimeInMilliSec() << "ms. Number of cliques: " << conditional_random_field_cliques.size() << std::endl; + + // show the found cliques if wanted + if(show_results == true) + { + for(size_t i = 0; i < conditional_random_field_cliques.size(); ++i) + { + int blue = rand() % 250; + int green = rand() % 250; + int red = rand() % 250; + + std::vector clique_points = conditional_random_field_cliques[i].getMemberPoints(); + + if(clique_points.size() > 0) + { + for(size_t p = 0; p < clique_points.size(); ++p) + for(size_t u = 0; u < clique_points.size(); ++u) + if(u != p) + cv::line(neighbor_map, clique_points[p], clique_points[u], cv::Scalar(blue, green, red), 1); + } + +// cv::imshow("neighbors", neighbor_map); +// cv::waitKey(); + } + } + + // ************* IV. Construct the Factor graph from the calculated random-Field ************* + // + // The calculated CRF has to be a Factor graph to use the OpenGM libraries Belief-Propagation algorithm. A Factor graph is a + // graphical model that calculates a large function by calculating each part of the function for itself (https://en.wikipedia.org/wiki/Factor_graph). + // Meaning it is a function in the form f(x_0:i) = f(x_j:k) * f(x_l:m) *... . In this case each clique-potential is one part + // of the whole function and the overall function should be minimized later. To do this the Typedefs in the header gets used. + + // 1. Create the Label-Space and a factor graph. A Label-Space consists of all variables and how many labels each variable + // can obtain. + LabelSpace space(conditional_field_nodes.size(), number_of_classes_); + + FactorGraph factor_graph(space); + + // 2. Create each part of the factor graph. Each Clique-potential is one part in the factor graph, so for each clique + // a opengm-function-object gets calculated. The opengm::ExplicitFunction template gets used, meaning for each + // possible configuration of the variable-labels the double-value of the function needs to be put in the object. So each + // object is like a lookup-table later on. The variables for a function are defined later, when the factors for the graph + // are defined. The factor stores the indices of the CRF-nodes as they are stored in the set containing all nodes. Because + // of this the indices need to be sorted by size, meaning e.g. 1 needs to be before 4. So the functions also need to be + // sorted by this, to ensure that the values of it are assigned to the right variable-combination. + + // vector that stores the possible label configurations for a clique, meaning e.g. if one clique has three members with + // two possible labels for each, it stores a vector that stores the label-configurations {(0,0,0), (0,0,1), (0,1,0), ...}. + // It has a size of 4, because in this algorithm only cliques with 2-5 members are possible. + std::vector > > label_configurations(5); + + // vector that stores the index for each possible label as element + // --> to get the order as index list, so it can be used to assign the value of functions + std::vector label_indices(possible_labels.size()); + for(uint index = 0; index < possible_labels.size(); ++index) + label_indices[index] = index; + + timer.start(); + + for(uint size = 1; size <= 5; ++size) + { + // vector that stores all possible configurations for one member-size + std::vector > possible_configurations; + + // use the above defined function to find all possible configurations for the possible labels and save them in the map + getPossibleConfigurations(possible_configurations, label_indices, size); + label_configurations[size-1] = possible_configurations; + } + + std::cout << "Created all possible label-configurations. Time: " << timer.getElapsedTimeInMilliSec() << "ms" << std::endl; + + timer.start(); + // Go trough each clique and define the function and factor for it. + for(std::vector::iterator current_clique = conditional_random_field_cliques.begin(); current_clique != conditional_random_field_cliques.end(); ++current_clique) + { + // get the number of members in this clique and depending on this the possible label configurations defined above + size_t number_of_members = current_clique->getNumberOfMembers(); + + std::vector > current_possible_configurations = label_configurations[number_of_members-1]; // -1 because this vector stores configurations for cliques with 1-5 members (others are not possible in this case). + + // find the real labels and assign them into the current configuration so the feature-vector gets calculated correctly + for(size_t configuration = 0; configuration < current_possible_configurations.size(); ++configuration) + for(size_t variable = 0; variable < current_possible_configurations[configuration].size(); ++variable) + current_possible_configurations[configuration][variable] = possible_labels[current_possible_configurations[configuration][variable]]; + + // define an array that has as many elements as the clique has members and assign the number of possible labels for each + size_t variable_space[number_of_members]; + std::fill_n(variable_space, number_of_members, number_of_classes_); + + // define a explicit function-object from OpenGM containing the initial value -1.0 for each combination + opengm::ExplicitFunction f(variable_space, variable_space + number_of_members, -1.0); + + // go trough all points of the clique and find the index of it in the vector the nodes of the CRF are stored in + // --> necessary to sort the nodes correctly + size_t indices[current_clique->getNumberOfMembers()]; // array the indices are stored in + std::vector clique_points = current_clique->getMemberPoints(); + for(size_t point = 0; point < clique_points.size(); ++point) + { + // get the iterator to the element and use the std::distance function to calculate the index of the point + std::set::iterator iterator = conditional_field_nodes.find(clique_points[point]); + + if(iterator != conditional_field_nodes.end()) // check if element was found --> should be + indices[point] = std::distance(conditional_field_nodes.begin(), iterator); + else + std::cout << "element not in set" << std::endl; + } + + // get the possible configurations and swap them, respecting the indices, then sort the indices themself + std::vector > swap_configurations = label_configurations[number_of_members-1]; // -1 because this vector stores configurations for cliques with 1-5 members (others are not possible in this case). + swapConfigsRegardingNodeIndices(swap_configurations, indices); + std::sort(indices, indices + current_clique->getNumberOfMembers()); + + // Go trough each possible configuration and compute the function value for it. Use the original configuration, because + // the nodes are stored in this way, but later the value is assigned in the position using the swaped configurations. + for(size_t configuration = 0; configuration < current_possible_configurations.size(); ++configuration) + { + std::vector current_configuration = current_possible_configurations[configuration]; + + // get current feature-vector and multiply it with the trained weights + std::vector current_features(number_of_classifiers_); + getAdaBoostFeatureVector(current_features, *current_clique, current_configuration, possible_labels); + + double clique_potential = 0; + for(size_t weight = 0; weight < number_of_classifiers_; ++weight) + { + clique_potential += trained_conditional_weights_[weight] * current_features[weight]; + } + + // assign the calculated clique potential at the right position in the function --> !!Important: factors need the variables to be sorted + // as increasing index + f(swap_configurations[configuration].begin()) = clique_potential;//std::exp(clique_potential); + } + + // add the defined function to the model and catch the returned function-identifier to specify which variables + // this function needs + FactorGraph::FunctionIdentifier identifier = factor_graph.addFunction(f); + + // add the Factor to the graph, that represents which variables (and labels of each) are used for the above defined function + factor_graph.addFactor(identifier, indices, indices+current_clique->getNumberOfMembers()); + + } + std::cout << "calculated all features for the cliques. Time: " << timer.getElapsedTimeInSec() << "s" << std::endl; + + // ************* V. Do inference in the defined factor-graph to find best labels. ************* + // + // Do Inference in the above created graphical model using OpengM. This function has three control parameters: + // i. The maximum number of iterations done + // ii. The convergence Bound, which is used to check if the messages haven't changed a lot after the last step. + // iii. The damping-factor, which implies how many messages should be dumped, in this case 0. + const double convergence_bound = 1e-7; + const double damping_factor = 0.0; + LoopyBeliefPropagation::Parameter parameters(max_inference_iterations, convergence_bound, damping_factor); + + // create LoopyBeliefPropagation object that does inference on the graphical model defined above + LoopyBeliefPropagation belief_propagation(factor_graph, parameters); + + // do inference + timer.start(); + belief_propagation.infer(); + std::cout << "Done Inference. Time: " << timer.getElapsedTimeInMilliSec() << "ms" << std::endl; + + // obtain the labels that get the max value of the defined function + std::vector best_labels(conditional_field_nodes.size()); + belief_propagation.arg(best_labels); + + // print the solution if wanted + if(show_results == true) + { + cv::Mat resulting_map = original_image.clone(); + + for(std::set::iterator i = conditional_field_nodes.begin(); i != conditional_field_nodes.end(); ++i) + { + size_t distance = std::distance(conditional_field_nodes.begin(), i); +#if CV_MAJOR_VERSION<=3 + cv::circle(resulting_map, *i, 3, cv::Scalar(possible_labels[best_labels[distance]]), CV_FILLED); +#else + cv::circle(resulting_map, *i, 3, cv::Scalar(possible_labels[best_labels[distance]]), cv::FILLED); +#endif + } + + cv::imshow("node-map", resulting_map); +// cv::waitKey(); + } + std::cout << "complete Potential: " << belief_propagation.value() << std::endl; + + // for optimization purpose +// return belief_propagation.value(); + + // ************* VI. Search for different regions of same color and make them a individual segment ************* + // + // 1. Connect the found nodes to the two nearest black pixels (base points) of them. Connect the nodes that are labeled + // as doorway with black lines to create intersections. This is done because it gives good results, when finding + // segments labeled only as one class, because later on this map with the base-lines a wavefront-region-growing is + // applied. + + // erode the map to close small gaps and remove errors --> also done when producing the voronoi-graph. + cv::Mat map_copy, eroded_map; + cv::Point anchor(-1, -1); + cv::erode(original_image, eroded_map, cv::Mat(), anchor, 2); + map_copy = eroded_map.clone(); + + // find the layout of the map and discretize it to get possible base points + std::vector < std::vector > map_contours; + std::vector < cv::Vec4i > hierarchy; +#if CV_MAJOR_VERSION<=3 + cv::findContours(map_copy, map_contours, hierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_NONE); +#else + cv::findContours(map_copy, map_contours, hierarchy, cv::RETR_CCOMP, cv::CHAIN_APPROX_NONE); +#endif + + timer.start(); + + // reassign the map because findContours destroys it and erode it to close small errors + map_copy = eroded_map.clone(); + + // go trough all crf-nodes + for(std::set::iterator node = conditional_field_nodes.begin(); node != conditional_field_nodes.end(); ++node) + { + // find index of point + size_t distance = std::distance(conditional_field_nodes.begin(), node); + + // set initial points and values for the basis points so the distance comparison can be done + cv::Point basis_point_1 = map_contours[0][0]; + cv::Point basis_point_2 = map_contours[0][1]; + + // initial values of the first vector from the current critical point to the contour points and for the squared distance of it + double vector_x_1 = node->x - map_contours[0][0].x; + double vector_y_1 = node->y - map_contours[0][0].y; + double distance_basis_square_1 = vector_x_1*vector_x_1 + vector_y_1*vector_y_1; + + // initial values of the second vector from the current critical point to the contour points and for the squared distance of it + double vector_x_2 = node->x - map_contours[0][1].x; + double vector_y_2 = node->y - map_contours[0][1].y; + double distance_basis_square_2 = vector_x_2*vector_x_2 + vector_y_2*vector_y_2; + + // find first basis point + int basis_vector_1_x, basis_vector_2_x, basis_vector_1_y, basis_vector_2_y; + for (int c = 0; c < map_contours.size(); c++) + { + for (int p = 0; p < map_contours[c].size(); p++) + { + // calculate the squared Euclidian distance from the critical Point to the Point on the contour + const double vector_x = map_contours[c][p].x - node->x; + const double vector_y = map_contours[c][p].y - node->y; + const double current_distance = vector_x*vector_x + vector_y*vector_y; + + // compare the distance to the saved distances if it is smaller + if (current_distance < distance_basis_square_1) + { + distance_basis_square_1 = current_distance; + basis_point_1 = map_contours[c][p]; + basis_vector_1_x = vector_x; + basis_vector_1_y = vector_y; + } + } + } + // find second basisPpoint + for (int c = 0; c < map_contours.size(); c++) + { + for (int p = 0; p < map_contours[c].size(); p++) + { + // calculate the squared Euclidian distance from the critical point to the point on the contour + const double vector_x = map_contours[c][p].x - node->x; + const double vector_y = map_contours[c][p].y - node->y; + const double current_distance = vector_x*vector_x + vector_y*vector_y; + + // calculate the distance between the current contour point and the first basis point to make sure they + // are not too close to each other + const double vector_x_basis = basis_point_1.x - map_contours[c][p].x; + const double vector_y_basis = basis_point_1.y - map_contours[c][p].y; + const double basis_distance = vector_x_basis*vector_x_basis + vector_y_basis*vector_y_basis; + if (current_distance > distance_basis_square_1 && current_distance < distance_basis_square_2 && + basis_distance > (double) distance_map.at(*node)*distance_map.at(*node)) + { + distance_basis_square_2 = current_distance; + basis_point_2 = map_contours[c][p]; + basis_vector_2_x = vector_x; + basis_vector_2_y = vector_y; + } + } + } + + // if the node is labeled as doorway draw the base-lines black --> as intersection + if(best_labels[distance] == 2) + { + // draw a line from the node to the two basis points + cv::line(map_copy, *node, basis_point_1, 0, 2); + cv::line(map_copy, *node, basis_point_2, 0, 2); + + // return the door-points if wanted + if(door_points != NULL) + door_points->push_back(*node); + } + else + { + // draw a line from the node to the two basis points + cv::line(map_copy, *node, basis_point_1, possible_labels[best_labels[distance]], 1); + cv::line(map_copy, *node, basis_point_2, possible_labels[best_labels[distance]], 1); + } + } + + std::cout << "drawn all segments. Time: " << timer.getElapsedTimeInMilliSec() << "ms" << std::endl; + + // 2. Apply a wavefront algorithm to the map copy to fill all segments with the labels that are given to the crf-nodes. + map_copy.convertTo(map_copy, CV_32SC1, 256, 0); + wavefrontRegionGrowing(map_copy); + + if(show_results == true) + { + cv::imshow("intersected map", map_copy); +// cv::waitKey(); + } + + // 3. search for points where the map copy is still white, because these points could get colored wrong + // by too large contours + std::vector white_points; + for(unsigned int u = 0; u < map_copy.rows; ++u) + for(unsigned int v = 0; v < map_copy.cols; ++v) + if(map_copy.at(u, v) == 255*256) + white_points.push_back(cv::Point(v, u)); + + // 3. Make everything black except the found segments of rooms/hallways, that become totally white. This is done because + // it makes possible to find the contours of the segments and draw them into the original map with a random color. + // Only rooms or hallways stay at one step to ensure that borders from hallways to rooms are recognized. + std::set, compContoursSize> segments; // variable to save found contours + + timer.start(); + for(int color = 0; color <= 1; ++color) + { + // clear previously used variables for contour extraction + map_contours.clear(); + hierarchy.clear(); + + // get the color that should get white --> label given as 8bit color, but it has changed to a 32bit color + int current_color = possible_labels[color] * 256; + + // create a map_copy + cv::Mat temporary_map = cv::Mat(map_copy.rows, map_copy.cols, original_image.type()); + + // make regions black and white + for(unsigned int u = 0; u < map_copy.rows; ++u) + { + for(unsigned int v = 0; v < map_copy.cols; ++v) + { + // check if color is found and pixel hasn't been drawn accidentally in a color + if(map_copy.at(u, v) == current_color && original_image.at(u, v) != 0) + temporary_map.at(u, v) = 255; + else + temporary_map.at(u, v) = 0; + } + } + // find the contours of the rooms/hallways +#if CV_MAJOR_VERSION<=3 + cv::findContours(temporary_map, map_contours, hierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_NONE); +#else + cv::findContours(temporary_map, map_contours, hierarchy, cv::RETR_CCOMP, cv::CHAIN_APPROX_NONE); +#endif + + // save the contours that are not holes (check with hierarchy --> [{0,1,2,3}]={next contour (same level), previous contour (same level), child contour, parent contour}) + // --> save everything with hierarchy[3] == -1 + for(size_t contour = 0; contour < map_contours.size(); ++contour) + if(hierarchy[contour][3] == -1 && map_contours.size() > 1) + segments.insert(map_contours[contour]); + } + + std::cout << "found segments: " << segments.size() << ". Time: " << timer.getElapsedTimeInMilliSec() << "ms" << std::endl; + + // 4. Draw the found contours with a unique color and apply a wavefront-region-growing algorithm to get rid of remaining + // white spaces. Also save the found segments as rooms to merge rooms together in the next step. + timer.start(); + original_map.convertTo(segmented_map, CV_32SC1, 256, 0); // convert input image to CV_32SC1 (needed for wavefront and to have enoguh possible rooms) + + std::vector < cv::Scalar > already_used_colors; //saving-vector to save the already used colors + + std::vector rooms; // vector to save the rooms in this map + + for(std::set >::iterator current_contour = segments.begin(); current_contour != segments.end(); ++current_contour) + { + // calculate area for the contour and check if it is large enough to be a separate segment + double room_area = map_resolution_from_subscription * map_resolution_from_subscription * cv::contourArea(*current_contour); + if (room_area >= room_area_factor_lower_limit && room_area <= room_area_factor_upper_limit) + { + // Draw the region with a random color into the map if it is large/small enough + bool drawn = false; + int loop_counter = 0; //counter if the loop gets into a endless loop + do + { + loop_counter++; + int random_number = rand() % 52224 + 13056; + cv::Scalar fill_color(random_number); + + //check if color has already been used + if (!contains(already_used_colors, fill_color) || loop_counter > 1000) + { +#if CV_MAJOR_VERSION<=3 + cv::drawContours(segmented_map, std::vector >(1,*current_contour), -1, fill_color, CV_FILLED); +#else + cv::drawContours(segmented_map, std::vector >(1,*current_contour), -1, fill_color, cv::FILLED); +#endif + already_used_colors.push_back(fill_color); + Room current_room(random_number); //add the current Contour as a room + for (int point = 0; point < current_contour->size(); point++) //add contour points to room + { + current_room.insertMemberPoint(cv::Point(current_contour->at(point)), map_resolution_from_subscription); + } + rooms.push_back(current_room); + drawn = true; + } + } while (!drawn); + } + // draw too small segments white, to prevent that they are covered by large closed contours + if(room_area < room_area_factor_lower_limit) +#if CV_MAJOR_VERSION<=3 + cv::drawContours(segmented_map, std::vector >(1,*current_contour), -1, 255*256, CV_FILLED); +#else + cv::drawContours(segmented_map, std::vector >(1,*current_contour), -1, 255*256, cv::FILLED); +#endif + } + + // Make black what has been black before (drawContours draws filled areas and might overwrite black holes). + // Also make regions that are black on the eroded map but white on the original map also white to prevent large closed contours + // to go around small contours in them. Also make the intersections in got by connecting the voronoi nodes with their + // base points for doorways white for the same reason. + for(unsigned int u = 0; u < segmented_map.rows; ++u) + { + for(unsigned int v = 0; v < segmented_map.cols; ++v) + { + if(original_image.at(u, v) == 0 && eroded_map.at(u, v) == 0) + { + segmented_map.at(u, v) = 0; + } + + if(original_image.at(u, v) == 255 && eroded_map.at(u, v) == 0) + { + segmented_map.at(u, v) = 255*256; + } + if(map_copy.at(u, v) == 0 && original_image.at(u, v) == 255) + { + segmented_map.at(u, v) = 255*256; + } + } + } + + // go trough the saved white points and make them white in the segmented map again + for(std::vector::iterator point = white_points.begin(); point != white_points.end(); ++point) + segmented_map.at(*point) = 255*256; + + // color remaining white space + wavefrontRegionGrowing(segmented_map); + + std::cout << "filled map with unique colors. Time: " << timer.getElapsedTimeInMilliSec() << "ms" << std::endl; + + // 5. Merge rooms that are too small together with the surrounding big rooms. This is done because it is possible to + // create very small segments in the last step, tha don't make very much sense. + timer.start(); + + if(show_results == true) + cv::imshow("before merge", segmented_map); + + mergeRooms(segmented_map, rooms, map_resolution_from_subscription, max_area_for_merging, false); + + std::cout << "merged rooms together. Time: " << timer.getElapsedTimeInMilliSec() << "ms" << std::endl; + + if(show_results == true) + { + cv::imshow("segmented map", segmented_map); + cv::waitKey(); + } +} + +// Function to test several functions of this algorithm independent of other functions +// this current implementation is used for optimizing the trained algorithm. The idea behind this is: I train the algorithm +// with the same training-maps several times and calculate the crf-graph-potential for several maps and add them together. If +// then the current sum is better than a saved best sum, the trained parameters are saved, what is done with this function. +// It is good to do this, because OpenCV uses a Decision-Tree for the AdaBoost classifiers, which is depending on probabilites +// and so every training done creates different results. +void VoronoiRandomFieldSegmentation::testFunc(const cv::Mat& original_map) +{ + std::cout << "testfunc" << std::endl; + +// // if the training results haven't been loaded or trained before load them +// std::string filename_room = boost_storage_path + "vrf_room_boost.xml"; +// std::string filename_hallway = boost_storage_path + "vrf_hallway_boost.xml"; +// std::string filename_doorway = boost_storage_path + "vrf_doorway_boost.xml"; +// +// room_boost_.save(filename_room.c_str(), "boost"); +// hallway_boost_.save(filename_hallway.c_str(), "boost"); +// doorway_boost_.save(filename_doorway.c_str(), "boost"); +// +// std::ofstream output_file(crf_storage_path.c_str(), std::ios::out); +// if (output_file.is_open()==true) +// { +// for(size_t weight = 0; weight < trained_conditional_weights_.size(); ++ weight) +// { +// output_file << trained_conditional_weights_[weight] << std::endl; +// } +// } +// output_file.close(); +// +// +// if(trained_boost_ == false) +// { +// // load the AdaBoost-classifiers +// room_boost_.load(filename_room.c_str()); +// hallway_boost_.load(filename_hallway.c_str()); +// doorway_boost_.load(filename_doorway.c_str()); +// +// // set the trained-Boolean true to only load parameters once +// trained_boost_ = true; +// } +// +// if(trained_conditional_field_ == false) +// { +// // load the weights out of the file +// std::ifstream input_file(crf_storage_path.c_str()); +// std::string line; +// double value; +// if (input_file.is_open()) +// { +// while (getline(input_file, line)) +// { +// std::istringstream iss(line); +// while (iss >> value) +// { +// trained_conditional_weights_.push_back(value); +// } +// } +// input_file.close(); +// } +// +// // set the trained-Boolean to true so the weights only get read in once +// trained_conditional_field_ = true; +// } +// +// std::cout << "reading weights: " << std::endl; +// +// cv::Mat featuresMat(1, getFeatureCount(), CV_32FC1); //OpenCV expects a 32-floating-point Matrix as feature input +// for (int f = 1; f <= getFeatureCount(); ++f) +// { +// //get the features for each room and put it in the featuresMat +// featuresMat.at(0, f - 1) = (float) 1; +// } +// +// // Calculate the weak hypothesis by using the wanted classifier. +// CvMat features = featuresMat; +// cv::Mat weaker (1, number_of_classifiers_, CV_32F); +// CvMat weak_hypothesis = weaker; // Wanted from OpenCV to get the weak hypothesis from the +// // separate weak classifiers. +// +// // For each point the classifier depends on the given label. If the point is labeled as a room the room-boost should be +// // used and so on. +// room_boost_.predict(&features, 0, &weak_hypothesis); +// +// for(size_t f = 0; f < number_of_classifiers_; ++f) +// { +// std::cout << (double) CV_MAT_ELEM(weak_hypothesis, float, 0, f) << std::endl; +// } +// +// std::cout << "cols: " << weights.cols << " rows: " << weights.rows << std::endl; +// +// for(size_t i = 0; i < weights->cols; ++i) +// for(size_t j = 0; j < weights->rows; ++j) +// std::cout << (double) CV_MAT_ELEM(*weights, float, i, j) << std::endl; +// +// std::cout << "loaded files" << std::endl; +} + diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/src/voronoi_segmentation.cpp b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/src/voronoi_segmentation.cpp new file mode 100644 index 0000000..e0cb7db --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/src/voronoi_segmentation.cpp @@ -0,0 +1,355 @@ +#include + +#include +#include + +#include +#include + + + +VoronoiSegmentation::VoronoiSegmentation() +{ + +} + + +void VoronoiSegmentation::segmentMap(const cv::Mat& map_to_be_labeled, cv::Mat& segmented_map, double map_resolution_from_subscription, + double room_area_factor_lower_limit, double room_area_factor_upper_limit, int neighborhood_index, int max_iterations, + double min_critical_point_distance_factor, double max_area_for_merging, bool display_map) +{ + //****************Create the Generalized Voronoi-Diagram********************** + //This function takes a given map and segments it with the generalized Voronoi-Diagram. It takes following steps: + // I. It calculates the generalized Voronoi-Diagram using the function createVoronoiGraph. + // II. It extracts the critical points, which show the border between two segments. This part takes these steps: + // 1. Extract node-points of the Voronoi-Diagram, which have at least 3 neighbors. + // 2. Reduce the leave-nodes (Point on graph with only one neighbor) of the graph until the reduction + // hits a node-Point. This is done to reduce the lines along the real voronoi-graph, coming from the discretisation + // of the contour. + // 3. Find the critical points in the reduced graph by searching in a specified neighborhood for a local minimum + // in distance to the nearest black pixel. The size of the epsilon-neighborhood is dynamic and goes larger + // in small areas, so they are split into lesser regions. + // III. It gets the critical lines, which go from the critical point to its two nearest black pixels and separate the + // regions from each other. This part does following steps: + // 1. Get the discretized contours of the map and the holes, because these are the possible candidates for + // basis-points. + // 2. Find the basis-points for each critical-point by finding the two nearest neighbors of the vector from 1. + // Also it saves the angle between the two vectors pointing from the critical-point to its two basis-points. + // 3. Some critical-lines are too close to each other, so the next part eliminates some of them. For this the + // algorithm checks, which critical points are too close to each other. Then it compares the angles of these + // points, which were calculated in 3., and takes the one with the larger angle, because smaller angles + // (like 90 degree) are more likely to be at edges of the map or are too close to the borders. If they have + // the same angle, the point which comes first in the critical-point-vector is chosen (took good results for + // me, but is only subjective). + // 4. Draw the critical lines, selected by 3. in the map with color 0. + // IV. It finds the segments, which are seperated by the critical lines of III. and fills them with a random colour that + // hasn't been already used yet. For this it: + // 1. It erodes the map with critical lines, so small gaps are closed, and finds the contours of the segments. + // Only contours that are large/small enough are chosen to be drawn. + // 2. It draws the contours from 1. in a map with a random colour. Contours that belong to holes are not drawn + // into the map. + // 3. Spread the colour-regions to the last white Pixels, using the watershed-region-spreading function. + + //*********************I. Calculate and draw the Voronoi-Diagram in the given map***************** + + cv::Mat voronoi_map = map_to_be_labeled.clone(); + createVoronoiGraph(voronoi_map); //voronoi-map for the segmentation-algorithm + + //***************************II. extract the possible candidates for critical Points**************************** + // 1.extract the node-points that have at least three neighbors on the voronoi diagram + // node-points are points on the voronoi-graph that have at least 3 neighbors + // 2.reduce the side-lines along the voronoi-graph by checking if it has only one neighbor until a node-point is reached + // --> make it white + // repeat a large enough number of times so the graph converges + std::set node_points; //variable for node point extraction + pruneVoronoiGraph(voronoi_map, node_points); + + //3.find the critical points in the previously calculated generalized Voronoi-graph by searching in a specified + // neighborhood for the local minimum of distance to the nearest black pixel + // critical points need to have at least two neighbors (else they are end points, which would give a very small segment) + + //get the distance transformed map, which shows the distance of every white pixel to the closest zero-pixel + cv::Mat distance_map; //distance-map of the original-map (used to check the distance of each point to nearest black pixel) +#if CV_MAJOR_VERSION<=3 + cv::distanceTransform(map_to_be_labeled, distance_map, CV_DIST_L2, 5); +#else + cv::distanceTransform(map_to_be_labeled, distance_map, cv::DIST_L2, 5); +#endif + cv::convertScaleAbs(distance_map, distance_map); + + std::vector critical_points; //saving-variable for the critical points found on the Voronoi-graph + for (int v = 0; v < voronoi_map.rows; v++) + { + for (int u = 0; u < voronoi_map.cols; u++) + { + if (voronoi_map.at(v, u) == 127) + { + //make the size of the region to be checked dependent on the distance of the current pixel to the closest + //zero-pixel, so larger areas are split into more regions and small areas into fewer + int eps = neighborhood_index / (int) distance_map.at(v, u); //310 + int loopcounter = 0; //if a part of the graph is not connected to the rest this variable helps to stop the loop + std::vector temporary_points; //neighboring-variables, which are different for each point + std::set neighbor_points; //neighboring-variables, which are different for each point + int neighbor_count = 0; //variable to save the number of neighbors for each point + neighbor_points.insert(cv::Point(u,v)); //add the current Point to the neighborhood + //find every Point along the voronoi graph in a specified neighborhood + do + { + loopcounter++; + //check every point in the neighborhood for other neighbors connected to it + for(std::set::iterator it_neighbor_points = neighbor_points.begin(); it_neighbor_points != neighbor_points.end(); it_neighbor_points++) + { + for (int row_counter = -1; row_counter <= 1; row_counter++) + { + for (int column_counter = -1; column_counter <= 1; column_counter++) + { + if (row_counter == 0 && column_counter == 0) + continue; + + //check the neighboring points + //(if it already is in the neighborhood it doesn't need to be checked again) + const cv::Point& current_neighbor_point = *it_neighbor_points; + const int nu = current_neighbor_point.x + column_counter; + const int nv = current_neighbor_point.y + row_counter; + if (nv >= 0 && nu >= 0 && nv < voronoi_map.rows && nu < voronoi_map.cols && + voronoi_map.at(nv, nu) == 127 && neighbor_points.find(cv::Point(nu, nv))==neighbor_points.end()) + { + neighbor_count++; + temporary_points.push_back(cv::Point(nu, nv)); + } + } + } + } + //go trough every found point after all neighborhood points have been checked and add them to it + for (int temporary_point_index = 0; temporary_point_index < temporary_points.size(); temporary_point_index++) + { + neighbor_points.insert(temporary_points[temporary_point_index]); + //make the found points white in the voronoi-map (already looked at) + voronoi_map.at(temporary_points[temporary_point_index].y, temporary_points[temporary_point_index].x) = 255; + voronoi_map.at(v, u) = 255; + } + //check if enough neighbors have been checked or checked enough times (e.g. at a small segment of the graph) + } while (neighbor_count <= eps && loopcounter < max_iterations); + //check every found point in the neighborhood if it is the local minimum in the distanceMap + cv::Point current_critical_point = cv::Point(u, v); + for(std::set::iterator it_neighbor_points = neighbor_points.begin(); it_neighbor_points != neighbor_points.end(); it_neighbor_points++) + { + if (distance_map.at(it_neighbor_points->y, it_neighbor_points->x) < distance_map.at(current_critical_point.y, current_critical_point.x)) + { + current_critical_point = cv::Point(*it_neighbor_points); + } + } + //add the local minimum point to the critical points + critical_points.push_back(current_critical_point); + } + } + } + +// if(display_map == true) +// { +// cv::Mat display = map_to_be_labeled.clone(); +// for (size_t i=0; i 255*256 = 65280 + + // 1. Get the points of the contour, which are the possible closest points for a critical point + //clone the map to extract the contours, because after using OpenCV find-/drawContours + //the map will be different from the original one + cv::Mat temporary_map_to_extract_the_contours = segmented_map.clone(); + std::vector < std::vector > contours; +#if CV_MAJOR_VERSION<=3 + cv::findContours(temporary_map_to_extract_the_contours, contours, CV_RETR_CCOMP, CV_CHAIN_APPROX_NONE); +#else + cv::findContours(temporary_map_to_extract_the_contours, contours, cv::RETR_CCOMP, cv::CHAIN_APPROX_NONE); +#endif + + // 2. Get the basis-points for each critical-point + std::vector basis_points_1, basis_points_2; + std::vector length_of_critical_line; + std::vector angles; //the angles between the basis-lines of each critical Point + for (int critical_point_index = 0; critical_point_index < critical_points.size(); critical_point_index++) + { + //set inital points and values for the basis points so the distance comparison can be done + cv::Point basis_point_1 = contours[0][0]; + cv::Point basis_point_2 = contours[0][1]; + //inital values of the first vector from the current critical point to the contour points and for the distance of it + const cv::Point& critical_point = critical_points[critical_point_index]; + double vector_x_1 = critical_point.x - contours[0][0].x; + double vector_y_1 = critical_point.y - contours[0][0].y; + double distance_basis_1 = std::sqrt(vector_x_1*vector_x_1 + vector_y_1*vector_y_1); + //inital values of the second vector from the current critical point to the contour points and for the distance of it + double vector_x_2 = critical_point.x - contours[0][1].x; + double vector_y_2 = critical_point.y - contours[0][1].y; + double distance_basis_2 = std::sqrt(vector_x_2*vector_x_2 + vector_y_2*vector_y_2); + + //find first basis point + int basis_vector_1_x, basis_vector_2_x, basis_vector_1_y, basis_vector_2_y; + for (int c = 0; c < contours.size(); c++) + { + for (int p = 0; p < contours[c].size(); p++) + { + //calculate the Euclidian distance from the critical Point to the Point on the contour + const double vector_x = contours[c][p].x - critical_point.x; + const double vector_y = contours[c][p].y - critical_point.y; + const double current_distance = std::sqrt(vector_x*vector_x + vector_y*vector_y); + //compare the distance to the saved distances if it is smaller + if (current_distance < distance_basis_1) + { + distance_basis_1 = current_distance; + basis_point_1 = contours[c][p]; + basis_vector_1_x = vector_x; + basis_vector_1_y = vector_y; + } + } + } + //find second basisPpoint + for (int c = 0; c < contours.size(); c++) + { + for (int p = 0; p < contours[c].size(); p++) + { + //calculate the Euclidian distance from the critical point to the point on the contour + const double vector_x = contours[c][p].x - critical_point.x; + const double vector_y = contours[c][p].y - critical_point.y; + const double current_distance = std::sqrt(vector_x*vector_x + vector_y*vector_y); + //calculate the distance between the current contour point and the first basis point to make sure they + //are not too close to each other + const double vector_x_basis = basis_point_1.x - contours[c][p].x; + const double vector_y_basis = basis_point_1.y - contours[c][p].y; + const double basis_distance = std::sqrt(vector_x_basis*vector_x_basis + vector_y_basis*vector_y_basis); + if (current_distance > distance_basis_1 && current_distance < distance_basis_2 && + basis_distance > (double) distance_map.at(critical_point.y, critical_point.x)) + { + distance_basis_2 = current_distance; + basis_point_2 = contours[c][p]; + basis_vector_2_x = vector_x; + basis_vector_2_y = vector_y; + } + } + } + //calculate angle between the vectors from the critical Point to the found basis-points + double current_angle = std::acos((basis_vector_1_x * basis_vector_2_x + basis_vector_1_y * basis_vector_2_y) / (distance_basis_1 * distance_basis_2)) * 180.0 / PI; + + //save the critical line with its calculated values + basis_points_1.push_back(basis_point_1); + basis_points_2.push_back(basis_point_2); + length_of_critical_line.push_back(distance_basis_1 + distance_basis_2); + angles.push_back(current_angle); + } + + //3. Check which critical points should be used for the segmentation. This is done by checking the points that are + // in a specified distance to each other and take the point with the largest calculated angle, because larger angles + // correspond to a separation across the room, which is more useful + for (int first_critical_point = 0; first_critical_point < critical_points.size(); first_critical_point++) + { + //reset variable for checking if the line should be drawn + bool draw = true; + for (int second_critical_point = 0; second_critical_point < critical_points.size(); second_critical_point++) + { + if (second_critical_point != first_critical_point) + { + //get distance of the two current Points + const double vector_x = critical_points[second_critical_point].x - critical_points[first_critical_point].x; + const double vector_y = critical_points[second_critical_point].y - critical_points[first_critical_point].y; + const double critical_point_distance = std::sqrt(vector_x*vector_x + vector_y*vector_y); + //check if the points are too close to each other corresponding to the distance to the nearest black pixel + //of the current critical point. This is done because critical points at doors are closer to the black region + //and shorter and may be eliminated in the following step. By reducing the checking distance at this point + //it gets better. + if (critical_point_distance < ((int) distance_map.at(critical_points[first_critical_point].y, critical_points[first_critical_point].x) * min_critical_point_distance_factor)) //1.7 + { + //if one point in neighborhood is found that has a larger angle the actual to-be-checked point shouldn't be drawn + if (angles[first_critical_point] < angles[second_critical_point]) + { + draw = false; + } + //if the angles of the two neighborhood points are the same the shorter one should be drawn, because it is more likely something like e.g. a door + if (angles[first_critical_point] == angles[second_critical_point] && + length_of_critical_line[first_critical_point] > length_of_critical_line[second_critical_point] && + (length_of_critical_line[second_critical_point] > 3 || first_critical_point > second_critical_point)) + { + draw = false; + } + } + } + } + //4. draw critical-lines if angle of point is larger than the other + if (draw) + { + cv::line(voronoi_map, critical_points[first_critical_point], basis_points_1[first_critical_point], cv::Scalar(0), 2); + cv::line(voronoi_map, critical_points[first_critical_point], basis_points_2[first_critical_point], cv::Scalar(0), 2); + } + } +// if(display_map == true) +// cv::imshow("voronoi_map", voronoi_map); + + //***********************Find the Contours seperated from the critcal lines and fill them with color****************** + + std::vector < cv::Scalar > already_used_colors; //saving-vector to save the already used coloures + + std::vector < cv::Vec4i > hierarchy; //variables for coloring the map + + std::vector rooms; //Vector to save the rooms in this map + + //1. Erode map one time, so small gaps are closed +// cv::erode(voronoi_map_, voronoi_map_, cv::Mat(), cv::Point(-1, -1), 1); +#if CV_MAJOR_VERSION<=3 + cv::findContours(voronoi_map, contours, hierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE); +#else + cv::findContours(voronoi_map, contours, hierarchy, cv::RETR_CCOMP, cv::CHAIN_APPROX_SIMPLE); +#endif + + for (int current_contour = 0; current_contour < contours.size(); current_contour++) + { //only draw contours that aren't holes + if (hierarchy[current_contour][3] == -1) + { + //calculate area for the contour and check if it is large enough to be a room + double room_area = map_resolution_from_subscription * map_resolution_from_subscription * cv::contourArea(contours[current_contour]); + if (room_area >= room_area_factor_lower_limit && room_area <= room_area_factor_upper_limit) + { + //2. Draw the region with a random color into the map if it is large/small enough + bool drawn = false; + int loop_counter = 0; //counter if the loop gets into a endless loop + do + { + loop_counter++; + int random_number = rand() % 52224 + 13056; + cv::Scalar fill_colour(random_number); + //check if color has already been used + if (!contains(already_used_colors, fill_colour) || loop_counter > 1000) + { + cv::drawContours(segmented_map, contours, current_contour, fill_colour, 1); + already_used_colors.push_back(fill_colour); + Room current_room(random_number); //add the current Contour as a room + for (int point = 0; point < contours[current_contour].size(); point++) //add contour points to room + { + current_room.insertMemberPoint(cv::Point(contours[current_contour][point]), map_resolution_from_subscription); + } + rooms.push_back(current_room); + drawn = true; + } + } while (!drawn); + } + } + } + std::cout << "Found " << rooms.size() << " rooms.\n"; + + //3.fill the last white areas with the surrounding color + wavefrontRegionGrowing(segmented_map); + + if(display_map == true) + { + cv::imshow("before", segmented_map); + cv::waitKey(1); + } + + //4.merge the rooms together if neccessary + mergeRooms(segmented_map, rooms, map_resolution_from_subscription, max_area_for_merging, display_map); +} diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/src/wavefront_region_growing.cpp b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/src/wavefront_region_growing.cpp new file mode 100644 index 0000000..877ce8a --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/common/src/wavefront_region_growing.cpp @@ -0,0 +1,44 @@ +#include + +// spreading image is supposed to be of type CV_32SC1 +void wavefrontRegionGrowing(cv::Mat& image) +{ + //This function spreads the colored regions of the given map to the neighboring white pixels + if (image.type()!=CV_32SC1) + { + std::cout << "Error: wavefrontRegionGrowing: provided image is not of type CV_32SC1." << std::endl; + return; + } + + cv::Mat spreading_map = image.clone(); + bool finished = false; + while (finished == false) + { + finished = true; + for (int row = 1; row < spreading_map.rows-1; ++row) + { + for (int column = 1; column < spreading_map.cols-1; ++column) + { + if (spreading_map.at(row, column) > 65279) // unassigned pixels + { + //check 3x3 area around white pixel for fillcolour, if filled Pixel around fill white pixel with that colour + bool set_value = false; + for (int row_counter = -1; row_counter <= 1 && set_value==false; ++row_counter) + { + for (int column_counter = -1; column_counter <= 1 && set_value==false; ++column_counter) + { + int value = image.at(row + row_counter, column + column_counter); + if (value != 0 && value <= 65279) + { + spreading_map.at(row, column) = value; + set_value = true; + finished = false; // keep on iterating the wavefront propagation until no more changes occur + } + } + } + } + } + } + image = spreading_map.clone(); + } +} diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/package.xml b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/package.xml new file mode 100644 index 0000000..0341cf7 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/package.xml @@ -0,0 +1,40 @@ + + + ipa_room_segmentation + 0.1.0 + + Segmentation procedures for dividing floor maps into single rooms or areas. + It contains four different algorithms for doing this: + 1. A morphological algorithm, which provides morphological operations on the map and finds the room-like areas with opencv. (common/src/morphological_segmentation.cpp) + 2. A distance-transformation based algorithm, which first produces a distance-transformed map using opencv, then takes a variable threshold over the map, coloring everything + above it white, and finds the areas of the rooms in the threshold-map. (common/src/distance_segmentation.cpp) + 3. A voronoi-based algorithm, which produces a generalized voronoi-diagram in the map, finds critical Points in a epsilon-neighborhood on the graph and partion the map with the + borders trough this critical Points. (common/src/vornoi_segmentation.cpp) + 4. A semantic-labeling algorithm, which calculates simple single-valued features for a given Position and classifies to which class (room, hallway) this Position belongs using + Ada-Boost from opencv. (common/src/semantic_segmentation.cpp) + 5. A mix of the semantic-labeling and the voronoi-based algorithm, called voronoi-random-fields. It creates a conditional-random-field out of a pruned generalized Voronoi-graph + and uses the weak-responses of the AdaBoost classifiers to find the best labels for each node. Advantage of this over the semantic labeling is that it can take care of + neighbor-relations, which allows to detect doorways, as well as rooms and hallways. (common/src/voronoi_random_field_segmentation.cpp) + For more detailed descriptions see the corresponding files. This algorithms often uses OpenCV, so it's very important for these algorithms. + + Florian Jordan + Richard Bormann + LGPL for academic and non-commercial use; for commercial use, please contact Fraunhofer IPA + http://wiki.ros.org/ipa_room_segmentation + + catkin + + actionlib + boost + cv_bridge + dynamic_reconfigure + ipa_building_msgs + libdlib + libopencv-dev + nav_msgs + opengm + roscpp + roslib + sensor_msgs + + diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/readme.md b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/readme.md new file mode 100644 index 0000000..a9f0d67 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/readme.md @@ -0,0 +1,80 @@ +# ipa_room_segmentation +Algorithms for floor plan segmentation. + +If you find this software useful in your work, please cite our corresponding paper: +- R. Bormann, F. Jordan, W. Li, J. Hampp, and M. Hägele. Room Segmentation: Survey, Implementation, and Analysis. In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), 2016. https://ieeexplore.ieee.org/abstract/document/7487234 , https://publica.fraunhofer.de/entities/publication/0bf23149-75d5-4601-bfce-992d91698862/details + +# General Procedure + +1. Change the algorithm parameters in ros/launch/room_segmentation_action_server_params.yaml in ros/launch to the wanted algorithms and settings. + * room_segmentation_algorithm: Choose which segmentation algorithm should be used. + * display_segmented_map: Specify if you want a window to pop up after the given map has been segmented, showing the results. To continue the program press any button in the window. + * train_semantic, train_vrf: Booleans that show, if the semantic or voronoi random field segmentation should be trained. For details of this training method see the further document. + * Every algorithm has its own specific range of **allowed room areas**. Segments that are out of this range will not be drawn and are going to be filled by the surrounding valid rooms. **Remark:** The upper border in the semantic segmentation method has a different meaning. If found segments are too big they become separated by randomly putting centers in the segment and splitting it by using a wavefront algorithm. It's better to turn this off by setting the allowed maximal area very high. + * Some algorithms have specific parameters that change some functionalities of it, which is described in detail in the .yaml file. + +2. Start the action server using the file /ros/launch/room_segmentation_action_server.launch, which executes the /ros/src/room_segmentation_server.cpp file. If the server crashes for some reason (e.g. no valid map given by the client) it respawns with a little delay. + +3. Start an action client, which sends a goal to the segmentation action server, corresponding to the MapSegmentation.action message, which lies in ipa_building_msgs/action. The goal consists of the following parts + + * input_map: The map that should be segmented, as sensor_msgs/Image. **Has to be a 8-Bit single channel image, with 0 as occupied space and 255 as free space**. + * map_resolution: The resolution the map has been sampled with [meter/cell]. + * map_origin: The origin of the map in [meter] to bring the provided map coordinates in absolute coordinates, if wanted. + * return_format_in_pixel: Boolean to indicate if the results should be returned in pixel-coordinates. + * return_format_in_meter: Boolean to indicate if the results should be returned in absolute coordinates. + * robot_radius: The radius of your robot. If this is set to a value greater than 0 the room centers are chosen at locally reachable areas. +4. The action server returns a result, that has the following parts + * segmented_map: The map with the N found segments drawn in. The value of a pixel indicates its ID, which can be used to find pixels belonging to one room. The IDs start at 1 and go up to N, the **return format is 32-Bit single channel**. + * RoomInformation: For each found room the min/max x/y coordinate of points belonging to this room and the center is provided. See ipa_building_msgs/msg/RoomInformation.msg for details. This can be in pixel coordinates or absolute coordinates, depending on the booleans defined above. + * doorway_points: The fifth algorithm (voronoi random field segmentation) is capable of predicting doorways. If this algorithm is chosen, central points on a computed voronoi graph are returned that were labeled as doorways. + +A Client also can change the parameters of the server by using dynamic_reconfigure. This can be done using the dynamic_reconfigure_client.h file that provides a client for this purpose. With client.setConfig("param_name", value) one specific parameter can be changed from the client, see ros/src/room_segmentation_client.cpp for an example. Of course rqt_reconfigure can also be used to change specific parameters. + +The algorithms are implemented in common/src, using the headers in common/include/ipa_room_segmentation. Each algorithm has its own class, so if you want to use it alone you have to include the header for the algorithm you want to use and make a new object. Then you have to execute the segmentMap() function, which segments the map. + +For large maps the algorithms can take a few minutes to complete the segmentation, especially the semantic and voronoi random field segmentation. If you want fast results, the morphological and distance segmentation are the fastest, but they might return not the best results if you have wide open spaces. To see results on different maps, see [http://wiki.ros.org/ipa_room_segmentation](http://wiki.ros.org/ipa_room_segmentation). + +# Available algorithms + +1. Morphological segmentation: Given the map as binary image, a binary erosion is iteratively applied. After each step, the algorithm searches for separate regions that have an area in the defined range. If it finds one, this region is saved and drawn black such that it doesn't need to be looked at any further. After all white regions are black, the algorithm stops and draws the segments in an image, then applies a wavefront algorithm to fill remaining white spaces. + +2. Distance segmentation: Out of the given image a distance-map is computed that stores for each white pixel its distance to the nearest black pixel. On this distance-map a threshold operator is applied iteratively that makes every pixel above a certain threshold black and everything beneath it white. After each step the algorithm searches for segments in the given area range and stores them, then increases the threshold. After all possible thresholds have been looked at, the algorithm chooses the segmentation with the most valid segments as segmentation. After that it also draws the segments in an image and applies a wavefront algorithm. + +3. Voronoi segmentation: For the given map a pruned voronoi graph is computed. The points belonging to this graph are then separated into different neighborhoods and searching in this neighborhood for the point with the smallest distance to the black pixels, called a critical point. This critical point is connected with the two closest black pixels, creating a separation of two segments. This algorithm was programmed after Thrun and Bücken [1]. + +4. Semantic segmentation: For each white pixel a 360 degree laserscan is simulated, resulting in different distances to obstacles around this point. These laserscans are then used to compute different features, which are put into two AdaBoost algorithms. These compute the probability that the current point is a room or hallway. Depending on the probabilities a label is given to that point. The same procedure is then repeated for each white pixel in the given map, resulting in different separated regions for rooms/hallways that are treated as segments. This algorithm was programmed after Mozos [2]. + +5. Voronoi Random Field Segmentation: First a pruned voronoi graph is computed for the given map, on that a neighborhood gets concentrated into one central point. After all points are found, neighboring relations are obtained, meaning which node is next to another depending on the voronoi graph. This gives several cliques, resulting in a conditional random field. The features for this CRF are then the weak responses of the AdaBoost classifiers for rooms, doorways and hallways. These AdaBoost classifiers use the same features as the semantic segmentation. On this CRF then a loopy belief propagation is done, resulting in several intersected segments. This algorithm was programmed after Friedman et al. [3]. + + +# Training + +In this package two algorithms are implemented that use machine-learning techniques, which of course need pre-labeled examples for training. These algorithms are the semantic segmentation and the voronoi random field segmentation. The training procedure starts as soon, as you start the action server. + +**Important**: The algorithms assume that the maps are given as 8-Bit single channel image, with 0 as occupied space and 255 as free space. + +1. The semantic segmentation uses two AdaBoost classifiers, one to detect hallways and one to detect rooms, based on simulated laserscannner-data [2]. +For the training of this algorithm hand-labelled data showing where rooms and where hallways are. To do so see the given files in common/files/training_maps. You can see that for different maps some regions are grey and not white or black. These gray regions show where a hallway/room is, the color itself is not important. +After you have created some training files yourself you have to specify where they lay on your file-system in the room_segmentation_action_server.launch file. When you set the train Boolean to true these given maps are read in and given to the training routine. + +2. The voronoi random field segmentation also uses AdaBoost algorithms to check for rooms/hallways/doorways. Additionally the weak classifier results from these algorithms are used as Features in a conditional random field, resulting in two different algorithms that need to be trained [3]. +Examples for training data for this algorithm can be seen in common/files/training_maps/voronoi_random_field_training. The Data consist of the original maps, with only white and black regions and the labeled maps (training_maps) with colors showing if the class of the current point. Optionally one can also provide additional data to save computation time, the precomputed maps with a voronoi graph drawn in (color 127) and maps that show where nodes are in the given voronoi graph. If you provide one of it you also need to provide the other. +After you created your personal training files you also have to specify the path to them in room_segmentation_action_server.launch. The colors provided in the training maps have the meaning + + - 77: room + - 115: hallway + - 179: doorway + +# Provided test maps + +This package provides a database of 20 floor plans with and without furnitures, used to test the algorithms.Some of these are simulative mapped images, but most are simply drawn using graphics software. The floor maps lie in png format under common/files/test_maps. Relevant are the ones which are existing in a version with furnitures, shown by name extension _furniture. + +Additionally there are ground-truth maps, that show how a human might intersect these maps, shown by the name extension _gt. If you want to color these ground-truth maps, to show the different segments, you can use a flood-fill algorithm for the white pixels to collect each pixel that belong to one segment, because the drawn lines seperates the white spaces from each other. Then you can color each of this found pixel with a color that is unique for this segment. This is done in the computePrecisionRecall() function of the EvaluationSegmentation class (common/src/evaluation_segmentation.cpp) if you want it. + +# References + +[1] Thrun, S., and Bücken, A. Integrating Grid-Based and Topological Maps for Mobile Robot Navigation. In Proceedings of the National Conference on Artificial Intelligence (1996), pp. 944–951. + +[2] Martinez Mozos, O., Rottmann, A., Triebel, R., Jensfelt, P., Burgard, W., et al. Semantic labeling of places using information extracted from laser and vision sensor data (2006) + +[3] Friedman, Stephen, Hanna Pasula, and Dieter Fox. "Voronoi Random Fields: Extracting Topological Structure of Indoor Environments via Place Labeling." IJCAI. Vol. 7. 2007 diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/ros/include/ipa_room_segmentation/dynamic_reconfigure_client.h b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/ros/include/ipa_room_segmentation/dynamic_reconfigure_client.h new file mode 100644 index 0000000..8c9907f --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/ros/include/ipa_room_segmentation/dynamic_reconfigure_client.h @@ -0,0 +1,248 @@ +/*! + ***************************************************************** + * \file + * + * \note + * Copyright (c) 2015 \n + * Fraunhofer Institute for Manufacturing Engineering + * and Automation (IPA) \n\n + * + ***************************************************************** + * + * \note + * Project name: Care-O-bot + * \note + * ROS stack name: autopnp + * \note + * ROS package name: ipa_room_segmentation + * + * \author + * Author: Richard Bormann + * \author + * Supervised by: Richard Bormann + * + * \date Date of creation: 08.2016 + * + * \brief + * + * + ***************************************************************** + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * - Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. \n + * - Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. \n + * - Neither the name of the Fraunhofer Institute for Manufacturing + * Engineering and Automation (IPA) nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. \n + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License LGPL as + * published by the Free Software Foundation, either version 3 of the + * License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License LGPL for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License LGPL along with this program. + * If not, see . + * + ****************************************************************/ + +#ifndef _DYNAMIC_RECONFIGURE_CLIENT_H_ +#define _DYNAMIC_RECONFIGURE_CLIENT_H_ + +#include +#include + +#include + +#include +#include +#include +#include + +class DynamicReconfigureClient +{ +public: + DynamicReconfigureClient(ros::NodeHandle& nh, const std::string& dynamic_reconfigure_service_name, const std::string& parameter_updates_topic) + : dynamic_reconfigure_current_config_received_(false), node_handle_(nh), dynamic_reconfigure_service_name_(dynamic_reconfigure_service_name) + { + dynamic_reconfigure_sub_ = node_handle_.subscribe(parameter_updates_topic, 1, &DynamicReconfigureClient::dynamic_reconfigure_current_config_callback, this); + + // receive current configuration + ros::Duration sleep_rate(0.5); + while (dynamic_reconfigure_current_config_received_ == false) + { + ros::spinOnce(); + sleep_rate.sleep(); + } + } + + dynamic_reconfigure::Config& getConfig() + { + boost::mutex::scoped_lock lock(dynamic_reconfigure_lock_); + return dynamic_reconfigure_config_; + } + + bool setConfig(const std::string& param_name, const bool param_value) + { + boost::mutex::scoped_lock lock(dynamic_reconfigure_lock_); + + if (dynamic_reconfigure_current_config_received_ == false) + { + ROS_WARN("DynamicReconfigureClient: Did not receive the current configuration, yet."); + return false; + } + + bool found = false; + for (size_t i=0; i. + * + ****************************************************************/ + +#include "ros/ros.h" + +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + + +#include +#include +#include + +#include +#include +#include +#include +#include + +class RoomSegmentationServer +{ +protected: + + // parameters + //limits for the room-areas + double room_upper_limit_morphological_, room_upper_limit_distance_, room_upper_limit_voronoi_, room_upper_limit_semantic_, room_upper_limit_voronoi_random_, room_upper_limit_passthrough_; + double room_lower_limit_morphological_, room_lower_limit_distance_, room_lower_limit_voronoi_, room_lower_limit_semantic_, room_lower_limit_voronoi_random_, room_lower_limit_passthrough_; + int room_segmentation_algorithm_; // this variable selects the algorithm for room segmentation, + // 1 = morphological segmentation + // 2 = distance segmentation + // 3 = Voronoi segmentation + // 4 = semantic segmentation + // 5 = voronoi-random-field segmentation + // 99 = pass through segmentation + + bool train_semantic_, train_vrf_; //Boolean to say if the algorithm needs to be trained + bool load_semantic_features_; //Boolean to say if the training of the semantic algorithm should load precomputed features + + int voronoi_neighborhood_index_; //Variable for the Voronoi method that specifies the neighborhood that is looked at for critical Point extraction + int voronoi_random_field_epsilon_for_neighborhood_; //Variable that specifies the neighborhood for the vrf-segmentation. + int max_iterations_; //number of iterations for search of neighborhood in voronoi method and vrf method + int min_neighborhood_size_; //Variable that stores the minimum size of a neighborhood, used for the vrf method. + double min_voronoi_random_field_node_distance_; //Variable that shows how near two nodes of the conditional random field can be in the vrf method. [pixel] + int max_voronoi_random_field_inference_iterations_; //Variable that shows how many iterations should max. be done when infering in the conditional random field. + double min_critical_point_distance_factor_; //Variable that sets the minimal distance between two critical Points before one gets eliminated + double max_area_for_merging_; //Variable that shows the maximal area of a room that should be merged with its surrounding rooms + bool display_segmented_map_; // displays the segmented map upon service call + bool publish_segmented_map_; // publishes the segmented map as grid map upon service call + std::vector doorway_points_; // vector that saves the found doorway points, when using the 5th algorithm (vrf) + + std::vector semantic_training_maps_room_file_list_; // list of files containing maps with room labels for training the semantic segmentation + std::vector semantic_training_maps_hallway_file_list_; // list of files containing maps with hallway labels for training the semantic segmentation + std::vector vrf_original_maps_file_list_; // list of files containing the original maps for training the VRF segmentation + std::vector vrf_training_maps_file_list_; // list of files containing the labeled maps for training the VRF segmentation + std::vector vrf_voronoi_maps_file_list_; // list of files containing the Voronoi maps for training the VRF segmentation - these files are optional for training and just yield a speedup + std::vector vrf_voronoi_node_maps_file_list_; // list of files containing the Voronoi node maps for training the VRF segmentation - these files are optional for training and just yield a speedup + + //converter-> Pixel to meter for X coordinate + double convert_pixel_to_meter_for_x_coordinate(const int pixel_valued_object_x, const float map_resolution, const cv::Point2d map_origin) + { + double meter_value_obj_x = (pixel_valued_object_x * map_resolution) + map_origin.x; + return meter_value_obj_x; + } + //converter-> Pixel to meter for Y coordinate + double convert_pixel_to_meter_for_y_coordinate(int pixel_valued_object_y, const float map_resolution, const cv::Point2d map_origin) + { + double meter_value_obj_y = (pixel_valued_object_y * map_resolution) + map_origin.y; + return meter_value_obj_y; + } + + //This is the execution function used by action server + void execute_segmentation_server(const ipa_building_msgs::MapSegmentationGoalConstPtr &goal); + + //Callback for dynamic reconfigure server + void dynamic_reconfigure_callback(ipa_room_segmentation::RoomSegmentationConfig &config, uint32_t level); + + // service for generating a map of one single room from a labeled map + // The request message provides a segmented map which consists of cells with label 0 for inaccessible areas and other number from 1 to N + // for labeling membership with one of the N segmented areas. + // The return message shall deliver the same map but with only one area (segment_of_interest) labeled as 255 and the remainder labeled + // as inaccessible with 0. + bool extractAreaMapFromLabeledMap(ipa_building_msgs::ExtractAreaMapFromLabeledMapRequest& request, ipa_building_msgs::ExtractAreaMapFromLabeledMapResponse& response); + + //!!Important!! + // define the Nodehandle before the action server, or else the server won't start + // + ros::NodeHandle node_handle_; + ros::Publisher map_pub_; + ros::ServiceServer extract_area_map_from_labeled_map_server_; + actionlib::SimpleActionServer room_segmentation_server_; + dynamic_reconfigure::Server room_segmentation_dynamic_reconfigure_server_; + +public: + //initialize the action-server + RoomSegmentationServer(ros::NodeHandle nh, std::string name_of_the_action); + + //Default destructor for the class + ~RoomSegmentationServer(void) + { + } +}; diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/ros/launch/room_segmentation_action_server.launch b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/ros/launch/room_segmentation_action_server.launch new file mode 100644 index 0000000..6a14b1f --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/ros/launch/room_segmentation_action_server.launch @@ -0,0 +1,70 @@ + + + + + + + + + + + ["$(find ipa_room_segmentation)/common/files/training_maps/lab_ipa_room_training_map.png", +"$(find ipa_room_segmentation)/common/files/training_maps/lab_d_room_training_map.png", +"$(find ipa_room_segmentation)/common/files/training_maps/Freiburg52_scan_room_training.png", +"$(find ipa_room_segmentation)/common/files/training_maps/Freiburg52_scan_furnitures_room_training.png", +"$(find ipa_room_segmentation)/common/files/training_maps/lab_intel_furnitures_room_training_map.png"] + + + ["$(find ipa_room_segmentation)/common/files/training_maps/lab_ipa_hallway_training_map.png", +"$(find ipa_room_segmentation)/common/files/training_maps/lab_a_hallway_training_map.png", +"$(find ipa_room_segmentation)/common/files/training_maps/Freiburg52_scan_hallway_training.png", +"$(find ipa_room_segmentation)/common/files/training_maps/Freiburg52_scan_furnitures_hallway_training.png", +"$(find ipa_room_segmentation)/common/files/training_maps/lab_intel_hallway_training_map.png"] + + + + ["$(find ipa_room_segmentation)/common/files/training_maps/voronoi_random_field_training/original_maps/Fr52_original.png", +"$(find ipa_room_segmentation)/common/files/training_maps/voronoi_random_field_training/original_maps/Fr101_original.png", +"$(find ipa_room_segmentation)/common/files/training_maps/voronoi_random_field_training/original_maps/lab_intel_original.png", +"$(find ipa_room_segmentation)/common/files/training_maps/voronoi_random_field_training/original_maps/lab_d_furnitures_original.png", +"$(find ipa_room_segmentation)/common/files/training_maps/voronoi_random_field_training/original_maps/lab_ipa_original.png", +"$(find ipa_room_segmentation)/common/files/training_maps/voronoi_random_field_training/original_maps/NLB_original.png", +"$(find ipa_room_segmentation)/common/files/training_maps/voronoi_random_field_training/original_maps/office_e_original.png", +"$(find ipa_room_segmentation)/common/files/training_maps/voronoi_random_field_training/original_maps/office_h_original.png", +"$(find ipa_room_segmentation)/common/files/training_maps/voronoi_random_field_training/original_maps/lab_c_furnitures_original.png"] + + + ["$(find ipa_room_segmentation)/common/files/training_maps/voronoi_random_field_training/training_maps/training_Fr52.png", +"$(find ipa_room_segmentation)/common/files/training_maps/voronoi_random_field_training/training_maps/training_Fr101.png", +"$(find ipa_room_segmentation)/common/files/training_maps/voronoi_random_field_training/training_maps/training_intel.png", +"$(find ipa_room_segmentation)/common/files/training_maps/voronoi_random_field_training/training_maps/training_lab_d_furniture.png", +"$(find ipa_room_segmentation)/common/files/training_maps/voronoi_random_field_training/training_maps/training_lab_ipa.png", +"$(find ipa_room_segmentation)/common/files/training_maps/voronoi_random_field_training/training_maps/training_NLB_furniture.png", +"$(find ipa_room_segmentation)/common/files/training_maps/voronoi_random_field_training/training_maps/training_office_e.png", +"$(find ipa_room_segmentation)/common/files/training_maps/voronoi_random_field_training/training_maps/training_office_h.png", +"$(find ipa_room_segmentation)/common/files/training_maps/voronoi_random_field_training/training_maps/training_lab_c_furnitures.png"] + + + ["$(find ipa_room_segmentation)/common/files/training_maps/voronoi_random_field_training/voronoi_maps/Fr52_voronoi.png", +"$(find ipa_room_segmentation)/common/files/training_maps/voronoi_random_field_training/voronoi_maps/Fr101_voronoi.png", +"$(find ipa_room_segmentation)/common/files/training_maps/voronoi_random_field_training/voronoi_maps/lab_intel_voronoi.png", +"$(find ipa_room_segmentation)/common/files/training_maps/voronoi_random_field_training/voronoi_maps/lab_d_furnitures_voronoi.png", +"$(find ipa_room_segmentation)/common/files/training_maps/voronoi_random_field_training/voronoi_maps/lab_ipa_voronoi.png", +"$(find ipa_room_segmentation)/common/files/training_maps/voronoi_random_field_training/voronoi_maps/NLB_voronoi.png", +"$(find ipa_room_segmentation)/common/files/training_maps/voronoi_random_field_training/voronoi_maps/office_e_voronoi.png", +"$(find ipa_room_segmentation)/common/files/training_maps/voronoi_random_field_training/voronoi_maps/office_h_voronoi.png", +"$(find ipa_room_segmentation)/common/files/training_maps/voronoi_random_field_training/voronoi_maps/lab_c_furnitures_voronoi.png"] + + + ["$(find ipa_room_segmentation)/common/files/training_maps/voronoi_random_field_training/voronoi_node_maps/Fr52_voronoi_nodes.png", +"$(find ipa_room_segmentation)/common/files/training_maps/voronoi_random_field_training/voronoi_node_maps/Fr101_voronoi_nodes.png", +"$(find ipa_room_segmentation)/common/files/training_maps/voronoi_random_field_training/voronoi_node_maps/lab_intel_voronoi_nodes.png", +"$(find ipa_room_segmentation)/common/files/training_maps/voronoi_random_field_training/voronoi_node_maps/lab_d_furnitures_voronoi_nodes.png", +"$(find ipa_room_segmentation)/common/files/training_maps/voronoi_random_field_training/voronoi_node_maps/lab_ipa_voronoi_nodes.png", +"$(find ipa_room_segmentation)/common/files/training_maps/voronoi_random_field_training/voronoi_node_maps/NLB_voronoi_nodes.png", +"$(find ipa_room_segmentation)/common/files/training_maps/voronoi_random_field_training/voronoi_node_maps/office_e_voronoi_nodes.png", +"$(find ipa_room_segmentation)/common/files/training_maps/voronoi_random_field_training/voronoi_node_maps/office_h_voronoi_nodes.png", +"$(find ipa_room_segmentation)/common/files/training_maps/voronoi_random_field_training/voronoi_node_maps/lab_c_furnitures_voronoi_nodes.png"] + + + diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/ros/launch/room_segmentation_action_server_params.yaml b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/ros/launch/room_segmentation_action_server_params.yaml new file mode 100644 index 0000000..eac28af --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/ros/launch/room_segmentation_action_server_params.yaml @@ -0,0 +1,61 @@ +# this variable selects the algorithm for room segmentation +# 1 = morphological segmentation +# 2 = distance segmentation +# 3 = Voronoi segmentation +# 4 = semantic segmentation +# 5 = voronoi random field segmentation +# 99 = passthrough segmentation +# int +room_segmentation_algorithm: 3 + +# displays the segmented map upon service call +# bool +display_segmented_map: false + +# publishes the segmented map as grid map upon service call +# bool +publish_segmented_map: true + +# train the semantic segmentation and the voronoi random field segmentation +train_semantic: false +load_semantic_features: false +train_vrf: false + +# room area factor-> Set the limitation of area of the room -------> in [m^2] +#morphological segmentation: 47.0 - 0.8 (means the room area after eroding/shrinking s.t. too small/big contours are not treated as rooms) +room_area_factor_upper_limit_morphological: 47.0 +room_area_factor_lower_limit_morphological: 0.8 + +#distance segmentation: 163.0 - 0.35 (means the room area after decreasing the boundary for the binary filter s.t. too small/big contours are not treated as rooms) +room_area_factor_upper_limit_distance: 163.0 +room_area_factor_lower_limit_distance: 0.35 + +#Voronoi segmentation: 120.0 - 1.53 (means the max/min area that an area separated by critical lines is allowed to have) +room_area_factor_upper_limit_voronoi: 1000000.0 #120.0 +room_area_factor_lower_limit_voronoi: 0.1 #1.53 + +#Semantic Segmentation: 23.0 - 1.0 (means the max/min area a connected classified region is allowed to have) +room_area_factor_upper_limit_semantic: 1000000.0 # if you choose this value small (i.e 23.0) then too big hallway contours are randomly separated into smaller regions using a watershed algorithm, which can look bad +room_area_factor_lower_limit_semantic: 1.0 + +#Voronoi random field segmentation: 1000000.0 - 1.53 (means the max/min area a connected classified region is allowed to have) +room_area_upper_limit_voronoi_random: 1000000.0 +room_area_lower_limit_voronoi_random: 1.53 + +#Passthrough +room_area_factor_upper_limit_passthrough: 1000000.0 #120.0 +room_area_factor_lower_limit_passthrough: 1.0 + +#parameters for the voronoi segmentation that specify the neighborhood for critical Point extraction and the distance between critical Points. +voronoi_neighborhood_index: 280 #larger value sets a larger neighborhood for searching critical points --> int +max_iterations: 150 #sets the maximal number of iterations to search for a neighborhood, also used for the vrf segmentation --> int +min_critical_point_distance_factor: 0.5 #1.6 #minimal distance factor between two critical points before one of it gets eliminated --> double +max_area_for_merging: 12.5 #maximal area [m²] of a room that should be merged with its surrounding rooms, also used for the voronoi random field segmentation + +#parameters for the voronoi random field segmentation that specify the size of the neighborhood generated on the Voronoi graph, the minimal +#size this neighborhood can have, how far base nodes for each node on the graph need to be apart and how many iterations the inference +#max. should do +voronoi_random_field_epsilon_for_neighborhood: 5 #larger value sets larger neighborhood, concentrated in a node of the conditional random field --> int +min_neighborhood_size: 4 #min size of the above mentioned neighborhood --> int +min_voronoi_random_field_node_distance: 7.0 #min distance the base nodes for each crf node need to be apart --> double +max_voronoi_random_field_inference_iterations: 9000 #max number of iterations the inference algorithm should do --> int diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/ros/src/evaluation_numeric_properties.cpp b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/ros/src/evaluation_numeric_properties.cpp new file mode 100644 index 0000000..e96da71 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/ros/src/evaluation_numeric_properties.cpp @@ -0,0 +1,920 @@ +#include "ros/ros.h" +#include +#include +#include + +#include + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +void calculate_mean_min_max(const std::vector& values, double& mean, double& min_val, double& max_val) +{ + mean = 0.0; + max_val = 0.0; + min_val = 1e10; + for (size_t i = 0; i < values.size(); ++i) + { + mean += values[i]; + if (values[i] > max_val) + max_val = values[i]; + if (values[i] < min_val) + min_val = values[i]; + } + mean = mean/(double)values.size(); + +} + +double calculate_stddev(const std::vector& values, const double mean) +{ + //calculate the standard deviation + double sigma = 0.; + for (size_t i=0; i(v,u); + for (int dv=-1; dv<=1; ++dv) + { + for (int du=-1; du<=1; ++du) + { + const int nu = u+du; + const int nv = v+dv; + if (nu>=0 && nu=0 && nv(nv,nu)!=label) + return false; + } + } + } + return true; +} + +void calculate_basic_measures(const cv::Mat& map, const double map_resolution, const int number_rooms, std::vector& areas, std::vector& perimeters, + std::vector& area_perimeter_compactness, std::vector& bb_area_compactness, std::vector& pca_eigenvalue_ratio) +{ + areas.clear(); + areas.resize(number_rooms, 0.); + perimeters.clear(); + perimeters.resize(number_rooms, 0.); + area_perimeter_compactness.clear(); + area_perimeter_compactness.resize(number_rooms, 0.); + bb_area_compactness.clear(); + bb_area_compactness.resize(number_rooms, 0.); + pca_eigenvalue_ratio.clear(); + pca_eigenvalue_ratio.resize(number_rooms, 0.); + + std::vector< std::vector< cv::Point > > room_contours(number_rooms); + std::vector< std::vector< cv::Point > > filled_rooms(number_rooms); + + //const double map_resolution = 0.0500; // m/cell + for(size_t v = 0; v < map.rows; ++v) + { + for(size_t u = 0; u < map.cols; ++u) + { + if(map.at(v,u) != 0) + { + const int insert_index = map.at(v,u)-1; + if (insert_index >= number_rooms) + continue; + areas[insert_index] += map_resolution*map_resolution; + filled_rooms[insert_index].push_back(cv::Point(u,v)); + if (check_inner_pixel(map, u, v) == false) + room_contours[insert_index].push_back(cv::Point(u,v)); + } + } + } + for (size_t r=0; r(i, 0) = filled_rooms[r][i].x; + data.at(i, 1) = filled_rooms[r][i].y; + } + cv::PCA pca(data, cv::Mat(), CV_PCA_DATA_AS_ROW); + pca_eigenvalue_ratio[r] = pca.eigenvalues.at(0)/pca.eigenvalues.at(1); + } +} + +//calculate the compactness of the rooms. Compactness factor is given by area/perimeter +std::vector calculate_compactness(std::vector > rooms, const double map_resolution) +{ + double current_area, current_perimeter; + //double map_resolution = 0.05000; + std::vector compactness_factors; + //calculate the area and perimeter for each room using opencv + for (int current_room = 0; current_room < rooms.size(); current_room++) + { + current_area = map_resolution * map_resolution * cv::contourArea(rooms[current_room]); + current_perimeter = map_resolution * cv::arcLength(rooms[current_room], true); + compactness_factors.push_back(current_area / (current_perimeter * current_perimeter)); + } + return compactness_factors; +} + +//calculate too much area of the bounding box +std::vector calculate_bounding_error(std::vector > rooms, const double map_resolution) +{ + std::vector space_errors; + double bounding_box_area, room_area; + //double map_resolution = 0.05000; + cv::RotatedRect current_bounding_box; + //calculate the rotated bounding box for each room and subtract the roomarea from it + for (int current_room = 0; current_room < rooms.size(); current_room++) { + current_bounding_box = cv::minAreaRect(rooms[current_room]); + bounding_box_area = map_resolution * map_resolution * current_bounding_box.size.area(); + room_area = map_resolution * map_resolution * cv::contourArea(rooms[current_room]); + //put the difference in the error vector + space_errors.push_back(bounding_box_area - room_area); + } + return space_errors; +} + +//calculate area for every room +std::vector calculate_areas(std::vector > rooms, const double map_resolution) +{ + std::vector calculated_areas; + //double map_resolution = 0.0500; + for (int current_room = 0; current_room < rooms.size(); current_room++) + { + calculated_areas.push_back(map_resolution * map_resolution * cv::contourArea(rooms[current_room])); + } + return calculated_areas; +} + +//calculate area for every room +std::vector calculate_areas_from_segmented_map(const cv::Mat& map, const double map_resolution, const int number_rooms) +{ + std::vector calculated_areas(number_rooms, 0.); + //const double map_resolution = 0.0500; // m/cell + for(size_t v = 0; v < map.rows; ++v) + for(size_t u = 0; u < map.cols; ++u) + if(map.at(v,u) != 0) + calculated_areas[map.at(v,u)-1] += map_resolution*map_resolution; + + return calculated_areas; +} + +//calculate perimeter for every room +std::vector calculate_perimeters(std::vector > rooms) +{ + std::vector calculated_perimeters; + double map_resoultion = 0.0500; + for (int current_room = 0; current_room < rooms.size(); current_room++) + { + calculated_perimeters.push_back(map_resoultion * cv::arcLength(rooms[current_room], true)); + } + return calculated_perimeters; +} + +//check if every roomcenter is reachable +bool check_reachability(const std::vector >& rooms, const cv::Mat& map) +{ + bool reachable = true; + cv::RotatedRect current_bounding_box; + for (int current_room = 0; current_room < rooms.size(); current_room++) + { + current_bounding_box = minAreaRect(rooms[current_room]); + if (map.at(current_bounding_box.center) == 0) + { + reachable = false; + } + } + return reachable; +} + +//Calculate the length of the major axis of the minimum bounding ellipse +double calc_major_axis(std::vector room) +{ + cv::Point2f points[4]; + std::vector edge_points; + double distance = 0; + double map_resoultion = 0.05; + //saving-variable for the Points of the ellipse + cv::RotatedRect ellipse = cv::fitEllipse(cv::Mat(room)); + //get the edge-points of the ellipse + ellipse.points(points); + //saving the Points of the ellipse in a vector + for (int i = 0; i < 4; i++) + { + edge_points.push_back(points[i]); + } + //calculate the distance between the Points and take the largest one + for (int p = 0; p < edge_points.size(); p++) + { + for (int np = 0; np < edge_points.size(); np++) + { + if (std::sqrt(std::pow((edge_points[p].x - edge_points[np].x), 2)+ std::pow((edge_points[p].y - edge_points[np].y),2)) > distance) + { + distance = std::sqrt(std::pow((edge_points[p].x - edge_points[np].x), 2) + + std::pow((edge_points[p].y - edge_points[np].y),2)); + } + } + } + return map_resoultion * distance; +} + +//Calculate the length of the minor axis of the minimum bounding ellipse +double calc_minor_axis(std::vector room) { + cv::Point2f points[4]; + std::vector edge_points; + double distance = 10000000; + double map_resoultion = 0.05; + //saving-variable for the Points of the ellipse + cv::RotatedRect ellipse = cv::fitEllipse(cv::Mat(room)); + //get the edge-points of the ellipse + ellipse.points(points); + //saving the Points of the ellipse in a vector + for (int i = 0; i < 4; i++) + { + edge_points.push_back(points[i]); + } + //calculate the distance between the Points and take the largest one + for (int p = 0; p < edge_points.size(); p++) + { + for (int np = 0; np < edge_points.size(); np++) + { + //np != p: make sure the distance is nor calculated to the Point itself + if (std::sqrt(std::pow((edge_points[p].x - edge_points[np].x), 2) + + std::pow((edge_points[p].y - edge_points[np].y),2)) < distance && np != p) + { + distance = std::sqrt(std::pow((edge_points[p].x - edge_points[np].x), 2) + + std::pow((edge_points[p].y - edge_points[np].y),2)); + } + } + } + return map_resoultion * distance; +} + +//Calculate the Quotient of the langths of the major axis and the minor axis from the fitting ellipse for each room +std::vector calc_Ellipse_axis(std::vector > rooms) +{ + std::vector quotients; + for (int current_room = 0; current_room < rooms.size(); current_room++) + { + quotients.push_back(calc_major_axis(rooms[current_room]) / calc_minor_axis(rooms[current_room])); + } + return quotients; +} + +//Calculate the average distance between room-centers +double calc_average_distance(std::vector > rooms) +{ + double mean = 0.0; + double dx, dy; + double map_resoultion = 0.05; + std::vector centers; + cv::RotatedRect current_bounding_box; + for (int current_room = 0; current_room < rooms.size(); current_room++) + { + current_bounding_box = minAreaRect(rooms[current_room]); + centers.push_back(current_bounding_box.center); + } + //calculate the sum of distances + for (int current_center = 0; current_center < centers.size() - 1; current_center++) + { + dx = centers[current_center].x - centers[current_center + 1].x; + dy = centers[current_center].y - centers[current_center + 1].y; + mean += std::sqrt(std::pow(dx, 2.0) + std::pow(dy, 2.0)); + } + return map_resoultion * (mean / centers.size()); +} + +//Calculate standard deviation of room-areas +double calc_area_deviation(std::vector > rooms, const double map_resolution) +{ + double sigma = 0.0; + double mean = 0.0; + std::vector areas = calculate_areas(rooms, map_resolution); + //calculate the average room-area + for (int current_room = 0; current_room < areas.size(); current_room++) { + mean += areas[current_room]; + } + mean = mean / areas.size(); + //calculate the standard deviation + for (int current_room = 0; current_room < areas.size(); current_room++) { + sigma += std::pow(areas[current_room] - mean, 2.0); + } + sigma = sigma / (areas.size() - 1); + return std::sqrt(sigma); +} + +//Calculate standard deviation of room-perimeters +double calc_perimeter_deviation(std::vector > rooms) +{ + double sigma = 0.0; + double mean = 0.0; + std::vector perimeters = calculate_perimeters(rooms); + //calculate the average room-area + for (int current_room = 0; current_room < perimeters.size();current_room++) + { + mean += perimeters[current_room]; + } + mean = mean / perimeters.size(); + //calculate the standard deviation + for (int current_room = 0; current_room < perimeters.size();current_room++) + { + sigma += std::pow(perimeters[current_room] - mean, 2.0); + } + sigma = sigma / (perimeters.size() - 1); + return std::sqrt(sigma); +} + +//Calculate standard deviation of ellipsis-quotients +double calc_quotients_deviation(std::vector > rooms) +{ + double sigma = 0.0; + double mean = 0.0; + std::vector quotients = calc_Ellipse_axis(rooms); + //calculate the average room-area + for (int current_room = 0; current_room < quotients.size(); + current_room++) { + mean += quotients[current_room]; + } + mean = mean / quotients.size(); + //calculate the standard deviation + for (int current_room = 0; current_room < quotients.size(); + current_room++) { + sigma += std::pow(quotients[current_room] - mean, 2.0); + } + sigma = sigma / (quotients.size() - 1); + return std::sqrt(sigma); +} + +//Calculate standard deviation of bounding-box-errors +double calc_errors_deviation(std::vector > rooms, const double map_resolution) +{ + double sigma = 0.0; + double mean = 0.0; + std::vector errors = calculate_bounding_error(rooms, map_resolution); + //calculate the average room-area + for (int current_room = 0; current_room < errors.size(); current_room++) { + mean += errors[current_room]; + } + mean = mean / errors.size(); + //calculate the standard deviation + for (int current_room = 0; current_room < errors.size(); current_room++) { + sigma += std::pow(errors[current_room] - mean, 2.0); + } + sigma = sigma / (errors.size() - 1); + return std::sqrt(sigma); +} + +int segmentationNameToNumber(const std::string name) +{ + if (name.compare("1morphological") == 0) + return 1; + else if (name.compare("2distance") == 0) + return 2; + else if (name.compare("3voronoi") == 0) + return 3; + else if (name.compare("4semantic") == 0) + return 4; + else if (name.compare("5vrf") == 0) + return 5; + return 1; +} + + +int main(int argc, char **argv) { + ros::init(argc, argv, "evaluation"); + ros::NodeHandle n; +// ros::Subscriber semantic_labeler = n.Subscribe("Laser_scanner", 1000, segmentation_algorithm); + ROS_INFO("Evaluation of the segmented maps. Calculates some Parameters describing the rooms."); +// ros::spin(); + + double map_resolution = 0.05; + + std::vector segmentation_names; + segmentation_names.push_back("1morphological"); + segmentation_names.push_back("2distance"); + segmentation_names.push_back("3voronoi"); + segmentation_names.push_back("4semantic"); + segmentation_names.push_back("5vrf"); + +// std::string map_name = "NLB"; +//// "lab_ipa" //done +//// "lab_c_scan" //done +//// "Freiburg52_scan" //done +//// "Freiburg79_scan" //done +//// "lab_b_scan" //done +//// "lab_intel" //done +//// "Freiburg101_scan" //done +//// "lab_d_scan" //done +//// "lab_f_scan" //done +//// "lab_a_scan" //done +//// "NLB" //done + std::vector< std::string > map_names; + map_names.push_back("lab_ipa"); + map_names.push_back("lab_c_scan"); + map_names.push_back("Freiburg52_scan"); + map_names.push_back("Freiburg79_scan"); + map_names.push_back("lab_b_scan"); + map_names.push_back("lab_intel"); + map_names.push_back("Freiburg101_scan"); + map_names.push_back("lab_d_scan"); + map_names.push_back("lab_f_scan"); + map_names.push_back("lab_a_scan"); + map_names.push_back("NLB"); + map_names.push_back("office_a"); + map_names.push_back("office_b"); + map_names.push_back("office_c"); + map_names.push_back("office_d"); + map_names.push_back("office_e"); + map_names.push_back("office_f"); + map_names.push_back("office_g"); + map_names.push_back("office_h"); + map_names.push_back("office_i"); + map_names.push_back("lab_ipa_furnitures"); + map_names.push_back("lab_c_scan_furnitures"); + map_names.push_back("Freiburg52_scan_furnitures"); + map_names.push_back("Freiburg79_scan_furnitures"); + map_names.push_back("lab_b_scan_furnitures"); + map_names.push_back("lab_intel_furnitures"); + map_names.push_back("Freiburg101_scan_furnitures"); + map_names.push_back("lab_d_scan_furnitures"); + map_names.push_back("lab_f_scan_furnitures"); + map_names.push_back("lab_a_scan_furnitures"); + map_names.push_back("NLB_furnitures"); + map_names.push_back("office_a_furnitures"); + map_names.push_back("office_b_furnitures"); + map_names.push_back("office_c_furnitures"); + map_names.push_back("office_d_furnitures"); + map_names.push_back("office_e_furnitures"); + map_names.push_back("office_f_furnitures"); + map_names.push_back("office_g_furnitures"); + map_names.push_back("office_h_furnitures"); + map_names.push_back("office_i_furnitures"); + + const std::string segmented_map_path = "room_segmentation/"; //ros::package::getPath("ipa_room_segmentation") + "/common/files/segmented_maps/"; + const std::string command = "mkdir -p " + segmented_map_path; + int return_value = system(command.c_str()); + + // matrices to store results + // evaluation criteria are stored row-wise, i.e. each row stores a criterion + // - algorithm runtime in seconds [row 0] + // - number segments [row 1] + // - segment area (mean, min/max, std) [rows 2-5] + // - segment perimeter (mean, min/max, std) [rows 6-9] + // - area/perimeter compactness (mean, min/max, std) [rows 10-13] + // - area/bounding box compactness (mean, min/max, std) [rows 14-17] + // - spherical/ellipsoid measure (mean, min/max, std) [rows 18-21] + // - fit with giving ground truth (average of recalls, average recall, average of precisions, average precision) [rows 22-25] + std::vector results(segmentation_names.size()); + for (size_t i=0; i segments_number_vector(segmentation_names.size()); + std::vector av_area_vector(segmentation_names.size()), max_area_vector(segmentation_names.size()), min_area_vector(segmentation_names.size()), dev_area_vector(segmentation_names.size()); + std::vector av_per_vector(segmentation_names.size()), max_per_vector(segmentation_names.size()), min_per_vector(segmentation_names.size()), dev_per_vector(segmentation_names.size()); + std::vector av_compactness_vector(segmentation_names.size()), max_compactness_vector(segmentation_names.size()), min_compactness_vector(segmentation_names.size()), dev_compactness_vector(segmentation_names.size()); + std::vector av_bb_vector(segmentation_names.size()), max_bb_vector(segmentation_names.size()), min_bb_vector(segmentation_names.size()), dev_bb_vector(segmentation_names.size()); + std::vector av_quo_vector(segmentation_names.size()), max_quo_vector(segmentation_names.size()), min_quo_vector(segmentation_names.size()), dev_quo_vector(segmentation_names.size()); + std::vector reachable(segmentation_names.size()); + + //load map + std::string map_name = map_names[image_index]; + std::string image_filename = ros::package::getPath("ipa_room_segmentation") + "/common/files/test_maps/" + map_name + ".png"; + std::cout << "map: " << image_filename << std::endl; + cv::Mat map = cv::imread(image_filename.c_str(), 0); + //make non-white pixels black + for (int y = 0; y < map.rows; y++) + { + for (int x = 0; x < map.cols; x++) + { + //find not reachable regions and make them black + if (map.at(y, x) >= 250) + map.at(y, x) = 255; + else + map.at(y, x) = 0; + } + } + + //calculate parameters for each segmentation and save it + for (size_t segmentation_index = 0; segmentation_index < segmentation_names.size();++segmentation_index) + { + std::cout << "Evaluating image '" << map_name << "' with segmentation method " << segmentation_names[segmentation_index] << std::endl; + + // do the segmentation + // =================== + sensor_msgs::Image labeling; + cv_bridge::CvImage cv_image; + // cv_image.header.stamp = ros::Time::now(); + cv_image.encoding = "mono8"; + cv_image.image = map; + cv_image.toImageMsg(labeling); + // create the action client --> "name of server" + // true causes the client to spin its own thread + actionlib::SimpleActionClient ac("room_segmentation_server", true); + + ROS_INFO("Waiting for action server to start."); + // wait for the action server to start + ac.waitForServer(); //will wait for infinite time + ROS_INFO("Action server started, sending goal."); + + // send dynamic reconfigure config + DynamicReconfigureClient drc(n, "room_segmentation_server/set_parameters", "room_segmentation_server/parameter_updates"); + const int room_segmentation_algorithm = segmentationNameToNumber(segmentation_names[segmentation_index]); + drc.setConfig("room_segmentation_algorithm", room_segmentation_algorithm); + if(room_segmentation_algorithm == 1) //morpho + { + drc.setConfig("room_area_factor_lower_limit_morphological", 0.8); + drc.setConfig("room_area_factor_upper_limit_morphological", 47.0); + ROS_INFO("You have chosen the morphological segmentation."); + } + if(room_segmentation_algorithm == 2) //distance + { + drc.setConfig("room_area_factor_lower_limit_distance", 0.35); + drc.setConfig("room_area_factor_upper_limit_distance", 163.0); + ROS_INFO("You have chosen the distance segmentation."); + } + if(room_segmentation_algorithm == 3) //voronoi + { + drc.setConfig("room_area_factor_lower_limit_voronoi", 0.1); //1.53; + drc.setConfig("room_area_factor_upper_limit_voronoi", 1000000.); //120.0; + drc.setConfig("voronoi_neighborhood_index", 280); + drc.setConfig("max_iterations", 150); + drc.setConfig("min_critical_point_distance_factor", 0.5); //1.6; + drc.setConfig("max_area_for_merging", 12.5); + ROS_INFO("You have chosen the Voronoi segmentation"); + } + if(room_segmentation_algorithm == 4) //semantic + { + drc.setConfig("room_area_factor_lower_limit_semantic", 1.0); + drc.setConfig("room_area_factor_upper_limit_semantic", 1000000.);//23.0; + ROS_INFO("You have chosen the semantic segmentation."); + } + if(room_segmentation_algorithm == 5) //voronoi random field + { + drc.setConfig("room_area_lower_limit_voronoi_random", 1.53); //1.53 + drc.setConfig("room_area_upper_limit_voronoi_random", 1000000.); //1000000.0 + drc.setConfig("max_iterations", 150); + drc.setConfig("voronoi_random_field_epsilon_for_neighborhood", 7); + drc.setConfig("min_neighborhood_size", 5); + drc.setConfig("min_voronoi_random_field_node_distance", 7.0); // [pixel] + drc.setConfig("max_voronoi_random_field_inference_iterations", 9000); + drc.setConfig("max_area_for_merging", 12.5); + ROS_INFO("You have chosen the Voronoi random field segmentation."); + } + + // send a goal to the action + ipa_building_msgs::MapSegmentationGoal goal; + goal.input_map = labeling; + goal.map_origin.position.x = 0; + goal.map_origin.position.y = 0; + goal.map_resolution = map_resolution; + goal.return_format_in_meter = false; + goal.return_format_in_pixel = true; + //goal.room_segmentation_algorithm = segmentationNameToNumber(segmentation_names[segmentation_index]); + goal.robot_radius = 0.3; + Timer tim; + ac.sendGoal(goal); + + //wait for the action to return + bool finished_before_timeout = ac.waitForResult(); + if (!finished_before_timeout) + continue; + + double runtime = tim.getElapsedTimeInSec(); + ROS_INFO("Finished successfully!"); + + // retrieve segmentation + ipa_building_msgs::MapSegmentationResultConstPtr result = ac.getResult(); + std::cout << "number of found doorways: " << result->doorway_points.size() << std::endl; + cv_bridge::CvImagePtr cv_ptr_seq = cv_bridge::toCvCopy(result->segmented_map, sensor_msgs::image_encodings::TYPE_32SC1); + cv::Mat segmented_map = cv_ptr_seq->image; + + // generate colored segmented_map + cv::Mat color_segmented_map; + segmented_map.convertTo(color_segmented_map, CV_8U); + cv::cvtColor(color_segmented_map, color_segmented_map, CV_GRAY2BGR); + for(size_t i = 1; i <= result->room_information_in_pixel.size(); ++i) + { + //choose random color for each room + const cv::Vec3b color((rand() % 250) + 1, (rand() % 250) + 1, (rand() % 250) + 1); + for(size_t v = 0; v < segmented_map.rows; ++v) + for(size_t u = 0; u < segmented_map.cols; ++u) + if(segmented_map.at(v,u) == i) + color_segmented_map.at(v,u) = color; + } + std::string image_filename = segmented_map_path + map_name + "_segmented_" + segmentation_names[segmentation_index] + ".png"; + cv::imwrite(image_filename, color_segmented_map); + + + // evaluation: numeric properties + // ============================== + std::vector areas; + std::vector perimeters; + std::vector area_perimeter_compactness; + std::vector bb_area_compactness; + std::vector pca_eigenvalue_ratio; + calculate_basic_measures(segmented_map, map_resolution, (int)result->room_information_in_pixel.size(), areas, perimeters, area_perimeter_compactness, bb_area_compactness, pca_eigenvalue_ratio); + + // runtime + results[segmentation_index].at(0, image_index) = runtime; + + // number of segments + results[segmentation_index].at(1, image_index) = segments_number_vector[segmentation_index] = areas.size(); + + // area + //std::vector areas = calculate_areas_from_segmented_map(segmented_map, (int)result->room_information_in_pixel.size()); + double average = 0.0; + double max_area = 0.0; + double min_area = 100000000; + calculate_mean_min_max(areas, average, min_area, max_area); + results[segmentation_index].at(2, image_index) = av_area_vector[segmentation_index] = average; + results[segmentation_index].at(3, image_index) = max_area_vector[segmentation_index] = max_area; + results[segmentation_index].at(4, image_index) = min_area_vector[segmentation_index] = min_area; + results[segmentation_index].at(5, image_index) = dev_area_vector[segmentation_index] = calculate_stddev(areas, average); + + // perimeters + //std::vector perimeters = calculate_perimeters(saved_contours); + average = 0.0; + double max_per = 0.0; + double min_per = 100000000; + calculate_mean_min_max(perimeters, average, min_per, max_per); + results[segmentation_index].at(6, image_index) = av_per_vector[segmentation_index] = average; + results[segmentation_index].at(7, image_index) = max_per_vector[segmentation_index] = max_per; + results[segmentation_index].at(8, image_index) = min_per_vector[segmentation_index] = min_per; + results[segmentation_index].at(9, image_index) = dev_per_vector[segmentation_index] = calculate_stddev(perimeters, average); + + // area compactness + //std::vector area_perimeter_compactness = calculate_compactness(saved_contours); + average = 0.0; + double max_compactness = 0; + double min_compactness = 100000000; + calculate_mean_min_max(area_perimeter_compactness, average, min_compactness, max_compactness); + results[segmentation_index].at(10, image_index) = av_compactness_vector[segmentation_index] = average; + results[segmentation_index].at(11, image_index) = max_compactness_vector[segmentation_index] = max_compactness; + results[segmentation_index].at(12, image_index) = min_compactness_vector[segmentation_index] = min_compactness; + results[segmentation_index].at(13, image_index) = dev_compactness_vector[segmentation_index] = calculate_stddev(area_perimeter_compactness, average); + + // bounding box + //std::vector bb_area_compactness = calculate_bounding_error(saved_contours); + average = 0.0; + double max_error = 0; + double min_error = 10000000; + calculate_mean_min_max(bb_area_compactness, average, min_error, max_error); + results[segmentation_index].at(14, image_index) = av_bb_vector[segmentation_index] = average; + results[segmentation_index].at(15, image_index) = max_bb_vector[segmentation_index] = max_error; + results[segmentation_index].at(16, image_index) = min_bb_vector[segmentation_index] = min_error; + results[segmentation_index].at(17, image_index) = dev_bb_vector[segmentation_index] = calculate_stddev(bb_area_compactness, average); + + // quotient + //std::vector pca_eigenvalue_ratio = calc_Ellipse_axis(saved_contours); + average = 0.0; + double max_quo = 0.0; + double min_quo = 100000000; + calculate_mean_min_max(pca_eigenvalue_ratio, average, min_quo, max_quo); + results[segmentation_index].at(18, image_index) = av_quo_vector[segmentation_index] = average; + results[segmentation_index].at(19, image_index) = max_quo_vector[segmentation_index] = max_quo; + results[segmentation_index].at(20, image_index) = min_quo_vector[segmentation_index] = min_quo; + results[segmentation_index].at(21, image_index) = dev_quo_vector[segmentation_index] = calculate_stddev(pca_eigenvalue_ratio, average); + +// // retrieve room contours +// cv::Mat temporary_map = segmented_map.clone(); +// std::vector > contours, saved_contours; +// std::vector hierarchy; +// for(size_t i = 1; i <= result->room_information_in_pixel.size(); ++i) +// { +// cv::Mat single_room_map = cv::Mat::zeros(segmented_map.rows, segmented_map.cols, CV_8UC1); +// for(size_t v = 0; v < segmented_map.rows; ++v) +// for(size_t u = 0; u < segmented_map.cols; ++u) +// if(segmented_map.at(v,u) == i) +// single_room_map.at(v,u) = 255; +// cv::findContours(single_room_map, contours, hierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_NONE); +// cv::drawContours(temporary_map, contours, -1, cv::Scalar(0), CV_FILLED); +// for (int c = 0; c < contours.size(); c++) +// { +// if (map_resolution * map_resolution * cv::contourArea(contours[c]) > 1.0) +// { +// saved_contours.push_back(contours[c]); +// } +// } +// } +// // reachability +// if (check_reachability(saved_contours, segmented_map)) +// { +// reachable[segmentation_index] = true; +// } +// else +// { +// reachable[segmentation_index] = false; +// } + + std::cout << "Basic measures computed." << std::endl; + + // evaluation: against ground truth segmentation + // ============================================= + // load ground truth segmentation (just borders painted in between of rooms/areas, not colored yet --> coloring will be done here) + std::string map_name_basic = map_name; + std::size_t pos = map_name.find("_furnitures"); + if (pos != std::string::npos) + map_name_basic = map_name.substr(0, pos); + std::string gt_image_filename = ros::package::getPath("ipa_room_segmentation") + "/common/files/test_maps/" + map_name_basic + "_gt_segmentation.png"; + std::cout << "Loading ground truth segmentation from: " << gt_image_filename << std::endl; + cv::Mat gt_map = cv::imread(gt_image_filename.c_str(),CV_8U); + + // compute recall and precision, store colored gt segmentation + double precision_micro, precision_macro, recall_micro, recall_macro; + cv::Mat gt_map_color; + EvaluationSegmentation es; + es.computePrecisionRecall(gt_map, gt_map_color, segmented_map, precision_micro, precision_macro, recall_micro, recall_macro, true); + std::string gt_image_filename_color = segmented_map_path + map_name + "_gt_color_segmentation.png"; //ros::package::getPath("ipa_room_segmentation") + "/common/files/test_maps/" + map_name + "_gt_color_segmentation.png"; + cv::imwrite(gt_image_filename_color.c_str(), gt_map_color); + + results[segmentation_index].at(22, image_index) = recall_micro; + results[segmentation_index].at(23, image_index) = recall_macro; + results[segmentation_index].at(24, image_index) = precision_micro; + results[segmentation_index].at(25, image_index) = precision_macro; + } + + //write parameters into file + std::stringstream output; + output << "--------------Segmentierungsevaluierung----------------" << std::endl; + for(size_t i = 0; i < segmentation_names.size(); ++i) + output << segmentation_names[i] << " & "; + output << std::endl; + output << "Kompaktheitsmaße: " << std::endl; + output << "Durschnitt: "; + for(size_t i = 0; i < segmentation_names.size(); ++i) + output << av_compactness_vector[i] << " & "; + output << std::endl; + output << "Maximum: "; + for(size_t i = 0; i < segmentation_names.size(); ++i) + output << max_compactness_vector[i] << " & "; + output << std::endl; + output << "Minimum: "; + for(size_t i = 0; i < segmentation_names.size(); ++i) + output << min_compactness_vector[i] << " & "; + output << std::endl; + output << "****************************" << std::endl; + + output << "Überflüssige Fläche Bounding Box: " << std::endl; + output << "Durchschnitt Bounding Fehler: "; + for(size_t i = 0; i < segmentation_names.size(); ++i) + output << av_bb_vector[i] << " & "; + output << std::endl; + output << "Maximum: "; + for(size_t i = 0; i < segmentation_names.size(); ++i) + output << max_bb_vector[i] << " & "; + output << std::endl; + output << "Minimum: "; + for(size_t i = 0; i < segmentation_names.size(); ++i) + output << min_bb_vector[i] << " & "; + output << std::endl; + output << "Standardabweichung: "; + for(size_t i = 0; i < segmentation_names.size(); ++i) + output << dev_bb_vector[i] << " & "; + output << std::endl; + output << "**************************************" << std::endl; + + output << "Flächenmaße: " << std::endl; + output << "Durchschnitt: "; + for(size_t i = 0; i < segmentation_names.size(); ++i) + output << av_area_vector[i] << " & "; + output << std::endl; + output << "Maximum: "; + for(size_t i = 0; i < segmentation_names.size(); ++i) + output << max_area_vector[i] << " & "; + output << std::endl; + output << "Minimum: "; + for(size_t i = 0; i < segmentation_names.size(); ++i) + output << min_area_vector[i] << " & "; + output << std::endl; + output << "Standardabweichung: "; + for(size_t i = 0; i < segmentation_names.size(); ++i) + output << dev_area_vector[i] << " & "; + output << std::endl; + output << "**************************************" << std::endl; + + output << "Umfangsmaße: " << std::endl; + output << "Durchschnitt: "; + for(size_t i = 0; i < segmentation_names.size(); ++i) + output << av_per_vector[i] << " & "; + output << std::endl; + output << "Maximum: "; + for(size_t i = 0; i < segmentation_names.size(); ++i) + output << max_per_vector[i] << " & "; + output << std::endl; + output << "Minimum: "; + for(size_t i = 0; i < segmentation_names.size(); ++i) + output << min_per_vector[i] << " & "; + output << std::endl; + output << "Standardabweichung: "; + for(size_t i = 0; i < segmentation_names.size(); ++i) + output << dev_per_vector[i] << " & "; + output << std::endl; + output << "**************************************" << std::endl; + +// output << "Erreichbarkeit für alle Raumzentren: " << std::endl; +// for(size_t i = 0; i < segmentation_names.size(); ++i) +// { +// if(reachable[i] == true) +// output << "Alle Raumzentren erreichbar" << std::endl; +// else +// output << "Nicht alle erreichbar" << std::endl; +// } +// output << "****************************" << std::endl; + + output << "Quotienten der Ellipsenachsen: " << std::endl; + output << "Durchschnitt: "; + for(size_t i = 0; i < segmentation_names.size(); ++i) + output << av_quo_vector[i] << " & "; + output << std::endl; + output << "Maximum: "; + for(size_t i = 0; i < segmentation_names.size(); ++i) + output << max_quo_vector[i] << " & "; + output << std::endl; + output << "Minimum: "; + for(size_t i = 0; i < segmentation_names.size(); ++i) + output << min_quo_vector[i] << " & "; + output << std::endl; + output << "Standardabweichung: "; + for(size_t i = 0; i < segmentation_names.size(); ++i) + output << dev_quo_vector[i] << " & "; + output << std::endl; + output << "**************************************" << std::endl; + + output << "Anzahl Räume: "; + for(size_t i = 0; i < segmentation_names.size(); ++i) + output << segments_number_vector[i] << " & "; + output << std::endl; + output << "**************************************" << std::endl; + + output << "Recall/Precision: " << std::endl; + output << "recall_micro: "; + for(size_t i = 0; i < results.size(); ++i) + output << results[i].at(22, image_index) << " & "; + output << std::endl; + output << "recall_macro: "; + for(size_t i = 0; i < results.size(); ++i) + output << results[i].at(23, image_index) << " & "; + output << std::endl; + output << "precision_micro: "; + for(size_t i = 0; i < results.size(); ++i) + output << results[i].at(24, image_index) << " & "; + output << std::endl; + output << "precision_macro: "; + for(size_t i = 0; i < results.size(); ++i) + output << results[i].at(25, image_index) << " & "; + output << std::endl; + + std::string log_filename = segmented_map_path + map_name + "_evaluation.txt"; + std::ofstream file(log_filename.c_str(), std::ios::out); + if (file.is_open() == true) + file << output.str(); + file.close(); + + // write results summary to file (overwrite in each cycle in order to avoid loosing all data on crash) + for (size_t segmentation_index=0; segmentation_index(r,c) << "\t"; + file << std::endl; + } + } + file.close(); + } + } + + return 0; +} diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/ros/src/room_segmentation_client.cpp b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/ros/src/room_segmentation_client.cpp new file mode 100644 index 0000000..1a1450a --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/ros/src/room_segmentation_client.cpp @@ -0,0 +1,235 @@ +/*! + ***************************************************************** + * \file + * + * \note + * Copyright (c) 2015 \n + * Fraunhofer Institute for Manufacturing Engineering + * and Automation (IPA) \n\n + * + ***************************************************************** + * + * \note + * Project name: Care-O-bot + * \note + * ROS stack name: autopnp + * \note + * ROS package name: ipa_room_segmentation + * + * \author + * Author: Florian Jordan + * \author + * Supervised by: Richard Bormann + * + * \date Date of creation: 08.2015 + * + * \brief + * + * + ***************************************************************** + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * - Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. \n + * - Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. \n + * - Neither the name of the Fraunhofer Institute for Manufacturing + * Engineering and Automation (IPA) nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. \n + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License LGPL as + * published by the Free Software Foundation, either version 3 of the + * License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License LGPL for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License LGPL along with this program. + * If not, see . + * + ****************************************************************/ + +#include +#include + +#include +#include + +#include +#include + +#include + +#include +#include +#include + +//#include +#include + + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "room_segmentation_client"); + ros::NodeHandle nh; + + // map names + std::vector< std::string > map_names; + map_names.push_back("lab_ipa"); + map_names.push_back("lab_c_scan"); + map_names.push_back("Freiburg52_scan"); + map_names.push_back("Freiburg79_scan"); + map_names.push_back("lab_b_scan"); + map_names.push_back("lab_intel"); + map_names.push_back("Freiburg101_scan"); + map_names.push_back("lab_d_scan"); + map_names.push_back("lab_f_scan"); + map_names.push_back("lab_a_scan"); + map_names.push_back("NLB"); + map_names.push_back("office_a"); + map_names.push_back("office_b"); + map_names.push_back("office_c"); + map_names.push_back("office_d"); + map_names.push_back("office_e"); + map_names.push_back("office_f"); + map_names.push_back("office_g"); + map_names.push_back("office_h"); + map_names.push_back("office_i"); + map_names.push_back("lab_ipa_furnitures"); + map_names.push_back("lab_c_scan_furnitures"); + map_names.push_back("Freiburg52_scan_furnitures"); + map_names.push_back("Freiburg79_scan_furnitures"); + map_names.push_back("lab_b_scan_furnitures"); + map_names.push_back("lab_intel_furnitures"); + map_names.push_back("Freiburg101_scan_furnitures"); + map_names.push_back("lab_d_scan_furnitures"); + map_names.push_back("lab_f_scan_furnitures"); + map_names.push_back("lab_a_scan_furnitures"); + map_names.push_back("NLB_furnitures"); + map_names.push_back("office_a_furnitures"); + map_names.push_back("office_b_furnitures"); + map_names.push_back("office_c_furnitures"); + map_names.push_back("office_d_furnitures"); + map_names.push_back("office_e_furnitures"); + map_names.push_back("office_f_furnitures"); + map_names.push_back("office_g_furnitures"); + map_names.push_back("office_h_furnitures"); + map_names.push_back("office_i_furnitures"); + + for (size_t image_index = 0; image_index(y, x) < 250) + { + map.at(y, x) = 0; + } + //else make it white + else + { + map.at(y, x) = 255; + } + } + } +// cv::imshow("map", map); +// cv::waitKey(); + sensor_msgs::Image labeling; + cv_bridge::CvImage cv_image; + // cv_image.header.stamp = ros::Time::now(); + cv_image.encoding = "mono8"; + cv_image.image = map; + cv_image.toImageMsg(labeling); + + // create the action client --> "name of server" + // true causes the client to spin its own thread + actionlib::SimpleActionClient ac("room_segmentation_server", true); + ROS_INFO("Waiting for action server to start."); + // wait for the action server to start + ac.waitForServer(); //will wait for infinite time + ROS_INFO("Action server started, sending goal."); + + // test dynamic reconfigure + DynamicReconfigureClient drc(nh, "room_segmentation_server/set_parameters", "room_segmentation_server/parameter_updates"); + drc.setConfig("room_segmentation_algorithm", 5); +// drc.setConfig("display_segmented_map", true); + //drc.setConfig("room_area_factor_upper_limit_voronoi", 120.0); + + // send a goal to the action + ipa_building_msgs::MapSegmentationGoal goal; + goal.input_map = labeling; + goal.map_origin.position.x = 0; + goal.map_origin.position.y = 0; + goal.map_resolution = 0.05; + goal.return_format_in_meter = false; + goal.return_format_in_pixel = true; + goal.robot_radius = 0.4; + ac.sendGoal(goal); + + //wait for the action to return + bool finished_before_timeout = ac.waitForResult(ros::Duration()); + + if (finished_before_timeout) + { + ROS_INFO("Finished successfully!"); + ipa_building_msgs::MapSegmentationResultConstPtr result_seg = ac.getResult(); + + // display + cv_bridge::CvImagePtr cv_ptr_obj; + cv_ptr_obj = cv_bridge::toCvCopy(result_seg->segmented_map, sensor_msgs::image_encodings::TYPE_32SC1); + cv::Mat segmented_map = cv_ptr_obj->image; + cv::Mat colour_segmented_map = segmented_map.clone(); + colour_segmented_map.convertTo(colour_segmented_map, CV_8U); + cv::cvtColor(colour_segmented_map, colour_segmented_map, CV_GRAY2BGR); + for(size_t i = 1; i <= result_seg->room_information_in_pixel.size(); ++i) + { + //choose random color for each room + int blue = (rand() % 250) + 1; + int green = (rand() % 250) + 1; + int red = (rand() % 250) + 1; + for(size_t u = 0; u < segmented_map.rows; ++u) + { + for(size_t v = 0; v < segmented_map.cols; ++v) + { + if(segmented_map.at(u,v) == i) + { + colour_segmented_map.at(u,v)[0] = blue; + colour_segmented_map.at(u,v)[1] = green; + colour_segmented_map.at(u,v)[2] = red; + } + } + } + } + //draw the room centers into the map + for(size_t i = 0; i < result_seg->room_information_in_pixel.size(); ++i) + { + cv::Point current_center (result_seg->room_information_in_pixel[i].room_center.x, result_seg->room_information_in_pixel[i].room_center.y); +#if CV_MAJOR_VERSION<=3 + cv::circle(colour_segmented_map, current_center, 2, CV_RGB(0,0,255), CV_FILLED); +#else + cv::circle(colour_segmented_map, current_center, 2, CV_RGB(0,0,255), cv::FILLED); +#endif + } + + cv::imshow("segmentation", colour_segmented_map); + cv::waitKey(); + } + } + + //exit + return 0; +} diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/ros/src/room_segmentation_server.cpp b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/ros/src/room_segmentation_server.cpp new file mode 100644 index 0000000..8dbe04d --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/ros/src/room_segmentation_server.cpp @@ -0,0 +1,895 @@ +/*! + ***************************************************************** + * \file + * + * \note + * Copyright (c) 2015 \n + * Fraunhofer Institute for Manufacturing Engineering + * and Automation (IPA) \n\n + * + ***************************************************************** + * + * \note + * Project name: Care-O-bot + * \note + * ROS stack name: autopnp + * \note + * ROS package name: ipa_room_segmentation + * + * \author + * Author: Florian Jordan + * \author + * Supervised by: Richard Bormann + * + * \date Date of creation: 08.2015 + * + * \brief + * + * + ***************************************************************** + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * - Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. \n + * - Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. \n + * - Neither the name of the Fraunhofer Institute for Manufacturing + * Engineering and Automation (IPA) nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. \n + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License LGPL as + * published by the Free Software Foundation, either version 3 of the + * License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License LGPL for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License LGPL along with this program. + * If not, see . + * + ****************************************************************/ + +#include + +#include +#include +#include + +#include + +static bool DEBUG_DISPLAYS=false; + +RoomSegmentationServer::RoomSegmentationServer(ros::NodeHandle nh, std::string name_of_the_action) : + node_handle_(nh), + room_segmentation_server_(node_handle_, name_of_the_action, boost::bind(&RoomSegmentationServer::execute_segmentation_server, this, _1), false) +{ + // parameters to check if the algorithms need to be trained (not part of dynamic reconfigure) + node_handle_.param("train_semantic", train_semantic_, false); + std::cout << "room_segmentation/train_semantic_ = " << train_semantic_ << std::endl; + node_handle_.param("load_semantic_features", load_semantic_features_, false); + std::cout << "room_segmentation/load_semantic_features = " << load_semantic_features_ << std::endl; + node_handle_.param("train_vrf", train_vrf_, false); + std::cout << "room_segmentation/train_vrf_ = " << train_vrf_ << std::endl; + + // dynamic reconfigure + room_segmentation_dynamic_reconfigure_server_.setCallback(boost::bind(&RoomSegmentationServer::dynamic_reconfigure_callback, this, _1, _2)); + + // parameters + std::cout << "\n------------------------------------\nRoom Segmentation Parameters:\n------------------------------------\n"; + node_handle_.param("room_segmentation_algorithm", room_segmentation_algorithm_, 3); + std::cout << "room_segmentation/room_segmentation_algorithm = " << room_segmentation_algorithm_ << std::endl << std::endl; + if (room_segmentation_algorithm_ == 1) + ROS_INFO("You have chosen the morphological segmentation method."); + else if (room_segmentation_algorithm_ == 2) + ROS_INFO("You have chosen the distance segmentation method."); + else if (room_segmentation_algorithm_ == 3) + ROS_INFO("You have chosen the voronoi segmentation method."); + else if (room_segmentation_algorithm_ == 4) + ROS_INFO("You have chosen the semantic segmentation method."); + else if (room_segmentation_algorithm_ == 5) + ROS_INFO("You have chosen the voronoi random field segmentation method."); + std::cout << std::endl; + + //if (room_segmentation_algorithm_ == 1) //set morphological parameters + { + node_handle_.param("room_area_factor_upper_limit_morphological", room_upper_limit_morphological_, 47.0); + std::cout << "room_segmentation/room_area_factor_upper_limit = " << room_upper_limit_morphological_ << std::endl; + node_handle_.param("room_area_factor_lower_limit_morphological", room_lower_limit_morphological_, 0.8); + std::cout << "room_segmentation/room_area_factor_lower_limit = " << room_lower_limit_morphological_ << std::endl; + } + //if (room_segmentation_algorithm_ == 2) //set distance parameters + { + node_handle_.param("room_area_factor_upper_limit_distance", room_upper_limit_distance_, 163.0); + std::cout << "room_segmentation/room_area_factor_upper_limit = " << room_upper_limit_distance_ << std::endl; + node_handle_.param("room_area_factor_lower_limit_distance", room_lower_limit_distance_, 0.35); + std::cout << "room_segmentation/room_area_factor_lower_limit = " << room_lower_limit_distance_ << std::endl; + } + //if (room_segmentation_algorithm_ == 3) //set voronoi parameters + { + node_handle_.param("room_area_factor_upper_limit_voronoi", room_upper_limit_voronoi_, 120.0); + std::cout << "room_segmentation/room_area_factor_upper_limit = " << room_upper_limit_voronoi_ << std::endl; + node_handle_.param("room_area_factor_lower_limit_voronoi", room_lower_limit_voronoi_, 1.53); + std::cout << "room_segmentation/room_area_factor_lower_limit = " << room_lower_limit_voronoi_ << std::endl; + node_handle_.param("voronoi_neighborhood_index", voronoi_neighborhood_index_, 280); + std::cout << "room_segmentation/voronoi_neighborhood_index = " << voronoi_neighborhood_index_ << std::endl; + node_handle_.param("max_iterations", max_iterations_, 150); + std::cout << "room_segmentation/max_iterations = " << max_iterations_ << std::endl; + node_handle_.param("min_critical_point_distance_factor", min_critical_point_distance_factor_, 1.6); + std::cout << "room_segmentation/min_critical_point_distance_factor = " << min_critical_point_distance_factor_ << std::endl; + node_handle_.param("max_area_for_merging", max_area_for_merging_, 12.5); + std::cout << "room_segmentation/max_area_for_merging = " << max_area_for_merging_ << std::endl; + } + //if (room_segmentation_algorithm_ == 4 || train_semantic_ == true) //set semantic parameters + { + node_handle_.param("room_area_factor_upper_limit_semantic", room_upper_limit_semantic_, 23.0); + std::cout << "room_segmentation/room_area_factor_upper_limit = " << room_upper_limit_semantic_ << std::endl; + node_handle_.param("room_area_factor_lower_limit_semantic", room_lower_limit_semantic_, 1.0); + std::cout << "room_segmentation/room_area_factor_lower_limit = " << room_lower_limit_semantic_ << std::endl; + + // train the algorithm if wanted + if(train_semantic_ == true) + { + AdaboostClassifier semantic_segmentation; + const std::string package_path = ros::package::getPath("ipa_room_segmentation"); + const std::string classifier_default_path = package_path + "/common/files/classifier_models/"; + const std::string classifier_path = "room_segmentation/classifier_models/"; + + // load files to train the algorithm + node_handle_.getParam("semantic_training_maps_room_file_list", semantic_training_maps_room_file_list_); + std::cout << "room_segmentation/semantic_training_maps_room_file_list = \n"; + for (size_t i=0; i room_training_maps; + for (size_t i=0; i hallway_training_maps; + for (size_t i=0; i possible_labels(3); + possible_labels[0] = 77; + possible_labels[1] = 115; + possible_labels[2] = 179; + + // read given paths to training files + node_handle_.getParam("vrf_original_maps_file_list", vrf_original_maps_file_list_); + std::cout << "room_segmentation/vrf_original_maps_file_list = \n"; + for (size_t i=0; i training_maps; + for (size_t i=0; i voronoi_maps; + for (size_t i=0; i voronoi_node_maps; + for (size_t i=0; i original_maps; + for (size_t i=0; iroom_segmentation_algorithm > 0) + room_segmentation_algorithm_ = goal->room_segmentation_algorithm; + + ros::Rate looping_rate(1); + ROS_INFO("*****Segmentation action server*****"); + ROS_INFO("map resolution is : %f", goal->map_resolution); + ROS_INFO("segmentation algorithm: %d", room_segmentation_algorithm_); + + //converting the map msg in cv format + cv_bridge::CvImagePtr cv_ptr_obj; + cv_ptr_obj = cv_bridge::toCvCopy(goal->input_map, sensor_msgs::image_encodings::MONO8); + cv::Mat original_img = cv_ptr_obj->image; + + //set the resolution and the limits for the actual goal and the Map origin + const float map_resolution = goal->map_resolution; + const cv::Point2d map_origin(goal->map_origin.position.x, goal->map_origin.position.y); + + // these preset values are deactivated because they would override the dynamic reconfigure configuration +// const int room_segmentation_algorithm_value = room_segmentation_algorithm_; +// if (goal->room_segmentation_algorithm > 0 && goal->room_segmentation_algorithm < 6) +// { +// room_segmentation_algorithm_ = goal->room_segmentation_algorithm; +// if(room_segmentation_algorithm_ == 1) //morpho +// { +// room_lower_limit_morphological_ = 0.8; +// room_upper_limit_morphological_ = 47.0; +// ROS_INFO("You have chosen the morphologcial segmentation."); +// } +// if(room_segmentation_algorithm_ == 2) //distance +// { +// room_lower_limit_distance_ = 0.35; +// room_upper_limit_distance_ = 163.0; +// ROS_INFO("You have chosen the distance segmentation."); +// } +// if(room_segmentation_algorithm_ == 3) //voronoi +// { +// room_lower_limit_voronoi_ = 0.1; //1.53; +// room_upper_limit_voronoi_ = 1000000.; //120.0; +// voronoi_neighborhood_index_ = 280; +// max_iterations_ = 150; +// min_critical_point_distance_factor_ = 0.5; //1.6; +// max_area_for_merging_ = 12.5; +// ROS_INFO("You have chosen the Voronoi segmentation"); +// } +// if(room_segmentation_algorithm_ == 4) //semantic +// { +// room_lower_limit_semantic_ = 1.0; +// room_upper_limit_semantic_ = 1000000.;//23.0; +// ROS_INFO("You have chosen the semantic segmentation."); +// } +// if(room_segmentation_algorithm_ == 5) //voronoi random field +// { +// room_lower_limit_voronoi_random_ = 1.53; //1.53 +// room_upper_limit_voronoi_random_ = 1000000.; //1000000.0 +// voronoi_random_field_epsilon_for_neighborhood_ = 7; +// min_neighborhood_size_ = 5; +// min_voronoi_random_field_node_distance_ = 7; // [pixel] +// max_voronoi_random_field_inference_iterations_ = 9000; +// max_area_for_merging_ = 12.5; +// ROS_INFO("You have chosen the voronoi random field segmentation."); +// } +// } + + + //segment the given map + cv::Mat segmented_map; + if (room_segmentation_algorithm_ == 1) + { + MorphologicalSegmentation morphological_segmentation; //morphological segmentation method + morphological_segmentation.segmentMap(original_img, segmented_map, map_resolution, room_lower_limit_morphological_, room_upper_limit_morphological_); + } + else if (room_segmentation_algorithm_ == 2) + { + DistanceSegmentation distance_segmentation; //distance segmentation method + distance_segmentation.segmentMap(original_img, segmented_map, map_resolution, room_lower_limit_distance_, room_upper_limit_distance_); + } + else if (room_segmentation_algorithm_ == 3) + { + VoronoiSegmentation voronoi_segmentation; //voronoi segmentation method + voronoi_segmentation.segmentMap(original_img, segmented_map, map_resolution, room_lower_limit_voronoi_, room_upper_limit_voronoi_, + voronoi_neighborhood_index_, max_iterations_, min_critical_point_distance_factor_, max_area_for_merging_, (display_segmented_map_&&DEBUG_DISPLAYS)); + } + else if (room_segmentation_algorithm_ == 4) + { + AdaboostClassifier semantic_segmentation; //semantic segmentation method + const std::string package_path = ros::package::getPath("ipa_room_segmentation"); + const std::string classifier_default_path = package_path + "/common/files/classifier_models/"; + const std::string classifier_path = "room_segmentation/classifier_models/"; + semantic_segmentation.segmentMap(original_img, segmented_map, map_resolution, room_lower_limit_semantic_, room_upper_limit_semantic_, + classifier_path, classifier_default_path, (display_segmented_map_&&DEBUG_DISPLAYS)); + } + else if (room_segmentation_algorithm_ == 5) + { + VoronoiRandomFieldSegmentation vrf_segmentation; //voronoi random field segmentation method + const std::string package_path = ros::package::getPath("ipa_room_segmentation"); + std::string classifier_default_path = package_path + "/common/files/classifier_models/"; + std::string classifier_storage_path = "room_segmentation/classifier_models/"; + // vector that stores the possible labels that are drawn in the training maps. Order: room - hallway - doorway + std::vector possible_labels(3); + possible_labels[0] = 77; + possible_labels[1] = 115; + possible_labels[2] = 179; + doorway_points_.clear(); + vrf_segmentation.segmentMap(original_img, segmented_map, voronoi_random_field_epsilon_for_neighborhood_, max_iterations_, + min_neighborhood_size_, possible_labels, min_voronoi_random_field_node_distance_, + (display_segmented_map_&&DEBUG_DISPLAYS), classifier_storage_path, classifier_default_path, max_voronoi_random_field_inference_iterations_, + map_resolution, room_lower_limit_voronoi_random_, room_upper_limit_voronoi_random_, max_area_for_merging_, &doorway_points_); + } + else if (room_segmentation_algorithm_ == 99) + { + // pass through segmentation: takes a map which is already separated into unconnected areas and returns these as the resulting segmentation in the format of this program + // todo: closing operation explicitly for bad maps --> needs parameterization + //original_img.convertTo(segmented_map, CV_32SC1, 256, 0); // occupied space = 0, free space = 65280 + cv::Mat original_img_eroded, temp; + cv::erode(original_img, temp, cv::Mat(), cv::Point(-1, -1), 3); + cv::dilate(temp, original_img_eroded, cv::Mat(), cv::Point(-1, -1), 3); + original_img_eroded.convertTo(segmented_map, CV_32SC1, 256, 0); // occupied space = 0, free space = 65280 + int label_index = 1; + +// cv::imshow("original_img", original_img_eroded); +// cv::waitKey(); + + for (int y = 0; y < segmented_map.rows; y++) + { + for (int x = 0; x < segmented_map.cols; x++) + { + // if original map is occupied space here or if the segmented map has already received a label for that cell --> skip + if (original_img_eroded.at(y,x) != 255 || segmented_map.at(y,x)!=65280) + continue; + + // fill each room area with a unique id + cv::Rect rect; + cv::floodFill(segmented_map, cv::Point(x,y), label_index, &rect, 0, 0, 4); + + // determine filled area + double area = 0; + for (int v = rect.y; v < segmented_map.rows; v++) + for (int u = rect.x; u < segmented_map.cols; u++) + if (segmented_map.at(v,u)==label_index) + area += 1.; + area = map_resolution * map_resolution * area; // convert from cells to m^2 + + // exclude too small and too big rooms + if (area < room_lower_limit_passthrough_ || area > room_upper_limit_passthrough_) + { + for (int v = rect.y; v < segmented_map.rows; v++) + for (int u = rect.x; u < segmented_map.cols; u++) + if (segmented_map.at(v,u)==label_index) + segmented_map.at(v,u) = 0; + } + else + label_index++; + } + } + std::cout << "Labeled " << label_index-1 << " segments." << std::endl; + } + else + { + ROS_ERROR("Undefined algorithm selected."); + room_segmentation_algorithm_ = stored_room_segmentation_algorithm; + return; + } + + ROS_INFO("********Segmented the map************"); + // looping_rate.sleep(); + + // get the min/max-values and the room-centers + // compute room label codebook + std::map label_vector_index_codebook; // maps each room label to a position in the rooms vector + size_t vector_index = 0; + for (int v = 0; v < segmented_map.rows; ++v) + { + for (int u = 0; u < segmented_map.cols; ++u) + { + const int label = segmented_map.at(v, u); + if (label > 0 && label < 65280) // do not count walls/obstacles or free space as label + { + if (label_vector_index_codebook.find(label) == label_vector_index_codebook.end()) + { + label_vector_index_codebook[label] = vector_index; + vector_index++; + } + } + } + } + //min/max y/x-values vector for each room. Initialized with extreme values + std::vector min_x_value_of_the_room(label_vector_index_codebook.size(), 100000000); + std::vector max_x_value_of_the_room(label_vector_index_codebook.size(), 0); + std::vector min_y_value_of_the_room(label_vector_index_codebook.size(), 100000000); + std::vector max_y_value_of_the_room(label_vector_index_codebook.size(), 0); + //vector of the central Point for each room, initially filled with Points out of the map + std::vector room_centers_x_values(label_vector_index_codebook.size(), -1); + std::vector room_centers_y_values(label_vector_index_codebook.size(), -1); + //***********************Find min/max x and y coordinate and center of each found room******************** + //check y/x-value for every Pixel and make the larger/smaller value to the current value of the room + for (int y = 0; y < segmented_map.rows; ++y) + { + for (int x = 0; x < segmented_map.cols; ++x) + { + const int label = segmented_map.at(y, x); + if (label > 0 && label < 65280) //if Pixel is white or black it is no room --> doesn't need to be checked + { + const int index = label_vector_index_codebook[label]; + min_x_value_of_the_room[index] = std::min(x, min_x_value_of_the_room[index]); + max_x_value_of_the_room[index] = std::max(x, max_x_value_of_the_room[index]); + max_y_value_of_the_room[index] = std::max(y, max_y_value_of_the_room[index]); + min_y_value_of_the_room[index] = std::min(y, min_y_value_of_the_room[index]); + } + } + } + //get centers for each room +// for (size_t idx = 0; idx < room_centers_x_values.size(); ++idx) +// { +// if (max_x_value_of_the_room[idx] != 0 && max_y_value_of_the_room[idx] != 0 && min_x_value_of_the_room[idx] != 100000000 && min_y_value_of_the_room[idx] != 100000000) +// { +// room_centers_x_values[idx] = (min_x_value_of_the_room[idx] + max_x_value_of_the_room[idx]) / 2; +// room_centers_y_values[idx] = (min_y_value_of_the_room[idx] + max_y_value_of_the_room[idx]) / 2; +// cv::circle(segmented_map, cv::Point(room_centers_x_values[idx], room_centers_y_values[idx]), 2, cv::Scalar(200*256), CV_FILLED); +// } +// } + // use distance transform and mean shift to find good room centers that are reachable by the robot + // first check whether a robot radius shall be applied to obstacles in order to exclude room center points that are not reachable by the robot + cv::Mat segmented_map_copy = segmented_map; + cv::Mat connection_to_other_rooms = cv::Mat::zeros(segmented_map.rows, segmented_map.cols, CV_8UC1); // stores for each pixel whether a path to another rooms exists for a robot of size robot_radius + if (goal->robot_radius > 0.0) + { + // consider robot radius for exclusion of non-reachable points + segmented_map_copy = segmented_map.clone(); + cv::Mat map_8u, eroded_map; + segmented_map_copy.convertTo(map_8u, CV_8UC1, 1., 0.); + int number_of_erosions = (goal->robot_radius / goal->map_resolution); + cv::erode(map_8u, eroded_map, cv::Mat(), cv::Point(-1, -1), number_of_erosions); + for (int v=0; v(v,u) == 0) + segmented_map_copy.at(v,u) = 0; + + // compute connectivity of remaining accessible room cells to other rooms + bool stop = false; + while (stop == false) + { + stop = true; + for (int v=1; v(v,u) != 0) + continue; + + // only consider cells labeled as a room + const int label = segmented_map_copy.at(v,u); + if (label <= 0 || label >= 65280) + continue; + + for (int dv=-1; dv<=1; ++dv) + { + for (int du=-1; du<=1; ++du) + { + if (dv==0 && du==0) + continue; + const int neighbor_label = segmented_map_copy.at(v+dv,u+du); + if (neighbor_label>0 && neighbor_label<65280 && (neighbor_label!=label || (neighbor_label==label && connection_to_other_rooms.at(v+dv,u+du)==255))) + { + // either the room cell has a direct border to a different room or the room cell has a neighbor from the same room label with a connecting path to another room + connection_to_other_rooms.at(v,u) = 255; + stop = false; + } + } + } + } + } + } + } + // compute the room centers + MeanShift2D ms; + for (std::map::iterator it = label_vector_index_codebook.begin(); it != label_vector_index_codebook.end(); ++it) + { + int trial = 1; // use robot_radius to avoid room centers that are not accessible by a robot with a given radius + if (goal->robot_radius <= 0.) + trial = 2; + + for (; trial <= 2; ++trial) + { + // compute distance transform for each room on the room cells that have some connection to another room (trial 1) or just on all cells of that room (trial 2) + const int label = it->first; + int number_room_pixels = 0; + cv::Mat room = cv::Mat::zeros(segmented_map_copy.rows, segmented_map_copy.cols, CV_8UC1); + for (int v = 0; v < segmented_map_copy.rows; ++v) + for (int u = 0; u < segmented_map_copy.cols; ++u) + if (segmented_map_copy.at(v, u) == label && (trial==2 || connection_to_other_rooms.at(v,u)==255)) + { + room.at(v, u) = 255; + ++number_room_pixels; + } + if (number_room_pixels == 0) + continue; + cv::Mat distance_map; //variable for the distance-transformed map, type: CV_32FC1 + cv::distanceTransform(room, distance_map, CV_DIST_L2, 5); + // find point set with largest distance to obstacles + double min_val = 0., max_val = 0.; + cv::minMaxLoc(distance_map, &min_val, &max_val); + std::vector room_cells; + for (int v = 0; v < distance_map.rows; ++v) + for (int u = 0; u < distance_map.cols; ++u) + if (distance_map.at(v, u) > max_val * 0.95f) + room_cells.push_back(cv::Vec2d(u, v)); + if (room_cells.size()==0) + continue; + // use meanshift to find the modes in that set + cv::Vec2d room_center = ms.findRoomCenter(room, room_cells, map_resolution); + const int index = it->second; + room_centers_x_values[index] = room_center[0]; + room_centers_y_values[index] = room_center[1]; + + if (room_cells.size() > 0) + break; + } + } + + // convert the segmented map into an indexed map which labels the segments with consecutive numbers (instead of arbitrary unordered labels in segmented map) + cv::Mat indexed_map = segmented_map.clone(); + for (int y = 0; y < segmented_map.rows; ++y) + { + for (int x = 0; x < segmented_map.cols; ++x) + { + const int label = segmented_map.at(y,x); + if (label > 0 && label < 65280) + indexed_map.at(y,x) = label_vector_index_codebook[label]+1;//start value from 1 --> 0 is reserved for obstacles + } + } + + if (display_segmented_map_ == true) + { + // colorize the segmented map with the indices of the room_center vector + cv::Mat color_segmented_map = indexed_map.clone(); + color_segmented_map.convertTo(color_segmented_map, CV_8U); + cv::cvtColor(color_segmented_map, color_segmented_map, CV_GRAY2BGR); + for(size_t i = 1; i <= room_centers_x_values.size(); ++i) + { + //choose random color for each room + const cv::Vec3b color((rand() % 250) + 1, (rand() % 250) + 1, (rand() % 250) + 1); + for(size_t v = 0; v < indexed_map.rows; ++v) + for(size_t u = 0; u < indexed_map.cols; ++u) + if(indexed_map.at(v,u) == i) + color_segmented_map.at(v,u) = color; + } +// cv::Mat disp = segmented_map.clone(); + for (size_t index = 0; index < room_centers_x_values.size(); ++index) +#if CV_MAJOR_VERSION<=3 + cv::circle(color_segmented_map, cv::Point(room_centers_x_values[index], room_centers_y_values[index]), 2, cv::Scalar(256), CV_FILLED); +#else + cv::circle(color_segmented_map, cv::Point(room_centers_x_values[index], room_centers_y_values[index]), 2, cv::Scalar(256), cv::FILLED); +#endif + + cv::imshow("segmentation", color_segmented_map); + cv::waitKey(); + } + + if (publish_segmented_map_ == true) + { + // "colorize" the segmented map with gray scale values + nav_msgs::OccupancyGrid segmented_grid; + segmented_grid.header.stamp = ros::Time::now(); + segmented_grid.header.frame_id = "map"; + segmented_grid.info.resolution = map_resolution; + segmented_grid.info.width = indexed_map.cols; + segmented_grid.info.height = indexed_map.rows; + segmented_grid.info.origin.position.x = map_origin.x; + segmented_grid.info.origin.position.y = map_origin.y; + segmented_grid.data.resize(segmented_grid.info.width*segmented_grid.info.height); + std::map colors; + //choose random color for each room + colors[0] = 0; + for(int i = 1; i <= room_centers_x_values.size(); ++i) + colors[i] = 20 + rand() % 81; + int i=0; + for(int v = 0; v < indexed_map.rows; ++v) + for(int u = 0; u < indexed_map.cols; ++u, ++i) + segmented_grid.data[i] = colors[indexed_map.at(v,u)]; + map_pub_.publish(segmented_grid); + } + + //****************publish the results********************** + ipa_building_msgs::MapSegmentationResult action_result; + //converting the cv format in map msg format + cv_bridge::CvImage cv_image; + cv_image.header.stamp = ros::Time::now(); + cv_image.encoding = "32SC1"; + cv_image.image = indexed_map; + cv_image.toImageMsg(action_result.segmented_map); + + //setting value to the action msgs to publish + action_result.map_resolution = goal->map_resolution; + action_result.map_origin = goal->map_origin; + + //setting massages in pixel value + action_result.room_information_in_pixel.clear(); + if (goal->return_format_in_pixel == true) + { + std::vector room_information(room_centers_x_values.size()); + for (size_t i=0; i found_doorway_points(doorway_points_.size()); + for(size_t i = 0; i < doorway_points_.size(); ++i) + { + found_doorway_points[i].x = doorway_points_[i].x; + found_doorway_points[i].y = doorway_points_[i].y; + } + doorway_points_.clear(); + + action_result.doorway_points = found_doorway_points; + } + } + //setting messages in meter + action_result.room_information_in_meter.clear(); + if (goal->return_format_in_meter == true) + { + std::vector room_information(room_centers_x_values.size()); + for (size_t i=0; i found_doorway_points(doorway_points_.size()); + for(size_t i = 0; i < doorway_points_.size(); ++i) + { + found_doorway_points[i].x = convert_pixel_to_meter_for_x_coordinate(doorway_points_[i].x, map_resolution, map_origin);; + found_doorway_points[i].y = convert_pixel_to_meter_for_y_coordinate(doorway_points_[i].y, map_resolution, map_origin); + } + doorway_points_.clear(); + + action_result.doorway_points = found_doorway_points; + } + } + + // reset to parameterized segmentation algorithm + room_segmentation_algorithm_ = stored_room_segmentation_algorithm; + + //publish result + room_segmentation_server_.setSucceeded(action_result); + + ROS_INFO("********Map segmentation finished************"); +} + +bool RoomSegmentationServer::extractAreaMapFromLabeledMap(ipa_building_msgs::ExtractAreaMapFromLabeledMapRequest& request, ipa_building_msgs::ExtractAreaMapFromLabeledMapResponse& response) +{ + // convert the Image msg in cv format + cv_bridge::CvImagePtr cv_ptr_obj; + cv_ptr_obj = cv_bridge::toCvCopy(request.segmented_map, sensor_msgs::image_encodings::TYPE_32SC1); + cv::Mat segmented_map = cv_ptr_obj->image; + + // create a new map that only contains the segment with the label of interest + cv::Mat segmented_area = cv::Mat::zeros(segmented_map.rows, segmented_map.cols, CV_8UC1); + const int segment_of_interest = request.segment_of_interest; + for (int v=0; v(v,u)==segment_of_interest) + { + segmented_area.at(v,u) == 255; + } + } + } + + // convert the cv format in Image msg format + cv_bridge::CvImage cv_image; + cv_image.header.stamp = ros::Time::now(); + cv_image.encoding = "mono8"; + cv_image.image = segmented_area; + cv_image.toImageMsg(response.segmented_area); + + return true; +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "room_segmentation_server"); + + ros::NodeHandle nh("~"); + + RoomSegmentationServer segmentationAlgorithmObj(nh, ros::this_node::getName()); + ROS_INFO("Action Server for room segmentation has been initialized......"); + ros::spin(); + + return 0; +} diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/ros/test/room_segmentation_action_client.launch b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/ros/test/room_segmentation_action_client.launch new file mode 100644 index 0000000..367022e --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/ros/test/room_segmentation_action_client.launch @@ -0,0 +1,7 @@ + + + + + + + \ No newline at end of file diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/ros/test/room_segmentation_evaluation.launch b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/ros/test/room_segmentation_evaluation.launch new file mode 100644 index 0000000..5390037 --- /dev/null +++ b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/ipa_room_segmentation/ros/test/room_segmentation_evaluation.launch @@ -0,0 +1,7 @@ + + + + + + + \ No newline at end of file diff --git a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/src/ipa_coverage_planning b/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/src/ipa_coverage_planning deleted file mode 160000 index d2e9227..0000000 --- a/Code/RK3588/PIBot_ROS/ros_ws/src/ipa_coverage_planning/src/ipa_coverage_planning +++ /dev/null @@ -1 +0,0 @@ -Subproject commit d2e922718991210048848b6ca06a1c118c96be2e