From f92ecd437133340320373f7e6e2c1a23481d5cd5 Mon Sep 17 00:00:00 2001 From: Osenberg-Y Date: Thu, 9 Aug 2018 12:14:54 +0800 Subject: [PATCH 01/10] optimized pointscloud --- src/api/processor/disparity_processor.cc | 72 +++++++----------------- src/api/processor/disparity_processor.h | 6 +- 2 files changed, 26 insertions(+), 52 deletions(-) diff --git a/src/api/processor/disparity_processor.cc b/src/api/processor/disparity_processor.cc index dbb5ab3..83e7349 100644 --- a/src/api/processor/disparity_processor.cc +++ b/src/api/processor/disparity_processor.cc @@ -26,38 +26,26 @@ const char DisparityProcessor::NAME[] = "DisparityProcessor"; DisparityProcessor::DisparityProcessor(std::int32_t proc_period) : Processor(std::move(proc_period)) { VLOG(2) << __func__ << ": proc_period=" << proc_period; - int sgbmWinSize = 3; - int numberOfDisparities = 64; + int blockSize_ = 15; // 15 + int minDisparity_ = 0; // 0 + int numDisparities_ = 64; // 64 + int preFilterSize_ = 9; // 9 + int preFilterCap_ = 31; // 31 + int uniquenessRatio_ = 15; // 15 + int textureThreshold_ = 10; // 10 + int speckleWindowSize_ = 100; // 100 + int speckleRange_ = 4; // 4 -#ifdef USE_OPENCV2 - // StereoSGBM - // http://docs.opencv.org/2.4/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html?#stereosgbm - sgbm_ = 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_ = cv::StereoSGBM::create(0, 16, 3); - sgbm_->setPreFilterCap(63); - sgbm_->setBlockSize(sgbmWinSize); - sgbm_->setP1(8 * sgbmWinSize * sgbmWinSize); - sgbm_->setP2(32 * sgbmWinSize * sgbmWinSize); - sgbm_->setMinDisparity(0); - sgbm_->setNumDisparities(numberOfDisparities); - sgbm_->setUniquenessRatio(10); - sgbm_->setSpeckleWindowSize(100); - sgbm_->setSpeckleRange(32); - sgbm_->setDisp12MaxDiff(1); -#endif + bm_ = cv::StereoBM::create(); + bm_->setBlockSize(blockSize_); + bm_->setMinDisparity(minDisparity_); + bm_->setNumDisparities(numDisparities_); + bm_->setPreFilterSize(preFilterSize_); + bm_->setPreFilterCap(preFilterCap_); + bm_->setUniquenessRatio(uniquenessRatio_); + bm_->setTextureThreshold(textureThreshold_); + bm_->setSpeckleWindowSize(speckleWindowSize_); + bm_->setSpeckleRange(speckleRange_); } DisparityProcessor::~DisparityProcessor() { @@ -79,26 +67,8 @@ bool DisparityProcessor::OnProcess( ObjMat *output = Object::Cast(out); cv::Mat disparity; -#ifdef USE_OPENCV2 - // StereoSGBM::operator() - // http://docs.opencv.org/2.4/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html#stereosgbm-operator - // Output disparity map. It is a 16-bit signed single-channel image of the - // same size as the input image. - // It contains disparity values scaled by 16. So, to get the floating-point - // disparity map, - // you need to divide each disp element by 16. - (*sgbm_)(input->first, input->second, disparity); -#else - // compute() - // http://docs.opencv.org/master/d2/d6e/classcv_1_1StereoMatcher.html - // Output disparity map. It has the same size as the input images. - // Some algorithms, like StereoBM or StereoSGBM compute 16-bit fixed-point - // disparity map - // (where each disparity value has 4 fractional bits), - // whereas other algorithms output 32-bit floating-point disparity map. - sgbm_->compute(input->first, input->second, disparity); -#endif - output->value = disparity / 16 + 1; + bm_->compute(input->first, input->second, disparity); + output->value = disparity; return true; } diff --git a/src/api/processor/disparity_processor.h b/src/api/processor/disparity_processor.h index 82b075f..da4f78b 100644 --- a/src/api/processor/disparity_processor.h +++ b/src/api/processor/disparity_processor.h @@ -17,13 +17,17 @@ #include +#include + #include "api/processor/processor.h" +#if 0 namespace cv { class StereoSGBM; } // namespace cv +#endif MYNTEYE_BEGIN_NAMESPACE @@ -42,7 +46,7 @@ class DisparityProcessor : public Processor { Object *const in, Object *const out, Processor *const parent) override; private: - cv::Ptr sgbm_; + cv::Ptr bm_; }; MYNTEYE_END_NAMESPACE From 5677aa56b244d13aca4749bd22f82f7742abddb7 Mon Sep 17 00:00:00 2001 From: Osenberg-Y Date: Fri, 10 Aug 2018 15:55:22 +0800 Subject: [PATCH 02/10] Made some optimization about pointscloud --- samples/tutorials/util/pc_viewer.cc | 1 + src/api/processor/disparity_processor.cc | 3 +- src/api/processor/points_processor.cc | 37 +++++++++++++++++++++++- src/api/processor/rectify_processor.cc | 4 +++ 4 files changed, 43 insertions(+), 2 deletions(-) diff --git a/samples/tutorials/util/pc_viewer.cc b/samples/tutorials/util/pc_viewer.cc index 2fb7394..13d6c00 100644 --- a/samples/tutorials/util/pc_viewer.cc +++ b/samples/tutorials/util/pc_viewer.cc @@ -84,6 +84,7 @@ void PCViewer::ConvertMatToPointCloud( for (int i = 0; i < xyz.rows; i++) { for (int j = 0; j < xyz.cols; j++) { auto &&p = xyz.at(i, j); + if (std::abs(p.z) > 9999) continue; if (std::isfinite(p.x) && std::isfinite(p.y) && std::isfinite(p.z)) { // LOG(INFO) << "[" << i << "," << j << "] x: " << p.x << ", y: " << p.y // << ", z: " << p.z; diff --git a/src/api/processor/disparity_processor.cc b/src/api/processor/disparity_processor.cc index 83e7349..2f4ccda 100644 --- a/src/api/processor/disparity_processor.cc +++ b/src/api/processor/disparity_processor.cc @@ -26,6 +26,7 @@ const char DisparityProcessor::NAME[] = "DisparityProcessor"; DisparityProcessor::DisparityProcessor(std::int32_t proc_period) : Processor(std::move(proc_period)) { VLOG(2) << __func__ << ": proc_period=" << proc_period; + int blockSize_ = 15; // 15 int minDisparity_ = 0; // 0 int numDisparities_ = 64; // 64 @@ -68,7 +69,7 @@ bool DisparityProcessor::OnProcess( cv::Mat disparity; bm_->compute(input->first, input->second, disparity); - output->value = disparity; + disparity.convertTo(output->value, CV_32F, 1./16); return true; } diff --git a/src/api/processor/points_processor.cc b/src/api/processor/points_processor.cc index 0d594b1..529c547 100644 --- a/src/api/processor/points_processor.cc +++ b/src/api/processor/points_processor.cc @@ -45,7 +45,42 @@ bool PointsProcessor::OnProcess( UNUSED(parent) const ObjMat *input = Object::Cast(in); ObjMat *output = Object::Cast(out); - cv::reprojectImageTo3D(input->value, output->value, Q_, true); + // cv::reprojectImageTo3D(input->value, output->value, Q_, true, -1); + bool handleMissingValues = true; + cv::Mat disparity = input->value; + output->value.create(disparity.size(), CV_MAKETYPE(CV_32FC3, 3)); + cv::Mat _3dImage = output->value; + + const float bigZ = 10000.f; + cv::Matx44d Q; + Q_.convertTo(Q, CV_64F); + + int x, cols = disparity.cols; + CV_Assert(cols >= 0); + + double minDisparity = FLT_MAX; + + if (handleMissingValues) { + cv::minMaxIdx(disparity, &minDisparity, 0, 0, 0); + } + + for (int y = 0; y < disparity.rows; y++) { + + float *sptr = disparity.ptr(y); + cv::Vec3f *dptr = _3dImage.ptr(y); + + for( x = 0; x < cols; x++) + { + double d = sptr[x]; + cv::Vec4d homg_pt = Q*cv::Vec4d(x, y, d, 1.0); + dptr[x] = cv::Vec3d(homg_pt.val); + dptr[x] /= homg_pt[3]; + + if( fabs(d-minDisparity) <= FLT_EPSILON ) { + dptr[x][2] = bigZ; + } + } + } return true; } diff --git a/src/api/processor/rectify_processor.cc b/src/api/processor/rectify_processor.cc index 31ce62c..ba5b3b2 100644 --- a/src/api/processor/rectify_processor.cc +++ b/src/api/processor/rectify_processor.cc @@ -69,12 +69,16 @@ void RectifyProcessor::InitParams( in_right.cy, 0, 0, 1); cv::Mat D1(1, 5, CV_64F, in_left.coeffs); cv::Mat D2(1, 5, CV_64F, in_right.coeffs); + /* cv::Mat R = (cv::Mat_(3, 3) << ex_right_to_left.rotation[0][0], ex_right_to_left.rotation[0][1], ex_right_to_left.rotation[0][2], ex_right_to_left.rotation[1][0], ex_right_to_left.rotation[1][1], ex_right_to_left.rotation[1][2], ex_right_to_left.rotation[2][0], ex_right_to_left.rotation[2][1], ex_right_to_left.rotation[2][2]); + */ + cv::Mat R = + (cv::Mat_(3, 3) << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0); cv::Mat T(3, 1, CV_64F, ex_right_to_left.translation); VLOG(2) << "InitParams size: " << size; From 23e28e20a606c087099902427b3f50b06cb6522e Mon Sep 17 00:00:00 2001 From: Osenberg-Y Date: Mon, 13 Aug 2018 18:42:53 +0800 Subject: [PATCH 03/10] Made some modification of code style and perfect function --- src/api/processor/disparity_processor.cc | 17 +++++++++++++++-- src/api/processor/disparity_processor.h | 4 +--- src/api/processor/points_processor.cc | 9 +++------ 3 files changed, 19 insertions(+), 11 deletions(-) diff --git a/src/api/processor/disparity_processor.cc b/src/api/processor/disparity_processor.cc index 2f4ccda..cea855a 100644 --- a/src/api/processor/disparity_processor.cc +++ b/src/api/processor/disparity_processor.cc @@ -28,8 +28,16 @@ DisparityProcessor::DisparityProcessor(std::int32_t proc_period) VLOG(2) << __func__ << ": proc_period=" << proc_period; int blockSize_ = 15; // 15 - int minDisparity_ = 0; // 0 int numDisparities_ = 64; // 64 + +#ifdef USE_OPENCV2 + bm_ = cv::Ptr( + new cv::StereoBM( + cv::StereoBM::BASIC_PRESET, + numDisparities_, + blockSize_)); +#else + int minDisparity_ = 0; // 0 int preFilterSize_ = 9; // 9 int preFilterCap_ = 31; // 31 int uniquenessRatio_ = 15; // 15 @@ -37,7 +45,7 @@ DisparityProcessor::DisparityProcessor(std::int32_t proc_period) int speckleWindowSize_ = 100; // 100 int speckleRange_ = 4; // 4 - bm_ = cv::StereoBM::create(); + bm_ = cv::StereoBM::create(16, 9); bm_->setBlockSize(blockSize_); bm_->setMinDisparity(minDisparity_); bm_->setNumDisparities(numDisparities_); @@ -47,6 +55,7 @@ DisparityProcessor::DisparityProcessor(std::int32_t proc_period) bm_->setTextureThreshold(textureThreshold_); bm_->setSpeckleWindowSize(speckleWindowSize_); bm_->setSpeckleRange(speckleRange_); +#endif } DisparityProcessor::~DisparityProcessor() { @@ -68,7 +77,11 @@ bool DisparityProcessor::OnProcess( ObjMat *output = Object::Cast(out); cv::Mat disparity; +#ifdef USE_OPENCV2 + (*bm_)(input->first, input->second, disparity); +#else bm_->compute(input->first, input->second, disparity); +#endif disparity.convertTo(output->value, CV_32F, 1./16); return true; } diff --git a/src/api/processor/disparity_processor.h b/src/api/processor/disparity_processor.h index da4f78b..6a92ce6 100644 --- a/src/api/processor/disparity_processor.h +++ b/src/api/processor/disparity_processor.h @@ -21,13 +21,11 @@ #include "api/processor/processor.h" -#if 0 namespace cv { -class StereoSGBM; +class StereoBM; } // namespace cv -#endif MYNTEYE_BEGIN_NAMESPACE diff --git a/src/api/processor/points_processor.cc b/src/api/processor/points_processor.cc index 529c547..563beb8 100644 --- a/src/api/processor/points_processor.cc +++ b/src/api/processor/points_processor.cc @@ -45,8 +45,7 @@ bool PointsProcessor::OnProcess( UNUSED(parent) const ObjMat *input = Object::Cast(in); ObjMat *output = Object::Cast(out); - // cv::reprojectImageTo3D(input->value, output->value, Q_, true, -1); - bool handleMissingValues = true; + cv::Mat disparity = input->value; output->value.create(disparity.size(), CV_MAKETYPE(CV_32FC3, 3)); cv::Mat _3dImage = output->value; @@ -60,12 +59,9 @@ bool PointsProcessor::OnProcess( double minDisparity = FLT_MAX; - if (handleMissingValues) { - cv::minMaxIdx(disparity, &minDisparity, 0, 0, 0); - } + cv::minMaxIdx(disparity, &minDisparity, 0, 0, 0); for (int y = 0; y < disparity.rows; y++) { - float *sptr = disparity.ptr(y); cv::Vec3f *dptr = _3dImage.ptr(y); @@ -81,6 +77,7 @@ bool PointsProcessor::OnProcess( } } } + return true; } From 94fd56a65d0f72edddd5c84b13b77f714175e8c7 Mon Sep 17 00:00:00 2001 From: Osenberg-Y Date: Mon, 13 Aug 2018 18:49:59 +0800 Subject: [PATCH 04/10] code style --- src/api/processor/points_processor.cc | 47 +++++++++++++-------------- 1 file changed, 23 insertions(+), 24 deletions(-) diff --git a/src/api/processor/points_processor.cc b/src/api/processor/points_processor.cc index 563beb8..9b2fc76 100644 --- a/src/api/processor/points_processor.cc +++ b/src/api/processor/points_processor.cc @@ -46,37 +46,36 @@ bool PointsProcessor::OnProcess( const ObjMat *input = Object::Cast(in); ObjMat *output = Object::Cast(out); - cv::Mat disparity = input->value; - output->value.create(disparity.size(), CV_MAKETYPE(CV_32FC3, 3)); - cv::Mat _3dImage = output->value; + cv::Mat disparity = input->value; + output->value.create(disparity.size(), CV_MAKETYPE(CV_32FC3, 3)); + cv::Mat _3dImage = output->value; - const float bigZ = 10000.f; - cv::Matx44d Q; - Q_.convertTo(Q, CV_64F); + const float bigZ = 10000.f; + cv::Matx44d Q; + Q_.convertTo(Q, CV_64F); - int x, cols = disparity.cols; - CV_Assert(cols >= 0); + int x, cols = disparity.cols; + CV_Assert(cols >= 0); - double minDisparity = FLT_MAX; + double minDisparity = FLT_MAX; - cv::minMaxIdx(disparity, &minDisparity, 0, 0, 0); + cv::minMaxIdx(disparity, &minDisparity, 0, 0, 0); - for (int y = 0; y < disparity.rows; y++) { - float *sptr = disparity.ptr(y); - cv::Vec3f *dptr = _3dImage.ptr(y); + for (int y = 0; y < disparity.rows; y++) { + float *sptr = disparity.ptr(y); + cv::Vec3f *dptr = _3dImage.ptr(y); - for( x = 0; x < cols; x++) - { - double d = sptr[x]; - cv::Vec4d homg_pt = Q*cv::Vec4d(x, y, d, 1.0); - dptr[x] = cv::Vec3d(homg_pt.val); - dptr[x] /= homg_pt[3]; + for (x = 0; x < cols; x++) { + double d = sptr[x]; + cv::Vec4d homg_pt = Q * cv::Vec4d(x, y, d, 1.0); + dptr[x] = cv::Vec3d(homg_pt.val); + dptr[x] /= homg_pt[3]; - if( fabs(d-minDisparity) <= FLT_EPSILON ) { - dptr[x][2] = bigZ; - } - } - } + if (fabs(d - minDisparity) <= FLT_EPSILON) { + dptr[x][2] = bigZ; + } + } + } return true; } From c3fc4ba3ba743729c8e658007156f0409675c64c Mon Sep 17 00:00:00 2001 From: Osenberg-Y Date: Tue, 14 Aug 2018 14:47:26 +0800 Subject: [PATCH 05/10] Fixed no pointcloud on ros --- 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 64dd7b9..8855d4a 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 @@ -324,7 +324,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet { is_published_[stream] = true; } - if (camera_publishers_[Stream::POINTS].getNumSubscribers() > 0 && + if (points_publisher_.getNumSubscribers() > 0 && !is_published_[Stream::POINTS]) { api_->SetStreamCallback( Stream::POINTS, [this](const api::StreamData &data) { From 97701188b8ed5b83e3b801d39b0a827493451296 Mon Sep 17 00:00:00 2001 From: kalman Date: Fri, 7 Dec 2018 16:00:29 +0800 Subject: [PATCH 06/10] Do samll change in wrapper_nodelet.cc --- .../src/wrapper_nodelet.cc | 19 ------------------- 1 file changed, 19 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 81c9d24..189418e 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 @@ -409,25 +409,6 @@ class ROSWrapperNodelet : public nodelet::Nodelet { } else { publishPoint(stream); } - api_->SetStreamCallback( - stream, [this, stream](const api::StreamData &data) { - // data.img is null, not hard timestamp - static std::size_t count = 0; - ++count; - publishCamera(stream, data, count, ros::Time::now()); - }); - is_published_[stream] = true; - } - - if (points_publisher_.getNumSubscribers() > 0 && - !is_published_[Stream::POINTS]) { - api_->SetStreamCallback( - Stream::POINTS, [this](const api::StreamData &data) { - static std::size_t count = 0; - ++count; - publishPoints(data, count, ros::Time::now()); - }); - is_published_[Stream::POINTS] = true; } if (!is_motion_published_) { From 0ecb3950447671bf0a484bd382bac205c7799148 Mon Sep 17 00:00:00 2001 From: kalman Date: Fri, 7 Dec 2018 16:22:32 +0800 Subject: [PATCH 07/10] Replace tab with space --- samples/tutorials/util/pc_viewer.cc | 2 +- .../api/processor/disparity_processor.cc | 53 +++++++++---------- .../api/processor/rectify_processor.cc | 6 +-- 3 files changed, 30 insertions(+), 31 deletions(-) diff --git a/samples/tutorials/util/pc_viewer.cc b/samples/tutorials/util/pc_viewer.cc index 1ad50a1..08bcce6 100644 --- a/samples/tutorials/util/pc_viewer.cc +++ b/samples/tutorials/util/pc_viewer.cc @@ -84,7 +84,7 @@ void PCViewer::ConvertMatToPointCloud( for (int i = 0; i < xyz.rows; i++) { for (int j = 0; j < xyz.cols; j++) { auto &&p = xyz.at(i, j); - if (std::abs(p.z) > 9999) continue; + if (std::abs(p.z) > 9999) continue; if (std::isfinite(p.x) && std::isfinite(p.y) && std::isfinite(p.z)) { // LOG(INFO) << "[" << i << "," << j << "] x: " << p.x << ", y: " << p.y // << ", z: " << p.z; diff --git a/src/mynteye/api/processor/disparity_processor.cc b/src/mynteye/api/processor/disparity_processor.cc index b662366..c60de02 100644 --- a/src/mynteye/api/processor/disparity_processor.cc +++ b/src/mynteye/api/processor/disparity_processor.cc @@ -27,34 +27,33 @@ DisparityProcessor::DisparityProcessor(std::int32_t proc_period) : Processor(std::move(proc_period)) { VLOG(2) << __func__ << ": proc_period=" << proc_period; - int blockSize_ = 15; // 15 - int numDisparities_ = 64; // 64 + int blockSize_ = 15; // 15 + int numDisparities_ = 64; // 64 #ifdef WITH_OPENCV2 - bm_ = cv::Ptr( - new cv::StereoBM( - cv::StereoBM::BASIC_PRESET, - numDisparities_, - blockSize_)); + bm_ = cv::Ptr( + new cv::StereoBM( + cv::StereoBM::BASIC_PRESET, + numDisparities_, + blockSize_)); #else - int minDisparity_ = 0; // 0 - int preFilterSize_ = 9; // 9 - int preFilterCap_ = 31; // 31 - int uniquenessRatio_ = 15; // 15 - int textureThreshold_ = 10; // 10 - int speckleWindowSize_ = 100; // 100 - int speckleRange_ = 4; // 4 - - bm_ = cv::StereoBM::create(16, 9); - bm_->setBlockSize(blockSize_); - bm_->setMinDisparity(minDisparity_); - bm_->setNumDisparities(numDisparities_); - bm_->setPreFilterSize(preFilterSize_); - bm_->setPreFilterCap(preFilterCap_); - bm_->setUniquenessRatio(uniquenessRatio_); - bm_->setTextureThreshold(textureThreshold_); - bm_->setSpeckleWindowSize(speckleWindowSize_); - bm_->setSpeckleRange(speckleRange_); + int minDisparity_ = 0; // 0 + int preFilterSize_ = 9; // 9 + int preFilterCap_ = 31; // 31 + int uniquenessRatio_ = 15; // 15 + int textureThreshold_ = 10; // 10 + int speckleWindowSize_ = 100; // 100 + int speckleRange_ = 4; // 4 + bm_ = cv::StereoBM::create(16, 9); + bm_->setBlockSize(blockSize_); + bm_->setMinDisparity(minDisparity_); + bm_->setNumDisparities(numDisparities_); + bm_->setPreFilterSize(preFilterSize_); + bm_->setPreFilterCap(preFilterCap_); + bm_->setUniquenessRatio(uniquenessRatio_); + bm_->setTextureThreshold(textureThreshold_); + bm_->setSpeckleWindowSize(speckleWindowSize_); + bm_->setSpeckleRange(speckleRange_); #endif } @@ -78,11 +77,11 @@ bool DisparityProcessor::OnProcess( cv::Mat disparity; #ifdef WITH_OPENCV2 - (*bm_)(input->first, input->second, disparity); + (*bm_)(input->first, input->second, disparity); #else bm_->compute(input->first, input->second, disparity); #endif - disparity.convertTo(output->value, CV_32F, 1./16); + disparity.convertTo(output->value, CV_32F, 1./16); return true; } diff --git a/src/mynteye/api/processor/rectify_processor.cc b/src/mynteye/api/processor/rectify_processor.cc index ab47402..2b14e12 100644 --- a/src/mynteye/api/processor/rectify_processor.cc +++ b/src/mynteye/api/processor/rectify_processor.cc @@ -72,15 +72,15 @@ void RectifyProcessor::InitParams( in_right.cy, 0, 0, 1); cv::Mat D1(1, 5, CV_64F, in_left.coeffs); cv::Mat D2(1, 5, CV_64F, in_right.coeffs); - /* + /* cv::Mat R = (cv::Mat_(3, 3) << ex_right_to_left.rotation[0][0], ex_right_to_left.rotation[0][1], ex_right_to_left.rotation[0][2], ex_right_to_left.rotation[1][0], ex_right_to_left.rotation[1][1], ex_right_to_left.rotation[1][2], ex_right_to_left.rotation[2][0], ex_right_to_left.rotation[2][1], ex_right_to_left.rotation[2][2]); - */ - cv::Mat R = + */ + cv::Mat R = (cv::Mat_(3, 3) << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0); cv::Mat T(3, 1, CV_64F, ex_right_to_left.translation); From 28ca9b60b0ef6231179f96700d06a7d393065ca2 Mon Sep 17 00:00:00 2001 From: kalman Date: Sat, 8 Dec 2018 20:53:11 +0800 Subject: [PATCH 08/10] Rollback to sgbm --- samples/tutorials/util/pc_viewer.cc | 1 - .../api/processor/disparity_processor.cc | 75 ++++++++++++------- .../api/processor/disparity_processor.h | 6 +- src/mynteye/api/processor/points_processor.cc | 35 +-------- .../api/processor/rectify_processor.cc | 4 - 5 files changed, 52 insertions(+), 69 deletions(-) diff --git a/samples/tutorials/util/pc_viewer.cc b/samples/tutorials/util/pc_viewer.cc index 08bcce6..bec96dc 100644 --- a/samples/tutorials/util/pc_viewer.cc +++ b/samples/tutorials/util/pc_viewer.cc @@ -84,7 +84,6 @@ void PCViewer::ConvertMatToPointCloud( for (int i = 0; i < xyz.rows; i++) { for (int j = 0; j < xyz.cols; j++) { auto &&p = xyz.at(i, j); - if (std::abs(p.z) > 9999) continue; if (std::isfinite(p.x) && std::isfinite(p.y) && std::isfinite(p.z)) { // LOG(INFO) << "[" << i << "," << j << "] x: " << p.x << ", y: " << p.y // << ", z: " << p.z; diff --git a/src/mynteye/api/processor/disparity_processor.cc b/src/mynteye/api/processor/disparity_processor.cc index c60de02..b6c37a3 100644 --- a/src/mynteye/api/processor/disparity_processor.cc +++ b/src/mynteye/api/processor/disparity_processor.cc @@ -26,34 +26,37 @@ const char DisparityProcessor::NAME[] = "DisparityProcessor"; DisparityProcessor::DisparityProcessor(std::int32_t proc_period) : Processor(std::move(proc_period)) { VLOG(2) << __func__ << ": proc_period=" << proc_period; - - int blockSize_ = 15; // 15 - int numDisparities_ = 64; // 64 + int sgbmWinSize = 3; + int numberOfDisparities = 64; #ifdef WITH_OPENCV2 - bm_ = cv::Ptr( - new cv::StereoBM( - cv::StereoBM::BASIC_PRESET, - numDisparities_, - blockSize_)); + // StereoSGBM + // http://docs.opencv.org/2.4/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html?#stereosgbm + sgbm_ = 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 - int minDisparity_ = 0; // 0 - int preFilterSize_ = 9; // 9 - int preFilterCap_ = 31; // 31 - int uniquenessRatio_ = 15; // 15 - int textureThreshold_ = 10; // 10 - int speckleWindowSize_ = 100; // 100 - int speckleRange_ = 4; // 4 - bm_ = cv::StereoBM::create(16, 9); - bm_->setBlockSize(blockSize_); - bm_->setMinDisparity(minDisparity_); - bm_->setNumDisparities(numDisparities_); - bm_->setPreFilterSize(preFilterSize_); - bm_->setPreFilterCap(preFilterCap_); - bm_->setUniquenessRatio(uniquenessRatio_); - bm_->setTextureThreshold(textureThreshold_); - bm_->setSpeckleWindowSize(speckleWindowSize_); - bm_->setSpeckleRange(speckleRange_); + sgbm_ = cv::StereoSGBM::create(0, 16, 3); + sgbm_->setPreFilterCap(63); + sgbm_->setBlockSize(sgbmWinSize); + sgbm_->setP1(8 * sgbmWinSize * sgbmWinSize); + sgbm_->setP2(32 * sgbmWinSize * sgbmWinSize); + sgbm_->setMinDisparity(0); + sgbm_->setNumDisparities(numberOfDisparities); + sgbm_->setUniquenessRatio(10); + sgbm_->setSpeckleWindowSize(100); + sgbm_->setSpeckleRange(32); + sgbm_->setDisp12MaxDiff(1); #endif } @@ -77,11 +80,27 @@ bool DisparityProcessor::OnProcess( cv::Mat disparity; #ifdef WITH_OPENCV2 - (*bm_)(input->first, input->second, disparity); + // StereoSGBM::operator() + // http://docs.opencv.org/2.4/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html#stereosgbm-operator + // Output disparity map. It is a 16-bit signed single-channel image of the + // same size as the input image. + // It contains disparity values scaled by 16. So, to get the floating-point + // disparity map, + // you need to divide each disp element by 16. + (*sgbm_)(input->first, input->second, disparity); #else - bm_->compute(input->first, input->second, disparity); + // compute() + // http://docs.opencv.org/master/d2/d6e/classcv_1_1StereoMatcher.html + // Output disparity map. It has the same size as the input images. + // Some algorithms, like StereoBM or StereoSGBM compute 16-bit fixed-point + // disparity map + // (where each disparity value has 4 fractional bits), + // whereas other algorithms output 32-bit floating-point disparity map. + sgbm_->compute(input->first, input->second, disparity); #endif - disparity.convertTo(output->value, CV_32F, 1./16); + output->value = disparity / 16 + 1; + output->id = input->first_id; + output->data = inpu t->first_data; return true; } diff --git a/src/mynteye/api/processor/disparity_processor.h b/src/mynteye/api/processor/disparity_processor.h index 0ec43b6..370dca3 100644 --- a/src/mynteye/api/processor/disparity_processor.h +++ b/src/mynteye/api/processor/disparity_processor.h @@ -17,13 +17,11 @@ #include -#include - #include "mynteye/api/processor.h" namespace cv { -class StereoBM; +class StereoSGBM; } // namespace cv @@ -44,7 +42,7 @@ class DisparityProcessor : public Processor { Object *const in, Object *const out, Processor *const parent) override; private: - cv::Ptr bm_; + cv::Ptr sgbm_; }; MYNTEYE_END_NAMESPACE diff --git a/src/mynteye/api/processor/points_processor.cc b/src/mynteye/api/processor/points_processor.cc index 9b40e85..7d2b6d4 100644 --- a/src/mynteye/api/processor/points_processor.cc +++ b/src/mynteye/api/processor/points_processor.cc @@ -45,38 +45,9 @@ bool PointsProcessor::OnProcess( MYNTEYE_UNUSED(parent) const ObjMat *input = Object::Cast(in); ObjMat *output = Object::Cast(out); - - cv::Mat disparity = input->value; - output->value.create(disparity.size(), CV_MAKETYPE(CV_32FC3, 3)); - cv::Mat _3dImage = output->value; - - const float bigZ = 10000.f; - cv::Matx44d Q; - Q_.convertTo(Q, CV_64F); - - int x, cols = disparity.cols; - CV_Assert(cols >= 0); - - double minDisparity = FLT_MAX; - - cv::minMaxIdx(disparity, &minDisparity, 0, 0, 0); - - for (int y = 0; y < disparity.rows; y++) { - float *sptr = disparity.ptr(y); - cv::Vec3f *dptr = _3dImage.ptr(y); - - for (x = 0; x < cols; x++) { - double d = sptr[x]; - cv::Vec4d homg_pt = Q * cv::Vec4d(x, y, d, 1.0); - dptr[x] = cv::Vec3d(homg_pt.val); - dptr[x] /= homg_pt[3]; - - if (fabs(d - minDisparity) <= FLT_EPSILON) { - dptr[x][2] = bigZ; - } - } - } - + cv::reprojectImageTo3D(input->value, output->value, Q_, true); + output->id = input->id; + output->data = input->data; return true; } diff --git a/src/mynteye/api/processor/rectify_processor.cc b/src/mynteye/api/processor/rectify_processor.cc index 2b14e12..9c570f0 100644 --- a/src/mynteye/api/processor/rectify_processor.cc +++ b/src/mynteye/api/processor/rectify_processor.cc @@ -72,16 +72,12 @@ void RectifyProcessor::InitParams( in_right.cy, 0, 0, 1); cv::Mat D1(1, 5, CV_64F, in_left.coeffs); cv::Mat D2(1, 5, CV_64F, in_right.coeffs); - /* cv::Mat R = (cv::Mat_(3, 3) << ex_right_to_left.rotation[0][0], ex_right_to_left.rotation[0][1], ex_right_to_left.rotation[0][2], ex_right_to_left.rotation[1][0], ex_right_to_left.rotation[1][1], ex_right_to_left.rotation[1][2], ex_right_to_left.rotation[2][0], ex_right_to_left.rotation[2][1], ex_right_to_left.rotation[2][2]); - */ - cv::Mat R = - (cv::Mat_(3, 3) << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0); cv::Mat T(3, 1, CV_64F, ex_right_to_left.translation); VLOG(2) << "InitParams size: " << size; From b9db889fa319f3644b4a491763bf1fd04ea16a49 Mon Sep 17 00:00:00 2001 From: kalman Date: Sat, 8 Dec 2018 20:55:14 +0800 Subject: [PATCH 09/10] Delete uesless space --- src/mynteye/api/processor/disparity_processor.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/mynteye/api/processor/disparity_processor.cc b/src/mynteye/api/processor/disparity_processor.cc index b6c37a3..c22232c 100644 --- a/src/mynteye/api/processor/disparity_processor.cc +++ b/src/mynteye/api/processor/disparity_processor.cc @@ -100,7 +100,7 @@ bool DisparityProcessor::OnProcess( #endif output->value = disparity / 16 + 1; output->id = input->first_id; - output->data = inpu t->first_data; + output->data = input->first_data; return true; } From 206683b76f91bcc666e724e4f9f3fc06fdc5f3b3 Mon Sep 17 00:00:00 2001 From: kalman Date: Sat, 8 Dec 2018 21:10:31 +0800 Subject: [PATCH 10/10] Fix disparity_processor.cc bug --- src/mynteye/api/processor/disparity_processor.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/mynteye/api/processor/disparity_processor.cc b/src/mynteye/api/processor/disparity_processor.cc index c22232c..1c4f939 100644 --- a/src/mynteye/api/processor/disparity_processor.cc +++ b/src/mynteye/api/processor/disparity_processor.cc @@ -98,7 +98,7 @@ bool DisparityProcessor::OnProcess( // whereas other algorithms output 32-bit floating-point disparity map. sgbm_->compute(input->first, input->second, disparity); #endif - output->value = disparity / 16 + 1; + disparity.convertTo(output->value, CV_32F, 1./16, 1); output->id = input->first_id; output->data = input->first_data; return true;