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