├── catkin_ws └── src │ ├── CMakeLists.txt │ ├── image_segmentation │ ├── srv │ │ └── maskImage.srv │ ├── package.xml │ ├── scripts │ │ └── image_masking_server.py │ ├── rviz │ │ └── mask_rcnn.rviz │ ├── CMakeLists.txt │ └── src │ │ └── pcl_proc.cpp │ ├── rtabmap_ros │ ├── README │ └── MapAssemblerNode.cpp │ └── robot_launch │ ├── param │ ├── navfn_global_planner_params.yaml │ ├── move_base_params.yaml │ ├── depth.yaml │ ├── rgb.yaml │ ├── global_costmap_params.yaml │ ├── local_costmap_params.yaml │ ├── costmap_common_params.yaml │ └── dwa_local_planner_params.yaml │ ├── urdf │ ├── display_model.launch │ ├── guidebot.urdf │ └── display.rviz │ ├── guidebot.launch │ └── rviz │ └── guidebot.rviz ├── MaskRCNN ├── README └── sunrgbd.py ├── rtabmap ├── README └── util3d.cpp └── README.md /catkin_ws/src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | /opt/ros/kinetic/share/catkin/cmake/toplevel.cmake -------------------------------------------------------------------------------- /catkin_ws/src/image_segmentation/srv/maskImage.srv: -------------------------------------------------------------------------------- 1 | sensor_msgs/Image inputImage 2 | --- 3 | sensor_msgs/Image maskedImage 4 | -------------------------------------------------------------------------------- /MaskRCNN/README: -------------------------------------------------------------------------------- 1 | This folder contains files that were added on top of the original Mask-RCNN implementation. 2 | See: https://github.com/matterport/Mask_RCNN 3 | -------------------------------------------------------------------------------- /rtabmap/README: -------------------------------------------------------------------------------- 1 | This folder contains files that were modified in the original implementation of standalone RTAB-Map. 2 | See: https://github.com/introlab/rtabmap 3 | -------------------------------------------------------------------------------- /catkin_ws/src/rtabmap_ros/README: -------------------------------------------------------------------------------- 1 | This folder contains files that were modified in the original implementation of ROS RTAB-Map. 2 | See: https://github.com/introlab/rtabmap_ros 3 | -------------------------------------------------------------------------------- /catkin_ws/src/robot_launch/param/navfn_global_planner_params.yaml: -------------------------------------------------------------------------------- 1 | 2 | NavfnROS: 3 | visualize_potential: false 4 | allow_unknown: false # Whether the planner is allowed to plan in unknown areas or not. 5 | planner_window_x: 0.0 6 | planner_window_y: 0.0 7 | 8 | default_tolerance: 0.0 # Radius of the area around the goal pose which the planner will search for a valid goal. 9 | -------------------------------------------------------------------------------- /catkin_ws/src/robot_launch/param/move_base_params.yaml: -------------------------------------------------------------------------------- 1 | 2 | shutdown_costmaps: false 3 | 4 | controller_frequency: 1.0 # Frequency of running the local planner and issuing velocity commands 5 | controller_patience: 3.0 6 | 7 | 8 | planner_frequency: 1.0 # Frequency of running the global planner 9 | planner_patience: 5.0 10 | 11 | oscillation_timeout: 10.0 12 | oscillation_distance: 0.2 13 | 14 | base_local_planner: "dwa_local_planner/DWAPlannerROS" # The local planner used 15 | 16 | base_global_planner: "navfn/NavfnROS" # The global planner used -------------------------------------------------------------------------------- /catkin_ws/src/robot_launch/param/depth.yaml: -------------------------------------------------------------------------------- 1 | image_width: 640 2 | image_height: 488 3 | camera_name: depth_A00362800947048A 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [631.2562116652756, 0, 292.985581057041, 0, 635.547418422373, 204.16188$ 8 | distortion_model: plumb_bob 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 5 12 | data: [-0.01244253956866485, 0.2264001587471495, -0.01548072435733289, -0.021$ 13 | rectification_matrix: 14 | rows: 3 15 | cols: 3 16 | data: [1, 0, 0, 0, 1, 0, 0, 0, 1] 17 | projection_matrix: 18 | rows: 3 19 | cols: 4 20 | data: [646.6484375, 0, 283.8413031648379, 0, 0, 657.05615234375, 199.53450080$ 21 | 22 | 23 | -------------------------------------------------------------------------------- /catkin_ws/src/robot_launch/param/rgb.yaml: -------------------------------------------------------------------------------- 1 | image_width: 640 2 | image_height: 480 3 | camera_name: rgb_A00362800947048A 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [536.2724509813037, 0, 333.6128070868875, 0, 534.2678746918485, 250.617$ 8 | distortion_model: plumb_bob 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 5 12 | data: [0.1552338632500986, -0.3418659707736176, -0.003950982879756475, 0.0065$ 13 | rectification_matrix: 14 | rows: 3 15 | cols: 3 16 | data: [1, 0, 0, 0, 1, 0, 0, 0, 1] 17 | projection_matrix: 18 | rows: 3 19 | cols: 4 20 | data: [541.0818481445312, 0, 337.8059459605902, 0, 0, 544.3114624023438, 248.$ 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /catkin_ws/src/robot_launch/param/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: /map 3 | robot_base_frame: /base_link 4 | update_frequency: 1.0 # The frequency which it is calculated. 5 | publish_frequency: 0.5 # The frequency which it is published. 6 | static_map: true # Should be created from the static map supplied 7 | transform_tolerance: 0.5 # The maximum delay there can be in the costmap before it is invalid 8 | plugins: 9 | - {name: static_layer, type: "costmap_2d::StaticLayer"} 10 | - {name: obstacle_layer, type: "costmap_2d::VoxelLayer"} 11 | - {name: inflation_layer, type: "costmap_2d::InflationLayer"} 12 | 13 | -------------------------------------------------------------------------------- /catkin_ws/src/robot_launch/urdf/display_model.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Semantic mapping 2 | ___ 3 | ### Description 4 | ___ 5 | The goal of this project is to provide a full semantic mapping solution. It is based on a fusion of deep learning image segmnetation algorithm (Mask-RCNN) and SLAM algorithm (RTAB-Map). 6 | Additionally few algorithms are provided for semantic pointcloud processing to prove usefulness of the concept and the solution. 7 | 8 | The proposed solution is based on exisiting repositories with rtabmap and Mask-RCNN implementations. For clarity, in this repository, only modified files of these repos are included. 9 | ___ 10 | ### References: 11 | ###### RTAB-Map ROS implementation: https://github.com/introlab/rtabmap_ros 12 | ###### RTAB-Map implementation: https://github.com/introlab/rtabmap 13 | ###### Mask-RCNN implementation: https://github.com/matterport/Mask_RCNN 14 | -------------------------------------------------------------------------------- /catkin_ws/src/robot_launch/param/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | global_frame: odom 3 | robot_base_frame: /base_link 4 | update_frequency: 5.0 # Frequency of updating the local costmap 5 | publish_frequency: 2.0 # Frequency of publishing the local costmap 6 | static_map: false # Should it be created from a static map 7 | rolling_window: true # Whether to created af rolling window (it follows the robot) 8 | width: 4.0 # Width of the rolling window 9 | height: 4.0 # Height of the rolling window 10 | resolution: 0.05 # Resolution of the local costmap, might be overwritten by the parent node (move_base) 11 | transform_tolerance: 0.5 # The maximum delay there can be in the costmap before it is invalid 12 | plugins: 13 | - {name: obstacle_layer, type: "costmap_2d::VoxelLayer"} 14 | - {name: inflation_layer, type: "costmap_2d::InflationLayer"} -------------------------------------------------------------------------------- /catkin_ws/src/robot_launch/param/costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | max_obstacle_height: 0.60 2 | robot_radius: 0.325 # Radius of the outer circle of the robot. 3 | footprint: [[0.0, 0.125], [0.25, 0.125], [0.25, -0.125], [0.0, -0.125]] 4 | 5 | map_type: voxel 6 | 7 | obstacle_layer: 8 | enabled: true 9 | max_obstacle_height: 0.6 10 | origin_z: 0.0 11 | z_resolution: 0.2 12 | z_voxels: 2 13 | unknown_threshold: 15 14 | mark_threshold: 0 15 | combination_method: 1 16 | track_unknown_space: true 17 | obstacle_range: 2.5 18 | raytrace_range: 3.0 19 | origin_z: 0.0 20 | z_resolution: 0.2 21 | z_voxels: 2 22 | publish_voxel_map: false 23 | observation_sources: scan 24 | scan: 25 | data_type: LaserScan 26 | topic: scan 27 | marking: true 28 | clearing: true 29 | min_obstacle_height: 0.25 30 | max_obstacle_height: 0.35 31 | 32 | inflation_layer: 33 | enabled: true 34 | cost_scaling_factor: 5.0 # exponential rate at which the obstacle cost decays 35 | inflation_radius: 1 # max distance from an obstacle which the cost is calculated for 36 | 37 | static_layer: 38 | enabled: true 39 | -------------------------------------------------------------------------------- /catkin_ws/src/image_segmentation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | image_segmentation 4 | 0.1.0 5 | The image_masking package 6 | 7 | Szymon Kowalewski 8 | TODO 9 | 10 | catkin 11 | 12 | roscpp 13 | rospy 14 | std_msgs 15 | opencv2 16 | sensor_msgs 17 | pcl_conversions 18 | pcl_ros 19 | libpcl-all-dev 20 | cv_bridge 21 | message_generation 22 | octomap 23 | rtabmap_ros 24 | gridmap_2d 25 | 26 | roscpp 27 | rospy 28 | std_msgs 29 | opencv2 30 | sensor_msgs 31 | pcl_conversions 32 | pcl_ros 33 | cv_bridge 34 | message_generation 35 | rtabmap_ros 36 | gridmap_2d 37 | 38 | roscpp 39 | rospy 40 | std_msgs 41 | opencv2 42 | sensor_msgs 43 | message_runtime 44 | pcl_conversions 45 | pcl_ros 46 | cv_bridge 47 | libpcl-all 48 | rtabmap_ros 49 | gridmap_2d 50 | 51 | 52 | -------------------------------------------------------------------------------- /catkin_ws/src/robot_launch/param/dwa_local_planner_params.yaml: -------------------------------------------------------------------------------- 1 | DWAPlannerROS: 2 | 3 | # Robot Configuration Parameters - Guidebot 4 | max_vel_x: 0.3 # The max linear velocity of the Guidebot, in our case also the max velocity of the motors 5 | min_vel_x: 0.0 6 | 7 | max_vel_y: 0.0 # differential drive robot so no y velocity 8 | min_vel_y: 0.0 # differential drive robot so no y velocity 9 | 10 | max_trans_vel: 0.3 #Redundant as it is equal to the max_vel_x 11 | min_trans_vel: 0.05 #Minimum speed for the robot to actually move 12 | trans_stopped_vel: 0.05 13 | 14 | max_rot_vel: 1.0 # Maximum rotational velocity 15 | min_rot_vel: 0.1 # Minimum rotational velocity for it to actually move 16 | rot_stopped_vel: 0.1 17 | 18 | acc_lim_x: 5.0 # Acceleration limits for the robot, set high as the Guidebot is not really limited 19 | acc_lim_theta: 5.0 20 | acc_lim_y: 0.0 # Cannot move in y-direction 21 | 22 | # Goal Tolerance Parameters, somewhat lax as we are not that critical of the position 23 | yaw_goal_tolerance: 0.5 24 | xy_goal_tolerance: 0.3 25 | latch_xy_goal_tolerance: true # Only rotate in place when (x,y) goal is reached 26 | 27 | # Forward Simulation Parameters (DWA planner) 28 | sim_time: 2.0 # Simulated each velocity 2 seconds into the future 29 | vx_samples: 10 # How many samples in the x-velocity 30 | vy_samples: 1 # Only one sample in y (y=0) 31 | vtheta_samples: 50 # How many samples in the theta-velocity 32 | # Changes to diamond 33 | use_diamond_velocity: true # Use the "diamond" sampling algorithm (robot is limited by the single motor velocity) 34 | wheel_base: 0.25 # Wheelbase of the robot 35 | 36 | # Trajectory Scoring Parameters 37 | path_distance_bias: 64.0 # The weight for following the global plan 38 | goal_distance_bias: 24.0 # The weight for trying to achive the local goal (controls the speed) 39 | occdist_scale: 0.5 # The weight fir how much the planner should avoid obstacles 40 | forward_point_distance: 0.325 # how far along to place an additional scoring point (default) 41 | stop_time_buffer: 0.2 # The amount of time that the robot must stop before a collision in order for a trajectory to be considered valid in seconds. (default) 42 | scaling_speed: 0.25 # The absolute value of the veolicty at which to start scaling the robot's footprint, in m/s (Default) 43 | max_scaling_factor: 0.2 # The maximum factor to scale the robot's footprint by. (default) 44 | 45 | # Oscillation Prevention Parameters 46 | oscillation_reset_dist: 0.05 # How far the robot must travel in meters before oscillation flags are reset. (default) 47 | 48 | # Debugging 49 | publish_traj_pc : true 50 | publish_cost_grid_pc: true 51 | global_frame_id: odom 52 | -------------------------------------------------------------------------------- /catkin_ws/src/robot_launch/guidebot.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 | 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /catkin_ws/src/image_segmentation/scripts/image_masking_server.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import sys 4 | import os 5 | #from image_masking.msg import maskedImage 6 | 7 | # Root directory of the project 8 | ROOT_DIR = '/home/aut/s160948/rgb_new/mrcnn/' 9 | # Weights path 10 | WEIGHTS_DIR = "/home/aut/s160948/rgb_new/mrcnn/logs/sunrgbd20180511T1720/mask_rcnn_sunrgbd_0020.h5" 11 | # Directory to save logs and trained model 12 | MODEL_DIR = os.path.join(ROOT_DIR, "logs") 13 | 14 | sys.path.insert(0, ROOT_DIR) 15 | 16 | import rospy 17 | import ros_numpy 18 | import tensorflow as tf 19 | from sensor_msgs.msg import Image 20 | import matplotlib.pyplot as plt 21 | import time 22 | import numpy as np 23 | import skimage.io 24 | import model as modellib 25 | import sunrgbd 26 | 27 | from image_segmentation.srv import * 28 | import rospy 29 | 30 | 31 | 32 | class InferenceConfig(sunrgbd.SunRgbdConfig): 33 | GPU_COUNT = 1 34 | IMAGES_PER_GPU = 1 35 | 36 | def handle_mask_image(req): 37 | print("Returning response") 38 | 39 | # Convert image message to numpy array 40 | np_image = ros_numpy.numpify(req.inputImage) 41 | 42 | # Save original timestamp and frame id 43 | original_stamp = req.inputImage.header.stamp 44 | original_id = req.inputImage.header.frame_id 45 | 46 | with graph.as_default(): 47 | results = model.detect([np_image], verbose=1) 48 | 49 | r = results[0] 50 | 51 | checktime = time.time() 52 | 53 | masks_r = np.zeros((np_image.shape[0], np_image.shape[1]), dtype=np.uint8) 54 | masks_g = np.zeros((np_image.shape[0], np_image.shape[1]), dtype=np.uint8) 55 | masks_b = np.zeros((np_image.shape[0], np_image.shape[1]), dtype=np.uint8) 56 | 57 | if r['class_ids'].shape[0] != 0: 58 | 59 | split_masks = np.split(r['masks'], r['masks'].shape[2], 2) 60 | 61 | for idx, mask_img in enumerate(split_masks): 62 | mask_img = np.reshape(mask_img, [mask_img.shape[0], mask_img.shape[1]]) 63 | masks_r[mask_img==1] = colors_arr[r['class_ids'][idx]][0] 64 | masks_g[mask_img==1] = colors_arr[r['class_ids'][idx]][1] 65 | masks_b[mask_img==1] = colors_arr[r['class_ids'][idx]][2] 66 | 67 | 68 | masks_r = np.reshape(masks_r, [masks_r.shape[0], masks_r.shape[1], 1]) 69 | masks_g = np.reshape(masks_g, [masks_g.shape[0], masks_g.shape[1], 1]) 70 | masks_b = np.reshape(masks_b, [masks_b.shape[0], masks_b.shape[1], 1]) 71 | 72 | masks_all = np.append(masks_r, masks_g, 2) 73 | masks_all = np.append(masks_all, masks_b, 2) 74 | 75 | print("PROCESSING %s seconds ---" % (time.time() - checktime)) 76 | 77 | msg_image = ros_numpy.msgify(Image, masks_all, encoding='rgb8') 78 | 79 | msg_image.header.stamp = original_stamp 80 | msg_image.header.frame_id = original_id 81 | 82 | return maskImageResponse(msg_image) 83 | 84 | def mask_image__server(): 85 | rospy.init_node('mask_image_server') 86 | s = rospy.Service('maskImage', maskImage, handle_mask_image) 87 | print("Ready to mask image.") 88 | rospy.spin() 89 | 90 | if __name__ == "__main__": 91 | # Create inference configuration 92 | inference_config = InferenceConfig() 93 | 94 | # Visualization constants 95 | class_names = ['bg', 'chair', 'table', 'window', 'box', 'door', 'shelves', 'sofa', 'cabinet'] 96 | colors_arr = [[255, 255, 255], 97 | [255, 0, 0], 98 | [0, 255, 0], 99 | [0, 0, 255], 100 | [100, 0, 0], 101 | [0, 100, 0], 102 | [0, 0, 100], 103 | [255, 255, 0], 104 | [255, 0, 255], 105 | ] 106 | 107 | # Recreate the model in inference mode 108 | model = modellib.MaskRCNN(mode="inference", 109 | config=inference_config, 110 | model_dir=MODEL_DIR) 111 | 112 | # Get path to saved weights 113 | print("Loading weights from ", WEIGHTS_DIR) 114 | 115 | #Load model weights 116 | model.load_weights(WEIGHTS_DIR, by_name=True) 117 | 118 | #Save the graph to make it accessible in the subsriber 119 | graph = tf.get_default_graph() 120 | 121 | mask_image__server() -------------------------------------------------------------------------------- /catkin_ws/src/robot_launch/urdf/guidebot.urdf: -------------------------------------------------------------------------------- 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 | 66 | 67 | 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 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 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 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 217 | 218 | 219 | 220 | -------------------------------------------------------------------------------- /catkin_ws/src/robot_launch/urdf/display.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 | Splitter Ratio: 0.5 10 | Tree Height: 565 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.588679 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: "" 30 | Visualization Manager: 31 | Class: "" 32 | Displays: 33 | - Alpha: 0.5 34 | Cell Size: 1 35 | Class: rviz/Grid 36 | Color: 160; 160; 164 37 | Enabled: true 38 | Line Style: 39 | Line Width: 0.03 40 | Value: Lines 41 | Name: Grid 42 | Normal Cell Count: 0 43 | Offset: 44 | X: 0 45 | Y: 0 46 | Z: 0 47 | Plane: XY 48 | Plane Cell Count: 10 49 | Reference Frame: 50 | Value: true 51 | - Alpha: 1 52 | Class: rviz/RobotModel 53 | Collision Enabled: false 54 | Enabled: true 55 | Links: 56 | All Links Enabled: true 57 | Expand Joint Details: false 58 | Expand Link Details: false 59 | Expand Tree: false 60 | Link Tree Style: Links in Alphabetic Order 61 | base_link: 62 | Alpha: 1 63 | Show Axes: false 64 | Show Trail: false 65 | Value: true 66 | base_scan: 67 | Alpha: 1 68 | Show Axes: false 69 | Show Trail: false 70 | Value: true 71 | head_disk: 72 | Alpha: 1 73 | Show Axes: false 74 | Show Trail: false 75 | Value: true 76 | kinect_support: 77 | Alpha: 1 78 | Show Axes: false 79 | Show Trail: false 80 | Value: true 81 | laser: 82 | Alpha: 1 83 | Show Axes: false 84 | Show Trail: false 85 | Value: true 86 | lower_torso: 87 | Alpha: 1 88 | Show Axes: false 89 | Show Trail: false 90 | Value: true 91 | upper_torso: 92 | Alpha: 1 93 | Show Axes: false 94 | Show Trail: false 95 | Value: true 96 | wheel_L: 97 | Alpha: 1 98 | Show Axes: false 99 | Show Trail: false 100 | Value: true 101 | wheel_R: 102 | Alpha: 1 103 | Show Axes: false 104 | Show Trail: false 105 | Value: true 106 | Name: RobotModel 107 | Robot Description: robot_description 108 | TF Prefix: "" 109 | Update Interval: 0 110 | Value: true 111 | Visual Enabled: true 112 | - Class: rviz/TF 113 | Enabled: true 114 | Frame Timeout: 15 115 | Frames: 116 | All Enabled: true 117 | base_link: 118 | Value: true 119 | base_scan: 120 | Value: true 121 | head_disk: 122 | Value: true 123 | kinect_support: 124 | Value: true 125 | laser: 126 | Value: true 127 | lower_torso: 128 | Value: true 129 | upper_torso: 130 | Value: true 131 | wheel_L: 132 | Value: true 133 | wheel_R: 134 | Value: true 135 | Marker Scale: 1 136 | Name: TF 137 | Show Arrows: true 138 | Show Axes: true 139 | Show Names: true 140 | Tree: 141 | base_link: 142 | base_scan: 143 | laser: 144 | {} 145 | lower_torso: 146 | upper_torso: 147 | head_disk: 148 | kinect_support: 149 | {} 150 | wheel_L: 151 | {} 152 | wheel_R: 153 | {} 154 | Update Interval: 0 155 | Value: true 156 | Enabled: true 157 | Global Options: 158 | Background Color: 48; 48; 48 159 | Fixed Frame: base_link 160 | Frame Rate: 30 161 | Name: root 162 | Tools: 163 | - Class: rviz/Interact 164 | Hide Inactive Objects: true 165 | - Class: rviz/MoveCamera 166 | - Class: rviz/Select 167 | - Class: rviz/FocusCamera 168 | - Class: rviz/Measure 169 | - Class: rviz/SetInitialPose 170 | Topic: /initialpose 171 | - Class: rviz/SetGoal 172 | Topic: /move_base_simple/goal 173 | - Class: rviz/PublishPoint 174 | Single click: true 175 | Topic: /clicked_point 176 | Value: true 177 | Views: 178 | Current: 179 | Class: rviz/Orbit 180 | Distance: 4.64404 181 | Enable Stereo Rendering: 182 | Stereo Eye Separation: 0.06 183 | Stereo Focal Distance: 1 184 | Swap Stereo Eyes: false 185 | Value: false 186 | Focal Point: 187 | X: 0 188 | Y: 0 189 | Z: 0 190 | Name: Current View 191 | Near Clip Distance: 0.01 192 | Pitch: 0.785398 193 | Target Frame: 194 | Value: Orbit (rviz) 195 | Yaw: 0.785398 196 | Saved: ~ 197 | Window Geometry: 198 | Displays: 199 | collapsed: false 200 | Height: 846 201 | Hide Left Dock: false 202 | Hide Right Dock: false 203 | QMainWindow State: 000000ff00000000fd00000004000000000000016a000002c4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002c4000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000028000002c4000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002f600fffffffb0000000800540069006d006501000000000000045000000000000000000000022b000002c400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 204 | Selection: 205 | collapsed: false 206 | Time: 207 | collapsed: false 208 | Tool Properties: 209 | collapsed: false 210 | Views: 211 | collapsed: false 212 | Width: 1200 213 | X: 55 214 | Y: 50 215 | -------------------------------------------------------------------------------- /catkin_ws/src/image_segmentation/rviz/mask_rcnn.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 81 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /masked pointcloud1 10 | Splitter Ratio: 0.5 11 | Tree Height: 202 12 | - Class: rviz/Selection 13 | Name: Selection 14 | - Class: rviz/Tool Properties 15 | Expanded: 16 | - /2D Pose Estimate1 17 | - /2D Nav Goal1 18 | - /Publish Point1 19 | Name: Tool Properties 20 | Splitter Ratio: 0.588679016 21 | - Class: rviz/Views 22 | Expanded: 23 | - /Current View1 24 | Name: Views 25 | Splitter Ratio: 0.5 26 | - Class: rviz/Time 27 | Experimental: false 28 | Name: Time 29 | SyncMode: 0 30 | SyncSource: masked image 31 | Visualization Manager: 32 | Class: "" 33 | Displays: 34 | - Alpha: 0.5 35 | Cell Size: 1 36 | Class: rviz/Grid 37 | Color: 160; 160; 164 38 | Enabled: true 39 | Line Style: 40 | Line Width: 0.0299999993 41 | Value: Lines 42 | Name: Grid 43 | Normal Cell Count: 0 44 | Offset: 45 | X: 0 46 | Y: 0 47 | Z: 0 48 | Plane: XY 49 | Plane Cell Count: 10 50 | Reference Frame: 51 | Value: true 52 | - Class: rviz/Image 53 | Enabled: true 54 | Image Topic: /rcnn_publisher 55 | Max Value: 1 56 | Median window: 5 57 | Min Value: 0 58 | Name: masked image 59 | Normalize Range: true 60 | Queue Size: 2 61 | Transport Hint: raw 62 | Unreliable: false 63 | Value: true 64 | - Alpha: 1 65 | Autocompute Intensity Bounds: true 66 | Autocompute Value Bounds: 67 | Max Value: 10 68 | Min Value: -10 69 | Value: true 70 | Axis: Z 71 | Channel Name: intensity 72 | Class: rviz/PointCloud2 73 | Color: 255; 255; 255 74 | Color Transformer: RGB8 75 | Decay Time: 0 76 | Enabled: true 77 | Invert Rainbow: false 78 | Max Color: 255; 255; 255 79 | Max Intensity: 4096 80 | Min Color: 0; 0; 0 81 | Min Intensity: 0 82 | Name: masked pointcloud 83 | Position Transformer: XYZ 84 | Queue Size: 1 85 | Selectable: true 86 | Size (Pixels): 2 87 | Size (m): 0.00999999978 88 | Style: Points 89 | Topic: /masked_points 90 | Unreliable: false 91 | Use Fixed Frame: true 92 | Use rainbow: true 93 | Value: true 94 | - Alpha: 1 95 | Autocompute Intensity Bounds: true 96 | Autocompute Value Bounds: 97 | Max Value: 10 98 | Min Value: -10 99 | Value: true 100 | Axis: Z 101 | Channel Name: intensity 102 | Class: rviz/PointCloud2 103 | Color: 255; 255; 255 104 | Color Transformer: RGB8 105 | Decay Time: 0 106 | Enabled: true 107 | Invert Rainbow: false 108 | Max Color: 255; 255; 255 109 | Max Intensity: 4096 110 | Min Color: 0; 0; 0 111 | Min Intensity: 0 112 | Name: original pointcloud 113 | Position Transformer: XYZ 114 | Queue Size: 10 115 | Selectable: true 116 | Size (Pixels): 3 117 | Size (m): 0.00999999978 118 | Style: Flat Squares 119 | Topic: /camera/depth_registered/points 120 | Unreliable: false 121 | Use Fixed Frame: true 122 | Use rainbow: true 123 | Value: true 124 | - Class: rviz/Image 125 | Enabled: true 126 | Image Topic: /camera/rgb/image_color 127 | Max Value: 1 128 | Median window: 5 129 | Min Value: 0 130 | Name: original image 131 | Normalize Range: true 132 | Queue Size: 2 133 | Transport Hint: raw 134 | Unreliable: false 135 | Value: true 136 | Enabled: true 137 | Global Options: 138 | Background Color: 48; 48; 48 139 | Default Light: true 140 | Fixed Frame: camera_link 141 | Frame Rate: 30 142 | Name: root 143 | Tools: 144 | - Class: rviz/Interact 145 | Hide Inactive Objects: true 146 | - Class: rviz/MoveCamera 147 | - Class: rviz/Select 148 | - Class: rviz/FocusCamera 149 | - Class: rviz/Measure 150 | - Class: rviz/SetInitialPose 151 | Topic: /initialpose 152 | - Class: rviz/SetGoal 153 | Topic: /move_base_simple/goal 154 | - Class: rviz/PublishPoint 155 | Single click: true 156 | Topic: /clicked_point 157 | Value: true 158 | Views: 159 | Current: 160 | Class: rviz/Orbit 161 | Distance: 9.36312675 162 | Enable Stereo Rendering: 163 | Stereo Eye Separation: 0.0599999987 164 | Stereo Focal Distance: 1 165 | Swap Stereo Eyes: false 166 | Value: false 167 | Focal Point: 168 | X: 2.9990449 169 | Y: -0.556100428 170 | Z: 0.768553972 171 | Focal Shape Fixed Size: true 172 | Focal Shape Size: 0.0500000007 173 | Invert Z Axis: false 174 | Name: Current View 175 | Near Clip Distance: 0.00999999978 176 | Pitch: -0.0102023464 177 | Target Frame: 178 | Value: Orbit (rviz) 179 | Yaw: 3.00040174 180 | Saved: ~ 181 | Window Geometry: 182 | Displays: 183 | collapsed: false 184 | Height: 846 185 | Hide Left Dock: false 186 | Hide Right Dock: false 187 | QMainWindow State: 000000ff00000000fd00000004000000000000016a000002a9fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006700fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000430000015c000000e300fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018006d00610073006b0065006400200069006d00610067006501000001a5000000860000001600fffffffb0000001c006f0072006900670069006e0061006c00200069006d0061006700650100000231000000bb0000001600ffffff000000010000010f000002a9fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000043000002a9000000b300fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000021f00fffffffb0000000800540069006d006501000000000000045000000000000000000000022b000002a900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 188 | Selection: 189 | collapsed: false 190 | Time: 191 | collapsed: false 192 | Tool Properties: 193 | collapsed: false 194 | Views: 195 | collapsed: false 196 | Width: 1200 197 | X: 1819 198 | Y: 115 199 | masked image: 200 | collapsed: false 201 | original image: 202 | collapsed: false 203 | -------------------------------------------------------------------------------- /catkin_ws/src/image_segmentation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(image_segmentation) 3 | 4 | add_definitions(-std=c++11) 5 | 6 | set(PCL_DIR "/usr/share/pcl-1.8/") 7 | 8 | set(OpenCV_INCLUDE_DIRS "/opt/ros/kinetic/include/opencv-3.3.1-dev/") 9 | set(OpenCV_DIR "/opt/ros/kinetic/include/opencv-3.3.1-dev/") 10 | 11 | set(CMAKE_CXX_FLAGS "-lusb-1.0") 12 | 13 | ## Compile as C++11, supported in ROS Kinetic and newer 14 | # add_compile_options(-std=c++11) 15 | 16 | ## Find catkin macros and libraries 17 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 18 | ## is used, also find other catkin packages 19 | 20 | find_package(catkin REQUIRED COMPONENTS 21 | roscpp 22 | cv_bridge 23 | rospy 24 | std_msgs 25 | sensor_msgs 26 | message_generation 27 | pcl_ros 28 | gridmap_2d 29 | #rtabmap_ros 30 | ) 31 | 32 | #find_package(octomap REQUIRED) 33 | #find_package(RTABMap 0.16.3 REQUIRED) 34 | find_package(OpenCV REQUIRED) 35 | find_package(Boost COMPONENTS program_options REQUIRED) 36 | 37 | ## System dependencies are found with CMake's conventions 38 | # find_package(Boost REQUIRED COMPONENTS system) 39 | 40 | ## Uncomment this if the package has a setup.py. This macro ensures 41 | ## modules and global scripts declared therein get installed 42 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 43 | # catkin_python_setup() 44 | 45 | ################################################ 46 | ## Declare ROS messages, services and actions ## 47 | ################################################ 48 | 49 | ## To declare and build messages, services or actions from within this 50 | ## package, follow these steps: 51 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 52 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 53 | ## * In the file package.xml: 54 | ## * add a build_depend tag for "message_generation" 55 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 56 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 57 | ## but can be declared for certainty nonetheless: 58 | ## * add a run_depend tag for "message_runtime" 59 | ## * In this file (CMakeLists.txt): 60 | ## * add "message_generation" and every package in MSG_DEP_SET to 61 | ## find_package(catkin REQUIRED COMPONENTS ...) 62 | ## * add "message_runtime" and every package in MSG_DEP_SET to 63 | ## catkin_package(CATKIN_DEPENDS ...) 64 | ## * uncomment the add_*_files sections below as needed 65 | ## and list every .msg/.srv/.action file to be processed 66 | ## * uncomment the generate_messages entry below 67 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 68 | 69 | ## Generate messages in the 'msg' folder 70 | #add_message_files( 71 | # FILES 72 | # maskedImage.msg 73 | #) 74 | 75 | ## Generate services in the 'srv' folder 76 | add_service_files( 77 | FILES 78 | maskImage.srv 79 | ) 80 | 81 | ## Generate actions in the 'action' folder 82 | # add_action_files( 83 | # FILES 84 | # Action1.action 85 | # Action2.action 86 | # ) 87 | 88 | ## Generate added messages and services with any dependencies listed here 89 | generate_messages(DEPENDENCIES std_msgs sensor_msgs) 90 | 91 | ################################################ 92 | ## Declare ROS dynamic reconfigure parameters ## 93 | ################################################ 94 | 95 | ## To declare and build dynamic reconfigure parameters within this 96 | ## package, follow these steps: 97 | ## * In the file package.xml: 98 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 99 | ## * In this file (CMakeLists.txt): 100 | ## * add "dynamic_reconfigure" to 101 | ## find_package(catkin REQUIRED COMPONENTS ...) 102 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 103 | ## and list every .cfg file to be processed 104 | 105 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 106 | # generate_dynamic_reconfigure_options( 107 | # cfg/DynReconf1.cfg 108 | # cfg/DynReconf2.cfg 109 | # ) 110 | #find_package(catkin REQUIRED COMPONENTS pcl_conversions pcl_ros) 111 | 112 | 113 | 114 | ################################### 115 | ## catkin specific configuration ## 116 | ################################### 117 | ## The catkin_package macro generates cmake config files for your package 118 | ## Declare things to be passed to dependent projects 119 | ## INCLUDE_DIRS: uncomment this if your package contains header files 120 | ## LIBRARIES: libraries you create in this project that dependent projects also need 121 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 122 | ## DEPENDS: system dependencies of this project that dependent projects also need 123 | catkin_package( 124 | # INCLUDE_DIRS include 125 | # LIBRARIES image_masking 126 | CATKIN_DEPENDS roscpp rospy std_msgs sensor_msgs message_runtime 127 | # DEPENDS system_lib 128 | ) 129 | 130 | ########### 131 | ## Build ## 132 | ########### 133 | 134 | ## Specify additional locations of header files 135 | ## Your package locations should be listed before other locations 136 | include_directories( 137 | ${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS} #${RTABMap_INCLUDE_DIRS} 138 | ) 139 | 140 | # libraries 141 | SET(Libraries 142 | ${OpenCV_LIBRARIES} 143 | ${catkin_LIBRARIES} 144 | ) 145 | ## Declare a C++ library 146 | # add_library(${PROJECT_NAME} 147 | # src/${PROJECT_NAME}/image_masking.cpp 148 | # ) 149 | 150 | ## Add cmake target dependencies of the library 151 | ## as an example, code may need to be generated before libraries 152 | ## either from message generation or dynamic reconfigure 153 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 154 | 155 | ## Declare a C++ executable 156 | ## With catkin_make all packages are built within a single CMake context 157 | ## The recommended prefix ensures that target names across packages don't collide 158 | # add_executable(${PROJECT_NAME}_node src/image_masking_node.cpp) 159 | 160 | ## Rename C++ executable without prefix 161 | ## The above recommended prefix causes long target names, the following renames the 162 | ## target back to the shorter version for ease of user use 163 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 164 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 165 | 166 | ## Add cmake target dependencies of the executable 167 | ## same as for the library above 168 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 169 | 170 | ## Specify libraries to link a library or executable target against 171 | # target_link_libraries(${PROJECT_NAME}_node 172 | # ${catkin_LIBRARIES} 173 | # ) 174 | 175 | ############# 176 | ## Install ## 177 | ############# 178 | 179 | # all install targets should use catkin DESTINATION variables 180 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 181 | 182 | ## Mark executable scripts (Python etc.) for installation 183 | ## in contrast to setup.py, you can choose the destination 184 | # install(PROGRAMS 185 | # scripts/my_python_script 186 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 187 | # ) 188 | 189 | ## Mark executables and/or libraries for installation 190 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 191 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 192 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 193 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 194 | # ) 195 | 196 | ## Mark cpp header files for installation 197 | # install(DIRECTORY include/${PROJECT_NAME}/ 198 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 199 | # FILES_MATCHING PATTERN "*.h" 200 | # PATTERN ".svn" EXCLUDE 201 | # ) 202 | 203 | ## Mark other files for installation (e.g. launch and bag files, etc.) 204 | # install(FILES 205 | # # myfile1 206 | # # myfile2 207 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 208 | # ) 209 | 210 | ############# 211 | ## Testing ## 212 | ############# 213 | 214 | ## Add gtest based cpp test target and link libraries 215 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_image_masking.cpp) 216 | # if(TARGET ${PROJECT_NAME}-test) 217 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 218 | # endif() 219 | 220 | ## Add folders to be run by python nosetests 221 | # catkin_add_nosetests(test) 222 | 223 | #recover this 224 | add_executable(pcl_proc src/pcl_proc.cpp) 225 | target_link_libraries(pcl_proc ${OpenCV_LIBRARIES} ${catkin_LIBRARIES}) 226 | add_dependencies(pcl_proc image_segmentation_generate_messages_cpp) 227 | 228 | 229 | #add_executable(old_pcl_proc src/old_pcl_proc.cpp) 230 | #target_link_libraries(old_pcl_proc ${OpenCV_LIBRARIES} ${catkin_LIBRARIES}) 231 | 232 | #add_executable(simple_navigation_goals src/simple_navigation_goals.cpp) 233 | #target_link_libraries(simple_navigation_goals ${catkin_LIBRARIES}) 234 | 235 | #recover this 236 | #add_executable(image_masking_client src/image_masking_client.cpp) 237 | #target_link_libraries(image_masking_client ${OpenCV_LIBRARIES} ${catkin_LIBRARIES}) 238 | #add_dependencies(image_masking_client image_segmentation_generate_messages_cpp) 239 | 240 | #add_executable(masked_graph_generator src/masked_graph_generator.cpp) 241 | #target_link_libraries(masked_graph_generator ${OpenCV_LIBRARIES} ${catkin_LIBRARIES}) 242 | #add_dependencies(masked_graph_generator image_segmentation_generate_messages_cpp) -------------------------------------------------------------------------------- /MaskRCNN/sunrgbd.py: -------------------------------------------------------------------------------- 1 | """ 2 | Mask R-CNN 3 | Configurations and data loading code for the SUN-RGBD dataset 4 | This is an sub-class created using the original matterport class. 5 | Written by Szymon Kowalewski 6 | """ 7 | 8 | from config import Config 9 | import skimage.io 10 | import utils 11 | import os 12 | import glob 13 | import numpy as np 14 | import model as modellib 15 | from skimage import img_as_ubyte 16 | 17 | import warnings 18 | 19 | # Root directory of the project 20 | ROOT_DIR = os.getcwd() 21 | 22 | # Directory to save logs and trained model 23 | MODEL_DIR = os.path.join(ROOT_DIR, "logs") 24 | 25 | # Local path to trained weights file 26 | COCO_MODEL_PATH = os.path.join(ROOT_DIR, "mask_rcnn_coco.h5") 27 | 28 | 29 | # Sun-RGBD Configuration Class 30 | # It overwrites the original values from the original class 31 | class SunRgbdConfig(Config): 32 | """Base configuration class. For custom configurations, create a 33 | sub-class that inherits from this one and override properties 34 | that need to be changed. 35 | """ 36 | 37 | # Name the configurations. For example, 'COCO', 'Experiment 3', ...etc. 38 | # Useful if your code needs to do things differently depending on which 39 | # experiment is running. 40 | NAME = "SUNRGBD" # Override in sub-classes 41 | 42 | # NUMBER OF GPUs to use. For CPU training, use 1 43 | GPU_COUNT = 1 44 | 45 | # Number of images to train with on each GPU. A 12GB GPU can typically 46 | # handle 2 images of 1024x1024px. 47 | # Adjust based on your GPU memory and image sizes. Use the highest 48 | # number that your GPU can handle for best performance. 49 | #IMAGES_PER_GPU = 8 50 | IMAGES_PER_GPU = 2 51 | 52 | # Number of training steps per epoch 53 | # This doesn't need to match the size of the training set. Tensorboard 54 | # updates are saved at the end of each epoch, so setting this to a 55 | # smaller number means getting more frequent TensorBoard updates. 56 | # Validation stats are also calculated at each epoch end and they 57 | # might take a while, so don't set this too small to avoid spending 58 | # a lot of time on validation stats. 59 | #STEPS_PER_EPOCH = 3000 60 | STEPS_PER_EPOCH = 2350 61 | 62 | # Number of validation steps to run at the end of every training epoch. 63 | # A bigger number improves accuracy of validation stats, but slows 64 | # down the training. 65 | #VALIDATION_STEPS = 2000 66 | VALIDATION_STEPS = 150 67 | 68 | # Number of classification classes (including background) 69 | NUM_CLASSES = 9 # For now background + chair + table 70 | 71 | # Input image resizing 72 | # Generally, use the "square" resizing mode for training and inferencing 73 | # and it should work well in most cases. In this mode, images are scaled 74 | # up such that the small side is = IMAGE_MIN_DIM, but ensuring that the 75 | # scaling doesn't make the long side > IMAGE_MAX_DIM. Then the image is 76 | # padded with zeros to make it a square so multiple images can be put 77 | # in one batch. 78 | # Available resizing modes: 79 | # none: No resizing or padding. Return the image unchanged. 80 | # square: Resize and pad with zeros to get a square image 81 | # of size [max_dim, max_dim]. 82 | # pad64: Pads width and height with zeros to make them multiples of 64. 83 | # If IMAGE_MIN_DIM or IMAGE_MIN_SCALE are not None, then it scales 84 | # up before padding. IMAGE_MAX_DIM is ignored in this mode. 85 | # The multiple of 64 is needed to ensure smooth scaling of feature 86 | # maps up and down the 6 levels of the FPN pyramid (2**6=64). 87 | # crop: Picks random crops from the image. First, scales the image based 88 | # on IMAGE_MIN_DIM and IMAGE_MIN_SCALE, then picks a random crop of 89 | # size IMAGE_MIN_DIM x IMAGE_MIN_DIM. Can be used in training only. 90 | # IMAGE_MAX_DIM is not used in this mode. 91 | IMAGE_RESIZE_MODE = "square" 92 | IMAGE_MIN_DIM = 512 93 | IMAGE_MAX_DIM = 768 94 | # Minimum scaling ratio. Checked after MIN_IMAGE_DIM and can force further 95 | # up scaling. For example, if set to 2 then images are scaled up to double 96 | # the width and height, or more, even if MIN_IMAGE_DIM doesn't require it. 97 | # Howver, in 'square' mode, it can be overruled by IMAGE_MAX_DIM. 98 | IMAGE_MIN_SCALE = 0 99 | 100 | # Maximum number of ground truth instances to use in one image 101 | MAX_GT_INSTANCES = 20 102 | 103 | # Max number of final detections 104 | DETECTION_MAX_INSTANCES = 20 105 | 106 | # Image mean (RGB) 107 | MEAN_PIXEL = np.array([126.38, 117.52, 111.82]) 108 | 109 | # Length of square anchor side in pixels 110 | RPN_ANCHOR_SCALES = (16, 32, 64, 128, 256) 111 | 112 | # Learning rate and momentum 113 | # The Mask RCNN paper uses lr=0.02, but on TensorFlow it causes 114 | # weights to explode. Likely due to differences in optimzer 115 | # implementation. 116 | LEARNING_RATE = 0.001 117 | LEARNING_MOMENTUM = 0.9 118 | 119 | 120 | class SunRgbdDataset(utils.Dataset): 121 | """The base class for dataset classes. 122 | To use it, create a new class that adds functions specific to the dataset 123 | you want to use. For example: 124 | 125 | class CatsAndDogsDataset(Dataset): 126 | def load_cats_and_dogs(self): 127 | ... 128 | def load_mask(self, image_id): 129 | ... 130 | def image_reference(self, image_id): 131 | ... 132 | 133 | See COCODataset and ShapesDataset as examples. 134 | """ 135 | 136 | def __init__(self, class_map=None): 137 | self._image_ids = [] 138 | self.image_info = [] 139 | # Background is always the first class 140 | self.class_info = [{"source": "", "id": 0, "name": "BG"}] 141 | self.source_class_ids = {} 142 | 143 | def load_sunrgbd(self, dataset_dir, dataset_type): 144 | """Generate the requested number of synthetic images. 145 | count: number of images to generate. 146 | height, width: the size of the generated images. 147 | """ 148 | # Add classes 149 | self.add_class("sunrgbd", 1, "chair") 150 | self.add_class("sunrgbd", 2, "table") 151 | self.add_class("sunrgbd", 3, "window") 152 | self.add_class("sunrgbd", 4, "door") 153 | #self.add_class("sunrgbd", 5, "desk") 154 | self.add_class("sunrgbd", 5, "box") 155 | self.add_class("sunrgbd", 6, "shelves") 156 | #self.add_class("sunrgbd", 8, "bookshelf") 157 | self.add_class("sunrgbd", 7, "sofa") 158 | self.add_class("sunrgbd", 8, "cabinet") 159 | 160 | # Get folders 161 | dataset_dir = os.path.join(dataset_dir, dataset_type) 162 | examples_paths = sorted([os.path.join(dataset_dir,f) for f in os.listdir(dataset_dir)]) 163 | number_of_examples = len(examples_paths) 164 | 165 | # Add images 166 | for example, example_path in enumerate(examples_paths): 167 | image_path = os.path.join(example_path,'rgb.jpg') 168 | #Probably not neccessary anymore as naming convention was changed 169 | #image_path = os.path.join(example_path,'*.jpg') 170 | #image_path = glob.glob(image_path)[0] 171 | self.add_image("sunrgbd", image_id=example, path=image_path) 172 | 173 | def load_image(self, image_id): 174 | """Load the specified image and return a [H,W,3] Numpy array. 175 | """ 176 | # Get paths 177 | rgb_path = os.path.join(self.image_info[image_id]['path']) 178 | 179 | # Load images 180 | image_rgb = skimage.io.imread(rgb_path) 181 | 182 | return image_rgb 183 | 184 | def load_mask(self, image_id): 185 | """Load instance masks for the given image. 186 | 187 | Different datasets use different ways to store masks. Override this 188 | method to load instance masks and return them in the form of am 189 | array of binary masks of shape [height, width, instances]. 190 | 191 | Returns: 192 | masks: A bool array of shape [height, width, instance count] with 193 | a binary mask per instance. 194 | class_ids: a 1D array of class IDs of the instance masks. 195 | """ 196 | # Override this function to load a mask from your dataset. 197 | # Otherwise, it returns an empty mask. 198 | 199 | instance_masks = [] 200 | class_ids = [] 201 | 202 | labels_path = os.path.dirname(self.image_info[image_id]['path']) 203 | labels_path = os.path.join(labels_path, 'labels') 204 | 205 | #Get all .png files in the folder 206 | file_paths = os.path.join(labels_path,'*.png') 207 | file_paths = sorted(glob.glob(file_paths)) 208 | 209 | #Add mask to instance_masks and append the class name found in the filename 210 | for file_path in file_paths: 211 | for cat in self.class_names: 212 | if cat in file_path: 213 | mask = skimage.io.imread(file_path) 214 | instance_masks.append(mask) 215 | class_ids.append(self.class_names.index(cat)) 216 | #print("Filename loaded: ", file_path) 217 | #print("Class loaded: ", cat) 218 | 219 | #Pack instance masks into an array 220 | if class_ids: 221 | mask = np.stack(instance_masks, axis=2) 222 | class_ids = np.array(class_ids, dtype=np.int32) 223 | return mask, class_ids 224 | else: 225 | # Call super class to return an empty mask 226 | return super(SunRgbdDataset, self).load_mask(image_id) 227 | 228 | 229 | 230 | ############################################################ 231 | # Training 232 | ############################################################ 233 | 234 | 235 | if __name__ == '__main__': 236 | import argparse 237 | 238 | # Download COCO trained weights from Releases if needed 239 | if not os.path.exists(COCO_MODEL_PATH): 240 | utils.download_trained_weights(COCO_MODEL_PATH) 241 | 242 | # Parse command line arguments 243 | parser = argparse.ArgumentParser( 244 | description='Train Mask R-CNN on SUN-RGBD.') 245 | parser.add_argument('--dataset', required=True, 246 | metavar="/path/to/dataset", 247 | help='Directory of the SUNRGBD dataset') 248 | parser.add_argument('--model', required=True, 249 | metavar="/path/to/weights.h5", 250 | help="Path to weights .h5 file") 251 | parser.add_argument('--logs', required=False, 252 | default=MODEL_DIR, 253 | metavar="/path/to/logs/", 254 | help='Logs and checkpoints directory (default=logs/)') 255 | args = parser.parse_args() 256 | print("Model: ", args.model) 257 | print("Dataset: ", args.dataset) 258 | print("Logs: ", args.logs) 259 | 260 | # Configurations 261 | config = SunRgbdConfig() 262 | config.display() 263 | 264 | # Create model 265 | model = modellib.MaskRCNN(mode="training", config=config,model_dir=args.logs) 266 | 267 | if args.model.lower() == "imagenet": 268 | model.load_weights(model.get_imagenet_weights(), by_name=True) 269 | elif args.model.lower() == "coco": 270 | # Load weights trained on MS COCO, but skip layers that 271 | # are different due to the different number of classes 272 | # See README for instructions to download the COCO weights 273 | print("Loading weights ", COCO_MODEL_PATH) 274 | model.load_weights(COCO_MODEL_PATH, by_name=True, 275 | exclude=["conv1", "mrcnn_class_logits", "mrcnn_bbox_fc", 276 | "mrcnn_bbox", "mrcnn_mask"]) 277 | elif args.model.lower() == "last": 278 | # Load the last model you trained and continue training 279 | print("Loading weights ", model.find_last()[1]) 280 | model.load_weights(model.find_last()[1], by_name=True) 281 | else: 282 | model.load_weights(args.model, by_name=True) 283 | 284 | # Training dataset. Use the training set and 35K from the 285 | # validation set, as as in the Mask RCNN paper. 286 | dataset_train = SunRgbdDataset() 287 | dataset_train.load_sunrgbd(args.dataset, "training") 288 | dataset_train.prepare() 289 | 290 | # Validation dataset 291 | dataset_val = SunRgbdDataset() 292 | dataset_val.load_sunrgbd(args.dataset, "validation") 293 | dataset_val.prepare() 294 | 295 | # *** This training schedule is an example. Update to your needs *** 296 | # Training - Stage 1 297 | print("Training network heads") 298 | model.train(dataset_train, dataset_val, 299 | learning_rate=config.LEARNING_RATE, 300 | epochs=60, 301 | layers='all') 302 | -------------------------------------------------------------------------------- /catkin_ws/src/robot_launch/rviz/guidebot.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 81 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Odometry1 10 | - /TF1/Tree1 11 | Splitter Ratio: 0.5 12 | Tree Height: 720 13 | - Class: rviz/Selection 14 | Name: Selection 15 | - Class: rviz/Tool Properties 16 | Expanded: 17 | - /2D Pose Estimate1 18 | - /2D Nav Goal1 19 | - /Publish Point1 20 | Name: Tool Properties 21 | Splitter Ratio: 0.588679016 22 | - Class: rviz/Views 23 | Expanded: 24 | - /Current View1 25 | Name: Views 26 | Splitter Ratio: 0.5 27 | - Class: rviz/Time 28 | Experimental: false 29 | Name: Time 30 | SyncMode: 0 31 | SyncSource: "" 32 | Visualization Manager: 33 | Class: "" 34 | Displays: 35 | - Alpha: 0.5 36 | Cell Size: 1 37 | Class: rviz/Grid 38 | Color: 160; 160; 164 39 | Enabled: false 40 | Line Style: 41 | Line Width: 0.0299999993 42 | Value: Lines 43 | Name: Grid 44 | Normal Cell Count: 0 45 | Offset: 46 | X: 0 47 | Y: 0 48 | Z: 0 49 | Plane: XY 50 | Plane Cell Count: 10 51 | Reference Frame: 52 | Value: false 53 | - Angle Tolerance: 0.100000001 54 | Class: rviz/Odometry 55 | Covariance: 56 | Orientation: 57 | Alpha: 0.5 58 | Color: 255; 255; 127 59 | Color Style: Unique 60 | Frame: Local 61 | Offset: 1 62 | Scale: 1 63 | Value: true 64 | Position: 65 | Alpha: 0.300000012 66 | Color: 204; 51; 204 67 | Scale: 1 68 | Value: true 69 | Value: true 70 | Enabled: true 71 | Keep: 1 72 | Name: Odometry 73 | Position Tolerance: 0.100000001 74 | Shape: 75 | Alpha: 1 76 | Axes Length: 1 77 | Axes Radius: 0.100000001 78 | Color: 255; 25; 0 79 | Head Length: 0.300000012 80 | Head Radius: 0.100000001 81 | Shaft Length: 1 82 | Shaft Radius: 0.0500000007 83 | Value: Arrow 84 | Topic: /odom 85 | Unreliable: false 86 | Value: true 87 | - Alpha: 1 88 | Autocompute Intensity Bounds: true 89 | Autocompute Value Bounds: 90 | Max Value: 10 91 | Min Value: -10 92 | Value: true 93 | Axis: Z 94 | Channel Name: intensity 95 | Class: rviz/LaserScan 96 | Color: 255; 0; 0 97 | Color Transformer: FlatColor 98 | Decay Time: 0 99 | Enabled: true 100 | Invert Rainbow: false 101 | Max Color: 255; 255; 255 102 | Max Intensity: 4096 103 | Min Color: 0; 0; 0 104 | Min Intensity: 0 105 | Name: LaserScan 106 | Position Transformer: XYZ 107 | Queue Size: 10 108 | Selectable: true 109 | Size (Pixels): 3 110 | Size (m): 0.0299999993 111 | Style: Flat Squares 112 | Topic: /base_scan 113 | Unreliable: false 114 | Use Fixed Frame: true 115 | Use rainbow: true 116 | Value: true 117 | - Alpha: 0.699999988 118 | Class: rviz/Map 119 | Color Scheme: map 120 | Draw Behind: false 121 | Enabled: false 122 | Name: Map 123 | Topic: /map 124 | Unreliable: false 125 | Use Timestamp: false 126 | Value: false 127 | - Alpha: 1 128 | Buffer Length: 1 129 | Class: rviz/Path 130 | Color: 255; 85; 255 131 | Enabled: true 132 | Head Diameter: 0.300000012 133 | Head Length: 0.200000003 134 | Length: 0.300000012 135 | Line Style: Lines 136 | Line Width: 0.0299999993 137 | Name: Path 138 | Offset: 139 | X: 0 140 | Y: 0 141 | Z: 0 142 | Pose Color: 255; 85; 255 143 | Pose Style: None 144 | Radius: 0.0299999993 145 | Shaft Diameter: 0.100000001 146 | Shaft Length: 0.100000001 147 | Topic: /move_base_node/NavfnROS/plan 148 | Unreliable: false 149 | Value: true 150 | - Alpha: 0.699999988 151 | Class: rviz/Map 152 | Color Scheme: map 153 | Draw Behind: false 154 | Enabled: false 155 | Name: Global Costmap 156 | Topic: /move_base_node/global_costmap/costmap 157 | Unreliable: false 158 | Use Timestamp: false 159 | Value: false 160 | - Alpha: 1 161 | Arrow Length: 0.300000012 162 | Axes Length: 0.300000012 163 | Axes Radius: 0.00999999978 164 | Class: rviz/PoseArray 165 | Color: 85; 0; 255 166 | Enabled: true 167 | Head Length: 0.0700000003 168 | Head Radius: 0.0299999993 169 | Name: PoseArray 170 | Shaft Length: 0.230000004 171 | Shaft Radius: 0.00999999978 172 | Shape: Arrow (Flat) 173 | Topic: /particlecloud 174 | Unreliable: false 175 | Value: true 176 | - Class: rviz/TF 177 | Enabled: false 178 | Frame Timeout: 15 179 | Frames: 180 | All Enabled: true 181 | Marker Scale: 1 182 | Name: TF 183 | Show Arrows: true 184 | Show Axes: true 185 | Show Names: true 186 | Tree: 187 | {} 188 | Update Interval: 0 189 | Value: false 190 | - Alpha: 1 191 | Class: rviz/RobotModel 192 | Collision Enabled: false 193 | Enabled: true 194 | Links: 195 | All Links Enabled: true 196 | Expand Joint Details: false 197 | Expand Link Details: false 198 | Expand Tree: false 199 | Link Tree Style: Links in Alphabetic Order 200 | base_link: 201 | Alpha: 1 202 | Show Axes: false 203 | Show Trail: false 204 | Value: true 205 | base_scan: 206 | Alpha: 1 207 | Show Axes: false 208 | Show Trail: false 209 | Value: true 210 | camera_link_fake: 211 | Alpha: 1 212 | Show Axes: false 213 | Show Trail: false 214 | Value: true 215 | head_disk: 216 | Alpha: 1 217 | Show Axes: false 218 | Show Trail: false 219 | Value: true 220 | kinect_support: 221 | Alpha: 1 222 | Show Axes: false 223 | Show Trail: false 224 | Value: true 225 | laser: 226 | Alpha: 1 227 | Show Axes: false 228 | Show Trail: false 229 | Value: true 230 | lower_torso: 231 | Alpha: 1 232 | Show Axes: false 233 | Show Trail: false 234 | Value: true 235 | upper_torso: 236 | Alpha: 1 237 | Show Axes: false 238 | Show Trail: false 239 | Value: true 240 | wheel_L: 241 | Alpha: 1 242 | Show Axes: false 243 | Show Trail: false 244 | Value: true 245 | wheel_R: 246 | Alpha: 1 247 | Show Axes: false 248 | Show Trail: false 249 | Value: true 250 | Name: RobotModel 251 | Robot Description: robot_description 252 | TF Prefix: "" 253 | Update Interval: 0 254 | Value: true 255 | Visual Enabled: true 256 | - Alpha: 1 257 | Autocompute Intensity Bounds: true 258 | Autocompute Value Bounds: 259 | Max Value: 10 260 | Min Value: -10 261 | Value: true 262 | Axis: Z 263 | Channel Name: intensity 264 | Class: rviz/PointCloud2 265 | Color: 255; 255; 255 266 | Color Transformer: RGB8 267 | Decay Time: 0 268 | Enabled: true 269 | Invert Rainbow: false 270 | Max Color: 255; 255; 255 271 | Max Intensity: 4096 272 | Min Color: 0; 0; 0 273 | Min Intensity: 0 274 | Name: semantic cloud 275 | Position Transformer: XYZ 276 | Queue Size: 10 277 | Selectable: true 278 | Size (Pixels): 3 279 | Size (m): 0.00999999978 280 | Style: Flat Squares 281 | Topic: /map_assembler/cloud_map 282 | Unreliable: false 283 | Use Fixed Frame: true 284 | Use rainbow: true 285 | Value: true 286 | - Alpha: 1 287 | Autocompute Intensity Bounds: true 288 | Autocompute Value Bounds: 289 | Max Value: 10 290 | Min Value: -10 291 | Value: true 292 | Axis: Z 293 | Channel Name: intensity 294 | Class: rviz/PointCloud2 295 | Color: 255; 255; 255 296 | Color Transformer: RGB8 297 | Decay Time: 0 298 | Enabled: true 299 | Invert Rainbow: false 300 | Max Color: 255; 255; 255 301 | Max Intensity: 4096 302 | Min Color: 0; 0; 0 303 | Min Intensity: 0 304 | Name: instance cloud 305 | Position Transformer: XYZ 306 | Queue Size: 10 307 | Selectable: true 308 | Size (Pixels): 3 309 | Size (m): 0.00999999978 310 | Style: Flat Squares 311 | Topic: /instance_model 312 | Unreliable: false 313 | Use Fixed Frame: true 314 | Use rainbow: true 315 | Value: true 316 | - Alpha: 0.699999988 317 | Class: rviz/Map 318 | Color Scheme: map 319 | Draw Behind: false 320 | Enabled: true 321 | Name: obstacle map 322 | Topic: /octomap_grid 323 | Unreliable: false 324 | Use Timestamp: false 325 | Value: true 326 | Enabled: true 327 | Global Options: 328 | Background Color: 48; 48; 48 329 | Default Light: true 330 | Fixed Frame: map 331 | Frame Rate: 30 332 | Name: root 333 | Tools: 334 | - Class: rviz/Interact 335 | Hide Inactive Objects: true 336 | - Class: rviz/MoveCamera 337 | - Class: rviz/Select 338 | - Class: rviz/FocusCamera 339 | - Class: rviz/Measure 340 | - Class: rviz/SetInitialPose 341 | Topic: /initialpose 342 | - Class: rviz/SetGoal 343 | Topic: /move_base_simple/goal 344 | - Class: rviz/PublishPoint 345 | Single click: true 346 | Topic: /clicked_point 347 | Value: true 348 | Views: 349 | Current: 350 | Class: rviz/Orbit 351 | Distance: 19.6404285 352 | Enable Stereo Rendering: 353 | Stereo Eye Separation: 0.0599999987 354 | Stereo Focal Distance: 1 355 | Swap Stereo Eyes: false 356 | Value: false 357 | Focal Point: 358 | X: -1.12706363 359 | Y: -3.7334075 360 | Z: -5.80372858 361 | Focal Shape Fixed Size: true 362 | Focal Shape Size: 0.0500000007 363 | Invert Z Axis: false 364 | Name: Current View 365 | Near Clip Distance: 0.00999999978 366 | Pitch: 0.584795415 367 | Target Frame: 368 | Value: Orbit (rviz) 369 | Yaw: 0.897280276 370 | Saved: ~ 371 | Window Geometry: 372 | Displays: 373 | collapsed: false 374 | Height: 1028 375 | Hide Left Dock: false 376 | Hide Right Dock: false 377 | QMainWindow State: 000000ff00000000fd0000000400000000000002a200000362fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006700fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004300000362000000e300fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001400640065007000740068005f007200650063007402000002e8000002420000017300000109fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e006701000001ea000001d70000000000000000000000010000010f00000362fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000004300000362000000b300fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000073f000000a9fc0100000002fb00000010007200670062005f0072006500630074020000046000000244000001750000010bfb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006380000003bfc0100000002fb0000000800540069006d00650100000000000006380000021f00fffffffb0000000800540069006d006501000000000000045000000000000000000000027b0000036200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 378 | Selection: 379 | collapsed: false 380 | Time: 381 | collapsed: false 382 | Tool Properties: 383 | collapsed: false 384 | Views: 385 | collapsed: false 386 | Width: 1592 387 | X: 1600 388 | Y: 21 389 | -------------------------------------------------------------------------------- /catkin_ws/src/rtabmap_ros/MapAssemblerNode.cpp: -------------------------------------------------------------------------------- 1 | /* The original implementation was modified to make it suitable for the project */ 2 | 3 | /* 4 | Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke 5 | All rights reserved. 6 | 7 | Redistribution and use in source and binary forms, with or without 8 | modification, are permitted provided that the following conditions are met: 9 | * Redistributions of source code must retain the above copyright 10 | notice, this list of conditions and the following disclaimer. 11 | * Redistributions in binary form must reproduce the above copyright 12 | notice, this list of conditions and the following disclaimer in the 13 | documentation and/or other materials provided with the distribution. 14 | * Neither the name of the Universite de Sherbrooke nor the 15 | names of its contributors may be used to endorse or promote products 16 | derived from this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 19 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 20 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY 22 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 23 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 24 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 25 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 27 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include 31 | #include "rtabmap_ros/MapData.h" 32 | #include "rtabmap_ros/MsgConversion.h" 33 | #include "rtabmap_ros/MapsManager.h" 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include "image_segmentation/maskImage.h" 49 | #include 50 | #include 51 | #include 52 | 53 | using namespace rtabmap; 54 | 55 | 56 | 57 | class MapAssembler 58 | { 59 | 60 | public: 61 | MapAssembler(int & argc, char** argv) 62 | { 63 | ros::NodeHandle pnh("~"); 64 | ros::NodeHandle nh; 65 | 66 | std::string configPath; 67 | pnh.param("config_path", configPath, configPath); 68 | 69 | //parameters 70 | rtabmap::ParametersMap parameters; 71 | uInsert(parameters, rtabmap::Parameters::getDefaultParameters("Grid")); 72 | uInsert(parameters, rtabmap::Parameters::getDefaultParameters("StereoBM")); 73 | if(!configPath.empty()) 74 | { 75 | if(UFile::exists(configPath.c_str())) 76 | { 77 | ROS_INFO( "%s: Loading parameters from %s", ros::this_node::getName().c_str(), configPath.c_str()); 78 | rtabmap::ParametersMap allParameters; 79 | Parameters::readINI(configPath.c_str(), allParameters); 80 | // only update odometry parameters 81 | for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter) 82 | { 83 | ParametersMap::iterator jter = allParameters.find(iter->first); 84 | if(jter!=allParameters.end()) 85 | { 86 | iter->second = jter->second; 87 | } 88 | } 89 | } 90 | else 91 | { 92 | ROS_ERROR( "Config file \"%s\" not found!", configPath.c_str()); 93 | } 94 | } 95 | for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter) 96 | { 97 | std::string vStr; 98 | bool vBool; 99 | int vInt; 100 | double vDouble; 101 | if(pnh.getParam(iter->first, vStr)) 102 | { 103 | ROS_INFO( "Setting %s parameter \"%s\"=\"%s\"", ros::this_node::getName().c_str(), iter->first.c_str(), vStr.c_str()); 104 | iter->second = vStr; 105 | } 106 | else if(pnh.getParam(iter->first, vBool)) 107 | { 108 | ROS_INFO( "Setting %s parameter \"%s\"=\"%s\"", ros::this_node::getName().c_str(), iter->first.c_str(), uBool2Str(vBool).c_str()); 109 | iter->second = uBool2Str(vBool); 110 | } 111 | else if(pnh.getParam(iter->first, vDouble)) 112 | { 113 | ROS_INFO( "Setting %s parameter \"%s\"=\"%s\"", ros::this_node::getName().c_str(), iter->first.c_str(), uNumber2Str(vDouble).c_str()); 114 | iter->second = uNumber2Str(vDouble); 115 | } 116 | else if(pnh.getParam(iter->first, vInt)) 117 | { 118 | ROS_INFO( "Setting %s parameter \"%s\"=\"%s\"", ros::this_node::getName().c_str(), iter->first.c_str(), uNumber2Str(vInt).c_str()); 119 | iter->second = uNumber2Str(vInt); 120 | } 121 | 122 | if(iter->first.compare(Parameters::kVisMinInliers()) == 0 && atoi(iter->second.c_str()) < 8) 123 | { 124 | ROS_WARN( "Parameter min_inliers must be >= 8, setting to 8..."); 125 | iter->second = uNumber2Str(8); 126 | } 127 | 128 | /* Limit the range here - this is my change*/ 129 | if(iter->first.compare(Parameters::kGridRangeMax()) == 0) 130 | { 131 | ROS_WARN( "Parameter found"); 132 | iter->second = uNumber2Str(2.0); 133 | } 134 | } 135 | 136 | rtabmap::ParametersMap argParameters = rtabmap::Parameters::parseArguments(argc, argv); 137 | for(rtabmap::ParametersMap::iterator iter=argParameters.begin(); iter!=argParameters.end(); ++iter) 138 | { 139 | rtabmap::ParametersMap::iterator jter = parameters.find(iter->first); 140 | if(jter!=parameters.end()) 141 | { 142 | ROS_INFO( "Update %s parameter \"%s\"=\"%s\" from arguments", ros::this_node::getName().c_str(), iter->first.c_str(), iter->second.c_str()); 143 | jter->second = iter->second; 144 | } 145 | } 146 | 147 | // Backward compatibility 148 | for(std::map >::const_iterator iter=Parameters::getRemovedParameters().begin(); 149 | iter!=Parameters::getRemovedParameters().end(); 150 | ++iter) 151 | { 152 | std::string vStr; 153 | if(pnh.getParam(iter->first, vStr)) 154 | { 155 | if(iter->second.first && parameters.find(iter->second.second) != parameters.end()) 156 | { 157 | // can be migrated 158 | parameters.at(iter->second.second)= vStr; 159 | ROS_WARN( "%s: Parameter name changed: \"%s\" -> \"%s\". Please update your launch file accordingly. Value \"%s\" is still set to the new parameter name.", 160 | ros::this_node::getName().c_str(), iter->first.c_str(), iter->second.second.c_str(), vStr.c_str()); 161 | } 162 | else 163 | { 164 | if(iter->second.second.empty()) 165 | { 166 | ROS_ERROR( "%s: Parameter \"%s\" doesn't exist anymore!", 167 | ros::this_node::getName().c_str(), iter->first.c_str()); 168 | } 169 | else 170 | { 171 | ROS_ERROR( "%s: Parameter \"%s\" doesn't exist anymore! You may look at this similar parameter: \"%s\"", 172 | ros::this_node::getName().c_str(), iter->first.c_str(), iter->second.second.c_str()); 173 | } 174 | } 175 | } 176 | } 177 | 178 | for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter) 179 | { 180 | std::string str = "Param: " + iter->first + " = \"" + iter->second + "\""; 181 | std::cout << 182 | str << 183 | std::setw(60 - str.size()) << 184 | " [" << 185 | rtabmap::Parameters::getDescription(iter->first).c_str() << 186 | "]" << 187 | std::endl; 188 | } 189 | 190 | mapsManager_.init(nh, pnh, ros::this_node::getName(), false); 191 | mapsManager_.backwardCompatibilityParameters(pnh, parameters); 192 | mapsManager_.setParameters(parameters); 193 | 194 | mapDataTopic_ = nh.subscribe("mapData", 1, &MapAssembler::mapDataReceivedCallback, this); 195 | client = nh.serviceClient("maskImage"); 196 | 197 | // private service 198 | resetService_ = pnh.advertiseService("reset", &MapAssembler::reset, this); 199 | } 200 | 201 | ~MapAssembler() 202 | { 203 | } 204 | 205 | void mapDataReceivedCallback(const rtabmap_ros::MapDataPtr & msg) 206 | { 207 | UTimer timer; 208 | cv::Mat image; 209 | cv::Mat masked_image; 210 | rtabmap_ros::MapData localMap = *msg; 211 | sensor_msgs::ImagePtr server_message; 212 | cv_bridge::CvImagePtr cv_ptr; 213 | 214 | std::map poses; 215 | std::multimap constraints; 216 | Transform mapOdom; 217 | 218 | rtabmap_ros::mapGraphFromROS(msg->graph, poses, constraints, mapOdom); 219 | 220 | for(unsigned int i=0; inodes.size(); ++i) 221 | { 222 | if(msg->nodes[i].image.size() || 223 | msg->nodes[i].depth.size() || 224 | msg->nodes[i].laserScan.size()) 225 | { 226 | 227 | image = rtabmap::uncompressImage(msg->nodes[i].image); 228 | server_message = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image).toImageMsg(); 229 | srv.request.inputImage = *server_message; 230 | 231 | if (client.call(srv)) 232 | { 233 | ROS_INFO("The request was processed"); 234 | 235 | try 236 | { 237 | cv_ptr = cv_bridge::toCvCopy(srv.response.maskedImage, sensor_msgs::image_encodings::BGR8); 238 | } 239 | catch(cv_bridge::Exception& e) 240 | { 241 | ROS_ERROR("cv_bridge exception: %s", e.what()); 242 | } 243 | 244 | int erosion_size = 3; 245 | cv::Mat element = cv::getStructuringElement( cv::MORPH_RECT, 246 | cv::Size(2*erosion_size + 1, 2*erosion_size+1 ), 247 | cv::Point(erosion_size, erosion_size ) ); 248 | 249 | cv::imshow( "Before erosion - masks", cv_ptr->image); 250 | 251 | masked_image = image.clone(); 252 | for(int row = 0; row < image.rows; row++) 253 | { 254 | for(int col = 0; col < image.cols; col++) 255 | { 256 | if(cv_ptr->image.at(row,col)[0] != 0 || 257 | cv_ptr->image.at(row,col)[1] != 0 || 258 | cv_ptr->image.at(row,col)[2] != 0) 259 | masked_image.at(row,col)[0] = 255; 260 | } 261 | } 262 | cv::imshow( "Before erosion - masked", masked_image); 263 | 264 | cv::erode( cv_ptr->image, cv_ptr->image, element , cv::Point(-1,-1), 2); 265 | 266 | masked_image = image.clone(); 267 | for(int row = 0; row < image.rows; row++) 268 | { 269 | for(int col = 0; col < image.cols; col++) 270 | { 271 | if(cv_ptr->image.at(row,col)[0] != 0 || 272 | cv_ptr->image.at(row,col)[1] != 0 || 273 | cv_ptr->image.at(row,col)[2] != 0) 274 | masked_image.at(row,col)[0] = 255; 275 | } 276 | } 277 | cv::imshow( "After erosion - masked", masked_image); 278 | cv::imshow( "After erosion - masks", cv_ptr->image); 279 | 280 | msg->nodes[i].image = rtabmap::compressImage(cv_ptr->image); 281 | 282 | cv::imshow( "Original image", image); 283 | } 284 | else 285 | { 286 | ROS_ERROR("Failed to process the request"); 287 | } 288 | 289 | msg->nodes[i].grid_cell_size = 0; 290 | 291 | uInsert(nodes_, std::make_pair(msg->nodes[i].id, rtabmap_ros::nodeDataFromROS(msg->nodes[i]))); 292 | } 293 | } 294 | 295 | // create a tmp signature with latest sensory data 296 | if(poses.size() && nodes_.find(poses.rbegin()->first) != nodes_.end()) 297 | { 298 | Signature tmpS = nodes_.at(poses.rbegin()->first); 299 | SensorData tmpData = tmpS.sensorData(); 300 | tmpData.setId(-1); 301 | uInsert(nodes_, std::make_pair(-1, Signature(-1, -1, 0, tmpS.getStamp(), "", tmpS.getPose(), Transform(), tmpData))); 302 | poses.insert(std::make_pair(-1, poses.rbegin()->second)); 303 | } 304 | 305 | // Update maps 306 | poses = mapsManager_.updateMapCaches( 307 | poses, 308 | 0, 309 | false, 310 | false, 311 | nodes_); 312 | 313 | mapsManager_.publishMaps(poses, msg->header.stamp, msg->header.frame_id); 314 | 315 | ROS_INFO("map_assembler: Publishing data = %fs", timer.ticks()); 316 | } 317 | 318 | bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&) 319 | { 320 | ROS_INFO("map_assembler: reset!"); 321 | mapsManager_.clear(); 322 | return true; 323 | } 324 | 325 | private: 326 | MapsManager mapsManager_; 327 | std::map nodes_; 328 | 329 | ros::Subscriber mapDataTopic_; 330 | image_segmentation::maskImage srv; 331 | ros::ServiceClient client; 332 | ros::ServiceServer resetService_; 333 | }; 334 | 335 | int main(int argc, char** argv) 336 | { 337 | ros::init(argc, argv, "map_assembler"); 338 | 339 | // process "--params" argument 340 | for(int i=1;ifirst + " = \"" + iter->second + "\""; 350 | std::cout << 351 | str << 352 | std::setw(60 - str.size()) << 353 | " [" << 354 | rtabmap::Parameters::getDescription(iter->first).c_str() << 355 | "]" << 356 | std::endl; 357 | } 358 | ROS_WARN("Node will now exit after showing default parameters because " 359 | "argument \"--params\" is detected!"); 360 | exit(0); 361 | } 362 | else if(strcmp(argv[i], "--udebug") == 0) 363 | { 364 | ULogger::setLevel(ULogger::kDebug); 365 | } 366 | else if(strcmp(argv[i], "--uinfo") == 0) 367 | { 368 | ULogger::setLevel(ULogger::kInfo); 369 | } 370 | } 371 | 372 | MapAssembler assembler(argc, argv); 373 | ros::spin(); 374 | return 0; 375 | } 376 | -------------------------------------------------------------------------------- /catkin_ws/src/image_segmentation/src/pcl_proc.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | 7 | #include 8 | 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | // PCL 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | #include 32 | #include 33 | 34 | const int class_colours[8][3] = 35 | { 36 | {255 , 0, 0}, 37 | {0, 255 , 0}, 38 | {0 , 0, 255 }, 39 | {100, 0 , 0 }, 40 | {0 , 100 , 0 }, 41 | {0 , 0 , 100 }, 42 | {255 , 255 , 0 }, 43 | {255 , 0 , 255 }, 44 | }; 45 | 46 | std::string class_names[8] = 47 | { 48 | "chair", 49 | "table", 50 | "window", 51 | "door", 52 | "box", 53 | "shelves", 54 | "sofa", 55 | "cabinet" 56 | }; 57 | 58 | /****** GLOBALS *************/ 59 | /* Publishers for pointclouds */ 60 | ros::Publisher pub_instance; 61 | ros::Publisher pub_target; 62 | ros::Publisher pub_goal_position; 63 | /* Global variables with instance selection information */ 64 | uint16_t global_class_id = 0; 65 | uint16_t global_instance_id = 0; 66 | bool global_start_moving = false; 67 | gridmap_2d::GridMap2D global_grid_map; 68 | 69 | /* Number of classes */ 70 | const uint8_t num_classes = 8; 71 | 72 | /* Function used to find the direction of the front of the chair */ 73 | bool find_chair_direction(pcl::PointCloud::Ptr cloud, pcl::ModelCoefficients::Ptr coefficients) 74 | { 75 | double max_positive = 0; 76 | double max_negative = 0; 77 | 78 | for(int i=0; ipoints.size(); i++) 79 | { 80 | double point_plane_dist; 81 | 82 | /* calculate the distance of point from plane (signed) */ 83 | point_plane_dist = cloud->points[i].x * coefficients->values[0] + 84 | cloud->points[i].y * coefficients->values[1] + 85 | cloud->points[i].z * coefficients->values[2] + 86 | coefficients->values[3] ; 87 | 88 | /* Keep track of maximum distance in each direction */ 89 | if(point_plane_dist > 0) 90 | { 91 | if(point_plane_dist>max_positive) 92 | { 93 | max_positive = point_plane_dist; 94 | } 95 | } 96 | else 97 | { 98 | if(-1*point_plane_dist>max_negative) 99 | { 100 | max_negative = -1*point_plane_dist; 101 | } 102 | } 103 | } 104 | 105 | /* Flip the plane if neccessary */ 106 | if(max_negative>max_positive) 107 | { 108 | coefficients->values[0] = -1 * coefficients->values[0]; 109 | coefficients->values[1] = -1 * coefficients->values[1]; 110 | coefficients->values[2] = -1 * coefficients->values[2]; 111 | } 112 | 113 | /* Return if plane had to be flipped */ 114 | return(max_negative>max_positive); 115 | } 116 | 117 | /* This function is used to check if given point is occupied */ 118 | bool check_if_valid(pcl::PointXYZ *outputPoint) 119 | { 120 | bool isOccupied = global_grid_map.isOccupiedAt(outputPoint->x, outputPoint->y); 121 | bool isDistanceValid = (global_grid_map.distanceMapAt(outputPoint->x, outputPoint->y) >= 0.5); 122 | 123 | return((!isOccupied) && isDistanceValid); 124 | } 125 | 126 | /* This function is used to find a clear point on given vector */ 127 | bool find_point_on_ray(pcl::PointCloud::Ptr cloud, Eigen::Vector2f coefficients, Eigen::Vector3f mean_point, pcl::PointXYZ *outputPoint, float distance_range) 128 | { 129 | bool pointFound = false; 130 | 131 | for(float current_distance = 0; current_distance<=distance_range; current_distance+=0.1) 132 | { 133 | outputPoint->x = mean_point(0) + current_distance*coefficients(0); 134 | outputPoint->y = mean_point(1) + current_distance*coefficients(1); 135 | 136 | if(check_if_valid(outputPoint)) 137 | { 138 | pointFound = true; 139 | break; 140 | } 141 | } 142 | 143 | return(pointFound); 144 | } 145 | 146 | /* This function is used to find a clear point on given vector */ 147 | bool find_point_on_arc(pcl::PointCloud::Ptr cloud, Eigen::Vector2f coefficients, Eigen::Vector3f mean_point, pcl::PointXYZ *outputPoint, float angle_range, float current_distance) 148 | { 149 | bool pointFound = false; 150 | Eigen::Vector2f rotated_coefficients; 151 | 152 | for(float current_angle = 0; current_angle<=angle_range; current_angle+=0.1) 153 | { 154 | rotated_coefficients(0) = coefficients(0)*cos(current_angle) - 155 | coefficients(1)*sin(current_angle); 156 | rotated_coefficients(1) = coefficients(0)*sin(current_angle) + 157 | coefficients(1)*cos(current_angle); 158 | 159 | outputPoint->x = mean_point(0) + current_distance*rotated_coefficients(0); 160 | outputPoint->y = mean_point(1) + current_distance*rotated_coefficients(1); 161 | 162 | if(check_if_valid(outputPoint)) 163 | { 164 | pointFound = true; 165 | break; 166 | } 167 | 168 | rotated_coefficients(0) = coefficients(0)*cos(-1*current_angle) - 169 | coefficients(1)*sin(-1*current_angle); 170 | rotated_coefficients(1) = coefficients(0)*sin(-1*current_angle) + 171 | coefficients(1)*cos(-1*current_angle); 172 | 173 | outputPoint->x = mean_point(0) + current_distance*rotated_coefficients(0); 174 | outputPoint->y = mean_point(1) + current_distance*rotated_coefficients(1); 175 | 176 | if(check_if_valid(outputPoint)) 177 | { 178 | pointFound = true; 179 | break; 180 | } 181 | } 182 | 183 | return(pointFound); 184 | } 185 | 186 | /* This function is used to find a clear point */ 187 | bool find_clear_point(pcl::PointCloud::Ptr cloud, Eigen::Vector2f coefficients, Eigen::Vector3f mean_point, pcl::PointXYZ *outputPoint, float angle_range, float distance_range) 188 | { 189 | bool pointFound = false; 190 | Eigen::Vector2f rotated_coefficients; 191 | 192 | for(float current_angle = 0; current_angle<=angle_range; current_angle+=0.1) 193 | { 194 | if(current_angle == 0) 195 | { 196 | rotated_coefficients(0) = coefficients(0); 197 | rotated_coefficients(1) = coefficients(1); 198 | pointFound = find_point_on_ray(cloud, rotated_coefficients, mean_point, outputPoint, distance_range); 199 | if(pointFound) 200 | { 201 | break; 202 | } 203 | } 204 | else 205 | { 206 | rotated_coefficients(0) = coefficients(0)*cos(current_angle) - 207 | coefficients(1)*sin(current_angle); 208 | rotated_coefficients(1) = coefficients(0)*sin(current_angle) + 209 | coefficients(1)*cos(current_angle); 210 | pointFound = find_point_on_ray(cloud, rotated_coefficients, mean_point, outputPoint, distance_range); 211 | if(pointFound) 212 | { 213 | break; 214 | } 215 | 216 | rotated_coefficients(0) = coefficients(0)*cos(-1*current_angle) - 217 | coefficients(1)*sin(-1*current_angle); 218 | rotated_coefficients(1) = coefficients(0)*sin(-1*current_angle) + 219 | coefficients(1)*cos(-1*current_angle); 220 | pointFound = find_point_on_ray(cloud, rotated_coefficients, mean_point, outputPoint, distance_range); 221 | if(pointFound) 222 | { 223 | break; 224 | } 225 | } 226 | } 227 | return pointFound; 228 | } 229 | 230 | bool find_clear_point_arc(pcl::PointCloud::Ptr cloud, Eigen::Vector2f coefficients, Eigen::Vector3f mean_point, pcl::PointXYZ *outputPoint, float angle_range, float distance_range) 231 | { 232 | bool pointFound = false; 233 | 234 | for(float current_distance = 0; current_distance<=distance_range; current_distance+=0.1) 235 | { 236 | pointFound = find_point_on_arc(cloud, coefficients, mean_point, outputPoint, angle_range, current_distance); 237 | if(pointFound) 238 | { 239 | break; 240 | } 241 | } 242 | return pointFound; 243 | } 244 | 245 | 246 | /* Main callback called when semantic pointcloud is available */ 247 | void callback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg) 248 | { 249 | /******** DECLARATIONS **********/ 250 | /* Create a ROS message to publish pointclouds */ 251 | sensor_msgs::PointCloud2 pcl_message; 252 | 253 | /* Matrix of vectors holding cluster extraction results */ 254 | std::vector cluster_indices[num_classes]; 255 | 256 | /* Allocate memory for the PCL pointcloud */ 257 | pcl::PointCloud::Ptr pclCloud (new pcl::PointCloud); 258 | 259 | /* KdTree object */ 260 | pcl::search::KdTree::Ptr tree (new pcl::search::KdTree); 261 | 262 | /* Cluster extraction object */ 263 | pcl::EuclideanClusterExtraction extractor; 264 | 265 | /* Create a vector with point clouds for each of the classes */ 266 | pcl::PointCloud::Ptr cloud_class[num_classes]; 267 | for(int i =0; i::Ptr(new pcl::PointCloud); 270 | } 271 | 272 | 273 | 274 | /* Save the header */ 275 | std_msgs::Header header; 276 | header = cloud_msg->header; 277 | 278 | /* Convert the recieved message to PCL pointcloud */ 279 | pcl::fromROSMsg(*cloud_msg, *pclCloud); 280 | 281 | /* Split the general cloud into class clouds */ 282 | for(int i=0; ipoints.size(); i++) 283 | { 284 | for(int j = 0; jpoints[i].r == class_colours[j][0] && 287 | (int)pclCloud->points[i].g == class_colours[j][1] && 288 | (int)pclCloud->points[i].b == class_colours[j][2] ) 289 | { 290 | cloud_class[j]->points.push_back(pclCloud->points[i]); 291 | break; 292 | } 293 | } 294 | } 295 | 296 | /* Display information */ 297 | ROS_INFO("Finished spliting cloud into class clouds"); 298 | ROS_INFO("Total points: %d", (int)pclCloud->points.size()); 299 | 300 | for(int i =0; ipoints.size()); 303 | } 304 | 305 | 306 | 307 | 308 | /******* CLUSTER EXTRACTION ********/ 309 | 310 | /* Set extraction parameters */ 311 | extractor.setClusterTolerance (0.1); 312 | extractor.setMinClusterSize (50); 313 | extractor.setMaxClusterSize (25000); 314 | 315 | /* Extract clusters from each class pointcloud */ 316 | for(int i =0; ipoints.size() == 0) 320 | { 321 | continue; 322 | } 323 | 324 | /* Create the tree */ 325 | tree->setInputCloud(cloud_class[i]); 326 | 327 | /* Select pointcloud */ 328 | extractor.setSearchMethod(tree); 329 | extractor.setInputCloud(cloud_class[i]); 330 | 331 | /* Extract cluster indices */ 332 | extractor.extract(cluster_indices[i]); 333 | 334 | ROS_INFO("For class %d extracted %d clusters", i, (int)cluster_indices[i].size()); 335 | } 336 | 337 | 338 | 339 | 340 | /* 341 | Go through all extracted clusters to: 342 | -calculate mean 343 | -save the pointcloud to file 344 | */ 345 | 346 | /* Declarations for this part */ 347 | pcl::VectorAverage vector_mean; 348 | pcl::PointXYZRGB newPoint; 349 | int instance_id = 0; 350 | std::ofstream myfile; 351 | pcl::PointIndices::Ptr one_side(new pcl::PointIndices()); 352 | pcl::ExtractIndices extract; 353 | 354 | /* Open results file */ 355 | myfile.open ("/home/aut/s160948/results.txt", std::ofstream::out | std::ofstream::trunc); 356 | 357 | for(int class_id = 0; class_id::const_iterator it = cluster_indices[class_id].begin(); it != cluster_indices[class_id].end (); ++it) 361 | { 362 | /* Get the currently processed instance id */ 363 | instance_id = (int)std::distance::const_iterator>(cluster_indices[class_id].begin(), it); 364 | 365 | /* Reset the vector mean data */ 366 | vector_mean.reset(); 367 | 368 | /* Create a cloud and assign a readom color for this instance */ 369 | pcl::PointCloud::Ptr cloud_cluster (new pcl::PointCloud); 370 | 371 | /* Loop through all of the indices for currently processed instance */ 372 | for (std::vector::const_iterator pit = it->indices.begin(); pit != it->indices.end (); ++pit) 373 | { 374 | /* Create a new point with selected colour */ 375 | newPoint.x = cloud_class[class_id]->points[*pit].x; 376 | newPoint.y = cloud_class[class_id]->points[*pit].y; 377 | newPoint.z = cloud_class[class_id]->points[*pit].z; 378 | newPoint.r = 255; 379 | newPoint.g = 255; 380 | newPoint.b = 255; 381 | /* Push the point the the cloud */ 382 | cloud_cluster->points.push_back(newPoint); 383 | 384 | /* Add point to mean calculation */ 385 | vector_mean.add(Eigen::Vector3f(newPoint.x, newPoint.y, newPoint.z)); 386 | } 387 | 388 | /* The cluster cloud has been created - calculate mean */ 389 | Eigen::Vector3f mean_val = vector_mean.getMean(); 390 | 391 | /* Print Information */ 392 | myfile << "Instance " << instance_id << " of class " << class_names[class_id] << " consists of " << (int)cloud_cluster->points.size() << " points.\n"; 393 | myfile << "Mean x: " << mean_val(0) << " y: " << mean_val(1) << " z: " << mean_val(2) << std::endl << std::endl; 394 | ROS_INFO("Cluster %d of class %d consists of %d points", instance_id, class_id, (int)cloud_cluster->points.size()); 395 | ROS_INFO("Mean x: %f, y: %f, z: %f", mean_val(0), mean_val(1), mean_val(2)); 396 | 397 | /* Publish the selected instance */ 398 | if(class_id == global_class_id && instance_id == global_instance_id) 399 | { 400 | pcl::PointXYZ targetPoint; 401 | Eigen::Vector2f target_coefficients; 402 | Eigen::Vector2f initial_angle; 403 | bool target_found; 404 | /* For the selected instance (if it's a chair) find the front */ 405 | if(class_id == 0) 406 | { 407 | /* Find a vertical plane */ 408 | pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients); 409 | pcl::PointIndices::Ptr inliers (new pcl::PointIndices); 410 | 411 | // Create the segmentation object 412 | pcl::SACSegmentation seg; 413 | seg.setOptimizeCoefficients (true); 414 | seg.setModelType (pcl::SACMODEL_PARALLEL_PLANE); 415 | seg.setMethodType (pcl::SAC_RANSAC); 416 | seg.setDistanceThreshold (0.05); 417 | seg.setAxis (Eigen::Vector3f (0.0, 0.0, 1.0)); 418 | seg.setEpsAngle (0.4); //+-10deg 419 | seg.setMaxIterations(200); 420 | 421 | seg.setInputCloud(cloud_cluster); 422 | seg.segment (*inliers, *coefficients); 423 | 424 | /* Split the general cloud into class clouds */ 425 | find_chair_direction(cloud_cluster, coefficients); 426 | 427 | initial_angle(0) = coefficients->values[0]; 428 | initial_angle(1) = coefficients->values[1]; 429 | 430 | target_found = find_clear_point(cloud_cluster, initial_angle, mean_val, &targetPoint, 0.5, 1.5); 431 | } 432 | else 433 | { 434 | initial_angle(0) = 0; 435 | initial_angle(1) = 1; 436 | target_found = find_clear_point_arc(cloud_cluster, initial_angle, mean_val, &targetPoint, M_PI, 1.5); 437 | } 438 | 439 | /* Calculate target direction vector */ 440 | double x = mean_val(0)-targetPoint.x; 441 | double y = mean_val(1)-targetPoint.y; 442 | target_coefficients(0) = x/sqrt(pow(x,2)+pow(y,2)); 443 | target_coefficients(1) = y/sqrt(pow(x,2)+pow(y,2)); 444 | 445 | if(target_found && global_start_moving) 446 | { 447 | geometry_msgs::PoseStamped goal_pose; 448 | 449 | goal_pose.header.stamp = header.stamp; 450 | goal_pose.header.frame_id = header.frame_id; 451 | 452 | goal_pose.pose.position.x = targetPoint.x; 453 | goal_pose.pose.position.y = targetPoint.y; 454 | goal_pose.pose.position.z = 0; 455 | 456 | goal_pose.pose.orientation = tf::createQuaternionMsgFromYaw(asin(targetPoint.y)); 457 | 458 | pub_goal_position.publish(goal_pose); 459 | global_start_moving = false; 460 | } 461 | 462 | /* Set cloud parameters redo as points were added */ 463 | cloud_cluster->width = cloud_cluster->points.size(); 464 | cloud_cluster->height = 1; 465 | cloud_cluster->is_dense = true; 466 | 467 | pcl::toROSMsg(*cloud_cluster,pcl_message); 468 | /* Copy the header */ 469 | pcl_message.header.frame_id = header.frame_id; 470 | pcl_message.header.stamp = header.stamp; 471 | 472 | /* Publish the pointcloud */ 473 | pub_instance.publish(pcl_message); 474 | 475 | /* Create a cloud */ 476 | pcl::PointCloud::Ptr targetpoint_cluster (new pcl::PointCloud); 477 | pcl::PointXYZRGB pnt; 478 | float radius = 0.01; 479 | float px, py; 480 | for (float phi=0; phi < M_PI; phi+=M_PI/50) 481 | { 482 | for (float theta=0; theta<2*M_PI;theta+=2*M_PI/50) 483 | { 484 | px = radius*sin(phi)*cos(theta)+targetPoint.x; 485 | py = radius*sin(phi)*sin(theta)+targetPoint.y; 486 | pnt.x = px; 487 | pnt.y = py; 488 | pnt.z = 0; 489 | pnt.r = 0; 490 | pnt.g = 255; 491 | pnt.b = 0; 492 | targetpoint_cluster->points.push_back(pnt); 493 | } 494 | } 495 | 496 | /* Show line */ 497 | for(double distfromstart=0.0;distfromstart<0.5;distfromstart+=0.02) 498 | { 499 | pnt.x = targetPoint.x + distfromstart*target_coefficients(0); 500 | pnt.y = targetPoint.y + distfromstart*target_coefficients(1); 501 | pnt.z = 0; 502 | pnt.r = 0; 503 | pnt.g = 255; 504 | pnt.b = 0; 505 | targetpoint_cluster->points.push_back(pnt); 506 | } 507 | /* Set cloud parameters redo as points were added */ 508 | targetpoint_cluster->width = targetpoint_cluster->points.size(); 509 | targetpoint_cluster->height = 1; 510 | targetpoint_cluster->is_dense = true; 511 | 512 | pcl::toROSMsg(*targetpoint_cluster,pcl_message); 513 | /* Copy the header */ 514 | pcl_message.header.frame_id = header.frame_id; 515 | pcl_message.header.stamp = header.stamp; 516 | 517 | /* Publish the pointcloud */ 518 | pub_target.publish(pcl_message); 519 | } 520 | } 521 | 522 | myfile << std::endl << std::endl; 523 | } 524 | 525 | 526 | /* Close results file */ 527 | myfile.close(); 528 | 529 | } 530 | 531 | /* Callback for class id setting */ 532 | void callback_class(const std_msgs::UInt16::ConstPtr& class_id) 533 | { 534 | global_class_id = (uint16_t)class_id->data; 535 | ROS_INFO("Recieved global class id: %d", global_class_id); 536 | } 537 | 538 | /* Callback for instance id setting */ 539 | void callback_instance(const std_msgs::UInt16::ConstPtr& instance_id) 540 | { 541 | global_instance_id = (uint16_t)instance_id->data; 542 | ROS_INFO("Recieved global instance id: %d", global_instance_id); 543 | } 544 | 545 | void callback_map(const nav_msgs::OccupancyGrid::ConstPtr& msg) 546 | { 547 | ROS_INFO("Setting global grid map"); 548 | ROS_INFO("Width %d height %d", msg->info.width, msg->info.height); 549 | global_grid_map.setMap(msg); 550 | ROS_INFO("Map set"); 551 | } 552 | 553 | void callback_start(const std_msgs::Bool::ConstPtr& start_allowed) 554 | { 555 | global_start_moving = (bool)start_allowed->data; 556 | ROS_INFO("Recieved start going msg"); 557 | } 558 | 559 | int main(int argc, char** argv) 560 | { 561 | ros::init(argc, argv, "instance_extractor"); 562 | ros::NodeHandle nh; 563 | 564 | /* Create subsribers */ 565 | //ros::Subscriber sub = nh.subscribe("/map_assembler/cloud_map", 1, callback); 566 | ros::Subscriber sub = nh.subscribe("/map_assembler/cloud_map", 1, callback); 567 | ros::Subscriber sub_class = nh.subscribe("/set_class", 1, callback_class); 568 | ros::Subscriber sub_instance = nh.subscribe("/set_instance", 1, callback_instance); 569 | ros::Subscriber sub_cost_map = nh.subscribe("/octomap_grid", 1, callback_map); 570 | ros::Subscriber sub_start = nh.subscribe("/start_going", 1, callback_start); 571 | 572 | /* Create publisher */ 573 | pub_instance = nh.advertise ("/instance_model", 1); 574 | pub_target = nh.advertise ("/target_point", 1); 575 | pub_goal_position = nh.advertise ("move_base_simple/goal", 1); 576 | 577 | ROS_INFO("Started processing - clustering pcl"); 578 | 579 | ros::spin(); 580 | return 0; 581 | } 582 | -------------------------------------------------------------------------------- /rtabmap/util3d.cpp: -------------------------------------------------------------------------------- 1 | /* Small modification added by Szymon Kowalewski to make the implementation suitable for the project. 2 | Look for MODIFCIATION tag 3 | */ 4 | 5 | /* 6 | Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke 7 | All rights reserved. 8 | 9 | Redistribution and use in source and binary forms, with or without 10 | modification, are permitted provided that the following conditions are met: 11 | * Redistributions of source code must retain the above copyright 12 | notice, this list of conditions and the following disclaimer. 13 | * Redistributions in binary form must reproduce the above copyright 14 | notice, this list of conditions and the following disclaimer in the 15 | documentation and/or other materials provided with the distribution. 16 | * Neither the name of the Universite de Sherbrooke nor the 17 | names of its contributors may be used to endorse or promote products 18 | derived from this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 21 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 22 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY 24 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 25 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 26 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 27 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 28 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 29 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | 47 | namespace rtabmap 48 | { 49 | 50 | namespace util3d 51 | { 52 | 53 | cv::Mat bgrFromCloud(const pcl::PointCloud & cloud, bool bgrOrder) 54 | { 55 | cv::Mat frameBGR = cv::Mat(cloud.height,cloud.width,CV_8UC3); 56 | 57 | for(unsigned int h = 0; h < cloud.height; h++) 58 | { 59 | for(unsigned int w = 0; w < cloud.width; w++) 60 | { 61 | if(bgrOrder) 62 | { 63 | frameBGR.at(h,w)[0] = cloud.at(h*cloud.width + w).b; 64 | frameBGR.at(h,w)[1] = cloud.at(h*cloud.width + w).g; 65 | frameBGR.at(h,w)[2] = cloud.at(h*cloud.width + w).r; 66 | } 67 | else 68 | { 69 | frameBGR.at(h,w)[0] = cloud.at(h*cloud.width + w).r; 70 | frameBGR.at(h,w)[1] = cloud.at(h*cloud.width + w).g; 71 | frameBGR.at(h,w)[2] = cloud.at(h*cloud.width + w).b; 72 | } 73 | } 74 | } 75 | return frameBGR; 76 | } 77 | 78 | // return float image in meter 79 | cv::Mat depthFromCloud( 80 | const pcl::PointCloud & cloud, 81 | float & fx, 82 | float & fy, 83 | bool depth16U) 84 | { 85 | cv::Mat frameDepth = cv::Mat(cloud.height,cloud.width,depth16U?CV_16UC1:CV_32FC1); 86 | fx = 0.0f; // needed to reconstruct the cloud 87 | fy = 0.0f; // needed to reconstruct the cloud 88 | for(unsigned int h = 0; h < cloud.height; h++) 89 | { 90 | for(unsigned int w = 0; w < cloud.width; w++) 91 | { 92 | float depth = cloud.at(h*cloud.width + w).z; 93 | if(depth16U) 94 | { 95 | depth *= 1000.0f; 96 | unsigned short depthMM = 0; 97 | if(depth <= (float)USHRT_MAX) 98 | { 99 | depthMM = (unsigned short)depth; 100 | } 101 | frameDepth.at(h,w) = depthMM; 102 | } 103 | else 104 | { 105 | frameDepth.at(h,w) = depth; 106 | } 107 | 108 | // update constants 109 | if(fx == 0.0f && 110 | uIsFinite(cloud.at(h*cloud.width + w).x) && 111 | uIsFinite(depth) && 112 | w != cloud.width/2 && 113 | depth > 0) 114 | { 115 | fx = cloud.at(h*cloud.width + w).x / ((float(w) - float(cloud.width)/2.0f) * depth); 116 | if(depth16U) 117 | { 118 | fx*=1000.0f; 119 | } 120 | } 121 | if(fy == 0.0f && 122 | uIsFinite(cloud.at(h*cloud.width + w).y) && 123 | uIsFinite(depth) && 124 | h != cloud.height/2 && 125 | depth > 0) 126 | { 127 | fy = cloud.at(h*cloud.width + w).y / ((float(h) - float(cloud.height)/2.0f) * depth); 128 | if(depth16U) 129 | { 130 | fy*=1000.0f; 131 | } 132 | } 133 | } 134 | } 135 | return frameDepth; 136 | } 137 | 138 | // return (unsigned short 16bits image in mm) (float 32bits image in m) 139 | void rgbdFromCloud(const pcl::PointCloud & cloud, 140 | cv::Mat & frameBGR, 141 | cv::Mat & frameDepth, 142 | float & fx, 143 | float & fy, 144 | bool bgrOrder, 145 | bool depth16U) 146 | { 147 | frameDepth = cv::Mat(cloud.height,cloud.width,depth16U?CV_16UC1:CV_32FC1); 148 | frameBGR = cv::Mat(cloud.height,cloud.width,CV_8UC3); 149 | 150 | fx = 0.0f; // needed to reconstruct the cloud 151 | fy = 0.0f; // needed to reconstruct the cloud 152 | for(unsigned int h = 0; h < cloud.height; h++) 153 | { 154 | for(unsigned int w = 0; w < cloud.width; w++) 155 | { 156 | //rgb 157 | if(bgrOrder) 158 | { 159 | frameBGR.at(h,w)[0] = cloud.at(h*cloud.width + w).b; 160 | frameBGR.at(h,w)[1] = cloud.at(h*cloud.width + w).g; 161 | frameBGR.at(h,w)[2] = cloud.at(h*cloud.width + w).r; 162 | } 163 | else 164 | { 165 | frameBGR.at(h,w)[0] = cloud.at(h*cloud.width + w).r; 166 | frameBGR.at(h,w)[1] = cloud.at(h*cloud.width + w).g; 167 | frameBGR.at(h,w)[2] = cloud.at(h*cloud.width + w).b; 168 | } 169 | 170 | //depth 171 | float depth = cloud.at(h*cloud.width + w).z; 172 | if(depth16U) 173 | { 174 | depth *= 1000.0f; 175 | unsigned short depthMM = 0; 176 | if(depth <= (float)USHRT_MAX) 177 | { 178 | depthMM = (unsigned short)depth; 179 | } 180 | frameDepth.at(h,w) = depthMM; 181 | } 182 | else 183 | { 184 | frameDepth.at(h,w) = depth; 185 | } 186 | 187 | // update constants 188 | if(fx == 0.0f && 189 | uIsFinite(cloud.at(h*cloud.width + w).x) && 190 | uIsFinite(depth) && 191 | w != cloud.width/2 && 192 | depth > 0) 193 | { 194 | fx = 1.0f/(cloud.at(h*cloud.width + w).x / ((float(w) - float(cloud.width)/2.0f) * depth)); 195 | if(depth16U) 196 | { 197 | fx/=1000.0f; 198 | } 199 | } 200 | if(fy == 0.0f && 201 | uIsFinite(cloud.at(h*cloud.width + w).y) && 202 | uIsFinite(depth) && 203 | h != cloud.height/2 && 204 | depth > 0) 205 | { 206 | fy = 1.0f/(cloud.at(h*cloud.width + w).y / ((float(h) - float(cloud.height)/2.0f) * depth)); 207 | if(depth16U) 208 | { 209 | fy/=1000.0f; 210 | } 211 | } 212 | } 213 | } 214 | } 215 | 216 | pcl::PointXYZ projectDepthTo3D( 217 | const cv::Mat & depthImage, 218 | float x, float y, 219 | float cx, float cy, 220 | float fx, float fy, 221 | bool smoothing, 222 | float depthErrorRatio) 223 | { 224 | UASSERT(depthImage.type() == CV_16UC1 || depthImage.type() == CV_32FC1); 225 | 226 | pcl::PointXYZ pt; 227 | 228 | float depth = util2d::getDepth(depthImage, x, y, smoothing, depthErrorRatio); 229 | if(depth > 0.0f) 230 | { 231 | // Use correct principal point from calibration 232 | cx = cx > 0.0f ? cx : float(depthImage.cols/2) - 0.5f; //cameraInfo.K.at(2) 233 | cy = cy > 0.0f ? cy : float(depthImage.rows/2) - 0.5f; //cameraInfo.K.at(5) 234 | 235 | // Fill in XYZ 236 | pt.x = (x - cx) * depth / fx; 237 | pt.y = (y - cy) * depth / fy; 238 | pt.z = depth; 239 | } 240 | else 241 | { 242 | pt.x = pt.y = pt.z = std::numeric_limits::quiet_NaN(); 243 | } 244 | return pt; 245 | } 246 | 247 | Eigen::Vector3f projectDepthTo3DRay( 248 | const cv::Size & imageSize, 249 | float x, float y, 250 | float cx, float cy, 251 | float fx, float fy) 252 | { 253 | Eigen::Vector3f ray; 254 | 255 | // Use correct principal point from calibration 256 | cx = cx > 0.0f ? cx : float(imageSize.width/2) - 0.5f; //cameraInfo.K.at(2) 257 | cy = cy > 0.0f ? cy : float(imageSize.height/2) - 0.5f; //cameraInfo.K.at(5) 258 | 259 | // Fill in XYZ 260 | ray[0] = (x - cx) / fx; 261 | ray[1] = (y - cy) / fy; 262 | ray[2] = 1.0f; 263 | 264 | return ray; 265 | } 266 | 267 | pcl::PointCloud::Ptr cloudFromDepth( 268 | const cv::Mat & imageDepth, 269 | float cx, float cy, 270 | float fx, float fy, 271 | int decimation, 272 | float maxDepth, 273 | float minDepth, 274 | std::vector * validIndices) 275 | { 276 | CameraModel model(fx, fy, cx, cy); 277 | return cloudFromDepth(imageDepth, model, decimation, maxDepth, minDepth, validIndices); 278 | } 279 | 280 | pcl::PointCloud::Ptr cloudFromDepth( 281 | const cv::Mat & imageDepthIn, 282 | const CameraModel & model, 283 | int decimation, 284 | float maxDepth, 285 | float minDepth, 286 | std::vector * validIndices) 287 | { 288 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud); 289 | if(decimation == 0) 290 | { 291 | decimation = 1; 292 | } 293 | float rgbToDepthFactorX = 1.0f; 294 | float rgbToDepthFactorY = 1.0f; 295 | 296 | UASSERT(model.isValidForProjection()); 297 | UASSERT(!imageDepthIn.empty() && (imageDepthIn.type() == CV_16UC1 || imageDepthIn.type() == CV_32FC1)); 298 | 299 | cv::Mat imageDepth = imageDepthIn; 300 | if(model.imageHeight()>0 && model.imageWidth()>0) 301 | { 302 | UASSERT(model.imageHeight() % imageDepthIn.rows == 0 && model.imageWidth() % imageDepthIn.cols == 0); 303 | 304 | if(decimation < 0) 305 | { 306 | UDEBUG("Decimation from model (%d)", decimation); 307 | if(model.imageHeight() % decimation != 0) 308 | { 309 | UERROR("Decimation is not valid for current image size (model.imageHeight()=%d decimation=%d). The cloud is not created.", model.imageHeight(), decimation); 310 | return cloud; 311 | } 312 | if(model.imageWidth() % decimation != 0) 313 | { 314 | UERROR("Decimation is not valid for current image size (model.imageWidth()=%d decimation=%d). The cloud is not created.", model.imageWidth(), decimation); 315 | return cloud; 316 | } 317 | 318 | // decimate from RGB image size, upsample depth if needed 319 | decimation = -1*decimation; 320 | 321 | int targetSize = model.imageHeight() / decimation; 322 | if(targetSize > imageDepthIn.rows) 323 | { 324 | UDEBUG("Depth interpolation factor=%d", targetSize/imageDepthIn.rows); 325 | imageDepth = util2d::interpolate(imageDepthIn, targetSize/imageDepthIn.rows); 326 | decimation = 1; 327 | } 328 | else if(targetSize == imageDepthIn.rows) 329 | { 330 | decimation = 1; 331 | } 332 | else 333 | { 334 | UASSERT(imageDepthIn.rows % targetSize == 0); 335 | decimation = imageDepthIn.rows / targetSize; 336 | } 337 | } 338 | else 339 | { 340 | if(imageDepthIn.rows % decimation != 0) 341 | { 342 | UERROR("Decimation is not valid for current image size (imageDepth.rows=%d decimation=%d). The cloud is not created.", imageDepthIn.rows, decimation); 343 | return cloud; 344 | } 345 | if(imageDepthIn.cols % decimation != 0) 346 | { 347 | UERROR("Decimation is not valid for current image size (imageDepth.cols=%d decimation=%d). The cloud is not created.", imageDepthIn.cols, decimation); 348 | return cloud; 349 | } 350 | } 351 | 352 | rgbToDepthFactorX = 1.0f/float((model.imageWidth() / imageDepth.cols)); 353 | rgbToDepthFactorY = 1.0f/float((model.imageHeight() / imageDepth.rows)); 354 | } 355 | else 356 | { 357 | decimation = abs(decimation); 358 | UASSERT_MSG(imageDepth.rows % decimation == 0, uFormat("rows=%d decimation=%d", imageDepth.rows, decimation).c_str()); 359 | UASSERT_MSG(imageDepth.cols % decimation == 0, uFormat("cols=%d decimation=%d", imageDepth.cols, decimation).c_str()); 360 | } 361 | 362 | //cloud.header = cameraInfo.header; 363 | cloud->height = imageDepth.rows/decimation; 364 | cloud->width = imageDepth.cols/decimation; 365 | cloud->is_dense = false; 366 | cloud->resize(cloud->height * cloud->width); 367 | if(validIndices) 368 | { 369 | validIndices->resize(cloud->size()); 370 | } 371 | 372 | float depthFx = model.fx() * rgbToDepthFactorX; 373 | float depthFy = model.fy() * rgbToDepthFactorY; 374 | float depthCx = model.cx() * rgbToDepthFactorX; 375 | float depthCy = model.cy() * rgbToDepthFactorY; 376 | 377 | UDEBUG("depth=%dx%d fx=%f fy=%f cx=%f cy=%f (depth factors=%f %f) decimation=%d", 378 | imageDepth.cols, imageDepth.rows, 379 | model.fx(), model.fy(), model.cx(), model.cy(), 380 | rgbToDepthFactorX, 381 | rgbToDepthFactorY, 382 | decimation); 383 | 384 | int oi = 0; 385 | for(int h = 0; h < imageDepth.rows && h/decimation < (int)cloud->height; h+=decimation) 386 | { 387 | for(int w = 0; w < imageDepth.cols && w/decimation < (int)cloud->width; w+=decimation) 388 | { 389 | pcl::PointXYZ & pt = cloud->at((h/decimation)*cloud->width + (w/decimation)); 390 | 391 | pcl::PointXYZ ptXYZ = projectDepthTo3D(imageDepth, w, h, depthCx, depthCy, depthFx, depthFy, false); 392 | if(pcl::isFinite(ptXYZ) && ptXYZ.z>=minDepth && (maxDepth<=0.0f || ptXYZ.z <= maxDepth)) 393 | { 394 | pt.x = ptXYZ.x; 395 | pt.y = ptXYZ.y; 396 | pt.z = ptXYZ.z; 397 | if(validIndices) 398 | { 399 | validIndices->at(oi++) = (h/decimation)*cloud->width + (w/decimation); 400 | } 401 | } 402 | else 403 | { 404 | pt.x = pt.y = pt.z = std::numeric_limits::quiet_NaN(); 405 | } 406 | } 407 | } 408 | 409 | if(validIndices) 410 | { 411 | validIndices->resize(oi); 412 | } 413 | 414 | return cloud; 415 | } 416 | 417 | pcl::PointCloud::Ptr cloudFromDepthRGB( 418 | const cv::Mat & imageRgb, 419 | const cv::Mat & imageDepth, 420 | float cx, float cy, 421 | float fx, float fy, 422 | int decimation, 423 | float maxDepth, 424 | float minDepth, 425 | std::vector * validIndices) 426 | { 427 | CameraModel model(fx, fy, cx, cy); 428 | return cloudFromDepthRGB(imageRgb, imageDepth, model, decimation, maxDepth, minDepth, validIndices); 429 | } 430 | 431 | pcl::PointCloud::Ptr cloudFromDepthRGB( 432 | const cv::Mat & imageRgb, 433 | const cv::Mat & imageDepthIn, 434 | const CameraModel & model, 435 | int decimation, 436 | float maxDepth, 437 | float minDepth, 438 | std::vector * validIndices) 439 | { 440 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud); 441 | if(decimation == 0) 442 | { 443 | decimation = 1; 444 | } 445 | UDEBUG(""); 446 | UASSERT(model.isValidForProjection()); 447 | UASSERT_MSG((model.imageHeight() == 0 && model.imageWidth() == 0) || 448 | (model.imageHeight() == imageRgb.rows && model.imageWidth() == imageRgb.cols), 449 | uFormat("model=%dx%d rgb=%dx%d", model.imageWidth(), model.imageHeight(), imageRgb.cols, imageRgb.rows).c_str()); 450 | UASSERT_MSG(imageRgb.rows % imageDepthIn.rows == 0 && imageRgb.cols % imageDepthIn.cols == 0, 451 | uFormat("rgb=%dx%d depth=%dx%d", imageRgb.cols, imageRgb.rows, imageDepthIn.cols, imageDepthIn.rows).c_str()); 452 | UASSERT(!imageDepthIn.empty() && (imageDepthIn.type() == CV_16UC1 || imageDepthIn.type() == CV_32FC1)); 453 | if(decimation < 0) 454 | { 455 | if(imageRgb.rows % decimation != 0 || imageRgb.cols % decimation != 0) 456 | { 457 | int oldDecimation = decimation; 458 | while(decimation <= -1) 459 | { 460 | if(imageRgb.rows % decimation == 0 && imageRgb.cols % decimation == 0) 461 | { 462 | break; 463 | } 464 | ++decimation; 465 | } 466 | 467 | if(imageRgb.rows % oldDecimation != 0 || imageRgb.cols % oldDecimation != 0) 468 | { 469 | UWARN("Decimation (%d) is not valid for current image size (rgb=%dx%d). Highest compatible decimation used=%d.", oldDecimation, imageRgb.cols, imageRgb.rows, decimation); 470 | } 471 | } 472 | } 473 | else 474 | { 475 | if(imageDepthIn.rows % decimation != 0 || imageDepthIn.cols % decimation != 0) 476 | { 477 | int oldDecimation = decimation; 478 | while(decimation >= 1) 479 | { 480 | if(imageDepthIn.rows % decimation == 0 && imageDepthIn.cols % decimation == 0) 481 | { 482 | break; 483 | } 484 | --decimation; 485 | } 486 | 487 | if(imageDepthIn.rows % oldDecimation != 0 || imageDepthIn.cols % oldDecimation != 0) 488 | { 489 | UWARN("Decimation (%d) is not valid for current image size (depth=%dx%d). Highest compatible decimation used=%d.", oldDecimation, imageDepthIn.cols, imageDepthIn.rows, decimation); 490 | } 491 | } 492 | } 493 | 494 | cv::Mat imageDepth = imageDepthIn; 495 | if(decimation < 0) 496 | { 497 | UDEBUG("Decimation from RGB image (%d)", decimation); 498 | // decimate from RGB image size, upsample depth if needed 499 | decimation = -1*decimation; 500 | 501 | int targetSize = imageRgb.rows / decimation; 502 | if(targetSize > imageDepthIn.rows) 503 | { 504 | UDEBUG("Depth interpolation factor=%d", targetSize/imageDepthIn.rows); 505 | imageDepth = util2d::interpolate(imageDepthIn, targetSize/imageDepthIn.rows); 506 | decimation = 1; 507 | } 508 | else if(targetSize == imageDepthIn.rows) 509 | { 510 | decimation = 1; 511 | } 512 | else 513 | { 514 | UASSERT(imageDepthIn.rows % targetSize == 0); 515 | decimation = imageDepthIn.rows / targetSize; 516 | } 517 | } 518 | 519 | bool mono; 520 | if(imageRgb.channels() == 3) // BGR 521 | { 522 | mono = false; 523 | } 524 | else if(imageRgb.channels() == 1) // Mono 525 | { 526 | mono = true; 527 | } 528 | else 529 | { 530 | return cloud; 531 | } 532 | 533 | //cloud.header = cameraInfo.header; 534 | cloud->height = imageDepth.rows/decimation; 535 | cloud->width = imageDepth.cols/decimation; 536 | cloud->is_dense = false; 537 | cloud->resize(cloud->height * cloud->width); 538 | if(validIndices) 539 | { 540 | validIndices->resize(cloud->size()); 541 | } 542 | 543 | float rgbToDepthFactorX = float(imageRgb.cols) / float(imageDepth.cols); 544 | float rgbToDepthFactorY = float(imageRgb.rows) / float(imageDepth.rows); 545 | float depthFx = model.fx() / rgbToDepthFactorX; 546 | float depthFy = model.fy() / rgbToDepthFactorY; 547 | float depthCx = model.cx() / rgbToDepthFactorX; 548 | float depthCy = model.cy() / rgbToDepthFactorY; 549 | 550 | UDEBUG("rgb=%dx%d depth=%dx%d fx=%f fy=%f cx=%f cy=%f (depth factors=%f %f) decimation=%d", 551 | imageRgb.cols, imageRgb.rows, 552 | imageDepth.cols, imageDepth.rows, 553 | model.fx(), model.fy(), model.cx(), model.cy(), 554 | rgbToDepthFactorX, 555 | rgbToDepthFactorY, 556 | decimation); 557 | 558 | int oi = 0; 559 | for(int h = 0; h < imageDepth.rows && h/decimation < (int)cloud->height; h+=decimation) 560 | { 561 | for(int w = 0; w < imageDepth.cols && w/decimation < (int)cloud->width; w+=decimation) 562 | { 563 | pcl::PointXYZRGB & pt = cloud->at((h/decimation)*cloud->width + (w/decimation)); 564 | 565 | int x = int(w*rgbToDepthFactorX); 566 | int y = int(h*rgbToDepthFactorY); 567 | UASSERT(x >=0 && x=0 && y(y,x); 571 | pt.b = bgr[0]; 572 | pt.g = bgr[1]; 573 | pt.r = bgr[2]; 574 | /* START MODIFCIATION */ 575 | /* Remove black points, it's a fair assumption that they will not appear */ 576 | if(bgr[0] == 0 && bgr[1] == 0 && bgr[2] == 0) 577 | { 578 | continue; 579 | } 580 | /* END MODIFCIATION */ 581 | } 582 | else 583 | { 584 | unsigned char v = imageRgb.at(y,x); 585 | pt.b = v; 586 | pt.g = v; 587 | pt.r = v; 588 | } 589 | 590 | pcl::PointXYZ ptXYZ = projectDepthTo3D(imageDepth, w, h, depthCx, depthCy, depthFx, depthFy, false); 591 | if (pcl::isFinite(ptXYZ) && ptXYZ.z >= minDepth && (maxDepth <= 0.0f || ptXYZ.z <= maxDepth)) 592 | { 593 | pt.x = ptXYZ.x; 594 | pt.y = ptXYZ.y; 595 | pt.z = ptXYZ.z; 596 | if (validIndices) 597 | { 598 | validIndices->at(oi) = (h / decimation)*cloud->width + (w / decimation); 599 | } 600 | ++oi; 601 | } 602 | else 603 | { 604 | pt.x = pt.y = pt.z = std::numeric_limits::quiet_NaN(); 605 | } 606 | } 607 | } 608 | if(validIndices) 609 | { 610 | validIndices->resize(oi); 611 | } 612 | if(oi == 0) 613 | { 614 | UWARN("Cloud with only NaN values created!"); 615 | } 616 | UDEBUG(""); 617 | return cloud; 618 | } 619 | 620 | pcl::PointCloud::Ptr cloudFromDisparity( 621 | const cv::Mat & imageDisparity, 622 | const StereoCameraModel & model, 623 | int decimation, 624 | float maxDepth, 625 | float minDepth, 626 | std::vector * validIndices) 627 | { 628 | UASSERT(imageDisparity.type() == CV_32FC1 || imageDisparity.type()==CV_16SC1); 629 | UASSERT(decimation >= 1); 630 | 631 | if(imageDisparity.rows % decimation != 0 || imageDisparity.cols % decimation != 0) 632 | { 633 | int oldDecimation = decimation; 634 | while(decimation >= 1) 635 | { 636 | if(imageDisparity.rows % decimation == 0 && imageDisparity.cols % decimation == 0) 637 | { 638 | break; 639 | } 640 | --decimation; 641 | } 642 | 643 | if(imageDisparity.rows % oldDecimation != 0 || imageDisparity.cols % oldDecimation != 0) 644 | { 645 | UWARN("Decimation (%d) is not valid for current image size (depth=%dx%d). Highest compatible decimation used=%d.", oldDecimation, imageDisparity.cols, imageDisparity.rows, decimation); 646 | } 647 | } 648 | 649 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud); 650 | 651 | //cloud.header = cameraInfo.header; 652 | cloud->height = imageDisparity.rows/decimation; 653 | cloud->width = imageDisparity.cols/decimation; 654 | cloud->is_dense = false; 655 | cloud->resize(cloud->height * cloud->width); 656 | if(validIndices) 657 | { 658 | validIndices->resize(cloud->size()); 659 | } 660 | 661 | int oi = 0; 662 | if(imageDisparity.type()==CV_16SC1) 663 | { 664 | for(int h = 0; h < imageDisparity.rows && h/decimation < (int)cloud->height; h+=decimation) 665 | { 666 | for(int w = 0; w < imageDisparity.cols && w/decimation < (int)cloud->width; w+=decimation) 667 | { 668 | float disp = float(imageDisparity.at(h,w))/16.0f; 669 | cv::Point3f pt = projectDisparityTo3D(cv::Point2f(w, h), disp, model); 670 | if(pt.z >= minDepth && (maxDepth <= 0.0f || pt.z <= maxDepth)) 671 | { 672 | cloud->at((h/decimation)*cloud->width + (w/decimation)) = pcl::PointXYZ(pt.x, pt.y, pt.z); 673 | if(validIndices) 674 | { 675 | validIndices->at(oi++) = (h/decimation)*cloud->width + (w/decimation); 676 | } 677 | } 678 | else 679 | { 680 | cloud->at((h/decimation)*cloud->width + (w/decimation)) = pcl::PointXYZ( 681 | std::numeric_limits::quiet_NaN(), 682 | std::numeric_limits::quiet_NaN(), 683 | std::numeric_limits::quiet_NaN()); 684 | } 685 | } 686 | } 687 | } 688 | else 689 | { 690 | for(int h = 0; h < imageDisparity.rows && h/decimation < (int)cloud->height; h+=decimation) 691 | { 692 | for(int w = 0; w < imageDisparity.cols && w/decimation < (int)cloud->width; w+=decimation) 693 | { 694 | float disp = imageDisparity.at(h,w); 695 | cv::Point3f pt = projectDisparityTo3D(cv::Point2f(w, h), disp, model); 696 | if(pt.z > minDepth && (maxDepth <= 0.0f || pt.z <= maxDepth)) 697 | { 698 | cloud->at((h/decimation)*cloud->width + (w/decimation)) = pcl::PointXYZ(pt.x, pt.y, pt.z); 699 | if(validIndices) 700 | { 701 | validIndices->at(oi++) = (h/decimation)*cloud->width + (w/decimation); 702 | } 703 | } 704 | else 705 | { 706 | cloud->at((h/decimation)*cloud->width + (w/decimation)) = pcl::PointXYZ( 707 | std::numeric_limits::quiet_NaN(), 708 | std::numeric_limits::quiet_NaN(), 709 | std::numeric_limits::quiet_NaN()); 710 | } 711 | } 712 | } 713 | } 714 | if(validIndices) 715 | { 716 | validIndices->resize(oi); 717 | } 718 | return cloud; 719 | } 720 | 721 | pcl::PointCloud::Ptr cloudFromDisparityRGB( 722 | const cv::Mat & imageRgb, 723 | const cv::Mat & imageDisparity, 724 | const StereoCameraModel & model, 725 | int decimation, 726 | float maxDepth, 727 | float minDepth, 728 | std::vector * validIndices) 729 | { 730 | UASSERT(!imageRgb.empty() && !imageDisparity.empty()); 731 | UASSERT(imageRgb.rows == imageDisparity.rows && 732 | imageRgb.cols == imageDisparity.cols && 733 | (imageDisparity.type() == CV_32FC1 || imageDisparity.type()==CV_16SC1)); 734 | UASSERT(imageRgb.channels() == 3 || imageRgb.channels() == 1); 735 | UASSERT(decimation >= 1); 736 | 737 | if(imageDisparity.rows % decimation != 0 || imageDisparity.cols % decimation != 0) 738 | { 739 | int oldDecimation = decimation; 740 | while(decimation >= 1) 741 | { 742 | if(imageDisparity.rows % decimation == 0 && imageDisparity.cols % decimation == 0) 743 | { 744 | break; 745 | } 746 | --decimation; 747 | } 748 | 749 | if(imageDisparity.rows % oldDecimation != 0 || imageDisparity.cols % oldDecimation != 0) 750 | { 751 | UWARN("Decimation (%d) is not valid for current image size (depth=%dx%d). Highest compatible decimation used=%d.", oldDecimation, imageDisparity.cols, imageDisparity.rows, decimation); 752 | } 753 | } 754 | 755 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud); 756 | 757 | bool mono; 758 | if(imageRgb.channels() == 3) // BGR 759 | { 760 | mono = false; 761 | } 762 | else // Mono 763 | { 764 | mono = true; 765 | } 766 | 767 | //cloud.header = cameraInfo.header; 768 | cloud->height = imageRgb.rows/decimation; 769 | cloud->width = imageRgb.cols/decimation; 770 | cloud->is_dense = false; 771 | cloud->resize(cloud->height * cloud->width); 772 | if(validIndices) 773 | { 774 | validIndices->resize(cloud->size()); 775 | } 776 | 777 | int oi=0; 778 | for(int h = 0; h < imageRgb.rows && h/decimation < (int)cloud->height; h+=decimation) 779 | { 780 | for(int w = 0; w < imageRgb.cols && w/decimation < (int)cloud->width; w+=decimation) 781 | { 782 | pcl::PointXYZRGB & pt = cloud->at((h/decimation)*cloud->width + (w/decimation)); 783 | if(!mono) 784 | { 785 | pt.b = imageRgb.at(h,w)[0]; 786 | pt.g = imageRgb.at(h,w)[1]; 787 | pt.r = imageRgb.at(h,w)[2]; 788 | } 789 | else 790 | { 791 | unsigned char v = imageRgb.at(h,w); 792 | pt.b = v; 793 | pt.g = v; 794 | pt.r = v; 795 | } 796 | 797 | float disp = imageDisparity.type()==CV_16SC1?float(imageDisparity.at(h,w))/16.0f:imageDisparity.at(h,w); 798 | cv::Point3f ptXYZ = projectDisparityTo3D(cv::Point2f(w, h), disp, model); 799 | if(util3d::isFinite(ptXYZ) && ptXYZ.z >= minDepth && (maxDepth<=0.0f || ptXYZ.z <= maxDepth)) 800 | { 801 | pt.x = ptXYZ.x; 802 | pt.y = ptXYZ.y; 803 | pt.z = ptXYZ.z; 804 | if(validIndices) 805 | { 806 | validIndices->at(oi++) = (h/decimation)*cloud->width + (w/decimation); 807 | } 808 | } 809 | else 810 | { 811 | pt.x = pt.y = pt.z = std::numeric_limits::quiet_NaN(); 812 | } 813 | } 814 | } 815 | if(validIndices) 816 | { 817 | validIndices->resize(oi); 818 | } 819 | return cloud; 820 | } 821 | 822 | pcl::PointCloud::Ptr cloudFromStereoImages( 823 | const cv::Mat & imageLeft, 824 | const cv::Mat & imageRight, 825 | const StereoCameraModel & model, 826 | int decimation, 827 | float maxDepth, 828 | float minDepth, 829 | std::vector * validIndices, 830 | const ParametersMap & parameters) 831 | { 832 | UASSERT(!imageLeft.empty() && !imageRight.empty()); 833 | UASSERT(imageRight.type() == CV_8UC1); 834 | UASSERT(imageLeft.channels() == 3 || imageLeft.channels() == 1); 835 | UASSERT(imageLeft.rows == imageRight.rows && 836 | imageLeft.cols == imageRight.cols); 837 | UASSERT(decimation >= 1.0f); 838 | 839 | cv::Mat leftColor = imageLeft; 840 | cv::Mat rightMono = imageRight; 841 | 842 | cv::Mat leftMono; 843 | if(leftColor.channels() == 3) 844 | { 845 | cv::cvtColor(leftColor, leftMono, CV_BGR2GRAY); 846 | } 847 | else 848 | { 849 | leftMono = leftColor; 850 | } 851 | 852 | return cloudFromDisparityRGB( 853 | leftColor, 854 | util2d::disparityFromStereoImages(leftMono, rightMono, parameters), 855 | model, 856 | decimation, 857 | maxDepth, 858 | minDepth, 859 | validIndices); 860 | } 861 | 862 | pcl::PointCloud::Ptr RTABMAP_EXP cloudFromSensorData( 863 | const SensorData & sensorData, 864 | int decimation, 865 | float maxDepth, 866 | float minDepth, 867 | std::vector * validIndices, 868 | const ParametersMap & stereoParameters, 869 | const std::vector & roiRatios) 870 | { 871 | if(decimation == 0) 872 | { 873 | decimation = 1; 874 | } 875 | 876 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud); 877 | 878 | if(!sensorData.depthRaw().empty() && sensorData.cameraModels().size()) 879 | { 880 | //depth 881 | UASSERT(int((sensorData.depthRaw().cols/sensorData.cameraModels().size())*sensorData.cameraModels().size()) == sensorData.depthRaw().cols); 882 | int subImageWidth = sensorData.depthRaw().cols/sensorData.cameraModels().size(); 883 | for(unsigned int i=0; i 0.0f || 891 | roiRatios[1] > 0.0f || 892 | roiRatios[2] > 0.0f || 893 | roiRatios[3] > 0.0f)) 894 | { 895 | cv::Rect roiDepth = util2d::computeRoi(depth, roiRatios); 896 | cv::Rect roiRgb; 897 | if(model.imageWidth() && model.imageHeight()) 898 | { 899 | roiRgb = util2d::computeRoi(model.imageSize(), roiRatios); 900 | } 901 | if( roiDepth.width%decimation==0 && 902 | roiDepth.height%decimation==0 && 903 | (roiRgb.width != 0 || 904 | (roiRgb.width%decimation==0 && 905 | roiRgb.height%decimation==0))) 906 | { 907 | depth = cv::Mat(depth, roiDepth); 908 | if(model.imageWidth() != 0 && model.imageHeight() != 0) 909 | { 910 | model = model.roi(util2d::computeRoi(model.imageSize(), roiRatios)); 911 | } 912 | else 913 | { 914 | model = model.roi(roiDepth); 915 | } 916 | } 917 | else 918 | { 919 | UERROR("Cannot apply ROI ratios [%f,%f,%f,%f] because resulting " 920 | "dimension (depth=%dx%d rgb=%dx%d) cannot be divided exactly " 921 | "by decimation parameter (%d). Ignoring ROI ratios...", 922 | roiRatios[0], 923 | roiRatios[1], 924 | roiRatios[2], 925 | roiRatios[3], 926 | roiDepth.width, 927 | roiDepth.height, 928 | roiRgb.width, 929 | roiRgb.height, 930 | decimation); 931 | } 932 | } 933 | 934 | pcl::PointCloud::Ptr tmp = util3d::cloudFromDepth( 935 | depth, 936 | model, 937 | decimation, 938 | maxDepth, 939 | minDepth, 940 | sensorData.cameraModels().size()==1?validIndices:0); 941 | 942 | if(tmp->size()) 943 | { 944 | if(!model.localTransform().isNull() && !model.localTransform().isIdentity()) 945 | { 946 | tmp = util3d::transformPointCloud(tmp, model.localTransform()); 947 | } 948 | 949 | if(sensorData.cameraModels().size() > 1) 950 | { 951 | tmp = util3d::removeNaNFromPointCloud(tmp); 952 | *cloud += *tmp; 953 | } 954 | else 955 | { 956 | cloud = tmp; 957 | } 958 | } 959 | } 960 | else 961 | { 962 | UERROR("Camera model %d is invalid", i); 963 | } 964 | } 965 | 966 | if(cloud->is_dense && validIndices) 967 | { 968 | //generate indices for all points (they are all valid) 969 | validIndices->resize(cloud->size()); 970 | for(unsigned int i=0; isize(); ++i) 971 | { 972 | validIndices->at(i) = i; 973 | } 974 | } 975 | } 976 | else if(!sensorData.imageRaw().empty() && !sensorData.rightRaw().empty() && sensorData.stereoCameraModel().isValidForProjection()) 977 | { 978 | //stereo 979 | UASSERT(sensorData.rightRaw().type() == CV_8UC1); 980 | 981 | cv::Mat leftMono; 982 | if(sensorData.imageRaw().channels() == 3) 983 | { 984 | cv::cvtColor(sensorData.imageRaw(), leftMono, CV_BGR2GRAY); 985 | } 986 | else 987 | { 988 | leftMono = sensorData.imageRaw(); 989 | } 990 | 991 | cv::Mat right(sensorData.rightRaw()); 992 | StereoCameraModel model = sensorData.stereoCameraModel(); 993 | if( roiRatios.size() == 4 && 994 | ((roiRatios[0] > 0.0f && roiRatios[0] <= 1.0f) || 995 | (roiRatios[1] > 0.0f && roiRatios[1] <= 1.0f) || 996 | (roiRatios[2] > 0.0f && roiRatios[2] <= 1.0f) || 997 | (roiRatios[3] > 0.0f && roiRatios[3] <= 1.0f))) 998 | { 999 | cv::Rect roi = util2d::computeRoi(leftMono, roiRatios); 1000 | if( roi.width%decimation==0 && 1001 | roi.height%decimation==0) 1002 | { 1003 | leftMono = cv::Mat(leftMono, roi); 1004 | right = cv::Mat(right, roi); 1005 | model.roi(roi); 1006 | } 1007 | else 1008 | { 1009 | UERROR("Cannot apply ROI ratios [%f,%f,%f,%f] because resulting " 1010 | "dimension (left=%dx%d) cannot be divided exactly " 1011 | "by decimation parameter (%d). Ignoring ROI ratios...", 1012 | roiRatios[0], 1013 | roiRatios[1], 1014 | roiRatios[2], 1015 | roiRatios[3], 1016 | roi.width, 1017 | roi.height, 1018 | decimation); 1019 | } 1020 | } 1021 | 1022 | cloud = cloudFromDisparity( 1023 | util2d::disparityFromStereoImages(leftMono, right, stereoParameters), 1024 | model, 1025 | decimation, 1026 | maxDepth, 1027 | minDepth, 1028 | validIndices); 1029 | 1030 | if(cloud->size()) 1031 | { 1032 | if(cloud->size() && !sensorData.stereoCameraModel().left().localTransform().isNull() && !sensorData.stereoCameraModel().left().localTransform().isIdentity()) 1033 | { 1034 | cloud = util3d::transformPointCloud(cloud, sensorData.stereoCameraModel().left().localTransform()); 1035 | } 1036 | } 1037 | } 1038 | return cloud; 1039 | } 1040 | 1041 | pcl::PointCloud::Ptr RTABMAP_EXP cloudRGBFromSensorData( 1042 | const SensorData & sensorData, 1043 | int decimation, 1044 | float maxDepth, 1045 | float minDepth, 1046 | std::vector * validIndices, 1047 | const ParametersMap & stereoParameters, 1048 | const std::vector & roiRatios) 1049 | { 1050 | if(decimation == 0) 1051 | { 1052 | decimation = 1; 1053 | } 1054 | 1055 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud); 1056 | 1057 | if(!sensorData.imageRaw().empty() && !sensorData.depthRaw().empty() && sensorData.cameraModels().size()) 1058 | { 1059 | //depth 1060 | UDEBUG(""); 1061 | UASSERT(int((sensorData.imageRaw().cols/sensorData.cameraModels().size())*sensorData.cameraModels().size()) == sensorData.imageRaw().cols); 1062 | UASSERT(int((sensorData.depthRaw().cols/sensorData.cameraModels().size())*sensorData.cameraModels().size()) == sensorData.depthRaw().cols); 1063 | UASSERT_MSG(sensorData.imageRaw().cols % sensorData.depthRaw().cols == 0, uFormat("rgb=%d depth=%d", sensorData.imageRaw().cols, sensorData.depthRaw().cols).c_str()); 1064 | UASSERT_MSG(sensorData.imageRaw().rows % sensorData.depthRaw().rows == 0, uFormat("rgb=%d depth=%d", sensorData.imageRaw().rows, sensorData.depthRaw().rows).c_str()); 1065 | int subRGBWidth = sensorData.imageRaw().cols/sensorData.cameraModels().size(); 1066 | int subDepthWidth = sensorData.depthRaw().cols/sensorData.cameraModels().size(); 1067 | 1068 | for(unsigned int i=0; i 0.0f && roiRatios[0] <= 1.0f) || 1077 | (roiRatios[1] > 0.0f && roiRatios[1] <= 1.0f) || 1078 | (roiRatios[2] > 0.0f && roiRatios[2] <= 1.0f) || 1079 | (roiRatios[3] > 0.0f && roiRatios[3] <= 1.0f))) 1080 | { 1081 | cv::Rect roiDepth = util2d::computeRoi(depth, roiRatios); 1082 | cv::Rect roiRgb = util2d::computeRoi(rgb, roiRatios); 1083 | if( roiDepth.width%decimation==0 && 1084 | roiDepth.height%decimation==0 && 1085 | roiRgb.width%decimation==0 && 1086 | roiRgb.height%decimation==0) 1087 | { 1088 | depth = cv::Mat(depth, roiDepth); 1089 | rgb = cv::Mat(rgb, roiRgb); 1090 | model = model.roi(roiRgb); 1091 | } 1092 | else 1093 | { 1094 | UERROR("Cannot apply ROI ratios [%f,%f,%f,%f] because resulting " 1095 | "dimension (depth=%dx%d rgb=%dx%d) cannot be divided exactly " 1096 | "by decimation parameter (%d). Ignoring ROI ratios...", 1097 | roiRatios[0], 1098 | roiRatios[1], 1099 | roiRatios[2], 1100 | roiRatios[3], 1101 | roiDepth.width, 1102 | roiDepth.height, 1103 | roiRgb.width, 1104 | roiRgb.height, 1105 | decimation); 1106 | } 1107 | } 1108 | 1109 | pcl::PointCloud::Ptr tmp = util3d::cloudFromDepthRGB( 1110 | rgb, 1111 | depth, 1112 | model, 1113 | decimation, 1114 | maxDepth, 1115 | minDepth, 1116 | sensorData.cameraModels().size() == 1?validIndices:0); 1117 | 1118 | if(tmp->size()) 1119 | { 1120 | if(!model.localTransform().isNull() && !model.localTransform().isIdentity()) 1121 | { 1122 | tmp = util3d::transformPointCloud(tmp, model.localTransform()); 1123 | } 1124 | 1125 | if(sensorData.cameraModels().size() > 1) 1126 | { 1127 | tmp = util3d::removeNaNFromPointCloud(tmp); 1128 | *cloud += *tmp; 1129 | } 1130 | else 1131 | { 1132 | cloud = tmp; 1133 | } 1134 | } 1135 | } 1136 | else 1137 | { 1138 | UERROR("Camera model %d is invalid", i); 1139 | } 1140 | } 1141 | 1142 | if(cloud->is_dense && validIndices) 1143 | { 1144 | //generate indices for all points (they are all valid) 1145 | validIndices->resize(cloud->size()); 1146 | for(unsigned int i=0; isize(); ++i) 1147 | { 1148 | validIndices->at(i) = i; 1149 | } 1150 | } 1151 | } 1152 | else if(!sensorData.imageRaw().empty() && !sensorData.rightRaw().empty() && sensorData.stereoCameraModel().isValidForProjection()) 1153 | { 1154 | //stereo 1155 | UDEBUG(""); 1156 | 1157 | cv::Mat left(sensorData.imageRaw()); 1158 | cv::Mat right(sensorData.rightRaw()); 1159 | StereoCameraModel model = sensorData.stereoCameraModel(); 1160 | if( roiRatios.size() == 4 && 1161 | ((roiRatios[0] > 0.0f && roiRatios[0] <= 1.0f) || 1162 | (roiRatios[1] > 0.0f && roiRatios[1] <= 1.0f) || 1163 | (roiRatios[2] > 0.0f && roiRatios[2] <= 1.0f) || 1164 | (roiRatios[3] > 0.0f && roiRatios[3] <= 1.0f))) 1165 | { 1166 | cv::Rect roi = util2d::computeRoi(left, roiRatios); 1167 | if( roi.width%decimation==0 && 1168 | roi.height%decimation==0) 1169 | { 1170 | left = cv::Mat(left, roi); 1171 | right = cv::Mat(right, roi); 1172 | model.roi(roi); 1173 | } 1174 | else 1175 | { 1176 | UERROR("Cannot apply ROI ratios [%f,%f,%f,%f] because resulting " 1177 | "dimension (left=%dx%d) cannot be divided exactly " 1178 | "by decimation parameter (%d). Ignoring ROI ratios...", 1179 | roiRatios[0], 1180 | roiRatios[1], 1181 | roiRatios[2], 1182 | roiRatios[3], 1183 | roi.width, 1184 | roi.height, 1185 | decimation); 1186 | } 1187 | } 1188 | 1189 | cloud = cloudFromStereoImages( 1190 | left, 1191 | right, 1192 | model, 1193 | decimation, 1194 | maxDepth, 1195 | minDepth, 1196 | validIndices, 1197 | stereoParameters); 1198 | 1199 | if(cloud->size() && !sensorData.stereoCameraModel().left().localTransform().isNull() && !sensorData.stereoCameraModel().left().localTransform().isIdentity()) 1200 | { 1201 | cloud = util3d::transformPointCloud(cloud, sensorData.stereoCameraModel().left().localTransform()); 1202 | } 1203 | } 1204 | return cloud; 1205 | } 1206 | 1207 | pcl::PointCloud laserScanFromDepthImage( 1208 | const cv::Mat & depthImage, 1209 | float fx, 1210 | float fy, 1211 | float cx, 1212 | float cy, 1213 | float maxDepth, 1214 | float minDepth, 1215 | const Transform & localTransform) 1216 | { 1217 | UASSERT(depthImage.type() == CV_16UC1 || depthImage.type() == CV_32FC1); 1218 | UASSERT(!localTransform.isNull()); 1219 | 1220 | pcl::PointCloud scan; 1221 | int middle = depthImage.rows/2; 1222 | if(middle) 1223 | { 1224 | scan.resize(depthImage.cols); 1225 | int oi = 0; 1226 | for(int i=depthImage.cols-1; i>=0; --i) 1227 | { 1228 | pcl::PointXYZ pt = util3d::projectDepthTo3D(depthImage, i, middle, cx, cy, fx, fy, false); 1229 | if(pcl::isFinite(pt) && pt.z >= minDepth && (maxDepth == 0 || pt.z < maxDepth)) 1230 | { 1231 | if(!localTransform.isIdentity()) 1232 | { 1233 | pt = util3d::transformPoint(pt, localTransform); 1234 | } 1235 | scan[oi++] = pt; 1236 | } 1237 | } 1238 | scan.resize(oi); 1239 | } 1240 | return scan; 1241 | } 1242 | 1243 | pcl::PointCloud laserScanFromDepthImages( 1244 | const cv::Mat & depthImages, 1245 | const std::vector & cameraModels, 1246 | float maxDepth, 1247 | float minDepth) 1248 | { 1249 | pcl::PointCloud scan; 1250 | UASSERT(int((depthImages.cols/cameraModels.size())*cameraModels.size()) == depthImages.cols); 1251 | int subImageWidth = depthImages.cols/cameraModels.size(); 1252 | for(int i=(int)cameraModels.size()-1; i>=0; --i) 1253 | { 1254 | if(cameraModels[i].isValidForProjection()) 1255 | { 1256 | cv::Mat depth = cv::Mat(depthImages, cv::Rect(subImageWidth*i, 0, subImageWidth, depthImages.rows)); 1257 | 1258 | scan += laserScanFromDepthImage( 1259 | depth, 1260 | cameraModels[i].fx(), 1261 | cameraModels[i].fy(), 1262 | cameraModels[i].cx(), 1263 | cameraModels[i].cy(), 1264 | maxDepth, 1265 | minDepth, 1266 | cameraModels[i].localTransform()); 1267 | } 1268 | else 1269 | { 1270 | UERROR("Camera model %d is invalid", i); 1271 | } 1272 | } 1273 | return scan; 1274 | } 1275 | 1276 | LaserScan laserScanFromPointCloud(const pcl::PCLPointCloud2 & cloud) 1277 | { 1278 | if(cloud.data.empty()) 1279 | { 1280 | return LaserScan(); 1281 | } 1282 | //determine the output type 1283 | int fieldStates[8] = {0}; // x,y,z,normal_x,normal_y,normal_z,rgb,intensity 1284 | pcl::uint32_t fieldOffsets[8] = {0}; 1285 | for(unsigned int i=0; i(0, oi); 1396 | 1397 | bool valid = true; 1398 | if(laserScan.channels() == 2) 1399 | { 1400 | ptr[0] = *(float*)(msg_data + fieldOffsets[0]); 1401 | ptr[1] = *(float*)(msg_data + fieldOffsets[1]); 1402 | valid = uIsFinite(ptr[0]) && uIsFinite(ptr[1]); 1403 | } 1404 | else if(laserScan.channels() == 3) 1405 | { 1406 | ptr[0] = *(float*)(msg_data + fieldOffsets[0]); 1407 | ptr[1] = *(float*)(msg_data + fieldOffsets[1]); 1408 | if(format == LaserScan::kXYI) 1409 | { 1410 | ptr[2] = *(float*)(msg_data + fieldOffsets[7]); 1411 | } 1412 | else // XYZ 1413 | { 1414 | ptr[2] = *(float*)(msg_data + fieldOffsets[2]); 1415 | valid = uIsFinite(ptr[0]) && uIsFinite(ptr[1]) && uIsFinite(ptr[2]); 1416 | } 1417 | } 1418 | else if(laserScan.channels() == 4) 1419 | { 1420 | ptr[0] = *(float*)(msg_data + fieldOffsets[0]); 1421 | ptr[1] = *(float*)(msg_data + fieldOffsets[1]); 1422 | ptr[2] = *(float*)(msg_data + fieldOffsets[2]); 1423 | if(format == LaserScan::kXYZI) 1424 | { 1425 | ptr[3] = *(float*)(msg_data + fieldOffsets[7]); 1426 | } 1427 | else // XYZRGB 1428 | { 1429 | pcl::uint8_t b=*(msg_data + fieldOffsets[6]); 1430 | pcl::uint8_t g=*(msg_data + fieldOffsets[6]+1); 1431 | pcl::uint8_t r=*(msg_data + fieldOffsets[6]+2); 1432 | int * ptrInt = (int*)ptr; 1433 | ptrInt[3] = int(b) | (int(g) << 8) | (int(r) << 16); 1434 | } 1435 | valid = uIsFinite(ptr[0]) && uIsFinite(ptr[1]) && uIsFinite(ptr[2]); 1436 | } 1437 | else if(laserScan.channels() == 5) 1438 | { 1439 | ptr[0] = *(float*)(msg_data + fieldOffsets[0]); 1440 | ptr[1] = *(float*)(msg_data + fieldOffsets[1]); 1441 | ptr[2] = *(float*)(msg_data + fieldOffsets[3]); 1442 | ptr[3] = *(float*)(msg_data + fieldOffsets[4]); 1443 | ptr[4] = *(float*)(msg_data + fieldOffsets[5]); 1444 | valid = uIsFinite(ptr[0]) && uIsFinite(ptr[1]) && uIsFinite(ptr[2]) && uIsFinite(ptr[3]) && uIsFinite(ptr[4]); 1445 | } 1446 | else if(laserScan.channels() == 6) 1447 | { 1448 | ptr[0] = *(float*)(msg_data + fieldOffsets[0]); 1449 | ptr[1] = *(float*)(msg_data + fieldOffsets[1]); 1450 | if(format == LaserScan::kXYINormal) 1451 | { 1452 | ptr[2] = *(float*)(msg_data + fieldOffsets[7]); 1453 | } 1454 | else // XYZNormal 1455 | { 1456 | ptr[2] = *(float*)(msg_data + fieldOffsets[2]); 1457 | } 1458 | ptr[3] = *(float*)(msg_data + fieldOffsets[3]); 1459 | ptr[4] = *(float*)(msg_data + fieldOffsets[4]); 1460 | ptr[5] = *(float*)(msg_data + fieldOffsets[5]); 1461 | valid = uIsFinite(ptr[0]) && uIsFinite(ptr[1]) && uIsFinite(ptr[2]) && uIsFinite(ptr[3]) && uIsFinite(ptr[4]) && uIsFinite(ptr[5]); 1462 | } 1463 | else if(laserScan.channels() == 7) 1464 | { 1465 | ptr[0] = *(float*)(msg_data + fieldOffsets[0]); 1466 | ptr[1] = *(float*)(msg_data + fieldOffsets[1]); 1467 | ptr[2] = *(float*)(msg_data + fieldOffsets[2]); 1468 | if(format == LaserScan::kXYZINormal) 1469 | { 1470 | ptr[3] = *(float*)(msg_data + fieldOffsets[7]); 1471 | } 1472 | else // XYZRGBNormal 1473 | { 1474 | pcl::uint8_t b=*(msg_data + fieldOffsets[6]); 1475 | pcl::uint8_t g=*(msg_data + fieldOffsets[6]+1); 1476 | pcl::uint8_t r=*(msg_data + fieldOffsets[6]+2); 1477 | int * ptrInt = (int*)ptr; 1478 | ptrInt[3] = int(b) | (int(g) << 8) | (int(r) << 16); 1479 | } 1480 | ptr[4] = *(float*)(msg_data + fieldOffsets[3]); 1481 | ptr[5] = *(float*)(msg_data + fieldOffsets[4]); 1482 | ptr[6] = *(float*)(msg_data + fieldOffsets[5]); 1483 | valid = uIsFinite(ptr[0]) && uIsFinite(ptr[1]) && uIsFinite(ptr[2]) && uIsFinite(ptr[4]) && uIsFinite(ptr[5]) && uIsFinite(ptr[6]); 1484 | } 1485 | else 1486 | { 1487 | UFATAL("Cannot handle as many channels (%d)!", laserScan.channels()); 1488 | } 1489 | 1490 | if(valid) 1491 | { 1492 | ++oi; 1493 | } 1494 | } 1495 | } 1496 | if(oi == 0) 1497 | { 1498 | return LaserScan(); 1499 | } 1500 | return LaserScan(laserScan(cv::Range::all(), cv::Range(0,oi)), 0, 0, format); 1501 | } 1502 | 1503 | cv::Mat laserScanFromPointCloud(const pcl::PointCloud & cloud, const Transform & transform) 1504 | { 1505 | return laserScanFromPointCloud(cloud, pcl::IndicesPtr(), transform); 1506 | } 1507 | cv::Mat laserScanFromPointCloud(const pcl::PointCloud & cloud, const pcl::IndicesPtr & indices, const Transform & transform) 1508 | { 1509 | cv::Mat laserScan; 1510 | bool nullTransform = transform.isNull() || transform.isIdentity(); 1511 | Eigen::Affine3f transform3f = transform.toEigen3f(); 1512 | int oi = 0; 1513 | if(indices.get()) 1514 | { 1515 | laserScan = cv::Mat(1, (int)indices->size(), CV_32FC3); 1516 | for(unsigned int i=0; isize(); ++i) 1517 | { 1518 | int index = indices->at(i); 1519 | if(pcl::isFinite(cloud.at(index))) 1520 | { 1521 | float * ptr = laserScan.ptr(0, oi++); 1522 | if(!nullTransform) 1523 | { 1524 | pcl::PointXYZ pt = pcl::transformPoint(cloud.at(index), transform3f); 1525 | ptr[0] = pt.x; 1526 | ptr[1] = pt.y; 1527 | ptr[2] = pt.z; 1528 | } 1529 | else 1530 | { 1531 | ptr[0] = cloud.at(index).x; 1532 | ptr[1] = cloud.at(index).y; 1533 | ptr[2] = cloud.at(index).z; 1534 | } 1535 | } 1536 | } 1537 | } 1538 | else 1539 | { 1540 | laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC3); 1541 | for(unsigned int i=0; i(0, oi++); 1546 | if(!nullTransform) 1547 | { 1548 | pcl::PointXYZ pt = pcl::transformPoint(cloud.at(i), transform3f); 1549 | ptr[0] = pt.x; 1550 | ptr[1] = pt.y; 1551 | ptr[2] = pt.z; 1552 | } 1553 | else 1554 | { 1555 | ptr[0] = cloud.at(i).x; 1556 | ptr[1] = cloud.at(i).y; 1557 | ptr[2] = cloud.at(i).z; 1558 | } 1559 | } 1560 | } 1561 | } 1562 | if(oi == 0) 1563 | { 1564 | return cv::Mat(); 1565 | } 1566 | return laserScan(cv::Range::all(), cv::Range(0,oi)); 1567 | } 1568 | 1569 | cv::Mat laserScanFromPointCloud(const pcl::PointCloud & cloud, const Transform & transform) 1570 | { 1571 | return laserScanFromPointCloud(cloud, pcl::IndicesPtr(), transform); 1572 | } 1573 | cv::Mat laserScanFromPointCloud(const pcl::PointCloud & cloud, const pcl::IndicesPtr & indices, const Transform & transform) 1574 | { 1575 | cv::Mat laserScan; 1576 | bool nullTransform = transform.isNull() || transform.isIdentity(); 1577 | int oi=0; 1578 | if(indices.get()) 1579 | { 1580 | laserScan = cv::Mat(1, (int)indices->size(), CV_32FC(6)); 1581 | for(unsigned int i=0; isize(); ++i) 1582 | { 1583 | int index = indices->at(i); 1584 | if(pcl::isFinite(cloud.at(index)) && 1585 | uIsFinite(cloud.at(index).normal_x) && 1586 | uIsFinite(cloud.at(index).normal_y) && 1587 | uIsFinite(cloud.at(index).normal_z)) 1588 | { 1589 | float * ptr = laserScan.ptr(0, oi++); 1590 | if(!nullTransform) 1591 | { 1592 | pcl::PointNormal pt = util3d::transformPoint(cloud.at(index), transform); 1593 | ptr[0] = pt.x; 1594 | ptr[1] = pt.y; 1595 | ptr[2] = pt.z; 1596 | ptr[3] = pt.normal_x; 1597 | ptr[4] = pt.normal_y; 1598 | ptr[5] = pt.normal_z; 1599 | } 1600 | else 1601 | { 1602 | ptr[0] = cloud.at(index).x; 1603 | ptr[1] = cloud.at(index).y; 1604 | ptr[2] = cloud.at(index).z; 1605 | ptr[3] = cloud.at(index).normal_x; 1606 | ptr[4] = cloud.at(index).normal_y; 1607 | ptr[5] = cloud.at(index).normal_z; 1608 | } 1609 | } 1610 | } 1611 | } 1612 | else 1613 | { 1614 | laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC(6)); 1615 | for(unsigned int i=0; i(0, oi++); 1623 | if(!nullTransform) 1624 | { 1625 | pcl::PointNormal pt = util3d::transformPoint(cloud.at(i), transform); 1626 | ptr[0] = pt.x; 1627 | ptr[1] = pt.y; 1628 | ptr[2] = pt.z; 1629 | ptr[3] = pt.normal_x; 1630 | ptr[4] = pt.normal_y; 1631 | ptr[5] = pt.normal_z; 1632 | } 1633 | else 1634 | { 1635 | ptr[0] = cloud.at(i).x; 1636 | ptr[1] = cloud.at(i).y; 1637 | ptr[2] = cloud.at(i).z; 1638 | ptr[3] = cloud.at(i).normal_x; 1639 | ptr[4] = cloud.at(i).normal_y; 1640 | ptr[5] = cloud.at(i).normal_z; 1641 | } 1642 | } 1643 | } 1644 | } 1645 | if(oi == 0) 1646 | { 1647 | return cv::Mat(); 1648 | } 1649 | return laserScan(cv::Range::all(), cv::Range(0,oi)); 1650 | } 1651 | 1652 | cv::Mat laserScanFromPointCloud(const pcl::PointCloud & cloud, const pcl::PointCloud & normals, const Transform & transform) 1653 | { 1654 | UASSERT(cloud.size() == normals.size()); 1655 | cv::Mat laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC(6)); 1656 | bool nullTransform = transform.isNull() || transform.isIdentity(); 1657 | int oi =0; 1658 | for(unsigned int i=0; i(0, oi++); 1663 | if(!nullTransform) 1664 | { 1665 | pcl::PointNormal pt; 1666 | pt.x = cloud.at(i).x; 1667 | pt.y = cloud.at(i).y; 1668 | pt.z = cloud.at(i).z; 1669 | pt.normal_x = normals.at(i).normal_x; 1670 | pt.normal_y = normals.at(i).normal_y; 1671 | pt.normal_z = normals.at(i).normal_z; 1672 | pt = util3d::transformPoint(pt, transform); 1673 | ptr[0] = pt.x; 1674 | ptr[1] = pt.y; 1675 | ptr[2] = pt.z; 1676 | ptr[3] = pt.normal_x; 1677 | ptr[4] = pt.normal_y; 1678 | ptr[5] = pt.normal_z; 1679 | } 1680 | else 1681 | { 1682 | ptr[0] = cloud.at(i).x; 1683 | ptr[1] = cloud.at(i).y; 1684 | ptr[2] = cloud.at(i).z; 1685 | ptr[3] = normals.at(i).normal_x; 1686 | ptr[4] = normals.at(i).normal_y; 1687 | ptr[5] = normals.at(i).normal_z; 1688 | } 1689 | } 1690 | } 1691 | if(oi == 0) 1692 | { 1693 | return cv::Mat(); 1694 | } 1695 | return laserScan(cv::Range::all(), cv::Range(0,oi)); 1696 | } 1697 | 1698 | cv::Mat laserScanFromPointCloud(const pcl::PointCloud & cloud, const Transform & transform) 1699 | { 1700 | return laserScanFromPointCloud(cloud, pcl::IndicesPtr(), transform); 1701 | } 1702 | 1703 | cv::Mat laserScanFromPointCloud(const pcl::PointCloud & cloud, const pcl::IndicesPtr & indices, const Transform & transform) 1704 | { 1705 | cv::Mat laserScan; 1706 | bool nullTransform = transform.isNull() || transform.isIdentity(); 1707 | Eigen::Affine3f transform3f = transform.toEigen3f(); 1708 | int oi=0; 1709 | if(indices.get()) 1710 | { 1711 | laserScan = cv::Mat(1, (int)indices->size(), CV_32FC(4)); 1712 | for(unsigned int i=0; isize(); ++i) 1713 | { 1714 | int index = indices->at(i); 1715 | if(pcl::isFinite(cloud.at(index))) 1716 | { 1717 | float * ptr = laserScan.ptr(0, oi++); 1718 | if(!nullTransform) 1719 | { 1720 | pcl::PointXYZRGB pt = pcl::transformPoint(cloud.at(index), transform3f); 1721 | ptr[0] = pt.x; 1722 | ptr[1] = pt.y; 1723 | ptr[2] = pt.z; 1724 | } 1725 | else 1726 | { 1727 | ptr[0] = cloud.at(index).x; 1728 | ptr[1] = cloud.at(index).y; 1729 | ptr[2] = cloud.at(index).z; 1730 | } 1731 | int * ptrInt = (int*)ptr; 1732 | ptrInt[3] = int(cloud.at(index).b) | (int(cloud.at(index).g) << 8) | (int(cloud.at(index).r) << 16); 1733 | } 1734 | } 1735 | } 1736 | else 1737 | { 1738 | laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC(4)); 1739 | for(unsigned int i=0; i(0, oi++); 1744 | if(!nullTransform) 1745 | { 1746 | pcl::PointXYZRGB pt = pcl::transformPoint(cloud.at(i), transform3f); 1747 | ptr[0] = pt.x; 1748 | ptr[1] = pt.y; 1749 | ptr[2] = pt.z; 1750 | } 1751 | else 1752 | { 1753 | ptr[0] = cloud.at(i).x; 1754 | ptr[1] = cloud.at(i).y; 1755 | ptr[2] = cloud.at(i).z; 1756 | } 1757 | int * ptrInt = (int*)ptr; 1758 | ptrInt[3] = int(cloud.at(i).b) | (int(cloud.at(i).g) << 8) | (int(cloud.at(i).r) << 16); 1759 | } 1760 | } 1761 | } 1762 | if(oi == 0) 1763 | { 1764 | return cv::Mat(); 1765 | } 1766 | return laserScan(cv::Range::all(), cv::Range(0,oi)); 1767 | } 1768 | 1769 | cv::Mat laserScanFromPointCloud(const pcl::PointCloud & cloud, const Transform & transform) 1770 | { 1771 | return laserScanFromPointCloud(cloud, pcl::IndicesPtr(), transform); 1772 | } 1773 | 1774 | cv::Mat laserScanFromPointCloud(const pcl::PointCloud & cloud, const pcl::IndicesPtr & indices, const Transform & transform) 1775 | { 1776 | cv::Mat laserScan; 1777 | bool nullTransform = transform.isNull() || transform.isIdentity(); 1778 | Eigen::Affine3f transform3f = transform.toEigen3f(); 1779 | int oi=0; 1780 | if(indices.get()) 1781 | { 1782 | laserScan = cv::Mat(1, (int)indices->size(), CV_32FC(4)); 1783 | for(unsigned int i=0; isize(); ++i) 1784 | { 1785 | int index = indices->at(i); 1786 | if(pcl::isFinite(cloud.at(index))) 1787 | { 1788 | float * ptr = laserScan.ptr(0, oi++); 1789 | if(!nullTransform) 1790 | { 1791 | pcl::PointXYZI pt = pcl::transformPoint(cloud.at(index), transform3f); 1792 | ptr[0] = pt.x; 1793 | ptr[1] = pt.y; 1794 | ptr[2] = pt.z; 1795 | } 1796 | else 1797 | { 1798 | ptr[0] = cloud.at(index).x; 1799 | ptr[1] = cloud.at(index).y; 1800 | ptr[2] = cloud.at(index).z; 1801 | } 1802 | ptr[3] = cloud.at(index).intensity; 1803 | } 1804 | } 1805 | } 1806 | else 1807 | { 1808 | laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC(4)); 1809 | for(unsigned int i=0; i(0, oi++); 1814 | if(!nullTransform) 1815 | { 1816 | pcl::PointXYZI pt = pcl::transformPoint(cloud.at(i), transform3f); 1817 | ptr[0] = pt.x; 1818 | ptr[1] = pt.y; 1819 | ptr[2] = pt.z; 1820 | } 1821 | else 1822 | { 1823 | ptr[0] = cloud.at(i).x; 1824 | ptr[1] = cloud.at(i).y; 1825 | ptr[2] = cloud.at(i).z; 1826 | } 1827 | ptr[3] = cloud.at(i).intensity; 1828 | } 1829 | } 1830 | } 1831 | if(oi == 0) 1832 | { 1833 | return cv::Mat(); 1834 | } 1835 | return laserScan(cv::Range::all(), cv::Range(0,oi)); 1836 | } 1837 | 1838 | cv::Mat laserScanFromPointCloud(const pcl::PointCloud & cloud, const pcl::PointCloud & normals, const Transform & transform) 1839 | { 1840 | UASSERT(cloud.size() == normals.size()); 1841 | cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(7)); 1842 | bool nullTransform = transform.isNull() || transform.isIdentity(); 1843 | int oi = 0; 1844 | for(unsigned int i=0; i(0, oi++); 1849 | if(!nullTransform) 1850 | { 1851 | pcl::PointXYZRGBNormal pt; 1852 | pt.x = cloud.at(i).x; 1853 | pt.y = cloud.at(i).y; 1854 | pt.z = cloud.at(i).z; 1855 | pt.normal_x = normals.at(i).normal_x; 1856 | pt.normal_y = normals.at(i).normal_y; 1857 | pt.normal_z = normals.at(i).normal_z; 1858 | pt = util3d::transformPoint(pt, transform); 1859 | ptr[0] = pt.x; 1860 | ptr[1] = pt.y; 1861 | ptr[2] = pt.z; 1862 | ptr[4] = pt.normal_x; 1863 | ptr[5] = pt.normal_y; 1864 | ptr[6] = pt.normal_z; 1865 | } 1866 | else 1867 | { 1868 | ptr[0] = cloud.at(i).x; 1869 | ptr[1] = cloud.at(i).y; 1870 | ptr[2] = cloud.at(i).z; 1871 | ptr[4] = normals.at(i).normal_x; 1872 | ptr[5] = normals.at(i).normal_y; 1873 | ptr[6] = normals.at(i).normal_z; 1874 | } 1875 | int * ptrInt = (int*)ptr; 1876 | ptrInt[3] = int(cloud.at(i).b) | (int(cloud.at(i).g) << 8) | (int(cloud.at(i).r) << 16); 1877 | } 1878 | } 1879 | if(oi == 0) 1880 | { 1881 | return cv::Mat(); 1882 | } 1883 | return laserScan(cv::Range::all(), cv::Range(0,oi)); 1884 | } 1885 | 1886 | cv::Mat laserScanFromPointCloud(const pcl::PointCloud & cloud, const Transform & transform) 1887 | { 1888 | return laserScanFromPointCloud(cloud, pcl::IndicesPtr(), transform); 1889 | } 1890 | cv::Mat laserScanFromPointCloud(const pcl::PointCloud & cloud, const pcl::IndicesPtr & indices, const Transform & transform) 1891 | { 1892 | cv::Mat laserScan; 1893 | bool nullTransform = transform.isNull() || transform.isIdentity(); 1894 | int oi = 0; 1895 | if(indices.get()) 1896 | { 1897 | laserScan = cv::Mat(1, (int)indices->size(), CV_32FC(7)); 1898 | for(unsigned int i=0; isize(); ++i) 1899 | { 1900 | int index = indices->at(i); 1901 | if(pcl::isFinite(cloud.at(index)) && 1902 | uIsFinite(cloud.at(index).normal_x) && 1903 | uIsFinite(cloud.at(index).normal_y) && 1904 | uIsFinite(cloud.at(index).normal_z)) 1905 | { 1906 | float * ptr = laserScan.ptr(0, oi++); 1907 | if(!nullTransform) 1908 | { 1909 | pcl::PointXYZRGBNormal pt = util3d::transformPoint(cloud.at(index), transform); 1910 | ptr[0] = pt.x; 1911 | ptr[1] = pt.y; 1912 | ptr[2] = pt.z; 1913 | ptr[4] = pt.normal_x; 1914 | ptr[5] = pt.normal_y; 1915 | ptr[6] = pt.normal_z; 1916 | } 1917 | else 1918 | { 1919 | ptr[0] = cloud.at(index).x; 1920 | ptr[1] = cloud.at(index).y; 1921 | ptr[2] = cloud.at(index).z; 1922 | ptr[4] = cloud.at(index).normal_x; 1923 | ptr[5] = cloud.at(index).normal_y; 1924 | ptr[6] = cloud.at(index).normal_z; 1925 | } 1926 | int * ptrInt = (int*)ptr; 1927 | ptrInt[3] = int(cloud.at(index).b) | (int(cloud.at(index).g) << 8) | (int(cloud.at(index).r) << 16); 1928 | } 1929 | } 1930 | } 1931 | else 1932 | { 1933 | laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC(7)); 1934 | for(unsigned int i=0; i(0, oi++); 1942 | if(!nullTransform) 1943 | { 1944 | pcl::PointXYZRGBNormal pt = util3d::transformPoint(cloud.at(i), transform); 1945 | ptr[0] = pt.x; 1946 | ptr[1] = pt.y; 1947 | ptr[2] = pt.z; 1948 | ptr[4] = pt.normal_x; 1949 | ptr[5] = pt.normal_y; 1950 | ptr[6] = pt.normal_z; 1951 | } 1952 | else 1953 | { 1954 | ptr[0] = cloud.at(i).x; 1955 | ptr[1] = cloud.at(i).y; 1956 | ptr[2] = cloud.at(i).z; 1957 | ptr[4] = cloud.at(i).normal_x; 1958 | ptr[5] = cloud.at(i).normal_y; 1959 | ptr[6] = cloud.at(i).normal_z; 1960 | } 1961 | int * ptrInt = (int*)ptr; 1962 | ptrInt[3] = int(cloud.at(i).b) | (int(cloud.at(i).g) << 8) | (int(cloud.at(i).r) << 16); 1963 | } 1964 | } 1965 | } 1966 | if(oi == 0) 1967 | { 1968 | return cv::Mat(); 1969 | } 1970 | return laserScan(cv::Range::all(), cv::Range(0,oi)); 1971 | } 1972 | 1973 | cv::Mat laserScanFromPointCloud(const pcl::PointCloud & cloud, const pcl::PointCloud & normals, const Transform & transform) 1974 | { 1975 | UASSERT(cloud.size() == normals.size()); 1976 | cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(7)); 1977 | bool nullTransform = transform.isNull() || transform.isIdentity(); 1978 | int oi=0; 1979 | for(unsigned int i=0; i(0, oi++); 1984 | if(!nullTransform) 1985 | { 1986 | pcl::PointXYZINormal pt; 1987 | pt.x = cloud.at(i).x; 1988 | pt.y = cloud.at(i).y; 1989 | pt.z = cloud.at(i).z; 1990 | pt.normal_x = normals.at(i).normal_x; 1991 | pt.normal_y = normals.at(i).normal_y; 1992 | pt.normal_z = normals.at(i).normal_z; 1993 | pt = util3d::transformPoint(pt, transform); 1994 | ptr[0] = pt.x; 1995 | ptr[1] = pt.y; 1996 | ptr[2] = pt.z; 1997 | ptr[4] = pt.normal_x; 1998 | ptr[5] = pt.normal_y; 1999 | ptr[6] = pt.normal_z; 2000 | } 2001 | else 2002 | { 2003 | ptr[0] = cloud.at(i).x; 2004 | ptr[1] = cloud.at(i).y; 2005 | ptr[2] = cloud.at(i).z; 2006 | ptr[4] = normals.at(i).normal_x; 2007 | ptr[5] = normals.at(i).normal_y; 2008 | ptr[6] = normals.at(i).normal_z; 2009 | } 2010 | ptr[3] = cloud.at(i).intensity; 2011 | } 2012 | } 2013 | if(oi == 0) 2014 | { 2015 | return cv::Mat(); 2016 | } 2017 | return laserScan(cv::Range::all(), cv::Range(0,oi)); 2018 | } 2019 | cv::Mat laserScanFromPointCloud(const pcl::PointCloud & cloud, const Transform & transform) 2020 | { 2021 | cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(7)); 2022 | bool nullTransform = transform.isNull() || transform.isIdentity(); 2023 | int oi = 0; 2024 | for(unsigned int i=0; i(0, oi++); 2032 | if(!nullTransform) 2033 | { 2034 | pcl::PointXYZINormal pt = util3d::transformPoint(cloud.at(i), transform); 2035 | ptr[0] = pt.x; 2036 | ptr[1] = pt.y; 2037 | ptr[2] = pt.z; 2038 | ptr[4] = pt.normal_x; 2039 | ptr[5] = pt.normal_y; 2040 | ptr[6] = pt.normal_z; 2041 | } 2042 | else 2043 | { 2044 | ptr[0] = cloud.at(i).x; 2045 | ptr[1] = cloud.at(i).y; 2046 | ptr[2] = cloud.at(i).z; 2047 | ptr[4] = cloud.at(i).normal_x; 2048 | ptr[5] = cloud.at(i).normal_y; 2049 | ptr[6] = cloud.at(i).normal_z; 2050 | } 2051 | ptr[3] = cloud.at(i).intensity; 2052 | } 2053 | } 2054 | if(oi == 0) 2055 | { 2056 | return cv::Mat(); 2057 | } 2058 | return laserScan(cv::Range::all(), cv::Range(0,oi)); 2059 | } 2060 | 2061 | cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud & cloud, const Transform & transform) 2062 | { 2063 | cv::Mat laserScan(1, (int)cloud.size(), CV_32FC2); 2064 | bool nullTransform = transform.isNull(); 2065 | Eigen::Affine3f transform3f = transform.toEigen3f(); 2066 | int oi=0; 2067 | for(unsigned int i=0; i(0, oi++); 2072 | if(!nullTransform) 2073 | { 2074 | pcl::PointXYZ pt = pcl::transformPoint(cloud.at(i), transform3f); 2075 | ptr[0] = pt.x; 2076 | ptr[1] = pt.y; 2077 | } 2078 | else 2079 | { 2080 | ptr[0] = cloud.at(i).x; 2081 | ptr[1] = cloud.at(i).y; 2082 | } 2083 | } 2084 | 2085 | } 2086 | if(oi == 0) 2087 | { 2088 | return cv::Mat(); 2089 | } 2090 | return laserScan(cv::Range::all(), cv::Range(0,oi)); 2091 | } 2092 | 2093 | cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud & cloud, const Transform & transform) 2094 | { 2095 | cv::Mat laserScan(1, (int)cloud.size(), CV_32FC3); 2096 | bool nullTransform = transform.isNull(); 2097 | Eigen::Affine3f transform3f = transform.toEigen3f(); 2098 | int oi=0; 2099 | for(unsigned int i=0; i(0, oi++); 2104 | if(!nullTransform) 2105 | { 2106 | pcl::PointXYZI pt = pcl::transformPoint(cloud.at(i), transform3f); 2107 | ptr[0] = pt.x; 2108 | ptr[1] = pt.y; 2109 | ptr[2] = pt.intensity; 2110 | } 2111 | else 2112 | { 2113 | ptr[0] = cloud.at(i).x; 2114 | ptr[1] = cloud.at(i).y; 2115 | ptr[2] = cloud.at(i).intensity; 2116 | } 2117 | } 2118 | 2119 | } 2120 | if(oi == 0) 2121 | { 2122 | return cv::Mat(); 2123 | } 2124 | return laserScan(cv::Range::all(), cv::Range(0,oi)); 2125 | } 2126 | 2127 | cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud & cloud, const Transform & transform) 2128 | { 2129 | cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(5)); 2130 | bool nullTransform = transform.isNull(); 2131 | int oi=0; 2132 | for(unsigned int i=0; i(0, oi++); 2140 | if(!nullTransform) 2141 | { 2142 | pcl::PointNormal pt = util3d::transformPoint(cloud.at(i), transform); 2143 | ptr[0] = pt.x; 2144 | ptr[1] = pt.y; 2145 | ptr[2] = pt.normal_x; 2146 | ptr[3] = pt.normal_y; 2147 | ptr[4] = pt.normal_z; 2148 | } 2149 | else 2150 | { 2151 | const pcl::PointNormal & pt = cloud.at(i); 2152 | ptr[0] = pt.x; 2153 | ptr[1] = pt.y; 2154 | ptr[2] = pt.normal_x; 2155 | ptr[3] = pt.normal_y; 2156 | ptr[4] = pt.normal_z; 2157 | } 2158 | } 2159 | } 2160 | if(oi == 0) 2161 | { 2162 | return cv::Mat(); 2163 | } 2164 | return laserScan(cv::Range::all(), cv::Range(0,oi)); 2165 | } 2166 | 2167 | cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud & cloud, const pcl::PointCloud & normals, const Transform & transform) 2168 | { 2169 | UASSERT(cloud.size() == normals.size()); 2170 | cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(5)); 2171 | bool nullTransform = transform.isNull() || transform.isIdentity(); 2172 | int oi=0; 2173 | for(unsigned int i=0; i(0, oi++); 2178 | if(!nullTransform) 2179 | { 2180 | pcl::PointNormal pt; 2181 | pt.x = cloud.at(i).x; 2182 | pt.y = cloud.at(i).y; 2183 | pt.z = cloud.at(i).z; 2184 | pt.normal_x = normals.at(i).normal_x; 2185 | pt.normal_y = normals.at(i).normal_y; 2186 | pt.normal_z = normals.at(i).normal_z; 2187 | pt = util3d::transformPoint(pt, transform); 2188 | ptr[0] = pt.x; 2189 | ptr[1] = pt.y; 2190 | ptr[2] = pt.normal_x; 2191 | ptr[3] = pt.normal_y; 2192 | ptr[4] = pt.normal_z; 2193 | } 2194 | else 2195 | { 2196 | ptr[0] = cloud.at(i).x; 2197 | ptr[1] = cloud.at(i).y; 2198 | ptr[2] = normals.at(i).normal_x; 2199 | ptr[3] = normals.at(i).normal_y; 2200 | ptr[4] = normals.at(i).normal_z; 2201 | } 2202 | } 2203 | } 2204 | if(oi == 0) 2205 | { 2206 | return cv::Mat(); 2207 | } 2208 | return laserScan(cv::Range::all(), cv::Range(0,oi)); 2209 | } 2210 | 2211 | cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud & cloud, const Transform & transform) 2212 | { 2213 | cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(6)); 2214 | bool nullTransform = transform.isNull(); 2215 | int oi=0; 2216 | for(unsigned int i=0; i(0, oi++); 2224 | if(!nullTransform) 2225 | { 2226 | pcl::PointXYZINormal pt = util3d::transformPoint(cloud.at(i), transform); 2227 | ptr[0] = pt.x; 2228 | ptr[1] = pt.y; 2229 | ptr[2] = pt.intensity; 2230 | ptr[3] = pt.normal_x; 2231 | ptr[4] = pt.normal_y; 2232 | ptr[5] = pt.normal_z; 2233 | } 2234 | else 2235 | { 2236 | const pcl::PointXYZINormal & pt = cloud.at(i); 2237 | ptr[0] = pt.x; 2238 | ptr[1] = pt.y; 2239 | ptr[2] = pt.intensity; 2240 | ptr[3] = pt.normal_x; 2241 | ptr[4] = pt.normal_y; 2242 | ptr[5] = pt.normal_z; 2243 | } 2244 | } 2245 | } 2246 | if(oi == 0) 2247 | { 2248 | return cv::Mat(); 2249 | } 2250 | return laserScan(cv::Range::all(), cv::Range(0,oi)); 2251 | } 2252 | 2253 | cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud & cloud, const pcl::PointCloud & normals, const Transform & transform) 2254 | { 2255 | UASSERT(cloud.size() == normals.size()); 2256 | cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(6)); 2257 | bool nullTransform = transform.isNull() || transform.isIdentity(); 2258 | int oi=0; 2259 | for(unsigned int i=0; i(0, oi++); 2264 | if(!nullTransform) 2265 | { 2266 | pcl::PointXYZINormal pt; 2267 | pt.x = cloud.at(i).x; 2268 | pt.y = cloud.at(i).y; 2269 | pt.z = cloud.at(i).z; 2270 | pt.normal_x = normals.at(i).normal_x; 2271 | pt.normal_y = normals.at(i).normal_y; 2272 | pt.normal_z = normals.at(i).normal_z; 2273 | pt = util3d::transformPoint(pt, transform); 2274 | ptr[0] = pt.x; 2275 | ptr[1] = pt.y; 2276 | ptr[2] = pt.intensity; 2277 | ptr[3] = pt.normal_x; 2278 | ptr[4] = pt.normal_y; 2279 | ptr[5] = pt.normal_z; 2280 | } 2281 | else 2282 | { 2283 | ptr[0] = cloud.at(i).x; 2284 | ptr[1] = cloud.at(i).y; 2285 | ptr[2] = cloud.at(i).intensity; 2286 | ptr[3] = normals.at(i).normal_x; 2287 | ptr[4] = normals.at(i).normal_y; 2288 | ptr[5] = normals.at(i).normal_z; 2289 | } 2290 | } 2291 | } 2292 | if(oi == 0) 2293 | { 2294 | return cv::Mat(); 2295 | } 2296 | return laserScan(cv::Range::all(), cv::Range(0,oi)); 2297 | } 2298 | 2299 | pcl::PointCloud::Ptr laserScanToPointCloud(const LaserScan & laserScan, const Transform & transform) 2300 | { 2301 | pcl::PointCloud::Ptr output(new pcl::PointCloud); 2302 | output->resize(laserScan.size()); 2303 | output->is_dense = true; 2304 | bool nullTransform = transform.isNull(); 2305 | Eigen::Affine3f transform3f = transform.toEigen3f(); 2306 | for(int i=0; iat(i) = util3d::laserScanToPoint(laserScan, i); 2309 | if(!nullTransform) 2310 | { 2311 | output->at(i) = pcl::transformPoint(output->at(i), transform3f); 2312 | } 2313 | } 2314 | return output; 2315 | } 2316 | 2317 | pcl::PointCloud::Ptr laserScanToPointCloudNormal(const LaserScan & laserScan, const Transform & transform) 2318 | { 2319 | pcl::PointCloud::Ptr output(new pcl::PointCloud); 2320 | output->resize(laserScan.size()); 2321 | output->is_dense = true; 2322 | bool nullTransform = transform.isNull(); 2323 | for(int i=0; iat(i) = laserScanToPointNormal(laserScan, i); 2326 | if(!nullTransform) 2327 | { 2328 | output->at(i) = util3d::transformPoint(output->at(i), transform); 2329 | } 2330 | } 2331 | return output; 2332 | } 2333 | 2334 | pcl::PointCloud::Ptr laserScanToPointCloudRGB(const LaserScan & laserScan, const Transform & transform, unsigned char r, unsigned char g, unsigned char b) 2335 | { 2336 | pcl::PointCloud::Ptr output(new pcl::PointCloud); 2337 | output->resize(laserScan.size()); 2338 | output->is_dense = true; 2339 | bool nullTransform = transform.isNull() || transform.isIdentity(); 2340 | Eigen::Affine3f transform3f = transform.toEigen3f(); 2341 | for(int i=0; iat(i) = util3d::laserScanToPointRGB(laserScan, i, r, g, b); 2344 | if(!nullTransform) 2345 | { 2346 | output->at(i) = pcl::transformPoint(output->at(i), transform3f); 2347 | } 2348 | } 2349 | return output; 2350 | } 2351 | 2352 | pcl::PointCloud::Ptr laserScanToPointCloudI(const LaserScan & laserScan, const Transform & transform, float intensity) 2353 | { 2354 | pcl::PointCloud::Ptr output(new pcl::PointCloud); 2355 | output->resize(laserScan.size()); 2356 | output->is_dense = true; 2357 | bool nullTransform = transform.isNull() || transform.isIdentity(); 2358 | Eigen::Affine3f transform3f = transform.toEigen3f(); 2359 | for(int i=0; iat(i) = util3d::laserScanToPointI(laserScan, i, intensity); 2362 | if(!nullTransform) 2363 | { 2364 | output->at(i) = pcl::transformPoint(output->at(i), transform3f); 2365 | } 2366 | } 2367 | return output; 2368 | } 2369 | 2370 | pcl::PointCloud::Ptr laserScanToPointCloudRGBNormal(const LaserScan & laserScan, const Transform & transform, unsigned char r, unsigned char g, unsigned char b) 2371 | { 2372 | pcl::PointCloud::Ptr output(new pcl::PointCloud); 2373 | output->resize(laserScan.size()); 2374 | output->is_dense = true; 2375 | bool nullTransform = transform.isNull() || transform.isIdentity(); 2376 | for(int i=0; iat(i) = util3d::laserScanToPointRGBNormal(laserScan, i, r, g, b); 2379 | if(!nullTransform) 2380 | { 2381 | output->at(i) = util3d::transformPoint(output->at(i), transform); 2382 | } 2383 | } 2384 | return output; 2385 | } 2386 | 2387 | pcl::PointCloud::Ptr laserScanToPointCloudINormal(const LaserScan & laserScan, const Transform & transform, float intensity) 2388 | { 2389 | pcl::PointCloud::Ptr output(new pcl::PointCloud); 2390 | output->resize(laserScan.size()); 2391 | output->is_dense = true; 2392 | bool nullTransform = transform.isNull() || transform.isIdentity(); 2393 | for(int i=0; iat(i) = util3d::laserScanToPointINormal(laserScan, i, intensity); 2396 | if(!nullTransform) 2397 | { 2398 | output->at(i) = util3d::transformPoint(output->at(i), transform); 2399 | } 2400 | } 2401 | return output; 2402 | } 2403 | 2404 | pcl::PointXYZ laserScanToPoint(const LaserScan & laserScan, int index) 2405 | { 2406 | UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size()); 2407 | pcl::PointXYZ output; 2408 | const float * ptr = laserScan.data().ptr(0, index); 2409 | output.x = ptr[0]; 2410 | output.y = ptr[1]; 2411 | if(!laserScan.is2d()) 2412 | { 2413 | output.z = ptr[2]; 2414 | } 2415 | return output; 2416 | } 2417 | 2418 | pcl::PointNormal laserScanToPointNormal(const LaserScan & laserScan, int index) 2419 | { 2420 | UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size()); 2421 | pcl::PointNormal output; 2422 | const float * ptr = laserScan.data().ptr(0, index); 2423 | output.x = ptr[0]; 2424 | output.y = ptr[1]; 2425 | if(!laserScan.is2d()) 2426 | { 2427 | output.z = ptr[2]; 2428 | } 2429 | if(laserScan.hasNormals()) 2430 | { 2431 | int offset = laserScan.getNormalsOffset(); 2432 | output.normal_x = ptr[offset]; 2433 | output.normal_y = ptr[offset+1]; 2434 | output.normal_z = ptr[offset+2]; 2435 | } 2436 | return output; 2437 | } 2438 | 2439 | pcl::PointXYZRGB laserScanToPointRGB(const LaserScan & laserScan, int index, unsigned char r, unsigned char g, unsigned char b) 2440 | { 2441 | UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size()); 2442 | pcl::PointXYZRGB output; 2443 | const float * ptr = laserScan.data().ptr(0, index); 2444 | output.x = ptr[0]; 2445 | output.y = ptr[1]; 2446 | if(!laserScan.is2d()) 2447 | { 2448 | output.z = ptr[2]; 2449 | } 2450 | 2451 | if(laserScan.hasRGB()) 2452 | { 2453 | int * ptrInt = (int*)ptr; 2454 | int indexRGB = laserScan.getRGBOffset(); 2455 | output.b = (unsigned char)(ptrInt[indexRGB] & 0xFF); 2456 | output.g = (unsigned char)((ptrInt[indexRGB] >> 8) & 0xFF); 2457 | output.r = (unsigned char)((ptrInt[indexRGB] >> 16) & 0xFF); 2458 | } 2459 | else 2460 | { 2461 | output.r = r; 2462 | output.g = g; 2463 | output.b = b; 2464 | } 2465 | return output; 2466 | } 2467 | 2468 | pcl::PointXYZI laserScanToPointI(const LaserScan & laserScan, int index, float intensity) 2469 | { 2470 | UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size()); 2471 | pcl::PointXYZI output; 2472 | const float * ptr = laserScan.data().ptr(0, index); 2473 | output.x = ptr[0]; 2474 | output.y = ptr[1]; 2475 | if(!laserScan.is2d()) 2476 | { 2477 | output.z = ptr[2]; 2478 | } 2479 | 2480 | if(laserScan.hasIntensity()) 2481 | { 2482 | int offset = laserScan.getIntensityOffset(); 2483 | output.intensity = ptr[offset]; 2484 | } 2485 | else 2486 | { 2487 | output.intensity = intensity; 2488 | } 2489 | 2490 | return output; 2491 | } 2492 | 2493 | pcl::PointXYZRGBNormal laserScanToPointRGBNormal(const LaserScan & laserScan, int index, unsigned char r, unsigned char g, unsigned char b) 2494 | { 2495 | UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size()); 2496 | pcl::PointXYZRGBNormal output; 2497 | const float * ptr = laserScan.data().ptr(0, index); 2498 | output.x = ptr[0]; 2499 | output.y = ptr[1]; 2500 | if(!laserScan.is2d()) 2501 | { 2502 | output.z = ptr[2]; 2503 | } 2504 | 2505 | if(laserScan.hasRGB()) 2506 | { 2507 | int * ptrInt = (int*)ptr; 2508 | int indexRGB = laserScan.getRGBOffset(); 2509 | output.b = (unsigned char)(ptrInt[indexRGB] & 0xFF); 2510 | output.g = (unsigned char)((ptrInt[indexRGB] >> 8) & 0xFF); 2511 | output.r = (unsigned char)((ptrInt[indexRGB] >> 16) & 0xFF); 2512 | } 2513 | else 2514 | { 2515 | output.r = r; 2516 | output.g = g; 2517 | output.b = b; 2518 | } 2519 | 2520 | if(laserScan.hasNormals()) 2521 | { 2522 | int offset = laserScan.getNormalsOffset(); 2523 | output.normal_x = ptr[offset]; 2524 | output.normal_y = ptr[offset+1]; 2525 | output.normal_z = ptr[offset+2]; 2526 | } 2527 | 2528 | return output; 2529 | } 2530 | 2531 | pcl::PointXYZINormal laserScanToPointINormal(const LaserScan & laserScan, int index, float intensity) 2532 | { 2533 | UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size()); 2534 | pcl::PointXYZINormal output; 2535 | const float * ptr = laserScan.data().ptr(0, index); 2536 | output.x = ptr[0]; 2537 | output.y = ptr[1]; 2538 | if(!laserScan.is2d()) 2539 | { 2540 | output.z = ptr[2]; 2541 | } 2542 | 2543 | if(laserScan.hasIntensity()) 2544 | { 2545 | int offset = laserScan.getIntensityOffset(); 2546 | output.intensity = ptr[offset]; 2547 | } 2548 | else 2549 | { 2550 | output.intensity = intensity; 2551 | } 2552 | 2553 | if(laserScan.hasNormals()) 2554 | { 2555 | int offset = laserScan.getNormalsOffset(); 2556 | output.normal_x = ptr[offset]; 2557 | output.normal_y = ptr[offset+1]; 2558 | output.normal_z = ptr[offset+2]; 2559 | } 2560 | 2561 | return output; 2562 | } 2563 | 2564 | void getMinMax3D(const cv::Mat & laserScan, cv::Point3f & min, cv::Point3f & max) 2565 | { 2566 | UASSERT(!laserScan.empty()); 2567 | UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7)); 2568 | 2569 | const float * ptr = laserScan.ptr(0, 0); 2570 | min.x = max.x = ptr[0]; 2571 | min.y = max.y = ptr[1]; 2572 | bool is3d = laserScan.channels() >= 3 && laserScan.channels() != 5; 2573 | min.z = max.z = is3d?ptr[2]:0.0f; 2574 | for(int i=1; i(0, i); 2577 | 2578 | if(ptr[0] < min.x) min.x = ptr[0]; 2579 | else if(ptr[0] > max.x) max.x = ptr[0]; 2580 | 2581 | if(ptr[1] < min.y) min.y = ptr[1]; 2582 | else if(ptr[1] > max.y) max.y = ptr[1]; 2583 | 2584 | if(is3d) 2585 | { 2586 | if(ptr[2] < min.z) min.z = ptr[2]; 2587 | else if(ptr[2] > max.z) max.z = ptr[2]; 2588 | } 2589 | } 2590 | } 2591 | void getMinMax3D(const cv::Mat & laserScan, pcl::PointXYZ & min, pcl::PointXYZ & max) 2592 | { 2593 | cv::Point3f minCV, maxCV; 2594 | getMinMax3D(laserScan, minCV, maxCV); 2595 | min.x = minCV.x; 2596 | min.y = minCV.y; 2597 | min.z = minCV.z; 2598 | max.x = maxCV.x; 2599 | max.y = maxCV.y; 2600 | max.z = maxCV.z; 2601 | } 2602 | 2603 | // inspired from ROS image_geometry/src/stereo_camera_model.cpp 2604 | cv::Point3f projectDisparityTo3D( 2605 | const cv::Point2f & pt, 2606 | float disparity, 2607 | const StereoCameraModel & model) 2608 | { 2609 | if(disparity > 0.0f && model.baseline() > 0.0f && model.left().fx() > 0.0f) 2610 | { 2611 | //Z = baseline * f / (d + cx1-cx0); 2612 | float c = 0.0f; 2613 | if(model.right().cx()>0.0f && model.left().cx()>0.0f) 2614 | { 2615 | c = model.right().cx() - model.left().cx(); 2616 | } 2617 | float W = model.baseline()/(disparity + c); 2618 | return cv::Point3f((pt.x - model.left().cx())*W, (pt.y - model.left().cy())*W, model.left().fx()*W); 2619 | } 2620 | float bad_point = std::numeric_limits::quiet_NaN (); 2621 | return cv::Point3f(bad_point, bad_point, bad_point); 2622 | } 2623 | 2624 | cv::Point3f projectDisparityTo3D( 2625 | const cv::Point2f & pt, 2626 | const cv::Mat & disparity, 2627 | const StereoCameraModel & model) 2628 | { 2629 | UASSERT(!disparity.empty() && (disparity.type() == CV_32FC1 || disparity.type() == CV_16SC1)); 2630 | int u = int(pt.x+0.5f); 2631 | int v = int(pt.y+0.5f); 2632 | float bad_point = std::numeric_limits::quiet_NaN (); 2633 | if(uIsInBounds(u, 0, disparity.cols) && 2634 | uIsInBounds(v, 0, disparity.rows)) 2635 | { 2636 | float d = disparity.type() == CV_16SC1?float(disparity.at(v,u))/16.0f:disparity.at(v,u); 2637 | return projectDisparityTo3D(pt, d, model); 2638 | } 2639 | return cv::Point3f(bad_point, bad_point, bad_point); 2640 | } 2641 | 2642 | // Register point cloud to camera (return registered depth image) 2643 | cv::Mat projectCloudToCamera( 2644 | const cv::Size & imageSize, 2645 | const cv::Mat & cameraMatrixK, 2646 | const cv::Mat & laserScan, // assuming laser scan points are already in /base_link coordinate 2647 | const rtabmap::Transform & cameraTransform) // /base_link -> /camera_link 2648 | { 2649 | UASSERT(!cameraTransform.isNull()); 2650 | UASSERT(!laserScan.empty()); 2651 | UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7)); 2652 | UASSERT(cameraMatrixK.type() == CV_64FC1 && cameraMatrixK.cols == 3 && cameraMatrixK.cols == 3); 2653 | 2654 | float fx = cameraMatrixK.at(0,0); 2655 | float fy = cameraMatrixK.at(1,1); 2656 | float cx = cameraMatrixK.at(0,2); 2657 | float cy = cameraMatrixK.at(1,2); 2658 | 2659 | cv::Mat registered = cv::Mat::zeros(imageSize, CV_32FC1); 2660 | Transform t = cameraTransform.inverse(); 2661 | 2662 | int count = 0; 2663 | for(int i=0; i(0, i); 2666 | 2667 | // Get 3D from laser scan 2668 | cv::Point3f ptScan; 2669 | if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC(5)) 2670 | { 2671 | // 2D scans 2672 | ptScan.x = ptr[0]; 2673 | ptScan.y = ptr[1]; 2674 | ptScan.z = 0; 2675 | } 2676 | else // 3D scans 2677 | { 2678 | ptScan.x = ptr[0]; 2679 | ptScan.y = ptr[1]; 2680 | ptScan.z = ptr[2]; 2681 | } 2682 | ptScan = util3d::transformPoint(ptScan, t); 2683 | 2684 | // re-project in camera frame 2685 | float z = ptScan.z; 2686 | 2687 | bool set = false; 2688 | if(z > 0.0f) 2689 | { 2690 | float invZ = 1.0f/z; 2691 | float dx = (fx*ptScan.x)*invZ + cx; 2692 | float dy = (fy*ptScan.y)*invZ + cy; 2693 | int dx_low = dx; 2694 | int dy_low = dy; 2695 | int dx_high = dx + 0.5f; 2696 | int dy_high = dy + 0.5f; 2697 | 2698 | if(uIsInBounds(dx_low, 0, registered.cols) && uIsInBounds(dy_low, 0, registered.rows)) 2699 | { 2700 | float &zReg = registered.at(dy_low, dx_low); 2701 | if(zReg == 0 || z < zReg) 2702 | { 2703 | zReg = z; 2704 | } 2705 | set = true; 2706 | } 2707 | if((dx_low != dx_high || dy_low != dy_high) && 2708 | uIsInBounds(dx_high, 0, registered.cols) && uIsInBounds(dy_high, 0, registered.rows)) 2709 | { 2710 | float &zReg = registered.at(dy_high, dx_high); 2711 | if(zReg == 0 || z < zReg) 2712 | { 2713 | zReg = z; 2714 | } 2715 | set = true; 2716 | } 2717 | } 2718 | if(set) 2719 | { 2720 | count++; 2721 | } 2722 | } 2723 | UDEBUG("Points in camera=%d/%d", count, laserScan.cols); 2724 | 2725 | return registered; 2726 | } 2727 | 2728 | cv::Mat projectCloudToCamera( 2729 | const cv::Size & imageSize, 2730 | const cv::Mat & cameraMatrixK, 2731 | const pcl::PointCloud::Ptr laserScan, // assuming points are already in /base_link coordinate 2732 | const rtabmap::Transform & cameraTransform) // /base_link -> /camera_link 2733 | { 2734 | UASSERT(!cameraTransform.isNull()); 2735 | UASSERT(!laserScan->empty()); 2736 | UASSERT(cameraMatrixK.type() == CV_64FC1 && cameraMatrixK.cols == 3 && cameraMatrixK.cols == 3); 2737 | 2738 | float fx = cameraMatrixK.at(0,0); 2739 | float fy = cameraMatrixK.at(1,1); 2740 | float cx = cameraMatrixK.at(0,2); 2741 | float cy = cameraMatrixK.at(1,2); 2742 | 2743 | cv::Mat registered = cv::Mat::zeros(imageSize, CV_32FC1); 2744 | Transform t = cameraTransform.inverse(); 2745 | 2746 | int count = 0; 2747 | for(int i=0; i<(int)laserScan->size(); ++i) 2748 | { 2749 | // Get 3D from laser scan 2750 | pcl::PointXYZ ptScan = laserScan->at(i); 2751 | ptScan = util3d::transformPoint(ptScan, t); 2752 | 2753 | // re-project in camera frame 2754 | float z = ptScan.z; 2755 | bool set = false; 2756 | if(z > 0.0f) 2757 | { 2758 | float invZ = 1.0f/z; 2759 | float dx = (fx*ptScan.x)*invZ + cx; 2760 | float dy = (fy*ptScan.y)*invZ + cy; 2761 | int dx_low = dx; 2762 | int dy_low = dy; 2763 | int dx_high = dx + 0.5f; 2764 | int dy_high = dy + 0.5f; 2765 | if(uIsInBounds(dx_low, 0, registered.cols) && uIsInBounds(dy_low, 0, registered.rows)) 2766 | { 2767 | set = true; 2768 | float &zReg = registered.at(dy_low, dx_low); 2769 | if(zReg == 0 || z < zReg) 2770 | { 2771 | zReg = z; 2772 | } 2773 | } 2774 | if((dx_low != dx_high || dy_low != dy_high) && 2775 | uIsInBounds(dx_high, 0, registered.cols) && uIsInBounds(dy_high, 0, registered.rows)) 2776 | { 2777 | set = true; 2778 | float &zReg = registered.at(dy_high, dx_high); 2779 | if(zReg == 0 || z < zReg) 2780 | { 2781 | zReg = z; 2782 | } 2783 | } 2784 | } 2785 | if(set) 2786 | { 2787 | count++; 2788 | } 2789 | } 2790 | UDEBUG("Points in camera=%d/%d", count, (int)laserScan->size()); 2791 | 2792 | return registered; 2793 | } 2794 | 2795 | cv::Mat projectCloudToCamera( 2796 | const cv::Size & imageSize, 2797 | const cv::Mat & cameraMatrixK, 2798 | const pcl::PCLPointCloud2::Ptr laserScan, // assuming points are already in /base_link coordinate 2799 | const rtabmap::Transform & cameraTransform) // /base_link -> /camera_link 2800 | { 2801 | UASSERT(!cameraTransform.isNull()); 2802 | UASSERT(!laserScan->data.empty()); 2803 | UASSERT(cameraMatrixK.type() == CV_64FC1 && cameraMatrixK.cols == 3 && cameraMatrixK.cols == 3); 2804 | 2805 | float fx = cameraMatrixK.at(0,0); 2806 | float fy = cameraMatrixK.at(1,1); 2807 | float cx = cameraMatrixK.at(0,2); 2808 | float cy = cameraMatrixK.at(1,2); 2809 | 2810 | cv::Mat registered = cv::Mat::zeros(imageSize, CV_32FC1); 2811 | Transform t = cameraTransform.inverse(); 2812 | 2813 | pcl::MsgFieldMap field_map; 2814 | pcl::createMapping (laserScan->fields, field_map); 2815 | 2816 | int count = 0; 2817 | if(field_map.size() == 1) 2818 | { 2819 | for (uint32_t row = 0; row < laserScan->height; ++row) 2820 | { 2821 | const uint8_t* row_data = &laserScan->data[row * laserScan->row_step]; 2822 | for (uint32_t col = 0; col < laserScan->width; ++col) 2823 | { 2824 | const uint8_t* msg_data = row_data + col * laserScan->point_step; 2825 | pcl::PointXYZ ptScan; 2826 | memcpy (&ptScan, msg_data + field_map.front().serialized_offset, field_map.front().size); 2827 | ptScan = util3d::transformPoint(ptScan, t); 2828 | 2829 | // re-project in camera frame 2830 | float z = ptScan.z; 2831 | bool set = false; 2832 | if(z > 0.0f) 2833 | { 2834 | float invZ = 1.0f/z; 2835 | float dx = (fx*ptScan.x)*invZ + cx; 2836 | float dy = (fy*ptScan.y)*invZ + cy; 2837 | int dx_low = dx; 2838 | int dy_low = dy; 2839 | int dx_high = dx + 0.5f; 2840 | int dy_high = dy + 0.5f; 2841 | if(uIsInBounds(dx_low, 0, registered.cols) && uIsInBounds(dy_low, 0, registered.rows)) 2842 | { 2843 | set = true; 2844 | float &zReg = registered.at(dy_low, dx_low); 2845 | if(zReg == 0 || z < zReg) 2846 | { 2847 | zReg = z; 2848 | } 2849 | } 2850 | if((dx_low != dx_high || dy_low != dy_high) && 2851 | uIsInBounds(dx_high, 0, registered.cols) && uIsInBounds(dy_high, 0, registered.rows)) 2852 | { 2853 | set = true; 2854 | float &zReg = registered.at(dy_high, dx_high); 2855 | if(zReg == 0 || z < zReg) 2856 | { 2857 | zReg = z; 2858 | } 2859 | } 2860 | } 2861 | if(set) 2862 | { 2863 | count++; 2864 | } 2865 | } 2866 | } 2867 | } 2868 | else 2869 | { 2870 | UERROR("field map pcl::pointXYZ not found!"); 2871 | } 2872 | /* 2873 | int count = 0; 2874 | for(int i=0; i<(int)laserScan->size(); ++i) 2875 | { 2876 | // Get 3D from laser scan 2877 | pcl::PointXYZ ptScan = laserScan->at(i); 2878 | ptScan = util3d::transformPoint(ptScan, t); 2879 | 2880 | // re-project in camera frame 2881 | float z = ptScan.z; 2882 | bool set = false; 2883 | if(z > 0.0f) 2884 | { 2885 | float invZ = 1.0f/z; 2886 | float dx = (fx*ptScan.x)*invZ + cx; 2887 | float dy = (fy*ptScan.y)*invZ + cy; 2888 | int dx_low = dx; 2889 | int dy_low = dy; 2890 | int dx_high = dx + 0.5f; 2891 | int dy_high = dy + 0.5f; 2892 | if(uIsInBounds(dx_low, 0, registered.cols) && uIsInBounds(dy_low, 0, registered.rows)) 2893 | { 2894 | set = true; 2895 | float &zReg = registered.at(dy_low, dx_low); 2896 | if(zReg == 0 || z < zReg) 2897 | { 2898 | zReg = z; 2899 | } 2900 | } 2901 | if((dx_low != dx_high || dy_low != dy_high) && 2902 | uIsInBounds(dx_high, 0, registered.cols) && uIsInBounds(dy_high, 0, registered.rows)) 2903 | { 2904 | set = true; 2905 | float &zReg = registered.at(dy_high, dx_high); 2906 | if(zReg == 0 || z < zReg) 2907 | { 2908 | zReg = z; 2909 | } 2910 | } 2911 | } 2912 | if(set) 2913 | { 2914 | count++; 2915 | } 2916 | } 2917 | */ 2918 | UDEBUG("Points in camera=%d/%d", count, (int)laserScan->data.size()); 2919 | 2920 | return registered; 2921 | } 2922 | 2923 | void fillProjectedCloudHoles(cv::Mat & registeredDepth, bool verticalDirection, bool fillToBorder) 2924 | { 2925 | UASSERT(registeredDepth.type() == CV_32FC1); 2926 | if(verticalDirection) 2927 | { 2928 | // vertical, for each column 2929 | for(int x=0; x(y,x); 2936 | if(fillToBorder && y == registeredDepth.rows-1 && v<=0.0f && indexA>=0) 2937 | { 2938 | v = valueA; 2939 | } 2940 | if(v > 0.0f) 2941 | { 2942 | if(fillToBorder && indexA < 0) 2943 | { 2944 | indexA = 0; 2945 | valueA = v; 2946 | } 2947 | if(indexA >=0) 2948 | { 2949 | int range = y-indexA; 2950 | if(range > 1) 2951 | { 2952 | float slope = (v-valueA)/(range); 2953 | for(int k=1; k(indexA+k,x) = valueA+slope*float(k); 2956 | } 2957 | } 2958 | } 2959 | valueA = v; 2960 | indexA = y; 2961 | } 2962 | } 2963 | } 2964 | } 2965 | else 2966 | { 2967 | // horizontal, for each row 2968 | for(int y=0; y(y,x); 2975 | if(fillToBorder && x == registeredDepth.cols-1 && v<=0.0f && indexA>=0) 2976 | { 2977 | v = valueA; 2978 | } 2979 | if(v > 0.0f) 2980 | { 2981 | if(fillToBorder && indexA < 0) 2982 | { 2983 | indexA = 0; 2984 | valueA = v; 2985 | } 2986 | if(indexA >=0) 2987 | { 2988 | int range = x-indexA; 2989 | if(range > 1) 2990 | { 2991 | float slope = (v-valueA)/(range); 2992 | for(int k=1; k(y,indexA+k) = valueA+slope*float(k); 2995 | } 2996 | } 2997 | } 2998 | valueA = v; 2999 | indexA = x; 3000 | } 3001 | } 3002 | } 3003 | } 3004 | } 3005 | 3006 | bool isFinite(const cv::Point3f & pt) 3007 | { 3008 | return uIsFinite(pt.x) && uIsFinite(pt.y) && uIsFinite(pt.z); 3009 | } 3010 | 3011 | pcl::PointCloud::Ptr concatenateClouds(const std::list::Ptr> & clouds) 3012 | { 3013 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud); 3014 | for(std::list::Ptr>::const_iterator iter = clouds.begin(); iter!=clouds.end(); ++iter) 3015 | { 3016 | *cloud += *(*iter); 3017 | } 3018 | return cloud; 3019 | } 3020 | 3021 | pcl::PointCloud::Ptr concatenateClouds(const std::list::Ptr> & clouds) 3022 | { 3023 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud); 3024 | for(std::list::Ptr>::const_iterator iter = clouds.begin(); iter!=clouds.end(); ++iter) 3025 | { 3026 | *cloud+=*(*iter); 3027 | } 3028 | return cloud; 3029 | } 3030 | 3031 | pcl::IndicesPtr concatenate(const std::vector & indices) 3032 | { 3033 | //compute total size 3034 | unsigned int totalSize = 0; 3035 | for(unsigned int i=0; isize(); 3038 | } 3039 | pcl::IndicesPtr ind(new std::vector(totalSize)); 3040 | unsigned int io = 0; 3041 | for(unsigned int i=0; isize(); ++j) 3044 | { 3045 | ind->at(io++) = indices[i]->at(j); 3046 | } 3047 | } 3048 | return ind; 3049 | } 3050 | 3051 | pcl::IndicesPtr concatenate(const pcl::IndicesPtr & indicesA, const pcl::IndicesPtr & indicesB) 3052 | { 3053 | pcl::IndicesPtr ind(new std::vector(*indicesA)); 3054 | ind->resize(ind->size()+indicesB->size()); 3055 | unsigned int oi = (unsigned int)indicesA->size(); 3056 | for(unsigned int i=0; isize(); ++i) 3057 | { 3058 | ind->at(oi++) = indicesB->at(i); 3059 | } 3060 | return ind; 3061 | } 3062 | 3063 | void savePCDWords( 3064 | const std::string & fileName, 3065 | const std::multimap & words, 3066 | const Transform & transform) 3067 | { 3068 | if(words.size()) 3069 | { 3070 | pcl::PointCloud cloud; 3071 | cloud.resize(words.size()); 3072 | int i=0; 3073 | for(std::multimap::const_iterator iter=words.begin(); iter!=words.end(); ++iter) 3074 | { 3075 | cloud[i++] = transformPoint(iter->second, transform); 3076 | } 3077 | pcl::io::savePCDFile(fileName, cloud); 3078 | } 3079 | } 3080 | 3081 | void savePCDWords( 3082 | const std::string & fileName, 3083 | const std::multimap & words, 3084 | const Transform & transform) 3085 | { 3086 | if(words.size()) 3087 | { 3088 | pcl::PointCloud cloud; 3089 | cloud.resize(words.size()); 3090 | int i=0; 3091 | for(std::multimap::const_iterator iter=words.begin(); iter!=words.end(); ++iter) 3092 | { 3093 | cv::Point3f pt = transformPoint(iter->second, transform); 3094 | cloud[i++] = pcl::PointXYZ(pt.x, pt.y, pt.z); 3095 | } 3096 | pcl::io::savePCDFile(fileName, cloud); 3097 | } 3098 | } 3099 | 3100 | cv::Mat loadBINScan(const std::string & fileName) 3101 | { 3102 | cv::Mat output; 3103 | long bytes = UFile::length(fileName); 3104 | if(bytes) 3105 | { 3106 | int dim = 4; 3107 | UASSERT(bytes % sizeof(float) == 0); 3108 | size_t num = bytes/sizeof(float); 3109 | UASSERT(num % dim == 0); 3110 | output = cv::Mat(1, num/dim, CV_32FC(dim)); 3111 | 3112 | // load point cloud 3113 | FILE *stream; 3114 | stream = fopen (fileName.c_str(),"rb"); 3115 | size_t actualReadNum = fread(output.data,sizeof(float),num,stream); 3116 | UASSERT(num == actualReadNum); 3117 | fclose(stream); 3118 | } 3119 | 3120 | return output; 3121 | } 3122 | 3123 | pcl::PointCloud::Ptr loadBINCloud(const std::string & fileName) 3124 | { 3125 | return laserScanToPointCloud(loadScan(fileName)); 3126 | } 3127 | 3128 | pcl::PointCloud::Ptr loadBINCloud(const std::string & fileName, int dim) 3129 | { 3130 | return loadBINCloud(fileName); 3131 | } 3132 | 3133 | LaserScan loadScan(const std::string & path) 3134 | { 3135 | std::string fileName = UFile::getName(path); 3136 | if(UFile::getExtension(fileName).compare("bin") == 0) 3137 | { 3138 | return LaserScan(loadBINScan(path), 0, 0, LaserScan::kXYZI); 3139 | } 3140 | else if(UFile::getExtension(fileName).compare("pcd") == 0) 3141 | { 3142 | pcl::PCLPointCloud2::Ptr cloud(new pcl::PCLPointCloud2); 3143 | pcl::io::loadPCDFile(path, *cloud); 3144 | return laserScanFromPointCloud(*cloud); 3145 | } 3146 | else 3147 | { 3148 | pcl::PCLPointCloud2::Ptr cloud(new pcl::PCLPointCloud2); 3149 | pcl::io::loadPLYFile(path, *cloud); 3150 | return laserScanFromPointCloud(*cloud); 3151 | } 3152 | return LaserScan(); 3153 | } 3154 | 3155 | pcl::PointCloud::Ptr loadCloud( 3156 | const std::string & path, 3157 | const Transform & transform, 3158 | int downsampleStep, 3159 | float voxelSize) 3160 | { 3161 | UASSERT(!transform.isNull()); 3162 | UDEBUG("Loading cloud (step=%d, voxel=%f m) : %s", downsampleStep, voxelSize, path.c_str()); 3163 | std::string fileName = UFile::getName(path); 3164 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud); 3165 | if(UFile::getExtension(fileName).compare("bin") == 0) 3166 | { 3167 | cloud = util3d::loadBINCloud(path); // Assume KITTI velodyne format 3168 | } 3169 | else if(UFile::getExtension(fileName).compare("pcd") == 0) 3170 | { 3171 | pcl::io::loadPCDFile(path, *cloud); 3172 | } 3173 | else 3174 | { 3175 | pcl::io::loadPLYFile(path, *cloud); 3176 | } 3177 | int previousSize = (int)cloud->size(); 3178 | if(downsampleStep > 1 && cloud->size()) 3179 | { 3180 | cloud = util3d::downsample(cloud, downsampleStep); 3181 | UDEBUG("Downsampling scan (step=%d): %d -> %d", downsampleStep, previousSize, (int)cloud->size()); 3182 | } 3183 | previousSize = (int)cloud->size(); 3184 | if(voxelSize > 0.0f && cloud->size()) 3185 | { 3186 | cloud = util3d::voxelize(cloud, voxelSize); 3187 | UDEBUG("Voxel filtering scan (voxel=%f m): %d -> %d", voxelSize, previousSize, (int)cloud->size()); 3188 | } 3189 | if(transform.isIdentity()) 3190 | { 3191 | return cloud; 3192 | } 3193 | return util3d::transformPointCloud(cloud, transform); 3194 | } 3195 | 3196 | } 3197 | 3198 | } 3199 | --------------------------------------------------------------------------------