xyztpl2xyz.m

来自「移动机器人同时定位与地图创建最前沿技术」· M 代码 · 共 17 行

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% SLAM Summer School 2006, Oxford.
% Practical 3. SLAM using Monocular Vision.
% Practical exercise.
% J.M.M. Montiel, Javier Civera, Andrew J. Davison.
% {josemari, jcivera}@unizar.es, [e-mail]
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [ XYZ ] = XYZTPL2XYZ( XYZTPL )

% XYZ
rw = XYZTPL(1:3);
theta = XYZTPL(4);
phi = XYZTPL(5);
lambda = XYZTPL(6);
mi = [cos(phi)*sin(theta)   -sin(phi)  cos(phi)*cos(theta)]';   
XYZ = rw+(1/lambda)*mi;