摄影测量核线影像生成

摄影测量核线影像生成

双目匹配建立在核线影像（极线影像）基础上，目的是利用核线相关的该概念将沿x、y方向搜索同名像点的二维相关问题转化为沿核线搜索同名像点。为了生成核线影像，需要影像的内外方位元素或者相对方位元素。这里可以分别用相对方位元素或外方位元素来生成核线影像。
原理参考：https://blog.csdn.net/AAAA202012/article/details/123628994
该博客也提供了一些代码，但是只有main函数，涉及的一些函数没有详细提供，在此基础上，本文实现利用影像的外方位元素来生成核线影像。

struct Camera {
	int width;
	int height;
	double f;
	double cx;
	double cy;
	double pxlSize;
	double k1, k2, k3, p1, p2;

	double ref[6];
	Camera() {}
	Camera(int width_, int height_, double f_, double cx_, double cy_) :width(width_), height(height_), f(f_), cx(cx_), cy(cy_) { k1 = k2 = k3 = p1 = p2 = 0; }
	Camera(int width_, int height_, double f_, double cx_, double cy_, double pxlSize_, double k1_, double k2_, double k3_, double p1_, double p2_):width(width_), height(height_), f(f_),cx(cx_),cy(cy_),pxlSize(pxlSize_),k1(k1_),k2(k2_),k3(k3_),p1(p1_),p2(p2_) {}

};

static inline double* nPOK2M(double p, double o, double k, double *r){
    double cosp = cos(p);double cosw = cos(o);double cosk = cos(k);
    double sinp = sin(p);double sinw = sin(o);double sink = sin(k);
    r[0] =  cosp*cosk - sinp*sinw*sink;
    r[1] = -cosp*sink - sinp*sinw*cosk;
    r[2] = -sinp*cosw;
    r[3] =  cosw*sink;
    r[4] =  cosw*cosk;
    r[5] = -sinw;
    r[6] =  sinp*cosk + cosp*sinw*sink;
    r[7] = -sinp*sink + cosp*sinw*cosk;
    r[8] =  cosp*cosw;
    return r; 
}

struct Image {
	uint32_t id;
	string imgName;
	double Xs, Ys, Zs;
	double phi, omega, kappa;
	double R[9];

	Image() {}
	Image(uint32_t id_, string imgName_, double xs, double ys, double zs, double p, double o, double k) :id(id_), imgName(imgName_),Xs(xs),Ys(ys),Zs(zs),phi(p),omega(o),kappa(k){
		nPOK2M(p, o, k, R);
	}
};

class EpiRectify
{
public:
	EpiRectify() {
		Image img; img.id = 0;
		m_ImgL = img;
		m_ImgR = img;
	}
	EpiRectify(Image imgL,Image imgR,Camera cam) {
		m_ImgL = imgL;
		m_ImgR = imgR;
		m_cam = cam;
	}

	//地面坐标系到基线辅助坐标系的转换矩阵
	void GroundToBaseAuxMatrix(double t, double v, double* matrix)
	{
		double sint = sin(t), cost = cos(t);
		double sinv = sin(v), cosv = cos(v);

		matrix[0] = cost * cosv;      matrix[1] = sint * cosv;         matrix[2] = sinv;
		matrix[3] = -sint;          matrix[4] = cost;                matrix[5] = 0;
		matrix[6] = -cost * sinv;     matrix[7] = -sint * sinv;          matrix[8] = cosv;
	}

	bool GetEpiImgSize(double* Rl, double* Rr, int* EpiWidth, int* EpiHeight) {

		double uplx, uply, uprx, upry, downlx, downly, downrx, downry;
		double Oriuplx, Oriuply, Oriuprx, Oriupry, Oridownlx, Oridownly, Oridownrx, Oridownry;

		int nWidth = m_cam.width;
		int nHeight = m_cam.height;
		double f = m_cam.f;
		double pSize = 1;//to be done

		int x0 = nWidth / 2;
		int y0 = nHeight / 2;
		//double x0 = m_cam.cx;
		//double y0 = m_cam.cy;

		Oriuplx = -x0 * pSize;
		Oriuply = (nHeight - y0) * pSize;
		Oriuprx = (nWidth - x0) * pSize;
		Oriupry = (nHeight - y0) * pSize;
		Oridownlx = -x0 * pSize;
		Oridownly = -y0 * pSize;
		Oridownrx = (nWidth - x0) * pSize;
		Oridownry = -y0 * pSize;
		uplx = -f * (Rl[0] * Oriuplx + Rl[1] * Oriuply - Rl[2] * f) / (Rl[6] * Oriuplx + Rl[7] * Oriuply - Rl[8] * f);
		uply = -f * (Rl[3] * Oriuplx + Rl[4] * Oriuply - Rl[5] * f) / (Rl[6] * Oriuplx + Rl[7] * Oriuply - Rl[8] * f);
		uprx = -f * (Rl[0] * Oriuprx + Rl[1] * Oriupry - Rl[2] * f) / (Rl[6] * Oriuprx + Rl[7] * Oriupry - Rl[8] * f);
		upry = -f * (Rl[3] * Oriuprx + Rl[4] * Oriupry - Rl[5] * f) / (Rl[6] * Oriuprx + Rl[7] * Oriupry - Rl[8] * f);
		downlx = -f * (Rl[0] * Oridownlx + Rl[1] * Oridownly - Rl[2] * f) / (Rl[6] * Oridownlx + Rl[7] * Oridownly - Rl[8] * f);
		downly = -f * (Rl[3] * Oridownlx + Rl[4] * Oridownly - Rl[5] * f) / (Rl[6] * Oridownlx + Rl[7] * Oridownly - Rl[8] * f);
		downrx = -f * (Rl[0] * Oridownrx + Rl[1] * Oridownry - Rl[2] * f) / (Rl[6] * Oridownrx + Rl[7] * Oridownry - Rl[8] * f);
		downry = -f * (Rl[3] * Oridownrx + Rl[4] * Oridownry - Rl[5] * f) / (Rl[6] * Oridownrx + Rl[7] * Oridownry - Rl[8] * f);

		// 根据x、y的最大值和最小值确定核线影像的范围
		double a[4] = { uplx,uprx,downlx,downrx };
		OnBubbleSort(a, 4);// 从小到大排序
		double x_Min = a[0];// 负值
		double x_Max = a[3];// 正值
		double b[4] = { uply,upry,downly,downry };
		OnBubbleSort(b, 4);
		double y_Max = b[3];// 正值
		double y_Min = b[0];// 负值

		int EpipolarWidth1 = x_Max > -x_Min ? (2 * x_Max / pSize) : (-2 * x_Min / pSize);
		int EpipolarHeight1 = y_Max > -y_Min ? (2 * y_Max / pSize) : (-2 * y_Min / pSize);

		uplx = -f * (Rr[0] * Oriuplx + Rr[1] * Oriuply - Rr[2] * f) / (Rr[6] * Oriuplx + Rr[7] * Oriuply - Rr[8] * f);
		uply = -f * (Rr[3] * Oriuplx + Rr[4] * Oriuply - Rr[5] * f) / (Rr[6] * Oriuplx + Rr[7] * Oriuply - Rr[8] * f);
		uprx = -f * (Rr[0] * Oriuprx + Rr[1] * Oriupry - Rr[2] * f) / (Rr[6] * Oriuprx + Rr[7] * Oriupry - Rr[8] * f);
		upry = -f * (Rr[3] * Oriuprx + Rr[4] * Oriupry - Rr[5] * f) / (Rr[6] * Oriuprx + Rr[7] * Oriupry - Rr[8] * f);
		downlx = -f * (Rr[0] * Oridownlx + Rr[1] * Oridownly - Rr[2] * f) / (Rr[6] * Oridownlx + Rr[7] * Oridownly - Rr[8] * f);
		downly = -f * (Rr[3] * Oridownlx + Rr[4] * Oridownly - Rr[5] * f) / (Rr[6] * Oridownlx + Rr[7] * Oridownly - Rr[8] * f);
		downrx = -f * (Rr[0] * Oridownrx + Rr[1] * Oridownry - Rr[2] * f) / (Rr[6] * Oridownrx + Rr[7] * Oridownry - Rr[8] * f);
		downry = -f * (Rr[3] * Oridownrx + Rr[4] * Oridownry - Rr[5] * f) / (Rr[6] * Oridownrx + Rr[7] * Oridownry - Rr[8] * f);
		// 根据x、y的最大值和最小值确定核线影像的范围
		double c[4] = { uplx,uprx,downlx,downrx };
		OnBubbleSort(c, 4);// 从小到大排序
		x_Min = c[0];// 负值
		x_Max = c[3];// 正值
		double d[4] = { uply,upry,downly,downry };
		OnBubbleSort(d, 4);
		y_Max = d[3];// 正值
		y_Min = d[0];// 负值

		int EpipolarWidth2 = x_Max > -x_Min ? (2 * x_Max / pSize) : (-2 * x_Min / pSize);
		int EpipolarHeight2 = y_Max > -y_Min ? (2 * y_Max / pSize) : (-2 * y_Min / pSize);

		*EpiWidth = EpipolarWidth1 > EpipolarWidth2 ? EpipolarWidth1 : EpipolarWidth2;
		*EpiHeight = EpipolarHeight1 > EpipolarHeight2 ? EpipolarHeight1 : EpipolarHeight2;

		return true;
	}

	bool GenerateEpipolarImg(string ImgPath,double Bx, double By, double Bz, double psize) {
		CWuDPImage imgFileL, imgFileR;
		string imgParL = ImgPath + "\\" + m_ImgL.imgName; string imgParR = ImgPath + "\\" + m_ImgR.imgName;
		double f = m_cam.f;
		if (!imgFileL.Open(imgParL.c_str())) { cout << "Can not open Li: %s \n", imgParL.c_str(); return 0; }
		if (!imgFileR.Open(imgParR.c_str())) { cout << "Can not open Ri: %s \n", imgParR.c_str(); return 0; }

		int colsL = imgFileL.GetCols(), rowsL = imgFileR.GetRows(), c0L = 0, r0L = 0;
		int colsR = imgFileR.GetCols(), rowsR = imgFileR.GetRows(), c0R = 0, r0R = 0;
		if (colsL != colsR || rowsL != rowsR) return false;
		//int cols = max(colsL, colsR), rows = max(rowsL, rowsR);

		BYTE* pL = new BYTE[colsL * rowsL * 3];
		BYTE* pR = new BYTE[colsR * rowsR * 3];

		imgFileL.Read(pL, 3, 0, 0, rowsL, colsL);
		imgFileR.Read(pR, 3, 0, 0, rowsR, colsR);


		//将当前坐标系转换至基线坐标系
		double B = sqrt(Bx * Bx + By * By + Bz * Bz);
		double m_T = atan2(By, Bx);
		double m_V = asin(Bz / B);


		double Rtemp[9], Rl[9], Rr[9];
		GroundToBaseAuxMatrix(m_T, m_V, Rtemp);

		MultMatrix(Rtemp, m_ImgL.R, Rl, 3, 3, 3);
		MultMatrix(Rtemp, m_ImgR.R, Rr, 3, 3, 3);

		int EpipolarHeight, EpipolarWidth;
		GetEpiImgSize(Rl,Rr,&EpipolarWidth, &EpipolarHeight);

		BYTE* pEL = new BYTE[EpipolarWidth * EpipolarHeight*3];
		BYTE* pER = new BYTE[EpipolarWidth * EpipolarHeight*3];

		//开始制作核线影像
		double u1, v1, x1, y1;
		double u2, v2, x2, y2;
		int c1, r1, c2, r2;
		int i, j;
		double xo = colsL / 2;
		double yo = rowsL / 2;

		for (i = 0; i < EpipolarHeight; i++)
		{
			for (j = 0; j < EpipolarWidth; j++)
			{
				u1 = j * psize - EpipolarWidth * psize / 2;
				v1 = -i * psize + EpipolarHeight * psize / 2;

				x1 = -f * (Rl[0] * u1 + Rl[3] * v1 - Rl[6] * f) / (Rl[2] * u1 + Rl[5] * v1 - Rl[8] * f);
				y1 = -f * (Rl[1] * u1 + Rl[4] * v1 - Rl[7] * f) / (Rl[2] * u1 + Rl[5] * v1 - Rl[8] * f);

				c1 = x1 / psize + xo;
				r1 = -y1 / psize + yo;

				if ((c1 < 0.0) || (r1 < 0.0) || (r1 > rowsL - 1) || (c1 > colsL - 1))
				{
					pEL[i * EpipolarWidth * 3 + j * 3] = 0;
					pEL[i * EpipolarWidth * 3 + j * 3 + 1] = 0;
					pEL[i * EpipolarWidth * 3 + j * 3 + 2] = 0;
				}
				else// 双线性内插
				{
					pEL[i * EpipolarWidth * 3 + j * 3] = pL[r1 * colsL * 3 + c1 * 3];
					pEL[i * EpipolarWidth * 3 + j * 3 + 1] = pL[r1 * colsL * 3 + c1 * 3 + 1];
					pEL[i * EpipolarWidth * 3 + j * 3 + 2] = pL[r1 * colsL * 3 + c1 * 3 + 2];
				}

				u2 = j * psize - EpipolarWidth / 2 * psize;
				v2 = v1;
				x2 = -f * (Rr[0] * u2 + Rr[3] * v2 - Rr[6] * f) / (Rr[2] * u2 + Rr[5] * v2 - Rr[8] * f);
				y2 = -f * (Rr[1] * u2 + Rr[4] * v2 - Rr[7] * f) / (Rr[2] * u2 + Rr[5] * v2 - Rr[8] * f);
				c2 = x2 / psize + xo;
				r2 = -y2 / psize + yo;
				if ((c2 < 0.0) || (r2 < 0.0) || (r2 > rowsL - 1) || (c2 > colsL - 1))
				{
					pER[i * EpipolarWidth * 3 + j * 3] = 0;
					pER[i * EpipolarWidth * 3 + j * 3 + 1] = 0;
					pER[i * EpipolarWidth * 3 + j * 3 + 2] = 0;
				}
				else// 双线性内插
				{
					pER[i * EpipolarWidth * 3 + j * 3] = pR[r2 * colsL * 3 + c2 * 3];
					pER[i * EpipolarWidth * 3 + j * 3 + 1] = pR[r2 * colsL * 3 + c2 * 3 + 1];
					pER[i * EpipolarWidth * 3 + j * 3 + 2] = pR[r2 * colsL * 3 + c2 * 3 + 2];
				}
			}
		}
		SaveImg("d:\\testl.jpg", pEL, EpipolarWidth, EpipolarHeight, 3);
		SaveImg("d:\\testr.jpg", pER, EpipolarWidth, EpipolarHeight, 3);

		
	}



public:
	Image m_ImgL;
	Image m_ImgR;

	Camera m_cam;

};

只贴出了核线生产关键的函数，无法直接运行，需根据实际情况修改和调用。