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. " << 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:
end