vec2d

namespace :

cv::vec2d;

void src2ipm(cv::Mat &srcimage, cv::Mat& uvgrid, cv::Mat& outimage, cv::Mat& coord, cv::Size& sz, double ht, double roll, double pitch, double* camera_param_KK)
{
 
	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);
 
	double means = cv::mean(srcimage).val[0];
 
	outimage = cv::Mat::zeros(sz.height, sz.width, CV_64FC1);
	coord = cv::Mat::zeros(sz.height, sz.width, CV_64FC2);//
 
	for (int i = 0; i < sz.height; i++)
	{
		for (int j = 0; j < sz.width; j++)
		{
			float ui = uvgrid.at<double>(0, i*sz.width + j);
			float vi = uvgrid.at<double>(1, i*sz.width + j);
			//cout << "vi: " << vi << endl;
			if (ui<0 || ui>1278)
				outimage.at<double>(i, j) = means / 255;
			else
			{
				int x1 = (int)ui, x2 = (int)(ui + 1);
				int y1 = (int)vi, y2 = (int)(vi + 1);
				float x = ui - (float)x1;
				float y = vi - (float)y1;
 
				double val = srcimage.at<uchar>(y1, x1)*(1 - x)*(1 - y) +
					srcimage.at<uchar>(y1, x2)*x*(1 - y) +
					srcimage.at<uchar>(y2, x1)*(1 - x)*y +
					srcimage.at<uchar>(y1, x1)* x* y;
				outimage.at<double>(i, j) = val/255;
				//
				coord.at<cv::Vec2d>(i, j)[1] =
					height*(fy*s1 + cy*c1 - y1*c1) / (fy*c1 - cy*s1 + y1*s1)*(1 - y) +
					height*(fy*s1 + cy*c1 - y2*c1) / (fy*c1 - cy*s1 + y2*s1)*y;
				double y_w = coord.at<cv::Vec2d>(i, j)[1];
				coord.at<cv::Vec2d>(i, j)[0] = (s1*y_w + height*c1)*(cx - x1) / fx;	//Vec2d
			}
		}
	}
	outimage = outimage*255;
	//cout << "coordinate.rows: " << coordinate_.rows << "--- coordinate.cols: " << coordinate_.cols << endl;
 
	//imshow("ipm", outimage);
	//cv::waitKey(0);
	//imwrite("./ipm.png", outimage);
}

　　

cv::Mat temp = cv::Mat::ones(3, uv.cols, CV_64FC1);

void xyp2ipmp(cv::Mat& xyp, cv::Mat& ipmp, cv::Mat& xylim, Size sz){
 
	//xylimist_[0]-latteral/xylimist_[1]-longitudinal...
	//ipmp-row0-cols-latteral/ipmp-row1-rows-longitudinal...
 
	std::cout << "start probp2ipmp: " << std::endl;
	double xmin = 0, xmax = 0, ymin = 0, ymax = 0;
	minMaxLoc(xylim.row(0), &xmin, &xmax);
	minMaxLoc(xylim.row(1), &ymin, &ymax);
 
	double stepcol = (xmax - xmin) / sz.width;
	double steprow = (ymax - ymin) / sz.height;
 
	cv::Mat tempx = cv::Mat::ones(1, xyp.cols, CV_64FC1) * xmin;
	cv::Mat tempy = cv::Mat::ones(1, xyp.cols, CV_64FC1) * ymax;
 
	ipmp = cv::Mat::zeros(2, xyp.cols, CV_8UC1);
	ipmp.rowRange(0, 1) = ( xyp.rowRange(0, 1) - tempx ) / stepcol;
	ipmp.rowRange(1, 2) = ( tempy - xyp.rowRange(1, 2) ) / steprow;
 
}

　　

save image:

char output_path[100];

sprintf(output_path,"./ipmp/00000%05d.png",cnt);
cv::imwrite(output_path, ipm3);