准备好一个标定板，确认标定板的宽边和长边的角点数量，还有角点的间距。 （2） 拍摄 5 组标定在不同角度下的图片，每组图片需要符合以下特点： 1 激光线需要打在标定板角点边缘以内； 2 标定板所有角点在图像内清晰可见； 3 每组照片的激光点都需要发生图像坐标 y 方向上的变化； 4 每组照片需要两张图，一张是没有激光的标定板图），一张是只有激光的 激光点图，两张图只是有无激光点区别，标定板的位置不能变； （3） 每组组照片中，根据标定板图的最外围角点位置（如图 3.4），在激光图中截取出只 在标定板范围内的激光点。 （4） 第（3）步中得到的标定板范围内的激光点，因为每组图片中的激光点与标定板共面， 可以根据标定板建立世界坐标系，并求出每组激光点在相机坐标系下的坐标。 （5） 利用最小二乘法，将第（4）步得到的所有在相机坐标系下的激光点坐标拟合出最终的 光平面方程。写一段代码，这是jpg图片的地址

<think>好的，我现在需要帮助用户解决如何通过拍摄标定板图片来提取角点信息，并利用最小二乘法拟合光平面方程的问题。用户还希望得到代码示例。首先，我需要理解用户的具体需求和他们提到的技术背景。 根据用户提供的引用内容，光平面标定的流程包括标定相机内参、获取外参、提取激光点坐标，然后转换到相机坐标系下，最后用最小二乘法拟合平面方程。用户已经了解这些步骤，现在需要具体的代码实现。 首先，我需要分解问题。标定过程分为几个步骤：相机标定（内参和外参）、激光点提取、坐标转换、平面拟合。代码示例需要涵盖这些步骤吗？或者用户可能已经有了部分代码，比如相机标定的部分？用户的问题提到“拍摄多组标定板图片（含激光点和无激光点）”，所以可能需要处理两种图片：带激光的和不带激光的。不带激光的可能用于标定板角点检测，而带激光的用于提取激光点。 接下来，我需要考虑如何提取角点信息。使用OpenCV的findChessboardCorners函数应该是标准做法。对于激光点的提取，可能需要图像处理技术，比如阈值分割、形态学操作，或者更高级的方法如Steger算法提取光条中心线。但用户提到的是标定板上的激光点，可能是在标定板上照射的线激光，因此需要从图像中提取激光线的中心点。 然后，坐标转换部分。用户提到需要将激光点的世界坐标转换到相机坐标系，这需要外参（即旋转和平移矩阵）。这部分涉及到标定板的世界坐标系到相机坐标系的转换，可以通过solvePnP函数获取外参。 最后，使用最小二乘法拟合平面方程。平面方程的一般形式是Ax + By + Cz + D = 0，或者可以表示为z = ax + by + c。通过收集多个点在相机坐标系下的坐标，构建方程组，求解系数。 现在考虑代码结构。可能需要分为以下几个部分： 1. 相机内参标定：使用标定板的图片（无激光）计算相机的内参矩阵和畸变系数。 2. 外参估计：对于每张带激光的图片，使用标定板的角点坐标和已知的世界坐标，通过solvePnP获取旋转和平移向量。 3. 激光点提取：从带激光的图片中提取激光线的中心点，获取其像素坐标。 4. 坐标转换：将激光点的像素坐标转换为相机坐标系下的3D坐标。这里可能需要使用外参，但具体如何转换？可能需要将像素坐标转换为世界坐标（基于标定板），然后通过外参转换到相机坐标系，或者直接使用逆投影？这部分可能需要更仔细的思考。 根据引用内容，提到“提取到的激光点的世界坐标通过像素坐标的相对位置计算得到，再使用对应的外参将激光点的世界坐标转换到相机坐标系中”。所以步骤可能是：已知标定板的角点世界坐标，检测激光点在图像中的像素坐标，然后通过标定板的外参（即标定板到相机的变换）将激光点的像素坐标转换为相机坐标系下的3D点。具体来说，可能需要将激光点的像素坐标转换为标定板坐标系下的3D坐标，然后通过外参（旋转和平移）转换到相机坐标系。 这里可能有一个问题：标定板是平面的，通常其世界坐标假设为Z=0。因此，对于标定板上的点，世界坐标是(X, Y, 0)。当激光线照射在标定板上时，激光点的Z坐标在标定板坐标系下为0，所以其3D坐标在标定板坐标系下是(X, Y, 0)。然后通过外参（旋转矩阵R和平移向量t）转换到相机坐标系：P_cam = R * [X; Y; 0] + t。 但如何从激光点的像素坐标(u, v)得到其在标定板上的世界坐标(X, Y, 0)呢？这可能需要知道标定板的棋盘格每个角点的位置，而激光点可能位于棋盘格的某个位置，比如在两个角点之间。或者，可能需要将激光点视为在标定板平面上，因此可以通过逆投影得到其在标定板平面上的3D坐标。 这里可能需要使用undistortPoints函数来校正图像中的激光点，然后使用solvePnP得到的rvec和tvec，将像素坐标转换为标定板坐标系下的3D点。或者使用projectPoints的逆过程？这部分可能需要查阅OpenCV的文档。 具体来说，对于一个激光点的像素坐标(u, v)，我们需要找到它在标定板平面上的对应3D坐标。因为标定板平面是Z=0，所以可以通过逆投影来求解。已知相机的内参矩阵K，外参R和t（从solvePnP得到），可以将像素坐标转换为标定板坐标系下的点，或者直接转换到相机坐标系下的点。 或者，更简单的方法是：对于每个激光点，假设它位于标定板平面上，因此其世界坐标为(X, Y, 0)，然后通过solvePnP得到的外参，将这些点转换到相机坐标系下。但如何确定X和Y？这可能涉及到在标定板平面上的位置，比如激光点可能位于标定板的某个已知位置，但实际中激光线可能覆盖多个角点之间的区域，所以需要插值或者其他方法来确定X和Y。 这里可能存在误解。正确的步骤可能是：首先检测标定板的角点，获得其在图像中的像素坐标，然后通过solvePnP得到标定板相对于相机的位置（外参）。然后，对于激光点，其位于标定板平面上，因此它们的3D坐标在标定板坐标系下是(X, Y, 0)。然后，将这些点转换到相机坐标系下，使用外参的R和t：P_cam = R * [X, Y, 0]^T + t。但是如何确定X和Y的值？因为激光点可能不在角点上，所以它们的X和Y需要根据图像中的位置来确定。 可能的解决方法是：将激光点的像素坐标通过逆透视变换映射到标定板平面上，得到对应的X和 Y。例如，使用cv2.undistortPoints和 cv2.projectPoints的逆过程。或者，更简单的方法是，假设标定板平面上的点可以通过单应性矩阵将像素坐标映射到世界坐标。单应性矩阵H可以将标定板平面上的点从像素坐标转换到世界坐标（X, Y, 0）。单应性矩阵可以通过cv2.findHomography获得，基于标定板的角点检测。 例如，假设标定板的世界坐标是已知的（比如每个角点的位置是已知的，比如棋盘格，间隔为某个尺寸），那么通过检测角点的像素坐标，可以计算单应性矩阵H。然后，对于激光点的像素坐标(u, v)，应用H的逆变换可以得到其在标定板平面上的世界坐标(X, Y, 0)。然后，使用solvePnP得到的外参（或者通过单应性矩阵分解得到R和 t）将这些世界坐标转换为相机坐标系下的点。 这个过程可能比较复杂，需要分步骤处理。例如，标定内参后，对于每张带激光的图片： 1. 检测标定板角点，得到对应的像素坐标和世界坐标（已知棋盘格尺寸）。 2. 使用solvePnP得到该图片的外参R和t。 3. 提取激光点的像素坐标(u, v)。 4. 将这些像素坐标转换为标定板平面上的世界坐标(X, Y, 0)。这可以通过求解平面上的点的逆投影，或者使用单应性矩阵。 5. 使用外参将(X, Y, 0)转换为相机坐标系下的坐标P_cam = R * [X; Y; 0] + t。 6. 收集多个这样的P_cam点，使用最小二乘法拟合平面方程。 现在，我需要考虑如何实现这些步骤的代码。例如，使用OpenCV的函数来实现角点检测、solvePnP、坐标转换等。 对于激光点的提取，可能需要图像处理步骤，比如： - 对带激光的图片，减去无激光的图片，得到差异图像，突出显示激光区域。 - 应用二值化、形态学操作来提取激光线。 - 使用骨架化或中心线提取算法获取激光线的中心点坐标。 这可能涉及到图像处理的具体参数调整，比如阈值的选择，可能需要用户根据实际情况调整。 在代码示例中，可能需要模拟或实际处理这些步骤。但用户可能希望看到具体的代码框架，即使某些部分需要根据实际情况调整。 总结步骤： 1. 标定相机内参：使用无激光的标定板图片，进行相机标定，得到内参矩阵和畸变系数。 2. 对于每张带激光的图片： a. 检测标定板角点，获取其像素坐标。 b. 使用solvePnP得到该图片的外参（R, t）。 c. 提取激光点的像素坐标。 d. 将激光点的像素坐标转换为标定板坐标系下的3D坐标（X, Y, 0）。 e. 使用外参将3D坐标转换到相机坐标系下的点。 3. 收集所有转换后的相机坐标系下的点。 4. 使用最小二乘法拟合平面方程。 现在，针对这些步骤，我需要编写相应的代码示例。例如： 相机内参标定部分可能需要使用cv2.calibrateCamera，但用户可能已经完成了这部分，所以代码示例可能从处理带激光的图片开始。 对于激光点提取，假设已经得到了二值化的激光线，可以通过寻找轮廓或者逐行扫描找到中心点。 坐标转换部分，需要将激光点的像素坐标转换为标定板坐标系下的3D点。这里可能需要使用cv2.undistortPoints来校正畸变，然后使用逆投影。或者，更简单的方法是利用solvePnP得到的外参，将像素坐标投影到标定板平面上。 此外，平面拟合的最小二乘法可以通过构建一个超定方程组，求解Ax + By + Cz + D = 0的参数，通常可以设定D=1或者另一个约束，然后使用SVD分解求解。 现在，我需要考虑代码的具体实现。例如： 假设已经获得了内参矩阵K和畸变系数dist，对于一张带激光的图片： 1. 检测角点，得到objpoints（世界坐标）和 imgpoints（像素坐标）。 2. 使用cv2.solvePnP得到rvec和 tvec（外参）。 3. 提取激光线的中心点像素坐标，保存为laser_pts。 4. 对于每个激光点(u, v)，将其转换为标定板坐标系下的3D点(X, Y, 0)。这可能需要知道该点在标定板平面上的位置。这里可能需要使用逆透视映射，或者假设该点位于标定板平面上，通过解方程得到X和 Y。 这里可能需要更详细的计算。例如，给定像素坐标(u, v)，如何得到对应的标定板平面上的点？ 一种方法是：使用cv2.undistortPoints对激光点进行去畸变，得到归一化相机坐标系下的坐标(x', y')，然后利用外参（R, t）将其投影到标定板平面上。或者，可以考虑构造一个射线，从相机原点经过去畸变的点，与标定板平面相交的点即为3D坐标。 标定板平面的方程在相机坐标系下可以由外参确定。因为标定板的世界坐标系是Z=0的平面，其到相机坐标系的变换由R和 t 决定。因此，标定板平面在相机坐标系下的方程可以通过变换后的点来确定。 或者，可以使用投影矩阵。将像素坐标转换为射线，然后找到该射线与标定板平面的交点。 这可能需要一些数学推导。例如，相机坐标系下的点P满足： s * [u; v; 1] = K * (R * [X; Y; 0] + t) 其中，s是尺度因子。因为标定板上的点世界坐标是(X, Y, 0)，将其带入： s * [u; v; 1] = K * (R * [X Y 0]^T + t) 这可以重写为： s * K^{-1} * [u; v; 1] = R * [X; Y; 0] + t 令左边为 [x; y; z]，右边为 R * [X; Y; 0] + t。由于右边是标定板坐标系到相机坐标系的变换，我们可以解这个方程来找到X和 Y。 这可能比较复杂，或许更简单的方法是使用cv2.projectPoints的逆过程，但OpenCV没有直接提供这个函数。因此，可能需要手动计算。 另一种思路是：对于标定板平面上的点，其相机坐标系下的坐标可以表示为： P_cam = R * [X, Y, 0]^T + t = R[:, 0:2] * [X, Y]^T + t 因此，给定P_cam，可以解出[X, Y]。但这里的问题是如何从像素坐标得到P_cam。 可能需要更深入的分析。例如，已知像素坐标(u, v)，经过去畸变后得到归一化坐标(x, y)，则射线方向为 (x, y, 1)。这个射线与标定板平面（在相机坐标系下）的交点即为P_cam。 标定板平面在相机坐标系下的方程可以由其法向量和距离表示。例如，标定板平面的法向量n和距离d满足n·P_cam + d = 0。但如何得到n和d？ 因为标定板的世界坐标系是Z=0，经过R和 t 变换后，标定板在相机坐标系下的平面方程可以通过变换后的三个点来确定。或者，更简单的方式是，标定板的原点在相机坐标系下的坐标是t，而标定板的X和Y轴方向由R的第一和第二列给出。因此，标定板平面在相机坐标系下可以表示为所有点 P = t + X*R[:,0] + Y*R[:,1]，其中X和Y是标定板坐标系下的坐标。因此，标定板平面的法向量是 R[:,0] 和 R[:,1] 的叉乘，即 n = R[:,0] × R[:,1]，然后单位化。 不过，这可能比较复杂。或许更简单的方式是，对于每个激光点，找到其对应的射线与标定板平面的交点。 射线方程：P = lambda * (x, y, 1)，其中lambda是深度，x和 y 是归一化后的坐标（去畸变后的）。 标定板平面方程：n·(P - Q) = 0，其中Q是平面上的一点，n是法向量。这里，标定板平面在相机坐标系下可以表示为由外参R和t确定的位置。例如，标定板原点在相机坐标系下的坐标是t，而标定板的X和Y轴由R的列向量给出。因此，标定板平面的方程可以表示为： (P - t) · (R[:,2]) = 0 因为标定板的世界坐标系是Z=0平面，其法向量在标定板坐标系下是(0,0,1)，转换到相机坐标系下是R*[0;0;1] = R[:,2]。所以，标定板平面在相机坐标系下的方程是R[:,2]·(P - t) = 0，即 R[:,2]·P = R[:,2]·t。 因此，射线 P = lambda*(x, y, 1) 代入平面方程： R[:,2]·(lambda*(x, y, 1)) = R[:,2]·t 解得 lambda = (R[:,2]·t) / (R[:,2]·[x, y, 1]^T ) 然后，P_cam = lambda * [x, y, 1] 这样就能得到激光点在相机坐标系下的坐标。 所以，步骤总结： 1. 对激光点进行去畸变，得到归一化坐标(x, y)。 2. 计算射线方向向量 [x, y, 1]。 3. 计算lambda = (R[:,2]·t) / (R[:,2]·[x, y, 1]) 4. P_cam = lambda * [x, y, 1] 这将给出激光点在相机坐标系下的3D坐标。 现在，将这些步骤转化为代码： 首先，去畸变激光点： undistorted_points = cv2.undistortPoints(laser_pts, K, dist, P=K) 这里，laser_pts是Nx1x2的数组，undistorted_points得到的是归一化坐标（在相机坐标系下，去除了畸变）。 然后，对于每个去畸变的点 (x, y)，构造射线方向向量 [x, y, 1]。 然后，计算lambda： R的第三列是 R[:,2]，即 R[2] 的转置。在代码中，R是旋转向量，需要通过cv2.Rodrigues转换为旋转矩阵。 例如： rvec, tvec = cv2.solvePnP(objpoints, imgpoints, K, dist)[1:3] R, _ = cv2.Rodrigues(rvec) 然后，计算分子：np.dot(R[:,2], tvec) 分母：np.dot(R[:,2], [x, y, 1]) lambda_val = (np.dot(R[:,2], tvec)) / np.dot(R[:,2], [x, y, 1]) 然后，P_cam = lambda_val * np.array([x, y, 1]) 这样，每个激光点对应的相机坐标系下的3D坐标就可以得到。 收集所有这些点后，使用最小二乘法拟合平面方程。 平面方程可以表示为：ax + by + cz + d = 0。为了简化，通常设d= -1，或者其他约束，例如使用奇异值分解（SVD）来求解。 具体来说，构建矩阵A，其中每行是点的坐标(x, y, z, 1)，然后求解A * [a; b; c; d] = 0。通过SVD分解，取最小奇异值对应的右奇异向量作为解。 或者，可以将平面方程表示为z = Ax + By + C，这样构建方程组： 对于每个点 (x_i, y_i, z_i)，有 z_i = A x_i + B y_i + C 对应的矩阵形式是： [x_i y_i 1] * [A; B; C] = z_i 构建超定方程组，使用np.linalg.lstsq求解。 这可能更简单，因为这样可以直接用线性最小二乘法。 例如： points = np.array([...]) # Nx3的数组，每行是x, y, z A = np.hstack([points[:,0:2], np.ones((len(points),1))]) b = points[:,2] coeffs, residuals, _, _ = np.linalg.lstsq(A, b, rcond=None) # 平面方程：z = coeffs[0]*x + coeffs[1]*y + coeffs[2] 对应的平面方程可以转换为ax + by - z + d = 0，其中a = coeffs[0], b=coeffs[1], d=coeffs[2]. 现在，将这些步骤整合到代码中： 示例代码的大致结构可能如下： import cv2 import numpy as np # 假设已经标定好相机内参K和畸变系数dist K = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]]) dist = np.array([k1, k2, p1, p2, k3]) # 标定板参数 pattern_size = (9, 6) # 棋盘格角点数 square_size = 0.025 # 单位：米 # 准备标定板的世界坐标（假设Z=0） objp = np.zeros((pattern_size[0]*pattern_size[1], 3), np.float32) objp[:,:2] = np.mgrid[0:pattern_size[0], 0:pattern_size[1]].T.reshape(-1,2) * square_size # 存储所有相机坐标系下的激光点 all_cam_points = [] # 处理每张带激光的图片 for img_path in laser_images: # 读取图片 img = cv2.imread(img_path) gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) # 检测标定板角点 ret, corners = cv2.findChessboardCorners(gray, pattern_size, None) if not ret: continue # 优化角点坐标 corners_refined = cv2.cornerSubPix(gray, corners, (11,11), (-1,-1), (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)) # 计算外参 ret, rvec, tvec = cv2.solvePnP(objp, corners_refined, K, dist) # 提取激光点（假设已经提取到laser_pts的像素坐标） # 这里需要激光点提取的代码，例如： # 假设mask是二值化的激光区域 # contours = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) # 提取中心线，例如逐行取中点 # 这里简化处理，假设laser_pts是Nx2的数组 laser_pts = extract_laser_points(img) # 需要用户自行实现 # 去畸变激光点，得到归一化坐标 undistorted_pts = cv2.undistortPoints(laser_pts.reshape(-1,1,2), K, dist, P=K) # 转换到相机坐标系 R, _ = cv2.Rodrigues(rvec) n = R[:,2] # 标定板法向量 t = tvec.flatten() cam_points = [] for pt in undistorted_pts: x, y = pt[0] # 构造射线方向向量 ray_dir = np.array([x, y, 1]) # 计算lambda numerator = np.dot(n, t) denominator = np.dot(n, ray_dir) if denominator == 0: continue # 避免除以零 lambda_val = numerator / denominator # 计算相机坐标系下的3D点 P_cam = lambda_val * ray_dir cam_points.append(P_cam) all_cam_points.extend(cam_points) # 转换为numpy数组 all_cam_points = np.array(all_cam_points) # 使用最小二乘法拟合平面方程 z = Ax + By + C A = np.hstack([all_cam_points[:,:2], np.ones((len(all_cam_points),1))]) b = all_cam_points[:,2] coeffs, residuals, rank, singular_values = np.linalg.lstsq(A, b, rcond=None) plane_equation = f"z = {coeffs[0]:.4f}x + {coeffs[1]:.4f}y + {coeffs[2]:.4f}" print("拟合的平面方程:", plane_equation) # 或者表示为Ax + By + Cz + D = 0形式 # 这里将z = ax + by + c 转换为 ax + by - z + c =0 a, b, c = coeffs plane_eq = np.array([a, b, -1, c]) plane_eq_normalized = plane_eq / np.linalg.norm(plane_eq[:3]) print(f"平面方程: {plane_eq_normalized[0]:.4f}x + {plane_eq_normalized[1]:.4f}y + {plane_eq_normalized[2]:.4f}z + {plane_eq_normalized[3]:.4f} = 0") 这段代码假设已经完成了相机内参标定，并且能够提取激光点的像素坐标。用户需要自行实现激光点提取的部分，比如通过图像处理技术提取激光线的中心点。 在激光点提取部分，可能需要更详细的代码，比如： def extract_laser_points(img): # 转换为灰度图 gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) # 使用阈值法提取激光区域 _, thresh = cv2.threshold(gray, 200, 255, cv2.THRESH_BINARY) # 形态学操作去除噪声 kernel = np.ones((5,5), np.uint8) cleaned = cv2.morphologyEx(thresh, cv2.MORPH_OPEN, kernel) # 提取轮廓 contours, _ = cv2.findContours(cleaned, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) laser_pts = [] for contour in contours: # 对每个轮廓，按行提取中心点 for y in range(img.shape[0]): # 找到当前行的所有点 row_points = contour[contour[:,0,1] == y] if len(row_points) > 0: # 取x坐标的中值 x = np.median(row_points[:,0,0]) laser_pts.append([x, y]) return np.array(laser_pts) 当然，这只是一个简化的示例，实际应用中可能需要更鲁棒的方法，比如Steger算法来提取光条中心线。 最后，需要提醒用户注意的事项，比如标定过程中相机和激光器的相对位置必须固定，标定板需要充分覆盖激光平面等。 可能还需要验证平面拟合的效果，比如计算点到平面的距离的均方误差。 总结，用户的代码需求涉及多个步骤，需要综合使用OpenCV进行标定、坐标转换和平面拟合，同时需要处理图像以提取激光点。代码示例需要涵盖主要步骤，但某些部分（如激光点提取）可能需要根据实际情况调整。</think>以下是基于标定板的光平面标定实现方案和代码示例，包含关键步骤说明和数学推导： ### 一、标定流程及原理 1. **相机内参标定** 通过多角度棋盘格图像标定得到相机内参矩阵$K$和畸变系数$D$[^1] 2. **激光点三维重建** 对于每张激光标定板图像： - 检测棋盘格角点并计算外参$R|t$ - 提取激光条纹中心像素坐标$(u,v)$ - 通过逆投影将像素坐标转换为相机坐标系三维点$(x,y,z)$ 3. **平面方程拟合** 使用最小二乘法拟合三维点集得到光平面方程： $$Ax + By + Cz + D = 0$$ ### 二、核心代码实现 ```python import cv2 import numpy as np # 标定板参数 pattern_size = (8, 6) # 内角点数量 square_size = 0.025 # 标定板方格尺寸(m) # 生成标定板三维坐标(世界坐标系) obj_points = np.zeros((pattern_size[0]*pattern_size[1],3), np.float32) obj_points[:,:2] = np.mgrid[0:pattern_size[0],0:pattern_size[1]].T.reshape(-1,2) * square_size def calibrate_laser_plane(laser_images, normal_images, pattern_size): # 1. 相机内参标定 all_obj_points = [] all_img_points = [] for img_path in normal_images: img = cv2.imread(img_path) gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) # 检测棋盘格角点 ret, corners = cv2.findChessboardCorners(gray, pattern_size, None) if ret: corners_refined = cv2.cornerSubPix(gray, corners, (11,11), (-1,-1), (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)) all_obj_points.append(obj_points) all_img_points.append(corners_refined) # 执行相机标定 ret, K, D, _, _ = cv2.calibrateCamera(all_obj_points, all_img_points, gray.shape[::-1], None, None) # 2. 激光平面标定 all_3d_points = [] for img_path in laser_images: img = cv2.imread(img_path) gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) # 检测棋盘格获取外参 ret, corners = cv2.findChessboardCorners(gray, pattern_size, None) if not ret: continue # 计算外参R|t _, rvec, tvec = cv2.solvePnP(obj_points, corners, K, D) R = cv2.Rodrigues(rvec)[0] # 提取激光条纹中心点 laser_mask = extract_laser_line(img) points = cv2.findNonZero(laser_mask) # 三维重建 for pt in points: u, v = pt[0] # 逆投影计算三维坐标 ray = np.linalg.inv(K) @ np.array([u, v, 1]) lambda_val = - (R[:,2] @ tvec) / (R[:,2] @ ray) P_cam = lambda_val * ray.flatten() all_3d_points.append(P_cam) # 3. 平面拟合 A = np.array(all_3d_points) centroid = A.mean(axis=0) U, S, Vt = np.linalg