双目相机空间坐标重建.图像畸变处理（四）

cv::Mat Im0 = cv::imread(“E:\zcb_work\2113\pic\ch1\left11.jpg”, 0);
if (Im0.empty())
return -1;
printf("++++ open image succesful +++\r\n");
imshow(“image0”, Im0);

double fx1 = 534.10766364;//mm
double fy1 = 534.01052742;//mm
double cx1 = 341.14525437;//mm
double cy1 = 234.85237461;//mm

double k1 = -0.291268580087220;
double k2 = 0.112956040249740;
double p1 = 0.000644024243857767;
double p2 = -0.000278487828264588;
double k3 = 0;

#if 1
//pic2 camera position
printf("pic2 camera position\r\n");	
{
		int i = 0;
		int j = 0;
		for( i = 0; i < h;i++)
		{			
			for( j = 0; j < w ;j++)
			{
			        double xDistortion = (j - cx1) / fx1;
			        double yDistortion = (i - cy1) / fy1;

			        double xCorrected, yCorrected;

			        double x0 = xDistortion;
			        double y0 = yDistortion;
					
			        for (int j = 0; j < 10; j++)
			        {
			            double r2 = xDistortion*xDistortion + yDistortion*yDistortion;

			            double distRadialA = 1 / (1 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2);
			            double distRadialB = 1;

			            double deltaX = 2* p1 * xDistortion * yDistortion + p2 * (r2 + 2 * xDistortion * xDistortion);
			            double deltaY = p1 * (r2 + 2* yDistortion * yDistortion) + 2* p2 * xDistortion * yDistortion;

			            xCorrected = (x0 - deltaX)* distRadialA * distRadialB;
			            yCorrected = (y0 - deltaY)* distRadialA * distRadialB;

			            xDistortion = xCorrected;
			            yDistortion = yCorrected;
			        }
					
				Pxy1[2*(i * w + j) + 0] = fx1 * xCorrected +cx1;
				Pxy1[2*(i * w + j) + 1] = fy1 * yCorrected +cy1;
				
				 if((0 < Pxy1[2*(i * w + j) + 0]  &&  Pxy1[2*(i * w + j) + 0]< Im0.cols - 1) \
				&& (0 < Pxy1[2*(i * w + j) + 1]  &&  Pxy1[2*(i * w + j) + 1]< Im0.rows - 1))
				{
					Im2.at<uchar>(round(Pxy1[2*(i * w + j) + 1]),round(Pxy1[2*(i * w + j) + 0])) = Im0.at<uchar>(i, j);
					Im3.at<uchar>(i, j) = Im0.at<uchar>(round(Pxy1[2*(i * w + j) + 1]),round(Pxy1[2*(i * w + j) + 0]));
				}
			}
		}
}
#endif

imshow(“image2”, Im2);
imshow(“image3”, Im3);