fix: publish matching camera distortion models

orbbec_camera/CMakeLists.txt

@@ -391,7 +391,11 @@ install(TARGETS list_devices_node list_depth_work_mode_node list_camera_profile_
 
 if(BUILD_TESTING)
     find_package(ament_lint_auto REQUIRED)
+    find_package(ament_cmake_gtest REQUIRED)
     ament_lint_auto_find_test_dependencies()
+
+    ament_add_gtest(camera_info_distortion_test test/camera_info_distortion_test.cpp)
+    target_link_libraries(camera_info_distortion_test ${PROJECT_NAME})
 endif()
 
 ament_export_include_directories(include)

orbbec_camera/package.xml

@@ -55,6 +55,8 @@
   <exec_depend>python3-yaml</exec_depend>
   <exec_depend>rclpy</exec_depend>
 
+  <test_depend>ament_cmake_gtest</test_depend>
+
   <export>
     <build_type>ament_cmake</build_type>
   </export>

orbbec_camera/src/utils.cpp

@@ -95,15 +95,14 @@ sensor_msgs::msg::CameraInfo convertToCameraInfo(OBCameraIntrinsic intrinsic,
   info.distortion_model = getDistortionModels(distortion);
   info.width = intrinsic.width;
   info.height = intrinsic.height;
-  info.d.resize(8, 0.0);
+  if (info.distortion_model == sensor_msgs::distortion_models::RATIONAL_POLYNOMIAL) {
-  info.d[0] = distortion.k1;
+    info.d = {distortion.k1, distortion.k2, distortion.p1, distortion.p2,
-  info.d[1] = distortion.k2;
+              distortion.k3, distortion.k4, distortion.k5, distortion.k6};
-  info.d[2] = distortion.p1;
+  } else if (info.distortion_model == sensor_msgs::distortion_models::EQUIDISTANT) {
-  info.d[3] = distortion.p2;
+    info.d = {distortion.k1, distortion.k2, distortion.k3, distortion.k4};
-  info.d[4] = distortion.k3;
+  } else {
-  info.d[5] = distortion.k4;
+    info.d = {distortion.k1, distortion.k2, distortion.p1, distortion.p2, distortion.k3};
-  info.d[6] = distortion.k5;
+  }
-  info.d[7] = distortion.k6;
   bool all_zero = std::all_of(info.d.begin(), info.d.end(), [](double val) { return val == 0.0; });
   info.roi.do_rectify = all_zero;
 
@@ -1174,7 +1173,7 @@ std::string getDistortionModels(OBCameraDistortion distortion) {
     case OB_DISTORTION_BROWN_CONRADY:
       return sensor_msgs::distortion_models::PLUMB_BOB;
     case OB_DISTORTION_BROWN_CONRADY_K6:
-      return sensor_msgs::distortion_models::PLUMB_BOB;
+      return sensor_msgs::distortion_models::RATIONAL_POLYNOMIAL;
     case OB_DISTORTION_KANNALA_BRANDT4:
       return sensor_msgs::distortion_models::EQUIDISTANT;
     default:

orbbec_camera/test/camera_info_distortion_test.cpp

@@ -0,0 +1,73 @@
+#include <gtest/gtest.h>
+
+#include <vector>
+
+#include "orbbec_camera/utils.h"
+
+namespace orbbec_camera {
+namespace {
+
+OBCameraIntrinsic makeIntrinsic() {
+  OBCameraIntrinsic intrinsic{};
+  intrinsic.width = 1280;
+  intrinsic.height = 800;
+  intrinsic.fx = 600.0F;
+  intrinsic.fy = 601.0F;
+  intrinsic.cx = 640.0F;
+  intrinsic.cy = 400.0F;
+  return intrinsic;
+}
+
+OBCameraDistortion makeDistortion(OBCameraDistortionModel model) {
+  OBCameraDistortion distortion{};
+  distortion.k1 = 0.1F;
+  distortion.k2 = 0.2F;
+  distortion.k3 = 0.3F;
+  distortion.k4 = 0.4F;
+  distortion.k5 = 0.5F;
+  distortion.k6 = 0.6F;
+  distortion.p1 = 0.01F;
+  distortion.p2 = 0.02F;
+  distortion.model = model;
+  return distortion;
+}
+
+TEST(CameraInfoDistortionTest, ConvertsBrownConradyToPlumbBob) {
+  const auto intrinsic = makeIntrinsic();
+  const auto distortion = makeDistortion(OB_DISTORTION_BROWN_CONRADY);
+
+  const auto info = convertToCameraInfo(intrinsic, distortion, intrinsic.width);
+
+  EXPECT_EQ(info.distortion_model, sensor_msgs::distortion_models::PLUMB_BOB);
+  EXPECT_EQ(info.d, std::vector<double>({distortion.k1, distortion.k2, distortion.p1, distortion.p2,
+                                         distortion.k3}));
+}
+
+TEST(CameraInfoDistortionTest, KeepsK6ModelWhenHigherOrderCoefficientsAreZero) {
+  const auto intrinsic = makeIntrinsic();
+  auto distortion = makeDistortion(OB_DISTORTION_BROWN_CONRADY_K6);
+  distortion.k4 = 0.0F;
+  distortion.k5 = 0.0F;
+  distortion.k6 = 0.0F;
+
+  const auto info = convertToCameraInfo(intrinsic, distortion, intrinsic.width);
+
+  EXPECT_EQ(info.distortion_model, sensor_msgs::distortion_models::RATIONAL_POLYNOMIAL);
+  EXPECT_EQ(info.d,
+            std::vector<double>({distortion.k1, distortion.k2, distortion.p1, distortion.p2,
+                                 distortion.k3, distortion.k4, distortion.k5, distortion.k6}));
+}
+
+TEST(CameraInfoDistortionTest, ConvertsKannalaBrandtToEquidistant) {
+  const auto intrinsic = makeIntrinsic();
+  const auto distortion = makeDistortion(OB_DISTORTION_KANNALA_BRANDT4);
+
+  const auto info = convertToCameraInfo(intrinsic, distortion, intrinsic.width);
+
+  EXPECT_EQ(info.distortion_model, sensor_msgs::distortion_models::EQUIDISTANT);
+  EXPECT_EQ(info.d,
+            std::vector<double>({distortion.k1, distortion.k2, distortion.k3, distortion.k4}));
+}
+
+}  // namespace
+}  // namespace orbbec_camera