tami/MYNT-EYE-S-SDK
6
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fix(calib): covert params change.

Browse Source
This commit is contained in:
TinyO 2019-08-17 18:52:31 +08:00
parent 7baf7ba744
commit 35d91c25b9
6 changed files with 28 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
include/mynteye/types.h
View File

@ -775,6 +775,7 @@ struct CameraROSMsgInfoPair {
struct CameraROSMsgInfo left;
struct CameraROSMsgInfo right;
double T_mul_f = -1.f;
double cx1_minus_cx2 = 0.f;
double R[9] = {0};
double P[12] = {0};
};

9
src/mynteye/api/processor/disparity_processor.cc
View File

@ -27,8 +27,14 @@ MYNTEYE_BEGIN_NAMESPACE
const char DisparityProcessor::NAME[] = "DisparityProcessor";
DisparityProcessor::DisparityProcessor(DisparityComputingMethod type,
std::shared_ptr<struct CameraROSMsgInfoPair> calib_infos,
std::int32_t proc_period)
: Processor(std::move(proc_period)), type_(type) {
if (calib_infos) {
cx1_minus_cx2_ = calib_infos->cx1_minus_cx2;
} else {
cx1_minus_cx2_ = 1.f;
}
VLOG(2) << __func__ << ": proc_period=" << proc_period;
int sgbmWinSize = 3;
int numberOfDisparities = 64;
@ -156,6 +162,7 @@ bool DisparityProcessor::OnProcess(
} else if (type_ == DisparityComputingMethod::BM) {
// LOG(ERROR) << "not supported in opencv 2.x";
(*sgbm_matcher)(input->first, input->second, disparity);
disparity.convertTo(output->value, CV_32F, 1./16, 1);
// cv::Mat tmp1, tmp2;
// cv::cvtColor(input->first, tmp1, CV_RGB2GRAY);
// cv::cvtColor(input->second, tmp2, CV_RGB2GRAY);
@ -188,7 +195,7 @@ bool DisparityProcessor::OnProcess(
sgbm_matcher->compute(input->first, input->second, disparity);
}
#endif
disparity.convertTo(output->value, CV_32F, 1./16, 1);
disparity.convertTo(output->value, CV_32F, 1./16, cx1_minus_cx2_);
output->id = input->first_id;
output->data = input->first_data;
return true;

2
src/mynteye/api/processor/disparity_processor.h
View File

@ -32,6 +32,7 @@ class DisparityProcessor : public Processor {
static const char NAME[];
explicit DisparityProcessor(DisparityComputingMethod type,
std::shared_ptr<struct CameraROSMsgInfoPair> calib_infos,
std::int32_t proc_period = 0);
virtual ~DisparityProcessor();
@ -54,6 +55,7 @@ class DisparityProcessor : public Processor {
cv::Ptr<cv::StereoSGBM> sgbm_matcher;
cv::Ptr<cv::StereoBM> bm_matcher;
DisparityComputingMethod type_;
double cx1_minus_cx2_;
};
MYNTEYE_END_NAMESPACE

8
src/mynteye/api/processor/rectify_processor.cc
View File

@ -42,6 +42,7 @@ void RectifyProcessor::stereoRectify(models::CameraPtr leftOdo,
const CvMat* D1, const CvMat* D2, CvSize imageSize,
const CvMat* matR, const CvMat* matT,
CvMat* _R1, CvMat* _R2, CvMat* _P1, CvMat* _P2, double* T_mul_f,
double *cx1_min_cx2,
int flags, double alpha, CvSize newImgSize) {
// std::cout << _alpha << std::endl;
alpha = _alpha;
@ -218,6 +219,8 @@ void RectifyProcessor::stereoRectify(models::CameraPtr leftOdo,
cvmSet(_P2, 0, 2, cx2);
cvmSet(_P2, 1, 2, cy2);
cvmSet(_P2, idx, 3, s*cvmGet(_P2, idx, 3));
*cx1_min_cx2 = -(cx1 - cx2);
}
}
@ -299,8 +302,10 @@ std::shared_ptr<struct CameraROSMsgInfoPair> RectifyProcessor::stereoRectify(
CvMat c_K1 = K1, c_K2 = K2, c_D1 = D1, c_D2 = D2;
CvMat c_R1 = R1, c_R2 = R2, c_P1 = P1, c_P2 = P2;
double T_mul_f;
double cx1_min_cx2;
stereoRectify(leftOdo, rightOdo, &c_K1, &c_K2, &c_D1, &c_D2,
image_size1, &c_R, &c_t, &c_R1, &c_R2, &c_P1, &c_P2, &T_mul_f);
image_size1, &c_R, &c_t, &c_R1, &c_R2, &c_P1, &c_P2, &T_mul_f,
&cx1_min_cx2);
VLOG(2) << "K1: " << K1 << std::endl;
VLOG(2) << "D1: " << D1 << std::endl;
@ -334,6 +339,7 @@ std::shared_ptr<struct CameraROSMsgInfoPair> RectifyProcessor::stereoRectify(
info_pair.left = calib_mat_data_left;
info_pair.right = calib_mat_data_right;
info_pair.T_mul_f = T_mul_f;
info_pair.cx1_minus_cx2 = cx1_min_cx2;
for (std::size_t i = 0; i< 3 * 4; i++) {
info_pair.P[i] = calib_mat_data_left.P[i];
}

1
src/mynteye/api/processor/rectify_processor.h
View File

@ -87,6 +87,7 @@ class RectifyProcessor : public Processor {
const CvMat* D1, const CvMat* D2, CvSize imageSize,
const CvMat* matR, const CvMat* matT,
CvMat* _R1, CvMat* _R2, CvMat* _P1, CvMat* _P2, double* T_mul_f,
double *cx1_min_cx2,
int flags = cv::CALIB_ZERO_DISPARITY, double alpha = -1,
CvSize newImgSize = cv::Size());

12
src/mynteye/api/synthetic.cc
View File

@ -313,10 +313,8 @@ void Synthetic::InitProcessors() {
std::shared_ptr<Processor> rectify_processor = nullptr;
std::shared_ptr<Processor> points_processor = nullptr;
std::shared_ptr<Processor> depth_processor = nullptr;
std::shared_ptr<Processor> disparity_processor = nullptr;
auto &&disparity_processor =
std::make_shared<DisparityProcessor>(DisparityComputingMethod::BM,
DISPARITY_PROC_PERIOD);
auto &&disparitynormalized_processor =
std::make_shared<DisparityNormalizedProcessor>(
DISPARITY_NORM_PROC_PERIOD);
@ -333,6 +331,10 @@ void Synthetic::InitProcessors() {
points_processor = std::make_shared<PointsProcessorOCV>(
rectify_processor_ocv->Q, POINTS_PROC_PERIOD);
depth_processor = std::make_shared<DepthProcessorOCV>(DEPTH_PROC_PERIOD);
disparity_processor =
std::make_shared<DisparityProcessor>(DisparityComputingMethod::BM,
nullptr,
DISPARITY_PROC_PERIOD);
root_processor->AddChild(rectify_processor);
rectify_processor->AddChild(disparity_processor);
@ -352,6 +354,10 @@ void Synthetic::InitProcessors() {
depth_processor = std::make_shared<DepthProcessor>(
rectify_processor_imp -> getCameraROSMsgInfoPair(),
DEPTH_PROC_PERIOD);
disparity_processor =
std::make_shared<DisparityProcessor>(DisparityComputingMethod::BM,
rectify_processor_imp -> getCameraROSMsgInfoPair(),
DISPARITY_PROC_PERIOD);
root_processor->AddChild(rectify_processor);
rectify_processor->AddChild(disparity_processor);