根据相机帧帧之间相对位姿和初值数据恢复姿态角和纬经高数据

前言

根据相机帧帧之间相对位姿和初始姿态角、纬经高数据恢复姿态角和纬经高数据。

一、公式推导

二、程序

clear
clc
%2023/03/4 11：35
%已知求解出的相机位姿pose，初始横滚俯仰航向角和纬经高坐标，求解姿态角和纬经高
%% 初始数据
load data_0001_10Hz  %帧帧之间相对位姿
load pose_vo
pose=pose_vo(:,:,2:end);
att0=[data_all(1,[4,5]),data_all(1,6)+3*pi/2];%横滚 俯仰 航向 弧度
lla0=data_all(1,1:3);lla0(1:2)=deg2rad(lla0(1:2));%纬度 经度 高度 deg deg m
%% 初始IMU位姿
pose0=attlla2pose(att0,lla0);
%% IMU转换到相机坐标系
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;
%% 初始相机坐标系
pose0_cam=T_imu2cam*pose0;
pose_cam=zeros(4,4,length(pose)+1);
pose_cam(:,:,1)=pose0_cam;
for i=1:length(pose)
    pose_cam(:,:,i+1)=pose(:,:,i)*pose_cam(:,:,i);
end
%% 转化为 att lla
att_vo=zeros(length(pose_cam),3);lla_vo=zeros(length(pose_cam),3);
for i=1:length(pose_cam)
    pose_imu=T_imu2cam^-1*pose_cam(:,:,i);
    [att_vo(i,:),lla_vo(i,:)]=pose2attlla(pose_imu);
end
% save('attlla_vo','att_vo','lla_vo')

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

function [att,lla]=pose2attlla(pose)
pose=pose^-1;
t=pose(1:3,4);
lla=t2lla(t);
M=Ce2g(lla);
Cn2b=pose(1:3,1:3)'*M^-1;
att=mat2att(Cn2b);
end

function [ lla ] = t2lla(xyz)
% lla=xyz2blh(xyz);
x=xyz(1);y=xyz(2);z=xyz(3);
lla(2)=atan2(y,x);%经度
Re= 6378137.0;   
f=1/298.257223563;
e2=2*f-f^2;

t=0;
for i=1:6
    t=(z+Re*e2*t/sqrt(1+(1-e2)*t^2))/sqrt(x^2+y^2);
end
lla(1)=atan(t);%纬度

RN = Re/sqrt(1- e2*sin(lla(1))*sin(lla(1)));
% RN=Re/(cos(lla(1))*sqrt(1+(1-e2)*tan(lla(1))^2));
lla(3)=sqrt(x^2+y^2)/cos(lla(1))-RN;%高度

end