四元数法计算两个物体之间的相对位姿参数

四元素代码，基于MATLAB，计算两个物体相对姿态的四元素参数

                 
function [bestnestfmin]=cuckoo_search_new（nMatchedPoints1MatchedPoints2）
if nargin<1
% Number of nests （or different solutions）
n=25;
end

% Discovery rate of alien eggs/solutions
pa=0.25;

%% Change this if you want to get better results
N_IterTotal=1000;
%% Simple bounds of the search domain
% Lower bounds
nd=9; 
Lb=-1*ones（1nd）; 
% Upper bounds
Ub=1*ones（1nd）;

% Random initial solutions
for i=1:n
nest（i:）=Lb+（Ub-Lb）.*rand（size（Lb））;
end

% Get the current best
fitness=10^10*ones（n1）;
[fminbestnestnestfitness]=get_best_nest（nestnestfitnessMatchedPoints1MatchedPoints2）;

N_iter=0;
%% Starting iterations
for iter=1:N_IterTotal
    % Generate new solutions （but keep the current best）
     new_nest=get_cuckoos（nestbestnestLbUb）;   
     [fnewbestnestfitness]=get_best_nest（nestnew_nestfitnessMatchedPoints1MatchedPoints2）;
    % Update the counter
      N_iter=N_iter+n; 
    % Discovery and randomization
      new_nest=empty_nests（nestLbUbpa） ;
    
    % Evaluate this set of solutions
      [fnewbestnestfitness]=get_best_nest（nestnew_nestfitnessMatchedPoints1MatchedPoints2）;
    % Update the counter again
      N_iter=N_iter+n;
    % Find the best objective so far  
    if fnew        fmin=fnew;
        bestnest=best;
    end
end %% End of iterations

%% Post-optimization processing
%% Display all the nests
disp（strcat（‘Total number of iterations=‘num2str（N_iter）））;
fmin
bestnest

%% --------------- All subfunctions are list below ------------------
%% Get cuckoos by ramdom walk
function nest=get_cuckoos（nestbestLbUb）
% Levy flights
n=size（nest1）;
% Levy exponent and coefficient
% For details see equation （2.21） Page 16 （chapter 2） of the book
% X. S. Yang Nature-Inspired metaheuristic Algorithms 2nd Edition Luniver Press （2010）.
beta=3/2;
sigma=（gamma（1+beta）*sin（pi*beta/2）/（gamma（（1+beta）/2）*beta*2^（（beta-1）/2）））^（1/beta）;

for j=1:n
    s=nest（j:）;
    % This is a simple way of implementing Levy flights
    % For standard random walks use step=1;
    %% Levy flights by Mantegna‘s algorithm
    u=randn（size（s））*sigma;
    v=randn（size（s））;
    step=u./abs（v）.^（1/beta）;
  
    % In the next equation the difference factor （s-best） means that 
    % when the solution is the best solution it remains unchanged.     
    stepsize=0.01*step.*（s-best）;
    % Here the factor 0.01 comes from the fact that L/100 should the typical
    % step size of walks/flights where L is the typical lenghtscale; 
    % otherwise Levy flights may become too aggresive/efficient 
    % which makes new solutions （even） jump out side of the design domain 
    % （and thus wasting evaluations）.
    % Now the actual random walks or flights
    s=s+stepsize.*randn（size（s））;
   % Apply simple bounds/limits
   nest（j:）=simplebounds（sLbUb）;
end

%% Find the current best nest
function [fminbestnestfitness]=get

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

     文件      40241  2015-12-04 10:42  fourelemrents\20151204104234266.jpg

     文件      37492  2015-12-04 10:43  fourelemrents\20151204104308441.jpg

     文件       4404  2015-12-04 19:33  fourelemrents\cuckoo_search_new.m

     文件        315  2015-12-04 19:30  fourelemrents\fobj.m

     文件      39738  2015-12-03 10:12  fourelemrents\fuyang30.jpg

     文件      37571  2015-12-03 10:10  fourelemrents\ping1.jpg

     文件        344  2015-11-10 19:04  fourelemrents\Ransac.m

     文件        641  2015-10-22 11:57  fourelemrents\Rotate.m

     文件        629  2015-11-03 15:51  fourelemrents\SurfPoint.m

     文件       3678  2015-12-05 14:35  fourelemrents\Test.m

     文件        186  2015-11-28 16:02  fourelemrents\TX.m

     目录          0  2018-05-22 10:10  fourelemrents

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

               165239                    12