From 7b8ed810188291c41da31249c7950272f6502893 Mon Sep 17 00:00:00 2001 From: TinyOh Date: Tue, 15 Jan 2019 10:07:58 +0800 Subject: [PATCH 01/16] fix(bm matcher): add complie switch --- cmake/Option.cmake | 1 + include/mynteye/mynteye.h.in | 1 + src/mynteye/api/processor/depth_processor.cc | 2 +- .../api/processor/disparity_processor.cc | 45 ++++++++++++++++++- .../api/processor/disparity_processor.h | 1 - src/mynteye/api/synthetic.cc | 2 +- 6 files changed, 47 insertions(+), 5 deletions(-) diff --git a/cmake/Option.cmake b/cmake/Option.cmake index 7380672..00fc825 100644 --- a/cmake/Option.cmake +++ b/cmake/Option.cmake @@ -24,6 +24,7 @@ option(WITH_API "Build with API layer, need OpenCV" ON) option(WITH_DEVICE_INFO_REQUIRED "Build with device info required" ON) option(WITH_CAM_MODELS "Build with more camera models, WITH_API must be ON" OFF) +option(WITH_BM_SOBEL_FILTER "Build with bm and sobel filter, need OpenCV contronb" OFF) # 3rdparty components diff --git a/include/mynteye/mynteye.h.in b/include/mynteye/mynteye.h.in index bd2fafb..7872768 100644 --- a/include/mynteye/mynteye.h.in +++ b/include/mynteye/mynteye.h.in @@ -71,6 +71,7 @@ MYNTEYE_END_NAMESPACE #cmakedefine WITH_API #cmakedefine WITH_DEVICE_INFO_REQUIRED #cmakedefine WITH_CAM_MODELS +#cmakedefine WITH_BM_SOBEL_FILTER #cmakedefine WITH_OPENCV #cmakedefine WITH_OPENCV2 diff --git a/src/mynteye/api/processor/depth_processor.cc b/src/mynteye/api/processor/depth_processor.cc index ae8f5e5..9441132 100644 --- a/src/mynteye/api/processor/depth_processor.cc +++ b/src/mynteye/api/processor/depth_processor.cc @@ -51,7 +51,7 @@ 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 cv::Mat depth_mat = cv::Mat::zeros(rows, cols, CV_16U); for (int i = 0; i < rows; i++) { diff --git a/src/mynteye/api/processor/disparity_processor.cc b/src/mynteye/api/processor/disparity_processor.cc index dff775d..4454eab 100644 --- a/src/mynteye/api/processor/disparity_processor.cc +++ b/src/mynteye/api/processor/disparity_processor.cc @@ -17,6 +17,9 @@ #include #include +#ifdef WITH_BM_SOBEL_FILTER +#include +#endif #include "mynteye/logger.h" @@ -62,6 +65,7 @@ DisparityProcessor::DisparityProcessor(DisparityProcessorType type, sgbm_matcher->setSpeckleRange(32); sgbm_matcher->setDisp12MaxDiff(1); #endif +#ifdef WITH_BM_SOBEL_FILTER } else if (type_ == DisparityProcessorType::BM) { #ifdef WITH_OPENCV2 int bmWinSize = 3; @@ -90,9 +94,42 @@ DisparityProcessor::DisparityProcessor(DisparityProcessorType type, bm_matcher->setSpeckleWindowSize(100); bm_matcher->setSpeckleRange(4); bm_matcher->setPreFilterType(cv::StereoBM::PREFILTER_XSOBEL); +#endif #endif } else { - LOG(ERROR) << "no enum DisparityProcessorType" << static_cast(type); + LOG(ERROR) << "no enum DisparityProcessorType,use default sgbm"; + int sgbmWinSize = 3; + int numberOfDisparities = 64; + +#ifdef WITH_OPENCV2 + // StereoSGBM + // http://docs.opencv.org/2.4/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html?#stereosgbm + sgbm_matcher = cv::Ptr( + new cv::StereoSGBM( + 0, // minDisparity + numberOfDisparities, // numDisparities + sgbmWinSize, // SADWindowSize + 8 * sgbmWinSize * sgbmWinSize, // P1 + 32 * sgbmWinSize * sgbmWinSize, // P2 + 1, // disp12MaxDiff + 63, // preFilterCap + 10, // uniquenessRatio + 100, // speckleWindowSize + 32, // speckleRange + false)); // fullDP +#else + sgbm_matcher = cv::StereoSGBM::create(0, 16, 3); + sgbm_matcher->setPreFilterCap(63); + sgbm_matcher->setBlockSize(sgbmWinSize); + sgbm_matcher->setP1(8 * sgbmWinSize * sgbmWinSize); + sgbm_matcher->setP2(32 * sgbmWinSize * sgbmWinSize); + sgbm_matcher->setMinDisparity(0); + sgbm_matcher->setNumDisparities(numberOfDisparities); + sgbm_matcher->setUniquenessRatio(10); + sgbm_matcher->setSpeckleWindowSize(100); + sgbm_matcher->setSpeckleRange(32); + sgbm_matcher->setDisp12MaxDiff(1); +#endif } } @@ -138,12 +175,16 @@ bool DisparityProcessor::OnProcess( // whereas other algorithms output 32-bit floating-point disparity map. if (type_ == DisparityProcessorType::SGBM) { sgbm_matcher->compute(input->first, input->second, disparity); +#ifdef WITH_BM_SOBEL_FILTER } else if (type_ == DisparityProcessorType::BM) { - CvSize size = input->first.size(); cv::Mat tmp1, tmp2; cv::cvtColor(input->first, tmp1, CV_RGB2GRAY); cv::cvtColor(input->second, tmp2, CV_RGB2GRAY); bm_matcher->compute(tmp1, tmp2, disparity); +#endif + } else { + // default + sgbm_matcher->compute(input->first, input->second, disparity); } #endif disparity.convertTo(output->value, CV_32F, 1./16, 1); diff --git a/src/mynteye/api/processor/disparity_processor.h b/src/mynteye/api/processor/disparity_processor.h index f7fd952..3595185 100644 --- a/src/mynteye/api/processor/disparity_processor.h +++ b/src/mynteye/api/processor/disparity_processor.h @@ -16,7 +16,6 @@ #pragma once #include -#include #include "mynteye/api/processor.h" namespace cv { diff --git a/src/mynteye/api/synthetic.cc b/src/mynteye/api/synthetic.cc index ccd3c11..ca695fa 100644 --- a/src/mynteye/api/synthetic.cc +++ b/src/mynteye/api/synthetic.cc @@ -613,7 +613,7 @@ void Synthetic::InitProcessors() { rectify_processor = rectify_processor_ocv; } auto &&disparity_processor = - std::make_shared(DisparityProcessorType::BM, + std::make_shared(DisparityProcessorType::SGBM, DISPARITY_PROC_PERIOD); auto &&disparitynormalized_processor = std::make_shared( From c3698b6e40d0665da77833b7e3b5d150996d9757 Mon Sep 17 00:00:00 2001 From: TinyOh Date: Tue, 15 Jan 2019 10:16:29 +0800 Subject: [PATCH 02/16] fix(enum) fix DisparityProcessorType enum comment --- src/mynteye/api/processor/disparity_processor.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/mynteye/api/processor/disparity_processor.h b/src/mynteye/api/processor/disparity_processor.h index 3595185..026de30 100644 --- a/src/mynteye/api/processor/disparity_processor.h +++ b/src/mynteye/api/processor/disparity_processor.h @@ -27,9 +27,9 @@ class StereoBM; enum class DisparityProcessorType : std::uint8_t { /** bm */ SGBM = 0, - /** Equidistant: KANNALA_BRANDT */ + /** sgbm */ BM = 1, - /** Unknow */ + /** unknow */ UNKNOW }; From 41c882d0a7b8ed1c52908375e166e29672fe7b86 Mon Sep 17 00:00:00 2001 From: TinyOh Date: Tue, 15 Jan 2019 14:05:11 +0800 Subject: [PATCH 03/16] fix(process): opencv2 and remove some useless include --- src/mynteye/api/processor/disparity_processor.cc | 11 ++++++++--- src/mynteye/api/processor/rectify_processor.h | 3 --- 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/src/mynteye/api/processor/disparity_processor.cc b/src/mynteye/api/processor/disparity_processor.cc index 4454eab..b209291 100644 --- a/src/mynteye/api/processor/disparity_processor.cc +++ b/src/mynteye/api/processor/disparity_processor.cc @@ -160,10 +160,15 @@ bool DisparityProcessor::OnProcess( // It contains disparity values scaled by 16. So, to get the floating-point // disparity map, // you need to divide each disp element by 16. - if (type_ == SGBM) { + if (type_ == DisparityProcessorType::SGBM) { (*sgbm_matcher)(input->first, input->second, disparity); - } else if (type_ == BM) { - (*bm_matcher)(input->first, input->second, disparity); +#ifdef WITH_BM_SOBEL_FILTER + } else if (type_ == DisparityProcessorType::BM) { + cv::Mat tmp1, tmp2; + cv::cvtColor(input->first, tmp1, CV_RGB2GRAY); + cv::cvtColor(input->second, tmp2, CV_RGB2GRAY); + (*bm_matcher)(tmp1, tmp2, disparity); +#endif } #else // compute() diff --git a/src/mynteye/api/processor/rectify_processor.h b/src/mynteye/api/processor/rectify_processor.h index 147bc1e..7ebe2d8 100644 --- a/src/mynteye/api/processor/rectify_processor.h +++ b/src/mynteye/api/processor/rectify_processor.h @@ -25,9 +25,6 @@ #include "mynteye/device/device.h" #include #include -#include -#include -#include #include #include #include From cb49eea85e9ff9afcd1620a7d4a2c7c42717cc29 Mon Sep 17 00:00:00 2001 From: Osenberg Date: Tue, 15 Jan 2019 16:49:08 +0800 Subject: [PATCH 04/16] docs(readme): modified temp to temperature --- tools/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tools/README.md b/tools/README.md index 4d64abc..16b7436 100644 --- a/tools/README.md +++ b/tools/README.md @@ -69,7 +69,7 @@ roslaunch mynt_eye_ros_wrapper mynteye.launch ``` ```bash -rosbag record -O mynteye.bag /mynteye/left/image_raw /mynteye/imu/data_raw /mynteye/temp/data_raw +rosbag record -O mynteye.bag /mynteye/left/image_raw /mynteye/imu/data_raw /mynteye/temperature/data_raw ``` ## Analytics data (rosbag) From b3bc96f12b15c784c10cf16195691194db994b43 Mon Sep 17 00:00:00 2001 From: TinyOh Date: Tue, 15 Jan 2019 17:18:16 +0800 Subject: [PATCH 05/16] fix(ros): add default intrinsics in ros node --- .../api/processor/disparity_processor.cc | 14 +++- .../src/wrapper_nodelet.cc | 70 +++++++++++++++++-- 2 files changed, 75 insertions(+), 9 deletions(-) diff --git a/src/mynteye/api/processor/disparity_processor.cc b/src/mynteye/api/processor/disparity_processor.cc index b209291..18134af 100644 --- a/src/mynteye/api/processor/disparity_processor.cc +++ b/src/mynteye/api/processor/disparity_processor.cc @@ -23,6 +23,8 @@ #include "mynteye/logger.h" +#define WITH_BM_SOBEL_FILTER + MYNTEYE_BEGIN_NAMESPACE const char DisparityProcessor::NAME[] = "DisparityProcessor"; @@ -67,6 +69,7 @@ DisparityProcessor::DisparityProcessor(DisparityProcessorType type, #endif #ifdef WITH_BM_SOBEL_FILTER } else if (type_ == DisparityProcessorType::BM) { + int bmWinSize = 3; #ifdef WITH_OPENCV2 int bmWinSize = 3; // StereoBM @@ -86,7 +89,7 @@ DisparityProcessor::DisparityProcessor(DisparityProcessorType type, bm_matcher = cv::StereoBM::create(0, 3); bm_matcher->setPreFilterSize(9); bm_matcher->setPreFilterCap(31); - bm_matcher->setBlockSize(15); + bm_matcher->setBlockSize(bmWinSize); bm_matcher->setMinDisparity(0); bm_matcher->setNumDisparities(64); bm_matcher->setUniquenessRatio(15); @@ -183,8 +186,13 @@ bool DisparityProcessor::OnProcess( #ifdef WITH_BM_SOBEL_FILTER } else if (type_ == DisparityProcessorType::BM) { cv::Mat tmp1, tmp2; - cv::cvtColor(input->first, tmp1, CV_RGB2GRAY); - cv::cvtColor(input->second, tmp2, CV_RGB2GRAY); + if (input->first.channels() == 1) { + // s1030 + } else if (input->first.channels() == 3) { + // s210 + cv::cvtColor(input->first, tmp1, CV_RGB2GRAY); + cv::cvtColor(input->second, tmp2, CV_RGB2GRAY); + } bm_matcher->compute(tmp1, tmp2, disparity); #endif } else { diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc b/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc index 5929cf9..fd433e8 100644 --- a/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc +++ b/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc @@ -924,18 +924,71 @@ class ROSWrapperNodelet : public nodelet::Nodelet { computeRectTransforms(); } + std::shared_ptr getDefaultIntrinsics() { + auto res = std::make_shared(); + res->width = 640; + res->height = 400; + res->model = 0; + res->fx = 3.6220059643202876e+02; + res->fy = 3.6350065250745848e+02; + res->cx = 4.0658699068023441e+02; + res->cy = 2.3435161110061483e+02; + double codffs[5] = { + -2.5034765682756088e-01, + 5.0579399202897619e-02, + -7.0536676161976066e-04, + -8.5255451307033846e-03, + 0. + }; + for (unsigned int i = 0; i < 5; i++) { + res->coeffs[i] = codffs[i]; + } + return res; + } + + std::shared_ptr getDefaultExtrinsics() { + auto res = std::make_shared(); + double rotation[9] = { + 9.9867908939669447e-01, -6.3445566137485428e-03, 5.0988459509619687e-02, + 5.9890316389333252e-03, 9.9995670037792639e-01, 7.1224201868366971e-03, + -5.1031440326695092e-02, -6.8076406092671274e-03, 9.9867384471984544e-01 + }; + double translation[3] = {-1.2002489764113250e+02, -1.1782637409050747e+00, + -5.2058205159996538e+00}; + for (unsigned int i = 0; i < 3; i++) { + for (unsigned int j = 0; j < 3; j++) { + res->rotation[i][j] = rotation[i*3 + j]; + } + } + for (unsigned int i = 0; i < 3; i++) { + res->translation[i] = translation[i]; + } + return res; + } + + void computeRectTransforms() { ROS_ASSERT(api_); auto in_left_base = api_->GetIntrinsicsBase(Stream::LEFT); auto in_right_base = api_->GetIntrinsicsBase(Stream::RIGHT); - if (in_left_base->calib_model() != CalibrationModel::PINHOLE || - in_right_base->calib_model() != CalibrationModel::PINHOLE) { - return; + is_intrinsics_enable_ = in_left_base && in_right_base; + if (is_intrinsics_enable_) { + if (in_left_base->calib_model() != CalibrationModel::PINHOLE || + in_right_base->calib_model() != CalibrationModel::PINHOLE) { + return; + } + } else { + in_left_base = getDefaultIntrinsics(); + in_right_base = getDefaultIntrinsics(); } + auto in_left = *std::dynamic_pointer_cast(in_left_base); auto in_right = *std::dynamic_pointer_cast( in_right_base); auto ex_right_to_left = api_->GetExtrinsics(Stream::RIGHT, Stream::LEFT); + if (!is_intrinsics_enable_) { + ex_right_to_left = *(getDefaultExtrinsics()); + } cv::Size size{in_left.width, in_left.height}; cv::Mat M1 = @@ -975,10 +1028,14 @@ class ROSWrapperNodelet : public nodelet::Nodelet { camera_info_ptrs_[stream] = sensor_msgs::CameraInfoPtr(camera_info); std::shared_ptr in_base; - if (stream == Stream::RIGHT || stream == Stream::RIGHT_RECTIFIED) { - in_base = api_->GetIntrinsicsBase(Stream::RIGHT); + if (is_intrinsics_enable_) { + if (stream == Stream::RIGHT || stream == Stream::RIGHT_RECTIFIED) { + in_base = api_->GetIntrinsicsBase(Stream::RIGHT); + } else { + in_base = api_->GetIntrinsicsBase(Stream::LEFT); + } } else { - in_base = api_->GetIntrinsicsBase(Stream::LEFT); + in_base = getDefaultIntrinsics(); } camera_info->header.frame_id = frame_ids_[stream]; @@ -1275,6 +1332,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet { bool is_motion_published_; bool is_started_; int frame_rate_; + bool is_intrinsics_enable_; }; MYNTEYE_END_NAMESPACE From 2f087ce1f055aa87bc7da83c285ce8fbd78b62d3 Mon Sep 17 00:00:00 2001 From: kalman Date: Tue, 15 Jan 2019 19:21:14 +0800 Subject: [PATCH 06/16] fix(ros): set publish_imu_by_sync true --- wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc b/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc index fd433e8..869ef46 100644 --- a/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc +++ b/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc @@ -1327,7 +1327,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet { std::size_t imu_sync_count_ = 0; std::shared_ptr imu_accel_; std::shared_ptr imu_gyro_; - bool publish_imu_by_sync_ = false; + bool publish_imu_by_sync_ = true; std::map is_published_; bool is_motion_published_; bool is_started_; From 988a1584894ed1de159d7670d9e34d09f2fe0a69 Mon Sep 17 00:00:00 2001 From: kalman Date: Tue, 15 Jan 2019 19:24:57 +0800 Subject: [PATCH 07/16] fix(samples): delete get all info sample --- samples/tutorials/CMakeLists.txt | 4 -- .../intermediate/get_all_device_info.cc | 50 ------------------- 2 files changed, 54 deletions(-) delete mode 100644 samples/tutorials/intermediate/get_all_device_info.cc diff --git a/samples/tutorials/CMakeLists.txt b/samples/tutorials/CMakeLists.txt index 992c157..8e1eefa 100644 --- a/samples/tutorials/CMakeLists.txt +++ b/samples/tutorials/CMakeLists.txt @@ -127,10 +127,6 @@ make_executable2(ctrl_manual_exposure WITH_OPENCV ) -# intermediate level - -make_executable2(get_all_device_info SRCS intermediate/get_all_device_info.cc WITH_OPENCV) - if(PCL_FOUND) if(OpenCV_VERSION VERSION_LESS 4.0) diff --git a/samples/tutorials/intermediate/get_all_device_info.cc b/samples/tutorials/intermediate/get_all_device_info.cc deleted file mode 100644 index 1c335f7..0000000 --- a/samples/tutorials/intermediate/get_all_device_info.cc +++ /dev/null @@ -1,50 +0,0 @@ -// Copyright 2018 Slightech Co., Ltd. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -#include "mynteye/logger.h" -#include "mynteye/device/context.h" -#include "mynteye/device/device.h" - -MYNTEYE_USE_NAMESPACE - -int main(int argc, char *argv[]) { - glog_init _(argc, argv); - - LOG(INFO) << "Detecting MYNT EYE devices"; - Context context; - auto &&devices = context.devices(); - - size_t n = devices.size(); - if (n <= 0) { - LOG(ERROR) << "No MYNT EYE devices :("; - return 1; - } - - for (size_t i = 0; i < n; i++) { - auto &&device = devices[i]; - LOG(INFO) << "MYNT EYE device index: " << i; - LOG(INFO) << " Device name: " << device->GetInfo(Info::DEVICE_NAME); - LOG(INFO) << " Serial number: " << device->GetInfo(Info::SERIAL_NUMBER); - LOG(INFO) << " Firmware version: " - << device->GetInfo(Info::FIRMWARE_VERSION); - LOG(INFO) << " Hardware version: " - << device->GetInfo(Info::HARDWARE_VERSION); - LOG(INFO) << " Spec version: " << device->GetInfo(Info::SPEC_VERSION); - LOG(INFO) << " Lens type: " << device->GetInfo(Info::LENS_TYPE); - LOG(INFO) << " IMU type: " << device->GetInfo(Info::IMU_TYPE); - LOG(INFO) << " Nominal baseline: " - << device->GetInfo(Info::NOMINAL_BASELINE); - } - - return 0; -} From a281877285b8f84caff281a13f5ed293497c7254 Mon Sep 17 00:00:00 2001 From: Osenberg Date: Tue, 15 Jan 2019 19:35:42 +0800 Subject: [PATCH 08/16] fix(tools): compatible avatar and s --- tools/analytics/stamp_analytics.py | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/tools/analytics/stamp_analytics.py b/tools/analytics/stamp_analytics.py index 2aa93d0..b36d3fb 100644 --- a/tools/analytics/stamp_analytics.py +++ b/tools/analytics/stamp_analytics.py @@ -15,7 +15,6 @@ # limitations under the License. # pylint: disable=missing-docstring - from __future__ import print_function import os @@ -27,7 +26,6 @@ sys.path.append(os.path.join(TOOLBOX_DIR, 'internal')) # pylint: disable=import-error,wrong-import-position from data import ROSBag, MYNTEYE, What - ANGLE_DEGREES = 'd' ANGLE_RADIANS = 'r' ANGLE_UNITS = (ANGLE_DEGREES, ANGLE_RADIANS) @@ -38,6 +36,11 @@ BIN_IMU_NAME = 'stamp_analytics_imu.bin' RESULT_FIGURE = 'stamp_analytics.png' +IMU_ALL = 0 +IMU_ACCEL = 1 +IMU_GYRO = 2 + + class BinDataset(object): def __init__(self, path, dataset_creator): @@ -147,9 +150,9 @@ class BinDataset(object): imgs_t_diff = np.diff(imgs['t']) # imus_t_diff = np.diff(imus['t']) - accel = imus[imus['flag'] == 1] + accel = imus[(imus['flag'] == IMU_ALL) | (imus['flag'] == IMU_ACCEL)] accel_t_diff = np.diff(accel['t']) - gyro = imus[imus['flag'] == 2] + gyro = imus[(imus['flag'] == IMU_ALL) | (imus['flag'] == IMU_GYRO)] gyro_t_diff = np.diff(gyro['t']) print('\ncount') From 48509547a2ceda6f7f628efaafdd278f261a6607 Mon Sep 17 00:00:00 2001 From: kalman Date: Tue, 15 Jan 2019 21:20:23 +0800 Subject: [PATCH 09/16] fix(ros): fix imu count bug --- .../src/wrapper_nodelet.cc | 22 ++++++++++++------- 1 file changed, 14 insertions(+), 8 deletions(-) diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc b/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc index 869ef46..803c12f 100644 --- a/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc +++ b/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc @@ -72,14 +72,20 @@ class ROSWrapperNodelet : public nodelet::Nodelet { << (right_count_ / compute_time(time_end, right_time_beg_)); } if (imu_time_beg_ != -1) { - if (publish_imu_by_sync_) { - LOG(INFO) << "imu_sync_count: " << imu_sync_count_ << ", hz: " - << (imu_sync_count_ / - compute_time(time_end, imu_time_beg_)); - } else { - LOG(INFO) << "Imu count: " << imu_count_ << ", hz: " - << (imu_count_ / - compute_time(time_end, imu_time_beg_)); + if (model_ == Model::STANDARD) { + LOG(INFO) << "Imu count: " << imu_count_ << ", hz: " + << (imu_count_ / + compute_time(time_end, imu_time_beg_)); + } else { + if (publish_imu_by_sync_) { + LOG(INFO) << "imu_sync_count: " << imu_sync_count_ << ", hz: " + << (imu_sync_count_ / + compute_time(time_end, imu_time_beg_)); + } else { + LOG(INFO) << "Imu count: " << imu_count_ << ", hz: " + << (imu_count_ / + compute_time(time_end, imu_time_beg_)); + } } } From c42534d719b3c6d131666472079f6022786aa7a9 Mon Sep 17 00:00:00 2001 From: TinyOh Date: Wed, 16 Jan 2019 10:42:14 +0800 Subject: [PATCH 10/16] fix(ros): publish order --- wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc b/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc index fd433e8..b9d7102 100644 --- a/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc +++ b/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc @@ -412,7 +412,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet { } break; case Stream::RIGHT_RECTIFIED: { if (is_published_[Stream::LEFT_RECTIFIED]) { - SetIsPublished(Stream::RIGHT_RECTIFIED); + SetIsPublished(Stream::LEFT_RECTIFIED); } if (is_published_[Stream::DISPARITY]) { SetIsPublished(Stream::DISPARITY); From b4ddf5a81679c6d3f0f1f67fdfc2a22f4fc91f08 Mon Sep 17 00:00:00 2001 From: TinyOh Date: Wed, 16 Jan 2019 11:08:16 +0800 Subject: [PATCH 11/16] fix(synthetic): disable order --- src/mynteye/api/synthetic.cc | 29 ++++++++++++++++++++++------- 1 file changed, 22 insertions(+), 7 deletions(-) diff --git a/src/mynteye/api/synthetic.cc b/src/mynteye/api/synthetic.cc index ca695fa..e1329d0 100644 --- a/src/mynteye/api/synthetic.cc +++ b/src/mynteye/api/synthetic.cc @@ -534,13 +534,28 @@ void Synthetic::DisableStreamData(const Stream &stream, std::uint32_t depth) { } } break; case Stream::DISPARITY: { - if (IsStreamEnabledSynthetic(Stream::DISPARITY_NORMALIZED)) { - DisableStreamData(Stream::DISPARITY_NORMALIZED, depth + 1); + if (calib_model_ == CalibrationModel::PINHOLE) { + if (IsStreamEnabledSynthetic(Stream::DISPARITY_NORMALIZED)) { + DisableStreamData(Stream::DISPARITY_NORMALIZED, depth + 1); + } + if (IsStreamEnabledSynthetic(Stream::POINTS)) { + DisableStreamData(Stream::POINTS, depth + 1); + } + DeactivateProcessor(); +#ifdef WITH_CAM_MODELS + } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) { + if (IsStreamEnabledSynthetic(Stream::DISPARITY_NORMALIZED)) { + DisableStreamData(Stream::DISPARITY_NORMALIZED, depth + 1); + } + if (IsStreamEnabledSynthetic(Stream::DEPTH)) { + DisableStreamData(Stream::DEPTH, depth + 1); + } + DeactivateProcessor(); +#endif + } else { + LOG(ERROR) << "Unknow calib model type in device: " + << calib_model_; } - if (IsStreamEnabledSynthetic(Stream::POINTS)) { - DisableStreamData(Stream::POINTS, depth + 1); - } - DeactivateProcessor(); } break; case Stream::DISPARITY_NORMALIZED: { DeactivateProcessor(); @@ -565,7 +580,7 @@ void Synthetic::DisableStreamData(const Stream &stream, std::uint32_t depth) { DeactivateProcessor(); #ifdef WITH_CAM_MODELS } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) { - if (IsStreamEnabledSynthetic(Stream::DEPTH)) { + if (IsStreamEnabledSynthetic(Stream::POINTS)) { DisableStreamData(Stream::POINTS, depth + 1); } DeactivateProcessor(); From 47fb693b616c9e01df323e3b848eadf5f03c92d1 Mon Sep 17 00:00:00 2001 From: TinyOh Date: Wed, 16 Jan 2019 11:49:50 +0800 Subject: [PATCH 12/16] fix(disable order): fix disable order DISPARITY before RECTIFIED --- src/mynteye/api/synthetic.cc | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/mynteye/api/synthetic.cc b/src/mynteye/api/synthetic.cc index e1329d0..54963d3 100644 --- a/src/mynteye/api/synthetic.cc +++ b/src/mynteye/api/synthetic.cc @@ -496,12 +496,12 @@ void Synthetic::DisableStreamData(const Stream &stream, std::uint32_t depth) { stream_enabled_mode_.erase(stream); switch (stream) { case Stream::LEFT_RECTIFIED: { - if (IsStreamEnabledSynthetic(Stream::RIGHT_RECTIFIED)) { - DisableStreamData(Stream::RIGHT_RECTIFIED, depth + 1); - } if (IsStreamEnabledSynthetic(Stream::DISPARITY)) { DisableStreamData(Stream::DISPARITY, depth + 1); } + if (IsStreamEnabledSynthetic(Stream::RIGHT_RECTIFIED)) { + DisableStreamData(Stream::RIGHT_RECTIFIED, depth + 1); + } if (calib_model_ == CalibrationModel::PINHOLE) { DeactivateProcessor(); #ifdef WITH_CAM_MODELS @@ -515,12 +515,12 @@ void Synthetic::DisableStreamData(const Stream &stream, std::uint32_t depth) { } } break; case Stream::RIGHT_RECTIFIED: { - if (IsStreamEnabledSynthetic(Stream::LEFT_RECTIFIED)) { - DisableStreamData(Stream::LEFT_RECTIFIED, depth + 1); - } if (IsStreamEnabledSynthetic(Stream::DISPARITY)) { DisableStreamData(Stream::DISPARITY, depth + 1); } + if (IsStreamEnabledSynthetic(Stream::LEFT_RECTIFIED)) { + DisableStreamData(Stream::LEFT_RECTIFIED, depth + 1); + } if (calib_model_ == CalibrationModel::PINHOLE) { DeactivateProcessor(); #ifdef WITH_CAM_MODELS From c64fec21d7e9074dbd0c37b2f8791ed7991dd6fb Mon Sep 17 00:00:00 2001 From: kalman Date: Wed, 16 Jan 2019 15:20:13 +0800 Subject: [PATCH 13/16] fix(ros): add mutex_data_ in mono --- wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc | 2 ++ 1 file changed, 2 insertions(+) diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc b/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc index 9eae936..3f7b7cb 100644 --- a/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc +++ b/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc @@ -668,9 +668,11 @@ class ROSWrapperNodelet : public nodelet::Nodelet { header.seq = seq; header.stamp = stamp; header.frame_id = frame_ids_[stream]; + pthread_mutex_lock(&mutex_data_); cv::Mat mono; cv::cvtColor(data.frame, mono, CV_RGB2GRAY); auto &&msg = cv_bridge::CvImage(header, enc::MONO8, mono).toImageMsg(); + pthread_mutex_unlock(&mutex_data_); auto &&info = getCameraInfo(stream); info->header.stamp = msg->header.stamp; mono_publishers_[stream].publish(msg, info); From a80a72e78fe088f645216a86edca8b95acaa1033 Mon Sep 17 00:00:00 2001 From: kalman Date: Wed, 16 Jan 2019 19:22:57 +0800 Subject: [PATCH 14/16] docs(doc): update version --- CMakeLists.txt | 2 +- doc/en/api.doxyfile | 2 +- doc/zh-Hans/api.doxyfile | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 0605ead..67b6bd9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -14,7 +14,7 @@ cmake_minimum_required(VERSION 3.0) -project(mynteye VERSION 2.2.4 LANGUAGES C CXX) +project(mynteye VERSION 2.3.0 LANGUAGES C CXX) include(cmake/Common.cmake) diff --git a/doc/en/api.doxyfile b/doc/en/api.doxyfile index f7cb1b2..9a6bb26 100644 --- a/doc/en/api.doxyfile +++ b/doc/en/api.doxyfile @@ -38,7 +38,7 @@ PROJECT_NAME = "MYNT EYE S SDK" # could be handy for archiving the generated documentation or if some version # control system is used. -PROJECT_NUMBER = 2.2.4 +PROJECT_NUMBER = 2.3.0 # Using the PROJECT_BRIEF tag one can provide an optional one line description # for a project that appears at the top of each page and should give viewer a diff --git a/doc/zh-Hans/api.doxyfile b/doc/zh-Hans/api.doxyfile index 7b0ace2..312be3e 100644 --- a/doc/zh-Hans/api.doxyfile +++ b/doc/zh-Hans/api.doxyfile @@ -38,7 +38,7 @@ PROJECT_NAME = "MYNT EYE S SDK" # could be handy for archiving the generated documentation or if some version # control system is used. -PROJECT_NUMBER = 2.2.4 +PROJECT_NUMBER = 2.3.0 # Using the PROJECT_BRIEF tag one can provide an optional one line description # for a project that appears at the top of each page and should give viewer a From bfce242334787eeeb6cdabf2b6c3a0da7db5ba70 Mon Sep 17 00:00:00 2001 From: John Zhao Date: Wed, 16 Jan 2019 20:03:52 +0800 Subject: [PATCH 15/16] fix(option.cmake): find boost quiet --- cmake/Option.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/Option.cmake b/cmake/Option.cmake index 00fc825..9349320 100644 --- a/cmake/Option.cmake +++ b/cmake/Option.cmake @@ -44,7 +44,7 @@ else() endif() if(WITH_BOOST) - find_package(Boost COMPONENTS filesystem) + find_package(Boost QUIET COMPONENTS filesystem) if(Boost_FOUND) set(Boost_VERSION_STRING "${Boost_MAJOR_VERSION}.${Boost_MINOR_VERSION}.${Boost_SUBMINOR_VERSION}") set(WITH_FILESYSTEM TRUE) From 637d08984d57c633511077b8943b3bb62af1e9be Mon Sep 17 00:00:00 2001 From: John Zhao Date: Wed, 16 Jan 2019 20:04:33 +0800 Subject: [PATCH 16/16] build(makefile): do small change --- Makefile | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/Makefile b/Makefile index 5e47797..9e23713 100644 --- a/Makefile +++ b/Makefile @@ -27,7 +27,7 @@ MKFILE_DIR := $(patsubst %/,%,$(dir $(MKFILE_PATH))) # SUDO: sudo command # CAM_MODELS: cmake build with -DWITH_CAM_MODELS=ON # -# e.g. make [TARGET] SUOD= +# e.g. make [TARGET] SUDO= # e.g. make [TARGET] CAM_MODELS=1 SUDO ?= sudo @@ -35,7 +35,9 @@ SUDO ?= sudo CAM_MODELS ?= CMAKE_BUILD_EXTRA_OPTIONS := -ifneq ($(CAM_MODELS),) +ifeq ($(CAM_MODELS),) + CMAKE_BUILD_EXTRA_OPTIONS := $(CMAKE_BUILD_EXTRA_OPTIONS) -DWITH_CAM_MODELS=OFF +else CMAKE_BUILD_EXTRA_OPTIONS := $(CMAKE_BUILD_EXTRA_OPTIONS) -DWITH_CAM_MODELS=ON endif