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

style(calib_model): mask the depth processer temporarily;

Browse Source
This commit is contained in:
TinyOh 2019-01-08 13:20:09 +08:00
parent cfbfcf06a5
commit 257a6c8bef
2 changed files with 90 additions and 138 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -41,13 +41,17 @@ 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) {
#ifdef WITH_CAM_MODELS
#else
MYNTEYE_UNUSED(parent) MYNTEYE_UNUSED(parent)
const ObjMat *input = Object::Cast<ObjMat>(in); const ObjMat *input = Object::Cast<ObjMat>(in);
ObjMat *output = Object::Cast<ObjMat>(out); ObjMat *output = Object::Cast<ObjMat>(out);
cv::reprojectImageTo3D(input->value, output->value, Q_, true); cv::reprojectImageTo3D(input->value, output->value, Q_, true);
output->id = input->id; output->id = input->id;
output->data = input->data; output->data = input->data;
#endif
return true; return true;
} }

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

@ -120,76 +120,72 @@ void stereoRectify(camodocal::CameraPtr leftOdo,
for (i = 0; i < 3; ++i) { for (i = 0; i < 3; ++i) {
_wr[idx][i] = -_t[i] / nt; _wr[idx][i] = -_t[i] / nt;
_wr[idx ^ 1][i] = -_ww[i]; _wr[idx ^ 1][i] = -_ww[i];
_wr[2][i] = _w3[i] * (1 - 2 * idx); // if idx == 1 -> opposite direction _wr[2][i] = _w3[i] * (1 - 2 * idx); // if idx == 1 -> opposite direction
} }
// apply to both views // apply to both views
cvGEMM(&wR, &r_r, 1, 0, 0, &Ri, CV_GEMM_B_T); cvGEMM(&wR, &r_r, 1, 0, 0, &Ri, CV_GEMM_B_T);
cvConvert( &Ri, _R1 ); cvConvert(&Ri, _R1);
cvGEMM(&wR, &r_r, 1, 0, 0, &Ri, 0); cvGEMM(&wR, &r_r, 1, 0, 0, &Ri, 0);
cvConvert( &Ri, _R2 ); cvConvert(&Ri, _R2);
cvMatMul(&Ri, matT, &t); cvMatMul(&Ri, matT, &t);
// calculate projection/camera matrices // calculate projection/camera matrices
// these contain the relevant rectified image internal params (fx, fy=fx, cx, cy) // these contain the relevant rectified image internal params (fx, fy=fx, cx, cy)
double fc_new = DBL_MAX; double fc_new = DBL_MAX;
CvPoint2D64f cc_new[2] = {{0,0}, {0,0}}; CvPoint2D64f cc_new[2] = {{0, 0}, {0, 0}};
newImgSize = newImgSize.width * newImgSize.height != 0 ? newImgSize : imageSize; newImgSize = newImgSize.width * newImgSize.height != 0 ? newImgSize : imageSize;
const double ratio_x = (double)newImgSize.width / imageSize.width / 2; const double ratio_x = (double)newImgSize.width / imageSize.width / 2;
const double ratio_y = (double)newImgSize.height / imageSize.height / 2; const double ratio_y = (double)newImgSize.height / imageSize.height / 2;
const double ratio = idx == 1 ? ratio_x : ratio_y; const double ratio = idx == 1 ? ratio_x : ratio_y;
fc_new = (cvmGet(K1, idx ^ 1, idx ^ 1) + cvmGet(K2, idx ^ 1, idx ^ 1)) * ratio; fc_new = (cvmGet(K1, idx ^ 1, idx ^ 1) + cvmGet(K2, idx ^ 1, idx ^ 1)) * ratio;
for( k = 0; k < 2; k++ ) for (k = 0; k < 2; k++) {
{ CvPoint2D32f _pts[4];
CvPoint2D32f _pts[4]; CvPoint3D32f _pts_3[4];
CvPoint3D32f _pts_3[4]; CvMat pts = cvMat(1, 4, CV_32FC2, _pts);
CvMat pts = cvMat(1, 4, CV_32FC2, _pts); CvMat pts_3 = cvMat(1, 4, CV_32FC3, _pts_3);
CvMat pts_3 = cvMat(1, 4, CV_32FC3, _pts_3); Eigen::Vector2d a;
Eigen::Vector2d a; Eigen::Vector3d b;
Eigen::Vector3d b; for (i = 0; i < 4; i++) {
for( i = 0; i < 4; i++ ) int j = (i < 2) ? 0 : 1;
{ a.x() = (float)((i % 2)*(nx));
int j = (i<2) ? 0 : 1; a.y() = (float)(j*(ny));
a.x() = (float)((i % 2)*(nx)); if (0 == k) {
a.y() = (float)(j*(ny)); leftOdo->liftProjective(a, b);
if (0 == k) { } else {
leftOdo->liftProjective(a, b); rightOdo->liftProjective(a, b);
} else {
rightOdo->liftProjective(a, b);
}
_pts[i].x = b.x()/b.z();
_pts[i].y = b.y()/b.z();
} }
cvConvertPointsHomogeneous( &pts, &pts_3 ); _pts[i].x = b.x()/b.z();
_pts[i].y = b.y()/b.z();
}
cvConvertPointsHomogeneous(&pts, &pts_3);
//Change camera matrix to have cc=[0,0] and fc = fc_new // Change camera matrix to have cc=[0,0] and fc = fc_new
double _a_tmp[3][3]; double _a_tmp[3][3];
CvMat A_tmp = cvMat(3, 3, CV_64F, _a_tmp); CvMat A_tmp = cvMat(3, 3, CV_64F, _a_tmp);
_a_tmp[0][0]=fc_new; _a_tmp[0][0] = fc_new;
_a_tmp[1][1]=fc_new; _a_tmp[1][1] = fc_new;
_a_tmp[0][2]=0.0; _a_tmp[0][2] = 0.0;
_a_tmp[1][2]=0.0; _a_tmp[1][2] = 0.0;
cvProjectPoints2( &pts_3, k == 0 ? _R1 : _R2, &Z, &A_tmp, 0, &pts ); cvProjectPoints2(&pts_3, k == 0 ? _R1 : _R2, &Z, &A_tmp, 0, &pts);
CvScalar avg = cvAvg(&pts); CvScalar avg = cvAvg(&pts);
cc_new[k].x = (nx)/2 - avg.val[0]; cc_new[k].x = (nx)/2 - avg.val[0];
cc_new[k].y = (ny)/2 - avg.val[1]; cc_new[k].y = (ny)/2 - avg.val[1];
//cc_new[k].x = (nx)/2;
//cc_new[k].y = (ny)/2;
} }
if( flags & cv::CALIB_ZERO_DISPARITY ) if (flags & cv::CALIB_ZERO_DISPARITY) {
{ cc_new[0].x = cc_new[1].x = (cc_new[0].x + cc_new[1].x)*0.5;
cc_new[0].x = cc_new[1].x = (cc_new[0].x + cc_new[1].x)*0.5; cc_new[0].y = cc_new[1].y = (cc_new[0].y + cc_new[1].y)*0.5;
cc_new[0].y = cc_new[1].y = (cc_new[0].y + cc_new[1].y)*0.5; } else if (idx == 0) {
// horizontal stereo
cc_new[0].y = cc_new[1].y = (cc_new[0].y + cc_new[1].y)*0.5;
} else {
// vertical stereo
cc_new[0].x = cc_new[1].x = (cc_new[0].x + cc_new[1].x)*0.5;
} }
else if( idx == 0 ) // horizontal stereo
cc_new[0].y = cc_new[1].y = (cc_new[0].y + cc_new[1].y)*0.5;
else // vertical stereo
cc_new[0].x = cc_new[1].x = (cc_new[0].x + cc_new[1].x)*0.5;
cvZero( &pp ); cvZero(&pp);
_pp[0][0] = _pp[1][1] = fc_new; _pp[0][0] = _pp[1][1] = fc_new;
_pp[0][2] = cc_new[0].x; _pp[0][2] = cc_new[0].x;
_pp[1][2] = cc_new[0].y; _pp[1][2] = cc_new[0].y;
@ -198,62 +194,36 @@ void 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
cvConvert(&pp, _P2); cvConvert(&pp, _P2);
alpha = MIN(alpha, 1.); alpha = MIN(alpha, 1.);
//icvGetRectangles( K1, D1, _R1, _P1, imageSize, inner1, outer1 );
//icvGetRectangles( K2, D2, _R2, _P2, imageSize, inner2, outer2 );
{ {
newImgSize = newImgSize.width*newImgSize.height != 0 ? newImgSize : imageSize; newImgSize = newImgSize.width*newImgSize.height != 0 ? newImgSize : imageSize;
double cx1_0 = cc_new[0].x; double cx1_0 = cc_new[0].x;
double cy1_0 = cc_new[0].y; double cy1_0 = cc_new[0].y;
double cx2_0 = cc_new[1].x; double cx2_0 = cc_new[1].x;
double cy2_0 = cc_new[1].y; double cy2_0 = cc_new[1].y;
double cx1 = newImgSize.width*cx1_0/imageSize.width; double cx1 = newImgSize.width*cx1_0/imageSize.width;
double cy1 = newImgSize.height*cy1_0/imageSize.height; double cy1 = newImgSize.height*cy1_0/imageSize.height;
double cx2 = newImgSize.width*cx2_0/imageSize.width; double cx2 = newImgSize.width*cx2_0/imageSize.width;
double cy2 = newImgSize.height*cy2_0/imageSize.height; double cy2 = newImgSize.height*cy2_0/imageSize.height;
double s = 1.; double s = 1.;
//if( alpha >= 0 ) fc_new *= s;
//{ cc_new[0] = cvPoint2D64f(cx1, cy1);
// double s0 = std::max(std::max(std::max((double)cx1/(cx1_0 - inner1.x), (double)cy1/(cy1_0 - inner1.y)), cc_new[1] = cvPoint2D64f(cx2, cy2);
// (double)(newImgSize.width - cx1)/(inner1.x + inner1.width - cx1_0)),
// (double)(newImgSize.height - cy1)/(inner1.y + inner1.height - cy1_0));
// s0 = std::max(std::max(std::max(std::max((double)cx2/(cx2_0 - inner2.x), (double)cy2/(cy2_0 - inner2.y)),
// (double)(newImgSize.width - cx2)/(inner2.x + inner2.width - cx2_0)),
// (double)(newImgSize.height - cy2)/(inner2.y + inner2.height - cy2_0)),
// s0);
// double s1 = std::min(std::min(std::min((double)cx1/(cx1_0 - outer1.x), (double)cy1/(cy1_0 - outer1.y)), cvmSet(_P1, 0, 0, fc_new);
// (double)(newImgSize.width - cx1)/(outer1.x + outer1.width - cx1_0)), cvmSet(_P1, 1, 1, fc_new);
// (double)(newImgSize.height - cy1)/(outer1.y + outer1.height - cy1_0)); cvmSet(_P1, 0, 2, cx1);
// s1 = std::min(std::min(std::min(std::min((double)cx2/(cx2_0 - outer2.x), (double)cy2/(cy2_0 - outer2.y)), cvmSet(_P1, 1, 2, cy1);
// (double)(newImgSize.width - cx2)/(outer2.x + outer2.width - cx2_0)),
// (double)(newImgSize.height - cy2)/(outer2.y + outer2.height - cy2_0)),
// s1);
// s = s0*(1 - alpha) + s1*alpha; cvmSet(_P2, 0, 0, fc_new);
//} cvmSet(_P2, 1, 1, fc_new);
cvmSet(_P2, 0, 2, cx2);
fc_new *= s; cvmSet(_P2, 1, 2, cy2);
cc_new[0] = cvPoint2D64f(cx1, cy1); cvmSet(_P2, idx, 3, s*cvmGet(_P2, idx, 3));
cc_new[1] = cvPoint2D64f(cx2, cy2);
cvmSet(_P1, 0, 0, fc_new);
cvmSet(_P1, 1, 1, fc_new);
cvmSet(_P1, 0, 2, cx1);
cvmSet(_P1, 1, 2, cy1);
cvmSet(_P2, 0, 0, fc_new);
cvmSet(_P2, 1, 1, fc_new);
cvmSet(_P2, 0, 2, cx2);
cvmSet(_P2, 1, 2, cy2);
cvmSet(_P2, idx, 3, s*cvmGet(_P2, idx, 3));
} }
} }
@ -277,8 +247,9 @@ Eigen::Matrix4d loadT(const mynteye::Extrinsics& in) {
return T; return T;
} }
void loadCameraMatrix(cv::Mat& K, cv::Mat& D, cv::Size& image_size, void loadCameraMatrix(cv::Mat& K, cv::Mat& D, // NOLINT
struct camera_info& calib_data) { cv::Size& image_size, // NOLINT
struct camera_info& calib_data) { // NOLINT
K = cv::Mat(3, 3, CV_64F, calib_data.K); K = cv::Mat(3, 3, CV_64F, calib_data.K);
std::size_t d_length = 4; std::size_t d_length = 4;
D = cv::Mat(1, d_length, CV_64F, calib_data.D); D = cv::Mat(1, d_length, CV_64F, calib_data.D);
@ -335,16 +306,16 @@ struct camera_mat_info_pair stereoRectify(
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);
std::cout << "K1: " << K1 << std::endl; // std::cout << "K1: " << K1 << std::endl;
std::cout << "D1: " << D1 << std::endl; // std::cout << "D1: " << D1 << std::endl;
std::cout << "K2: " << K2 << std::endl; // std::cout << "K2: " << K2 << std::endl;
std::cout << "D2: " << D2 << std::endl; // std::cout << "D2: " << D2 << std::endl;
std::cout << "R: " << cv_R << std::endl; // std::cout << "R: " << cv_R << std::endl;
std::cout << "t: " << cv_t << std::endl; // std::cout << "t: " << cv_t << std::endl;
std::cout << "R1: " << R1 << std::endl; // std::cout << "R1: " << R1 << std::endl;
std::cout << "R2: " << R2 << std::endl; // std::cout << "R2: " << R2 << std::endl;
std::cout << "P1: " << P1 << std::endl; // std::cout << "P1: " << P1 << std::endl;
std::cout << "P2: " << P2 << std::endl; // std::cout << "P2: " << P2 << std::endl;
R1 = rectifyrad(R1); R1 = rectifyrad(R1);
R2 = rectifyrad(R2); R2 = rectifyrad(R2);
@ -441,7 +412,7 @@ bool RectifyProcessor::OnProcess(
output->first_data = input->first_data; output->first_data = input->first_data;
output->second_id = input->second_id; output->second_id = input->second_id;
output->second_data = input->second_data; output->second_data = input->second_data;
return true; return false;
#else #else
return false; return false;
#endif #endif
@ -490,23 +461,6 @@ void RectifyProcessor::InitParams(
#ifdef WITH_CAM_MODELS #ifdef WITH_CAM_MODELS
camodocal::CameraPtr getCamOdoCameraPtr(const struct camera_info& calib_mat_data) {
std::string camera_model = calib_mat_data.distortion_model;
int w = calib_mat_data.width;
int h = calib_mat_data.height;
double fx = calib_mat_data.K[0];
double fy = calib_mat_data.K[4];
double cx = calib_mat_data.K[2];
double cy = calib_mat_data.K[5];
camodocal::EquidistantCameraPtr camera(new camodocal::EquidistantCamera);
camodocal::EquidistantCamera::Parameters params(camera_model, w, h, calib_mat_data.D[0],
calib_mat_data.D[1], calib_mat_data.D[2], calib_mat_data.D[3], fx, fy, cx, cy);
camera->setParameters(params);
return camera;
}
camodocal::CameraPtr generateCameraFromIntrinsicsEquidistant( camodocal::CameraPtr generateCameraFromIntrinsicsEquidistant(
const mynteye::IntrinsicsEquidistant & in) { const mynteye::IntrinsicsEquidistant & in) {
camodocal::EquidistantCameraPtr camera( camodocal::EquidistantCameraPtr camera(
@ -528,24 +482,18 @@ void RectifyProcessor::InitParams(
IntrinsicsEquidistant in_left, IntrinsicsEquidistant in_left,
IntrinsicsEquidistant in_right, IntrinsicsEquidistant in_right,
Extrinsics ex_right_to_left) { Extrinsics ex_right_to_left) {
calib_model = CalibrationModel::KANNALA_BRANDT; calib_model = CalibrationModel::KANNALA_BRANDT;
camodocal::CameraPtr camera_odo_ptr_left_o = camodocal::CameraPtr camera_odo_ptr_left =
generateCameraFromIntrinsicsEquidistant(in_left); generateCameraFromIntrinsicsEquidistant(in_left);
camodocal::CameraPtr camera_odo_ptr_right_o = camodocal::CameraPtr camera_odo_ptr_right =
generateCameraFromIntrinsicsEquidistant(in_right); generateCameraFromIntrinsicsEquidistant(in_right);
struct camera_mat_info_pair calib_info_pair = struct camera_mat_info_pair calib_info_pair =
stereoRectify(camera_odo_ptr_left_o, stereoRectify(camera_odo_ptr_left,
camera_odo_ptr_right_o, camera_odo_ptr_right,
in_left, in_left,
in_right, in_right,
ex_right_to_left); ex_right_to_left);
camodocal::CameraPtr camera_odo_ptr_left =
getCamOdoCameraPtr(calib_info_pair.left);
camodocal::CameraPtr camera_odo_ptr_right =
getCamOdoCameraPtr(calib_info_pair.right);
cv::Mat rect_R_l = cv::Mat rect_R_l =
cv::Mat::eye(3, 3, CV_32F), rect_R_r = cv::Mat::eye(3, 3, CV_32F); cv::Mat::eye(3, 3, CV_32F), rect_R_r = cv::Mat::eye(3, 3, CV_32F);
for (size_t i = 0; i < 3; i++) { for (size_t i = 0; i < 3; i++) {