Load D and check if D is not empty

leggedrobotics · Jun 10, 2024 · 99dd406 · 99dd406
1 parent bf8fc22
commit 99dd406
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 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
@@ -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
@@ -388,8 +388,14 @@ void ElevationMappingNode::inputImage(const sensor_msgs::ImageConstPtr& image_ms
   Eigen::Map<const Eigen::Matrix<double, 3, 3, Eigen::RowMajor>> cameraMatrix(&camera_info_msg->K[0]);
 
   // Extract distortion coefficients
-  Eigen::Map<const Eigen::VectorXd> distortionCoeffs(camera_info_msg->D.data(), camera_info_msg->D.size());
-
+  Eigen::VectorXd distortionCoeffs;
+  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;
+  }
+
   // Get pose of sensor in map frame
   tf::StampedTransform transformTf;
   std::string sensorFrameId = image_msg->header.frame_id;