相机位姿恢复姿态角和纬经高数据对比分析

前言

相机位姿恢复姿态角和纬经高数据对比分析

一、数据分析

二、程序

clear
clc
%2023/03/4 16：03
%将相机相对位姿转化为绝对的姿态角和纬经高，对结果和参考值对比
%程序分节运行！！！
%% 参数定义
%pose_vo相机恢复得到的帧帧相对位姿 包括初始[eye(3),zeros(3,1);0 0 0 1]
%pose_reference真值数据得到的相对位姿
%att_vo(横滚 俯仰 航向角 rad) lla_vo（纬度 经度 高度 rad m）相机位姿和初始姿态得到的姿态角和纬经高数据
%att_reference lla_reference 真值姿态角和纬经高
%times 时间戳
%% 数据提取
load data_0001_10Hz  %原始数据
load pose_vo  %pose_vo
load pose_reference %pose_reference
load attlla_vo %att_vo lla_vo
att_reference=[data_all(:,[4,5]),data_all(:,6)+3*pi/2];
lla_reference=[deg2rad(data_all(:,1:2)) data_all(:,3)];
%% 求解出的位姿与参考位姿R矩阵按照zyx旋转顺序分解到三个坐标轴上，t按照顺序对比,不包括初始值
% pose_reference_min0=pose_reference(:,:,2:end);
% pose_vo_min0=pose_vo(:,:,2:end);
% for i=1:length(pose_reference_min0)
%     attxyz_reference(i,:)=mat2att(pose_reference(:,:,i));t_reference(i,:)=pose_reference_min0(1:3,4,i)';
%     attxyz_pic(i,:)=mat2att(pose_vo(:,:,i));t_pic(i,:)=pose_vo_min0(1:3,4,i)';
% end
% attxyz_reference=rad2deg(attxyz_reference);
% attxyz_pic=rad2deg(attxyz_pic);
% time=times(2:end);
% figure  %R投影到xyz坐标系上的位姿对比，左侧为对比图，右侧为求解值-真值
% subplot(321);plot(time,attxyz_reference(:,1),time,attxyz_pic(:,1));legend('reference','vo');xlabel('\itt \rm / s');ylabel('\it\theta \itx( \circ )');grid on;
% subplot(323);plot(time,attxyz_reference(:,2),time,attxyz_pic(:,2));legend('reference','vo');xlabel('\itt \rm / s');ylabel('\it\theta \ity( \circ )');grid on;
% subplot(325);plot(time,attxyz_reference(:,3),time,attxyz_pic(:,3));legend('reference','vo');xlabel('\itt \rm / s');ylabel('\it\theta \itz( \circ )');grid on;
% subplot(322);plot(time,attxyz_pic(:,1)-attxyz_reference(:,1));xlabel('\itt \rm / s');ylabel('\it\Delta \it\theta x( \circ )');grid on;
% subplot(324);plot(time,attxyz_pic(:,2)-attxyz_reference(:,2));xlabel('\itt \rm / s');ylabel('\it\Delta \it\theta y( \circ )');grid on;
% subplot(326);plot(time,attxyz_pic(:,3)-attxyz_reference(:,3));xlabel('\itt \rm / s');ylabel('\it\Delta \it\theta z( \circ )');grid on;
% figure %t对比
% subplot(321);plot(time,t_reference(:,1),time,t_pic(:,1));legend('reference','vo');xlabel('\itt \rm / s');ylabel('\itt \itx( m )');grid on;
% subplot(323);plot(time,t_reference(:,2),time,t_pic(:,2));legend('reference','vo');xlabel('\itt \rm / s');ylabel('\itt \ity( m )');grid on;
% subplot(325);plot(time,t_reference(:,3),time,t_pic(:,3));legend('reference','vo');xlabel('\itt \rm / s');ylabel('\itt \itz( m )');grid on;
% subplot(322);plot(time,t_pic(:,1)-t_reference(:,1));xlabel('\itt \rm / s');ylabel('\it\Delta \itt \itx( m )');grid on;
% subplot(324);plot(time,t_pic(:,2)-t_reference(:,2));xlabel('\itt \rm / s');ylabel('\it\Delta \itt \ity( m )');grid on;
% subplot(326);plot(time,t_pic(:,3)-t_reference(:,3));xlabel('\itt \rm / s');ylabel('\it\Delta \itt \itz( m )');grid on;
%% 相机恢复的姿态角和GPS数据与真值数据对比
% att_reference=rad2deg(att_reference);att_vo=rad2deg(att_vo);
% lla_reference(:,1:2)=rad2deg(lla_reference(:,1:2));lla_vo(:,1:2)=rad2deg(lla_vo(:,1:2));
% figure  %att
% subplot(321);plot(times,att_reference(:,1),times,att_vo(:,1));legend('reference','vo');xlabel('\itt \rm / s');ylabel('Roll \it\gamma\rm / ( \circ )');grid on;
% subplot(323);plot(times,att_reference(:,2),times,att_vo(:,2));legend('reference','vo');xlabel('\itt \rm / s');ylabel('Pitch \it\theta\rm / ( \circ )');grid on;
% subplot(325);plot(times,att_reference(:,3),times,att_vo(:,3));legend('reference','vo');xlabel('\itt \rm / s');ylabel('Yaw \it\psi\rm / ( \circ )');grid on;
% subplot(322);plot(times,att_vo(:,1)-att_reference(:,1));xlabel('\itt \rm / s');ylabel('\it\Delta \it\gamma\rm / ( \circ )');grid on;
% subplot(324);plot(times,att_vo(:,2)-att_reference(:,2));xlabel('\itt \rm / s');ylabel('\it\Delta \it\theta\rm / ( \circ )');grid on;
% subplot(326);plot(times,att_vo(:,3)-att_reference(:,3));xlabel('\itt \rm / s');ylabel('\it\Delta \it\psi\rm / ( \circ )');grid on;
% figure %lla
% subplot(321);plot(times,lla_reference(:,1),times,lla_vo(:,1));legend('reference','vo');xlabel('\itt \rm / s');ylabel('\itL\rm / ( \circ )');grid on;
% subplot(323);plot(times,lla_reference(:,2),times,lla_vo(:,2));legend('reference','vo');xlabel('\itt \rm / s');ylabel('\it\lambda\rm / ( \circ )');grid on;
% subplot(325);plot(times,lla_reference(:,3),times,lla_vo(:,3));legend('reference','vo');xlabel('\itt \rm / s');ylabel('\ith\rm / ( m )');grid on;
% subplot(322);plot(times,lla_vo(:,1)-lla_reference(:,1));xlabel('\itt \rm / s');ylabel('\it\Delta \itL\rm / ( \circ )');grid on;
% subplot(324);plot(times,lla_vo(:,2)-lla_reference(:,2));xlabel('\itt \rm / s');ylabel('\it\Delta \it\lambda\rm / ( \circ )');grid on;
% subplot(326);plot(times,lla_vo(:,3)-lla_reference(:,3));xlabel('\itt \rm / s');ylabel('\it\Delta \ith\rm / ( m )');grid on;
%% 将经纬高数据转化为m数据(参考严恭敏老师工具箱程序)
dxyz_reference = gpsdata2dxyz(lla_reference);dxyz_vo= gpsdata2dxyz(lla_vo);%这里的lla为弧度单位
figure %lla
subplot(321);plot(times,dxyz_reference(:,2),times,dxyz_vo(:,2));legend('reference','vo');xlabel('\itt \rm / s');ylabel('\DeltaLat \rm / m');grid on;
subplot(323);plot(times,dxyz_reference(:,1),times,dxyz_vo(:,1));legend('reference','vo');xlabel('\itt \rm / s');ylabel('\DeltaLon \rm / m');grid on;
subplot(325);plot(times,dxyz_reference(:,3),times,dxyz_vo(:,3));legend('reference','vo');xlabel('\itt \rm / s');ylabel('\DeltaHgt \rm / m');grid on;
subplot(322);plot(times,dxyz_vo(:,2)-dxyz_reference(:,2));xlabel('\itt \rm / s');ylabel('\it\Delta \DeltaLat \rm / m');grid on;
subplot(324);plot(times,dxyz_vo(:,1)-dxyz_reference(:,1));xlabel('\itt \rm / s');ylabel('\it\Delta \DeltaLon \rm / m');grid on;
subplot(326);plot(times,dxyz_vo(:,3)-dxyz_reference(:,3));xlabel('\itt \rm / s');ylabel('\it\Delta \DeltaHgt \rm / m');grid on;

figure %轨迹图
plot(0, 0, 'rp');
hold on;plot(dxyz_reference(:,1), dxyz_reference(:,2));
hold on;plot(dxyz_vo(:,1), dxyz_vo(:,2));
grid on;xlabel('East\rm / m');ylabel('North\rm / m');
legend(sprintf('LON0:%.4f, LAT0:%.4f (deg)', rad2deg(lla_reference(1,2)),rad2deg(lla_reference(1,1))),'reference','vo');

function [ RMh, clRNh ] = gps2RMRN( pos )
Re= 6378137.0;   
f=1/298.257223563;
e2=2*f-f^2;
sl=sin(pos(:,1)); cl=cos(pos(:,1)); sl2=sl.*sl;
sq = 1-e2*sl2; sq2 = sqrt(sq);
RMh = Re*(1-e2)./sq./sq2+pos(:,3);
RNh = Re./sq2+pos(:,3);    
clRNh = cl.*RNh;
end

function [ dxyz ] = gpsdata2dxyz( gps_data )
pos0 = gps_data(1,1:3)'; 
dpos = diff([pos0';gps_data(:,1:3)]);
[RMh, clRNh] = gps2RMRN(gps_data(:,1:3));
ddxyz = [dpos(:,2).*clRNh, dpos(:,1).*RMh, dpos(:,3)];
dxyz = cumsum(ddxyz,1);
end

function [ att ] = mat2att( Mat )
%输出为弧度 横滚 俯仰 航向 xyz轴
att=[atan2(Mat(2,3),Mat(3,3)) asin(-Mat(1,3))  atan2(Mat(1,2),Mat(1,1))];
end