From eb1c76c03dfa2733441fc5445f141e03a8baac6b Mon Sep 17 00:00:00 2001 From: Takahiro Date: Mon, 24 Jun 2024 11:19:15 +0200 Subject: [PATCH] Passing distortion model information --- .../elevation_mapping_wrapper.hpp | 2 +- .../elevation_mapping_cupy/elevation_mapping.py | 13 +++++++++++++ .../src/elevation_mapping_ros.cpp | 5 ++++- .../src/elevation_mapping_wrapper.cpp | 4 ++-- 4 files changed, 20 insertions(+), 4 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 2b026c17..712ed10a 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 Eigen::VectorXd& D, int height, int width); + const Eigen::VectorXd& t, const RowMatrixXd& cameraMatrix, const Eigen::VectorXd& D, const std::string distortion_model, 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 bba41ec6..43cbe884 100644 --- a/elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping.py +++ b/elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping.py @@ -474,6 +474,7 @@ def input_image( t: cp._core.core.ndarray, K: cp._core.core.ndarray, D: cp._core.core.ndarray, + distortion_model: str, image_height: int, image_width: int, ): @@ -512,6 +513,18 @@ def input_image( else: D = D[:5] + if distortion_model == "radtan": + pass + elif distortion_model == "equidistant": + # Not implemented yet. + D *= 0 + elif distortion_model == "plumb_bob": + # Not implemented yet. + D *= 0 + else: + # Not implemented yet. + D *= 0 + # Calculate transformation matrix P = cp.asarray(K @ cp.concatenate([R, t[:, None]], 1), dtype=np.float32) t_cam_map = -R.T @ t - self.center diff --git a/elevation_mapping_cupy/src/elevation_mapping_ros.cpp b/elevation_mapping_cupy/src/elevation_mapping_ros.cpp index 072a117c..2fbc3128 100644 --- a/elevation_mapping_cupy/src/elevation_mapping_ros.cpp +++ b/elevation_mapping_cupy/src/elevation_mapping_ros.cpp @@ -397,6 +397,9 @@ void ElevationMappingNode::inputImage(const sensor_msgs::ImageConstPtr& image_ms distortionCoeffs = Eigen::VectorXd::Zero(5); // return; } + + // distortion model + std::string distortion_model = camera_info_msg->distortion_model; // Get pose of sensor in map frame tf::StampedTransform transformTf; @@ -439,7 +442,7 @@ void ElevationMappingNode::inputImage(const sensor_msgs::ImageConstPtr& image_ms // Pass image to pipeline map_.input_image(multichannel_image, channels, transformationMapToSensor.rotation(), transformationMapToSensor.translation(), cameraMatrix, - distortionCoeffs, image.rows, image.cols); + distortionCoeffs, distortion_model, 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 07f904c8..3980a8be 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 Eigen::VectorXd& D, int height, int width) { + const Eigen::VectorXd& t, const RowMatrixXd& cameraMatrix, const Eigen::VectorXd& D, const std::string distortion_model, 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), distortion_model, height, width); } void ElevationMappingWrapper::move_to(const Eigen::VectorXd& p, const RowMatrixXd& R) {