void IPMapper::initialize(int ow,int oh, float fu, float fv, float cx, float cy, double pitch)
{
outputWidth = ow;
outputHeight = oh;
//CAMERA PARAMETERS:
f_u = fu; //focal lense values (mm)
f_v = fv;
c_u = cx; //camera optical center
c_v = cy;
c_1 = cos(pitch*DEG_TO_RAD); //cos(alpha : pitch angle),cos(beta : yaw angle)
c_2 = 1.0;
s_1 = sin(pitch*DEG_TO_RAD); //sin(alpha : pitch angle),sin(beta : yaw angle)
s_2 = 0.0;
cam_h = 21.5;
//init projection matrices
T = (Mat_<double>(4,4) << -c_2/f_u, s_1*s_2/f_v, c_u*c_2/f_u-c_v*s_1*s_2/f_v-c_1*s_2, 0,
s_2/f_u, s_1*c_1/f_v, -c_u*s_2/f_u-c_v*s_1*c_2/f_v-c_1*c_2, 0,
0, c_1/f_v, -c_v*c_1/f_v+s_1, 0,
0, -c_1/(f_v*cam_h), c_v*c_1/(cam_h*f_v)-s_1/cam_h, 0);
T = cam_h * T;
T_INV = (Mat_<double>(4,4) << f_u*c_2+c_u*c_1*s_2,c_u*c_1*c_2-s_2*f_u,-c_u*s_1,0,
s_2*(c_v*c_1-f_v*s_1),c_2*(c_v*c_1-f_v*s_1),-f_v*c_1-c_v*s_1,0,
c_1*s_2,c_1*c_2,-s_1,0,
c_1*s_2,c_1*c_2,-s_1,0);
mappingMatrix = Mat_<cv::Point>(outputHeight, outputWidth, cv::Point(0, 0));
initMappingMatrix(&mappingMatrix);
}
IPMapper::IPMapper(int ow,int oh)
{
outputWidth = ow;
outputHeight = oh;
//CAMERA PARAMETERS:
f_u = 524.692545; //focal lense values (mm)
f_v = 524.692545;
c_u = 319.5; //camera optical center
c_v = 239.5;
double pi = 3.1415926;
double deg = 15;
c_1 = cos(pi/180*deg); //cos(alpha : pitch angle),cos(beta : yaw angle)
c_2 = 1.0;
s_1 = sin(pi/180*deg); //sin(alpha : pitch angle),sin(beta : yaw angle)
s_2 = 0.0;
cam_h = 22.5;
//init projection matrices
T = (Mat_<double>(4,4) << -c_2/f_u, s_1*s_2/f_v, c_u*c_2/f_u-c_v*s_1*s_2/f_v-c_1*s_2, 0,
s_2/f_u, s_1*c_1/f_v, -c_u*s_2/f_u-c_v*s_1*c_2/f_v-c_1*c_2, 0,
0, c_1/f_v, -c_v*c_1/f_v+s_1, 0,
0, -c_1/(f_v*cam_h), c_v*c_1/(cam_h*f_v)-s_1/cam_h, 0);
T = cam_h * T;
T_INV = (Mat_<double>(4,4) << f_u*c_2+c_u*c_1*s_2,c_u*c_1*c_2-s_2*f_u,-c_u*s_1,0,
s_2*(c_v*c_1-f_v*s_1),c_2*(c_v*c_1-f_v*s_1),-f_v*c_1-c_v*s_1,0,
c_1*s_2,c_1*c_2,-s_1,0,
c_1*s_2,c_1*c_2,-s_1,0);
mappingMatrix = Mat_<cv::Point>(outputHeight, outputWidth, cv::Point(0, 0));
initMappingMatrix(&mappingMatrix);
}