From d36a28833650eae840f9fc80e7be0cb6a2d70d19 Mon Sep 17 00:00:00 2001 From: TinyOh Date: Tue, 26 Mar 2019 13:58:20 +0800 Subject: [PATCH] feat: add ROS camera info api. --- include/mynteye/api/api.h | 5 ++ include/mynteye/types.h | 34 +++++++++++- samples/tutorials/data/get_img_params.cc | 5 ++ src/mynteye/api/api.cc | 4 ++ src/mynteye/api/processor/depth_processor.cc | 2 +- src/mynteye/api/processor/depth_processor.h | 5 +- src/mynteye/api/processor/points_processor.cc | 2 +- src/mynteye/api/processor/points_processor.h | 6 +-- .../api/processor/rectify_processor.cc | 37 +++++++++---- src/mynteye/api/processor/rectify_processor.h | 28 +++------- .../api/processor/rectify_processor_ocv.cc | 53 +++++++++++++++++++ .../api/processor/rectify_processor_ocv.h | 10 ++++ src/mynteye/api/synthetic.cc | 20 ++++++- src/mynteye/api/synthetic.h | 1 + src/mynteye/types.cc | 30 +++++++++++ 15 files changed, 200 insertions(+), 42 deletions(-) diff --git a/include/mynteye/api/api.h b/include/mynteye/api/api.h index 7a23aff..f9756f3 100644 --- a/include/mynteye/api/api.h +++ b/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 GetCameraROSMsgInfoPair(); + private: std::shared_ptr device_; diff --git a/include/mynteye/types.h b/include/mynteye/types.h index c9ac390..711f177 100644 --- a/include/mynteye/types.h +++ b/include/mynteye/types.h @@ -16,6 +16,7 @@ #pragma once #include +#include #include @@ -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_ diff --git a/samples/tutorials/data/get_img_params.cc b/samples/tutorials/data/get_img_params.cc index 497ff5c..e1ca4c0 100644 --- a/samples/tutorials/data/get_img_params.cc +++ b/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; diff --git a/src/mynteye/api/api.cc b/src/mynteye/api/api.cc index c1e1694..ebeadb3 100644 --- a/src/mynteye/api/api.cc +++ b/src/mynteye/api/api.cc @@ -600,4 +600,8 @@ void API::EnableProcessMode(const std::int32_t& mode) { device_->EnableProcessMode(mode); } +std::shared_ptr API::GetCameraROSMsgInfoPair() { + return synthetic_->GetCameraROSMsgInfoPair(); +} + MYNTEYE_END_NAMESPACE diff --git a/src/mynteye/api/processor/depth_processor.cc b/src/mynteye/api/processor/depth_processor.cc index b1cc940..936ff4c 100644 --- a/src/mynteye/api/processor/depth_processor.cc +++ b/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 calib_infos, + std::shared_ptr calib_infos, std::int32_t proc_period) : Processor(std::move(proc_period)), calib_infos_(calib_infos) { diff --git a/src/mynteye/api/processor/depth_processor.h b/src/mynteye/api/processor/depth_processor.h index 0583c6a..c9b152f 100644 --- a/src/mynteye/api/processor/depth_processor.h +++ b/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 calib_infos, + std::shared_ptr 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 const parent) override; private: - std::shared_ptr calib_infos_; + std::shared_ptr calib_infos_; }; MYNTEYE_END_NAMESPACE diff --git a/src/mynteye/api/processor/points_processor.cc b/src/mynteye/api/processor/points_processor.cc index ea5b08e..30d9c79 100644 --- a/src/mynteye/api/processor/points_processor.cc +++ b/src/mynteye/api/processor/points_processor.cc @@ -53,7 +53,7 @@ struct DepthTraits { const char PointsProcessor::NAME[] = "PointsProcessor"; PointsProcessor::PointsProcessor( - std::shared_ptr calib_infos, + std::shared_ptr calib_infos, std::int32_t proc_period) : Processor(std::move(proc_period)), calib_infos_(calib_infos) { diff --git a/src/mynteye/api/processor/points_processor.h b/src/mynteye/api/processor/points_processor.h index 38de982..441cd93 100644 --- a/src/mynteye/api/processor/points_processor.h +++ b/src/mynteye/api/processor/points_processor.h @@ -20,7 +20,7 @@ #include #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 calib_infos, + std::shared_ptr calib_infos, std::int32_t proc_period = 0); virtual ~PointsProcessor(); @@ -45,7 +45,7 @@ class PointsProcessor : public Processor { std::shared_ptr const parent) override; private: - std::shared_ptr calib_infos_; + std::shared_ptr calib_infos_; }; MYNTEYE_END_NAMESPACE diff --git a/src/mynteye/api/processor/rectify_processor.cc b/src/mynteye/api/processor/rectify_processor.cc index c20f7d2..3dc207f 100644 --- a/src/mynteye/api/processor/rectify_processor.cc +++ b/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 RectifyProcessor::stereoRectify( +std::shared_ptr RectifyProcessor::stereoRectify( camodocal::CameraPtr leftOdo, camodocal::CameraPtr rightOdo, mynteye::IntrinsicsEquidistant in_left, @@ -263,8 +263,8 @@ std::shared_ptr 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 camera_calib_info calib_mat_data_right = getCalibMatData(in_right); + struct CameraROSMsgInfo calib_mat_data_left = getCalibMatData(in_left); + 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 RectifyProcessor::stereoRectify( calib_mat_data_right.R[i*3 +j] = R2.at(i, j); } } - struct camera_calib_info_pair res = - {calib_mat_data_left, calib_mat_data_right, T_mul_f}; - return std::make_shared(res); + + struct CameraROSMsgInfoPair info_pair; + 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(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(); + calib_infos = std::make_shared(); InitParams( *std::dynamic_pointer_cast(intr_left), *std::dynamic_pointer_cast(intr_right), diff --git a/src/mynteye/api/processor/rectify_processor.h b/src/mynteye/api/processor/rectify_processor.h index d5b21d7..acf1318 100644 --- a/src/mynteye/api/processor/rectify_processor.h +++ b/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 getCalibInfoPair() { + inline std::shared_ptr + 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 stereoRectify( + std::shared_ptr 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 calib_infos; + std::shared_ptr calib_infos; }; MYNTEYE_END_NAMESPACE diff --git a/src/mynteye/api/processor/rectify_processor_ocv.cc b/src/mynteye/api/processor/rectify_processor_ocv.cc index 44c1b77..c2c6496 100644 --- a/src/mynteye/api/processor/rectify_processor_ocv.cc +++ b/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(); InitParams( *std::dynamic_pointer_cast(intr_left), *std::dynamic_pointer_cast(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_(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(i, j); + info_pair.right.P[i*4 + j] = P2.at(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(i, j); + info_pair.right.R[i*3 +j] = R2.at(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); } diff --git a/src/mynteye/api/processor/rectify_processor_ocv.h b/src/mynteye/api/processor/rectify_processor_ocv.h index adf113d..089bd39 100644 --- a/src/mynteye/api/processor/rectify_processor_ocv.h +++ b/src/mynteye/api/processor/rectify_processor_ocv.h @@ -40,6 +40,14 @@ class RectifyProcessorOCV : public Processor { std::string Name() override; + inline std::shared_ptr + getCameraROSMsgInfoPair() { + return calib_infos; + } + + struct CameraROSMsgInfo getCalibMatData( + const mynteye::IntrinsicsPinhole& in); + void ReloadImageParams( std::shared_ptr intr_left, std::shared_ptr intr_right, @@ -66,6 +74,8 @@ class RectifyProcessorOCV : public Processor { IntrinsicsPinhole in_right, Extrinsics ex_right_to_left); CalibrationModel calib_model; + + std::shared_ptr calib_infos; }; MYNTEYE_END_NAMESPACE diff --git a/src/mynteye/api/synthetic.cc b/src/mynteye/api/synthetic.cc index ad21162..bf7e671 100644 --- a/src/mynteye/api/synthetic.cc +++ b/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( - rectify_processor_imp -> getCalibInfoPair(), + rectify_processor_imp -> getCameraROSMsgInfoPair(), POINTS_PROC_PERIOD); depth_processor = std::make_shared( - 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 + Synthetic::GetCameraROSMsgInfoPair() { + if (calib_model_ == CalibrationModel::PINHOLE) { + auto processor = getProcessorWithStream(Stream::LEFT_RECTIFIED); + auto proc = static_cast(&(*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(&(*processor)); + return proc->getCameraROSMsgInfoPair(); +#endif + } + return {}; +} + MYNTEYE_END_NAMESPACE diff --git a/src/mynteye/api/synthetic.h b/src/mynteye/api/synthetic.h index ee65dc8..2d2ed88 100644 --- a/src/mynteye/api/synthetic.h +++ b/src/mynteye/api/synthetic.h @@ -93,6 +93,7 @@ class Synthetic { std::shared_ptr getProcessorWithStream(const Stream& stream); void SetDisparityComputingMethodType( const DisparityComputingMethod &MethoType); + std::shared_ptr GetCameraROSMsgInfoPair(); private: void InitCalibInfo(); diff --git a/src/mynteye/types.cc b/src/mynteye/types.cc index 8b6ebcb..397863d 100644 --- a/src/mynteye/types.cc +++ b/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