Merge pull request #360 from agri-gaia/fix/camintrins-matrix-layout

Add check for the correct layout of camera intrinsics matrices.
DFKI-NI · Jan 26, 2024 · 3e1442e · 3e1442e
2 parents 154b059 + 7c7cc0b
commit 3e1442e
Show file tree

Hide file tree

Showing 12 changed files with 123 additions and 15 deletions.
diff --git a/examples/python/gRPC/images/gRPC_fb_addCameraIntrinsics.py b/examples/python/gRPC/images/gRPC_fb_addCameraIntrinsics.py
@@ -38,8 +38,15 @@
 header = createHeader(builder, ts, "map", projectuuid, ciuuid)
 roi = createRegionOfInterest(builder, 3, 5, 6, 7, True)
 
-matrix = [4, 5, 6]
-ci = createCameraIntrinsics(builder, header, 3, 4, "plump_bob", matrix, matrix, matrix, matrix, 4, 5, roi, 5)
+
+distortion_matrix = [4, 5, 6, 7, 8, 9, 10, 11, 12]
+rect_matrix = [4, 5, 6, 7, 8, 9, 10, 11, 12]
+intrins_matrix = [4, 5, 6, 7, 8, 9, 10, 11, 12]
+proj_matrix = [4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15]
+
+ci = createCameraIntrinsics(
+    builder, header, 3, 4, "plumb_bob", distortion_matrix, intrins_matrix, rect_matrix, proj_matrix, 4, 5, roi, 5
+)
 builder.Finish(ci)
 
 buf = builder.Output()

diff --git a/examples/python/gRPC/images/gRPC_pb_addCameraIntrinsics.py b/examples/python/gRPC/images/gRPC_pb_addCameraIntrinsics.py
@@ -60,13 +60,13 @@
 camin.height = 5
 camin.width = 4
 
-camin.distortion_model = "plump_bob"
+camin.distortion_model = "plumb_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.intrinsic_matrix.extend([3, 4, 5, 6, 7, 8, 9, 10, 11])
+camin.rectification_matrix.extend([3, 4, 5, 6, 7, 8, 9, 10, 11])
+camin.projection_matrix.extend([3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14])
 
 camin.binning_x = 6
 camin.binning_y = 7

diff --git a/examples/python/gRPC/images/gRPC_pb_sendLabeledImage.py b/examples/python/gRPC/images/gRPC_pb_sendLabeledImage.py
@@ -78,13 +78,13 @@
 camin.height = 5
 camin.width = 4
 
-camin.distortion_model = "plump_bob"
+camin.distortion_model = "plumb_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.intrinsic_matrix.extend([3, 4, 5, 6, 7, 8, 9, 10, 11])
+camin.rectification_matrix.extend([3, 4, 5, 6, 7, 8, 9, 10, 11])
+camin.projection_matrix.extend([3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14])
 
 camin.binning_x = 6
 camin.binning_y = 7

diff --git a/seerep_hdf5/seerep_hdf5_core/include/seerep_hdf5_core/hdf5_core_image.h b/seerep_hdf5/seerep_hdf5_core/include/seerep_hdf5_core/hdf5_core_image.h
@@ -131,7 +131,7 @@ class Hdf5CoreImage : public virtual Hdf5CoreGeneral, public Hdf5CoreDatatypeInt
    * @param camintrinsics_uuid The UUID of the camera intrinsic parameters to use.
    * @param bb Axis aligned bounding box to store the result in.
    */
-  void computeFrusturmBB(const std::string& camintrinsics_uuid, seerep_core_msgs::AABB& bb);
+  void computeFrustumBB(const std::string& camintrinsics_uuid, seerep_core_msgs::AABB& bb);
 
 public:
   // image attribute keys

diff --git a/seerep_hdf5/seerep_hdf5_core/src/hdf5_core_image.cpp b/seerep_hdf5/seerep_hdf5_core/src/hdf5_core_image.cpp
@@ -54,7 +54,7 @@ std::optional<seerep_core_msgs::DatasetIndexable> Hdf5CoreImage::readDataset(con
   }
   // lock released
 
-  computeFrusturmBB(camintrinsics_uuid, data.boundingbox);
+  computeFrustumBB(camintrinsics_uuid, data.boundingbox);
 
   return data;
 }
@@ -127,7 +127,7 @@ const std::string Hdf5CoreImage::getHdf5DataSetPath(const std::string& id) const
   return getHdf5GroupPath(id) + "/" + RAWDATA;
 }
 
-void Hdf5CoreImage::computeFrusturmBB(const std::string& camintrinsics_uuid, seerep_core_msgs::AABB& bb)
+void Hdf5CoreImage::computeFrustumBB(const std::string& camintrinsics_uuid, seerep_core_msgs::AABB& bb)
 {
   seerep_core_msgs::camera_intrinsics ci =
       m_ioCI->readCameraIntrinsics(boost::lexical_cast<boost::uuids::uuid>(camintrinsics_uuid));

diff --git a/seerep_srv/seerep_core_fb/src/core_fb_image.cpp b/seerep_srv/seerep_core_fb/src/core_fb_image.cpp
@@ -54,7 +54,7 @@ boost::uuids::uuid CoreFbImage::addData(const seerep::fb::Image& img)
     auto hdf5io = CoreFbGeneral::getHdf5(dataForIndices.header.uuidProject, m_seerepCore, m_hdf5IoMap);
     hdf5io->writeImage(boost::lexical_cast<std::string>(dataForIndices.header.uuidData), img);
 
-    hdf5io->computeFrusturmBB(img.uuid_cameraintrinsics()->str(), dataForIndices.boundingbox);
+    hdf5io->computeFrustumBB(img.uuid_cameraintrinsics()->str(), dataForIndices.boundingbox);
 
     m_seerepCore->addDataset(dataForIndices);
 

diff --git a/seerep_srv/seerep_core_pb/src/core_pb_image.cpp b/seerep_srv/seerep_core_pb/src/core_pb_image.cpp
@@ -54,7 +54,7 @@ boost::uuids::uuid CorePbImage::addData(const seerep::pb::Image& img)
     auto hdf5io = getHdf5(dataForIndices.header.uuidProject);
     hdf5io->writeImage(boost::lexical_cast<std::string>(dataForIndices.header.uuidData), img);
 
-    hdf5io->computeFrusturmBB(img.uuid_camera_intrinsics(), dataForIndices.boundingbox);
+    hdf5io->computeFrustumBB(img.uuid_camera_intrinsics(), dataForIndices.boundingbox);
 
     m_seerepCore->addDataset(dataForIndices);
 

diff --git a/seerep_srv/seerep_server/include/seerep_server/fb_cameraintrinsics_service.h b/seerep_srv/seerep_server/include/seerep_server/fb_cameraintrinsics_service.h
@@ -7,6 +7,7 @@
 #include <seerep_core_fb/core_fb_camera_intrinsics.h>
 #include <seerep_core_fb/core_fb_conversion.h>
 
+#include "service_constants.h"
 #include "util.hpp"
 
 // logging

diff --git a/seerep_srv/seerep_server/include/seerep_server/pb_camera_intrinsics_service.h b/seerep_srv/seerep_server/include/seerep_server/pb_camera_intrinsics_service.h
@@ -6,6 +6,7 @@
 #include <seerep_core/core.h>
 #include <seerep_core_pb/core_pb_camera_intrinsics.h>
 
+#include "service_constants.h"
 #include "util.hpp"
 
 // logging

diff --git a/seerep_srv/seerep_server/include/seerep_server/service_constants.h b/seerep_srv/seerep_server/include/seerep_server/service_constants.h
@@ -0,0 +1,13 @@
+#ifndef SEEREP_SERVER_SERVICE_CONSTANTS_H_
+#define SEEREP_SERVER_SERVICE_CONSTANTS_H_
+
+#include <sensor_msgs/distortion_models.h>
+
+namespace seerep_server_constants
+{
+/** @brief constants for allowed distortion models*/
+inline const std::string kCameraDistortionModels[] = { sensor_msgs::distortion_models::PLUMB_BOB,
+                                                       sensor_msgs::distortion_models::RATIONAL_POLYNOMIAL,
+                                                       sensor_msgs::distortion_models::EQUIDISTANT };
+}  // namespace seerep_server_constants
+#endif  // SEEREP_SERVER_SERVICE_CONSTANTS_H_
diff --git a/seerep_srv/seerep_server/src/fb_cameraintrinsics_service.cpp b/seerep_srv/seerep_server/src/fb_cameraintrinsics_service.cpp
@@ -68,6 +68,48 @@ grpc::Status FbCameraIntrinsicsService::TransferCameraIntrinsics(
 
   BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::info) << "received camera instrinsics for storage ";
 
+  // check if the distortion model is found in kCameraDistortionModels
+  bool distortion_model_err =
+      requestRoot->distortion_model()->size() > 0 &&
+      std::find(std::begin(seerep_server_constants::kCameraDistortionModels),
+                std::end(seerep_server_constants::kCameraDistortionModels),
+                requestRoot->distortion_model()->c_str()) == std::end(seerep_server_constants::kCameraDistortionModels);
+
+  bool intrinsics_mat_err = !requestRoot->intrinsic_matrix() || requestRoot->intrinsic_matrix()->size() != 9 ||
+                            (*requestRoot->intrinsic_matrix())[0] == 0 || (*requestRoot->intrinsic_matrix())[4] == 0;
+
+  bool rectification_mat_err =
+      requestRoot->rectification_matrix() &&
+      (requestRoot->rectification_matrix()->size() > 0 && requestRoot->rectification_matrix()->size() != 9);
+
+  bool projection_mat_err = requestRoot->projection_matrix() && (requestRoot->projection_matrix()->size() > 0 &&
+                                                                 requestRoot->projection_matrix()->size() != 12);
+
+  if (distortion_model_err || intrinsics_mat_err || rectification_mat_err || projection_mat_err)
+  {
+    response_message = "";
+    if (distortion_model_err)
+    {
+      response_message += "The distortion model is set but invalid. ";
+    }
+    if (intrinsics_mat_err)
+    {
+      response_message += "Instrinsics matrix is not set, dimensions are incorrect or the focal length values are "
+                          "invalid. ";
+    }
+    if (rectification_mat_err)
+    {
+      response_message += "Rectification matrix dimensions are incorrect. ";
+    }
+    if (projection_mat_err)
+    {
+      response_message += "Projection matrix dimensions are incorrect.";
+    }
+    BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::error) << response_message;
+    seerep_server_util::createResponseFb(response_message, seerep::fb::TRANSMISSION_STATE_FAILURE, response);
+    return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, response_message);
+  }
+
   if (requestRoot->header()->uuid_msgs()->str().empty())
   {
     response_message = "No UUID for Camera Intrinsics set";

diff --git a/seerep_srv/seerep_server/src/pb_camera_intrinsics_service.cpp b/seerep_srv/seerep_server/src/pb_camera_intrinsics_service.cpp
@@ -14,6 +14,50 @@ grpc::Status PbCameraIntrinsicsService::TransferCameraIntrinsics(grpc::ServerCon
   (void)context;  // ignore that variable without causing warnings
   BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) << "received camera intrinsics... ";
 
+  std::string response_message;
+
+  /* TODO: Add common implementation between Protocol Buffers and Flatbuffers for checking the correct matrix dimensions */
+  // check if the distortion model is found in kCameraDistortionModels
+  bool distortion_model_err = camintrinsics->distortion_model().size() > 0 &&
+                              std::find(std::begin(seerep_server_constants::kCameraDistortionModels),
+                                        std::end(seerep_server_constants::kCameraDistortionModels),
+                                        camintrinsics->distortion_model().c_str()) ==
+                                  std::end(seerep_server_constants::kCameraDistortionModels);
+
+  bool intrinsics_mat_err = camintrinsics->intrinsic_matrix_size() != 9 || camintrinsics->intrinsic_matrix()[0] == 0 ||
+                            camintrinsics->intrinsic_matrix()[4] == 0;
+
+  bool rectification_mat_err =
+      camintrinsics->rectification_matrix_size() > 0 && camintrinsics->rectification_matrix_size() != 9;
+
+  bool projection_mat_err =
+      camintrinsics->projection_matrix_size() > 0 && camintrinsics->projection_matrix_size() != 12;
+
+  if (distortion_model_err || intrinsics_mat_err || rectification_mat_err || projection_mat_err)
+  {
+    response_message = "";
+    if (distortion_model_err)
+    {
+      response_message += "The distortion model is set but invalid. ";
+    }
+    if (intrinsics_mat_err)
+    {
+      response_message += "Instrinsics matrix dimensions are incorrect or the focal length values are "
+                          "invalid. ";
+    }
+    if (rectification_mat_err)
+    {
+      response_message += "Rectification matrix dimensions are incorrect. ";
+    }
+    if (projection_mat_err)
+    {
+      response_message += "Projection matrix dimensions are incorrect.";
+    }
+    BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::error) << response_message;
+    seerep_server_util::createResponsePb(response_message, seerep::pb::ServerResponse::FAILURE, response);
+    return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, response_message);
+  }
+
   if (!camintrinsics->header().uuid_project().empty())
   {
     std::string grpc_msg = "Added Camera Intrinsics.";