fixed camera info , see #7

orbbec · Sep 20, 2023 · 01fc318 · 01fc318
1 parent 53d2a41
commit 01fc318
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 20 deletions.
diff --git a/src/ros_setup.cpp b/src/ros_setup.cpp
@@ -240,8 +240,8 @@ void OBCameraNode::setupPublishers() {
     image_publishers_[stream_index] = nh_.advertise<sensor_msgs::Image>(
         topic_name, 1, image_subscribed_cb, image_unsubscribed_cb);
     topic_name = "/" + camera_name_ + "/" + name + "/camera_info";
-    camera_info_publishers_[stream_index] =
-        nh_.advertise<sensor_msgs::CameraInfo>(topic_name, 1, true);
+    camera_info_publishers_[stream_index] = nh_.advertise<sensor_msgs::CameraInfo>(
+        topic_name, 1, image_subscribed_cb, image_unsubscribed_cb);
   }
   if (enable_point_cloud_) {
     ros::SubscriberStatusCallback depth_cloud_subscribed_cb =
@@ -277,14 +277,12 @@ void OBCameraNode::setupPublishers() {
 void OBCameraNode::setupCameraInfo() {
   auto param = getCameraParam();
   if (param) {
-    int base_depth_width = param->depthIntrinsic.width == 0 ? 640 : param->depthIntrinsic.width;
-    int base_rgb_width = param->rgbIntrinsic.width == 0 ? 640 : param->rgbIntrinsic.width;
-    camera_infos_[DEPTH] =
-        convertToCameraInfo(param->depthIntrinsic, param->depthDistortion, base_depth_width);
-    camera_infos_[INFRA0] =
-        convertToCameraInfo(param->depthIntrinsic, param->depthDistortion, base_depth_width);
+    camera_infos_[DEPTH] = convertToCameraInfo(param->depthIntrinsic, param->depthDistortion,
+                                               param->depthIntrinsic.width);
+    camera_infos_[INFRA0] = convertToCameraInfo(param->depthIntrinsic, param->depthDistortion,
+                                                param->depthIntrinsic.width);
     camera_infos_[COLOR] =
-        convertToCameraInfo(param->rgbIntrinsic, param->rgbDistortion, base_rgb_width);
+        convertToCameraInfo(param->rgbIntrinsic, param->rgbDistortion, param->rgbIntrinsic.width);
   } else {
     ROS_WARN_STREAM("Failed to get camera parameters");
   }

diff --git a/src/utils.cpp b/src/utils.cpp
@@ -150,6 +150,7 @@ std::string ObDeviceTypeToString(const OBDeviceType &type) {
 
 sensor_msgs::CameraInfo convertToCameraInfo(OBCameraIntrinsic intrinsic,
                                             OBCameraDistortion distortion, int width) {
+  (void)width;
   sensor_msgs::CameraInfo info;
   info.distortion_model = sensor_msgs::distortion_models::PLUMB_BOB;
   info.width = intrinsic.width;
@@ -179,17 +180,6 @@ sensor_msgs::CameraInfo convertToCameraInfo(OBCameraIntrinsic intrinsic,
   info.P[5] = info.K[4];
   info.P[6] = info.K[5];
   info.P[10] = 1.0;
-
-  double scaling = static_cast<double>(width) / 640;
-  info.K[0] *= scaling;  // fx
-  info.K[2] *= scaling;  // cx
-  info.K[4] *= scaling;  // fy
-  info.K[5] *= scaling;  // cy
-  info.P[0] *= scaling;  // fx
-  info.P[2] *= scaling;  // cx
-  info.P[5] *= scaling;  // fy
-  info.P[6] *= scaling;  // cy
-
   return info;
 }