From e6989e0986369443bc831967e81947788f293494 Mon Sep 17 00:00:00 2001 From: RobinSchmid7 Date: Mon, 8 Jan 2024 15:46:02 +0100 Subject: [PATCH 01/10] Added anomaly configs --- elevation_mapping_cupy/config/core/core_param.yaml | 4 ++-- .../config/setups/anymal/anymal_sensor_parameter.yaml | 10 ++++++++-- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/elevation_mapping_cupy/config/core/core_param.yaml b/elevation_mapping_cupy/config/core/core_param.yaml index 17baea09..f3cfa386 100644 --- a/elevation_mapping_cupy/config/core/core_param.yaml +++ b/elevation_mapping_cupy/config/core/core_param.yaml @@ -41,7 +41,7 @@ overlap_clear_range_xy: 4.0 # xy range [m] for clearing over overlap_clear_range_z: 2.0 # z range [m] for clearing overlapped area. cells outside this range will be cleared. (used for multi floor setting) map_frame: 'odom' # The map frame where the odometry source uses. -base_frame: 'base_footprint' # The robot's base frame. This frame will be a center of the map. +base_frame: 'footprint' # The robot's base frame. This frame will be a center of the map. corrected_map_frame: 'odom' #### Feature toggles ######## @@ -62,7 +62,7 @@ use_only_above_for_upper_bound: false #### Initializer ######## initialize_method: 'linear' # Choose one from 'nearest', 'linear', 'cubic' -initialize_frame_id: ['base_footprint'] # One tf (like ['footprint'] ) initializes a square around it. +initialize_frame_id: ['footprint'] # One tf (like ['footprint'] ) initializes a square around it. initialize_tf_offset: [0.0, 0.0, 0.0, 0.0] # z direction. Should be same number as initialize_frame_id. dilation_size_initialize: 2 # dilation size after the init. initialize_tf_grid_size: 0.5 # This is not used if number of tf is more than 3. diff --git a/elevation_mapping_cupy/config/setups/anymal/anymal_sensor_parameter.yaml b/elevation_mapping_cupy/config/setups/anymal/anymal_sensor_parameter.yaml index a130607f..26e3f7c3 100644 --- a/elevation_mapping_cupy/config/setups/anymal/anymal_sensor_parameter.yaml +++ b/elevation_mapping_cupy/config/setups/anymal/anymal_sensor_parameter.yaml @@ -18,7 +18,7 @@ image_channel_fusions: publishers: elevation_map_raw: - layers: ['elevation', 'traversability', 'variance', 'rgb'] + layers: ['elevation', 'traversability', 'variance', 'rgb', 'anomaly'] basic_layers: ['elevation', 'traversability'] fps: 5.0 @@ -73,4 +73,10 @@ subscribers: rear_bpearl: topic_name: /robot_self_filter/bpearl_rear/point_cloud - data_type: pointcloud \ No newline at end of file + data_type: pointcloud + + anomaly: + topic_name: "/wild_visual_navigation_node/front/traversability" + camera_info_topic_name: "/wild_visual_navigation_node/front/camera_info" + channel_info_topic_name: "/wild_visual_navigation_node/front/channel_info" + data_type: image \ No newline at end of file From 23a1f8eef05aa7c0798437a2ff3e5112ed504a94 Mon Sep 17 00:00:00 2001 From: RobinSchmid7 Date: Mon, 8 Jan 2024 18:24:11 +0100 Subject: [PATCH 02/10] reverted base footprint layer --- elevation_mapping_cupy/config/core/core_param.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/elevation_mapping_cupy/config/core/core_param.yaml b/elevation_mapping_cupy/config/core/core_param.yaml index f3cfa386..17baea09 100644 --- a/elevation_mapping_cupy/config/core/core_param.yaml +++ b/elevation_mapping_cupy/config/core/core_param.yaml @@ -41,7 +41,7 @@ overlap_clear_range_xy: 4.0 # xy range [m] for clearing over overlap_clear_range_z: 2.0 # z range [m] for clearing overlapped area. cells outside this range will be cleared. (used for multi floor setting) map_frame: 'odom' # The map frame where the odometry source uses. -base_frame: 'footprint' # The robot's base frame. This frame will be a center of the map. +base_frame: 'base_footprint' # The robot's base frame. This frame will be a center of the map. corrected_map_frame: 'odom' #### Feature toggles ######## @@ -62,7 +62,7 @@ use_only_above_for_upper_bound: false #### Initializer ######## initialize_method: 'linear' # Choose one from 'nearest', 'linear', 'cubic' -initialize_frame_id: ['footprint'] # One tf (like ['footprint'] ) initializes a square around it. +initialize_frame_id: ['base_footprint'] # One tf (like ['footprint'] ) initializes a square around it. initialize_tf_offset: [0.0, 0.0, 0.0, 0.0] # z direction. Should be same number as initialize_frame_id. dilation_size_initialize: 2 # dilation size after the init. initialize_tf_grid_size: 0.5 # This is not used if number of tf is more than 3. From 6cf88aca82c6551e2083c81d7b41a997d501790f Mon Sep 17 00:00:00 2001 From: RobinSchmid7 Date: Mon, 12 Feb 2024 13:40:58 +0100 Subject: [PATCH 03/10] changes on jetson --- .../setups/anymal/anymal_sensor_parameter.yaml | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/elevation_mapping_cupy/config/setups/anymal/anymal_sensor_parameter.yaml b/elevation_mapping_cupy/config/setups/anymal/anymal_sensor_parameter.yaml index 26e3f7c3..a14b08d0 100644 --- a/elevation_mapping_cupy/config/setups/anymal/anymal_sensor_parameter.yaml +++ b/elevation_mapping_cupy/config/setups/anymal/anymal_sensor_parameter.yaml @@ -18,10 +18,15 @@ image_channel_fusions: publishers: elevation_map_raw: - layers: ['elevation', 'traversability', 'variance', 'rgb', 'anomaly'] + layers: ['elevation', 'traversability', 'variance', 'rgb', 'anomaly', 'friction', 'stiffness', 'risk'] basic_layers: ['elevation', 'traversability'] fps: 5.0 + elevation_map_recordable: + layers: ['elevation', 'traversability', 'variance', 'rgb', 'anomaly', 'friction', 'stiffness', 'risk'] + basic_layers: ['elevation', 'traversability'] + fps: 2.0 + filtered_elevation_map: layers: ['inpaint', 'smooth', 'min_filter'] basic_layers: ['inpaint'] @@ -75,8 +80,14 @@ subscribers: topic_name: /robot_self_filter/bpearl_rear/point_cloud data_type: pointcloud + traversability: + topic_name: "/hdr_camera/semantic_image" + camera_info_topic_name: "/hdr_camera/semantic_camera_info" + channel_info_topic_name: "/hdr_camera/image_channel_info" + data_type: image + anomaly: topic_name: "/wild_visual_navigation_node/front/traversability" camera_info_topic_name: "/wild_visual_navigation_node/front/camera_info" channel_info_topic_name: "/wild_visual_navigation_node/front/channel_info" - data_type: image \ No newline at end of file + data_type: image From f8f8fe30ea39b4f4f78d49a5e1d11f87ac47f553 Mon Sep 17 00:00:00 2001 From: RobinSchmid7 Date: Thu, 14 Mar 2024 16:20:02 +0100 Subject: [PATCH 04/10] Font cleanup --- .../config/setups/anymal/anymal_parameters.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/elevation_mapping_cupy/config/setups/anymal/anymal_parameters.yaml b/elevation_mapping_cupy/config/setups/anymal/anymal_parameters.yaml index 5d1ad988..4a2ed7f4 100644 --- a/elevation_mapping_cupy/config/setups/anymal/anymal_parameters.yaml +++ b/elevation_mapping_cupy/config/setups/anymal/anymal_parameters.yaml @@ -1,6 +1,6 @@ #### Basic parameters ######## -resolution: 0.04 # resolution in m. -map_length: 8 # map's size in m. +resolution: 0.04 # resolution in m. +map_length: 8 # map's size in m. sensor_noise_factor: 0.05 # point's noise is sensor_noise_factor*z^2 (z is distance from sensor). mahalanobis_thresh: 2.0 # points outside this distance is outlier. outlier_variance: 0.01 # if point is outlier, add this value to the cell. @@ -9,7 +9,7 @@ max_drift: 0.1 # drift compensation happens onl drift_compensation_alpha: 0.1 # drift compensation alpha for smoother update of drift compensation time_variance: 0.0001 # add this value when update_variance is called. max_variance: 100.0 # maximum variance for each cell. -initial_variance: 1000.0 # initial variance for each cell. +initial_variance: 1000.0 # initial variance for each cell. traversability_inlier: 0.9 # cells with higher traversability are used for drift compensation. dilation_size: 3 # dilation filter size before traversability filter. wall_num_thresh: 20 # if there are more points than this value, only higher points than the current height are used to make the wall more sharp. From 0ecb24a1862996dab64e23738ddea64b9515fec9 Mon Sep 17 00:00:00 2001 From: RobinSchmid7 Date: Mon, 18 Mar 2024 10:51:32 +0100 Subject: [PATCH 05/10] Adding the upper bound layer to the default elevation map layers --- .../config/setups/anymal/anymal_sensor_parameter.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/elevation_mapping_cupy/config/setups/anymal/anymal_sensor_parameter.yaml b/elevation_mapping_cupy/config/setups/anymal/anymal_sensor_parameter.yaml index a14b08d0..a5a96577 100644 --- a/elevation_mapping_cupy/config/setups/anymal/anymal_sensor_parameter.yaml +++ b/elevation_mapping_cupy/config/setups/anymal/anymal_sensor_parameter.yaml @@ -18,7 +18,7 @@ image_channel_fusions: publishers: elevation_map_raw: - layers: ['elevation', 'traversability', 'variance', 'rgb', 'anomaly', 'friction', 'stiffness', 'risk'] + layers: ['elevation', 'traversability', 'variance', 'rgb', 'anomaly', 'friction', 'stiffness', 'risk', 'upper_bound'] basic_layers: ['elevation', 'traversability'] fps: 5.0 @@ -28,7 +28,7 @@ publishers: fps: 2.0 filtered_elevation_map: - layers: ['inpaint', 'smooth', 'min_filter'] + layers: ['inpaint', 'smooth', 'min_filter', 'upper_bound'] basic_layers: ['inpaint'] fps: 5.0 From a23191f36e08df2852de41b5e5f2a8d21bd2dfcf Mon Sep 17 00:00:00 2001 From: Jonas Frey Date: Thu, 18 Apr 2024 12:37:39 +0200 Subject: [PATCH 06/10] fix bug --- .../elevation_mapping_cupy/kernels/custom_image_kernels.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/elevation_mapping_cupy/script/elevation_mapping_cupy/kernels/custom_image_kernels.py b/elevation_mapping_cupy/script/elevation_mapping_cupy/kernels/custom_image_kernels.py index ce03ee8f..a29668a2 100644 --- a/elevation_mapping_cupy/script/elevation_mapping_cupy/kernels/custom_image_kernels.py +++ b/elevation_mapping_cupy/script/elevation_mapping_cupy/kernels/custom_image_kernels.py @@ -22,7 +22,7 @@ def image_to_map_correspondence_kernel(resolution, width, height, tolerance_z_co return layer * layer_n + idx; } __device__ bool is_inside_map(int x, int y) { - return (x >= 0 && y >= 0 && x<${width} && x<${height}); + return (x >= 0 && y >= 0 && x<${width} && y<${height}); } __device__ float get_l2_distance(int x0, int y0, int x1, int y1) { float dx = x0-x1; From c4af484ff6368e496c144224aa18fac43fc45e5f Mon Sep 17 00:00:00 2001 From: RobinSchmid7 Date: Sun, 9 Jun 2024 22:18:06 +0200 Subject: [PATCH 07/10] Start adding distortion, not tested yet --- .../elevation_mapping_wrapper.hpp | 2 +- .../elevation_mapping_cupy/elevation_mapping.py | 3 +++ .../elevation_mapping_ros.py | 2 +- .../kernels/custom_image_kernels.py | 17 ++++++++++++++++- .../src/elevation_mapping_ros.cpp | 7 +++++-- .../src/elevation_mapping_wrapper.cpp | 4 ++-- 6 files changed, 28 insertions(+), 7 deletions(-) diff --git a/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_wrapper.hpp b/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_wrapper.hpp index 116403e6..fb74574f 100644 --- a/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_wrapper.hpp +++ b/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_wrapper.hpp @@ -49,7 +49,7 @@ class ElevationMappingWrapper { void input(const RowMatrixXd& points, const std::vector& channels, const RowMatrixXd& R, const Eigen::VectorXd& t, const double positionNoise, const double orientationNoise); void input_image(const std::vector& multichannel_image, const std::vector& channels, const RowMatrixXd& R, - const Eigen::VectorXd& t, const RowMatrixXd& cameraMatrix, int height, int width); + const Eigen::VectorXd& t, const RowMatrixXd& cameraMatrix, const RowMatrixXd& D, int height, int width); void move_to(const Eigen::VectorXd& p, const RowMatrixXd& R); void clear(); void update_variance(); diff --git a/elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping.py b/elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping.py index 0ff32d9a..11136fe9 100644 --- a/elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping.py +++ b/elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping.py @@ -466,6 +466,7 @@ def input_image( R: cp._core.core.ndarray, t: cp._core.core.ndarray, K: cp._core.core.ndarray, + D: cp._core.core.ndarray, image_height: int, image_width: int, ): @@ -492,6 +493,7 @@ def input_image( K = cp.asarray(K, dtype=self.data_type) R = cp.asarray(R, dtype=self.data_type) t = cp.asarray(t, dtype=self.data_type) + D = cp.asarray(D, dtype=self.data_type) image_height = cp.float32(image_height) image_width = cp.float32(image_width) @@ -514,6 +516,7 @@ def input_image( y1, z1, P.reshape(-1), + D.reshape(-1), image_height, image_width, self.center, diff --git a/elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping_ros.py b/elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping_ros.py index 3756ebd7..6e69861c 100644 --- a/elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping_ros.py +++ b/elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping_ros.py @@ -177,7 +177,7 @@ def image_callback(self, camera_msg, camera_info_msg, sub_key): assert np.all(np.array(camera_info_msg.D) == 0.0), "Undistortion not implemented" K = np.array(camera_info_msg.K, dtype=np.float32).reshape(3, 3) # process pointcloud - self._map.input_image(sub_key, semantic_img, R, t, K, camera_info_msg.height, camera_info_msg.width) + self._map.input_image(sub_key, semantic_img, R, t, K, D, camera_info_msg.height, camera_info_msg.width) self._image_process_counter += 1 def pointcloud_callback(self, msg, sub_key): diff --git a/elevation_mapping_cupy/script/elevation_mapping_cupy/kernels/custom_image_kernels.py b/elevation_mapping_cupy/script/elevation_mapping_cupy/kernels/custom_image_kernels.py index a29668a2..4af01577 100644 --- a/elevation_mapping_cupy/script/elevation_mapping_cupy/kernels/custom_image_kernels.py +++ b/elevation_mapping_cupy/script/elevation_mapping_cupy/kernels/custom_image_kernels.py @@ -13,7 +13,7 @@ def image_to_map_correspondence_kernel(resolution, width, height, tolerance_z_co The function returns a kernel that can be used to perform the correspondence calculation. """ _image_to_map_correspondence_kernel = cp.ElementwiseKernel( - in_params="raw U map, raw U x1, raw U y1, raw U z1, raw U P, raw U image_height, raw U image_width, raw U center", + in_params="raw U map, raw U x1, raw U y1, raw U z1, raw U P, raw U D, raw U image_height, raw U image_width, raw U center", out_params="raw U uv_correspondence, raw B valid_correspondence", preamble=string.Template( """ @@ -65,6 +65,21 @@ def image_to_map_correspondence_kernel(resolution, width, height, tolerance_z_co if ((u < 0) || (v < 0) || (u >= image_width) || (v >= image_height)){ return; } + + // Apply undistortion using distortion matrix D + float k1 = D[0]; + float k2 = D[1]; + float p1 = D[2]; + float p2 = D[3]; + float k3 = D[4]; + float x = (u - image_width / 2.0) / (image_width / 2.0); + float y = (v - image_height / 2.0) / (image_height / 2.0); + float r2 = x * x + y * y; + float radial_distortion = 1 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2; + float x_corrected = x * radial_distortion + 2 * p1 * x * y + p2 * (r2 + 2 * x * x); + float y_corrected = y * radial_distortion + 2 * p2 * x * y + p1 * (r2 + 2 * y * y); + u = (x_corrected * (image_width / 2.0)) + (image_width / 2.0); + v = (y_corrected * (image_height / 2.0)) + (image_height / 2.0); int y0_c = y0; int x0_c = x0; diff --git a/elevation_mapping_cupy/src/elevation_mapping_ros.cpp b/elevation_mapping_cupy/src/elevation_mapping_ros.cpp index 4a72e8c6..50c2dd96 100644 --- a/elevation_mapping_cupy/src/elevation_mapping_ros.cpp +++ b/elevation_mapping_cupy/src/elevation_mapping_ros.cpp @@ -387,6 +387,9 @@ void ElevationMappingNode::inputImage(const sensor_msgs::ImageConstPtr& image_ms // Extract camera matrix Eigen::Map> cameraMatrix(&camera_info_msg->K[0]); + // Extract distortion coefficients + Eigen::Map distortionCoeffs(camera_info_msg->D.data(), camera_info_msg->D.size()); + // Get pose of sensor in map frame tf::StampedTransform transformTf; std::string sensorFrameId = image_msg->header.frame_id; @@ -427,8 +430,8 @@ void ElevationMappingNode::inputImage(const sensor_msgs::ImageConstPtr& image_ms } // Pass image to pipeline - map_.input_image(multichannel_image, channels, transformationMapToSensor.rotation(), transformationMapToSensor.translation(), cameraMatrix, - image.rows, image.cols); + map_.input_image(multichannel_image, channels, transformationMapToSensor.rotation(), transformationMapToSensor.translation(), cameraMatrix, + distortionCoeffs, image.rows, image.cols); } void ElevationMappingNode::imageCallback(const sensor_msgs::ImageConstPtr& image_msg, diff --git a/elevation_mapping_cupy/src/elevation_mapping_wrapper.cpp b/elevation_mapping_cupy/src/elevation_mapping_wrapper.cpp index f66f9a79..b23207d6 100644 --- a/elevation_mapping_cupy/src/elevation_mapping_wrapper.cpp +++ b/elevation_mapping_cupy/src/elevation_mapping_wrapper.cpp @@ -178,10 +178,10 @@ void ElevationMappingWrapper::input(const RowMatrixXd& points, const std::vector } void ElevationMappingWrapper::input_image(const std::vector& multichannel_image, const std::vector& channels, const RowMatrixXd& R, - const Eigen::VectorXd& t, const RowMatrixXd& cameraMatrix, int height, int width) { + const Eigen::VectorXd& t, const RowMatrixXd& cameraMatrix, const RowMatrixXd& D, int height, int width) { py::gil_scoped_acquire acquire; map_.attr("input_image")(multichannel_image, channels, Eigen::Ref(R), Eigen::Ref(t), - Eigen::Ref(cameraMatrix), height, width); + Eigen::Ref(cameraMatrix), Eigen::Ref(D), height, width); } void ElevationMappingWrapper::move_to(const Eigen::VectorXd& p, const RowMatrixXd& R) { From bf8fc22d98df52f4d19a34785d4810895db049cb Mon Sep 17 00:00:00 2001 From: RobinSchmid7 Date: Mon, 10 Jun 2024 21:32:15 +0200 Subject: [PATCH 08/10] Only apply kernel if not all zeros --- .../kernels/custom_image_kernels.py | 35 +++++++++++-------- 1 file changed, 20 insertions(+), 15 deletions(-) diff --git a/elevation_mapping_cupy/script/elevation_mapping_cupy/kernels/custom_image_kernels.py b/elevation_mapping_cupy/script/elevation_mapping_cupy/kernels/custom_image_kernels.py index 4af01577..abce0226 100644 --- a/elevation_mapping_cupy/script/elevation_mapping_cupy/kernels/custom_image_kernels.py +++ b/elevation_mapping_cupy/script/elevation_mapping_cupy/kernels/custom_image_kernels.py @@ -64,22 +64,27 @@ def image_to_map_correspondence_kernel(resolution, width, height, tolerance_z_co // filter point next to image plane if ((u < 0) || (v < 0) || (u >= image_width) || (v >= image_height)){ return; - } + } + + // Check if D is all zeros + bool is_D_zero = (D[0] == 0 && D[1] == 0 && D[2] == 0 && D[3] == 0 && D[4] == 0); - // Apply undistortion using distortion matrix D - float k1 = D[0]; - float k2 = D[1]; - float p1 = D[2]; - float p2 = D[3]; - float k3 = D[4]; - float x = (u - image_width / 2.0) / (image_width / 2.0); - float y = (v - image_height / 2.0) / (image_height / 2.0); - float r2 = x * x + y * y; - float radial_distortion = 1 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2; - float x_corrected = x * radial_distortion + 2 * p1 * x * y + p2 * (r2 + 2 * x * x); - float y_corrected = y * radial_distortion + 2 * p2 * x * y + p1 * (r2 + 2 * y * y); - u = (x_corrected * (image_width / 2.0)) + (image_width / 2.0); - v = (y_corrected * (image_height / 2.0)) + (image_height / 2.0); + // Apply undistortion using distortion matrix D if not all zeros + if (!is_D_zero) { + float k1 = D[0]; + float k2 = D[1]; + float p1 = D[2]; + float p2 = D[3]; + float k3 = D[4]; + float x = (u - image_width / 2.0) / (image_width / 2.0); + float y = (v - image_height / 2.0) / (image_height / 2.0); + float r2 = x * x + y * y; + float radial_distortion = 1 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2; + float x_corrected = x * radial_distortion + 2 * p1 * x * y + p2 * (r2 + 2 * x * x); + float y_corrected = y * radial_distortion + 2 * p2 * x * y + p1 * (r2 + 2 * y * y); + u = (x_corrected * (image_width / 2.0)) + (image_width / 2.0); + v = (y_corrected * (image_height / 2.0)) + (image_height / 2.0); + } int y0_c = y0; int x0_c = x0; From 99dd4065e7aa826c52536cc9330c28acab863870 Mon Sep 17 00:00:00 2001 From: RobinSchmid7 Date: Mon, 10 Jun 2024 21:58:21 +0200 Subject: [PATCH 09/10] Load D and check if D is not empty --- .../elevation_mapping_cupy/elevation_mapping_ros.py | 5 ++++- elevation_mapping_cupy/src/elevation_mapping_ros.cpp | 10 ++++++++-- 2 files changed, 12 insertions(+), 3 deletions(-) diff --git a/elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping_ros.py b/elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping_ros.py index 6e69861c..c28823e9 100644 --- a/elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping_ros.py +++ b/elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping_ros.py @@ -174,8 +174,11 @@ def image_callback(self, camera_msg, camera_info_msg, sub_key): else: semantic_img = [semantic_img] - assert np.all(np.array(camera_info_msg.D) == 0.0), "Undistortion not implemented" K = np.array(camera_info_msg.K, dtype=np.float32).reshape(3, 3) + + assert np.all(np.array(camera_info_msg.D) == 0.0), "Undistortion not implemented" + D = np.array(camera_info_msg.D, dtype=np.float32).reshape(5, 1) + # process pointcloud self._map.input_image(sub_key, semantic_img, R, t, K, D, camera_info_msg.height, camera_info_msg.width) self._image_process_counter += 1 diff --git a/elevation_mapping_cupy/src/elevation_mapping_ros.cpp b/elevation_mapping_cupy/src/elevation_mapping_ros.cpp index 50c2dd96..c10e5185 100644 --- a/elevation_mapping_cupy/src/elevation_mapping_ros.cpp +++ b/elevation_mapping_cupy/src/elevation_mapping_ros.cpp @@ -388,8 +388,14 @@ void ElevationMappingNode::inputImage(const sensor_msgs::ImageConstPtr& image_ms Eigen::Map> cameraMatrix(&camera_info_msg->K[0]); // Extract distortion coefficients - Eigen::Map distortionCoeffs(camera_info_msg->D.data(), camera_info_msg->D.size()); - + Eigen::VectorXd distortionCoeffs; + if (!camera_info_msg->D.empty()) { + distortionCoeffs = Eigen::Map(camera_info_msg->D.data(), camera_info_msg->D.size()); + } else { + ROS_ERROR("Distortion coefficients are empty."); + return; + } + // Get pose of sensor in map frame tf::StampedTransform transformTf; std::string sensorFrameId = image_msg->header.frame_id; From e0f7c735107f79dd661eae5698aa9e164ebd9a8c Mon Sep 17 00:00:00 2001 From: Takahiro Date: Tue, 11 Jun 2024 23:01:42 +0200 Subject: [PATCH 10/10] Fixed bugs for distortion model. --- .../elevation_mapping_wrapper.hpp | 2 +- .../elevation_mapping.py | 60 +++++++++++++++---- .../kernels/custom_image_kernels.py | 28 +++++---- .../src/elevation_mapping_ros.cpp | 5 +- .../src/elevation_mapping_wrapper.cpp | 4 +- 5 files changed, 70 insertions(+), 29 deletions(-) diff --git a/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_wrapper.hpp b/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_wrapper.hpp index fb74574f..2b026c17 100644 --- a/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_wrapper.hpp +++ b/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_wrapper.hpp @@ -49,7 +49,7 @@ class ElevationMappingWrapper { void input(const RowMatrixXd& points, const std::vector& channels, const RowMatrixXd& R, const Eigen::VectorXd& t, const double positionNoise, const double orientationNoise); void input_image(const std::vector& multichannel_image, const std::vector& channels, const RowMatrixXd& R, - const Eigen::VectorXd& t, const RowMatrixXd& cameraMatrix, const RowMatrixXd& D, int height, int width); + const Eigen::VectorXd& t, const RowMatrixXd& cameraMatrix, const Eigen::VectorXd& D, int height, int width); void move_to(const Eigen::VectorXd& p, const RowMatrixXd& R); void clear(); void update_variance(); diff --git a/elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping.py b/elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping.py index 11136fe9..bba41ec6 100644 --- a/elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping.py +++ b/elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping.py @@ -228,7 +228,10 @@ def shift_map_z(self, delta_z): def compile_kernels(self): """Compile all kernels belonging to the elevation map.""" - self.new_map = cp.zeros((self.elevation_map.shape[0], self.cell_n, self.cell_n), dtype=self.data_type,) + self.new_map = cp.zeros( + (self.elevation_map.shape[0], self.cell_n, self.cell_n), + dtype=self.data_type, + ) self.traversability_input = cp.zeros((self.cell_n, self.cell_n), dtype=self.data_type) self.traversability_mask_dummy = cp.zeros((self.cell_n, self.cell_n), dtype=self.data_type) self.min_filtered = cp.zeros((self.cell_n, self.cell_n), dtype=self.data_type) @@ -287,14 +290,18 @@ def compile_image_kernels(self): np.zeros((self.cell_n, self.cell_n), dtype=np.bool_), dtype=np.bool_ ) self.uv_correspondence = cp.asarray( - np.zeros((2, self.cell_n, self.cell_n), dtype=np.float32), dtype=np.float32, + np.zeros((2, self.cell_n, self.cell_n), dtype=np.float32), + dtype=np.float32, ) # self.distance_correspondence = cp.asarray( # np.zeros((self.cell_n, self.cell_n), dtype=np.float32), dtype=np.float32 # ) # TODO tolerance_z_collision add parameter self.image_to_map_correspondence_kernel = image_to_map_correspondence_kernel( - resolution=self.resolution, width=self.cell_n, height=self.cell_n, tolerance_z_collision=0.10, + resolution=self.resolution, + width=self.cell_n, + height=self.cell_n, + tolerance_z_collision=0.10, ) break @@ -484,6 +491,7 @@ def input_image( Returns: None: """ + image = np.stack(image, axis=0) if len(image.shape) == 2: image = image[None] @@ -497,6 +505,13 @@ def input_image( image_height = cp.float32(image_height) image_width = cp.float32(image_width) + if len(D) < 4: + D = cp.zeros(5, dtype=self.data_type) + elif len(D) == 4: + D = cp.concatenate([D, cp.zeros(1, dtype=self.data_type)]) + else: + D = D[:5] + # Calculate transformation matrix P = cp.asarray(K @ cp.concatenate([R, t[:, None]], 1), dtype=np.float32) t_cam_map = -R.T @ t - self.center @@ -507,7 +522,6 @@ def input_image( self.uv_correspondence *= 0 self.valid_correspondence[:, :] = False - # self.distance_correspondence *= 0.0 with self.map_lock: self.image_to_map_correspondence_kernel( @@ -516,6 +530,7 @@ def input_image( y1, z1, P.reshape(-1), + K.reshape(-1), D.reshape(-1), image_height, image_width, @@ -525,7 +540,12 @@ def input_image( size=int(self.cell_n * self.cell_n), ) self.semantic_map.update_layers_image( - image, channels, self.uv_correspondence, self.valid_correspondence, image_height, image_width, + image, + channels, + self.uv_correspondence, + self.valid_correspondence, + image_height, + image_width, ) def update_normal(self, dilated_map): @@ -537,7 +557,10 @@ def update_normal(self, dilated_map): with self.map_lock: self.normal_map *= 0.0 self.normal_filter_kernel( - dilated_map, self.elevation_map[2], self.normal_map, size=(self.cell_n * self.cell_n), + dilated_map, + self.elevation_map[2], + self.normal_map, + size=(self.cell_n * self.cell_n), ) def process_map_for_publish(self, input_map, fill_nan=False, add_z=False, xp=cp): @@ -583,7 +606,9 @@ def get_traversability(self): traversability layer """ traversability = cp.where( - (self.elevation_map[2] + self.elevation_map[6]) > 0.5, self.elevation_map[3].copy(), cp.nan, + (self.elevation_map[2] + self.elevation_map[6]) > 0.5, + self.elevation_map[3].copy(), + cp.nan, ) self.traversability_buffer[3:-3, 3:-3] = traversability[3:-3, 3:-3] traversability = self.traversability_buffer[1:-1, 1:-1] @@ -605,7 +630,8 @@ def get_upper_bound(self): """ if self.param.use_only_above_for_upper_bound: valid = cp.logical_or( - cp.logical_and(self.elevation_map[5] > 0.0, self.elevation_map[6] > 0.5), self.elevation_map[2] > 0.5, + cp.logical_and(self.elevation_map[5] > 0.0, self.elevation_map[6] > 0.5), + self.elevation_map[2] > 0.5, ) else: valid = cp.logical_or(self.elevation_map[2] > 0.5, self.elevation_map[6] > 0.5) @@ -621,7 +647,8 @@ def get_is_upper_bound(self): """ if self.param.use_only_above_for_upper_bound: valid = cp.logical_or( - cp.logical_and(self.elevation_map[5] > 0.0, self.elevation_map[6] > 0.5), self.elevation_map[2] > 0.5, + cp.logical_and(self.elevation_map[5] > 0.0, self.elevation_map[6] > 0.5), + self.elevation_map[2] > 0.5, ) else: valid = cp.logical_or(self.elevation_map[2] > 0.5, self.elevation_map[6] > 0.5) @@ -782,7 +809,11 @@ def get_layer(self, name): return_map = self.semantic_map.semantic_map[idx] elif name in self.plugin_manager.layer_names: self.plugin_manager.update_with_name( - name, self.elevation_map, self.layer_names, self.semantic_map, self.base_rotation, + name, + self.elevation_map, + self.layer_names, + self.semantic_map, + self.base_rotation, ) return_map = self.plugin_manager.get_map_with_name(name) else: @@ -828,7 +859,10 @@ def get_polygon_traversability(self, polygon, result): else: t = cp.asarray(0.0, dtype=self.data_type) is_safe, un_polygon = is_traversable( - masked, self.param.safe_thresh, self.param.safe_min_thresh, self.param.max_unsafe_n, + masked, + self.param.safe_thresh, + self.param.safe_min_thresh, + self.param.max_unsafe_n, ) untraversable_polygon_num = 0 if un_polygon is not None: @@ -884,7 +918,9 @@ def initialize_map(self, points, method="cubic"): t = xp.random.rand(3) print(R, t) param = Parameter( - use_chainer=False, weight_file="../config/weights.dat", plugin_config_file="../config/plugin_config.yaml", + use_chainer=False, + weight_file="../config/weights.dat", + plugin_config_file="../config/plugin_config.yaml", ) param.additional_layers = ["rgb", "grass", "tree", "people"] param.fusion_algorithms = ["color", "class_bayesian", "class_bayesian", "class_bayesian"] diff --git a/elevation_mapping_cupy/script/elevation_mapping_cupy/kernels/custom_image_kernels.py b/elevation_mapping_cupy/script/elevation_mapping_cupy/kernels/custom_image_kernels.py index abce0226..a020ba2e 100644 --- a/elevation_mapping_cupy/script/elevation_mapping_cupy/kernels/custom_image_kernels.py +++ b/elevation_mapping_cupy/script/elevation_mapping_cupy/kernels/custom_image_kernels.py @@ -13,7 +13,7 @@ def image_to_map_correspondence_kernel(resolution, width, height, tolerance_z_co The function returns a kernel that can be used to perform the correspondence calculation. """ _image_to_map_correspondence_kernel = cp.ElementwiseKernel( - in_params="raw U map, raw U x1, raw U y1, raw U z1, raw U P, raw U D, raw U image_height, raw U image_width, raw U center", + in_params="raw U map, raw U x1, raw U y1, raw U z1, raw U P, raw U K, raw U D, raw U image_height, raw U image_width, raw U center", out_params="raw U uv_correspondence, raw B valid_correspondence", preamble=string.Template( """ @@ -60,11 +60,6 @@ def image_to_map_correspondence_kernel(resolution, width, height, tolerance_z_co } u = u/d; v = v/d; - - // filter point next to image plane - if ((u < 0) || (v < 0) || (u >= image_width) || (v >= image_height)){ - return; - } // Check if D is all zeros bool is_D_zero = (D[0] == 0 && D[1] == 0 && D[2] == 0 && D[3] == 0 && D[4] == 0); @@ -76,14 +71,23 @@ def image_to_map_correspondence_kernel(resolution, width, height, tolerance_z_co float p1 = D[2]; float p2 = D[3]; float k3 = D[4]; - float x = (u - image_width / 2.0) / (image_width / 2.0); - float y = (v - image_height / 2.0) / (image_height / 2.0); + float fx = K[0]; + float fy = K[4]; + float cx = K[2]; + float cy = K[5]; + float x = (u - cx) / fx; + float y = (v - cy) / fy; float r2 = x * x + y * y; float radial_distortion = 1 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2; - float x_corrected = x * radial_distortion + 2 * p1 * x * y + p2 * (r2 + 2 * x * x); - float y_corrected = y * radial_distortion + 2 * p2 * x * y + p1 * (r2 + 2 * y * y); - u = (x_corrected * (image_width / 2.0)) + (image_width / 2.0); - v = (y_corrected * (image_height / 2.0)) + (image_height / 2.0); + float u_corrected = x * radial_distortion + 2 * p1 * x * y + p2 * (r2 + 2 * x * x); + float v_corrected = y * radial_distortion + 2 * p2 * x * y + p1 * (r2 + 2 * y * y); + u = fx * u_corrected + cx; + v = fy * v_corrected + cy; + } + + // filter point next to image plane + if ((u < 0) || (v < 0) || (u >= image_width) || (v >= image_height)){ + return; } int y0_c = y0; diff --git a/elevation_mapping_cupy/src/elevation_mapping_ros.cpp b/elevation_mapping_cupy/src/elevation_mapping_ros.cpp index c10e5185..290246cc 100644 --- a/elevation_mapping_cupy/src/elevation_mapping_ros.cpp +++ b/elevation_mapping_cupy/src/elevation_mapping_ros.cpp @@ -392,8 +392,9 @@ void ElevationMappingNode::inputImage(const sensor_msgs::ImageConstPtr& image_ms if (!camera_info_msg->D.empty()) { distortionCoeffs = Eigen::Map(camera_info_msg->D.data(), camera_info_msg->D.size()); } else { - ROS_ERROR("Distortion coefficients are empty."); - return; + ROS_WARN("Distortion coefficients are empty."); + distortionCoeffs = Eigen::VectorXd::Zero(5); + // return; } // Get pose of sensor in map frame diff --git a/elevation_mapping_cupy/src/elevation_mapping_wrapper.cpp b/elevation_mapping_cupy/src/elevation_mapping_wrapper.cpp index b23207d6..07f904c8 100644 --- a/elevation_mapping_cupy/src/elevation_mapping_wrapper.cpp +++ b/elevation_mapping_cupy/src/elevation_mapping_wrapper.cpp @@ -178,10 +178,10 @@ void ElevationMappingWrapper::input(const RowMatrixXd& points, const std::vector } void ElevationMappingWrapper::input_image(const std::vector& multichannel_image, const std::vector& channels, const RowMatrixXd& R, - const Eigen::VectorXd& t, const RowMatrixXd& cameraMatrix, const RowMatrixXd& D, int height, int width) { + const Eigen::VectorXd& t, const RowMatrixXd& cameraMatrix, const Eigen::VectorXd& D, int height, int width) { py::gil_scoped_acquire acquire; map_.attr("input_image")(multichannel_image, channels, Eigen::Ref(R), Eigen::Ref(t), - Eigen::Ref(cameraMatrix), Eigen::Ref(D), height, width); + Eigen::Ref(cameraMatrix), Eigen::Ref(D), height, width); } void ElevationMappingWrapper::move_to(const Eigen::VectorXd& p, const RowMatrixXd& R) {