feat: add ROS camera info api.

include/mynteye/api/api.h

@@ -363,6 +363,11 @@ class MYNTEYE_API API {
   /** Enable process mode, e.g. imu assembly, temp_drift */
   void EnableProcessMode(const std::int32_t& mode);
 
+  /** 
+   * Get ROS need camera info struct
+   */
+  std::shared_ptr<struct CameraROSMsgInfoPair> GetCameraROSMsgInfoPair();
+
  private:
   std::shared_ptr<Device> device_;
 

include/mynteye/types.h

@@ -16,6 +16,7 @@
 #pragma once
 
 #include <memory.h>
+#include <string>
 
 #include <cstdint>
 
@@ -670,7 +671,6 @@ struct MYNTEYE_API DisparityParamsBM : public DisparityParamsBase {
   }
 };
 
-
 /**
  * @defgroup datatypes Datatypes
  * @brief Public data types.
@@ -764,6 +764,38 @@ struct MYNTEYE_API OptionInfo {
 MYNTEYE_API
 std::ostream &operator<<(std::ostream &os, const OptionInfo &info);
 
+/**
+ * @ingroup datatypes
+ * ROS camera info.
+ */
+struct MYNTEYE_API CameraROSMsgInfo {
+  /** height */
+  unsigned int height = 0;
+  /** width */
+  unsigned int width = 0;
+  /** calib model */
+  std::string distortion_model = "null";
+  double  D[5] = {0};
+  double  K[9] = {0};
+  double  R[9] = {0};
+  double  P[12] = {0};
+};
+MYNTEYE_API
+std::ostream &operator<<(std::ostream &os, const CameraROSMsgInfo &info);
+
+struct CameraROSMsgInfoPair {
+  inline bool isEmpty() {
+    return left.height * left.width * right.height * right.width <= 0;
+  }
+  struct CameraROSMsgInfo left;
+  struct CameraROSMsgInfo right;
+  double T_mul_f = -1.f;
+  double  R[9] = {0};
+  double  P[12] = {0};
+};
+MYNTEYE_API
+std::ostream &operator<<(std::ostream &os, const CameraROSMsgInfoPair &info);
+
 MYNTEYE_END_NAMESPACE
 
 #endif  // MYNTEYE_TYPES_H_

samples/tutorials/data/get_img_params.cc

@@ -21,6 +21,11 @@ int main(int argc, char *argv[]) {
   auto &&api = API::Create(argc, argv);
   if (!api) return 1;
 
+  auto info = api->GetCameraROSMsgInfoPair();
+
+  if (!info->isEmpty())
+    std::cout << "ROSMsgInfoPair:"<< std::endl << *info << std::endl;
+
   bool ok;
   auto &&request = api->SelectStreamRequest(&ok);
   if (!ok) return 1;

src/mynteye/api/api.cc

@@ -600,4 +600,8 @@ void API::EnableProcessMode(const std::int32_t& mode) {
   device_->EnableProcessMode(mode);
 }
 
+std::shared_ptr<struct CameraROSMsgInfoPair> API::GetCameraROSMsgInfoPair() {
+  return synthetic_->GetCameraROSMsgInfoPair();
+}
+
 MYNTEYE_END_NAMESPACE

src/mynteye/api/processor/depth_processor.cc

@@ -25,7 +25,7 @@ const int DISPARITY_MIN = 0;
 const int DISPARITY_MAX = 64;
 
 DepthProcessor::DepthProcessor(
-    std::shared_ptr<struct camera_calib_info_pair> calib_infos,
+    std::shared_ptr<struct CameraROSMsgInfoPair> calib_infos,
     std::int32_t proc_period)
     : Processor(std::move(proc_period)),
     calib_infos_(calib_infos) {

src/mynteye/api/processor/depth_processor.h

@@ -19,6 +19,7 @@
 
 #include "mynteye/api/processor.h"
 #include "mynteye/api/processor/rectify_processor.h"
+#include "mynteye/types.h"
 
 MYNTEYE_BEGIN_NAMESPACE
 
@@ -27,7 +28,7 @@ class DepthProcessor : public Processor {
   static const char NAME[];
 
   explicit DepthProcessor(
-      std::shared_ptr<struct camera_calib_info_pair> calib_infos,
+      std::shared_ptr<struct CameraROSMsgInfoPair> calib_infos,
       std::int32_t proc_period = 0);
   virtual ~DepthProcessor();
 
@@ -42,7 +43,7 @@ class DepthProcessor : public Processor {
       Object *const in, Object *const out,
       std::shared_ptr<Processor> const parent) override;
  private:
-  std::shared_ptr<struct camera_calib_info_pair> calib_infos_;
+  std::shared_ptr<struct CameraROSMsgInfoPair> calib_infos_;
 };
 
 MYNTEYE_END_NAMESPACE

src/mynteye/api/processor/points_processor.cc

@@ -53,7 +53,7 @@ struct DepthTraits<float> {
 const char PointsProcessor::NAME[] = "PointsProcessor";
 
 PointsProcessor::PointsProcessor(
-    std::shared_ptr<struct camera_calib_info_pair> calib_infos,
+    std::shared_ptr<struct CameraROSMsgInfoPair> calib_infos,
     std::int32_t proc_period)
     : Processor(std::move(proc_period)),
     calib_infos_(calib_infos) {

src/mynteye/api/processor/points_processor.h

@@ -20,7 +20,7 @@
 #include <opencv2/core/core.hpp>
 
 #include "mynteye/api/processor.h"
-#include "mynteye/api/processor/rectify_processor.h"
+#include "mynteye/types.h"
 
 MYNTEYE_BEGIN_NAMESPACE
 
@@ -29,7 +29,7 @@ class PointsProcessor : public Processor {
   static const char NAME[];
 
   explicit PointsProcessor(
-      std::shared_ptr<struct camera_calib_info_pair> calib_infos,
+      std::shared_ptr<struct CameraROSMsgInfoPair> calib_infos,
       std::int32_t proc_period = 0);
   virtual ~PointsProcessor();
 
@@ -45,7 +45,7 @@ class PointsProcessor : public Processor {
       std::shared_ptr<Processor> const parent) override;
 
  private:
-  std::shared_ptr<struct camera_calib_info_pair> calib_infos_;
+  std::shared_ptr<struct CameraROSMsgInfoPair> calib_infos_;
 };
 
 MYNTEYE_END_NAMESPACE

src/mynteye/api/processor/rectify_processor.cc

@@ -222,16 +222,16 @@ Eigen::Matrix4d RectifyProcessor::loadT(const mynteye::Extrinsics& in) {
 
 void RectifyProcessor::loadCameraMatrix(cv::Mat& K, cv::Mat& D,  // NOLINT
     cv::Size& image_size,  // NOLINT
-    struct camera_calib_info& calib_data) {  // NOLINT
+    struct CameraROSMsgInfo& calib_data) {  // NOLINT
   K = cv::Mat(3, 3, CV_64F, calib_data.K);
   std::size_t d_length = 4;
   D = cv::Mat(1, d_length, CV_64F, calib_data.D);
   image_size = cv::Size(calib_data.width, calib_data.height);
 }
 
-struct camera_calib_info RectifyProcessor::getCalibMatData(
+struct CameraROSMsgInfo RectifyProcessor::getCalibMatData(
     const mynteye::IntrinsicsEquidistant& in) {
-  struct camera_calib_info calib_mat_data;
+  struct CameraROSMsgInfo calib_mat_data;
   calib_mat_data.distortion_model = "KANNALA_BRANDT";
   calib_mat_data.height = in.height;
   calib_mat_data.width = in.width;
@@ -248,7 +248,7 @@ struct camera_calib_info RectifyProcessor::getCalibMatData(
   return calib_mat_data;
 }
 
-std::shared_ptr<struct camera_calib_info_pair> RectifyProcessor::stereoRectify(
+std::shared_ptr<struct CameraROSMsgInfoPair> RectifyProcessor::stereoRectify(
     camodocal::CameraPtr leftOdo,
     camodocal::CameraPtr rightOdo,
     mynteye::IntrinsicsEquidistant in_left,
@@ -263,8 +263,8 @@ std::shared_ptr<struct camera_calib_info_pair> RectifyProcessor::stereoRectify(
   cv::Mat K1, D1, K2, D2;
   cv::Size image_size1, image_size2;
 
-  struct camera_calib_info calib_mat_data_left = getCalibMatData(in_left);
+  struct CameraROSMsgInfo calib_mat_data_left = getCalibMatData(in_left);
-  struct camera_calib_info calib_mat_data_right = getCalibMatData(in_right);
+  struct CameraROSMsgInfo calib_mat_data_right = getCalibMatData(in_right);
 
   loadCameraMatrix(K1, D1, image_size1, calib_mat_data_left);
   loadCameraMatrix(K2, D2, image_size2, calib_mat_data_right);
@@ -308,9 +308,26 @@ std::shared_ptr<struct camera_calib_info_pair> RectifyProcessor::stereoRectify(
       calib_mat_data_right.R[i*3 +j] = R2.at<double>(i, j);
     }
   }
-  struct camera_calib_info_pair res =
+
-      {calib_mat_data_left, calib_mat_data_right, T_mul_f};
+  struct CameraROSMsgInfoPair info_pair;
-  return std::make_shared<struct camera_calib_info_pair>(res);
+  info_pair.left = calib_mat_data_left;
+  info_pair.right = calib_mat_data_right;
+  info_pair.T_mul_f = T_mul_f;
+  for (std::size_t i = 0; i< 3 * 4; i++) {
+    info_pair.P[i] = calib_mat_data_left.P[i];
+  }
+
+  info_pair.R[0] = ex_right_to_left.rotation[0][0];
+  info_pair.R[1] = ex_right_to_left.rotation[0][1];
+  info_pair.R[2] = ex_right_to_left.rotation[0][2];
+  info_pair.R[3] = ex_right_to_left.rotation[1][0];
+  info_pair.R[4] = ex_right_to_left.rotation[1][1];
+  info_pair.R[5] = ex_right_to_left.rotation[1][2];
+  info_pair.R[6] = ex_right_to_left.rotation[2][0];
+  info_pair.R[7] = ex_right_to_left.rotation[2][1];
+  info_pair.R[8] = ex_right_to_left.rotation[2][2];
+
+  return std::make_shared<struct CameraROSMsgInfoPair>(info_pair);
 }
 
 camodocal::CameraPtr RectifyProcessor::generateCameraFromIntrinsicsEquidistant(
@@ -382,7 +399,7 @@ RectifyProcessor::RectifyProcessor(
       std::int32_t proc_period)
     : Processor(std::move(proc_period)),
       calib_model(CalibrationModel::UNKNOW) {
-  calib_infos = std::make_shared<struct camera_calib_info_pair>();
+  calib_infos = std::make_shared<struct CameraROSMsgInfoPair>();
   InitParams(
     *std::dynamic_pointer_cast<IntrinsicsEquidistant>(intr_left),
     *std::dynamic_pointer_cast<IntrinsicsEquidistant>(intr_right),

src/mynteye/api/processor/rectify_processor.h

@@ -31,22 +31,6 @@
 
 MYNTEYE_BEGIN_NAMESPACE
 
-struct camera_calib_info {
-  unsigned int height = 0;
-  unsigned int width = 0;
-  std::string distortion_model = "null";
-  double  D[4] = {0};
-  double  K[9] = {0};
-  double  R[9] = {0};
-  double  P[12] = {0};
-};
-
-struct camera_calib_info_pair {
-  struct camera_calib_info left;
-  struct camera_calib_info right;
-  double T_mul_f;
-};
-
 class Device;
 
 class RectifyProcessor : public Processor {
@@ -69,8 +53,8 @@ class RectifyProcessor : public Processor {
 
   cv::Mat R1, P1, R2, P2, Q;
   cv::Mat map11, map12, map21, map22;
-
+  inline std::shared_ptr<struct CameraROSMsgInfoPair>
-  inline std::shared_ptr<struct camera_calib_info_pair> getCalibInfoPair() {
+      getCameraROSMsgInfoPair() {
     return calib_infos;
   }
 
@@ -103,12 +87,12 @@ class RectifyProcessor : public Processor {
   Eigen::Matrix4d loadT(const mynteye::Extrinsics& in);
   void loadCameraMatrix(cv::Mat& K, cv::Mat& D,  // NOLINT
       cv::Size& image_size,  // NOLINT
-      struct camera_calib_info& calib_data); // NOLINT
+      struct CameraROSMsgInfo& calib_data); // NOLINT
 
-  struct camera_calib_info getCalibMatData(
+  struct CameraROSMsgInfo getCalibMatData(
       const mynteye::IntrinsicsEquidistant& in);
 
-  std::shared_ptr<struct camera_calib_info_pair> stereoRectify(
+  std::shared_ptr<struct CameraROSMsgInfoPair> stereoRectify(
       camodocal::CameraPtr leftOdo,
       camodocal::CameraPtr rightOdo,
       mynteye::IntrinsicsEquidistant in_left,
@@ -119,7 +103,7 @@ class RectifyProcessor : public Processor {
       const mynteye::IntrinsicsEquidistant & in);
 
   CalibrationModel calib_model;
-  std::shared_ptr<struct camera_calib_info_pair> calib_infos;
+  std::shared_ptr<struct CameraROSMsgInfoPair> calib_infos;
 };
 
 MYNTEYE_END_NAMESPACE

src/mynteye/api/processor/rectify_processor_ocv.cc

@@ -32,6 +32,7 @@ RectifyProcessorOCV::RectifyProcessorOCV(
     : Processor(std::move(proc_period)),
       calib_model(CalibrationModel::UNKNOW) {
   VLOG(2) << __func__ << ": proc_period=" << proc_period;
+  calib_infos = std::make_shared<struct CameraROSMsgInfoPair>();
   InitParams(
     *std::dynamic_pointer_cast<IntrinsicsPinhole>(intr_left),
     *std::dynamic_pointer_cast<IntrinsicsPinhole>(intr_right),
@@ -75,6 +76,25 @@ bool RectifyProcessorOCV::OnProcess(
   return true;
 }
 
+struct CameraROSMsgInfo RectifyProcessorOCV::getCalibMatData(
+    const mynteye::IntrinsicsPinhole& in) {
+  struct CameraROSMsgInfo calib_mat_data;
+  calib_mat_data.distortion_model = "PINHOLE";
+  calib_mat_data.height = in.height;
+  calib_mat_data.width = in.width;
+
+  for (std::size_t i = 0; i < 5; i++) {
+    calib_mat_data.D[i] = in.coeffs[i];
+  }
+
+  calib_mat_data.K[0] = in.fx;
+  calib_mat_data.K[4] = in.cx;
+  calib_mat_data.K[2] = in.fy;
+  calib_mat_data.K[5] = in.cy;
+  calib_mat_data.K[8] = 1;
+  return calib_mat_data;
+}
+
 void RectifyProcessorOCV::InitParams(
     IntrinsicsPinhole in_left,
     IntrinsicsPinhole in_right,
@@ -84,6 +104,9 @@ void RectifyProcessorOCV::InitParams(
   in_right.ResizeIntrinsics();
   cv::Size size{in_left.width, in_left.height};
 
+  struct CameraROSMsgInfoPair info_pair;
+  info_pair.left = getCalibMatData(in_left);
+  info_pair.right = getCalibMatData(in_right);
   cv::Mat M1 =
       (cv::Mat_<double>(3, 3) << in_left.fx, 0, in_left.cx, 0, in_left.fy,
        in_left.cy, 0, 0, 1);
@@ -100,6 +123,16 @@ void RectifyProcessorOCV::InitParams(
        ex_right_to_left.rotation[2][1], ex_right_to_left.rotation[2][2]);
   cv::Mat T(3, 1, CV_64F, ex_right_to_left.translation);
 
+  info_pair.R[0] = ex_right_to_left.rotation[0][0];
+  info_pair.R[1] = ex_right_to_left.rotation[0][1];
+  info_pair.R[2] = ex_right_to_left.rotation[0][2];
+  info_pair.R[3] = ex_right_to_left.rotation[1][0];
+  info_pair.R[4] = ex_right_to_left.rotation[1][1];
+  info_pair.R[5] = ex_right_to_left.rotation[1][2];
+  info_pair.R[6] = ex_right_to_left.rotation[2][0];
+  info_pair.R[7] = ex_right_to_left.rotation[2][1];
+  info_pair.R[8] = ex_right_to_left.rotation[2][2];
+
   VLOG(2) << "InitParams size: " << size;
   VLOG(2) << "M1: " << M1;
   VLOG(2) << "M2: " << M2;
@@ -113,6 +146,26 @@ void RectifyProcessorOCV::InitParams(
       M1, D1, M2, D2, size, R, T, R1, R2, P1, P2, Q, cv::CALIB_ZERO_DISPARITY,
       0, size, &left_roi, &right_roi);
 
+  for (std::size_t i = 0; i < 3; i++) {
+    for (std::size_t j = 0; j < 4; j++) {
+      info_pair.left.P[i*4 + j] = P1.at<double>(i, j);
+      info_pair.right.P[i*4 + j] = P2.at<double>(i, j);
+    }
+  }
+
+  for (std::size_t i = 0; i < 3; i++) {
+    for (std::size_t j = 0; j < 3; j++) {
+      info_pair.left.R[i*3 + j] = R1.at<double>(i, j);
+      info_pair.right.R[i*3 +j] = R2.at<double>(i, j);
+    }
+  }
+
+  for (std::size_t i = 0; i< 3 * 4; i++) {
+    info_pair.P[i] = info_pair.left.P[i];
+  }
+
+  *calib_infos = info_pair;
+
   cv::initUndistortRectifyMap(M1, D1, R1, P1, size, CV_16SC2, map11, map12);
   cv::initUndistortRectifyMap(M2, D2, R2, P2, size, CV_16SC2, map21, map22);
 }

src/mynteye/api/processor/rectify_processor_ocv.h

@@ -40,6 +40,14 @@ class RectifyProcessorOCV : public Processor {
 
   std::string Name() override;
 
+  inline std::shared_ptr<struct CameraROSMsgInfoPair>
+      getCameraROSMsgInfoPair() {
+    return calib_infos;
+  }
+
+  struct CameraROSMsgInfo getCalibMatData(
+    const mynteye::IntrinsicsPinhole& in);
+
   void ReloadImageParams(
       std::shared_ptr<IntrinsicsBase> intr_left,
       std::shared_ptr<IntrinsicsBase> intr_right,
@@ -66,6 +74,8 @@ class RectifyProcessorOCV : public Processor {
         IntrinsicsPinhole in_right, Extrinsics ex_right_to_left);
 
   CalibrationModel calib_model;
+
+  std::shared_ptr<struct CameraROSMsgInfoPair> calib_infos;
 };
 
 MYNTEYE_END_NAMESPACE

src/mynteye/api/synthetic.cc

@@ -352,10 +352,10 @@ void Synthetic::InitProcessors() {
                                            RECTIFY_PROC_PERIOD);
     rectify_processor = rectify_processor_imp;
     points_processor = std::make_shared<PointsProcessor>(
-        rectify_processor_imp -> getCalibInfoPair(),
+        rectify_processor_imp -> getCameraROSMsgInfoPair(),
         POINTS_PROC_PERIOD);
     depth_processor = std::make_shared<DepthProcessor>(
-        rectify_processor_imp -> getCalibInfoPair(),
+        rectify_processor_imp -> getCameraROSMsgInfoPair(),
         DEPTH_PROC_PERIOD);
 
     root_processor->AddChild(rectify_processor);
@@ -566,4 +566,20 @@ void Synthetic::NotifyStreamData(
   }
 }
 
+std::shared_ptr<struct CameraROSMsgInfoPair>
+    Synthetic::GetCameraROSMsgInfoPair() {
+  if (calib_model_ == CalibrationModel::PINHOLE) {
+    auto processor = getProcessorWithStream(Stream::LEFT_RECTIFIED);
+    auto proc = static_cast<RectifyProcessorOCV*>(&(*processor));
+    return proc->getCameraROSMsgInfoPair();
+#ifdef WITH_CAM_MODELS
+  } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) {
+    auto processor = getProcessorWithStream(Stream::LEFT_RECTIFIED);
+    auto proc = static_cast<RectifyProcessor*>(&(*processor));
+    return proc->getCameraROSMsgInfoPair();
+#endif
+  }
+  return {};
+}
+
 MYNTEYE_END_NAMESPACE

src/mynteye/api/synthetic.h

@@ -93,6 +93,7 @@ class Synthetic {
   std::shared_ptr<Processor> getProcessorWithStream(const Stream& stream);
   void SetDisparityComputingMethodType(
       const DisparityComputingMethod &MethoType);
+  std::shared_ptr<struct CameraROSMsgInfoPair> GetCameraROSMsgInfoPair();
 
  private:
   void InitCalibInfo();

src/mynteye/types.cc

@@ -317,4 +317,34 @@ std::ostream &operator<<(std::ostream &os, const OptionInfo &info) {
             << ", def: " << info.def;
 }
 
+std::ostream &operator<<(std::ostream &os, const CameraROSMsgInfo &info) {
+  os << "width: " << info.width << ", height: " << info.height << std::endl
+      << "distortion_model: " << info.distortion_model;
+  os << std::endl << "D: ";
+  for (size_t i = 0; i < 5; i++)
+    os << info.D[i] << ",";
+  os << std::endl << "K: ";
+  for (size_t i = 0; i < 9; i++)
+    os << info.K[i] << ",";
+  os << std::endl << "R: ";
+  for (size_t i = 0; i < 9; i++)
+    os << info.R[i] << ",";
+  os << std::endl << "P: ";
+  for (size_t i = 0; i < 12; i++)
+    os << info.P[i] << ",";
+  os << std::endl;
+}
+
+std::ostream &operator<<(std::ostream &os, const CameraROSMsgInfoPair &info) {
+  os << "left:\n" << info.left << std::endl;
+  os << "right:\n" << info.right << std::endl;
+  os << "base R:";
+  for (size_t i = 0; i < 9; i++)
+    os << info.R[i] << ",";
+  os << std::endl << "base P:";
+  for (size_t i = 0; i < 12; i++)
+    os << info.P[i] << ",";
+  os << std::endl;
+}
+
 MYNTEYE_END_NAMESPACE