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);