rodrigues.m

import numpy as np import cv2 import math class CDomPho: def __init__(self): self.m_f = self.m_Pxz = self.m_k1 = self.m_k2 = self.m_p1 = self.m_p2 = 0.0 self.m_X0 = self.m_Y0 = 0.0 self.m_cols = self.m_rows = 0 self.m_rotM = [0.0] * 9 self.m_ref = [0.0] * 6 def POK2M(self, p, o, k): cosp = math.cos(p) cosw = math.cos(o) cosk = math.cos(k) sinp = math.sin(p) sinw = math.sin(o) sink = math.sin(k) self.m_rotM[0] = cosp * cosk - sinp * sinw * sink self.m_rotM[1] = -cosp * sink - sinp * sinw * cosk self.m_rotM[2] = -sinp * cosw self.m_rotM[3] = cosw * sink self.m_rotM[4] = cosw * cosk self.m_rotM[5] = -sinw self.m_rotM[6] = sinp * cosk + cosp * sinw * sink self.m_rotM[7] = -sinp * sink + cosp * sinw * cosk self.m_rotM[8] = cosp * cosw def SetPara(self, aopX, aopY, aopZ, aopP, aopO, aopK, f, pxz, cols, rows, k1, k2, p1, p2, X0, Y0): self.POK2M(aopP, aopO, aopK) self.m_aopX = aopX self.m_aopY = aopY self.m_aopZ = aopZ self.m_f = f self.m_Pxz = pxz self.m_k1 = k1 self.m_k2 = k2 self.m_p1 = p1 self.m_p2 = p2 self.m_X0 = X0 self.m_Y0 = Y0 self.m_cols = cols self.m_rows = rows self.m_ref[0] = -(cols / 2) * pxz self.m_ref[1] = pxz self.m_ref[2] = 0 self.m_ref[3] = -(rows / 2) * pxz self.m_ref[4] = 0 self.m_ref[5] = -pxz def ImgZ2Grd(self, xyz, sum): for i in range(sum): px, py = self.Scan2Pho(xyz[i * 3], xyz[i * 3 + 1]) self.PhoZ2Grd(px, py, xyz[i * 3 + 2], xyz, i * 3) def Scan2Pho(self, scX, scY): phoX = self.m_ref[0] + self.m_ref[1] * scX - self.m_ref[2] * scY phoY = self.m_ref[3] + self.m_ref[4] * scX - self.m_ref[5] * scY if abs(self.m_k1) > 1e-12: x, y = phoX, phoY r2 = x * x + y * y r4 = r2 * r2 dx = x * (self.m_k1 * r2 + self.m_k2 * r4) + self.m_p1 * (r2 + 2 * x * x) + self.m_p2 * 2 * x * y dy = y * (self.m_k1 * r2 + self.m_k2 * r4) + self.m_p2 * (r2 + 2 * y * y) + self.m_p1 * 2 * x * y phoX -= dx phoY -= dy return phoX, phoY def Grd2Img(self, gx, gy, gz): px, py = self.Grd2Pho_(gx, gy, gz) scX, scY = self.Pho2Scan(px, py) return scX, scY def Grd2Pho_(self, gx, gy, gz): R = self.m_rotM dx = gx - self.m_aopX dy = gy - self.m_aopY dz = gz - self.m_aopZ denominator = R[2] * dx + R[5] * dy + R[8] * dz fm = -self.m_f / denominator if abs(denominator) > 1e-12 else 0 px = (R[0] * dx + R[3] * dy + R[6] * dz) * fm py = (R[1] * dx + R[4] * dy + R[7] * dz) * fm return px, py def Pho2Scan(self, phoX, phoY): if abs(self.m_k1) > 1e-12: x, y = phoX, phoY for _ in range(2): # 两次迭代近似矫正 r2 = x * x + y * y r4 = r2 * r2 dx = x * (self.m_k1 * r2 + self.m_k2 * r4) + self.m_p1 * (r2 + 2 * x * x) + self.m_p2 * 2 * x * y dy = y * (self.m_k1 * r2 + self.m_k2 * r4) + self.m_p2 * (r2 + 2 * y * y) + self.m_p1 * 2 * x * y x = phoX + dx y = phoY + dy phoX, phoY = x, y tt = self.m_ref[1] * self.m_ref[5] - self.m_ref[2] * self.m_ref[4] phoX -= self.m_ref[0] phoY -= self.m_ref[3] scX = (self.m_ref[5] * phoX - self.m_ref[2] * phoY) / tt scY = -(-self.m_ref[4] * phoX + self.m_ref[1] * phoY) / tt return scX, scY def PhoZ2Grd(self, px, py, gz, xyz, idx): R = self.m_rotM A = R[0] * px + R[1] * py - R[2] * self.m_f B = R[3] * px + R[4] * py - R[5] * self.m_f C = R[6] * px + R[7] * py - R[8] * self.m_f N = (gz - self.m_aopZ) / C if abs(C) > 1e-12 else 0 xyz[idx] = self.m_aopX + A * N xyz[idx + 1] = self.m_aopY + B * N def main(): # 从文件读取参数 (示例路径) with open(r'D:\shuiwei\para3.txt') as f: lines = f.readlines() params = list(map(float, lines[1].split())) # 假设第二行是参数 aopX, aopY, aopZ, aopP, aopO, aopK, f, Pxz, k1, k2, p1, p2, X0, Y0 = params[:14] # 读取原始影像 m_img = cv2.imread(r'D:\shuiwei\9.jpg') if m_img is None: print("Error: Image not found") return rows, cols = m_img.shape[:2] dompho = CDomPho() dompho.SetPara(aopX, aopY, aopZ, aopP, aopO, aopK, f, Pxz, cols, rows, k1, k2, p1, p2, X0, Y0) # 四个角点坐标 (x,y,高程) corPt = [0.0, 0.0,400, cols, 0,400, 0, rows,400, cols, rows,400] dompho.ImgZ2Grd(corPt, 4) # 计算地面坐标 # 计算地面范围 gxs = corPt[0::3] gys = corPt[1::3] mingx, maxgx = min(gxs), max(gxs) mingy, maxgy = min(gys), max(gys) # 创建正射影像 resolution = 0.3 domCols = int((maxgx - mingx) / resolution) + 1 domRows = int((maxgy - mingy) / resolution) + 1 domImg = np.zeros((domRows, domCols, 3), dtype=np.uint8) # 生成正射影像 h = 400# 地面高程 for r in range(domRows): gy = mingy + r * resolution for c in range(domCols): gx = mingx + c * resolution ix, iy = dompho.Grd2Img(gx, gy, h) # 最近邻插值 i = int(ix + 0.5) j = int(iy + 0.5) if 0 <= i < cols and 0 <= j < rows: # 注意坐标翻转 (OpenCV原点在左上角) domImg[domRows - 1 - r, c] = m_img[rows - 1 - j, i] # 显示并保存结果 cv2.imshow('DOM', domImg) cv2.waitKey(0) cv2.imwrite(r'D:\shuiwei\current_view2.jpg', domImg) cv2.destroyAllWindows() if __name__ == "__main__": main() 这段代码输入的参数都是什么，是否可以用旋转向量和平移向量，内参矩阵和畸变参数代替