cv::Size sz = cv::Size(PROB_W, PROB_H);//Size(srcimage.cols, srcimage.rows)
groundtoimage(xylimit, uvgd, sz, h, roll, pitch, camera_param_KK );
sz.height
sz.width
//groundtoimage...
void groundtoimage( cv::Mat& xylimt, cv::Mat& uvgrid, cv::Size& sz, double ht, double roll, double pitch, double* camera_param_KK )
{
//std::cout << "sz.height: " << sz.height << " ------ sz.width: " << sz.width << std::endl;
int height = (int)(ht*1000); //unit-mm.
double fx = camera_param_KK[0];
double fy = camera_param_KK[4];
double cx = camera_param_KK[2];
double cy = camera_param_KK[5];
double c1 = cos(pitch*PI / 180);
double s1 = sin(pitch*PI / 180);
double c2 = cos(roll*PI / 180);
double s2 = sin(roll*PI / 180);
cv::Mat Tx = (cv::Mat_<double>(3, 3) << 1, 0, 0,
0, c1, s1,
0, -s1, c1);
cv::Mat Tf = (cv::Mat_<double>(3, 3) << fx, 0, cx,
0, fy, cy,
0, 0, 1);
double xmin = 0, xmax = 0, ymin = 0, ymax = 0;
minMaxLoc(xylimt.row(0), &xmin, &xmax);
minMaxLoc(xylimt.row(1), &ymin, &ymax);
//std::cout << "xmin: " << xmin << " xmax: " << xmax << std::endl;
//std::cout << "ymin: " << ymin << " ymax: " << ymax << std::endl;
double steprow = (ymax - ymin) / sz.height;
double stepcol = (xmax - xmin) / sz.width;
cv::Mat xygrid = cv::Mat::zeros(2, sz.height*sz.width, CV_64FC1);
float y = ymax;
for (int i = 1; i <= sz.height; i++)
{
float x = xmin;
for (int j = 0; j < sz.width; j++)
{
xygrid.at<double>(0, (i - 1)*sz.width + j) = x;
xygrid.at<double>(1, (i - 1)*sz.width + j) = y;
x = x + stepcol;
}
y = y - steprow;
}
cv::Mat temp1, temp2;
xygrid.rowRange(0, 2).copyTo(temp2);
temp1 = cv::Mat::ones(1, temp2.cols, CV_64FC1)*(-height);
cv::vconcat(temp2, temp1, temp2);
cv::Mat T = Tf*Tx;
temp2 = T * temp2;
temp2.row(0) = temp2.row(0) / temp2.row(2);
temp2.row(1) = temp2.row(1) / temp2.row(2);
temp2.rowRange(0, 2).copyTo(uvgrid);
}
re:
1. Opencv Mat的三种常用类型简介;
end