From fbd416324e73c7552b64920091156b30021ae151 Mon Sep 17 00:00:00 2001 From: TinyOh Date: Thu, 10 Jan 2019 17:30:28 +0800 Subject: [PATCH 01/13] feat(calib models): float change to ushort ,fix the T_mul_f number --- src/mynteye/api/processor/depth_processor.cc | 9 ++++----- src/mynteye/api/processor/points_processor.cc | 13 ++++++------- src/mynteye/api/processor/rectify_processor.cc | 2 +- 3 files changed, 11 insertions(+), 13 deletions(-) diff --git a/src/mynteye/api/processor/depth_processor.cc b/src/mynteye/api/processor/depth_processor.cc index 10ce715..a1f9251 100644 --- a/src/mynteye/api/processor/depth_processor.cc +++ b/src/mynteye/api/processor/depth_processor.cc @@ -48,15 +48,14 @@ bool DepthProcessor::OnProcess( ObjMat *output = Object::Cast(out); int rows = input->value.rows; int cols = input->value.cols; - std::cout << calib_infos_->T_mul_f << std::endl; + // std::cout << calib_infos_->T_mul_f << std::endl; // 0.0793434 - // TODO(MYNTEYE): Put the corresponding parameters(T,f) - cv::Mat depth_mat = cv::Mat::zeros(rows, cols, CV_32F); + cv::Mat depth_mat = cv::Mat::zeros(rows, cols, CV_16U); for (int i = 0; i < rows; i++) { for (int j = 0; j < cols; j++) { float disparity_value = input->value.at(i, j); - float depth = calib_infos_->T_mul_f * 1000.0 / disparity_value ; - depth_mat.at(i, j) = depth; + float depth = calib_infos_->T_mul_f / disparity_value; + depth_mat.at(i, j) = depth * 1000; } } output->value = depth_mat; diff --git a/src/mynteye/api/processor/points_processor.cc b/src/mynteye/api/processor/points_processor.cc index e41c3f8..1d40ae1 100644 --- a/src/mynteye/api/processor/points_processor.cc +++ b/src/mynteye/api/processor/points_processor.cc @@ -32,7 +32,7 @@ struct DepthTraits { static inline bool valid(uint16_t depth) { return depth != 0; } static inline float toMeters(uint16_t depth) { return depth * 0.001f; } // originally mm static inline uint16_t fromMeters(float depth) { return (depth * 1000.0f) + 0.5f; } - static inline void initializeBuffer(std::vector& buffer) {} // Do nothing - already zero-filled + static inline void initializeBuffer(std::vector& buffer) {} // Do nothing - already zero-filled }; template<> @@ -100,16 +100,15 @@ bool PointsProcessor::OnProcess( 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); + float depth = input->value.at(v, u); // Missing points denoted by NaNs - if (!DepthTraits::valid(depth)) { + if (!DepthTraits::valid(depth)) { continue; } - - dptr[u][0] = (u - center_x) * depth * constant_x; - dptr[u][1] = (v - center_y) * depth * constant_y; - dptr[u][2] = DepthTraits::toMeters(depth); + dptr[u][0] = (u - center_x) * depth * constant_x * 1000.0; + dptr[u][1] = (v - center_y) * depth * constant_y * 1000.0; + dptr[u][2] = depth * 1000.0; } } diff --git a/src/mynteye/api/processor/rectify_processor.cc b/src/mynteye/api/processor/rectify_processor.cc index b246005..731935f 100644 --- a/src/mynteye/api/processor/rectify_processor.cc +++ b/src/mynteye/api/processor/rectify_processor.cc @@ -167,7 +167,7 @@ void RectifyProcessor::stereoRectify(camodocal::CameraPtr leftOdo, _pp[0][2] = cc_new[1].x; _pp[1][2] = cc_new[1].y; _pp[idx][3] = _t[idx]*fc_new; // baseline * focal length - *T_mul_f = 0. - _t[idx]; + *T_mul_f = 0. - _t[idx] * fc_new; cvConvert(&pp, _P2); alpha = MIN(alpha, 1.); From e2478c644adc50c6e57122c87b8398cbeeae271d Mon Sep 17 00:00:00 2001 From: kalman Date: Thu, 10 Jan 2019 18:11:34 +0800 Subject: [PATCH 02/13] fix(depth): add disparity limit --- src/mynteye/api/processor/depth_processor.cc | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/mynteye/api/processor/depth_processor.cc b/src/mynteye/api/processor/depth_processor.cc index a1f9251..f954db1 100644 --- a/src/mynteye/api/processor/depth_processor.cc +++ b/src/mynteye/api/processor/depth_processor.cc @@ -21,6 +21,9 @@ MYNTEYE_BEGIN_NAMESPACE const char DepthProcessor::NAME[] = "DepthProcessor"; +const int DISPARITY_MIN = 0; +const int DISPARITY_MAX = 64; + DepthProcessor::DepthProcessor( std::shared_ptr calib_infos, std::int32_t proc_period) @@ -54,8 +57,10 @@ bool DepthProcessor::OnProcess( for (int i = 0; i < rows; i++) { for (int j = 0; j < cols; j++) { float disparity_value = input->value.at(i, j); - float depth = calib_infos_->T_mul_f / disparity_value; - depth_mat.at(i, j) = depth * 1000; + if (disparity_value > DISPARITY_MAX && disparity_value > DISPARITY_MIN) { + float depth = calib_infos_->T_mul_f / disparity_value; + depth_mat.at(i, j) = depth * 1000; + } } } output->value = depth_mat; From 23775810b29213e8bf993074a50a050cc12baecf Mon Sep 17 00:00:00 2001 From: Osenberg Date: Thu, 10 Jan 2019 18:22:16 +0800 Subject: [PATCH 03/13] fix(channels.cc): compatible s_color and s --- src/mynteye/device/channel/channels.cc | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/mynteye/device/channel/channels.cc b/src/mynteye/device/channel/channels.cc index 6619cd7..0794aa7 100644 --- a/src/mynteye/device/channel/channels.cc +++ b/src/mynteye/device/channel/channels.cc @@ -758,11 +758,14 @@ std::set ChannelsAdapter::GetResolutionSupports() { std::set res; auto requests_map = stream_requests_map.at(model_); for (auto&& r_map : requests_map) { - if (r_map.first == Capabilities::STEREO || - r_map.first == Capabilities::STEREO_COLOR) { + if (r_map.first == Capabilities::STEREO) { for (auto&& r : r_map.second) { res.insert({r.width, r.height}); } + } else if (r_map.first == Capabilities::STEREO_COLOR) { + for (auto&& r : r_map.second) { + res.insert({static_cast(r.width / 2), r.height}); + } } } return res; From aca55759ada9286d0cfc77f867e66607f0c5e0fb Mon Sep 17 00:00:00 2001 From: kalman Date: Thu, 10 Jan 2019 19:22:25 +0800 Subject: [PATCH 04/13] fix(depth): fix mistake --- src/mynteye/api/processor/depth_processor.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/mynteye/api/processor/depth_processor.cc b/src/mynteye/api/processor/depth_processor.cc index f954db1..4f7e3c3 100644 --- a/src/mynteye/api/processor/depth_processor.cc +++ b/src/mynteye/api/processor/depth_processor.cc @@ -57,7 +57,7 @@ bool DepthProcessor::OnProcess( for (int i = 0; i < rows; i++) { for (int j = 0; j < cols; j++) { float disparity_value = input->value.at(i, j); - if (disparity_value > DISPARITY_MAX && disparity_value > DISPARITY_MIN) { + if (disparity_value < DISPARITY_MAX && disparity_value > DISPARITY_MIN) { float depth = calib_infos_->T_mul_f / disparity_value; depth_mat.at(i, j) = depth * 1000; } From beab7a155d343105e612c855bafba6c4ec1721f9 Mon Sep 17 00:00:00 2001 From: TinyOh Date: Fri, 11 Jan 2019 17:06:10 +0800 Subject: [PATCH 05/13] (fix): points cloud img ptr null --- src/mynteye/api/processor/points_processor.cc | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/mynteye/api/processor/points_processor.cc b/src/mynteye/api/processor/points_processor.cc index 1d40ae1..8811a99 100644 --- a/src/mynteye/api/processor/points_processor.cc +++ b/src/mynteye/api/processor/points_processor.cc @@ -111,7 +111,8 @@ bool PointsProcessor::OnProcess( dptr[u][2] = depth * 1000.0; } } - + output->id = input->id; + output->data = input->data; return true; } From cb135f45b5f506f2c2bb026247999f31ff18e7be Mon Sep 17 00:00:00 2001 From: TinyOh Date: Fri, 11 Jan 2019 17:11:02 +0800 Subject: [PATCH 06/13] fix(points cloud): show the points clopud in normal size --- src/mynteye/api/processor/points_processor.cc | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/mynteye/api/processor/points_processor.cc b/src/mynteye/api/processor/points_processor.cc index 8811a99..162f7f7 100644 --- a/src/mynteye/api/processor/points_processor.cc +++ b/src/mynteye/api/processor/points_processor.cc @@ -106,9 +106,9 @@ bool PointsProcessor::OnProcess( if (!DepthTraits::valid(depth)) { continue; } - dptr[u][0] = (u - center_x) * depth * constant_x * 1000.0; - dptr[u][1] = (v - center_y) * depth * constant_y * 1000.0; - dptr[u][2] = depth * 1000.0; + dptr[u][0] = (u - center_x) * depth * constant_x ; + dptr[u][1] = (v - center_y) * depth * constant_y ; + dptr[u][2] = depth ; } } output->id = input->id; From 7dc43ae4f93d2604e2e9e22aa7330f937d51d877 Mon Sep 17 00:00:00 2001 From: kalman Date: Fri, 11 Jan 2019 17:36:15 +0800 Subject: [PATCH 07/13] fix(depth): delete 1000 --- src/mynteye/api/processor/depth_processor.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/mynteye/api/processor/depth_processor.cc b/src/mynteye/api/processor/depth_processor.cc index 4f7e3c3..4107065 100644 --- a/src/mynteye/api/processor/depth_processor.cc +++ b/src/mynteye/api/processor/depth_processor.cc @@ -59,7 +59,7 @@ bool DepthProcessor::OnProcess( float disparity_value = input->value.at(i, j); if (disparity_value < DISPARITY_MAX && disparity_value > DISPARITY_MIN) { float depth = calib_infos_->T_mul_f / disparity_value; - depth_mat.at(i, j) = depth * 1000; + depth_mat.at(i, j) = depth; } } } From 9c8c2364751e6108725c2aa4dcf03cb0ae4848b3 Mon Sep 17 00:00:00 2001 From: kalman Date: Fri, 11 Jan 2019 19:55:12 +0800 Subject: [PATCH 08/13] fix(ros): rename rect topic name in rviz --- wrappers/ros/src/mynt_eye_ros_wrapper/rviz/mynteye.rviz | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/rviz/mynteye.rviz b/wrappers/ros/src/mynt_eye_ros_wrapper/rviz/mynteye.rviz index c448a18..987b405 100644 --- a/wrappers/ros/src/mynt_eye_ros_wrapper/rviz/mynteye.rviz +++ b/wrappers/ros/src/mynt_eye_ros_wrapper/rviz/mynteye.rviz @@ -82,7 +82,7 @@ Visualization Manager: Displays: - Class: rviz/Image Enabled: true - Image Topic: /mynteye/left/image_rect + Image Topic: /mynteye/left_rect/image_rect Max Value: 1 Median window: 5 Min Value: 0 @@ -94,7 +94,7 @@ Visualization Manager: Value: true - Class: rviz/Image Enabled: true - Image Topic: /mynteye/right/image_rect + Image Topic: /mynteye/right_rect/image_rect Max Value: 1 Median window: 5 Min Value: 0 From c8dbeeff222fb7e48a54d6c1de0a0c74736c3073 Mon Sep 17 00:00:00 2001 From: Osenberg Date: Fri, 11 Jan 2019 23:22:37 +0800 Subject: [PATCH 09/13] fix(device.cc): get img params bug --- src/mynteye/device/device.cc | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/mynteye/device/device.cc b/src/mynteye/device/device.cc index 372d62e..62b0cf6 100644 --- a/src/mynteye/device/device.cc +++ b/src/mynteye/device/device.cc @@ -640,7 +640,13 @@ void Device::UpdateStreamIntrinsics( for (auto &¶ms : all_img_params_) { auto &&img_res = params.first; auto &&img_params = params.second; - if (img_params.ok && img_res == request.GetResolution()) { + bool ok = false; + if (capability == Capabilities::STEREO_COLOR) { + ok = img_params.ok && img_res.width == request.GetResolution().width / 2; + } else if (capability == Capabilities::STEREO) { + ok = img_params.ok && img_res == request.GetResolution(); + } + if (ok) { SetIntrinsics(Stream::LEFT, img_params.in_left); SetIntrinsics(Stream::RIGHT, img_params.in_right); SetExtrinsics(Stream::LEFT, Stream::RIGHT, img_params.ex_right_to_left); From b38637c6d256463f2a668af6145721fd28d8a88d Mon Sep 17 00:00:00 2001 From: kalman Date: Sat, 12 Jan 2019 13:45:49 +0800 Subject: [PATCH 10/13] fix(mynteye.launch): add device options --- .../launch/mynteye.launch | 33 ++++++++++++++++++- 1 file changed, 32 insertions(+), 1 deletion(-) diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch index 524b9a4..17c38d5 100644 --- a/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch +++ b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch @@ -254,6 +254,22 @@ + + + + + + + + + + + + + + + + @@ -261,11 +277,26 @@ - + + + + + + + + + + + + + + + + From 0490c3328a62735e8501cedc4abace5d568e2c1e Mon Sep 17 00:00:00 2001 From: kalman Date: Sat, 12 Jan 2019 15:19:32 +0800 Subject: [PATCH 11/13] fix(api.h): remove mynteye/device.h --- include/mynteye/api/api.h | 1 - 1 file changed, 1 deletion(-) diff --git a/include/mynteye/api/api.h b/include/mynteye/api/api.h index f4ce9bb..2f2f9a2 100644 --- a/include/mynteye/api/api.h +++ b/include/mynteye/api/api.h @@ -25,7 +25,6 @@ #include "mynteye/mynteye.h" #include "mynteye/types.h" -#include "mynteye/device.h" MYNTEYE_BEGIN_NAMESPACE From 7746d2b1167a8996ab91a01be8a8c4bde4510b2e Mon Sep 17 00:00:00 2001 From: kalman Date: Sat, 12 Jan 2019 16:31:23 +0800 Subject: [PATCH 12/13] fix(device): fix intrinsic updating bug --- src/mynteye/device/device.cc | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/mynteye/device/device.cc b/src/mynteye/device/device.cc index 62b0cf6..6e1bd6e 100644 --- a/src/mynteye/device/device.cc +++ b/src/mynteye/device/device.cc @@ -642,7 +642,9 @@ void Device::UpdateStreamIntrinsics( auto &&img_params = params.second; bool ok = false; if (capability == Capabilities::STEREO_COLOR) { - ok = img_params.ok && img_res.width == request.GetResolution().width / 2; + ok = img_params.ok && + img_res.height == request.GetResolution().height && + img_res.width == request.GetResolution().width / 2; } else if (capability == Capabilities::STEREO) { ok = img_params.ok && img_res == request.GetResolution(); } From 883aef53419dc816a7807619a6f20f72dba4d74b Mon Sep 17 00:00:00 2001 From: kalman Date: Sat, 12 Jan 2019 18:53:14 +0800 Subject: [PATCH 13/13] fix(processor): fix reload image params bug --- .../api/processor/rectify_processor.cc | 19 +++++++++++-------- src/mynteye/api/processor/rectify_processor.h | 8 ++++---- .../api/processor/rectify_processor_ocv.cc | 19 +++++++++++-------- .../api/processor/rectify_processor_ocv.h | 9 ++++----- src/mynteye/api/synthetic.cc | 10 +++++++--- 5 files changed, 37 insertions(+), 28 deletions(-) diff --git a/src/mynteye/api/processor/rectify_processor.cc b/src/mynteye/api/processor/rectify_processor.cc index 731935f..cf34e73 100644 --- a/src/mynteye/api/processor/rectify_processor.cc +++ b/src/mynteye/api/processor/rectify_processor.cc @@ -381,11 +381,11 @@ RectifyProcessor::RectifyProcessor( std::int32_t proc_period) : Processor(std::move(proc_period)), calib_model(CalibrationModel::UNKNOW) { - intr_left_ = intr_left; - intr_right_ = intr_right; - extr_ = extr; VLOG(2) << __func__ << ": proc_period=" << proc_period; - NotifyImageParamsChanged(); + InitParams( + *std::dynamic_pointer_cast(intr_left), + *std::dynamic_pointer_cast(intr_right), + *extr); } RectifyProcessor::~RectifyProcessor() { @@ -396,11 +396,14 @@ std::string RectifyProcessor::Name() { return NAME; } -void RectifyProcessor::NotifyImageParamsChanged() { +void RectifyProcessor::ReloadImageParams( + std::shared_ptr intr_left, + std::shared_ptr intr_right, + std::shared_ptr extr) { InitParams( - *std::dynamic_pointer_cast(intr_left_), - *std::dynamic_pointer_cast(intr_right_), - *extr_); + *std::dynamic_pointer_cast(intr_left), + *std::dynamic_pointer_cast(intr_right), + *extr); } Object *RectifyProcessor::OnCreateOutput() { diff --git a/src/mynteye/api/processor/rectify_processor.h b/src/mynteye/api/processor/rectify_processor.h index 046b186..9721a38 100644 --- a/src/mynteye/api/processor/rectify_processor.h +++ b/src/mynteye/api/processor/rectify_processor.h @@ -71,7 +71,10 @@ class RectifyProcessor : public Processor { std::string Name() override; - void NotifyImageParamsChanged(); + void ReloadImageParams( + std::shared_ptr intr_left, + std::shared_ptr intr_right, + std::shared_ptr extr); cv::Mat R1, P1, R2, P2, Q; cv::Mat map11, map12, map21, map22; @@ -117,9 +120,6 @@ class RectifyProcessor : public Processor { 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; }; diff --git a/src/mynteye/api/processor/rectify_processor_ocv.cc b/src/mynteye/api/processor/rectify_processor_ocv.cc index f662cd7..5decbb1 100644 --- a/src/mynteye/api/processor/rectify_processor_ocv.cc +++ b/src/mynteye/api/processor/rectify_processor_ocv.cc @@ -32,10 +32,10 @@ RectifyProcessorOCV::RectifyProcessorOCV( : Processor(std::move(proc_period)), calib_model(CalibrationModel::UNKNOW) { VLOG(2) << __func__ << ": proc_period=" << proc_period; - intr_left_ = intr_left; - intr_right_ = intr_right; - extr_ = extr; - NotifyImageParamsChanged(); + InitParams( + *std::dynamic_pointer_cast(intr_left), + *std::dynamic_pointer_cast(intr_right), + *extr); } RectifyProcessorOCV::~RectifyProcessorOCV() { @@ -46,11 +46,14 @@ std::string RectifyProcessorOCV::Name() { return NAME; } -void RectifyProcessorOCV::NotifyImageParamsChanged() { +void RectifyProcessorOCV::ReloadImageParams( + std::shared_ptr intr_left, + std::shared_ptr intr_right, + std::shared_ptr extr) { InitParams( - *std::dynamic_pointer_cast(intr_left_), - *std::dynamic_pointer_cast(intr_right_), - *extr_); + *std::dynamic_pointer_cast(intr_left), + *std::dynamic_pointer_cast(intr_right), + *extr); } Object *RectifyProcessorOCV::OnCreateOutput() { diff --git a/src/mynteye/api/processor/rectify_processor_ocv.h b/src/mynteye/api/processor/rectify_processor_ocv.h index f9ea949..e67c4ae 100644 --- a/src/mynteye/api/processor/rectify_processor_ocv.h +++ b/src/mynteye/api/processor/rectify_processor_ocv.h @@ -40,7 +40,10 @@ class RectifyProcessorOCV : public Processor { std::string Name() override; - void NotifyImageParamsChanged(); + void ReloadImageParams( + std::shared_ptr intr_left, + std::shared_ptr intr_right, + std::shared_ptr extr); cv::Mat R1, P1, R2, P2, Q; cv::Mat map11, map12, map21, map22; @@ -54,10 +57,6 @@ class RectifyProcessorOCV : public Processor { void InitParams(IntrinsicsPinhole in_left, IntrinsicsPinhole in_right, Extrinsics ex_right_to_left); - std::shared_ptr intr_left_; - std::shared_ptr intr_right_; - std::shared_ptr extr_; - CalibrationModel calib_model; }; diff --git a/src/mynteye/api/synthetic.cc b/src/mynteye/api/synthetic.cc index 4265a76..ef5c14d 100644 --- a/src/mynteye/api/synthetic.cc +++ b/src/mynteye/api/synthetic.cc @@ -111,19 +111,23 @@ Synthetic::~Synthetic() { } void Synthetic::NotifyImageParamsChanged() { + intr_left_ = api_->GetIntrinsicsBase(Stream::LEFT); + intr_right_ = api_->GetIntrinsicsBase(Stream::RIGHT); + extr_ = std::make_shared( + api_->GetExtrinsics(Stream::LEFT, Stream::RIGHT)); if (calib_model_ == CalibrationModel::PINHOLE) { auto &&processor = find_processor(processor_); - if (processor) processor->NotifyImageParamsChanged(); + if (processor) processor->ReloadImageParams(intr_left_, intr_right_, extr_); #ifdef WITH_CAM_MODELS } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) { auto &&processor = find_processor(processor_); - if (processor) processor->NotifyImageParamsChanged(); + if (processor) processor->ReloadImageParams(intr_left_, intr_right_, extr_); #endif } else { LOG(ERROR) << "Unknow calib model type in device: " << calib_model_ << ", use default pinhole model"; auto &&processor = find_processor(processor_); - if (processor) processor->NotifyImageParamsChanged(); + if (processor) processor->ReloadImageParams(intr_left_, intr_right_, extr_); } }