style(calib_model): mask the depth processer temporarily;
This commit is contained in:
TinyOh
parent
cfbfcf06a5
commit
257a6c8bef
|
|
@ -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<ObjMat>(in);
|
||||
ObjMat *output = Object::Cast<ObjMat>(out);
|
||||
cv::reprojectImageTo3D(input->value, output->value, Q_, true);
|
||||
output->id = input->id;
|
||||
output->data = input->data;
|
||||
#endif
|
||||
return true;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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++) {
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user