Merge pull request #339 from agri-gaia/feat/addBbsToPcs

enable the later addition of bbs to point clouds

examples/python/gRPC/pointcloud/gRPC_fb_addBoundingBox.py

@@ -0,0 +1,80 @@
+#!/usr/bin/env python3
+NUM_BB_LABELS = 5
+
+import uuid
+
+import flatbuffers
+import numpy as np
+from seerep.fb import PointCloud2
+from seerep.fb import point_cloud_service_grpc_fb as pointcloudService
+from seerep.util.common import get_gRPC_channel
+from seerep.util.fb_helper import (
+    createBoundingBoxes,
+    createBoundingBoxesLabeled,
+    createBoundingBoxLabeledStamped,
+    createBoundingBoxLabeledWithCategory,
+    createHeader,
+    createLabelsWithInstance,
+    createPoint,
+    createQuery,
+    getProject,
+)
+
+builder = flatbuffers.Builder(1024)
+channel = get_gRPC_channel()
+
+projectuuid = getProject(builder, channel, 'testproject')
+
+if not projectuuid:
+    print("Requested project does not exist")
+    exit()
+
+stub = pointcloudService.PointCloudServiceStub(channel)
+
+query = createQuery(builder, projectUuids=[builder.CreateString(projectuuid)], withoutData=True)
+builder.Finish(query)
+buf = builder.Output()
+
+msgToSend = []
+
+for responseBuf in stub.GetPointCloud2(bytes(buf)):
+    response = PointCloud2.PointCloud2.GetRootAs(responseBuf)
+
+    header = createHeader(
+        builder,
+        projectUuid=response.Header().UuidProject().decode("utf-8"),
+        msgUuid=response.Header().UuidMsgs().decode("utf-8"),
+    )
+
+    # create bounding box labels
+    x1, y1, z1 = np.random.rand(3)
+    x2, y2, z2 = np.random.rand(3)
+
+    print("SENDING x1, y1, z1: ", x1, " , ", y1, " , ", z1)
+    print("SENDING x2, y2, z2: ", x2, " , ", y2, " , ", z2)
+
+    boundingBoxes = createBoundingBoxes(
+        builder,
+        [createPoint(builder, x1, y1, z1) for _ in range(NUM_BB_LABELS)],
+        [createPoint(builder, x2, y2, z2) for _ in range(NUM_BB_LABELS)],
+    )
+    labelWithInstances = createLabelsWithInstance(
+        builder,
+        ["BoundingBoxLabel" + str(i) for i in range(NUM_BB_LABELS)],
+        [1.0 / (i + 0.1) for i in range(NUM_BB_LABELS)],
+        [str(uuid.uuid4()) for _ in range(NUM_BB_LABELS)],
+    )
+    labelsBb = createBoundingBoxesLabeled(builder, labelWithInstances, boundingBoxes)
+
+    boundingBoxLabeledWithCategory = createBoundingBoxLabeledWithCategory(
+        builder, builder.CreateString("laterAddedBB"), labelsBb
+    )
+
+    labelsBbVector = createBoundingBoxLabeledStamped(builder, header, [boundingBoxLabeledWithCategory])
+    builder.Finish(labelsBbVector)
+    buf = builder.Output()
+
+    msgToSend.append(bytes(buf))
+
+
+stub.AddBoundingBoxesLabeled(iter(msgToSend))

examples/python/gRPC/util/fb_helper.py

@@ -5,6 +5,7 @@
     BoundingBox2DLabeled,
     BoundingBox2DLabeledWithCategory,
     BoundingBoxes2DLabeledStamped,
+    BoundingBoxesLabeledStamped,
     BoundingBoxLabeled,
     BoundingBoxLabeledWithCategory,
     BoundingboxStamped,
@@ -302,6 +303,19 @@ def createBoundingBox2dLabeledStamped(builder, header, labelsBb):
     return BoundingBoxes2DLabeledStamped.End(builder)
 
 
+def createBoundingBoxLabeledStamped(builder, header, labelsBb):
+    '''Creates a labeled bounding box in flatbuffers'''
+    BoundingBoxesLabeledStamped.StartLabelsBbVector(builder, len(labelsBb))
+    for labelBb in reversed(labelsBb):
+        builder.PrependUOffsetTRelative(labelBb)
+    labelsBbVector = builder.EndVector()
+
+    BoundingBoxesLabeledStamped.Start(builder)
+    BoundingBoxesLabeledStamped.AddHeader(builder, header)
+    BoundingBoxesLabeledStamped.AddLabelsBb(builder, labelsBbVector)
+    return BoundingBoxesLabeledStamped.End(builder)
+
+
 def createBoundingBox2DLabeledWithCategory(builder, category, bb2dLabeled):
     BoundingBox2DLabeledWithCategory.StartBoundingBox2dLabeledVector(builder, len(bb2dLabeled))
     for labelBb in reversed(bb2dLabeled):

seerep_com/fbs/point_cloud_service.fbs

@@ -1,6 +1,7 @@
 
 include "point_cloud_2.fbs";
 include "query.fbs";
+include "boundingboxes_labeled_stamped.fbs";
 
 include "server_response.fbs";
 
@@ -9,4 +10,5 @@ namespace seerep.fb;
 rpc_service PointCloudService {
     GetPointCloud2(seerep.fb.Query):seerep.fb.PointCloud2 (streaming: "server");
     TransferPointCloud2(seerep.fb.PointCloud2):seerep.fb.ServerResponse (streaming: "client");
+    AddBoundingBoxesLabeled(seerep.fb.BoundingBoxesLabeledStamped):seerep.fb.ServerResponse (streaming: "client");
 }

seerep_hdf5/seerep_hdf5_fb/include/seerep_hdf5_fb/hdf5_fb_pointcloud.h

@@ -42,7 +42,15 @@ class Hdf5FbPointCloud : public Hdf5FbGeneral
    */
   void writePointCloud2(const std::string& uuid, const seerep::fb::PointCloud2& pointcloud2,
                         std::vector<float>& boundingBox);
-
+  /**
+   * @brief Write a BoundingBoxes flatbuffers message to hdf5
+   *
+   * @param id the uuid of the image data group
+   * @param bbLabeledWithCategory the flatbuffers BoundingBoxes with category message
+   */
+  void writePointCloudBoundingBoxLabeled(
+      const std::string& id,
+      const flatbuffers::Vector<flatbuffers::Offset<seerep::fb::BoundingBoxLabeledWithCategory>>* bbLabeledWithCategory);
   /**
    * @brief Method for reading a flatbuffers PointCloud2 message from hdf5
    *

seerep_hdf5/seerep_hdf5_fb/src/hdf5_fb_general.cpp

@@ -79,13 +79,26 @@ void Hdf5FbGeneral::writeBoundingBoxLabeled(const std::string& datatypeGroup, co
     {
       labels.push_back(label->labelWithInstance()->label()->label()->str());
       labelConfidence.push_back(label->labelWithInstance()->label()->confidence());
-      std::vector<double> boxWithRotation{
-        label->bounding_box()->center_point()->x(),   label->bounding_box()->center_point()->y(),
-        label->bounding_box()->center_point()->z(),   label->bounding_box()->spatial_extent()->x(),
-        label->bounding_box()->spatial_extent()->y(), label->bounding_box()->spatial_extent()->z(),
-        label->bounding_box()->rotation()->x(),       label->bounding_box()->rotation()->y(),
-        label->bounding_box()->rotation()->z(),       label->bounding_box()->rotation()->w()
-      };
+
+      double rotX = 0.0, rotY = 0.0, rotZ = 0.0, rotW = 1.0;
+      if (label->bounding_box()->rotation() != NULL)
+      {
+        rotX = label->bounding_box()->rotation()->x();
+        rotY = label->bounding_box()->rotation()->y();
+        rotZ = label->bounding_box()->rotation()->z();
+        rotW = label->bounding_box()->rotation()->w();
+      }
+
+      std::vector<double> boxWithRotation{ label->bounding_box()->center_point()->x(),
+                                           label->bounding_box()->center_point()->y(),
+                                           label->bounding_box()->center_point()->z(),
+                                           label->bounding_box()->spatial_extent()->x(),
+                                           label->bounding_box()->spatial_extent()->y(),
+                                           label->bounding_box()->spatial_extent()->z(),
+                                           rotX,
+                                           rotY,
+                                           rotZ,
+                                           rotW };
       boundingBoxesWithRotation.push_back(boxWithRotation);
 
       instances.push_back(label->labelWithInstance()->instanceUuid()->str());

seerep_hdf5/seerep_hdf5_fb/src/hdf5_fb_pointcloud.cpp

@@ -208,6 +208,15 @@ void Hdf5FbPointCloud::writeGeneralAttributes(std::shared_ptr<HighFive::Group>&
   writeAttributeToHdf5<bool>(*dataGroupPtr, seerep_hdf5_core::Hdf5CorePointCloud::IS_DENSE, cloud.is_dense());
 }
 
+void Hdf5FbPointCloud::writePointCloudBoundingBoxLabeled(
+    const std::string& id,
+    const flatbuffers::Vector<flatbuffers::Offset<seerep::fb::BoundingBoxLabeledWithCategory>>* bbLabeledWithCategory)
+{
+  const std::scoped_lock lock(*m_write_mtx);
+
+  writeBoundingBoxLabeled(seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD, id, bbLabeledWithCategory);
+}
+
 // TODO partial point cloud read
 std::optional<flatbuffers::grpc::Message<seerep::fb::PointCloud2>>
 Hdf5FbPointCloud::readPointCloud2(const std::string& id, const bool withoutData)

seerep_msgs/CMakeLists.txt

@@ -80,6 +80,7 @@ set(MY_FBS_FILES
     fbs/boundingbox2d_labeled_with_category.fbs
     fbs/boundingbox2d_stamped.fbs
     fbs/boundingbox2d.fbs
+    fbs/boundingboxes_labeled_stamped.fbs
     fbs/boundingboxes2d_labeled_stamped.fbs
     fbs/camera_intrinsics.fbs
     fbs/camera_intrinsics_query.fbs

seerep_msgs/fbs/boundingboxes_labeled_stamped.fbs

@@ -0,0 +1,10 @@
+
+include "header.fbs";
+include "boundingbox_labeled_with_category.fbs";
+
+namespace seerep.fb;
+
+table BoundingBoxesLabeledStamped {
+  header:seerep.fb.Header;
+  labels_bb:[seerep.fb.BoundingBoxLabeledWithCategory];
+}

seerep_srv/seerep_core_fb/include/seerep_core_fb/core_fb_pointcloud.h

@@ -62,9 +62,14 @@ class CoreFbPointCloud
    * @brief Add a point cloud to the indices and write the data to hdf5
    *
    * @param pc the point cloud message to index and save
-   * @return boost::uuids::uuid the uuid of the stored image
+   * @return boost::uuids::uuid the uuid of the stored pointcloud
    */
   boost::uuids::uuid addData(const seerep::fb::PointCloud2& pc);
+  /**
+   * @brief Adds bounding box based labels to an existing pointcloud
+   * @param bbslabeled the flatbuffer message containing bounding box based labels
+   */
+  void addBoundingBoxesLabeled(const seerep::fb::BoundingBoxesLabeledStamped& boundingBoxeslabeled);
 
 private:
   /** @brief a shared pointer to the general core */

seerep_srv/seerep_core_fb/src/core_fb_pointcloud.cpp

@@ -56,4 +56,48 @@ boost::uuids::uuid CoreFbPointCloud::addData(const seerep::fb::PointCloud2& pc)
   return dataForIndices.header.uuidData;
 }
 
+void CoreFbPointCloud::addBoundingBoxesLabeled(const seerep::fb::BoundingBoxesLabeledStamped& boundingBoxeslabeled)
+{
+  boost::uuids::string_generator gen;
+  boost::uuids::uuid uuidMsg = gen(boundingBoxeslabeled.header()->uuid_msgs()->str());
+  boost::uuids::uuid uuidProject = gen(boundingBoxeslabeled.header()->uuid_project()->str());
+
+  auto hdf5io = CoreFbGeneral::getHdf5(uuidProject, m_seerepCore, m_hdf5IoMap);
+
+  hdf5io->writePointCloudBoundingBoxLabeled(boost::lexical_cast<std::string>(uuidMsg), boundingBoxeslabeled.labels_bb());
+
+  std::unordered_map<std::string, std::vector<seerep_core_msgs::LabelWithInstance>> labelWithInstancePerCategory;
+  for (auto bbWithCategory : *boundingBoxeslabeled.labels_bb())
+  {
+    if (bbWithCategory->boundingBoxLabeled())
+    {
+      std::vector<seerep_core_msgs::LabelWithInstance> labelsWithInstances;
+      for (auto bb : *bbWithCategory->boundingBoxLabeled())
+      {
+        seerep_core_msgs::LabelWithInstance labelWithInstance;
+        labelWithInstance.label = bb->labelWithInstance()->label()->label()->str();
+        labelWithInstance.labelConfidence = bb->labelWithInstance()->label()->confidence();
+
+        try
+        {
+          labelWithInstance.uuidInstance = gen(bb->labelWithInstance()->instanceUuid()->str());
+        }
+        catch (std::runtime_error const& e)
+        {
+          labelWithInstance.uuidInstance = boost::uuids::nil_uuid();
+        }
+        labelsWithInstances.push_back(labelWithInstance);
+      }
+      labelWithInstancePerCategory.emplace(bbWithCategory->category()->c_str(), labelsWithInstances);
+    }
+    // this only adds labels to the pointcloud in the core
+    // if there are already bounding box labels for this pointcloud
+    // those labels must be removed separately. The hdfio currently overrides
+    // existing labels. The data is only correct if labels are added and there
+    // weren't any bounding box labels before
+
+    m_seerepCore->addLabels(seerep_core_msgs::Datatype::PointCloud, labelWithInstancePerCategory, uuidMsg, uuidProject);
+  }
+}
+
 } /* namespace seerep_core_fb */

seerep_srv/seerep_server/include/seerep_server/fb_point_cloud_service.h

@@ -50,6 +50,18 @@ class FbPointCloudService final : public seerep::fb::PointCloudService::Service
   grpc::Status TransferPointCloud2(grpc::ServerContext* context,
                                    grpc::ServerReader<flatbuffers::grpc::Message<seerep::fb::PointCloud2>>* reader,
                                    flatbuffers::grpc::Message<seerep::fb::ServerResponse>* response) override;
+  /**
+   * @brief Adds a stream of labeled bounding boxes to already stored point clouds
+   *
+   * @param context custom inital and trailing metadata (currently not used)
+   * @param reader incoming message stream of bounding boxes from the client
+   * @param response gRPC message to describe the transmission state of the bounding boxes
+   * @return grpc::Status status of the request. Did it work?
+   */
+  grpc::Status AddBoundingBoxesLabeled(
+      grpc::ServerContext* context,
+      grpc::ServerReader<flatbuffers::grpc::Message<seerep::fb::BoundingBoxesLabeledStamped>>* reader,
+      flatbuffers::grpc::Message<seerep::fb::ServerResponse>* response) override;
 
 private:
   /** @brief a shared pointer to the general core */

seerep_srv/seerep_server/src/fb_point_cloud_service.cpp

@@ -135,4 +135,69 @@ grpc::Status FbPointCloudService::TransferPointCloud2(
   return grpc::Status::OK;
 }
 
+grpc::Status FbPointCloudService::AddBoundingBoxesLabeled(
+    grpc::ServerContext* context,
+    grpc::ServerReader<flatbuffers::grpc::Message<seerep::fb::BoundingBoxesLabeledStamped>>* reader,
+    flatbuffers::grpc::Message<seerep::fb::ServerResponse>* response)
+{
+  (void)context;  // ignore that variable without causing warnings
+  std::string answer = "everything stored!";
+
+  flatbuffers::grpc::Message<seerep::fb::BoundingBoxesLabeledStamped> bbsMsg;
+  while (reader->Read(&bbsMsg))
+  {
+    BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::info) << "received BoundingBoxesLabeledStamped... ";
+    auto bbslabeled = bbsMsg.GetRoot();
+
+    std::string uuidProject = bbslabeled->header()->uuid_project()->str();
+    if (uuidProject.empty())
+    {
+      answer = "a msg had no project uuid!";
+    }
+    else
+    {
+      if (!bbslabeled->labels_bb())
+      {
+        answer = "a msg had no bounding boxes!";
+      }
+      else
+      {
+        try
+        {
+          pointCloudFb->addBoundingBoxesLabeled(*bbslabeled);
+        }
+        catch (std::runtime_error const& e)
+        {
+          // mainly catching "invalid uuid string" when transforming uuid_project from string to uuid
+          // also catching core doesn't have project with uuid error
+          BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::error) << e.what();
+
+          seerep_server_util::createResponseFb(std::string(e.what()), seerep::fb::TRANSMISSION_STATE_FAILURE, response);
+
+          return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, e.what());
+        }
+        catch (const std::exception& e)
+        {
+          // specific handling for all exceptions extending std::exception, except
+          // std::runtime_error which is handled explicitly
+          BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::error) << e.what();
+          seerep_server_util::createResponseFb(std::string(e.what()), seerep::fb::TRANSMISSION_STATE_FAILURE, response);
+          return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, e.what());
+        }
+        catch (...)
+        {
+          // catch any other errors (that we have no information about)
+          std::string msg = "Unknown failure occurred. Possible memory corruption";
+          BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::error) << msg;
+          seerep_server_util::createResponseFb(msg, seerep::fb::TRANSMISSION_STATE_FAILURE, response);
+          return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, msg);
+        }
+      }
+    }
+  }
+  seerep_server_util::createResponseFb(answer, seerep::fb::TRANSMISSION_STATE_SUCCESS, response);
+
+  return grpc::Status::OK;
+}
+
 } /* namespace seerep_server */