单目视觉的位姿估算算法

单目视觉的位姿估算算法 matlab代码 基于位姿估算正交迭代的算法，有test程序和主程序，test中可以比对估算的精度

function [R t Qout err2] = abskernel（P Q F G method）
% ABSKERNEL -  Absolute orientation kernel
%   ABSKERNEL is the function for solving the
%   intermediate absolute orientation problems
%   in the inner loop of the OI pose estimation
%   algorithm
%
%   INPUTS:
%     P - the reference point set arranged as a 3xn matrix
%     Q - the point set obtained by transforming P with
%         some pose estimate （typically the last estimate）
%     F - the array of projection matrices arranged as
%         a 3x3xn array
%     G - a matrix precomputed for calculating t
%     method - ‘SVD‘  -> use SVD solution for rotation
%              ‘QTN‘ -> use quaterion solution for rotation
%
%
%   OUTPUTS:
%     R - estimated rotation matrix
%     t - estimated translation vector
%     Qout - the point set obtained by transforming P with
%         newest pose estimate
%     err2 - sum of squared object-space error associated 
%         with the estimate

n = size（P2）;

for i = 1:n
  Q（:i） = F（::i）*Q（:i）;
end

% compute P‘ and Q‘
pbar = sum（P2）/n;
qbar = sum（Q2）/n;
for i = 1:n
  P（:i） = P（:i）-pbar;
  Q（:i） = Q（:i）-qbar;
end

if method == ‘SVD‘ % use SVD solution
  % compute M matrix
  M（1:31:3） = 0;
  for i = 1:n
    M = M+P（:i）*Q（:i）.‘;
  end
  
  % calculate SVD of M
  [USV] = svd（M）;
  
  % compute rotation matrix R
  R = V*（U.‘）;
elseif method == ‘QTN‘ % use quaternion solution
  % compute M matrix
  A（1:41:4） = 0;
  for i = 1:n
    A = A + qmatQ（[1;Q（:i）]）.‘*qmatW（[1;P（:i）]）;
  end
  
  % Find the largest eigenvalue of A 
  eigs_options.disp = 0;
  [VD] = eigs（A eye（size（A）） 1 ‘LM‘ eigs_options）;
  
  % compute rotation matrix R from the quaternion that
  % corresponds to the largest egienvalue of A
  R = quat2mat（V）;
end

sum（1:31） = 0;
for i = 1:n
  sum = sum + F（::i）*R*P（:i）;
end
t = G*sum;

Qout = xform（P R t）;

% calculate error
err2 = 0;
vec（1:31） = 0;
for i = 1 : n
  vec = （eye（3）-F（::i））*Qout（:i）;
  err2 = err2 + dot（vecvec）;
end

% end of ABSKERNEL

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       2104  2015-08-05 16:22  正交迭代OI算法\abskernel.m

     文件        716  1999-11-18 11:27  正交迭代OI算法\abspose.m

     文件        754  1999-11-18 11:27  正交迭代OI算法\euler2mat.m

     文件       1540  2015-08-11 11:10  正交迭代OI算法\ghtest.m

     文件        391  1999-11-18 11:27  正交迭代OI算法\mat2euler.m

     文件        390  1999-11-18 11:27  正交迭代OI算法\mat2rot.m

     文件       5907  2015-08-11 16:36  正交迭代OI算法\objpose.m

     文件        203  1999-11-25 08:42  正交迭代OI算法\qmatQ.m

     文件        205  1999-11-25 08:44  正交迭代OI算法\qmatW.m

     文件        293  1999-11-30 10:22  正交迭代OI算法\quat2mat.m

     文件        431  1999-11-18 11:27  正交迭代OI算法\randrotmat.m

     文件        406  1999-11-18 11:27  正交迭代OI算法\rot2mat.m

     文件        225  1999-11-20 02:49  正交迭代OI算法\xform.m

     文件        357  1999-11-20 02:49  正交迭代OI算法\xformproj.m

     目录          0  2015-08-13 19:00  正交迭代OI算法

----------- ---------  ---------- -----  ----

                13922                    15