├── gmm_map_python ├── script │ ├── __init__.py │ ├── config.py │ ├── gmm_transform.py │ ├── visualgmm.py │ ├── generate_descriptor.py │ ├── TFGraph.py │ ├── map_registration.py │ ├── GMMmap.py │ └── PointNetVlad.py ├── msg │ ├── gmmlist.msg │ ├── SubmapList.msg │ ├── gmmFrame.msg │ ├── gmm.msg │ ├── SubmapEntry.msg │ └── Submap.msg ├── include │ └── gmm_map_python │ │ └── gmm_map_python.h ├── srv │ └── FeatureExtraction.srv ├── launch │ ├── pointcloud2octomap.launch │ ├── visualgmm_realsence_2robot.launch │ ├── visualgmm.launch │ ├── visualgmm_realsence.launch │ ├── visualgmm_realsence_2.launch │ ├── static_tf_pubs.launch │ └── visualgmm_realsence_debug.launch ├── package.xml ├── src │ ├── featureextraction.cpp │ └── downsample_cloud.cpp ├── CMakeLists.txt └── rviz │ ├── robot1.rviz │ └── robot2.rviz ├── turtle3sim ├── gazebo_images │ ├── wheel.jpg │ ├── main_body.jpg │ └── meshes │ │ ├── images │ │ ├── wheel.jpg │ │ └── main_body.jpg │ │ └── MTR_map.dae ├── param_teb │ ├── global_costmap_params.yaml │ ├── move_base_params.yaml │ ├── local_costmap_params.yaml │ ├── costmap_common_params_burger.yaml │ └── teb_local_planner_params_burger.yaml ├── launch │ ├── robots_slam │ │ ├── single_robot.launch │ │ ├── two_robots.launch │ │ └── three_robots.launch │ ├── includes │ │ ├── robot.launch.xml │ │ ├── cartographer_teb.launch │ │ └── move_base_robot.launch │ └── simulation_environment │ │ ├── small_env_single_robot.launch │ │ ├── large_env_single_robot.launch │ │ ├── large_env_two_robots.launch │ │ ├── small_env_two_robots.launch │ │ ├── large_env_three_robots.launch │ │ ├── small_env_three_robots.launch │ │ └── large_env_six_robots.launch ├── config │ ├── turtlebot3_lds_2d.lua │ ├── turtlebot3_robot3.lua │ ├── turtlebot3_robot4.lua │ ├── turtlebot3_robot5.lua │ ├── turtlebot3_robot6.lua │ ├── turtlebot3_robot1.lua │ └── turtlebot3_robot2.lua ├── script │ └── ImuChange.py ├── package.xml ├── urdf │ └── turtlebot3_burger.urdf.xacro └── CMakeLists.txt ├── .gitignore └── Readme.md /gmm_map_python/script/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /gmm_map_python/msg/gmmlist.msg: -------------------------------------------------------------------------------- 1 | gmm[] data 2 | -------------------------------------------------------------------------------- /gmm_map_python/include/gmm_map_python/gmm_map_python.h: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /gmm_map_python/msg/SubmapList.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | gmm_map_python/SubmapEntry[] submaps #返回所有的submap信息的列表 3 | -------------------------------------------------------------------------------- /gmm_map_python/msg/gmmFrame.msg: -------------------------------------------------------------------------------- 1 | int32 mix_num 2 | int32 dim 3 | float64[] weights 4 | float64[] means 5 | float64[] covariances -------------------------------------------------------------------------------- /gmm_map_python/srv/FeatureExtraction.srv: -------------------------------------------------------------------------------- 1 | sensor_msgs/PointCloud2 input_cloud 2 | --- 3 | sensor_msgs/PointCloud2 output_cloud -------------------------------------------------------------------------------- /turtle3sim/gazebo_images/wheel.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/efc-robot/gmm_map_python/HEAD/turtle3sim/gazebo_images/wheel.jpg -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode 2 | .idea/ 3 | *.vscode 4 | ./venv 5 | *.pyc 6 | *.ckpt 7 | script/model 8 | baseline2 9 | rosbag 10 | -------------------------------------------------------------------------------- /turtle3sim/gazebo_images/main_body.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/efc-robot/gmm_map_python/HEAD/turtle3sim/gazebo_images/main_body.jpg -------------------------------------------------------------------------------- /turtle3sim/gazebo_images/meshes/images/wheel.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/efc-robot/gmm_map_python/HEAD/turtle3sim/gazebo_images/meshes/images/wheel.jpg -------------------------------------------------------------------------------- /turtle3sim/gazebo_images/meshes/images/main_body.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/efc-robot/gmm_map_python/HEAD/turtle3sim/gazebo_images/meshes/images/main_body.jpg -------------------------------------------------------------------------------- /gmm_map_python/msg/gmm.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | int32 mix_num 3 | float32[] prior 4 | float32[] x 5 | float32[] y 6 | float32[] z 7 | float32[] x_var 8 | float32[] y_var 9 | float32[] z_var 10 | geometry_msgs/Pose pose 11 | -------------------------------------------------------------------------------- /turtle3sim/param_teb/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: map 3 | robot_base_frame: base_footprint 4 | 5 | update_frequency: 10.0 6 | publish_frequency: 10.0 7 | transform_tolerance: 0.5 8 | 9 | static_map: true 10 | 11 | -------------------------------------------------------------------------------- /gmm_map_python/msg/SubmapEntry.msg: -------------------------------------------------------------------------------- 1 | #每个Submap本质上是一个key frame 2 | 3 | int32 robot_id 4 | int32 submap_index # key frame 在某个机器人内部的 ID 5 | int32 submap_version # key frame 的版本,主要是在局部轨迹内更新 key frame 6 | geometry_msgs/Pose pose # key frame的位置, (后端优化会不断优化这个位姿) 7 | bool is_frozen #主要是判断这个地图是否不会再更新了 -------------------------------------------------------------------------------- /turtle3sim/param_teb/move_base_params.yaml: -------------------------------------------------------------------------------- 1 | shutdown_costmaps: false 2 | controller_frequency: 10.0 3 | planner_patience: 5.0 4 | controller_patience: 15.0 5 | conservative_reset_dist: 3.0 6 | planner_frequency: 5.0 7 | oscillation_timeout: 10.0 8 | oscillation_distance: 0.2 9 | 10 | -------------------------------------------------------------------------------- /turtle3sim/param_teb/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | global_frame: odom 3 | robot_base_frame: base_footprint 4 | 5 | update_frequency: 10.0 6 | publish_frequency: 10.0 7 | transform_tolerance: 0.5 8 | 9 | static_map: false 10 | rolling_window: true 11 | width: 3 12 | height: 3 13 | resolution: 0.05 14 | 15 | -------------------------------------------------------------------------------- /gmm_map_python/msg/Submap.msg: -------------------------------------------------------------------------------- 1 | int32 index # key frame 在某个机器人内部的 ID 2 | int32 robot_id 3 | geometry_msgs/Pose pose_odom 4 | geometry_msgs/Pose pose # key frame的位置, (后端优化会不断优化这个位姿) 5 | time add_time 6 | float64[] point_clouds 7 | gmmFrame submap_gmm 8 | bool freezed #主要是判断这个地图是否不会再更新了 9 | float64[] descriptor 10 | float64[] feature_point 11 | gmmFrame feature_gmm -------------------------------------------------------------------------------- /turtle3sim/param_teb/costmap_common_params_burger.yaml: -------------------------------------------------------------------------------- 1 | obstacle_range: 3.0 2 | raytrace_range: 3.5 3 | 4 | footprint: [[-0.105, -0.105], [-0.105, 0.105], [0.041, 0.105], [0.041, -0.105]] 5 | #robot_radius: 0.105 6 | 7 | inflation_radius: 0.4 8 | cost_scaling_factor: 1 9 | 10 | map_type: costmap 11 | observation_sources: laser_scan_sensor 12 | laser_scan_sensor: {sensor_frame: base_scan, data_type: LaserScan, topic: scan, marking: true, clearing: true} -------------------------------------------------------------------------------- /gmm_map_python/launch/pointcloud2octomap.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /turtle3sim/launch/robots_slam/single_robot.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /turtle3sim/launch/includes/robot.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /gmm_map_python/launch/visualgmm_realsence_2robot.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 15 | 16 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /turtle3sim/launch/robots_slam/two_robots.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /gmm_map_python/launch/visualgmm.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /turtle3sim/launch/simulation_environment/small_env_single_robot.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /turtle3sim/launch/simulation_environment/large_env_single_robot.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /turtle3sim/launch/robots_slam/three_robots.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /turtle3sim/launch/simulation_environment/large_env_two_robots.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /turtle3sim/launch/simulation_environment/small_env_two_robots.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /gmm_map_python/script/config.py: -------------------------------------------------------------------------------- 1 | # GLOBAL 2 | NUM_POINTS = 4096 3 | FEATURE_OUTPUT_DIM = 256 4 | RESULTS_FOLDER = "results/" 5 | OUTPUT_FILE = "results/results.txt" 6 | 7 | LOG_DIR = 'log/' 8 | MODEL_FILENAME = "model.ckpt" 9 | 10 | DATASET_FOLDER = '../../benchmark_datasets/' 11 | 12 | # TRAIN 13 | BATCH_NUM_QUERIES = 2 14 | TRAIN_POSITIVES_PER_QUERY = 2 15 | TRAIN_NEGATIVES_PER_QUERY = 18 16 | DECAY_STEP = 200000 17 | DECAY_RATE = 0.7 18 | BASE_LEARNING_RATE = 0.000005 19 | MOMENTUM = 0.9 20 | OPTIMIZER = 'ADAM' 21 | MAX_EPOCH = 20 22 | 23 | MARGIN_1 = 0.5 24 | MARGIN_2 = 0.2 25 | 26 | BN_INIT_DECAY = 0.5 27 | BN_DECAY_DECAY_RATE = 0.5 28 | BN_DECAY_CLIP = 0.99 29 | 30 | RESUME = False 31 | 32 | TRAIN_FILE = 'generating_queries/training_queries_baseline.pickle' 33 | TEST_FILE = 'generating_queries/test_queries_baseline.pickle' 34 | 35 | # LOSS 36 | LOSS_FUNCTION = 'quadruplet' 37 | LOSS_LAZY = True 38 | TRIPLET_USE_BEST_POSITIVES = False 39 | LOSS_IGNORE_ZERO_BATCH = False 40 | 41 | # EVAL6 42 | EVAL_BATCH_SIZE = 2 43 | EVAL_POSITIVES_PER_QUERY = 4 44 | EVAL_NEGATIVES_PER_QUERY = 12 45 | 46 | EVAL_DATABASE_FILE = 'generating_queries/oxford_evaluation_database.pickle' 47 | EVAL_QUERY_FILE = 'generating_queries/oxford_evaluation_query.pickle' 48 | 49 | 50 | def cfg_str(): 51 | out_string = "" 52 | for name in globals(): 53 | if not name.startswith("__") and not name.__contains__("cfg_str"): 54 | #print(name, "=", globals()[name]) 55 | out_string = out_string + "cfg." + name + \ 56 | "=" + str(globals()[name]) + "\n" 57 | return out_string 58 | -------------------------------------------------------------------------------- /turtle3sim/config/turtlebot3_lds_2d.lua: -------------------------------------------------------------------------------- 1 | include "map_builder.lua" 2 | include "trajectory_builder.lua" 3 | 4 | options = { 5 | map_builder = MAP_BUILDER, 6 | trajectory_builder = TRAJECTORY_BUILDER, 7 | map_frame = "map", 8 | tracking_frame = "base_footprint", -- imu_link, If you are using gazebo, use 'base_footprint' (libgazebo_ros_imu's bug) 9 | published_frame = "odom", 10 | odom_frame = "odom", 11 | provide_odom_frame = false, 12 | publish_frame_projected_to_2d = false, 13 | use_odometry = true, 14 | use_nav_sat = false, 15 | use_landmarks = false, 16 | num_laser_scans = 1, 17 | num_multi_echo_laser_scans = 0, 18 | num_subdivisions_per_laser_scan = 1, 19 | num_point_clouds = 0, 20 | lookup_transform_timeout_sec = 0.2, 21 | submap_publish_period_sec = 0.3, 22 | pose_publish_period_sec = 5e-3, 23 | trajectory_publish_period_sec = 30e-3, 24 | rangefinder_sampling_ratio = 1., 25 | odometry_sampling_ratio = 1., 26 | fixed_frame_pose_sampling_ratio = 1., 27 | imu_sampling_ratio = 1., 28 | landmarks_sampling_ratio = 1., 29 | } 30 | 31 | MAP_BUILDER.use_trajectory_builder_2d = true 32 | TRAJECTORY_BUILDER_2D.submaps.num_range_data = 20 33 | 34 | TRAJECTORY_BUILDER_2D.min_range = 0.1 35 | TRAJECTORY_BUILDER_2D.max_range = 7 36 | TRAJECTORY_BUILDER_2D.missing_data_ray_length = 3. 37 | TRAJECTORY_BUILDER_2D.use_imu_data = true 38 | TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true 39 | TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.1) 40 | 41 | POSE_GRAPH.constraint_builder.min_score = 0.65 42 | POSE_GRAPH.constraint_builder.global_localization_min_score = 0.7 43 | 44 | return options 45 | -------------------------------------------------------------------------------- /turtle3sim/config/turtlebot3_robot3.lua: -------------------------------------------------------------------------------- 1 | include "map_builder.lua" 2 | include "trajectory_builder.lua" 3 | 4 | options = { 5 | map_builder = MAP_BUILDER, 6 | trajectory_builder = TRAJECTORY_BUILDER, 7 | map_frame = "robot3/map", 8 | tracking_frame = "robot3/base_footprint", -- imu_link, If you are using gazebo, use 'base_footprint' (libgazebo_ros_imu's bug) 9 | published_frame = "robot3/odom", 10 | odom_frame = "robot3/odom", 11 | provide_odom_frame = false, 12 | publish_frame_projected_to_2d = false, 13 | use_odometry = true, 14 | use_nav_sat = false, 15 | use_landmarks = false, 16 | num_laser_scans = 1, 17 | num_multi_echo_laser_scans = 0, 18 | num_subdivisions_per_laser_scan = 1, 19 | num_point_clouds = 0, 20 | lookup_transform_timeout_sec = 0.2, 21 | submap_publish_period_sec = 0.3, 22 | pose_publish_period_sec = 5e-3, 23 | trajectory_publish_period_sec = 30e-3, 24 | rangefinder_sampling_ratio = 1., 25 | odometry_sampling_ratio = 1., 26 | fixed_frame_pose_sampling_ratio = 1., 27 | imu_sampling_ratio = 1., 28 | landmarks_sampling_ratio = 1., 29 | } 30 | 31 | MAP_BUILDER.use_trajectory_builder_2d = true 32 | TRAJECTORY_BUILDER_2D.submaps.num_range_data = 20 33 | 34 | TRAJECTORY_BUILDER_2D.min_range = 0.1 35 | TRAJECTORY_BUILDER_2D.max_range = 7 36 | TRAJECTORY_BUILDER_2D.missing_data_ray_length = 7. 37 | TRAJECTORY_BUILDER_2D.use_imu_data = true 38 | TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true 39 | TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.1) 40 | 41 | POSE_GRAPH.constraint_builder.min_score = 0.8 42 | POSE_GRAPH.constraint_builder.global_localization_min_score = 0.8 43 | 44 | return options 45 | -------------------------------------------------------------------------------- /turtle3sim/config/turtlebot3_robot4.lua: -------------------------------------------------------------------------------- 1 | include "map_builder.lua" 2 | include "trajectory_builder.lua" 3 | 4 | options = { 5 | map_builder = MAP_BUILDER, 6 | trajectory_builder = TRAJECTORY_BUILDER, 7 | map_frame = "robot4/map", 8 | tracking_frame = "robot4/base_footprint", -- imu_link, If you are using gazebo, use 'base_footprint' (libgazebo_ros_imu's bug) 9 | published_frame = "robot4/odom", 10 | odom_frame = "robot4/odom", 11 | provide_odom_frame = false, 12 | publish_frame_projected_to_2d = false, 13 | use_odometry = true, 14 | use_nav_sat = false, 15 | use_landmarks = false, 16 | num_laser_scans = 1, 17 | num_multi_echo_laser_scans = 0, 18 | num_subdivisions_per_laser_scan = 1, 19 | num_point_clouds = 0, 20 | lookup_transform_timeout_sec = 0.2, 21 | submap_publish_period_sec = 0.3, 22 | pose_publish_period_sec = 5e-3, 23 | trajectory_publish_period_sec = 30e-3, 24 | rangefinder_sampling_ratio = 1., 25 | odometry_sampling_ratio = 1., 26 | fixed_frame_pose_sampling_ratio = 1., 27 | imu_sampling_ratio = 1., 28 | landmarks_sampling_ratio = 1., 29 | } 30 | 31 | MAP_BUILDER.use_trajectory_builder_2d = true 32 | TRAJECTORY_BUILDER_2D.submaps.num_range_data = 20 33 | 34 | TRAJECTORY_BUILDER_2D.min_range = 0.1 35 | TRAJECTORY_BUILDER_2D.max_range = 7 36 | TRAJECTORY_BUILDER_2D.missing_data_ray_length = 7. 37 | TRAJECTORY_BUILDER_2D.use_imu_data = true 38 | TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true 39 | TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.1) 40 | 41 | POSE_GRAPH.constraint_builder.min_score = 0.8 42 | POSE_GRAPH.constraint_builder.global_localization_min_score = 0.8 43 | 44 | return options 45 | -------------------------------------------------------------------------------- /turtle3sim/config/turtlebot3_robot5.lua: -------------------------------------------------------------------------------- 1 | include "map_builder.lua" 2 | include "trajectory_builder.lua" 3 | 4 | options = { 5 | map_builder = MAP_BUILDER, 6 | trajectory_builder = TRAJECTORY_BUILDER, 7 | map_frame = "robot5/map", 8 | tracking_frame = "robot5/base_footprint", -- imu_link, If you are using gazebo, use 'base_footprint' (libgazebo_ros_imu's bug) 9 | published_frame = "robot5/odom", 10 | odom_frame = "robot5/odom", 11 | provide_odom_frame = false, 12 | publish_frame_projected_to_2d = false, 13 | use_odometry = true, 14 | use_nav_sat = false, 15 | use_landmarks = false, 16 | num_laser_scans = 1, 17 | num_multi_echo_laser_scans = 0, 18 | num_subdivisions_per_laser_scan = 1, 19 | num_point_clouds = 0, 20 | lookup_transform_timeout_sec = 0.2, 21 | submap_publish_period_sec = 0.3, 22 | pose_publish_period_sec = 5e-3, 23 | trajectory_publish_period_sec = 30e-3, 24 | rangefinder_sampling_ratio = 1., 25 | odometry_sampling_ratio = 1., 26 | fixed_frame_pose_sampling_ratio = 1., 27 | imu_sampling_ratio = 1., 28 | landmarks_sampling_ratio = 1., 29 | } 30 | 31 | MAP_BUILDER.use_trajectory_builder_2d = true 32 | TRAJECTORY_BUILDER_2D.submaps.num_range_data = 20 33 | 34 | TRAJECTORY_BUILDER_2D.min_range = 0.1 35 | TRAJECTORY_BUILDER_2D.max_range = 7 36 | TRAJECTORY_BUILDER_2D.missing_data_ray_length = 7. 37 | TRAJECTORY_BUILDER_2D.use_imu_data = true 38 | TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true 39 | TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.1) 40 | 41 | POSE_GRAPH.constraint_builder.min_score = 0.8 42 | POSE_GRAPH.constraint_builder.global_localization_min_score = 0.8 43 | 44 | return options 45 | -------------------------------------------------------------------------------- /turtle3sim/config/turtlebot3_robot6.lua: -------------------------------------------------------------------------------- 1 | include "map_builder.lua" 2 | include "trajectory_builder.lua" 3 | 4 | options = { 5 | map_builder = MAP_BUILDER, 6 | trajectory_builder = TRAJECTORY_BUILDER, 7 | map_frame = "robot6/map", 8 | tracking_frame = "robot6/base_footprint", -- imu_link, If you are using gazebo, use 'base_footprint' (libgazebo_ros_imu's bug) 9 | published_frame = "robot6/odom", 10 | odom_frame = "robot6/odom", 11 | provide_odom_frame = false, 12 | publish_frame_projected_to_2d = false, 13 | use_odometry = true, 14 | use_nav_sat = false, 15 | use_landmarks = false, 16 | num_laser_scans = 1, 17 | num_multi_echo_laser_scans = 0, 18 | num_subdivisions_per_laser_scan = 1, 19 | num_point_clouds = 0, 20 | lookup_transform_timeout_sec = 0.2, 21 | submap_publish_period_sec = 0.3, 22 | pose_publish_period_sec = 5e-3, 23 | trajectory_publish_period_sec = 30e-3, 24 | rangefinder_sampling_ratio = 1., 25 | odometry_sampling_ratio = 1., 26 | fixed_frame_pose_sampling_ratio = 1., 27 | imu_sampling_ratio = 1., 28 | landmarks_sampling_ratio = 1., 29 | } 30 | 31 | MAP_BUILDER.use_trajectory_builder_2d = true 32 | TRAJECTORY_BUILDER_2D.submaps.num_range_data = 20 33 | 34 | TRAJECTORY_BUILDER_2D.min_range = 0.1 35 | TRAJECTORY_BUILDER_2D.max_range = 7 36 | TRAJECTORY_BUILDER_2D.missing_data_ray_length = 7. 37 | TRAJECTORY_BUILDER_2D.use_imu_data = true 38 | TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true 39 | TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.1) 40 | 41 | POSE_GRAPH.constraint_builder.min_score = 0.8 42 | POSE_GRAPH.constraint_builder.global_localization_min_score = 0.8 43 | 44 | return options 45 | -------------------------------------------------------------------------------- /turtle3sim/config/turtlebot3_robot1.lua: -------------------------------------------------------------------------------- 1 | include "map_builder.lua" 2 | include "trajectory_builder.lua" 3 | 4 | options = { 5 | map_builder = MAP_BUILDER, 6 | trajectory_builder = TRAJECTORY_BUILDER, 7 | map_frame = "robot1/map", 8 | tracking_frame = "robot1/base_footprint", -- imu_link, If you are using gazebo, use 'base_footprint' (libgazebo_ros_imu's bug) 9 | published_frame = "robot1/odom", 10 | odom_frame = "robot1/odom", 11 | provide_odom_frame = false, 12 | publish_frame_projected_to_2d = false, 13 | use_odometry = true, 14 | use_nav_sat = false, 15 | use_landmarks = false, 16 | num_laser_scans = 1, 17 | num_multi_echo_laser_scans = 0, 18 | num_subdivisions_per_laser_scan = 1, 19 | num_point_clouds = 0, 20 | lookup_transform_timeout_sec = 0.2, 21 | submap_publish_period_sec = 0.3, 22 | pose_publish_period_sec = 5e-3, 23 | trajectory_publish_period_sec = 30e-3, 24 | rangefinder_sampling_ratio = 1., 25 | odometry_sampling_ratio = 1., 26 | fixed_frame_pose_sampling_ratio = 1., 27 | imu_sampling_ratio = 1., 28 | landmarks_sampling_ratio = 1., 29 | } 30 | 31 | MAP_BUILDER.use_trajectory_builder_2d = true 32 | TRAJECTORY_BUILDER_2D.submaps.num_range_data = 20 33 | 34 | TRAJECTORY_BUILDER_2D.min_range = 0.1 35 | TRAJECTORY_BUILDER_2D.max_range = 7 36 | TRAJECTORY_BUILDER_2D.missing_data_ray_length = 7. 37 | TRAJECTORY_BUILDER_2D.use_imu_data = true 38 | TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true 39 | TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.1) 40 | 41 | POSE_GRAPH.constraint_builder.min_score = 0.8 42 | POSE_GRAPH.constraint_builder.global_localization_min_score = 0.80 43 | 44 | 45 | return options 46 | -------------------------------------------------------------------------------- /turtle3sim/config/turtlebot3_robot2.lua: -------------------------------------------------------------------------------- 1 | include "map_builder.lua" 2 | include "trajectory_builder.lua" 3 | 4 | options = { 5 | map_builder = MAP_BUILDER, 6 | trajectory_builder = TRAJECTORY_BUILDER, 7 | map_frame = "robot2/map", 8 | tracking_frame = "robot2/base_footprint", -- imu_link, If you are using gazebo, use 'base_footprint' (libgazebo_ros_imu's bug) 9 | published_frame = "robot2/odom", 10 | odom_frame = "robot2/odom", 11 | provide_odom_frame = false, 12 | publish_frame_projected_to_2d = false, 13 | use_odometry = true, 14 | use_nav_sat = false, 15 | use_landmarks = false, 16 | num_laser_scans = 1, 17 | num_multi_echo_laser_scans = 0, 18 | num_subdivisions_per_laser_scan = 1, 19 | num_point_clouds = 0, 20 | lookup_transform_timeout_sec = 0.2, 21 | submap_publish_period_sec = 0.3, 22 | pose_publish_period_sec = 5e-3, 23 | trajectory_publish_period_sec = 30e-3, 24 | rangefinder_sampling_ratio = 1., 25 | odometry_sampling_ratio = 1., 26 | fixed_frame_pose_sampling_ratio = 1., 27 | imu_sampling_ratio = 1., 28 | landmarks_sampling_ratio = 1., 29 | } 30 | 31 | MAP_BUILDER.use_trajectory_builder_2d = true 32 | TRAJECTORY_BUILDER_2D.submaps.num_range_data = 20 33 | 34 | TRAJECTORY_BUILDER_2D.min_range = 0.1 35 | TRAJECTORY_BUILDER_2D.max_range = 7 36 | TRAJECTORY_BUILDER_2D.missing_data_ray_length = 7. 37 | TRAJECTORY_BUILDER_2D.use_imu_data = true 38 | TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true 39 | TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.1) 40 | 41 | POSE_GRAPH.constraint_builder.min_score = 0.8 42 | POSE_GRAPH.constraint_builder.global_localization_min_score = 0.8 43 | 44 | 45 | return options 46 | -------------------------------------------------------------------------------- /turtle3sim/launch/simulation_environment/large_env_three_robots.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /turtle3sim/launch/simulation_environment/small_env_three_robots.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /gmm_map_python/launch/visualgmm_realsence.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /gmm_map_python/launch/visualgmm_realsence_2.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /gmm_map_python/launch/static_tf_pubs.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 36 | 37 | -------------------------------------------------------------------------------- /turtle3sim/launch/includes/cartographer_teb.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 19 | 20 | 21 | 22 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | -------------------------------------------------------------------------------- /gmm_map_python/launch/visualgmm_realsence_debug.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 31 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /turtle3sim/launch/includes/move_base_robot.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /turtle3sim/script/ImuChange.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ odom_ekf.py - Version 0.1 2012-07-08 4 | Republish the /robot_pose_ekf/odom_combined topic which is of type 5 | geometry_msgs/PoseWithCovarianceStamped as an equivalent message of 6 | type nav_msgs/Odometry so we can view it in RViz. 7 | Created for the Pi Robot Project: http://www.pirobot.org 8 | Copyright (c) 2012 Patrick Goebel. All rights reserved. 9 | This program is free software; you can redistribute it and/or modify 10 | it under the terms of the GNU General Public License as published by 11 | the Free Software Foundation; either version 2 of the License, or 12 | (at your option) any later version.5 13 | 14 | This program is distributed in the hope that it will be useful, 15 | but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | GNU General Public License for more details at: 18 | 19 | http://www.gnu.org/licenses/gpl.html 20 | 21 | """ 22 | 23 | import roslib 24 | #roslib.load_manifest('rbx1_nav') 25 | import rospy 26 | import getopt 27 | import sys 28 | from sensor_msgs.msg import Imu 29 | 30 | class ImuChange(): 31 | def __init__(self, frame_id = 'base_footprint'): 32 | # Give the node a name 33 | rospy.init_node('ImuChangeHeader', anonymous=False) 34 | self.frame_id = frame_id 35 | 36 | # Publisher of type nav_msgs/Odometry 37 | self.Imu_pub = rospy.Publisher('output', Imu,queue_size=10) 38 | 39 | # Wait for the /odom_combined topic to become available 40 | rospy.wait_for_message('input', Imu) 41 | 42 | # Subscribe to the /odom_combined topic 43 | rospy.Subscriber('input', Imu, self.pub_Imu) 44 | 45 | rospy.loginfo("Publishing combined odometry on /ImuChange") 46 | 47 | def pub_Imu(self, msg): 48 | imu_data = msg 49 | imu_data.header.frame_id = self.frame_id 50 | 51 | self.Imu_pub.publish(imu_data) 52 | 53 | if __name__ == '__main__': 54 | # try: 55 | opts, args = getopt.getopt(sys.argv[1:],"c:") 56 | for opt, arg in opts: 57 | if opt in ("-c"): 58 | self_frame_id = str(arg) 59 | ImuChange(self_frame_id) 60 | rospy.spin() 61 | # except: 62 | # pass 63 | 64 | -------------------------------------------------------------------------------- /turtle3sim/param_teb/teb_local_planner_params_burger.yaml: -------------------------------------------------------------------------------- 1 | TebLocalPlannerROS: 2 | 3 | odom_topic: odom 4 | #odom_topic: /robot_pose_ekf/odom_combined 5 | map_frame: robot1/odom 6 | #map_frame: /odom_combined 7 | 8 | # Trajectory 9 | 10 | teb_autosize: True 11 | dt_ref: 0.45 12 | dt_hysteresis: 0.1 13 | global_plan_overwrite_orientation: True 14 | max_global_plan_lookahead_dist: 3.0 15 | feasibility_check_no_poses: 5 16 | 17 | # Robot 18 | 19 | max_vel_x: 0.5 20 | max_vel_y: 0 #差速导航注释掉此行 21 | max_vel_x_backwards: 0.35 22 | max_vel_theta: 0.4 23 | acc_lim_x: 0.3 24 | acc_lim_y: 0 #差速导航注释掉此行 25 | acc_lim_theta: 0.50 26 | min_turning_radius: 0.0 27 | footprint_model: # types: "point", "circular", "two_circles", "line", "polygon" 28 | #radius: 0.12 # for type "circular" 29 | vertices: [[-0.12, -0.12], [-0.12, 0.12],[0.12, 0.12], [0.12, -0.12]] 30 | #vertices: [[-0.1, -0.26], [-0.1, 0.26],[0.287, 0.26], [0.287, -0.26]] 31 | 32 | # GoalTolerance 33 | 34 | xy_goal_tolerance: 0.2 35 | yaw_goal_tolerance: 0.5 36 | free_goal_vel: False 37 | 38 | # Obstacles 39 | 40 | min_obstacle_dist: 0.15 41 | include_costmap_obstacles: True 42 | costmap_obstacles_behind_robot_dist: 1.0 43 | obstacle_poses_affected: 7 44 | costmap_converter_plugin: "" 45 | costmap_converter_spin_thread: True 46 | costmap_converter_rate: 5 47 | 48 | # Optimization 49 | 50 | no_inner_iterations: 5 51 | no_outer_iterations: 4 52 | optimization_activate: True 53 | optimization_verbose: False 54 | penalty_epsilon: 0.1 55 | weight_max_vel_x: 1 56 | weight_max_vel_y: 0 #差速导航注释掉此行 57 | weight_max_vel_theta: 1 58 | weight_acc_lim_x: 1 59 | weight_acc_lim_y: 0 #差速导航注释掉此行 60 | weight_acc_lim_theta: 0.5 61 | weight_kinematics_nh: 1000 #差速导航将此值改为1000 62 | weight_kinematics_forward_drive: 60 #差速导航将此值改为60 63 | weight_kinematics_turning_radius: 1 64 | weight_optimaltime: 1 65 | weight_obstacle: 50 66 | weight_dynamic_obstacle: 10 # not in use yet 67 | selection_alternative_time_cost: False # not in use yet 68 | 69 | # Homotopy Class Planner 70 | 71 | enable_homotopy_class_planning: False 72 | enable_multithreading: True 73 | simple_exploration: False 74 | max_number_classes: 4 75 | roadmap_graph_no_samples: 15 76 | roadmap_graph_area_width: 5 77 | h_signature_prescaler: 0.5 78 | h_signature_threshold: 0.1 79 | obstacle_keypoint_offset: 0.1 80 | obstacle_heading_threshold: 0.45 81 | visualize_hc_graph: False 82 | -------------------------------------------------------------------------------- /turtle3sim/launch/simulation_environment/large_env_six_robots.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | -------------------------------------------------------------------------------- /gmm_map_python/script/gmm_transform.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | #coding=utf-8 3 | import rospy 4 | import threading 5 | import numpy as np 6 | # import struct 7 | from submap.msg import gmm, gmmlist 8 | from geometry_msgs.msg import TransformStamped, PoseStamped, Pose, PoseArray, Transform 9 | from tf_conversions import posemath 10 | import tf 11 | import sys, getopt 12 | import random 13 | import pickle 14 | import math 15 | import time 16 | # from scipy.spatial.transform import Rotation 17 | from autolab_core import RigidTransform 18 | 19 | pub=None 20 | 21 | def callback(data): 22 | global pub 23 | # print("------------------------------------3") 24 | gmm_result=gmm() 25 | gmm_result.header=data.header 26 | gmm_result.header.frame_id="/map" 27 | 28 | for i in range(0,data.mix_num): 29 | rotation_quaternion = np.asarray([data.pose.orientation.w,data.pose.orientation.x,data.pose.orientation.y,data.pose.orientation.z]) 30 | T=np.asarray([data.pose.position.x,data.pose.position.y,data.pose.position.z]) 31 | T_qua2rota = RigidTransform(rotation_quaternion, T) 32 | T=T.reshape(T.shape[0],1) 33 | # print(T) 34 | R=T_qua2rota.rotation 35 | # print(R) 36 | # R_inv=np.linalg.inv(R) 37 | # R_T_inv=np.linalg.inv(np.transpose(R)) 38 | p_camera=np.asarray([data.x[i],data.y[i],data.z[i]]) 39 | p_camera=p_camera.reshape(p_camera.shape[0],1) 40 | p_result=np.dot(R,p_camera)+T 41 | gmm_result.x.append(p_result[0]) 42 | gmm_result.y.append(p_result[1]) 43 | gmm_result.z.append(p_result[2]) 44 | # print("------------------------------------4") 45 | 46 | covar=np.identity(3) 47 | covar[0,0]=data.x_var[i] 48 | covar[1,1]=data.y_var[i] 49 | covar[2,2]=data.z_var[i] 50 | covar_result=np.dot(R,covar) 51 | covar_result=np.dot(covar_result,np.transpose(R)) 52 | gmm_result.x_var.append(covar_result[0,0]) 53 | gmm_result.y_var.append(covar_result[1,1]) 54 | gmm_result.z_var.append(covar_result[2,2]) 55 | # print("------------------------------------5") 56 | 57 | gmm_result.pose.position.x=0 58 | gmm_result.pose.position.y=0 59 | gmm_result.pose.position.z=0 60 | gmm_result.pose.orientation.x=0 61 | gmm_result.pose.orientation.y=0 62 | gmm_result.pose.orientation.z=0 63 | gmm_result.pose.orientation.w=1 64 | 65 | gmm_result.mix_num=data.mix_num 66 | pub.publish(gmm_result) 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | def main(argv): 75 | # print("------------------------------------1") 76 | global pub 77 | rospy.init_node('talker', anonymous=True) 78 | pub = rospy.Publisher('gmm_after_trans', gmm, queue_size=10) 79 | rospy.Subscriber("subgmm", gmm, callback) 80 | # print("------------------------------------2") 81 | rate = rospy.Rate(10) # 10hz 82 | while not rospy.is_shutdown(): 83 | rospy.spin() 84 | rate.sleep() 85 | 86 | if __name__ == '__main__': 87 | main(sys.argv[1:]) -------------------------------------------------------------------------------- /gmm_map_python/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | gmm_map_python 4 | 0.0.0 5 | The gmm_map_python package 6 | 7 | 8 | 9 | 10 | yujc 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | roscpp 53 | rospy 54 | std_msgs 55 | roscpp 56 | rospy 57 | std_msgs 58 | roscpp 59 | rospy 60 | std_msgs 61 | 62 | message_generation 63 | message_runtime 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | -------------------------------------------------------------------------------- /gmm_map_python/script/visualgmm.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: UTF-8 -*- 3 | 4 | import signal 5 | import sys 6 | import rospy 7 | import copy 8 | 9 | from visualization_msgs.msg import Marker,MarkerArray 10 | from geometry_msgs.msg import Point,Vector3 11 | from geometry_msgs.msg import PoseWithCovarianceStamped 12 | from submap.msg import gmm, gmmlist 13 | 14 | pubVehiclePosition = None 15 | count_cb = 0 16 | 17 | def forcequit(signum, frame): 18 | print '123' 19 | print 'stop fusion' 20 | sys.exit() 21 | 22 | def new_SPHERE_Marker(frame_id, position, scale, id): 23 | marker = Marker() 24 | marker.header.frame_id = frame_id 25 | marker.header.stamp = rospy.Time.now() 26 | marker.id = id # enumerate subsequent markers here 27 | marker.action = Marker.ADD # can be ADD, REMOVE, or MODIFY 28 | marker.ns = "vehicle_model" 29 | marker.type = Marker.SPHERE 30 | 31 | marker.pose.position = position 32 | marker.pose.orientation.x = 0.0 33 | marker.pose.orientation.y = 0.0 34 | marker.pose.orientation.z = 0.0 35 | marker.pose.orientation.w = 1.0 36 | marker.scale = scale 37 | 38 | marker.color.r = 1.0 39 | marker.color.g = 0.50 40 | marker.color.b = 0.0 41 | marker.color.a = 1.0 42 | return marker 43 | 44 | def subgmm_list_cb(data): 45 | global pubVehiclePosition, count_cb 46 | # print(count_cb) 47 | count_cb += 1 48 | markerarray = MarkerArray() 49 | for index in range(len(data.data)): 50 | cu_gmm = data.data[index] 51 | cu_mix_num = cu_gmm.mix_num 52 | for jndex in range(cu_mix_num): 53 | marker = new_SPHERE_Marker("map",Point(cu_gmm.x[jndex],cu_gmm.y[jndex],cu_gmm.z[jndex]), 54 | Vector3(10*cu_gmm.x_var[jndex],10*cu_gmm.y_var[jndex],10*cu_gmm.z_var[jndex]), 55 | jndex + index*1000 ) 56 | markerarray.markers.append(marker) 57 | # print(markerarray) 58 | pubVehiclePosition.publish(markerarray) 59 | 60 | 61 | def subgmm_cb(data): 62 | global pubVehiclePosition, count_cb 63 | # print(count_cb) 64 | count_cb += 1 65 | markerarray = MarkerArray() 66 | cu_gmm = data 67 | cu_mix_num = cu_gmm.mix_num 68 | for jndex in range(cu_mix_num): 69 | marker = new_SPHERE_Marker("map",Point(cu_gmm.x[jndex],cu_gmm.y[jndex],cu_gmm.z[jndex]), 70 | Vector3(10*cu_gmm.x_var[jndex],10*cu_gmm.y_var[jndex],10*cu_gmm.z_var[jndex]), 71 | jndex ) 72 | markerarray.markers.append(marker) 73 | # print(markerarray) 74 | pubVehiclePosition.publish(markerarray) 75 | 76 | 77 | 78 | 79 | def main(): 80 | global pubVehiclePosition 81 | signal.signal(signal.SIGINT, forcequit) 82 | signal.signal(signal.SIGTERM, forcequit) 83 | rospy.init_node('visual_gmm', anonymous=True) 84 | rate = rospy.Rate(5) 85 | pubVehiclePosition = rospy.Publisher('vehicle_robot_position', MarkerArray, queue_size=10) 86 | sub_list_subgmm = rospy.Subscriber('subgmm_list_tmp',gmmlist, subgmm_list_cb,queue_size=100) 87 | sub_subgmm = rospy.Subscriber('GMMglobal',gmm, subgmm_cb,queue_size=100) 88 | 89 | 90 | # markerarray = MarkerArray( 91 | 92 | # for i in range(100): 93 | # rate.sleep() 94 | # print("OK" + str(i) ) 95 | 96 | rospy.spin() 97 | 98 | if __name__ == "__main__": 99 | main() -------------------------------------------------------------------------------- /gmm_map_python/script/generate_descriptor.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import os 3 | os.environ["CUDA_VISIBLE_DEVICES"] = "0" 4 | import torch 5 | from torch.autograd import Variable 6 | from torch.backends import cudnn 7 | 8 | # from loading_pointclouds import * 9 | import PointNetVlad as PNV 10 | 11 | import config as cfg 12 | 13 | cudnn.enabled = True 14 | 15 | device = torch.device("cuda" if torch.cuda.is_available() else "cpu") 16 | 17 | class Descriptor: 18 | def __init__(self, point_cloud=None, point_num=4096,model_dir=None): 19 | self.model = PNV.PointNetVlad(global_feat=True, feature_transform=True,\ 20 | max_pool=False, output_dim=cfg.FEATURE_OUTPUT_DIM, num_points=cfg.NUM_POINTS) 21 | self.model.to(device) 22 | print(device) 23 | self.point_cloud = point_cloud 24 | self.point_num = point_num 25 | self.filtered_pc = None 26 | # self.filter() 27 | print("load model weights from {}\n".format(model_dir)) 28 | if model_dir: 29 | model_dict = torch.load(model_dir) 30 | self.model.load_state_dict(model_dict) 31 | #self.filter() 32 | def filter(self): 33 | print("point_cloud: {}\n".format(self.point_cloud.shape[0])) 34 | idx = np.random.choice(self.point_cloud.shape[0], self.point_num, replace=True) 35 | self.filtered_pc = self.point_cloud[idx, :] 36 | mean = np.mean(self.filtered_pc, axis=0) 37 | mean = mean[np.newaxis, :] 38 | self.filtered_pc -= mean 39 | self.filtered_pc /= np.max(np.abs(self.filtered_pc)) 40 | return self.filtered_pc 41 | # nums = 0 42 | # idx = list() 43 | # # points = copy.copy(self.point_cloud) 44 | # filtered_points = np.zeros((self.point_num, 3)) 45 | # dist = np.zeros(self.point_cloud.shape[0]) + float("inf") 46 | # dist[0] = 0 47 | # idx.append(0) 48 | # # filtered_points[0, :] = self.point_cloud[0, :] 49 | # nums += 1 50 | # while nums < self.point_num: 51 | # for i in range(self.point_cloud.shape[0] - nums): 52 | # if i in idx: 53 | # continue 54 | # dist = min(np.linalg.norm(self.point_cloud - self.point_cloud[idx[nums-1, :]], axis=1) 55 | # dist[i] = min(np.linalg.norm(self.point_cloud[i, :] - filtered_points[nums - 1, :]), dist[i]) 56 | 57 | # s = np.argsort(dist) 58 | # for i in np.arange(self.point_cloud.shape[0]-1, -1, -1): 59 | # if s[i] in idx: 60 | # continue 61 | # idx.append(s[i]) 62 | # nums+=1 63 | # break 64 | # print(nums) 65 | # return self.point_cloud[idx, :] 66 | 67 | def generate_descriptor(self): 68 | model_in = Variable(torch.tensor(self.filtered_pc.reshape((1, 1, self.point_num, 3)), dtype=torch.float)).to(device) 69 | # print(model_in.shape) 70 | self.model.eval() 71 | with torch.no_grad(): 72 | model_out = self.model(model_in) 73 | return model_out.cpu() 74 | 75 | 76 | if __name__ == '__main__': 77 | pc = np.loadtxt('pc.txt') 78 | # pc = pc[1:10000, :] 79 | 80 | D = Descriptor(point_cloud=pc, model_dir='model13.ckpt') 81 | print(D.generate_descriptor()) 82 | 83 | 84 | 85 | 86 | -------------------------------------------------------------------------------- /turtle3sim/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | turtlebot3sim 4 | 0.0.0 5 | The mapmerge package 6 | 7 | 8 | 9 | 10 | jimmy 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | geometry_msgs 53 | message_generation 54 | nav_msgs 55 | roscpp 56 | rospy 57 | std_msgs 58 | tf 59 | visualization_msgs 60 | geometry_msgs 61 | nav_msgs 62 | roscpp 63 | rospy 64 | std_msgs 65 | tf 66 | visualization_msgs 67 | geometry_msgs 68 | nav_msgs 69 | roscpp 70 | rospy 71 | std_msgs 72 | tf 73 | visualization_msgs 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | -------------------------------------------------------------------------------- /Readme.md: -------------------------------------------------------------------------------- 1 | # MR-GMMapping 2 | 3 | This is the open-source project **MR-GMMapping**, a communication efficient **M**ulti-**R**obot **GMM**-based **Mapping** system. 4 | 5 | The related paper "MR-GMMapping: Communication Efficient Multi-Robot Mapping System via Gaussian Mixture Model" is submitted to the IEEE Robotics and Automation Letters (RA-L) with the 2022 International Conference on Robotics and Automation（ICRA 2022）. 6 | 7 | The video demo is resleased at [bilibili (Chinese Youtube)](https://www.bilibili.com/video/BV1TU4y1N7Yn?from=search&seid=3677956896739468342&spm_id_from=333.337.0.0) and [Youtube](https://www.youtube.com/watch?v=Ut77xZi6cXo) 8 | 9 | ## Platform 10 | - Multi-robots with NVIDIA Jetson TX2, Intel RealSense T265, and depth camera D435i 11 | - ubuntu 18.04 (bash) 12 | - ROS melodic 13 | - python2 14 | 15 | ## Dependency 16 | 17 | 19 | ### Pytorch for Place Recognition 20 | ``` 21 | pip install torch torchvision 22 | ``` 23 | 24 | Pre-trained model is available [here](https://cloud.tsinghua.edu.cn/f/cb267e85967744eaac5e/) 25 | Then change the regarding path of `model13.ckpt` in `MapBuilderNode.py`. 26 | 27 | ### Python-PCL 28 | 29 | Useful reference [here](https://python-pcl-fork.readthedocs.io/en/rc_patches4/install.html#install-python-pcl). 30 | 31 | ### GTSAM 32 | 33 | ``` 34 | git clone https://bitbucket.org/gtborg/gtsam.git 35 | mkdir build 36 | cd build 37 | cmake .. -DGTSAM_INSTALL_CYTHON_TOOLBOX=on 38 | make check 39 | make install 40 | cd cython 41 | sudo python setup.py install 42 | ``` 43 | 44 | ### Other dependency 45 | ``` 46 | sudo apt install ros-melodic-tf2-ros 47 | pip install autolab_core 48 | pip install sklearn 49 | pip install future 50 | pip install transforms3d 51 | pip install pygraph 52 | ``` 53 | 54 | For ROS libraries, you can use ```apt install ros-melodic-(missing library)``` to install the missing libraries. 55 | 56 | ## Datasets 57 | 58 | The ROS bags of the multi-robot simulators can be downloaded [here](https://cloud.tsinghua.edu.cn/f/fa70b8d8be1d4aa69eec/). 59 | 60 | You can also use the keyboard to control the robots in the Gazebo simulator by running 61 | ``` 62 | roslaunch turtlebot3sim small_env_two_robots.launch 63 | roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch 64 | ``` 65 | 66 | ## Usage Example 67 | 68 | ### Installation 69 | download the repo and then put it in your ros package 70 | ``` 71 | catkin_make 72 | source /devel/setup.bash 73 | ``` 74 | 75 | 76 | ### Single Robot Map Building 77 | ``` 78 | roslaunch gmm_map_python visualgmm_realsence.launch 79 | ``` 80 | 81 | 82 | ### Multi-robot Relative Pose Estimation 83 | ``` 84 | roslaunch gmm_map_python visualgmm_realsence_2robot.launch 85 | ``` 86 | If you want to run this system with your simulator, there are three params need to adjust for better performance: 87 | - `match_thr`: small `match_thr` reduce the probability of scene mismatch。 88 | - `fitness_thr`: smaller `fitness_thr` leads to a better global map accuracy after the first map merging. 89 | - `new_self_submap_count`: adjust according to the camera parameters. Larger `new_self_submap_count` is better for place recognition, but will bring longer mapping delay. 90 | 91 | 92 | 116 | -------------------------------------------------------------------------------- /gmm_map_python/src/featureextraction.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include "gmm_map_python/FeatureExtraction.h" 16 | using namespace std; 17 | 18 | bool FeatureExtract(gmm_map_python::FeatureExtraction::Request &img,gmm_map_python::FeatureExtraction::Response &res){ 19 | pcl::PointCloud cloud_input_; 20 | pcl::fromROSMsg(img.input_cloud, cloud_input_); 21 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud); 22 | cloud = cloud_input_.makeShared(); 23 | std::cout << "points sieze is:" << cloud->size() << std::endl; 24 | pcl::PointCloud::Ptr normals(new pcl::PointCloud); 25 | pcl::PointCloud boundaries; 26 | pcl::BoundaryEstimation est; 27 | pcl::search::KdTree::Ptr tree(new pcl::search::KdTree()); 28 | pcl::KdTreeFLANN kdtree; //创建一个快速k近邻查询,查询的时候若该点在点云中，则第一个近邻点是其本身 29 | kdtree.setInputCloud(cloud); 30 | int k =2; 31 | float everagedistance =0; 32 | for (int i =0; i < cloud->size()/2;i++) 33 | { 34 | vector nnh ; 35 | vector squaredistance; 36 | // pcl::PointXYZ p; 37 | // p = cloud->points[i]; 38 | kdtree.nearestKSearch(cloud->points[i],k,nnh,squaredistance); 39 | everagedistance += sqrt(squaredistance[1]); 40 | // cout<size()/2); 43 | cout<<"everage distance is : "< normEst; //其中pcl::PointXYZ表示输入类型数据，pcl::Normal表示输出类型,且pcl::Normal前三项是法向，最后一项是曲率 45 | normEst.setInputCloud(cloud); 46 | normEst.setSearchMethod(tree); 47 | // normEst.setRadiusSearch(2); //法向估计的半径 48 | normEst.setKSearch(9); //法向估计的点数 49 | normEst.compute(*normals); 50 | cout << "normal size is " << normals->size() << endl; 51 | //normal_est.setViewPoint(0,0,0); //这个应该会使法向一致 52 | est.setInputCloud(cloud); 53 | est.setInputNormals(normals); 54 | // est.setAngleThreshold(90); 55 | // est.setSearchMethod (pcl::search::KdTree::Ptr (new pcl::search::KdTree)); 56 | est.setSearchMethod(tree); 57 | est.setKSearch(50); //一般这里的数值越高，最终边界识别的精度越好 58 | // est.setRadiusSearch(everagedistance); //搜索半径 59 | est.compute(boundaries); 60 | // pcl::PointCloud boundPoints; 61 | pcl::PointCloud::Ptr boundPoints(new pcl::PointCloud); 62 | pcl::PointCloud noBoundPoints; 63 | int countBoundaries = 0; 64 | for (int i = 0; i < cloud->size(); i++) { 65 | uint8_t x = (boundaries.points[i].boundary_point); 66 | int a = static_cast(x); //该函数的功能是强制类型转换 67 | if (a == 1) 68 | { 69 | // boundPoints.push_back(cloud->points[i]); 70 | (*boundPoints).push_back(cloud->points[i]); 71 | countBoundaries++; 72 | } 73 | else 74 | noBoundPoints.push_back(cloud->points[i]); 75 | 76 | } 77 | std::cout << "boudary size is：" << countBoundaries << std::endl; 78 | sensor_msgs::PointCloud2 img_tmp; 79 | pcl::toROSMsg(*boundPoints,img_tmp); 80 | res.output_cloud=img_tmp; 81 | return true; 82 | } 83 | 84 | int main(int argc, char** argv) 85 | { 86 | std::cout<< "Service init Begin!" < 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 162 | 163 | 164 | 165 | 166 | -------------------------------------------------------------------------------- /gmm_map_python/src/downsample_cloud.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include "math.h" 10 | #include // give variable the biggest number 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | 25 | #include 26 | #include 27 | #include 28 | 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | using namespace std; 39 | 40 | 41 | 42 | class DownSamplePointCloud2{ 43 | public: 44 | void initFilter(ros::NodeHandle nh); 45 | void pointcloudCallback(const sensor_msgs::PointCloud2 img); 46 | private: 47 | ros::Subscriber point_sub_; 48 | ros::Publisher point_pub_; 49 | pcl::VoxelGrid sor_; 50 | pcl::PointCloud cloud_input_; 51 | ros::Time current_time = ros::Time(0.1); 52 | 53 | }; 54 | 55 | void DownSamplePointCloud2::pointcloudCallback(sensor_msgs::PointCloud2 img){ 56 | std::cout<< ros::Time::now() -img.header.stamp < indices; 65 | pcl::removeNaNFromPointCloud(cloud_input_,cloud_input_, indices); 66 | sor_.setInputCloud(cloud_input_.makeShared()); 67 | sor_.setLeafSize(0.1f, 0.1f, 0.1f); 68 | sor_.filter(cloud_input_); 69 | 70 | // //边缘检测 71 | // pcl::PointCloud::Ptr cloud(new pcl::PointCloud); 72 | // cloud = cloud_input_.makeShared(); 73 | // std::cout << "points sieze is:" << cloud->size() << std::endl; 74 | // pcl::PointCloud::Ptr normals(new pcl::PointCloud); 75 | // pcl::PointCloud boundaries; 76 | // pcl::BoundaryEstimation est; 77 | // pcl::search::KdTree::Ptr tree(new pcl::search::KdTree()); 78 | // pcl::KdTreeFLANN kdtree; //创建一个快速k近邻查询,查询的时候若该点在点云中，则第一个近邻点是其本身 79 | // kdtree.setInputCloud(cloud); 80 | // int k =2; 81 | // float everagedistance =0; 82 | // for (int i =0; i < cloud->size()/2;i++) 83 | // { 84 | // vector nnh ; 85 | // vector squaredistance; 86 | // // pcl::PointXYZ p; 87 | // // p = cloud->points[i]; 88 | // kdtree.nearestKSearch(cloud->points[i],k,nnh,squaredistance); 89 | // everagedistance += sqrt(squaredistance[1]); 90 | // // cout<size()/2); 93 | // cout<<"everage distance is : "< normEst; //其中pcl::PointXYZ表示输入类型数据，pcl::Normal表示输出类型,且pcl::Normal前三项是法向，最后一项是曲率 95 | // normEst.setInputCloud(cloud); 96 | // normEst.setSearchMethod(tree); 97 | // // normEst.setRadiusSearch(2); //法向估计的半径 98 | // normEst.setKSearch(9); //法向估计的点数 99 | // normEst.compute(*normals); 100 | // cout << "normal size is " << normals->size() << endl; 101 | // //normal_est.setViewPoint(0,0,0); //这个应该会使法向一致 102 | // est.setInputCloud(cloud); 103 | // est.setInputNormals(normals); 104 | // // est.setAngleThreshold(90); 105 | // // est.setSearchMethod (pcl::search::KdTree::Ptr (new pcl::search::KdTree)); 106 | // est.setSearchMethod(tree); 107 | // est.setKSearch(50); //一般这里的数值越高，最终边界识别的精度越好 108 | // // est.setRadiusSearch(everagedistance); //搜索半径 109 | // est.compute(boundaries); 110 | // // pcl::PointCloud boundPoints; 111 | // pcl::PointCloud::Ptr boundPoints(new pcl::PointCloud); 112 | // pcl::PointCloud noBoundPoints; 113 | // int countBoundaries = 0; 114 | // for (int i = 0; i < cloud->size(); i++) { 115 | // uint8_t x = (boundaries.points[i].boundary_point); 116 | // int a = static_cast(x); //该函数的功能是强制类型转换 117 | // if (a == 1) 118 | // { 119 | // // boundPoints.push_back(cloud->points[i]); 120 | // (*boundPoints).push_back(cloud->points[i]); 121 | // countBoundaries++; 122 | // } 123 | // else 124 | // noBoundPoints.push_back(cloud->points[i]); 125 | 126 | // } 127 | // std::cout << "boudary size is：" << countBoundaries << std::endl; 128 | 129 | //实车 观测范围滤波 130 | // pcl::PassThrough pass; 131 | // pass.setInputCloud(cloud_input_.makeShared()); 132 | // pass.setFilterFieldName("x"); 133 | // pass.setFilterLimits(-1.5,1.5); 134 | // pass.setFilterFieldName("y"); 135 | // pass.setFilterLimits(-1.5,1.5); 136 | // pass.setFilterFieldName("z"); 137 | // pass.setFilterLimits(-1.5,1.5); 138 | // pass.filter(cloud_input_); 139 | 140 | //点云转消息 141 | sensor_msgs::PointCloud2 img_tmp; 142 | // pcl::toROSMsg(*boundPoints,img_tmp); 143 | pcl::toROSMsg(cloud_input_,img_tmp);// to do: add other info, like tf, to this msg 144 | img_tmp.header=img.header; 145 | std::cout << "ros time" << img_tmp.header.stamp << std::endl; 146 | std::cout << "ros now time" << ros::Time::now() << std::endl; 147 | point_pub_.publish(img_tmp); 148 | 149 | } 150 | 151 | void DownSamplePointCloud2::initFilter(ros::NodeHandle nh){ 152 | point_sub_ = nh.subscribe("points", 0, &DownSamplePointCloud2::pointcloudCallback,this); 153 | point_pub_ = nh.advertise("sampled_points", 0,this); 154 | } 155 | 156 | int main(int argc, char** argv) 157 | { 158 | ros::init(argc, argv, "DownSampleNode"); 159 | // ros::NodeHandle node; 160 | ros::NodeHandle nh("~"); 161 | std::cout<<"Robot1 begin"<2): 160 | index_input=distance[0][0:merge_num] #合并的序号 161 | index_input.tolist() 162 | index_input = list(map(lambda x: int(x), index_input)) 163 | 164 | prior_tmp=self.weights[index_input] 165 | prior_tmp=prior_tmp.reshape((prior_tmp.shape[0],1)) 166 | # print(self.means[index_input][0],prior_tmp) 167 | w0=sum(self.weights[index_input]) 168 | mu0_x=sum(self.means[index_input][0]*prior_tmp[0])/w0 169 | mu0_y=sum(self.means[index_input][1]*prior_tmp[1])/w0 170 | mu0_z=sum(self.means[index_input][2]*prior_tmp[2])/w0 171 | sigma0_x=sum(prior_tmp[0]*(self.covariances[index_input][0]+pow(self.means[index_input][0]-mu0_x,2))) 172 | sigma0_y=sum(prior_tmp[1]*(self.covariances[index_input][1]+pow(self.means[index_input][1]-mu0_y,2))) 173 | sigma0_z=sum(prior_tmp[2]*(self.covariances[index_input][2]+pow(self.means[index_input][2]-mu0_z,2))) 174 | # sigma0_z=max(self.covariances[index_input][2]) 175 | # print(w0,mu0_x,mu0_y,mu0_z,sigma0_x,sigma0_y,sigma0_z) 176 | prior_tmp=self.weights.copy() 177 | means_tmp=self.means.copy() 178 | var_tmp=self.covariances.copy() 179 | prior_tmp=np.append(prior_tmp,[[float(w0)]],axis=0) 180 | means_tmp=np.append(means_tmp,[[float(mu0_x),float(mu0_y),float(mu0_z)]],axis=0) 181 | var_tmp=np.append(var_tmp,[[float(sigma0_x),float(sigma0_x),float(sigma0_x)]],axis=0) 182 | # print("after append shape:",prior_tmp.shape,means_tmp.shape,var_tmp.shape) 183 | prior_tmp=np.delete(prior_tmp,index_input,axis=0) 184 | means_tmp=np.delete(means_tmp,index_input,axis=0) 185 | var_tmp=np.delete(var_tmp,index_input,axis=0) 186 | # print("after delete shape:",prior_tmp.shape,means_tmp.shape,var_tmp.shape) 187 | mix=self.mix_num 188 | self.mix_num=self.mix_num-merge_num+1 189 | self.weights=np.array(prior_tmp) 190 | self.means=np.array(means_tmp) 191 | self.covariances=np.array(var_tmp) 192 | # print(self.mix_num,self.weights.shape,self.means.shape,self.covariances.shape) 193 | return mix, self.mix_num 194 | -------------------------------------------------------------------------------- /gmm_map_python/script/PointNetVlad.py: -------------------------------------------------------------------------------- 1 | from __future__ import print_function 2 | import torch 3 | import torch.nn as nn 4 | import torch.nn.parallel 5 | import torch.utils.data 6 | from torch.autograd import Variable 7 | import numpy as np 8 | import torch.nn.functional as F 9 | import math 10 | 11 | 12 | class NetVLADLoupe(nn.Module): 13 | def __init__(self, feature_size, max_samples, cluster_size, output_dim, 14 | gating=True, add_batch_norm=True, is_training=True): 15 | super(NetVLADLoupe, self).__init__() 16 | self.feature_size = feature_size 17 | self.max_samples = max_samples 18 | self.output_dim = output_dim 19 | self.is_training = is_training 20 | self.gating = gating 21 | self.add_batch_norm = add_batch_norm 22 | self.cluster_size = cluster_size 23 | self.softmax = nn.Softmax(dim=-1) 24 | self.cluster_weights = nn.Parameter(torch.randn( 25 | feature_size, cluster_size) * 1 / math.sqrt(feature_size)) 26 | self.cluster_weights2 = nn.Parameter(torch.randn( 27 | 1, feature_size, cluster_size) * 1 / math.sqrt(feature_size)) 28 | self.hidden1_weights = nn.Parameter( 29 | torch.randn(cluster_size * feature_size, output_dim) * 1 / math.sqrt(feature_size)) 30 | 31 | if add_batch_norm: 32 | self.cluster_biases = None 33 | self.bn1 = nn.BatchNorm1d(cluster_size) 34 | else: 35 | self.cluster_biases = nn.Parameter(torch.randn( 36 | cluster_size) * 1 / math.sqrt(feature_size)) 37 | self.bn1 = None 38 | 39 | self.bn2 = nn.BatchNorm1d(output_dim) 40 | 41 | if gating: 42 | self.context_gating = GatingContext( 43 | output_dim, add_batch_norm=add_batch_norm) 44 | 45 | def forward(self, x): 46 | x = x.transpose(1, 3).contiguous() 47 | x = x.view((-1, self.max_samples, self.feature_size)) 48 | activation = torch.matmul(x, self.cluster_weights) 49 | if self.add_batch_norm: 50 | # activation = activation.transpose(1,2).contiguous() 51 | activation = activation.view(-1, self.cluster_size) 52 | activation = self.bn1(activation) 53 | activation = activation.view(-1, 54 | self.max_samples, self.cluster_size) 55 | # activation = activation.transpose(1,2).contiguous() 56 | else: 57 | activation = activation + self.cluster_biases 58 | activation = self.softmax(activation) 59 | activation = activation.view((-1, self.max_samples, self.cluster_size)) 60 | 61 | a_sum = activation.sum(-2, keepdim=True) 62 | a = a_sum * self.cluster_weights2 63 | 64 | activation = torch.transpose(activation, 2, 1) 65 | x = x.view((-1, self.max_samples, self.feature_size)) 66 | vlad = torch.matmul(activation, x) 67 | vlad = torch.transpose(vlad, 2, 1) 68 | vlad = vlad - a 69 | 70 | vlad = F.normalize(vlad, dim=1, p=2) 71 | vlad = vlad.contiguous().view((-1, self.cluster_size * self.feature_size)) 72 | vlad = F.normalize(vlad, dim=1, p=2) 73 | 74 | vlad = torch.matmul(vlad, self.hidden1_weights) 75 | 76 | vlad = self.bn2(vlad) 77 | 78 | if self.gating: 79 | vlad = self.context_gating(vlad) 80 | 81 | return vlad 82 | 83 | 84 | class GatingContext(nn.Module): 85 | def __init__(self, dim, add_batch_norm=True): 86 | super(GatingContext, self).__init__() 87 | self.dim = dim 88 | self.add_batch_norm = add_batch_norm 89 | self.gating_weights = nn.Parameter( 90 | torch.randn(dim, dim) * 1 / math.sqrt(dim)) 91 | self.sigmoid = nn.Sigmoid() 92 | 93 | if add_batch_norm: 94 | self.gating_biases = None 95 | self.bn1 = nn.BatchNorm1d(dim) 96 | else: 97 | self.gating_biases = nn.Parameter( 98 | torch.randn(dim) * 1 / math.sqrt(dim)) 99 | self.bn1 = None 100 | 101 | def forward(self, x): 102 | gates = torch.matmul(x, self.gating_weights) 103 | 104 | if self.add_batch_norm: 105 | gates = self.bn1(gates) 106 | else: 107 | gates = gates + self.gating_biases 108 | 109 | gates = self.sigmoid(gates) 110 | 111 | activation = x * gates 112 | 113 | return activation 114 | 115 | 116 | class Flatten(nn.Module): 117 | def __init__(self): 118 | nn.Module.__init__(self) 119 | 120 | def forward(self, input): 121 | return input.view(input.size(0), -1) 122 | 123 | 124 | class STN3d(nn.Module): 125 | def __init__(self, num_points=2500, k=3, use_bn=True): 126 | super(STN3d, self).__init__() 127 | self.k = k 128 | self.kernel_size = 3 if k == 3 else 1 129 | self.channels = 1 if k == 3 else k 130 | self.num_points = num_points 131 | self.use_bn = use_bn 132 | self.conv1 = torch.nn.Conv2d(self.channels, 64, (1, self.kernel_size)) 133 | self.conv2 = torch.nn.Conv2d(64, 128, (1,1)) 134 | self.conv3 = torch.nn.Conv2d(128, 1024, (1,1)) 135 | self.mp1 = torch.nn.MaxPool2d((num_points, 1), 1) 136 | self.fc1 = nn.Linear(1024, 512) 137 | self.fc2 = nn.Linear(512, 256) 138 | self.fc3 = nn.Linear(256, k*k) 139 | self.fc3.weight.data.zero_() 140 | self.fc3.bias.data.zero_() 141 | self.relu = nn.ReLU() 142 | 143 | if use_bn: 144 | self.bn1 = nn.BatchNorm2d(64) 145 | self.bn2 = nn.BatchNorm2d(128) 146 | self.bn3 = nn.BatchNorm2d(1024) 147 | self.bn4 = nn.BatchNorm1d(512) 148 | self.bn5 = nn.BatchNorm1d(256) 149 | 150 | def forward(self, x): 151 | batchsize = x.size()[0] 152 | if self.use_bn: 153 | x = F.relu(self.bn1(self.conv1(x))) 154 | x = F.relu(self.bn2(self.conv2(x))) 155 | x = F.relu(self.bn3(self.conv3(x))) 156 | else: 157 | x = F.relu(self.conv1(x)) 158 | x = F.relu(self.conv2(x)) 159 | x = F.relu(self.conv3(x)) 160 | x = self.mp1(x) 161 | x = x.view(-1, 1024) 162 | 163 | if self.use_bn: 164 | x = F.relu(self.bn4(self.fc1(x))) 165 | x = F.relu(self.bn5(self.fc2(x))) 166 | else: 167 | x = F.relu(self.fc1(x)) 168 | x = F.relu(self.fc2(x)) 169 | x = self.fc3(x) 170 | 171 | iden = Variable(torch.from_numpy(np.eye(self.k).astype(np.float32))).view( 172 | 1, self.k*self.k).repeat(batchsize, 1) 173 | if x.is_cuda: 174 | iden = iden.cuda() 175 | x = x + iden 176 | x = x.view(-1, self.k, self.k) 177 | return x 178 | 179 | 180 | class PointNetfeat(nn.Module): 181 | def __init__(self, num_points=2500, global_feat=True, feature_transform=False, max_pool=True): 182 | super(PointNetfeat, self).__init__() 183 | self.stn = STN3d(num_points=num_points, k=3, use_bn=False) 184 | self.feature_trans = STN3d(num_points=num_points, k=64, use_bn=False) 185 | self.apply_feature_trans = feature_transform 186 | self.conv1 = torch.nn.Conv2d(1, 64, (1, 3)) 187 | self.conv2 = torch.nn.Conv2d(64, 64, (1, 1)) 188 | self.conv3 = torch.nn.Conv2d(64, 64, (1, 1)) 189 | self.conv4 = torch.nn.Conv2d(64, 128, (1, 1)) 190 | self.conv5 = torch.nn.Conv2d(128, 1024, (1, 1)) 191 | self.bn1 = nn.BatchNorm2d(64) 192 | self.bn2 = nn.BatchNorm2d(64) 193 | self.bn3 = nn.BatchNorm2d(64) 194 | self.bn4 = nn.BatchNorm2d(128) 195 | self.bn5 = nn.BatchNorm2d(1024) 196 | self.mp1 = torch.nn.MaxPool2d((num_points, 1), 1) 197 | self.num_points = num_points 198 | self.global_feat = global_feat 199 | self.max_pool = max_pool 200 | 201 | def forward(self, x): 202 | batchsize = x.size()[0] 203 | trans = self.stn(x) 204 | x = torch.matmul(torch.squeeze(x), trans) 205 | x = x.view(batchsize, 1, -1, 3) 206 | #x = x.transpose(2,1) 207 | #x = torch.bmm(x, trans) 208 | #x = x.transpose(2,1) 209 | x = F.relu(self.bn1(self.conv1(x))) 210 | x = F.relu(self.bn2(self.conv2(x))) 211 | pointfeat = x 212 | if self.apply_feature_trans: 213 | f_trans = self.feature_trans(x) 214 | x = torch.squeeze(x) 215 | if batchsize == 1: 216 | x = torch.unsqueeze(x, 0) 217 | x = torch.matmul(x.transpose(1, 2), f_trans) 218 | x = x.transpose(1, 2).contiguous() 219 | x = x.view(batchsize, 64, -1, 1) 220 | x = F.relu(self.bn3(self.conv3(x))) 221 | x = F.relu(self.bn4(self.conv4(x))) 222 | x = self.bn5(self.conv5(x)) 223 | if not self.max_pool: 224 | return x 225 | else: 226 | x = self.mp1(x) 227 | x = x.view(-1, 1024) 228 | if self.global_feat: 229 | return x, trans 230 | else: 231 | x = x.view(-1, 1024, 1).repeat(1, 1, self.num_points) 232 | return torch.cat([x, pointfeat], 1), trans 233 | 234 | 235 | class PointNetVlad(nn.Module): 236 | def __init__(self, num_points=2500, global_feat=True, feature_transform=False, max_pool=True, output_dim=1024): 237 | super(PointNetVlad, self).__init__() 238 | self.point_net = PointNetfeat(num_points=num_points, global_feat=global_feat, 239 | feature_transform=feature_transform, max_pool=max_pool) 240 | self.net_vlad = NetVLADLoupe(feature_size=1024, max_samples=num_points, cluster_size=64, 241 | output_dim=output_dim, gating=True, add_batch_norm=True, 242 | is_training=True) 243 | 244 | def forward(self, x): 245 | x = self.point_net(x) 246 | x = self.net_vlad(x) 247 | return x 248 | 249 | 250 | if __name__ == '__main__': 251 | num_points = 4096 252 | sim_data = Variable(torch.rand(44, 1, num_points, 3)) 253 | sim_data = sim_data.cuda() 254 | 255 | pnv = PointNetVlad.PointNetVlad(global_feat=True, feature_transform=True, max_pool=False, 256 | output_dim=256, num_points=num_points).cuda() 257 | pnv.train() 258 | out3 = pnv(sim_data) 259 | print('pnv', out3.size()) 260 | -------------------------------------------------------------------------------- /gmm_map_python/rviz/robot1.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /PointCloud21 10 | - /PointCloud22 11 | - /Pose1 12 | - /Pose2 13 | - /Path1 14 | - /OccupancyGrid1 15 | - /Path2 16 | - /Path3 17 | Splitter Ratio: 0.5 18 | Tree Height: 657 19 | - Class: rviz/Selection 20 | Name: Selection 21 | - Class: rviz/Tool Properties 22 | Expanded: 23 | - /2D Pose Estimate1 24 | - /2D Nav Goal1 25 | - /Publish Point1 26 | Name: Tool Properties 27 | Splitter Ratio: 0.5886790156364441 28 | - Class: rviz/Views 29 | Expanded: 30 | - /Current View1 31 | Name: Views 32 | Splitter Ratio: 0.5 33 | - Class: rviz/Time 34 | Experimental: false 35 | Name: Time 36 | SyncMode: 0 37 | SyncSource: PointCloud2 38 | Preferences: 39 | PromptSaveOnExit: true 40 | Toolbars: 41 | toolButtonStyle: 2 42 | Visualization Manager: 43 | Class: "" 44 | Displays: 45 | - Alpha: 0.5 46 | Cell Size: 1 47 | Class: rviz/Grid 48 | Color: 160; 160; 164 49 | Enabled: true 50 | Line Style: 51 | Line Width: 0.029999999329447746 52 | Value: Lines 53 | Name: Grid 54 | Normal Cell Count: 0 55 | Offset: 56 | X: 0 57 | Y: 0 58 | Z: 0 59 | Plane: XY 60 | Plane Cell Count: 10 61 | Reference Frame: 62 | Value: true 63 | - Alpha: 1 64 | Autocompute Intensity Bounds: true 65 | Autocompute Value Bounds: 66 | Max Value: 10 67 | Min Value: -10 68 | Value: true 69 | Axis: Z 70 | Channel Name: intensity 71 | Class: rviz/PointCloud2 72 | Color: 255; 255; 255 73 | Color Transformer: Intensity 74 | Decay Time: 0 75 | Enabled: false 76 | Invert Rainbow: false 77 | Max Color: 255; 255; 255 78 | Max Intensity: 4096 79 | Min Color: 0; 0; 0 80 | Min Intensity: 0 81 | Name: PointCloud2 82 | Position Transformer: XYZ 83 | Queue Size: 10 84 | Selectable: true 85 | Size (Pixels): 3 86 | Size (m): 0.009999999776482582 87 | Style: Flat Squares 88 | Topic: /robot1/Downsample/sampled_points 89 | Unreliable: false 90 | Use Fixed Frame: true 91 | Use rainbow: true 92 | Value: false 93 | - Alpha: 1 94 | Autocompute Intensity Bounds: true 95 | Autocompute Value Bounds: 96 | Max Value: 10 97 | Min Value: -10 98 | Value: true 99 | Axis: Z 100 | Channel Name: intensity 101 | Class: rviz/PointCloud2 102 | Color: 255; 255; 255 103 | Color Transformer: Intensity 104 | Decay Time: 0 105 | Enabled: true 106 | Invert Rainbow: false 107 | Max Color: 255; 255; 255 108 | Max Intensity: 4096 109 | Min Color: 0; 0; 0 110 | Min Intensity: 0 111 | Name: PointCloud2 112 | Position Transformer: XYZ 113 | Queue Size: 10 114 | Selectable: true 115 | Size (Pixels): 3 116 | Size (m): 0.009999999776482582 117 | Style: Flat Squares 118 | Topic: /robot1/all_localmap 119 | Unreliable: false 120 | Use Fixed Frame: true 121 | Use rainbow: true 122 | Value: true 123 | - Alpha: 1 124 | Axes Length: 1 125 | Axes Radius: 0.10000000149011612 126 | Class: rviz/Pose 127 | Color: 255; 25; 0 128 | Enabled: false 129 | Head Length: 0.30000001192092896 130 | Head Radius: 0.10000000149011612 131 | Name: Pose 132 | Shaft Length: 1 133 | Shaft Radius: 0.05000000074505806 134 | Shape: Arrow 135 | Topic: /move_base_simple/goal 136 | Unreliable: false 137 | Value: false 138 | - Alpha: 1 139 | Axes Length: 1 140 | Axes Radius: 0.10000000149011612 141 | Class: rviz/Pose 142 | Color: 255; 25; 0 143 | Enabled: true 144 | Head Length: 0.30000001192092896 145 | Head Radius: 0.10000000149011612 146 | Name: Pose 147 | Shaft Length: 0.20000000298023224 148 | Shaft Radius: 0.05000000074505806 149 | Shape: Arrow 150 | Topic: /pose 151 | Unreliable: false 152 | Value: true 153 | - Alpha: 1 154 | Buffer Length: 1 155 | Class: rviz/Path 156 | Color: 25; 255; 0 157 | Enabled: true 158 | Head Diameter: 0.30000001192092896 159 | Head Length: 0.20000000298023224 160 | Length: 0.30000001192092896 161 | Line Style: Lines 162 | Line Width: 0.029999999329447746 163 | Name: Path 164 | Offset: 165 | X: 0 166 | Y: 0 167 | Z: 0 168 | Pose Color: 255; 85; 255 169 | Pose Style: None 170 | Radius: 0.029999999329447746 171 | Shaft Diameter: 0.10000000149011612 172 | Shaft Length: 0.10000000149011612 173 | Topic: /robot1/path 174 | Unreliable: false 175 | Value: true 176 | - Class: octomap_rviz_plugin/OccupancyGrid 177 | Enabled: true 178 | Max. Height Display: 3.4028234663852886e+38 179 | Max. Octree Depth: 16 180 | Min. Height Display: -3.4028234663852886e+38 181 | Name: OccupancyGrid 182 | Octomap Topic: /octomap_full 183 | Queue Size: 5 184 | Value: true 185 | Voxel Alpha: 1 186 | Voxel Coloring: Z-Axis 187 | Voxel Rendering: Occupied Voxels 188 | - Class: rviz/TF 189 | Enabled: true 190 | Frame Timeout: 15 191 | Frames: 192 | All Enabled: true 193 | robot1/base_footprint: 194 | Value: true 195 | robot1/base_link: 196 | Value: true 197 | robot1/base_scan: 198 | Value: true 199 | robot1/camera_depth_frame: 200 | Value: true 201 | robot1/camera_depth_optical_frame: 202 | Value: true 203 | robot1/camera_link: 204 | Value: true 205 | robot1/camera_rgb_frame: 206 | Value: true 207 | robot1/camera_rgb_optical_frame: 208 | Value: true 209 | robot1/caster_back_left_link: 210 | Value: true 211 | robot1/caster_back_right_link: 212 | Value: true 213 | robot1/imu_link: 214 | Value: true 215 | robot1/map: 216 | Value: true 217 | robot1/odom: 218 | Value: true 219 | robot1/wheel_left_link: 220 | Value: true 221 | robot1/wheel_right_link: 222 | Value: true 223 | robot2/base_footprint: 224 | Value: true 225 | robot2/base_link: 226 | Value: true 227 | robot2/base_scan: 228 | Value: true 229 | robot2/camera_depth_frame: 230 | Value: true 231 | robot2/camera_depth_optical_frame: 232 | Value: true 233 | robot2/camera_link: 234 | Value: true 235 | robot2/camera_rgb_frame: 236 | Value: true 237 | robot2/camera_rgb_optical_frame: 238 | Value: true 239 | robot2/caster_back_left_link: 240 | Value: true 241 | robot2/caster_back_right_link: 242 | Value: true 243 | robot2/imu_link: 244 | Value: true 245 | robot2/odom: 246 | Value: true 247 | robot2/wheel_left_link: 248 | Value: true 249 | robot2/wheel_right_link: 250 | Value: true 251 | Marker Scale: 1 252 | Name: TF 253 | Show Arrows: true 254 | Show Axes: true 255 | Show Names: true 256 | Tree: 257 | robot1/map: 258 | robot1/odom: 259 | robot1/base_footprint: 260 | robot1/base_link: 261 | robot1/base_scan: 262 | {} 263 | robot1/camera_link: 264 | robot1/camera_depth_frame: 265 | robot1/camera_depth_optical_frame: 266 | {} 267 | robot1/camera_rgb_frame: 268 | robot1/camera_rgb_optical_frame: 269 | {} 270 | robot1/caster_back_left_link: 271 | {} 272 | robot1/caster_back_right_link: 273 | {} 274 | robot1/imu_link: 275 | {} 276 | robot1/wheel_left_link: 277 | {} 278 | robot1/wheel_right_link: 279 | {} 280 | Update Interval: 0 281 | Value: true 282 | - Alpha: 1 283 | Buffer Length: 1 284 | Class: rviz/Path 285 | Color: 164; 0; 0 286 | Enabled: true 287 | Head Diameter: 0.30000001192092896 288 | Head Length: 0.20000000298023224 289 | Length: 0.30000001192092896 290 | Line Style: Lines 291 | Line Width: 0.029999999329447746 292 | Name: Path 293 | Offset: 294 | X: 0 295 | Y: 0 296 | Z: 0 297 | Pose Color: 255; 85; 255 298 | Pose Style: None 299 | Radius: 0.029999999329447746 300 | Shaft Diameter: 0.10000000149011612 301 | Shaft Length: 0.10000000149011612 302 | Topic: /robot1/pathodom 303 | Unreliable: false 304 | Value: true 305 | - Alpha: 1 306 | Buffer Length: 1 307 | Class: rviz/Path 308 | Color: 114; 159; 207 309 | Enabled: true 310 | Head Diameter: 0.30000001192092896 311 | Head Length: 0.20000000298023224 312 | Length: 0.30000001192092896 313 | Line Style: Lines 314 | Line Width: 0.029999999329447746 315 | Name: Path 316 | Offset: 317 | X: 0 318 | Y: 0 319 | Z: 0 320 | Pose Color: 255; 85; 255 321 | Pose Style: None 322 | Radius: 0.029999999329447746 323 | Shaft Diameter: 0.10000000149011612 324 | Shaft Length: 0.10000000149011612 325 | Topic: /robot1/robot2_path 326 | Unreliable: false 327 | Value: true 328 | - Class: rviz/MarkerArray 329 | Enabled: true 330 | Marker Topic: /robot1/GMMvisual 331 | Name: MarkerArray 332 | Namespaces: 333 | {} 334 | Queue Size: 100 335 | Value: true 336 | Enabled: true 337 | Global Options: 338 | Background Color: 48; 48; 48 339 | Default Light: true 340 | Fixed Frame: robot1/map 341 | Frame Rate: 30 342 | Name: root 343 | Tools: 344 | - Class: rviz/Interact 345 | Hide Inactive Objects: true 346 | - Class: rviz/MoveCamera 347 | - Class: rviz/Select 348 | - Class: rviz/FocusCamera 349 | - Class: rviz/Measure 350 | - Class: rviz/SetInitialPose 351 | Theta std deviation: 0.2617993950843811 352 | Topic: /initialpose 353 | X std deviation: 0.5 354 | Y std deviation: 0.5 355 | - Class: rviz/SetGoal 356 | Topic: /move_base_simple/goal 357 | - Class: rviz/PublishPoint 358 | Single click: true 359 | Topic: /clicked_point 360 | Value: true 361 | Views: 362 | Current: 363 | Class: rviz/Orbit 364 | Distance: 39.28443908691406 365 | Enable Stereo Rendering: 366 | Stereo Eye Separation: 0.05999999865889549 367 | Stereo Focal Distance: 1 368 | Swap Stereo Eyes: false 369 | Value: false 370 | Focal Point: 371 | X: -3.991267204284668 372 | Y: -1.3327564001083374 373 | Z: -1.7767353057861328 374 | Focal Shape Fixed Size: true 375 | Focal Shape Size: 0.05000000074505806 376 | Invert Z Axis: false 377 | Name: Current View 378 | Near Clip Distance: 0.009999999776482582 379 | Pitch: 1.5597963333129883 380 | Target Frame: 381 | Value: Orbit (rviz) 382 | Yaw: 1.6822702884674072 383 | Saved: ~ 384 | Window Geometry: 385 | Displays: 386 | collapsed: false 387 | Height: 962 388 | Hide Left Dock: false 389 | Hide Right Dock: true 390 | QMainWindow State: 000000ff00000000fd0000000400000000000001560000031cfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed0000022b00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000031c000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010000000339fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000339000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003c000000046fc0100000002fb0000000800540069006d00650100000000000003c0000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002640000031c00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 391 | Selection: 392 | collapsed: false 393 | Time: 394 | collapsed: false 395 | Tool Properties: 396 | collapsed: false 397 | Views: 398 | collapsed: true 399 | Width: 960 400 | X: 960 401 | Y: 27 402 | -------------------------------------------------------------------------------- /gmm_map_python/rviz/robot2.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /PointCloud21 10 | - /PointCloud22 11 | - /Pose1 12 | - /Pose2 13 | - /Path1 14 | - /OccupancyGrid1 15 | - /Path2 16 | - /Path3 17 | Splitter Ratio: 0.5 18 | Tree Height: 657 19 | - Class: rviz/Selection 20 | Name: Selection 21 | - Class: rviz/Tool Properties 22 | Expanded: 23 | - /2D Pose Estimate1 24 | - /2D Nav Goal1 25 | - /Publish Point1 26 | Name: Tool Properties 27 | Splitter Ratio: 0.5886790156364441 28 | - Class: rviz/Views 29 | Expanded: 30 | - /Current View1 31 | Name: Views 32 | Splitter Ratio: 0.5 33 | - Class: rviz/Time 34 | Experimental: false 35 | Name: Time 36 | SyncMode: 0 37 | SyncSource: PointCloud2 38 | Preferences: 39 | PromptSaveOnExit: true 40 | Toolbars: 41 | toolButtonStyle: 2 42 | Visualization Manager: 43 | Class: "" 44 | Displays: 45 | - Alpha: 0.5 46 | Cell Size: 1 47 | Class: rviz/Grid 48 | Color: 160; 160; 164 49 | Enabled: true 50 | Line Style: 51 | Line Width: 0.029999999329447746 52 | Value: Lines 53 | Name: Grid 54 | Normal Cell Count: 0 55 | Offset: 56 | X: 0 57 | Y: 0 58 | Z: 0 59 | Plane: XY 60 | Plane Cell Count: 10 61 | Reference Frame: 62 | Value: true 63 | - Alpha: 1 64 | Autocompute Intensity Bounds: true 65 | Autocompute Value Bounds: 66 | Max Value: 10 67 | Min Value: -10 68 | Value: true 69 | Axis: Z 70 | Channel Name: intensity 71 | Class: rviz/PointCloud2 72 | Color: 255; 255; 255 73 | Color Transformer: Intensity 74 | Decay Time: 0 75 | Enabled: false 76 | Invert Rainbow: false 77 | Max Color: 255; 255; 255 78 | Max Intensity: 4096 79 | Min Color: 0; 0; 0 80 | Min Intensity: 0 81 | Name: PointCloud2 82 | Position Transformer: XYZ 83 | Queue Size: 10 84 | Selectable: true 85 | Size (Pixels): 3 86 | Size (m): 0.009999999776482582 87 | Style: Flat Squares 88 | Topic: /robot2/Downsample/sampled_points 89 | Unreliable: false 90 | Use Fixed Frame: true 91 | Use rainbow: true 92 | Value: false 93 | - Alpha: 1 94 | Autocompute Intensity Bounds: true 95 | Autocompute Value Bounds: 96 | Max Value: 10 97 | Min Value: -10 98 | Value: true 99 | Axis: Z 100 | Channel Name: intensity 101 | Class: rviz/PointCloud2 102 | Color: 255; 255; 255 103 | Color Transformer: Intensity 104 | Decay Time: 0 105 | Enabled: true 106 | Invert Rainbow: false 107 | Max Color: 255; 255; 255 108 | Max Intensity: 4096 109 | Min Color: 0; 0; 0 110 | Min Intensity: 0 111 | Name: PointCloud2 112 | Position Transformer: XYZ 113 | Queue Size: 10 114 | Selectable: true 115 | Size (Pixels): 3 116 | Size (m): 0.009999999776482582 117 | Style: Flat Squares 118 | Topic: /robot2/all_localmap 119 | Unreliable: false 120 | Use Fixed Frame: true 121 | Use rainbow: true 122 | Value: true 123 | - Alpha: 1 124 | Axes Length: 1 125 | Axes Radius: 0.10000000149011612 126 | Class: rviz/Pose 127 | Color: 255; 25; 0 128 | Enabled: false 129 | Head Length: 0.30000001192092896 130 | Head Radius: 0.10000000149011612 131 | Name: Pose 132 | Shaft Length: 1 133 | Shaft Radius: 0.05000000074505806 134 | Shape: Arrow 135 | Topic: /move_base_simple/goal 136 | Unreliable: false 137 | Value: false 138 | - Alpha: 1 139 | Axes Length: 1 140 | Axes Radius: 0.10000000149011612 141 | Class: rviz/Pose 142 | Color: 255; 25; 0 143 | Enabled: true 144 | Head Length: 0.30000001192092896 145 | Head Radius: 0.10000000149011612 146 | Name: Pose 147 | Shaft Length: 0.20000000298023224 148 | Shaft Radius: 0.05000000074505806 149 | Shape: Arrow 150 | Topic: /pose 151 | Unreliable: false 152 | Value: true 153 | - Alpha: 1 154 | Buffer Length: 1 155 | Class: rviz/Path 156 | Color: 25; 255; 0 157 | Enabled: true 158 | Head Diameter: 0.30000001192092896 159 | Head Length: 0.20000000298023224 160 | Length: 0.30000001192092896 161 | Line Style: Lines 162 | Line Width: 0.029999999329447746 163 | Name: Path 164 | Offset: 165 | X: 0 166 | Y: 0 167 | Z: 0 168 | Pose Color: 255; 85; 255 169 | Pose Style: None 170 | Radius: 0.029999999329447746 171 | Shaft Diameter: 0.10000000149011612 172 | Shaft Length: 0.10000000149011612 173 | Topic: /robot2/path 174 | Unreliable: false 175 | Value: true 176 | - Class: octomap_rviz_plugin/OccupancyGrid 177 | Enabled: true 178 | Max. Height Display: 3.4028234663852886e+38 179 | Max. Octree Depth: 16 180 | Min. Height Display: -3.4028234663852886e+38 181 | Name: OccupancyGrid 182 | Octomap Topic: /octomap_full 183 | Queue Size: 5 184 | Value: true 185 | Voxel Alpha: 1 186 | Voxel Coloring: Z-Axis 187 | Voxel Rendering: Occupied Voxels 188 | - Class: rviz/TF 189 | Enabled: true 190 | Frame Timeout: 15 191 | Frames: 192 | All Enabled: true 193 | robot1/base_footprint: 194 | Value: true 195 | robot1/base_link: 196 | Value: true 197 | robot1/base_scan: 198 | Value: true 199 | robot1/camera_depth_frame: 200 | Value: true 201 | robot1/camera_depth_optical_frame: 202 | Value: true 203 | robot1/camera_link: 204 | Value: true 205 | robot1/camera_rgb_frame: 206 | Value: true 207 | robot1/camera_rgb_optical_frame: 208 | Value: true 209 | robot1/caster_back_left_link: 210 | Value: true 211 | robot1/caster_back_right_link: 212 | Value: true 213 | robot1/imu_link: 214 | Value: true 215 | robot1/map: 216 | Value: true 217 | robot1/odom: 218 | Value: true 219 | robot1/wheel_left_link: 220 | Value: true 221 | robot1/wheel_right_link: 222 | Value: true 223 | robot2/base_footprint: 224 | Value: true 225 | robot2/base_link: 226 | Value: true 227 | robot2/base_scan: 228 | Value: true 229 | robot2/camera_depth_frame: 230 | Value: true 231 | robot2/camera_depth_optical_frame: 232 | Value: true 233 | robot2/camera_link: 234 | Value: true 235 | robot2/camera_rgb_frame: 236 | Value: true 237 | robot2/camera_rgb_optical_frame: 238 | Value: true 239 | robot2/caster_back_left_link: 240 | Value: true 241 | robot2/caster_back_right_link: 242 | Value: true 243 | robot2/imu_link: 244 | Value: true 245 | robot2/map: 246 | Value: true 247 | robot2/odom: 248 | Value: true 249 | robot2/wheel_left_link: 250 | Value: true 251 | robot2/wheel_right_link: 252 | Value: true 253 | Marker Scale: 1 254 | Name: TF 255 | Show Arrows: true 256 | Show Axes: true 257 | Show Names: true 258 | Tree: 259 | robot2/map: 260 | robot2/odom: 261 | robot2/base_footprint: 262 | robot2/base_link: 263 | robot2/base_scan: 264 | {} 265 | robot2/camera_link: 266 | robot2/camera_depth_frame: 267 | robot2/camera_depth_optical_frame: 268 | {} 269 | robot2/camera_rgb_frame: 270 | robot2/camera_rgb_optical_frame: 271 | {} 272 | robot2/caster_back_left_link: 273 | {} 274 | robot2/caster_back_right_link: 275 | {} 276 | robot2/imu_link: 277 | {} 278 | robot2/wheel_left_link: 279 | {} 280 | robot2/wheel_right_link: 281 | {} 282 | Update Interval: 0 283 | Value: true 284 | - Alpha: 1 285 | Buffer Length: 1 286 | Class: rviz/Path 287 | Color: 239; 41; 41 288 | Enabled: true 289 | Head Diameter: 0.30000001192092896 290 | Head Length: 0.20000000298023224 291 | Length: 0.30000001192092896 292 | Line Style: Lines 293 | Line Width: 0.029999999329447746 294 | Name: Path 295 | Offset: 296 | X: 0 297 | Y: 0 298 | Z: 0 299 | Pose Color: 255; 85; 255 300 | Pose Style: None 301 | Radius: 0.029999999329447746 302 | Shaft Diameter: 0.10000000149011612 303 | Shaft Length: 0.10000000149011612 304 | Topic: /robot2/pathodom 305 | Unreliable: false 306 | Value: true 307 | - Alpha: 1 308 | Buffer Length: 1 309 | Class: rviz/Path 310 | Color: 114; 159; 207 311 | Enabled: true 312 | Head Diameter: 0.30000001192092896 313 | Head Length: 0.20000000298023224 314 | Length: 0.30000001192092896 315 | Line Style: Lines 316 | Line Width: 0.029999999329447746 317 | Name: Path 318 | Offset: 319 | X: 0 320 | Y: 0 321 | Z: 0 322 | Pose Color: 255; 85; 255 323 | Pose Style: None 324 | Radius: 0.029999999329447746 325 | Shaft Diameter: 0.10000000149011612 326 | Shaft Length: 0.10000000149011612 327 | Topic: /robot2/robot1_path 328 | Unreliable: false 329 | Value: true 330 | - Class: rviz/MarkerArray 331 | Enabled: false 332 | Marker Topic: /robot2/GMMvisual 333 | Name: MarkerArray 334 | Namespaces: 335 | {} 336 | Queue Size: 100 337 | Value: false 338 | Enabled: true 339 | Global Options: 340 | Background Color: 48; 48; 48 341 | Default Light: true 342 | Fixed Frame: robot2/map 343 | Frame Rate: 30 344 | Name: root 345 | Tools: 346 | - Class: rviz/Interact 347 | Hide Inactive Objects: true 348 | - Class: rviz/MoveCamera 349 | - Class: rviz/Select 350 | - Class: rviz/FocusCamera 351 | - Class: rviz/Measure 352 | - Class: rviz/SetInitialPose 353 | Theta std deviation: 0.2617993950843811 354 | Topic: /initialpose 355 | X std deviation: 0.5 356 | Y std deviation: 0.5 357 | - Class: rviz/SetGoal 358 | Topic: /move_base_simple/goal 359 | - Class: rviz/PublishPoint 360 | Single click: true 361 | Topic: /clicked_point 362 | Value: true 363 | Views: 364 | Current: 365 | Class: rviz/Orbit 366 | Distance: 63.46217727661133 367 | Enable Stereo Rendering: 368 | Stereo Eye Separation: 0.05999999865889549 369 | Stereo Focal Distance: 1 370 | Swap Stereo Eyes: false 371 | Value: false 372 | Focal Point: 373 | X: 5.48568058013916 374 | Y: -5.884289741516113 375 | Z: 0.13747890293598175 376 | Focal Shape Fixed Size: true 377 | Focal Shape Size: 0.05000000074505806 378 | Invert Z Axis: false 379 | Name: Current View 380 | Near Clip Distance: 0.009999999776482582 381 | Pitch: 1.5697963237762451 382 | Target Frame: 383 | Value: Orbit (rviz) 384 | Yaw: 1.622267484664917 385 | Saved: ~ 386 | Window Geometry: 387 | Displays: 388 | collapsed: false 389 | Height: 962 390 | Hide Left Dock: false 391 | Hide Right Dock: true 392 | QMainWindow State: 000000ff00000000fd0000000400000000000001560000031cfc0200000008fb000000100044006900730070006c006100790073010000003d0000031c000000c900fffffffb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed0000022b00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010000000339fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000339000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003c000000046fc0100000002fb0000000800540069006d00650100000000000003c0000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002640000031c00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 393 | Selection: 394 | collapsed: false 395 | Time: 396 | collapsed: false 397 | Tool Properties: 398 | collapsed: false 399 | Views: 400 | collapsed: true 401 | Width: 960 402 | X: 67 403 | Y: 27 404 | -------------------------------------------------------------------------------- /turtle3sim/gazebo_images/meshes/MTR_map.dae: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | Blender User 6 | Blender 2.71.0 commit date:2014-06-12, commit time:18:39, hash:169c95b 7 | 8 | 2017-02-12T23:23:23 9 | 2017-02-12T23:23:23 10 | 11 | Z_UP 12 | 13 | 14 | 15 | 16 | 17 | 18 | 49.13434 19 | 1.777778 20 | 0.1 21 | 100 22 | 23 | 24 | 25 | 26 | 27 | 0 28 | 0 29 | 0 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 1 1 1 39 | 1 40 | 0 41 | 0.00111109 42 | 43 | 44 | 45 | 46 | 0.000999987 47 | 1 48 | 0.1 49 | 0.1 50 | 1 51 | 1 52 | 1 53 | 2 54 | 0 55 | 1 56 | 1 57 | 1 58 | 1 59 | 1 60 | 0 61 | 2880 62 | 2 63 | 30.002 64 | 1.000799 65 | 0.04999995 66 | 29.99998 67 | 1 68 | 2 69 | 0 70 | 0 71 | 1 72 | 1 73 | 1 74 | 1 75 | 8192 76 | 1 77 | 1 78 | 0 79 | 1 80 | 1 81 | 1 82 | 3 83 | 0 84 | 0 85 | 0 86 | 0 87 | 0 88 | 1 89 | 1 90 | 1 91 | 3 92 | 0.15 93 | 75 94 | 1 95 | 1 96 | 0 97 | 1 98 | 1 99 | 0 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | -581.6098 -1.297104 285.6434 -581.6098 -1.297104 282.6434 -581.6098 48.70289 285.6434 -581.6098 48.70289 282.6434 -777.074 -1.297104 285.6434 -777.074 48.70289 285.6434 -777.074 -1.297104 282.6434 -777.074 48.70289 282.6434 -777.074 48.70289 426.4126 -777.074 -1.297104 426.4126 -777.074 -1.297104 141.4973 -777.074 48.70289 141.4973 143.1296 -1.297104 326.6994 143.1296 -1.297104 426.4126 143.1296 48.70289 326.6994 143.1296 48.70289 426.4126 146.1296 -1.297104 326.6994 146.1296 48.70289 326.6994 146.1296 -1.297104 426.4126 146.1296 48.70289 426.4126 245.6325 -1.297104 -236.4223 348.6636 -1.297104 -236.4223 245.6325 48.70289 -236.4223 348.6636 48.70289 -236.4223 245.6325 -1.297104 -238.4223 245.6325 48.70289 -238.4223 348.6636 -1.297104 -238.4223 348.6636 48.70289 -238.4223 245.6325 -1.297104 126.9906 348.6636 -1.297104 126.9906 245.6325 48.70289 126.9906 348.6636 48.70289 126.9906 245.6325 -1.297104 124.9906 245.6325 48.70289 124.9906 348.6636 -1.297104 124.9906 348.6636 48.70289 124.9906 -382.9425 -1.297104 363.1365 -337.1954 -1.297104 426.4126 -382.9425 48.70289 363.1365 -337.1954 48.70289 426.4126 -398.3394 -1.297104 363.1365 -398.3394 48.70289 363.1365 -457.817 -1.297104 426.4126 -457.817 48.70289 426.4126 -393.1723 -1.297104 353.2592 -461.9343 -1.297104 426.4126 -393.1723 48.70289 353.2592 -461.9343 48.70289 426.4126 -393.1723 -1.297104 277.7263 -393.1723 48.70289 277.7263 -390.1723 -1.297104 277.7263 -390.1723 48.70289 277.7263 -390.1723 -1.297104 348.0162 -390.1723 48.70289 348.0162 -333.4934 -1.297104 426.4126 -333.4934 48.70289 426.4126 348.6636 48.70289 -638.2223 348.6636 -1.297104 -638.2223 348.6636 -1.297104 426.4126 348.6636 48.70289 426.4126 -177.7619 -1.297104 141.4973 -177.7619 48.70289 141.4973 -177.7619 -1.297104 248.4062 -177.7619 48.70289 248.4062 13.05762 -1.297104 248.4062 13.05762 48.70289 248.4062 13.05762 -1.297104 3.697876 13.05762 48.70289 3.697876 -130.057 -1.297104 3.697876 -130.057 48.70289 3.697876 -130.057 -1.297104 -267.5132 -130.057 48.70289 -267.5132 13.05762 -1.297104 -267.5132 13.05762 48.70289 -267.5132 13.05762 -1.297104 -473.3509 13.05762 48.70289 -473.3509 -489.8059 -1.297104 -473.3509 -489.8059 48.70289 -473.3509 -489.8059 -1.297104 -687.6239 -489.8059 48.70289 -687.6239 -243.3811 -1.297104 -687.6239 -243.3811 48.70289 -687.6239 -177.7619 -1.297104 -615.3928 -177.7619 48.70289 -615.3928 -98.25257 -1.297104 -687.6239 -98.25257 48.70289 -687.6239 78.51464 -1.297104 -687.6239 78.51464 48.70289 -687.6239 78.51464 -1.297104 -638.2223 78.51464 48.70289 -638.2223 -162.7619 -1.297104 126.4973 -792.074 -1.297104 126.4973 -162.7619 48.70289 126.4973 -792.074 48.70289 126.4973 -162.7619 -1.297104 233.4062 -162.7619 48.70289 233.4062 -1.942375 -1.297104 233.4062 -1.942375 48.70289 233.4062 -1.942375 -1.297104 18.69788 -1.942375 48.70289 18.69788 -145.057 -1.297104 18.69788 -145.057 48.70289 18.69788 -145.057 -1.297104 -282.5132 -145.057 48.70289 -282.5132 -1.942375 -1.297104 -282.5132 -1.942375 48.70289 -282.5132 -1.942375 -1.297104 -458.3509 -1.942375 48.70289 -458.3509 -504.8059 -1.297104 -458.3509 -504.8059 48.70289 -458.3509 -504.8059 -1.297104 -702.6239 -504.8059 48.70289 -702.6239 -236.7424 -1.297104 -702.6239 -236.7424 48.70289 -702.6239 -176.7455 -1.297104 -636.5816 -176.7455 48.70289 -636.5816 -104.0487 -1.297104 -702.6239 -104.0487 48.70289 -702.6239 93.51464 -1.297104 -702.6239 93.51464 48.70289 -702.6239 93.51464 -1.297104 -653.2223 93.51464 48.70289 -653.2223 363.6636 -1.297104 -653.2223 363.6636 48.70289 -653.2223 363.6636 -1.297104 441.4126 363.6636 48.70289 441.4126 -792.074 -1.297104 441.4126 -792.074 48.70289 441.4126 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 1 0 0 1 0 0 0 0 1 0 0 1 0 0 -1 0 0 -1 1 0 0 1 0 0 1 0 0 1 0 0 -1 0 0 -1 0 0 0 0 -1 0 0 -1 1 0 0 1 0 0 0 0 1 0 0 1 -1 0 0 -1 0 0 0 0 -1 0 0 -1 0 0 1 0 0 1 -1 0 0 -1 0 0 0 0 -1 0 0 -1 -0.8103889 0 0.5858924 -0.8103889 0 0.5858924 0 0 1 0 0 1 0.7286386 0 0.6848984 0.7286386 0 0.6848984 -0.7286384 0 -0.6848986 -0.7286384 0 -0.6848986 -1 0 0 -1 0 0 0 0 -1 0 0 -1 1 0 0 1 0 0 0.8103888 0 -0.5858926 0.8103888 0 -0.5858926 -1 0 0 -1 0 0 -1 0 0 -1 0 0 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 1 0 0 1 -1 0 0 -1 0 0 0 0 1 0 0 1 1 0 0 1 0 0 0 0 -1 0 0 -1 1 0 0 1 0 0 0 0 1 0 0 1 1 0 0 1 0 0 0 0 -1 0 0 -1 1 0 0 1 0 0 0 0 1 0 0 1 -0.740172 0 0.6724177 -0.740172 0 0.6724177 0.6724177 0 0.7401719 0.6724177 0 0.7401719 0 0 1 0 0 1 -1 0 0 -1 0 0 0 0 1 0 0 1 -1 0 0 -1 0 0 0 0 -1 0 0 -1 1 0 0 1 0 0 0 0 -1 0 0 -1 -1 0 0 -1 0 0 0 0 1 0 0 1 -1 0 0 -1 0 0 0 0 -1 0 0 -1 -1 0 0 -1 0 0 0 0 1 0 0 1 -1 0 0 -1 0 0 0 0 -1 0 0 -1 0.7401719 0 -0.6724177 0.7401719 0 -0.6724177 -0.6724177 0 -0.7401719 -0.6724177 0 -0.7401719 0 0 -1 0 0 -1 1 0 0 1 0 0 0 0 -1 0 0 -1 1 0 0 1 0 0 0 0 1 0 0 1 -1 0 0 -1 0 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 135 |

0 0 1 0 2 0 2 1 1 1 3 1 4 2 0 2 5 2 5 3 0 3 2 3 1 4 6 4 3 4 3 5 6 5 7 5 5 6 8 6 4 6 4 7 8 7 9 7 6 8 10 8 7 8 7 9 10 9 11 9 12 10 13 10 14 10 14 11 13 11 15 11 16 12 12 12 17 12 17 13 12 13 14 13 18 14 16 14 19 14 19 15 16 15 17 15 20 16 21 16 22 16 22 17 21 17 23 17 20 18 22 18 24 18 24 19 22 19 25 19 26 20 24 20 27 20 27 21 24 21 25 21 28 22 29 22 30 22 30 23 29 23 31 23 28 24 30 24 32 24 32 25 30 25 33 25 34 26 32 26 35 26 35 27 32 27 33 27 36 28 37 28 38 28 38 29 37 29 39 29 40 30 36 30 41 30 41 31 36 31 38 31 42 32 40 32 43 32 43 33 40 33 41 33 44 34 45 34 46 34 46 35 45 35 47 35 48 36 44 36 49 36 49 37 44 37 46 37 50 38 48 38 51 38 51 39 48 39 49 39 52 40 50 40 53 40 53 41 50 41 51 41 54 42 52 42 55 42 55 43 52 43 53 43 27 44 56 44 26 44 26 45 56 45 57 45 29 46 58 46 31 46 31 47 58 47 59 47 45 48 9 48 47 48 47 49 9 49 8 49 37 50 42 50 39 50 39 51 42 51 43 51 19 52 59 52 18 52 18 53 59 53 58 53 13 54 54 54 15 54 15 55 54 55 55 55 10 56 60 56 11 56 11 57 60 57 61 57 60 58 62 58 61 58 61 59 62 59 63 59 62 60 64 60 63 60 63 61 64 61 65 61 64 62 66 62 65 62 65 63 66 63 67 63 66 64 68 64 67 64 67 65 68 65 69 65 68 66 70 66 69 66 69 67 70 67 71 67 70 68 72 68 71 68 71 69 72 69 73 69 72 70 74 70 73 70 73 71 74 71 75 71 74 72 76 72 75 72 75 73 76 73 77 73 76 74 78 74 77 74 77 75 78 75 79 75 78 76 80 76 79 76 79 77 80 77 81 77 80 78 82 78 81 78 81 79 82 79 83 79 82 80 84 80 83 80 83 81 84 81 85 81 84 82 86 82 85 82 85 83 86 83 87 83 86 84 88 84 87 84 87 85 88 85 89 85 88 86 57 86 89 86 89 87 57 87 56 87 21 88 34 88 23 88 23 89 34 89 35 89 90 90 91 90 92 90 92 91 91 91 93 91 94 92 90 92 95 92 95 93 90 93 92 93 96 94 94 94 97 94 97 95 94 95 95 95 98 96 96 96 99 96 99 97 96 97 97 97 100 98 98 98 101 98 101 99 98 99 99 99 102 100 100 100 103 100 103 101 100 101 101 101 104 102 102 102 105 102 105 103 102 103 103 103 106 104 104 104 107 104 107 105 104 105 105 105 108 106 106 106 109 106 109 107 106 107 107 107 110 108 108 108 111 108 111 109 108 109 109 109 112 110 110 110 113 110 113 111 110 111 111 111 114 112 112 112 115 112 115 113 112 113 113 113 116 114 114 114 117 114 117 115 114 115 115 115 118 116 116 116 119 116 119 117 116 117 117 117 120 118 118 118 121 118 121 119 118 119 119 119 122 120 120 120 123 120 123 121 120 121 121 121 124 122 122 122 125 122 125 123 122 123 123 123 126 124 124 124 127 124 127 125 124 125 125 125 91 126 126 126 93 126 93 127 126 127 127 127 13 128 124 128 54 128 13 129 18 129 124 129 13 130 12 130 18 130 18 131 12 131 16 131 18 132 58 132 124 132 124 133 58 133 29 133 34 134 29 134 32 134 34 135 124 135 29 135 34 136 21 136 124 136 124 137 21 137 122 137 122 138 21 138 26 138 57 139 122 139 26 139 57 140 120 140 122 140 57 141 88 141 120 141 120 142 88 142 86 142 118 143 86 143 116 143 118 144 120 144 86 144 29 145 28 145 32 145 20 146 24 146 21 146 21 147 24 147 26 147 86 148 84 148 116 148 116 149 84 149 114 149 114 150 84 150 82 150 80 151 114 151 82 151 80 152 112 152 114 152 80 153 78 153 112 153 112 154 78 154 110 154 110 155 78 155 76 155 108 156 76 156 106 156 108 157 110 157 76 157 76 158 74 158 106 158 106 159 74 159 72 159 104 160 72 160 70 160 102 161 70 161 68 161 100 162 68 162 98 162 100 163 102 163 68 163 106 164 72 164 104 164 104 165 70 165 102 165 68 166 66 166 98 166 98 167 66 167 64 167 96 168 64 168 62 168 94 169 62 169 60 169 90 170 60 170 91 170 90 171 94 171 60 171 98 172 64 172 96 172 96 173 62 173 94 173 60 174 10 174 91 174 91 175 10 175 6 175 126 176 6 176 4 176 9 177 126 177 4 177 9 178 45 178 126 178 126 179 45 179 42 179 37 180 126 180 42 180 37 181 54 181 126 181 37 182 36 182 54 182 54 183 36 183 52 183 52 184 36 184 44 184 48 185 52 185 44 185 48 186 50 186 52 186 1 187 0 187 6 187 6 188 0 188 4 188 42 189 45 189 40 189 40 190 45 190 44 190 36 191 40 191 44 191 126 192 91 192 6 192 124 193 126 193 54 193 43 194 127 194 39 194 43 195 47 195 127 195 43 196 41 196 47 196 47 197 41 197 46 197 46 198 41 198 38 198 53 199 38 199 55 199 53 200 46 200 38 200 53 201 49 201 46 201 53 202 51 202 49 202 38 203 39 203 55 203 55 204 39 204 127 204 125 205 55 205 127 205 125 206 15 206 55 206 125 207 19 207 15 207 125 208 59 208 19 208 125 209 31 209 59 209 125 210 35 210 31 210 125 211 23 211 35 211 125 212 123 212 23 212 23 213 123 213 27 213 22 214 27 214 25 214 22 215 23 215 27 215 47 216 8 216 127 216 127 217 8 217 5 217 7 218 5 218 3 218 7 219 127 219 5 219 7 220 93 220 127 220 7 221 11 221 93 221 93 222 11 222 61 222 92 223 61 223 95 223 92 224 93 224 61 224 5 225 2 225 3 225 61 226 63 226 95 226 95 227 63 227 97 227 97 228 63 228 65 228 67 229 97 229 65 229 67 230 99 230 97 230 67 231 69 231 99 231 99 232 69 232 101 232 101 233 69 233 71 233 103 234 71 234 105 234 103 235 101 235 71 235 71 236 73 236 105 236 105 237 73 237 75 237 107 238 75 238 77 238 109 239 77 239 79 239 111 240 79 240 113 240 111 241 109 241 79 241 105 242 75 242 107 242 107 243 77 243 109 243 79 244 81 244 113 244 113 245 81 245 115 245 115 246 81 246 83 246 85 247 115 247 83 247 85 248 117 248 115 248 85 249 87 249 117 249 117 250 87 250 119 250 119 251 87 251 121 251 121 252 87 252 89 252 56 253 121 253 89 253 56 254 123 254 121 254 56 255 27 255 123 255 33 256 30 256 35 256 35 257 30 257 31 257 19 258 17 258 15 258 15 259 17 259 14 259

136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 0.6858805 -0.3173701 0.6548619 7.481132 0.7276338 0.3124686 -0.6106656 -6.50764 -0.01081678 0.8953432 0.4452454 5.343665 0 0 0 1 145 | 146 | 147 | 148 | -0.2908646 -0.7711008 0.5663932 4.076245 0.9551712 -0.1998834 0.2183912 1.005454 -0.05518906 0.6045247 0.7946723 5.903862 0 0 0 1 149 | 150 | 151 | 152 | -0.00999999 -9.96624e-15 1.58151e-8 2.307846 1.58151e-8 -1.91927e-8 0.00999999 2.998451 2.03872e-14 0.00999999 1.91927e-8 0 0 0 0 1 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | --------------------------------------------------------------------------------