feat: RigidCamera type and protos

farm-ng · Sep 21, 2023 · 15b90bb · 15b90bb
1 parent a83ecea
commit 15b90bb
Show file tree

Hide file tree

Showing 5 changed files with 29 additions and 31 deletions.
diff --git a/cpp/farm_ng/core/proto_conv/sensor/CMakeLists.txt b/cpp/farm_ng/core/proto_conv/sensor/CMakeLists.txt
@@ -12,6 +12,7 @@ target_link_libraries(farm_ng_core_proto_conv_sensor PUBLIC
   protobuf::libprotobuf
   farm_ng_core::farm_ng_core_prototools
   farm_ng_core::farm_ng_core_proto_conv_image
+  farm_ng_core::farm_ng_core_proto_conv_lie
 )
 
 

diff --git a/cpp/farm_ng/core/proto_conv/sensor/conv.cpp b/cpp/farm_ng/core/proto_conv/sensor/conv.cpp
@@ -15,6 +15,7 @@
 #include "farm_ng/core/proto_conv/sensor/conv.h"
 
 #include "farm_ng/core/proto_conv/image/conv.h"
+#include "farm_ng/core/proto_conv/lie/conv.h"
 
 namespace farm_ng {
 
@@ -73,4 +74,24 @@ auto toProt<std::vector<sophus::CameraModel>>(
   return proto;
 }
 
+template <>
+auto fromProt<core::proto::RigidCamera>(core::proto::RigidCamera const& proto)
+    -> Expected<sophus::RigidCamera> {
+  sophus::RigidCamera s;
+  SOPHUS_TRY(auto, intrinsics, fromProt(proto.intrinsics()));
+  SOPHUS_TRY(auto, extrinsics, fromProt(proto.rig_from_camera()));
+  s.intrinsics = intrinsics;
+  s.rig_from_camera = extrinsics;
+  return s;
+}
+
+template <>
+auto toProt<sophus::RigidCamera>(sophus::RigidCamera const& s)
+    -> core::proto::RigidCamera {
+  core::proto::RigidCamera proto;
+  *proto.mutable_intrinsics() = toProt(s.intrinsics);
+  *proto.mutable_rig_from_camera() = toProt(s.rig_from_camera);
+  return proto;
+}
+
 }  // namespace farm_ng
diff --git a/cpp/farm_ng/core/proto_conv/sensor/conv.h b/cpp/farm_ng/core/proto_conv/sensor/conv.h
@@ -17,11 +17,13 @@
 #include "farm_ng/core/proto_conv/traits.h"
 #include "farm_ng/core/sensor.pb.h"
 #include "sophus/sensor/camera_model.h"
+#include "sophus/sensor/camera_rig.h"
 
 namespace farm_ng {
 
 FARM_PROTO_CONV_TRAIT(sophus::CameraModel, core::proto::CameraModel);
 FARM_PROTO_CONV_TRAIT(
     std::vector<sophus::CameraModel>, core::proto::CameraModels);
+FARM_PROTO_CONV_TRAIT(sophus::RigidCamera, core::proto::RigidCamera);
 
 }  // namespace farm_ng
diff --git a/cpp/sophus/sensor/camera_rig.h b/cpp/sophus/sensor/camera_rig.h
@@ -13,35 +13,13 @@
 
 namespace sophus {
 
-/// Camera as part of a sensor `rig`.
-struct CameraInRig {
-  CameraInRig() {}
-  explicit CameraInRig(CameraModel const& camera_model)
-      : camera_model(camera_model) {}
-
+/// Camera attached to a rigid body. We call the frame of the rigid body "rig".
+struct RigidCamera {
   /// Camera intrinsics
-  CameraModel camera_model;
+  CameraModel intrinsics;
 
   /// Camera extrinsics
   sophus::SE3d rig_from_camera;
 };
 
-/// Sensor rig with multiple cameras.
-struct MultiCameraRig {
-  std::vector<CameraInRig> cameras_in_rig;
-
-  void transformRig(sophus::SE3d const& new_rig_from_rig) {
-    for (auto& camera_in_rig : cameras_in_rig) {
-      camera_in_rig.rig_from_camera =
-          new_rig_from_rig * camera_in_rig.rig_from_camera;
-    }
-  }
-
-  void transformRig(uint32_t camera_num) {
-    sophus::SE3d new_rig_from_rig =
-        cameras_in_rig[camera_num].rig_from_camera.inverse();
-    transformRig(new_rig_from_rig);
-  }
-};
-
 }  // namespace sophus
diff --git a/protos/farm_ng/core/sensor.proto b/protos/farm_ng/core/sensor.proto
@@ -62,16 +62,12 @@ message ClippingPlanes {
   float far = 2;
 }
 
-message CameraInRig {
-  CameraModel camera_model = 1;
+message RigidCamera {
+  CameraModel intrinsics = 1;
 
   Isometry3F64 rig_from_camera = 2;
 }
 
-message MultiCameraRig {
-  repeated CameraInRig cameras = 1;
-}
-
 message GyroModel {
   string model_type = 1;
   repeated double params = 2;