Passing distortion model information

leggedrobotics · Jun 24, 2024 · eb1c76c · eb1c76c
1 parent 3ebf196
commit eb1c76c
Show file tree

Hide file tree

Showing 4 changed files with 20 additions and 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
@@ -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 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
@@ -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
@@ -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
@@ -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 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<const RowMatrixXd>(R), Eigen::Ref<const Eigen::VectorXd>(t),
- Eigen::Ref<const RowMatrixXd>(cameraMatrix), Eigen::Ref<const Eigen::VectorXd>(D), height, width);
+ Eigen::Ref<const RowMatrixXd>(cameraMatrix), Eigen::Ref<const Eigen::VectorXd>(D), distortion_model, height, width);
 }
 
 void ElevationMappingWrapper::move_to(const Eigen::VectorXd& p, const RowMatrixXd& R) {