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

feat(calib model): add KANNALA_BRANDT code

Browse Source
This commit is contained in:
TinyOh 2019-01-07 17:11:01 +08:00
parent 675f5ff97f
commit a8c52fe000
2 changed files with 294 additions and 74 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -23,32 +23,55 @@
// #define WITH_CAM_MODELS // #define WITH_CAM_MODELS
#ifdef WITH_CAM_MODELS #ifdef WITH_CAM_MODELS
#include <boost/algorithm/string.hpp> #include <boost/algorithm/string.hpp>
#include <boost/filesystem.hpp> #include <boost/filesystem.hpp>
#include <boost/program_options.hpp> #include <boost/program_options.hpp>
#include <opencv2/highgui/highgui.hpp> #include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/opencv.hpp> #include <opencv2/opencv.hpp>
#include <camodocal/camera_models/Camera.h> #include <opencv2/core/core.hpp>
#include <camodocal/camera_models/CameraFactory.h> #include <opencv2/core/eigen.hpp>
#include <camodocal/camera_models/CataCamera.h>
#include <camodocal/camera_models/EquidistantCamera.h>
#include <camodocal/camera_models/PinholeCamera.h>
#include <camodocal/gpl/gpl.h>
#include <camodocal/camera_models/Camera.h>
std::string _left_camera_yaml; struct camera_info {
std::string _right_camera_yaml; unsigned int height = 0;
std::map<std::string , camodocal::CameraPtr> _cam_odo_ptr; 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* 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,
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()) {
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];
cv::Rect_<float> inner1, inner2, outer1, outer2; cv::Rect_<float> inner1, inner2, outer1, outer2;
CvMat om = cvMat(3, 1, CV_64F, _om); 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 uu = cvMat(3, 1, CV_64F, _uu);
CvMat r_r = cvMat(3, 3, CV_64F, _r_r); CvMat r_r = cvMat(3, 3, CV_64F, _r_r);
CvMat pp = cvMat(3, 4, CV_64F, _pp); CvMat pp = cvMat(3, 4, CV_64F, _pp);
CvMat ww = cvMat(3, 1, CV_64F, _ww); // temps CvMat ww = cvMat(3, 1, CV_64F, _ww); // temps
CvMat w3 = cvMat(3, 1, CV_64F, _w3); // temps CvMat w3 = cvMat(3, 1, CV_64F, _w3); // temps
CvMat wR = cvMat(3, 3, CV_64F, _wr); CvMat wR = cvMat(3, 3, CV_64F, _wr);
CvMat Z = cvMat(3, 1, CV_64F, _z); CvMat Z = cvMat(3, 1, CV_64F, _z);
CvMat Ri = cvMat(3, 3, CV_64F, _ri); CvMat Ri = cvMat(3, 3, CV_64F, _ri);
double nx = imageSize.width, ny = imageSize.height; double nx = imageSize.width, ny = imageSize.height;
int i, k; int i, k;
double nt, nw; double nt, nw;
if( matR->rows == 3 && matR->cols == 3 ) if ( matR->rows == 3 && matR->cols == 3)
cvRodrigues2(matR, &om); // get vector rotation cvRodrigues2(matR, &om); // get vector rotation
else else
cvConvert(matR, &om); // it's already a rotation vector cvConvert(matR, &om); // it's already a rotation vector
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;
@ -94,8 +117,7 @@ void stereoRectify(const CvMat* K1, const CvMat* K2,
cvConvertScale(&w3, &w3, 1 / nw); cvConvertScale(&w3, &w3, 1 / nw);
_uu[2] = 0; _uu[2] = 0;
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
@ -131,12 +153,10 @@ void stereoRectify(const CvMat* K1, const CvMat* K2,
int j = (i<2) ? 0 : 1; int j = (i<2) ? 0 : 1;
a.x() = (float)((i % 2)*(nx)); a.x() = (float)((i % 2)*(nx));
a.y() = (float)(j*(ny)); a.y() = (float)(j*(ny));
if (0 == k){ if (0 == k) {
_cam_odo_ptr["LEFT"]->liftProjective(a, b); leftOdo->liftProjective(a, b);
} } else {
else{ rightOdo->liftProjective(a, b);
_cam_odo_ptr["RIGHT"]->liftProjective(a, b);
} }
_pts[i].x = b.x()/b.z(); _pts[i].x = b.x()/b.z();
_pts[i].y = b.y()/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, 0, 2, cx2);
cvmSet(_P2, 1, 2, cy2); cvmSet(_P2, 1, 2, cy2);
cvmSet(_P2, idx, 3, s*cvmGet(_P2, idx, 3)); 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<double>(i, j);
calib_mat_data_right.P[i*4 + j] = P2.at<double>(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<double>(i, j);
calib_mat_data_right.R[i*3 +j] = R2.at<double>(i, j);
}
}
struct camera_mat_info_pair res = {calib_mat_data_left, calib_mat_data_right};
return res;
}
#endif #endif
MYNTEYE_BEGIN_NAMESPACE MYNTEYE_BEGIN_NAMESPACE
const char RectifyProcessor::NAME[] = "RectifyProcessor"; const char RectifyProcessor::NAME[] = "RectifyProcessor";
RectifyProcessor::RectifyProcessor( RectifyProcessor::RectifyProcessor(
std::shared_ptr<Device> device, std::int32_t proc_period) std::shared_ptr<Device> device, std::int32_t proc_period)
: Processor(std::move(proc_period)), device_(device) { : Processor(std::move(proc_period)), device_(device) {
VLOG(2) << __func__ << ": proc_period=" << proc_period; VLOG(2) << __func__ << ": proc_period=" << proc_period;
calib_model = CalibrationModel::UNKNOW;
NotifyImageParamsChanged(); NotifyImageParamsChanged();
} }
@ -272,19 +399,18 @@ void RectifyProcessor::NotifyImageParamsChanged() {
} else if (in_left->calib_model() == } else if (in_left->calib_model() ==
CalibrationModel::KANNALA_BRANDT) { CalibrationModel::KANNALA_BRANDT) {
#ifdef WITH_CAM_MODELS #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( InitParams(
*std::dynamic_pointer_cast<IntrinsicsEquidistant>(in_left), *std::dynamic_pointer_cast<IntrinsicsEquidistant>(in_left),
*std::dynamic_pointer_cast<IntrinsicsEquidistant>(in_right), *std::dynamic_pointer_cast<IntrinsicsEquidistant>(in_right),
device_->GetExtrinsics(Stream::RIGHT, Stream::LEFT)); device_->GetExtrinsics(Stream::RIGHT, Stream::LEFT));
#else #else
VLOG(2) << __func__ << "KANNALA_BRANDT macro seitch is not on"; VLOG(2) << "calib model type KANNALA_BRANDT"
<< " is not been enabled.";
#endif #endif
} else { } 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( bool RectifyProcessor::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)
const ObjMat2 *input = Object::Cast<ObjMat2>(in); if (calib_model == CalibrationModel::PINHOLE) {
ObjMat2 *output = Object::Cast<ObjMat2>(out); const ObjMat2 *input = Object::Cast<ObjMat2>(in);
cv::remap(input->first, output->first, map11, map12, cv::INTER_LINEAR); ObjMat2 *output = Object::Cast<ObjMat2>(out);
cv::remap(input->second, output->second, map21, map22, cv::INTER_LINEAR); cv::remap(input->first, output->first, map11, map12, cv::INTER_LINEAR);
output->first_id = input->first_id; cv::remap(input->second, output->second, map21, map22, cv::INTER_LINEAR);
output->first_data = input->first_data; output->first_id = input->first_id;
output->second_id = input->second_id; output->first_data = input->first_data;
output->second_data = input->second_data; output->second_id = input->second_id;
return true; output->second_data = input->second_data;
return true;
} else if (calib_model == CalibrationModel::KANNALA_BRANDT) {
#ifdef WITH_CAM_MODELS
const ObjMat2 *input = Object::Cast<ObjMat2>(in);
ObjMat2 *output = Object::Cast<ObjMat2>(out);
std::cout << in <<std::endl;
// 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
return false;
#endif
}
} }
void RectifyProcessor::InitParams( void RectifyProcessor::InitParams(
IntrinsicsPinhole in_left, IntrinsicsPinhole in_left,
IntrinsicsPinhole in_right, IntrinsicsPinhole in_right,
Extrinsics ex_right_to_left) { Extrinsics ex_right_to_left) {
calib_model = CalibrationModel::PINHOLE;
cv::Size size{in_left.width, in_left.height}; cv::Size size{in_left.width, in_left.height};
cv::Mat M1 = cv::Mat M1 =
@ -345,45 +489,111 @@ void RectifyProcessor::InitParams(
cv::initUndistortRectifyMap(M2, D2, R2, P2, size, CV_16SC2, map21, map22); 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( 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) {
cv::Size size{in_left.width, in_left.height};
cv::Mat M1 = calib_model = CalibrationModel::KANNALA_BRANDT;
(cv::Mat_<double>(3, 3) << 1.0, 0, 0.0, 0, 1.0, camodocal::CameraPtr camera_odo_ptr_left_o =
0.0, 0, 0, 1); generateCameraFromIntrinsicsEquidistant(in_left);
cv::Mat M2 = camodocal::CameraPtr camera_odo_ptr_right_o =
(cv::Mat_<double>(3, 3) << 1.0, 0, 0.0, 0, 1.0, generateCameraFromIntrinsicsEquidistant(in_right);
0.0, 0, 0, 1); struct camera_mat_info_pair calib_info_pair =
cv::Mat D1(1, 8, CV_64F, in_left.coeffs); stereoRectify(camera_odo_ptr_left_o,
cv::Mat D2(1, 8, CV_64F, in_right.coeffs); camera_odo_ptr_right_o,
cv::Mat R = in_left,
(cv::Mat_<double>(3, 3) << ex_right_to_left.rotation[0][0], in_right,
ex_right_to_left.rotation[0][1], ex_right_to_left.rotation[0][2], ex_right_to_left);
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);
VLOG(2) << "InitParams size: " << size; camodocal::CameraPtr camera_odo_ptr_left =
VLOG(2) << "M1: " << M1; getCamOdoCameraPtr(calib_info_pair.left);
VLOG(2) << "M2: " << M2; camodocal::CameraPtr camera_odo_ptr_right =
VLOG(2) << "D1: " << D1; getCamOdoCameraPtr(calib_info_pair.right);
VLOG(2) << "D2: " << D2;
VLOG(2) << "R: " << R;
VLOG(2) << "T: " << T;
cv::Rect left_roi, right_roi; // for (int i = 0; i < 4 ; i ++) {
cv::stereoRectify( // std::cout << " D" << i << ": " << calib_info_pair.left.D[i];
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 < 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); // 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
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: 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<float>(i, j) = calib_info_pair.left.R[i*3+j];
rect_R_r.at<float>(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 MYNTEYE_END_NAMESPACE

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

@ -22,6 +22,15 @@
#include "mynteye/types.h" #include "mynteye/types.h"
#include "mynteye/api/processor.h" #include "mynteye/api/processor.h"
#ifdef WITH_CAM_MODELS
#include <camodocal/camera_models/Camera.h>
#include <camodocal/camera_models/CameraFactory.h>
#include <camodocal/camera_models/CataCamera.h>
#include <camodocal/camera_models/EquidistantCamera.h>
#include <camodocal/camera_models/PinholeCamera.h>
#include <camodocal/gpl/gpl.h>
#include <camodocal/camera_models/Camera.h>
#endif
MYNTEYE_BEGIN_NAMESPACE MYNTEYE_BEGIN_NAMESPACE
@ -54,6 +63,7 @@ class RectifyProcessor : public Processor {
IntrinsicsEquidistant in_right, Extrinsics ex_right_to_left); IntrinsicsEquidistant in_right, Extrinsics ex_right_to_left);
std::shared_ptr<Device> device_; std::shared_ptr<Device> device_;
CalibrationModel calib_model;
}; };
MYNTEYE_END_NAMESPACE MYNTEYE_END_NAMESPACE