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) {