diff --git a/src/mynteye/api/processor/points_processor.cc b/src/mynteye/api/processor/points_processor.cc index 7d2b6d4..b82dd32 100644 --- a/src/mynteye/api/processor/points_processor.cc +++ b/src/mynteye/api/processor/points_processor.cc @@ -41,13 +41,17 @@ Object *PointsProcessor::OnCreateOutput() { } 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) const ObjMat *input = Object::Cast(in); ObjMat *output = Object::Cast(out); cv::reprojectImageTo3D(input->value, output->value, Q_, true); output->id = input->id; output->data = input->data; +#endif return true; } diff --git a/src/mynteye/api/processor/rectify_processor.cc b/src/mynteye/api/processor/rectify_processor.cc index ea27be8..c8b47a2 100644 --- a/src/mynteye/api/processor/rectify_processor.cc +++ b/src/mynteye/api/processor/rectify_processor.cc @@ -120,76 +120,72 @@ void stereoRectify(camodocal::CameraPtr leftOdo, for (i = 0; i < 3; ++i) { _wr[idx][i] = -_t[i] / nt; _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 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); - cvConvert( &Ri, _R2 ); + cvConvert(&Ri, _R2); cvMatMul(&Ri, matT, &t); // calculate projection/camera matrices // these contain the relevant rectified image internal params (fx, fy=fx, cx, cy) 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; const double ratio_x = (double)newImgSize.width / imageSize.width / 2; const double ratio_y = (double)newImgSize.height / imageSize.height / 2; const double ratio = idx == 1 ? ratio_x : ratio_y; fc_new = (cvmGet(K1, idx ^ 1, idx ^ 1) + cvmGet(K2, idx ^ 1, idx ^ 1)) * ratio; - for( k = 0; k < 2; k++ ) - { - CvPoint2D32f _pts[4]; - CvPoint3D32f _pts_3[4]; - CvMat pts = cvMat(1, 4, CV_32FC2, _pts); - CvMat pts_3 = cvMat(1, 4, CV_32FC3, _pts_3); - Eigen::Vector2d a; - Eigen::Vector3d b; - for( i = 0; i < 4; i++ ) - { - int j = (i<2) ? 0 : 1; - a.x() = (float)((i % 2)*(nx)); - a.y() = (float)(j*(ny)); - if (0 == k) { - leftOdo->liftProjective(a, b); - } else { - rightOdo->liftProjective(a, b); - } - _pts[i].x = b.x()/b.z(); - _pts[i].y = b.y()/b.z(); + for (k = 0; k < 2; k++) { + CvPoint2D32f _pts[4]; + CvPoint3D32f _pts_3[4]; + CvMat pts = cvMat(1, 4, CV_32FC2, _pts); + CvMat pts_3 = cvMat(1, 4, CV_32FC3, _pts_3); + Eigen::Vector2d a; + Eigen::Vector3d b; + for (i = 0; i < 4; i++) { + int j = (i < 2) ? 0 : 1; + a.x() = (float)((i % 2)*(nx)); + a.y() = (float)(j*(ny)); + if (0 == k) { + leftOdo->liftProjective(a, b); + } else { + rightOdo->liftProjective(a, b); } - 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 - double _a_tmp[3][3]; - CvMat A_tmp = cvMat(3, 3, CV_64F, _a_tmp); - _a_tmp[0][0]=fc_new; - _a_tmp[1][1]=fc_new; - _a_tmp[0][2]=0.0; - _a_tmp[1][2]=0.0; - cvProjectPoints2( &pts_3, k == 0 ? _R1 : _R2, &Z, &A_tmp, 0, &pts ); - CvScalar avg = cvAvg(&pts); - cc_new[k].x = (nx)/2 - avg.val[0]; - cc_new[k].y = (ny)/2 - avg.val[1]; - - //cc_new[k].x = (nx)/2; - //cc_new[k].y = (ny)/2; + // Change camera matrix to have cc=[0,0] and fc = fc_new + double _a_tmp[3][3]; + CvMat A_tmp = cvMat(3, 3, CV_64F, _a_tmp); + _a_tmp[0][0] = fc_new; + _a_tmp[1][1] = fc_new; + _a_tmp[0][2] = 0.0; + _a_tmp[1][2] = 0.0; + cvProjectPoints2(&pts_3, k == 0 ? _R1 : _R2, &Z, &A_tmp, 0, &pts); + CvScalar avg = cvAvg(&pts); + cc_new[k].x = (nx)/2 - avg.val[0]; + cc_new[k].y = (ny)/2 - avg.val[1]; } - 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].y = cc_new[1].y = (cc_new[0].y + cc_new[1].y)*0.5; + 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].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][2] = cc_new[0].x; _pp[1][2] = cc_new[0].y; @@ -198,62 +194,36 @@ void stereoRectify(camodocal::CameraPtr leftOdo, _pp[0][2] = cc_new[1].x; _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); - 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; - double cx1_0 = cc_new[0].x; - double cy1_0 = cc_new[0].y; - double cx2_0 = cc_new[1].x; - double cy2_0 = cc_new[1].y; - double cx1 = newImgSize.width*cx1_0/imageSize.width; - double cy1 = newImgSize.height*cy1_0/imageSize.height; - double cx2 = newImgSize.width*cx2_0/imageSize.width; - double cy2 = newImgSize.height*cy2_0/imageSize.height; - double s = 1.; + newImgSize = newImgSize.width*newImgSize.height != 0 ? newImgSize : imageSize; + double cx1_0 = cc_new[0].x; + double cy1_0 = cc_new[0].y; + double cx2_0 = cc_new[1].x; + double cy2_0 = cc_new[1].y; + double cx1 = newImgSize.width*cx1_0/imageSize.width; + double cy1 = newImgSize.height*cy1_0/imageSize.height; + double cx2 = newImgSize.width*cx2_0/imageSize.width; + double cy2 = newImgSize.height*cy2_0/imageSize.height; + double s = 1.; - //if( alpha >= 0 ) - //{ - // double s0 = std::max(std::max(std::max((double)cx1/(cx1_0 - inner1.x), (double)cy1/(cy1_0 - inner1.y)), - // (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); + fc_new *= s; + cc_new[0] = cvPoint2D64f(cx1, cy1); + cc_new[1] = cvPoint2D64f(cx2, cy2); - // double s1 = std::min(std::min(std::min((double)cx1/(cx1_0 - outer1.x), (double)cy1/(cy1_0 - outer1.y)), - // (double)(newImgSize.width - cx1)/(outer1.x + outer1.width - cx1_0)), - // (double)(newImgSize.height - cy1)/(outer1.y + outer1.height - cy1_0)); - // s1 = std::min(std::min(std::min(std::min((double)cx2/(cx2_0 - outer2.x), (double)cy2/(cy2_0 - outer2.y)), - // (double)(newImgSize.width - cx2)/(outer2.x + outer2.width - cx2_0)), - // (double)(newImgSize.height - cy2)/(outer2.y + outer2.height - cy2_0)), - // s1); + cvmSet(_P1, 0, 0, fc_new); + cvmSet(_P1, 1, 1, fc_new); + cvmSet(_P1, 0, 2, cx1); + cvmSet(_P1, 1, 2, cy1); - // s = s0*(1 - alpha) + s1*alpha; - //} - - fc_new *= s; - cc_new[0] = cvPoint2D64f(cx1, cy1); - 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)); + 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; } -void loadCameraMatrix(cv::Mat& K, cv::Mat& D, cv::Size& image_size, - struct camera_info& calib_data) { +void loadCameraMatrix(cv::Mat& K, cv::Mat& D, // NOLINT + cv::Size& image_size, // NOLINT + struct camera_info& calib_data) { // NOLINT K = cv::Mat(3, 3, CV_64F, calib_data.K); std::size_t d_length = 4; 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, image_size1, &c_R, &c_t, &c_R1, &c_R2, &c_P1, &c_P2); - std::cout << "K1: " << K1 << std::endl; - std::cout << "D1: " << D1 << std::endl; - std::cout << "K2: " << K2 << std::endl; - std::cout << "D2: " << D2 << std::endl; - std::cout << "R: " << cv_R << std::endl; - std::cout << "t: " << cv_t << std::endl; - std::cout << "R1: " << R1 << std::endl; - std::cout << "R2: " << R2 << std::endl; - std::cout << "P1: " << P1 << std::endl; - std::cout << "P2: " << P2 << std::endl; + // std::cout << "K1: " << K1 << std::endl; + // std::cout << "D1: " << D1 << std::endl; + // std::cout << "K2: " << K2 << std::endl; + // std::cout << "D2: " << D2 << std::endl; + // std::cout << "R: " << cv_R << std::endl; + // std::cout << "t: " << cv_t << std::endl; + // std::cout << "R1: " << R1 << std::endl; + // std::cout << "R2: " << R2 << std::endl; + // std::cout << "P1: " << P1 << std::endl; + // std::cout << "P2: " << P2 << std::endl; R1 = rectifyrad(R1); R2 = rectifyrad(R2); @@ -441,7 +412,7 @@ bool RectifyProcessor::OnProcess( output->first_data = input->first_data; output->second_id = input->second_id; output->second_data = input->second_data; - return true; + return false; #else return false; #endif @@ -490,23 +461,6 @@ void RectifyProcessor::InitParams( #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( const mynteye::IntrinsicsEquidistant & in) { camodocal::EquidistantCameraPtr camera( @@ -528,24 +482,18 @@ void RectifyProcessor::InitParams( IntrinsicsEquidistant in_left, IntrinsicsEquidistant in_right, Extrinsics ex_right_to_left) { - calib_model = CalibrationModel::KANNALA_BRANDT; - camodocal::CameraPtr camera_odo_ptr_left_o = + camodocal::CameraPtr camera_odo_ptr_left = generateCameraFromIntrinsicsEquidistant(in_left); - camodocal::CameraPtr camera_odo_ptr_right_o = + camodocal::CameraPtr camera_odo_ptr_right = generateCameraFromIntrinsicsEquidistant(in_right); struct camera_mat_info_pair calib_info_pair = - stereoRectify(camera_odo_ptr_left_o, - camera_odo_ptr_right_o, + stereoRectify(camera_odo_ptr_left, + camera_odo_ptr_right, in_left, in_right, 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::eye(3, 3, CV_32F), rect_R_r = cv::Mat::eye(3, 3, CV_32F); for (size_t i = 0; i < 3; i++) {