diff --git a/src/mynteye/api/processor/rectify_processor.cc b/src/mynteye/api/processor/rectify_processor.cc index 39252a5..66baf24 100644 --- a/src/mynteye/api/processor/rectify_processor.cc +++ b/src/mynteye/api/processor/rectify_processor.cc @@ -23,32 +23,55 @@ // #define WITH_CAM_MODELS #ifdef WITH_CAM_MODELS + #include #include #include #include -#include #include -#include -#include -#include -#include -#include -#include -#include +#include +#include -std::string _left_camera_yaml; -std::string _right_camera_yaml; -std::map _cam_odo_ptr; +struct camera_info { + unsigned int height = 0; + unsigned int width = 0; + std::string distortion_model = "null"; + float D[4] = {0}; + float K[9] = {0}; + float R[9] = {0}; + float P[12] = {0}; +}; -void stereoRectify(const CvMat* K1, const CvMat* K2, +struct camera_mat_info_pair { + struct camera_info left; + struct camera_info right; +}; + +cv::Mat rectifyrad(const cv::Mat& R) { + cv::Mat r_vec; + cv::Rodrigues(R, r_vec); + // pi/180 = x/179 ==> x = 3.1241 + double rad = cv::norm(r_vec); + if (rad >= 3.1241) { + cv::Mat r_dir; + cv::normalize(r_vec, r_dir); + cv::Mat r = r_dir*(3.1415926 - rad); + cv::Mat r_r; + cv::Rodrigues(r, r_r); + return r_r.clone(); + } + return R.clone(); +} + +void stereoRectify(camodocal::CameraPtr leftOdo, + camodocal::CameraPtr rightOdo, const CvMat* K1, const CvMat* K2, const CvMat* D1, const CvMat* D2, CvSize imageSize, const CvMat* matR, const CvMat* matT, CvMat* _R1, CvMat* _R2, CvMat* _P1, CvMat* _P2, int flags = cv::CALIB_ZERO_DISPARITY, double alpha = -1, CvSize newImgSize = cv::Size()) { - 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 _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]; cv::Rect_ inner1, inner2, outer1, outer2; CvMat om = cvMat(3, 1, CV_64F, _om); @@ -56,20 +79,20 @@ void stereoRectify(const CvMat* K1, const CvMat* K2, CvMat uu = cvMat(3, 1, CV_64F, _uu); CvMat r_r = cvMat(3, 3, CV_64F, _r_r); CvMat pp = cvMat(3, 4, CV_64F, _pp); - CvMat ww = cvMat(3, 1, CV_64F, _ww); // temps - CvMat w3 = cvMat(3, 1, CV_64F, _w3); // temps + CvMat ww = cvMat(3, 1, CV_64F, _ww); // temps + CvMat w3 = cvMat(3, 1, CV_64F, _w3); // temps CvMat wR = cvMat(3, 3, CV_64F, _wr); CvMat Z = cvMat(3, 1, CV_64F, _z); CvMat Ri = cvMat(3, 3, CV_64F, _ri); double nx = imageSize.width, ny = imageSize.height; int i, k; double nt, nw; - if( matR->rows == 3 && matR->cols == 3 ) + if ( matR->rows == 3 && matR->cols == 3) cvRodrigues2(matR, &om); // get vector rotation else - cvConvert(matR, &om); // it's already a rotation vector - cvConvertScale(&om, &om, -0.5); // get average rotation - cvRodrigues2(&om, &r_r); // rotate cameras to same orientation by averaging + cvConvert(matR, &om); // it's already a rotation vector + cvConvertScale(&om, &om, -0.5); // get average rotation + cvRodrigues2(&om, &r_r); // rotate cameras to same orientation by averaging cvMatMul(&r_r, matT, &t); int idx = fabs(_t[0]) > fabs(_t[1]) ? 0 : 1; @@ -94,8 +117,7 @@ void stereoRectify(const CvMat* K1, const CvMat* K2, cvConvertScale(&w3, &w3, 1 / nw); _uu[2] = 0; - for (i = 0; i < 3; ++i) - { + 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 @@ -131,12 +153,10 @@ void stereoRectify(const CvMat* K1, const CvMat* K2, int j = (i<2) ? 0 : 1; a.x() = (float)((i % 2)*(nx)); a.y() = (float)(j*(ny)); - if (0 == k){ - _cam_odo_ptr["LEFT"]->liftProjective(a, b); - } - else{ - - _cam_odo_ptr["RIGHT"]->liftProjective(a, b); + 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(); @@ -234,22 +254,129 @@ void stereoRectify(const CvMat* K1, const CvMat* K2, cvmSet(_P2, 0, 2, cx2); cvmSet(_P2, 1, 2, cy2); cvmSet(_P2, idx, 3, s*cvmGet(_P2, idx, 3)); + } +} +Eigen::Matrix4d loadT(const mynteye::Extrinsics& in) { + Eigen::Matrix3d R; + R<< + in.rotation[0][0], in.rotation[0][1], in.rotation[0][2], + in.rotation[1][0], in.rotation[1][1], in.rotation[1][2], + in.rotation[2][0], in.rotation[2][1], in.rotation[2][2]; + + double t_x = in.translation[0]; + double t_y = in.translation[1]; + double t_z = in.translation[2]; + + Eigen::Quaterniond q(R); + q.normalize(); + Eigen::Matrix4d T = Eigen::Matrix4d::Identity(); + T.topLeftCorner<3, 3>() = q.toRotationMatrix(); + T.topRightCorner<3, 1>() << t_x, t_y, t_z; + + return T; +} + +void loadCameraMatrix(cv::Mat& K, cv::Mat& D, cv::Size& image_size, + struct camera_info& calib_data) { + 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); + image_size = cv::Size(calib_data.width, calib_data.height); +} + +struct camera_info getCalibMatData(const mynteye::IntrinsicsEquidistant& in) { + struct camera_info calib_mat_data; + calib_mat_data.distortion_model = "KANNALA_BRANDT"; + calib_mat_data.height = in.height; + calib_mat_data.width = in.width; + + for (unsigned int i = 0; i < 4; i++) { + calib_mat_data.D[i] = in.coeffs[i]; } + calib_mat_data.K[0] = in.coeffs[4]; // mu + calib_mat_data.K[4] = in.coeffs[5]; // mv(); + calib_mat_data.K[2] = in.coeffs[6]; // u0(); + calib_mat_data.K[5] = in.coeffs[7]; // v0(); + calib_mat_data.K[8] = 1; + return calib_mat_data; } + +struct camera_mat_info_pair stereoRectify( + camodocal::CameraPtr leftOdo, + camodocal::CameraPtr rightOdo, + mynteye::IntrinsicsEquidistant in_left, + mynteye::IntrinsicsEquidistant in_right, + mynteye::Extrinsics ex_right_to_left) { + Eigen::Matrix4d T = loadT(ex_right_to_left); + Eigen::Matrix3d R = T.topLeftCorner<3, 3>(); + Eigen::Vector3d t = T.topRightCorner<3, 1>(); + cv::Mat cv_R, cv_t; + cv::eigen2cv(R, cv_R); + cv::eigen2cv(t, cv_t); + cv::Mat K1, D1, K2, D2; + cv::Size image_size1, image_size2; + + struct camera_info calib_mat_data_left = getCalibMatData(in_left); + struct camera_info calib_mat_data_right = getCalibMatData(in_right); + + loadCameraMatrix(K1, D1, image_size1, calib_mat_data_left); + loadCameraMatrix(K2, D2, image_size2, calib_mat_data_right); + + cv::Mat R1 = cv::Mat(cv::Size(3, 3), CV_64F); + cv::Mat R2 = cv::Mat(cv::Size(3, 3), CV_64F); + cv::Mat P1 = cv::Mat(3, 4, CV_64F); + cv::Mat P2 = cv::Mat(3, 4, CV_64F); + + CvMat c_R = cv_R, c_t = cv_t; + 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; + 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; + + R1 = rectifyrad(R1); + R2 = rectifyrad(R2); + + for (std::size_t i = 0; i < 3; i++) { + for (std::size_t j = 0; j < 4; j++) { + calib_mat_data_left.P[i*4 + j] = P1.at(i, j); + calib_mat_data_right.P[i*4 + j] = P2.at(i, j); + } + } + + for (std::size_t i = 0; i < 3; i++) { + for (std::size_t j = 0; j < 3; j++) { + calib_mat_data_left.R[i*3 + j] = R1.at(i, j); + calib_mat_data_right.R[i*3 +j] = R2.at(i, j); + } + } + struct camera_mat_info_pair res = {calib_mat_data_left, calib_mat_data_right}; + return res; +} + #endif MYNTEYE_BEGIN_NAMESPACE - - const char RectifyProcessor::NAME[] = "RectifyProcessor"; RectifyProcessor::RectifyProcessor( std::shared_ptr device, std::int32_t proc_period) : Processor(std::move(proc_period)), device_(device) { VLOG(2) << __func__ << ": proc_period=" << proc_period; + calib_model = CalibrationModel::UNKNOW; NotifyImageParamsChanged(); } @@ -272,19 +399,18 @@ void RectifyProcessor::NotifyImageParamsChanged() { } else if (in_left->calib_model() == CalibrationModel::KANNALA_BRANDT) { #ifdef WITH_CAM_MODELS - _left_camera_yaml = "./camera_left.yaml"; - _right_camera_yaml = "./camera_left.yaml"; - _cam_odo_ptr["LEFT"] = camodocal::CameraFactory::instance()->generateCameraFromYamlFile(_left_camera_yaml); - _cam_odo_ptr["RIGHT"] = camodocal::CameraFactory::instance()->generateCameraFromYamlFile(_right_camera_yaml); InitParams( *std::dynamic_pointer_cast(in_left), *std::dynamic_pointer_cast(in_right), device_->GetExtrinsics(Stream::RIGHT, Stream::LEFT)); #else - VLOG(2) << __func__ << "KANNALA_BRANDT macro seitch is not on"; + VLOG(2) << "calib model type KANNALA_BRANDT" + << " is not been enabled."; #endif } else { - // todo + VLOG(2) << "calib model type " + << in_left->calib_model() + <<" is not been enabled."; } } @@ -295,21 +421,39 @@ Object *RectifyProcessor::OnCreateOutput() { bool RectifyProcessor::OnProcess( Object *const in, Object *const out, Processor *const parent) { MYNTEYE_UNUSED(parent) - const ObjMat2 *input = Object::Cast(in); - ObjMat2 *output = Object::Cast(out); - cv::remap(input->first, output->first, map11, map12, cv::INTER_LINEAR); - cv::remap(input->second, output->second, map21, map22, cv::INTER_LINEAR); - output->first_id = input->first_id; - output->first_data = input->first_data; - output->second_id = input->second_id; - output->second_data = input->second_data; - return true; + if (calib_model == CalibrationModel::PINHOLE) { + const ObjMat2 *input = Object::Cast(in); + ObjMat2 *output = Object::Cast(out); + cv::remap(input->first, output->first, map11, map12, cv::INTER_LINEAR); + cv::remap(input->second, output->second, map21, map22, cv::INTER_LINEAR); + output->first_id = input->first_id; + output->first_data = input->first_data; + output->second_id = input->second_id; + output->second_data = input->second_data; + return true; + } else if (calib_model == CalibrationModel::KANNALA_BRANDT) { +#ifdef WITH_CAM_MODELS + const ObjMat2 *input = Object::Cast(in); + ObjMat2 *output = Object::Cast(out); + std::cout << in <first, output->first, map11, map12, cv::INTER_LINEAR); + // cv::remap(input->second, output->second, map21, map22, cv::INTER_LINEAR); + output->first_id = input->first_id; + output->first_data = input->first_data; + output->second_id = input->second_id; + output->second_data = input->second_data; + return true; +#else + return false; +#endif + } } void RectifyProcessor::InitParams( IntrinsicsPinhole in_left, IntrinsicsPinhole in_right, Extrinsics ex_right_to_left) { + calib_model = CalibrationModel::PINHOLE; cv::Size size{in_left.width, in_left.height}; cv::Mat M1 = @@ -345,45 +489,111 @@ void RectifyProcessor::InitParams( cv::initUndistortRectifyMap(M2, D2, R2, P2, size, CV_16SC2, map21, map22); } +#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( + new camodocal::EquidistantCamera("KANNALA_BRANDT", + in.width, + in.height, + in.coeffs[0], + in.coeffs[1], + in.coeffs[2], + in.coeffs[3], + in.coeffs[4], + in.coeffs[5], + in.coeffs[6], + in.coeffs[7])); + return camera; +} void RectifyProcessor::InitParams( IntrinsicsEquidistant in_left, IntrinsicsEquidistant in_right, Extrinsics ex_right_to_left) { - cv::Size size{in_left.width, in_left.height}; - cv::Mat M1 = - (cv::Mat_(3, 3) << 1.0, 0, 0.0, 0, 1.0, - 0.0, 0, 0, 1); - cv::Mat M2 = - (cv::Mat_(3, 3) << 1.0, 0, 0.0, 0, 1.0, - 0.0, 0, 0, 1); - cv::Mat D1(1, 8, CV_64F, in_left.coeffs); - cv::Mat D2(1, 8, CV_64F, in_right.coeffs); - cv::Mat R = - (cv::Mat_(3, 3) << ex_right_to_left.rotation[0][0], - ex_right_to_left.rotation[0][1], ex_right_to_left.rotation[0][2], - ex_right_to_left.rotation[1][0], ex_right_to_left.rotation[1][1], - ex_right_to_left.rotation[1][2], ex_right_to_left.rotation[2][0], - ex_right_to_left.rotation[2][1], ex_right_to_left.rotation[2][2]); - cv::Mat T(3, 1, CV_64F, ex_right_to_left.translation); + calib_model = CalibrationModel::KANNALA_BRANDT; + camodocal::CameraPtr camera_odo_ptr_left_o = + generateCameraFromIntrinsicsEquidistant(in_left); + camodocal::CameraPtr camera_odo_ptr_right_o = + generateCameraFromIntrinsicsEquidistant(in_right); + struct camera_mat_info_pair calib_info_pair = + stereoRectify(camera_odo_ptr_left_o, + camera_odo_ptr_right_o, + in_left, + in_right, + ex_right_to_left); - VLOG(2) << "InitParams size: " << size; - VLOG(2) << "M1: " << M1; - VLOG(2) << "M2: " << M2; - VLOG(2) << "D1: " << D1; - VLOG(2) << "D2: " << D2; - VLOG(2) << "R: " << R; - VLOG(2) << "T: " << T; + camodocal::CameraPtr camera_odo_ptr_left = + getCamOdoCameraPtr(calib_info_pair.left); + camodocal::CameraPtr camera_odo_ptr_right = + getCamOdoCameraPtr(calib_info_pair.right); - cv::Rect left_roi, right_roi; - cv::stereoRectify( - M1, D1, M2, D2, size, R, T, R1, R2, P1, P2, Q, cv::CALIB_ZERO_DISPARITY, - 0, size, &left_roi, &right_roi); + // for (int i = 0; i < 4 ; i ++) { + // std::cout << " D" << i << ": " << calib_info_pair.left.D[i]; + // } + // for (int i = 0; i < 9 ; i ++) { + // std::cout << " K" << i << ": " << calib_info_pair.left.K[i]; + // } + // for (int i = 0; i < 9 ; i ++) { + // std::cout << " R" << i << ": " << calib_info_pair.left.R[i]; + // } + // for (int i = 0; i < 12 ; i ++) { + // if (i == 0) { + // calib_info_pair.left.P[i] = 361.184; + // } + // if (i == 5) { + // calib_info_pair.left.P[i] = 361.184; + // } + // std::cout << " P" << i << ": " << calib_info_pair.left.P[i]; + // } - cv::initUndistortRectifyMap(M1, D1, R1, P1, size, CV_16SC2, map11, map12); - cv::initUndistortRectifyMap(M2, D2, R2, P2, size, CV_16SC2, map21, map22); + // D0: -0.0167962 D1: -0.0434362 D2: 0.0877926 D3: -0.0569189 K0: 361.172 K1: 0 K2: 381.437 K3: 0 K4: 361.2 K5: 244.849 K6: 0 K7: 0 K8: 1 R0: 0.999997 R1: 0.00230108 R2: 0.00118774 R3: -0.00229864 R4: 0.999995 R5: -0.00205134 R6: -0.00119245 R7: 0.0020486 R8: 0.999997 P0: 361.184 P1: 0 P2: 337.803 P3: 0 P4: 0 P5: 361.184 P6: 247.574 P7: 0 P8: 0 P9: 0 P10: 1 P11: 0 + // D0: -0.0167962 D1: -0.0434362 D2: 0.0877926 D3: -0.0569189 K0: 361.172 K1: 0 K2: 381.437 K3: 0 K4: 361.2 K5: 244.849 K6: 0 K7: 0 K8: 1 R0: 1 R1: -0.000291099 R2: -2.98628e-07 R3: 0.000291099 R4: 1 R5: 1.82261e-07 R6: 2.98575e-07 R7: -1.82348e-07 R8: 1 P0: 0 P1: 0 P2: 376 P3: 0 P4: 0 P5: 0 P6: 240 P7: 0 P8: 0 P9: 0 P10: 1 P11: 0 + 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++) { + for (size_t j = 0; j < 3; j++) { + rect_R_l.at(i, j) = calib_info_pair.left.R[i*3+j]; + rect_R_r.at(i, j) = calib_info_pair.right.R[i*3+j]; + } + } + double left_f[] = + {calib_info_pair.left.P[0], calib_info_pair.left.P[5]}; + double left_center[] = + {calib_info_pair.left.P[2], calib_info_pair.left.P[6]}; + double right_f[] = + {calib_info_pair.right.P[0], calib_info_pair.right.P[5]}; + double right_center[] = + {calib_info_pair.right.P[2], calib_info_pair.right.P[6]}; + camera_odo_ptr_left->initUndistortRectifyMap( + map11, map12, left_f[0], left_f[1], + cv::Size(0, 0), left_center[0], + left_center[1], rect_R_l); + camera_odo_ptr_right->initUndistortRectifyMap( + map21, map22, right_f[0], right_f[1], + cv::Size(0, 0), right_center[0], + right_center[1], rect_R_r); + std::cout << map11 << std::endl; } +#endif MYNTEYE_END_NAMESPACE diff --git a/src/mynteye/api/processor/rectify_processor.h b/src/mynteye/api/processor/rectify_processor.h index 813bb64..cdfd3b5 100644 --- a/src/mynteye/api/processor/rectify_processor.h +++ b/src/mynteye/api/processor/rectify_processor.h @@ -22,6 +22,15 @@ #include "mynteye/types.h" #include "mynteye/api/processor.h" +#ifdef WITH_CAM_MODELS +#include +#include +#include +#include +#include +#include +#include +#endif MYNTEYE_BEGIN_NAMESPACE @@ -54,6 +63,7 @@ class RectifyProcessor : public Processor { IntrinsicsEquidistant in_right, Extrinsics ex_right_to_left); std::shared_ptr device_; + CalibrationModel calib_model; }; MYNTEYE_END_NAMESPACE