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