load coordi; load cameraParams;%加载标定的参数结果 A = cameraParams.IntrinsicMatrix; %%%%%%%%%%%%%%需要修改的值%%%%%%%%%%%%% %Zc = 3670; %缩放因子Zc %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% column = [0;0;0]; A = [A' column]; %内参A % B = eye(4); % B(2,2) = -1; B(3,3) = -1; % B(3,4) = Zc; %外参B v = VideoReader('shoujilux7.mp4'); while hasFrame(v) frame = readFrame(v); gray_frame = rgb2gray(frame); % gamma校正 gamma = 1.5; gamma_corrected = imadjust(gray_frame,[],[],gamma); % 高斯滤波 sigma = 1; hsize = ceil(6*sigma); h = fspecial('gaussian', hsize, sigma); filtered_frame = imfilter(gamma_corrected, h); % Otsu阈值分割 T = graythresh(filtered_frame); [m, n] = size(filtered_frame); E = bwareaopen(im2bw(filtered_frame, T), round(m*n/1000), 8); % Canny边缘检测 canny_edge = edge(E, 'canny'); % 形态学膨胀 se = strel('disk', 2); dilated_edge = imdilate(canny_edge, se); % 连通域分析 stats = regionprops('table', dilated_edge, 'Area', 'Centroid'); % 筛选面积最大的连通区域 [~, idx] = max(stats.Area); centroid = stats.Centroid(idx, :); % 显示帧和质心 imshow(dilated_edge); hold on; plot(centroid(1), centroid(2), 'r+', 'MarkerSize', 10); hold off; 质心坐标转换为世界坐标

要将质心坐标转换为世界坐标，需要用到相机的内参和外参。假设质心在图像平面上的坐标为(x,y)，那么可以通过以下步骤将其转换为世界坐标：

1. 将图像平面上的坐标(x,y)转换为归一化平面坐标(u,v)，即：

u = (x - cx) / fx
v = (y - cy) / fy

其中cx和cy是相机内参矩阵A中的主点坐标，fx和fy是相机内参矩阵A中的焦距。

2. 将归一化平面坐标(u,v)转换为相机坐标系下的坐标(Xc,Yc,Zc)，即：

Xc = u * Zc
Yc = v * Zc
Zc是质心在相机坐标系下的深度，需要根据相机外参计算得到。

3. 将相机坐标系下的坐标(Xc,Yc,Zc)转换为世界坐标系下的坐标(Xw,Yw,Zw)，即：

[Xw; Yw; Zw] = B * [Xc; Yc; Zc; 1]

其中B是相机的外参矩阵，包含相机的旋转和平移信息。

最终，质心在世界坐标系下的坐标为(Xw,Yw,Zw)。