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

fix(calib depth): try 'T*f * 1000.0'

Browse Source
This commit is contained in:
TinyOh 2019-01-10 14:38:44 +08:00
parent b89b9a20e4
commit 39b656465b
4 changed files with 14 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/mynteye/api/processor/depth_processor.cc
View File

@ -48,14 +48,14 @@ bool DepthProcessor::OnProcess(
ObjMat *output = Object::Cast<ObjMat>(out); ObjMat *output = Object::Cast<ObjMat>(out);
int rows = input->value.rows; int rows = input->value.rows;
int cols = input->value.cols; int cols = input->value.cols;
std::cout << calib_infos_->T_mul_f << std::endl;
// 0.0793434
// TODO(MYNTEYE): Put the corresponding parameters(T,f) // TODO(MYNTEYE): Put the corresponding parameters(T,f)
float T = 0.08;
float f = 0.01;
cv::Mat depth_mat = cv::Mat::zeros(rows, cols, CV_32F); cv::Mat depth_mat = cv::Mat::zeros(rows, cols, CV_32F);
for (int i = 0; i < rows; i++) { for (int i = 0; i < rows; i++) {
for (int j = 0; j < cols; j++) { for (int j = 0; j < cols; j++) {
float disparity_value = input->value.at<float>(i, j); float disparity_value = input->value.at<float>(i, j);
float depth = T * f / disparity_value; float depth = calib_infos_->T_mul_f * 1000.0 / disparity_value ;
depth_mat.at<float>(i, j) = depth; depth_mat.at<float>(i, j) = depth;
} }
} }

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

@ -75,12 +75,11 @@ Object *PointsProcessor::OnCreateOutput() {
bool PointsProcessor::OnProcess( bool PointsProcessor::OnProcess(
Object *const in, Object *const out, Processor *const parent) { Object *const in, Object *const out, Processor *const parent) {
MYNTEYE_UNUSED(parent) MYNTEYE_UNUSED(parent)
std::cout << calib_infos_->left.height << std::endl;
float fx = 3.6797709792391299e+02; float fx = calib_infos_->left.K[0];
float fy = 3.6808712539453859e+02; float fy = calib_infos_->left.K[4];
float cx = 3.7414963027144353e+02; float cx = calib_infos_->left.K[2];
float cy = 2.3125000326472903e+02; float cy = calib_infos_->left.K[5];
// Use correct principal point from calibration // Use correct principal point from calibration
float center_x = cx; float center_x = cx;

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

@ -41,7 +41,7 @@ void RectifyProcessor::stereoRectify(camodocal::CameraPtr leftOdo,
camodocal::CameraPtr rightOdo, const CvMat* K1, const CvMat* K2, camodocal::CameraPtr rightOdo, const CvMat* K1, const CvMat* K2,
const CvMat* D1, const CvMat* D2, CvSize imageSize, const CvMat* D1, const CvMat* D2, CvSize imageSize,
const CvMat* matR, const CvMat* matT, const CvMat* matR, const CvMat* matT,
CvMat* _R1, CvMat* _R2, CvMat* _P1, CvMat* _P2, CvMat* _R1, CvMat* _R2, CvMat* _P1, CvMat* _P2, double* T_mul_f,
int flags, double alpha, CvSize newImgSize) { int flags, double alpha, CvSize newImgSize) {
double _om[3], _t[3] = {0}, _uu[3]={0, 0, 0}, _r_r[3][3], _pp[3][4]; double _om[3], _t[3] = {0}, _uu[3]={0, 0, 0}, _r_r[3][3], _pp[3][4];
double _ww[3], _wr[3][3], _z[3] = {0, 0, 0}, _ri[3][3], _w3[3]; double _ww[3], _wr[3][3], _z[3] = {0, 0, 0}, _ri[3][3], _w3[3];
@ -67,7 +67,6 @@ void RectifyProcessor::stereoRectify(camodocal::CameraPtr leftOdo,
cvConvertScale(&om, &om, -0.5); // get average rotation cvConvertScale(&om, &om, -0.5); // get average rotation
cvRodrigues2(&om, &r_r); // rotate cameras to same orientation by averaging cvRodrigues2(&om, &r_r); // rotate cameras to same orientation by averaging
cvMatMul(&r_r, matT, &t); cvMatMul(&r_r, matT, &t);
int idx = fabs(_t[0]) > fabs(_t[1]) ? 0 : 1; int idx = fabs(_t[0]) > fabs(_t[1]) ? 0 : 1;
// if idx == 0 // if idx == 0
@ -168,6 +167,7 @@ void RectifyProcessor::stereoRectify(camodocal::CameraPtr leftOdo,
_pp[0][2] = cc_new[1].x; _pp[0][2] = cc_new[1].x;
_pp[1][2] = cc_new[1].y; _pp[1][2] = cc_new[1].y;
_pp[idx][3] = _t[idx]*fc_new; // baseline * focal length _pp[idx][3] = _t[idx]*fc_new; // baseline * focal length
*T_mul_f = 0. - _t[idx];
cvConvert(&pp, _P2); cvConvert(&pp, _P2);
alpha = MIN(alpha, 1.); alpha = MIN(alpha, 1.);
@ -277,8 +277,9 @@ std::shared_ptr<struct camera_calib_info_pair> RectifyProcessor::stereoRectify(
CvMat c_R = cv_R, c_t = cv_t; CvMat c_R = cv_R, c_t = cv_t;
CvMat c_K1 = K1, c_K2 = K2, c_D1 = D1, c_D2 = D2; 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; CvMat c_R1 = R1, c_R2 = R2, c_P1 = P1, c_P2 = P2;
double T_mul_f;
stereoRectify(leftOdo, rightOdo, &c_K1, &c_K2, &c_D1, &c_D2, 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); image_size1, &c_R, &c_t, &c_R1, &c_R2, &c_P1, &c_P2, &T_mul_f);
// std::cout << "K1: " << K1 << std::endl; // std::cout << "K1: " << K1 << std::endl;
// std::cout << "D1: " << D1 << std::endl; // std::cout << "D1: " << D1 << std::endl;
@ -308,7 +309,7 @@ std::shared_ptr<struct camera_calib_info_pair> RectifyProcessor::stereoRectify(
} }
} }
struct camera_calib_info_pair res = struct camera_calib_info_pair res =
{calib_mat_data_left, calib_mat_data_right}; {calib_mat_data_left, calib_mat_data_right, T_mul_f};
return std::make_shared<struct camera_calib_info_pair>(res); return std::make_shared<struct camera_calib_info_pair>(res);
} }

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

@ -53,6 +53,7 @@ struct camera_calib_info {
struct camera_calib_info_pair { struct camera_calib_info_pair {
struct camera_calib_info left; struct camera_calib_info left;
struct camera_calib_info right; struct camera_calib_info right;
double T_mul_f;
}; };
class Device; class Device;
@ -94,7 +95,7 @@ class RectifyProcessor : public Processor {
camodocal::CameraPtr rightOdo, const CvMat* K1, const CvMat* K2, camodocal::CameraPtr rightOdo, const CvMat* K1, const CvMat* K2,
const CvMat* D1, const CvMat* D2, CvSize imageSize, const CvMat* D1, const CvMat* D2, CvSize imageSize,
const CvMat* matR, const CvMat* matT, const CvMat* matR, const CvMat* matT,
CvMat* _R1, CvMat* _R2, CvMat* _P1, CvMat* _P2, CvMat* _R1, CvMat* _R2, CvMat* _P1, CvMat* _P2, double* T_mul_f,
int flags = cv::CALIB_ZERO_DISPARITY, double alpha = -1, int flags = cv::CALIB_ZERO_DISPARITY, double alpha = -1,
CvSize newImgSize = cv::Size()); CvSize newImgSize = cv::Size());