Merge pull request #405 from agri-gaia/feat/ros_conversion_intrinsic

fb camera intrinsic to ros camera info
agri-gaia · Aug 8, 2024 · 5f87059 · 5f87059
2 parents f210a45 + bf85531
commit 5f87059
Show file tree

Hide file tree

Showing 2 changed files with 45 additions and 1 deletion.
diff --git a/seerep_ros/seerep_ros_conversions_fb/include/seerep_ros_conversions_fb/conversions.h b/seerep_ros/seerep_ros_conversions_fb/include/seerep_ros_conversions_fb/conversions.h
@@ -323,6 +323,13 @@ toFlat(const sensor_msgs::CameraInfo& ci, std::string& projectuuid,
        std::string& msgUuid, double maxViewingDistance,
 
        flatbuffers::grpc::MessageBuilder& builder);
+/**
+ * @brief Converts a Flatbuffer CameraIntrinsics message to the corresponding
+ * ROS sensor_msgs::CameraInfo message
+ * @param ci Flatbuffer CameraIntrinsics message
+ * @return ROS sensor_msgs::CameraInfo
+ */
+sensor_msgs::CameraInfo toROS(const seerep::fb::CameraIntrinsics& ci);
 /**
  * @brief Converts a ROS sensor_msgs::RegionOfInterest message to the
  * corresponding gRPC Flatbuffer RegionOfInterest message
@@ -334,7 +341,13 @@ flatbuffers::Offset<seerep::fb::RegionOfInterest>
 toFlat(const sensor_msgs::RegionOfInterest& roi,
 
        flatbuffers::grpc::MessageBuilder& builder);
-
+/**
+ * @brief Converts a Flatbuffer CameraIntrinsics message to the corresponding
+ * ROS sensor_msgs::CameraInfo message
+ * @param roi Flatbuffer RegionOfInterest message
+ * @return ROS sensor_msgs::RegionOfInterest
+ */
+sensor_msgs::RegionOfInterest toROS(const seerep::fb::RegionOfInterest& roi);
 } /* namespace seerep_ros_conversions_fb */
 
 #endif /* SEEREP_ROS_CONVERSIONS_FB */
diff --git a/seerep_ros/seerep_ros_conversions_fb/src/seerep_ros_conversions_fb/conversions.cpp b/seerep_ros/seerep_ros_conversions_fb/src/seerep_ros_conversions_fb/conversions.cpp
@@ -427,6 +427,26 @@ toFlat(const sensor_msgs::CameraInfo& ci, std::string& projectuuid,
   cibuilder.add_height(ci.height);
   return cibuilder.Finish();
 }
+
+sensor_msgs::CameraInfo toROS(const seerep::fb::CameraIntrinsics& ci)
+{
+  sensor_msgs::CameraInfo ret;
+  ret.header = toROS(*ci.header());
+  ret.height = ci.height();
+  ret.width = ci.width();
+
+  ret.distortion_model = ci.distortion_model()->str();
+  ret.binning_x = ci.binning_x();
+  ret.binning_y = ci.binning_y();
+  std::copy_n(ci.distortion()->Data(), ci.distortion()->size(),
+              std::back_inserter(ret.D));
+  std::memcpy(&ret.K, ci.intrinsic_matrix()->Data(), sizeof(ret.K));
+  std::memcpy(&ret.R, ci.rectification_matrix()->Data(), sizeof(ret.R));
+  std::memcpy(&ret.P, ci.projection_matrix()->Data(), sizeof(ret.P));
+  ret.roi = toROS(*ci.region_of_interest());
+  return ret;
+}
+
 flatbuffers::Offset<seerep::fb::RegionOfInterest>
 toFlat(const sensor_msgs::RegionOfInterest& roi,
 
@@ -440,4 +460,15 @@ toFlat(const sensor_msgs::RegionOfInterest& roi,
   roiBuilder.add_y_offset(roi.y_offset);
   return roiBuilder.Finish();
 }
+
+sensor_msgs::RegionOfInterest toROS(const seerep::fb::RegionOfInterest& roi)
+{
+  sensor_msgs::RegionOfInterest ret;
+  ret.x_offset = roi.x_offset();
+  ret.y_offset = roi.y_offset();
+  ret.height = roi.height();
+  ret.width = roi.width();
+  ret.do_rectify = roi.do_rectify();
+  return ret;
+}
 }  // namespace seerep_ros_conversions_fb