Robust ICP （matlab）

国外大牛 Andrew Fitzgibbon写的ICP算法实现代码，含2D和3D，工具包，可以打开主目录下的Untitled3看一下效果。

function [a v] = angleaxis（x）
% angleaxis quaternion direction cosine matrix angle axis
%*******************************************************************
%
% angleaxis calculates the rotation angle and rotation axis of the
% input quaternion or direction cosine matrix.
%
% Input: x = quaternion x（1） = scalar x（2:4） = vector
% Rotation sense = Successive rotations are right multiplies.
% Assumes x is normalized.
%
% or
%
% x = direction cosine matrix.
% Assumes x is orthonormalized.
%
% Output: a = rotation angle （radians）
% v = rotation axis （1x3 unit vector）
%
% Programmer: James Tursa
%
%*******************************************************************

if（ numel（x） == 9 ）
    q = dc2quat（x）;
else
    q = x;
end
a = 2 * acos（q（1））;
if（ nargout == 2 ）
    if（ a == 0 ）
        v = [1 0 0];
    else
        v = q（2:4）/sin（a/2）;
    end
end

return
end

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2018-07-07 21:45  robust-icp\
     文件          11  2013-03-23 00:37  robust-icp\.gitignore
     文件        3115  2013-03-23 00:37  robust-icp\DQicp.m
     文件      159603  2018-07-03 23:46  robust-icp\Head1.txt
     文件      159603  2018-07-03 23:46  robust-icp\Head2.txt
     文件          67  2013-03-23 00:37  robust-icp\README.md
     文件         459  2018-07-07 21:44  robust-icp\Untitled3.m
     文件         869  2013-03-23 00:37  robust-icp\angleaxis.m
     文件         771  2013-03-23 00:37  robust-icp\awf_m_estimator.m
     文件         376  2013-03-23 00:37  robust-icp\awf_translate_pts.m
     目录           0  2018-07-07 21:45  robust-icp\bwdistsc\
     文件        4485  2013-03-23 00:37  robust-icp\bwdistsc\bwdistX.m
     文件       18385  2013-03-23 00:37  robust-icp\bwdistsc\bwdistsc.m
     文件       18669  2013-03-23 00:37  robust-icp\bwdistsc\bwdistsc1.m
     文件        1337  2013-03-23 00:37  robust-icp\bwdistsc\license.txt
     文件        1536  2013-03-23 00:37  robust-icp\dc2quat.m
     文件       18337  2013-03-23 00:37  robust-icp\icp.m
     文件         379  2013-03-23 00:37  robust-icp\icp_2d_lm.m
     文件        5034  2013-03-23 00:37  robust-icp\icp_2d_lm_aux.m
     文件         657  2013-03-23 00:37  robust-icp\icp_2d_lm_init.m
     文件         375  2013-03-23 00:37  robust-icp\icp_2dbasic.m
     文件        2384  2013-03-23 00:37  robust-icp\icp_2dbasic_step.m
     文件        3686  2013-03-23 00:37  robust-icp\icp_3d_err_transformed.m
     文件        2116  2013-03-23 00:37  robust-icp\icp_3dbasic.m
     文件        5987  2013-03-23 00:37  robust-icp\icp_3dlm.m
     文件        4506  2013-03-23 00:37  robust-icp\icp_3dlm_normals.m
     文件         278  2013-03-23 00:37  robust-icp\icp_deparam.m
     文件         928  2013-03-23 00:37  robust-icp\lsqnormest.m
     文件         630  2013-03-23 00:37  robust-icp\plot_matches_1d.m
     文件         374  2013-03-23 00:37  robust-icp\plot_matches_3d.m
     文件         451  2013-03-23 00:37  robust-icp\pointclouddt.m
............此处省略17个文件信息