Merge pull request #274 from agri-gaia/camintrin-phase2

Camera Intrinsics Phase 2

examples/python/gRPC/images/gRPC_fb_addCameraIntrinsics.py

@@ -59,6 +59,7 @@
 
 ret = stub.GetCameraIntrinsics(bytes(buf))
 
-for ci in ret:
-    ci_fb = CameraIntrinsics.CameraIntrinsics.GetRootAs(ci)
-    print(ci_fb.DistortionModel())
+retrieved_ci = CameraIntrinsics.CameraIntrinsics.GetRootAs(ret)
+
+# printing the uuid of the retrieved camera intrinsics
+print(retrieved_ci.Header().UuidMsgs().decode('utf-8'))

examples/python/gRPC/images/gRPC_pb_addCameraIntrinsics.py

@@ -0,0 +1,83 @@
+#!/usr/bin/env python3
+
+import os
+import sys
+
+from google.protobuf import empty_pb2
+from seerep.pb import camera_intrinsics_pb2 as cameraintrinsics
+from seerep.pb import camera_intrinsics_query_pb2 as cameraintrinsicsquery
+from seerep.pb import camera_intrinsics_service_pb2_grpc as camintrinsics_service
+from seerep.pb import meta_operations_pb2_grpc as metaOperations
+
+# importing util functions. Assuming that this file is in the parent dir
+# https://github.com/agri-gaia/seerep/blob/6c4da5736d4a893228e97b01a9ada18620b1a83f/examples/python/gRPC/util.py
+script_dir = os.path.dirname(__file__)
+util_dir = os.path.join(script_dir, '..')
+sys.path.append(util_dir)
+import uuid
+
+import util
+
+# Default server is localhost !
+channel = util.get_gRPC_channel()
+
+# 1. Get gRPC service objects
+stub = camintrinsics_service.CameraIntrinsicsServiceStub(channel)
+stubMeta = metaOperations.MetaOperationsStub(channel)
+
+# 2. Get all projects from the server
+response = stubMeta.GetProjects(empty_pb2.Empty())
+
+# 3. Check if we have an existing test project, if not, we stop here
+projectuuid = ""
+for project in response.projects:
+    print(project.name + " " + project.uuid + "\n")
+    if project.name == "testproject":
+        projectuuid = project.uuid
+
+if projectuuid == "":
+    sys.exit()
+
+ciuuid = str(uuid.uuid4())
+print("Camera Intrinsics will be saved against the uuid: ", ciuuid)
+
+camin = cameraintrinsics.CameraIntrinsics()
+
+camin.header.stamp.seconds = 4
+camin.header.stamp.nanos = 3
+
+camin.header.frame_id = "camintrinsics"
+
+camin.header.uuid_project = projectuuid
+camin.header.uuid_msgs = ciuuid
+
+camin.region_of_interest.x_offset = 2
+camin.region_of_interest.y_offset = 1
+camin.region_of_interest.height = 5
+camin.region_of_interest.width = 4
+camin.region_of_interest.do_rectify = 4
+
+camin.height = 5
+camin.width = 4
+
+camin.distortion_model = "plump_bob"
+
+camin.distortion.extend([3, 4, 5])
+
+camin.intrinsic_matrix.extend([3, 4, 5])
+camin.rectification_matrix.extend([3, 4, 5])
+camin.projection_matrix.extend([3, 4, 5])
+
+camin.binning_x = 6
+camin.binning_y = 7
+
+stub.TransferCameraIntrinsics(camin)
+
+# Fetch the saved CI
+ci_query = cameraintrinsicsquery.CameraIntrinsicsQuery()
+
+ci_query.uuid_camera_intrinsics = ciuuid
+ci_query.uuid_project = projectuuid
+
+fetched_camintrinsics = stub.GetCameraIntrinsics(ci_query)
+print(fetched_camintrinsics)

examples/python/gRPC/images/gRPC_pb_sendLabeledImage.py

@@ -11,6 +11,8 @@
 from seerep.pb import (
     boundingbox2d_labeled_with_category_pb2 as boundingbox2d_labeled_with_category,
 )
+from seerep.pb import camera_intrinsics_pb2 as cameraintrinsics
+from seerep.pb import camera_intrinsics_service_pb2_grpc as camintrinsics_service
 from seerep.pb import image_pb2 as image
 from seerep.pb import image_service_pb2_grpc as imageService
 from seerep.pb import label_with_instance_pb2 as labelWithInstance
@@ -36,6 +38,7 @@
 stub = imageService.ImageServiceStub(channel)
 stubTf = tfService.TfServiceStub(channel)
 stubMeta = metaOperations.MetaOperationsStub(channel)
+stubCI = camintrinsics_service.CameraIntrinsicsServiceStub(channel)
 
 # 2. Get all projects from the server
 response = stubMeta.GetProjects(empty_pb2.Empty())
@@ -53,9 +56,47 @@
     projectCreated = stubMeta.CreateProject(creation)
     projectuuid = projectCreated.uuid
 
-
 theTime = int(time.time())
 
+#####
+# A valid camera intrinsics UUID is needed here for succesful storage of Images
+# Add new Camera Intrinsics
+
+ciuuid = str(uuid.uuid4())
+print("Camera Intrinsics will be saved against the uuid: ", ciuuid)
+
+camin = cameraintrinsics.CameraIntrinsics()
+
+camin.header.stamp.seconds = 4
+camin.header.stamp.nanos = 3
+
+camin.header.frame_id = "camintrinsics"
+
+camin.header.uuid_project = projectuuid
+camin.header.uuid_msgs = ciuuid
+
+camin.region_of_interest.x_offset = 2
+camin.region_of_interest.y_offset = 1
+camin.region_of_interest.height = 5
+camin.region_of_interest.width = 4
+camin.region_of_interest.do_rectify = 4
+
+camin.height = 5
+camin.width = 4
+
+camin.distortion_model = "plump_bob"
+
+camin.distortion.extend([3, 4, 5])
+
+camin.intrinsic_matrix.extend([3, 4, 5])
+camin.rectification_matrix.extend([3, 4, 5])
+camin.projection_matrix.extend([3, 4, 5])
+
+camin.binning_x = 6
+camin.binning_y = 7
+
+stubCI.TransferCameraIntrinsics(camin)
+
 # 4. Create ten images
 for n in range(10):
     theImage = image.Image()
@@ -83,6 +124,8 @@
     theImage.width = lim
     theImage.encoding = "rgb8"
     theImage.step = 3 * lim
+    theImage.uuid_camera_intrinsics = ciuuid
+
     # Add image data
     theImage.data = bytes(rgb)
 

seerep_com/CMakeLists.txt

@@ -21,6 +21,7 @@ set(MY_PROTO_FILES
     protos/tf_service.proto
     protos/image_service.proto
     protos/point_cloud_service.proto
+    protos/camera_intrinsics_service.proto
 )
 
 protobuf_generate_cpp(PROTO_SOURCES PROTO_HEADERS ${MY_PROTO_FILES})

seerep_com/fbs/camera_intrinsics_service.fbs

@@ -8,6 +8,6 @@ include "server_response.fbs";
 namespace seerep.fb;
 
 rpc_service CameraIntrinsicsService {
-  GetCameraIntrinsics(seerep.fb.CameraIntrinsicsQuery):seerep.fb.CameraIntrinsics (streaming: "server");
+  GetCameraIntrinsics(seerep.fb.CameraIntrinsicsQuery):seerep.fb.CameraIntrinsics;
   TransferCameraIntrinsics(seerep.fb.CameraIntrinsics):seerep.fb.ServerResponse;
 }

seerep_com/protos/camera_intrinsics_service.proto

@@ -0,0 +1,14 @@
+syntax = "proto3";
+
+package seerep.pb;
+
+import "camera_intrinsics.proto";
+
+import "camera_intrinsics_query.proto";
+import "server_response.proto";
+
+service CameraIntrinsicsService
+{
+  rpc GetCameraIntrinsics(CameraIntrinsicsQuery) returns (CameraIntrinsics);
+  rpc TransferCameraIntrinsics(CameraIntrinsics) returns (ServerResponse);
+}

seerep_hdf5/seerep_hdf5_core/include/seerep_hdf5_core/hdf5_core_cameraintrinsics.h

@@ -33,6 +33,15 @@ class Hdf5CoreCameraIntrinsics : public Hdf5CoreGeneral
    */
   void writeCameraIntrinsics(const seerep_core_msgs::camera_intrinsics& camIntrinsics);
 
+  /**
+   * @brief Check if there is an existing Camera Intrinsics against the provided UUID
+   *
+   * @param [in] cameraintrinsics_uuid boost uuid of a camera intrinsics object
+   * @return true If camera intrinsics exist
+   * @return false If camera intrinsics do not exist
+   */
+  bool checkCameraIntrinsicsExists(const boost::uuids::uuid& cameraintrinsics_uuid);
+
 private:
   const std::shared_ptr<HighFive::DataSet> generateDatasetPointer(const std::string& attribute);
   const std::string getHdf5GroupPath(const std::string& id) const;

seerep_hdf5/seerep_hdf5_core/include/seerep_hdf5_core/hdf5_core_image.h

@@ -36,6 +36,7 @@ struct ImageAttributes
   uint32_t step;
   std::string encoding;
   bool isBigendian;
+  std::string cameraIntrinsicsUuid;
 };
 
 /**
@@ -130,6 +131,7 @@ class Hdf5CoreImage : public virtual Hdf5CoreGeneral, public Hdf5CoreDatatypeInt
   inline static const std::string IS_BIGENDIAN = "is_bigendian";
   inline static const std::string STEP = "step";
   inline static const std::string IS_DENSE = "is_dense";
+  inline static const std::string CAMERA_INTRINSICS_UUID = "camera_intrinsics_uuid";
 
   inline static const std::string RAWDATA = "rawdata";
 

seerep_hdf5/seerep_hdf5_core/src/hdf5_core_cameraintrinsics.cpp

@@ -63,10 +63,8 @@ Hdf5CoreCameraIntrinsics::readCameraIntrinsics(const boost::uuids::uuid& camerai
 {
   const std::scoped_lock lock(*m_write_mtx);
 
-  //   std::string uuid = boost::lexical_cast<std::string>(project_uuid);
   std::string id = boost::lexical_cast<std::string>(cameraintrinsics_uuid);
 
-  //   std::string hdf5DatasetPath = getHdf5DataSetPath(uuid);
   std::string hdf5GroupPath = getHdf5GroupPath(id);
 
   auto dataGroupPtr = getHdf5Group(hdf5GroupPath, false);
@@ -118,6 +116,28 @@ Hdf5CoreCameraIntrinsics::readCameraIntrinsics(const boost::uuids::uuid& camerai
   return ci;
 }
 
+bool Hdf5CoreCameraIntrinsics::checkCameraIntrinsicsExists(const boost::uuids::uuid& cameraintrinsics_uuid)
+{
+  std::string id = boost::lexical_cast<std::string>(cameraintrinsics_uuid);
+
+  try
+  {
+    // call check exists for the provided camera intrinsics id
+    std::string id = boost::lexical_cast<std::string>(cameraintrinsics_uuid);
+
+    std::string cameraIntrinsicsGroupPath = getHdf5GroupPath(id);
+
+    checkExists(cameraIntrinsicsGroupPath);
+  }
+  catch (std::invalid_argument const& e)
+  {
+    // if an invalid arg exception is raised it does not exist, therefore return false
+    return false;
+  }
+  // otherwise return true
+  return true;
+}
+
 const std::string Hdf5CoreCameraIntrinsics::getHdf5GroupPath(const std::string& id) const
 {
   return HDF5_GROUP_CAMINTRINSICS + "/" + id;

seerep_hdf5/seerep_hdf5_core/src/hdf5_core_image.cpp

@@ -86,6 +86,8 @@ void Hdf5CoreImage::writeImageAttributes(const std::string& id, const ImageAttri
     writeAttributeToHdf5<std::string>(*dataSetPtr, seerep_hdf5_core::Hdf5CoreImage::ENCODING, attributes.encoding);
     writeAttributeToHdf5<bool>(*dataSetPtr, seerep_hdf5_core::Hdf5CoreImage::IS_BIGENDIAN, attributes.isBigendian);
     writeAttributeToHdf5<uint32_t>(*dataGroupPtr, seerep_hdf5_core::Hdf5CoreImage::STEP, attributes.step);
+    writeAttributeToHdf5<std::string>(*dataGroupPtr, seerep_hdf5_core::Hdf5CoreImage::CAMERA_INTRINSICS_UUID,
+                                      attributes.cameraIntrinsicsUuid);
   }
 }
 
@@ -108,6 +110,8 @@ ImageAttributes Hdf5CoreImage::readImageAttributes(const std::string& id)
     attributes.isBigendian =
         readAttributeFromHdf5<bool>(id, *dataSetPtr, seerep_hdf5_core::Hdf5CoreImage::IS_BIGENDIAN);
     attributes.step = readAttributeFromHdf5<uint32_t>(id, *dataGroupPtr, seerep_hdf5_core::Hdf5CoreImage::STEP);
+    attributes.cameraIntrinsicsUuid =
+        readAttributeFromHdf5<std::string>(id, *dataGroupPtr, seerep_hdf5_core::Hdf5CoreImage::CAMERA_INTRINSICS_UUID);
   }
   return attributes;
 }

seerep_hdf5/seerep_hdf5_fb/src/hdf5_fb_image.cpp

@@ -22,8 +22,9 @@ void Hdf5FbImage::writeImage(const std::string& id, const seerep::fb::Image& ima
 
   writeHeaderAttributes(*dataGroupPtr, image.header());
 
-  seerep_hdf5_core::ImageAttributes imageAttributes = { image.height(), image.width(), image.step(),
-                                                        image.encoding()->str(), image.is_bigendian() };
+  seerep_hdf5_core::ImageAttributes imageAttributes = { image.height(),       image.width(),
+                                                        image.step(),         image.encoding()->str(),
+                                                        image.is_bigendian(), image.uuid_cameraintrinsics()->str() };
   writeImageAttributes(id, imageAttributes);
 
   const uint8_t* arrayStartPtr = image.data()->Data();
@@ -91,11 +92,15 @@ std::optional<flatbuffers::grpc::Message<seerep::fb::Image>> Hdf5FbImage::readIm
 
   // construct flatbuffers image message
 
+  flatbuffers::Offset<flatbuffers::String> camintrinsics_offset =
+      builder.CreateString(imageAttributes.cameraIntrinsicsUuid);
+
   seerep::fb::ImageBuilder imageBuilder(builder);
   imageBuilder.add_height(imageAttributes.height);
   imageBuilder.add_width(imageAttributes.width);
   imageBuilder.add_encoding(encodingStringOffset);
   imageBuilder.add_is_bigendian(imageAttributes.isBigendian);
+  imageBuilder.add_uuid_cameraintrinsics(camintrinsics_offset);
   imageBuilder.add_step(imageAttributes.step);
   if (!withoutData)
   {

seerep_hdf5/seerep_hdf5_fb/test/fb_write_load_test.cpp

@@ -155,17 +155,20 @@ createLabelsGeneral(flatbuffers::FlatBufferBuilder& fbb)
 
 const seerep::fb::Image* createImageMessage(flatbuffers::FlatBufferBuilder& fbb, const unsigned int imageHeight,
                                             const unsigned imageWidth, const std::string& projectUUID,
-                                            const std::string& messageUUID)
+                                            const std::string& messageUUID, const std::string& camintrinsicsUUID)
 {
   auto encodingOffset = fbb.CreateString("rgb8");
+  auto camintrinsicsUUIDOffset = fbb.CreateString(camintrinsicsUUID);
+
   auto headerOffset = createHeader(fbb, "camera", projectUUID, messageUUID);
   auto imageOffset = createImageData(fbb, 256, 256);
 
   auto generalLabelsOffset = createLabelsGeneral(fbb);
   auto bB2DLabeledOffset = createBB2DLabeled(fbb);
 
-  auto imgMsgOffset = seerep::fb::CreateImage(fbb, headerOffset, imageHeight, imageWidth, encodingOffset, true,
-                                              3 * imageHeight, imageOffset, generalLabelsOffset, bB2DLabeledOffset);
+  auto imgMsgOffset =
+      seerep::fb::CreateImage(fbb, headerOffset, imageHeight, imageWidth, encodingOffset, true, 3 * imageHeight,
+                              imageOffset, generalLabelsOffset, bB2DLabeledOffset, camintrinsicsUUIDOffset);
   fbb.Finish(imgMsgOffset);
   uint8_t* buf = fbb.GetBufferPointer();
   return flatbuffers::GetRoot<seerep::fb::Image>(buf);
@@ -181,6 +184,7 @@ class fbWriteLoadTest : public testing::Test
 
   static boost::uuids::uuid projectUUID;
   static boost::uuids::uuid messageUUID;
+  static boost::uuids::uuid cameraintrinsicsUUID;
   static std::string projectName;
 
   static const seerep::fb::Image* writeImage;
@@ -195,6 +199,7 @@ class fbWriteLoadTest : public testing::Test
     projectName = "testProject";
     projectUUID = boost::uuids::random_generator()();
     messageUUID = boost::uuids::random_generator()();
+    cameraintrinsicsUUID = boost::uuids::random_generator()();
 
     hdf5FileMutex = std::make_shared<std::mutex>();
     hdf5FileName = boost::lexical_cast<std::string>(projectUUID) + ".h5";
@@ -209,7 +214,8 @@ class fbWriteLoadTest : public testing::Test
     }
 
     writeImage = createImageMessage(fbb, 256, 256, boost::lexical_cast<std::string>(projectUUID),
-                                    boost::lexical_cast<std::string>(messageUUID));
+                                    boost::lexical_cast<std::string>(messageUUID),
+                                    boost::lexical_cast<std::string>(cameraintrinsicsUUID));
     if (writeImage == nullptr)
     {
       GTEST_FATAL_FAILURE_("Error: No image data to write into HDF5 file");
@@ -238,6 +244,7 @@ std::string fbWriteLoadTest::hdf5FileName;
 
 boost::uuids::uuid fbWriteLoadTest::projectUUID;
 boost::uuids::uuid fbWriteLoadTest::messageUUID;
+boost::uuids::uuid fbWriteLoadTest::cameraintrinsicsUUID;
 std::string fbWriteLoadTest::projectName;
 
 flatbuffers::FlatBufferBuilder fbWriteLoadTest::fbb;
@@ -262,6 +269,7 @@ TEST_F(fbWriteLoadTest, testImageBaseFields)
   EXPECT_STREQ(readImage->encoding()->c_str(), writeImage->encoding()->c_str());
   EXPECT_EQ(readImage->is_bigendian(), writeImage->is_bigendian());
   EXPECT_EQ(readImage->step(), writeImage->step());
+  EXPECT_EQ(readImage->uuid_cameraintrinsics()->str(), writeImage->uuid_cameraintrinsics()->str());
 }
 
 TEST_F(fbWriteLoadTest, testImageData)