Skip to content

Commit

Permalink
Fixed bugs for distortion model.
Browse files Browse the repository at this point in the history
  • Loading branch information
@mktk1117
mktk1117 committed Jun 11, 2024
1 parent 99dd406 commit e0f7c73
Show file tree
Hide file tree
Showing 5 changed files with 70 additions and 29 deletions.
2 changes: 1 addition & 1 deletion elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_wrapper.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,7 @@ class ElevationMappingWrapper {
void input(const RowMatrixXd& points, const std::vector<std::string>& channels, const RowMatrixXd& R, const Eigen::VectorXd& t,
const double positionNoise, const double orientationNoise);
void input_image(const std::vector<ColMatrixXf>& multichannel_image, const std::vector<std::string>& 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();
Expand Down
60 changes: 48 additions & 12 deletions elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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

Expand Down Expand Up @@ -484,6 +491,7 @@ def input_image(
Returns:
None:
"""

image = np.stack(image, axis=0)
if len(image.shape) == 2:
image = image[None]
Expand All @@ -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
Expand All @@ -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(
Expand All @@ -516,6 +530,7 @@ def input_image(
y1,
z1,
P.reshape(-1),
K.reshape(-1),
D.reshape(-1),
image_height,
image_width,
Expand All @@ -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):
Expand All @@ -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):
Expand Down Expand Up @@ -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]
Expand All @@ -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)
Expand All @@ -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)
Expand Down Expand Up @@ -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:
Expand Down Expand Up @@ -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:
Expand Down Expand Up @@ -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"]
Expand Down
28 changes: 16 additions & 12 deletions elevation_mapping_cupy/script/elevation_mapping_cupy/kernels/custom_image_kernels.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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(
"""
Expand Down Expand Up @@ -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);
Expand All @@ -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;
Expand Down
5 changes: 3 additions & 2 deletions elevation_mapping_cupy/src/elevation_mapping_ros.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -392,8 +392,9 @@ void ElevationMappingNode::inputImage(const sensor_msgs::ImageConstPtr& image_ms
if (!camera_info_msg->D.empty()) {
distortionCoeffs = Eigen::Map<const Eigen::VectorXd>(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
Expand Down
4 changes: 2 additions & 2 deletions elevation_mapping_cupy/src/elevation_mapping_wrapper.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -178,10 +178,10 @@ void ElevationMappingWrapper::input(const RowMatrixXd& points, const std::vector
}

void ElevationMappingWrapper::input_image(const std::vector<ColMatrixXf>& multichannel_image, const std::vector<std::string>& 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<const RowMatrixXd>(R), Eigen::Ref<const Eigen::VectorXd>(t),
Eigen::Ref<const RowMatrixXd>(cameraMatrix), Eigen::Ref<const RowMatrixXd>(D), height, width);
Eigen::Ref<const RowMatrixXd>(cameraMatrix), Eigen::Ref<const Eigen::VectorXd>(D), height, width);
}

void ElevationMappingWrapper::move_to(const Eigen::VectorXd& p, const RowMatrixXd& R) {
Expand Down

0 comments on commit e0f7c73

Please sign in to comment.