diff --git a/src/mynteye/api/processor/depth_processor.cc b/src/mynteye/api/processor/depth_processor.cc index e6e5746..fcc6e4b 100644 --- a/src/mynteye/api/processor/depth_processor.cc +++ b/src/mynteye/api/processor/depth_processor.cc @@ -21,7 +21,9 @@ MYNTEYE_BEGIN_NAMESPACE const char DepthProcessor::NAME[] = "DepthProcessor"; -DepthProcessor::DepthProcessor(std::int32_t proc_period) +DepthProcessor::DepthProcessor( + std::shared_ptr calib_infos, + std::int32_t proc_period) : Processor(std::move(proc_period)) { VLOG(2) << __func__ << ": proc_period=" << proc_period; } diff --git a/src/mynteye/api/processor/depth_processor.h b/src/mynteye/api/processor/depth_processor.h index c0e0aba..7660c73 100644 --- a/src/mynteye/api/processor/depth_processor.h +++ b/src/mynteye/api/processor/depth_processor.h @@ -18,6 +18,7 @@ #include #include "mynteye/api/processor.h" +#include "mynteye/api/processor/rectify_processor.h" MYNTEYE_BEGIN_NAMESPACE @@ -25,7 +26,9 @@ class DepthProcessor : public Processor { public: static const char NAME[]; - explicit DepthProcessor(std::int32_t proc_period = 0); + explicit DepthProcessor( + std::shared_ptr calib_infos, + std::int32_t proc_period = 0); virtual ~DepthProcessor(); std::string Name() override; diff --git a/src/mynteye/api/processor/points_processor.cc b/src/mynteye/api/processor/points_processor.cc index 3bb9524..4bf4d95 100644 --- a/src/mynteye/api/processor/points_processor.cc +++ b/src/mynteye/api/processor/points_processor.cc @@ -52,7 +52,9 @@ struct DepthTraits { const char PointsProcessor::NAME[] = "PointsProcessor"; -PointsProcessor::PointsProcessor(std::int32_t proc_period) +PointsProcessor::PointsProcessor( + std::shared_ptr calib_infos, + std::int32_t proc_period) : Processor(std::move(proc_period)) { VLOG(2) << __func__ << ": proc_period=" << proc_period; } @@ -95,7 +97,6 @@ bool PointsProcessor::OnProcess( int height = static_cast(output->value.rows); int width = static_cast(output->value.cols); for (int v = 0; v < height; ++v) { - cv::Vec3f *dptr = output->value.ptr(v); for (int u = 0; u < width; ++u) { float depth = input->value.at(v, u); diff --git a/src/mynteye/api/processor/points_processor.h b/src/mynteye/api/processor/points_processor.h index 5ca43f3..7dfa068 100644 --- a/src/mynteye/api/processor/points_processor.h +++ b/src/mynteye/api/processor/points_processor.h @@ -20,6 +20,7 @@ #include #include "mynteye/api/processor.h" +#include "mynteye/api/processor/rectify_processor.h" MYNTEYE_BEGIN_NAMESPACE @@ -27,7 +28,9 @@ class PointsProcessor : public Processor { public: static const char NAME[]; - explicit PointsProcessor(std::int32_t proc_period = 0); + explicit PointsProcessor( + std::shared_ptr calib_infos, + std::int32_t proc_period = 0); virtual ~PointsProcessor(); std::string Name() override; @@ -38,7 +41,6 @@ class PointsProcessor : public Processor { Object *const in, Object *const out, Processor *const parent) override; private: - cv::Mat Q_; }; MYNTEYE_END_NAMESPACE diff --git a/src/mynteye/api/processor/rectify_processor.cc b/src/mynteye/api/processor/rectify_processor.cc index 74c5aea..5e2b862 100644 --- a/src/mynteye/api/processor/rectify_processor.cc +++ b/src/mynteye/api/processor/rectify_processor.cc @@ -18,38 +18,10 @@ #include #include #include "mynteye/logger.h" -#include "mynteye/device/device.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -struct camera_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}; -}; +MYNTEYE_BEGIN_NAMESPACE -struct camera_mat_info_pair { - struct camera_info left; - struct camera_info right; -}; - -cv::Mat rectifyrad(const cv::Mat& R) { +cv::Mat RectifyProcessor::rectifyrad(const cv::Mat& R) { cv::Mat r_vec; cv::Rodrigues(R, r_vec); // pi/180 = x/179 ==> x = 3.1241 @@ -65,13 +37,12 @@ cv::Mat rectifyrad(const cv::Mat& R) { return R.clone(); } -void stereoRectify(camodocal::CameraPtr leftOdo, +void RectifyProcessor::stereoRectify(camodocal::CameraPtr leftOdo, camodocal::CameraPtr rightOdo, const CvMat* K1, const CvMat* K2, const CvMat* D1, const CvMat* D2, CvSize imageSize, const CvMat* matR, const CvMat* matT, CvMat* _R1, CvMat* _R2, CvMat* _P1, CvMat* _P2, - int flags = cv::CALIB_ZERO_DISPARITY, double alpha = -1, - CvSize newImgSize = cv::Size()) { + int flags, double alpha, CvSize newImgSize) { double _om[3], _t[3] = {0}, _uu[3]={0, 0, 0}, _r_r[3][3], _pp[3][4]; double _ww[3], _wr[3][3], _z[3] = {0, 0, 0}, _ri[3][3], _w3[3]; cv::Rect_ inner1, inner2, outer1, outer2; @@ -229,7 +200,7 @@ void stereoRectify(camodocal::CameraPtr leftOdo, } } -Eigen::Matrix4d loadT(const mynteye::Extrinsics& in) { +Eigen::Matrix4d RectifyProcessor::loadT(const mynteye::Extrinsics& in) { Eigen::Matrix3d R; R<< in.rotation[0][0], in.rotation[0][1], in.rotation[0][2], @@ -249,17 +220,18 @@ Eigen::Matrix4d loadT(const mynteye::Extrinsics& in) { return T; } -void loadCameraMatrix(cv::Mat& K, cv::Mat& D, // NOLINT +void RectifyProcessor::loadCameraMatrix(cv::Mat& K, cv::Mat& D, // NOLINT cv::Size& image_size, // NOLINT - struct camera_info& calib_data) { // NOLINT + struct camera_calib_info& 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_info getCalibMatData(const mynteye::IntrinsicsEquidistant& in) { - struct camera_info calib_mat_data; +struct camera_calib_info RectifyProcessor::getCalibMatData( + const mynteye::IntrinsicsEquidistant& in) { + struct camera_calib_info calib_mat_data; calib_mat_data.distortion_model = "KANNALA_BRANDT"; calib_mat_data.height = in.height; calib_mat_data.width = in.width; @@ -276,7 +248,7 @@ struct camera_info getCalibMatData(const mynteye::IntrinsicsEquidistant& in) { return calib_mat_data; } -struct camera_mat_info_pair stereoRectify( +std::shared_ptr RectifyProcessor::stereoRectify( camodocal::CameraPtr leftOdo, camodocal::CameraPtr rightOdo, mynteye::IntrinsicsEquidistant in_left, @@ -291,8 +263,8 @@ struct camera_mat_info_pair stereoRectify( cv::Mat K1, D1, K2, D2; cv::Size image_size1, image_size2; - struct camera_info calib_mat_data_left = getCalibMatData(in_left); - struct camera_info calib_mat_data_right = getCalibMatData(in_right); + struct camera_calib_info calib_mat_data_left = getCalibMatData(in_left); + struct camera_calib_info 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); @@ -335,11 +307,12 @@ struct camera_mat_info_pair stereoRectify( calib_mat_data_right.R[i*3 +j] = R2.at(i, j); } } - struct camera_mat_info_pair res = {calib_mat_data_left, calib_mat_data_right}; - return res; + struct camera_calib_info_pair res = + {calib_mat_data_left, calib_mat_data_right}; + return std::make_shared(res); } -camodocal::CameraPtr generateCameraFromIntrinsicsEquidistant( +camodocal::CameraPtr RectifyProcessor::generateCameraFromIntrinsicsEquidistant( const mynteye::IntrinsicsEquidistant & in) { camodocal::EquidistantCameraPtr camera( new camodocal::EquidistantCamera("KANNALA_BRANDT", @@ -356,8 +329,6 @@ camodocal::CameraPtr generateCameraFromIntrinsicsEquidistant( return camera; } -MYNTEYE_BEGIN_NAMESPACE - void RectifyProcessor::InitParams( IntrinsicsEquidistant in_left, IntrinsicsEquidistant in_right, @@ -367,7 +338,7 @@ void RectifyProcessor::InitParams( generateCameraFromIntrinsicsEquidistant(in_left); camodocal::CameraPtr camera_odo_ptr_right = generateCameraFromIntrinsicsEquidistant(in_right); - struct camera_mat_info_pair calib_info_pair = + calib_infos = stereoRectify(camera_odo_ptr_left, camera_odo_ptr_right, in_left, @@ -378,18 +349,18 @@ void RectifyProcessor::InitParams( cv::Mat::eye(3, 3, CV_32F), rect_R_r = cv::Mat::eye(3, 3, CV_32F); for (size_t i = 0; i < 3; i++) { for (size_t j = 0; j < 3; j++) { - rect_R_l.at(i, j) = calib_info_pair.left.R[i*3+j]; - rect_R_r.at(i, j) = calib_info_pair.right.R[i*3+j]; + rect_R_l.at(i, j) = calib_infos->left.R[i*3+j]; + rect_R_r.at(i, j) = calib_infos->right.R[i*3+j]; } } double left_f[] = - {calib_info_pair.left.P[0], calib_info_pair.left.P[5]}; + {calib_infos->left.P[0], calib_infos->left.P[5]}; double left_center[] = - {calib_info_pair.left.P[2], calib_info_pair.left.P[6]}; + {calib_infos->left.P[2], calib_infos->left.P[6]}; double right_f[] = - {calib_info_pair.right.P[0], calib_info_pair.right.P[5]}; + {calib_infos->right.P[0], calib_infos->right.P[5]}; double right_center[] = - {calib_info_pair.right.P[2], calib_info_pair.right.P[6]}; + {calib_infos->right.P[2], calib_infos->right.P[6]}; camera_odo_ptr_left->initUndistortRectifyMap( map11, map12, left_f[0], left_f[1], cv::Size(0, 0), left_center[0], diff --git a/src/mynteye/api/processor/rectify_processor.h b/src/mynteye/api/processor/rectify_processor.h index 1e2b054..782eb6d 100644 --- a/src/mynteye/api/processor/rectify_processor.h +++ b/src/mynteye/api/processor/rectify_processor.h @@ -22,9 +22,39 @@ #include "mynteye/types.h" #include "mynteye/api/processor.h" +#include "mynteye/device/device.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include 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; +}; + class Device; class RectifyProcessor : public Processor { @@ -45,6 +75,10 @@ class RectifyProcessor : public Processor { cv::Mat R1, P1, R2, P2, Q; cv::Mat map11, map12, map21, map22; + inline std::shared_ptr getCalibInfoPair() { + return calib_infos; + } + protected: Object *OnCreateOutput() override; bool OnProcess( @@ -54,10 +88,39 @@ class RectifyProcessor : public Processor { void InitParams(IntrinsicsEquidistant in_left, IntrinsicsEquidistant in_right, Extrinsics ex_right_to_left); + cv::Mat rectifyrad(const cv::Mat& R); + + void stereoRectify(camodocal::CameraPtr leftOdo, + camodocal::CameraPtr rightOdo, const CvMat* K1, const CvMat* K2, + const CvMat* D1, const CvMat* D2, CvSize imageSize, + const CvMat* matR, const CvMat* matT, + CvMat* _R1, CvMat* _R2, CvMat* _P1, CvMat* _P2, + int flags = cv::CALIB_ZERO_DISPARITY, double alpha = -1, + CvSize newImgSize = cv::Size()); + + 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 camera_calib_info getCalibMatData( + const mynteye::IntrinsicsEquidistant& in); + + std::shared_ptr stereoRectify( + camodocal::CameraPtr leftOdo, + camodocal::CameraPtr rightOdo, + mynteye::IntrinsicsEquidistant in_left, + mynteye::IntrinsicsEquidistant in_right, + mynteye::Extrinsics ex_right_to_left); + + camodocal::CameraPtr generateCameraFromIntrinsicsEquidistant( + const mynteye::IntrinsicsEquidistant & in); + std::shared_ptr intr_left_; std::shared_ptr intr_right_; std::shared_ptr extr_; 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 1455e0f..12c6d85 100644 --- a/src/mynteye/api/synthetic.cc +++ b/src/mynteye/api/synthetic.cc @@ -570,6 +570,9 @@ void Synthetic::DisableStreamData(const Stream &stream, std::uint32_t depth) { void Synthetic::InitProcessors() { std::shared_ptr rectify_processor = nullptr; +#ifdef WITH_CAM_MODELS + std::shared_ptr rectify_processor_imp = nullptr; +#endif cv::Mat Q; if (calib_model_ == CalibrationModel::PINHOLE) { auto &&rectify_processor_ocv = @@ -579,9 +582,10 @@ void Synthetic::InitProcessors() { Q = rectify_processor_ocv->Q; #ifdef WITH_CAM_MODELS } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) { - rectify_processor = + rectify_processor_imp = std::make_shared(intr_left_, intr_right_, extr_, RECTIFY_PROC_PERIOD); + rectify_processor = rectify_processor_imp; #endif } else { LOG(ERROR) << "Unknow calib model type in device: " @@ -603,6 +607,7 @@ void Synthetic::InitProcessors() { #ifdef WITH_CAM_MODELS } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) { points_processor = std::make_shared( + rectify_processor_imp -> getCalibInfoPair(), POINTS_PROC_PERIOD); #endif } else { @@ -614,7 +619,9 @@ void Synthetic::InitProcessors() { depth_processor = std::make_shared(DEPTH_PROC_PERIOD); #ifdef WITH_CAM_MODELS } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) { - depth_processor = std::make_shared(DEPTH_PROC_PERIOD); + depth_processor = std::make_shared( + rectify_processor_imp -> getCalibInfoPair(), + DEPTH_PROC_PERIOD); #endif } else { depth_processor = std::make_shared(DEPTH_PROC_PERIOD);