From f92ecd437133340320373f7e6e2c1a23481d5cd5 Mon Sep 17 00:00:00 2001 From: Osenberg-Y Date: Thu, 9 Aug 2018 12:14:54 +0800 Subject: [PATCH 1/5] 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 2/5] 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 3/5] 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 4/5] 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 5/5] 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) {