姿态角纬经高数据转相机位姿

前言

将真值姿态角和GPS数据转化为相机帧帧间位姿参考值

一、公式推导

二、程序

clear
clc
%2023/02/27 21：46 
%KITTI数据m0_data_extract提取后vo真值，R,t解算，用于对比视觉位姿,用10Hz的oxts数据
%% 坐标系定义
%以IMU坐标系（前左上，北西天）为准，俯仰角低头为正， 横滚角右倾斜为正，航向角北向为0北偏西为正
load data_0001_10Hz
%% 转换为T阵,以初始时刻为基准
pose=zeros(4,4,length(data_all));   
att=[data_all(:,[4,5]),data_all(:,6)+3*pi/2];%横滚俯仰航向
lla=data_all(:,1:3);lla(:,1:2)=deg2rad(lla(:,1:2));%纬经高
for i=1:length(data_all)
    pose(:,:,i)=attlla2pose(att(i,:),lla(i,:));
end
%% 转换到相机坐标系
R_imu2velo=[9.999976e-01 7.553071e-04 -2.035826e-03; -7.854027e-04 9.998898e-01 -1.482298e-02; 2.024406e-03 1.482454e-02 9.998881e-01];
t_imu2velo=[-8.086759e-01 3.195559e-01 -7.997231e-01];
R_velo2cam=[7.533745e-03 -9.999714e-01 -6.166020e-04; 1.480249e-02 7.280733e-04 -9.998902e-01; 9.998621e-01 7.523790e-03 1.480755e-02];
t_velo2cam=[-4.069766e-03 -7.631618e-02 -2.717806e-01];
T_imu2velo=[R_imu2velo t_imu2velo';0 0 0 1];
T_velo2cam=[R_velo2cam t_velo2cam';0 0 0 1];
T_imu2cam=T_velo2cam*T_imu2velo;
for i=1:length(pose)
    pose(:,:,i)=T_imu2cam*pose(:,:,i);
end
att_reference=zeros(length(pose),3);
t_reference=zeros(length(pose),3);
pose_reference=zeros(4,4,length(pose));pose_reference(:,:,1)=[eye(3),zeros(3,1);0 0 0 1];
for i=1:length(pose)-1
    pose_reference(:,:,i+1)=pose(:,:,i+1)*pose(:,:,i)^-1;
    att_reference(i+1,:)=mat2att(pose_reference(:,:,i));
    t_reference(i+1,:)=pose_reference(1:3,4,i)';
end
att_reference=rad2deg(att_reference);
%%
% save('pose_reference','pose_reference')

function [ R ] = att2mat(att)
%输入弧度 横滚 俯仰  航向
C_x=[ 1       0            0;
      0  cos(att(1))   sin(att(1));
      0  -sin(att(1))  cos(att(1))];
C_y=[cos(att(2))   0  -sin(att(2));
            0      1        0;
     sin(att(2))   0  cos(att(2))];
C_z=[cos(att(3)) sin(att(3)) 0;
     -sin(att(3)) cos(att(3))  0;
           0          0      1];
R=(C_x*C_y*C_z);


end

function [ pose ] =attlla2pose(att,lla)
M=Ce2g(lla);
t=lla2t(lla);
R=(att2mat(att)*M)';
pose=inv([R t;0 0 0 1]);
end

function [ M ] = Ce2g( lla )

att=[-lla(2) -(pi/2-lla(1)) pi];
C_z2=[cos(att(1)) sin(att(1)) 0;
     -sin(att(1)) cos(att(1))  0;
           0          0      1];
C_y=[cos(att(2))   0  -sin(att(2));
            0      1        0;
     sin(att(2))   0  cos(att(2))];       
C_z1=[cos(att(3)) sin(att(3)) 0;
     -sin(att(3)) cos(att(3))  0;
           0          0      1];
M=(C_z2*C_y*C_z1)';

end

function [ t ] =lla2t(lla)
B=lla(1);L=lla(2);Height=lla(3);
Re= 6378137.0;   
f=1/298.257223563;
e2=2*f-f^2;
N = Re/sqrt(1- e2*sin(B)*sin(B));
h = Height;
X = (N+h)*cos(B)*cos(L);
Y = (N+h)*cos(B)*sin(L);
Z = ( N*(1-e2)+ h ) * sin(B);
t=[X,Y,Z]';
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