裁剪ICP代码

裁剪icp matlab

function [R t TriKSI minPhi] = FastTrICP（R t MoveStep）
global Model Data TrMin TrMax 
TrMin= 0.35;
TrMax= 1.0;
phiPre= 0;
minPhi= 100;
CurrStep = 0;
TData = transform_to_global（Data R t）;
KDtree= createns（Model‘）;
while （（CurrStep < MoveStep）&（abs（minPhi-phiPre）>10^（-4）））
    [corrTD] = knnsearch（KDtreeTData‘）;
    SortTD2 = sortrows（TD.^2）; % Sort the correspongding points
    minTDIndex = floor（TrMin*length（TD））; % Get minimum index of TD
    maxTDIndex = ceil（TrMax*length（TD））; % Get maxmum index of TD
    TDIndex = [minTDIndex : maxTDIndex]‘;
    mTr = TDIndex./length（TD）;
    mCumTD2 = cumsum（SortTD2）; % Get accumulative sum of sorted TD.^2
    mMSE = mCumTD2（minTDIndex : maxTDIndex）./TDIndex; % Compute all MSE
    mPhi = objectiveFunction（mMSE mTr）;  
    phiPre= minPhi;
    [minPhi nIndex] = min（mPhi）;    
    TriKSI = mTr（nIndex）; % Update Tr for next step    
    corr（:2） = [1 : length（corr）]‘;  
    % Sort the corresponding points
    corrTD = [corr TD];
    SortCorrTD = sortrows（corrTD 3）;
    [R t TCorr TData] = CalRtPhi（TData SortCorrTD TriKSI）;   
    % return the MSE and the minimized function value
    CurrStep= CurrStep+1;   
end


%%%%%%%%%%%%%%%%%%%%Integrated Function%%%%%%%%%%%%%%%%%%%%
%% Calculate RtPhi based on current overlap parameter
function [R tTCorrTData] = CalRtPhi（TDataSortCorrTDTr）
global Model Data
TrLength = floor（Tr*size（SortCorrTD1））; % The number of corresponding points after trimming
TCorr = SortCorrTD（1:TrLength 1:2）;     % Trim the corresponding points according to overlap parameter Tr
% Register MData with TData
[R t] = reg（TCorr）;
% To obtain the transformation data
TData = transform_to_global（Data R t）;

%%%%%%%%%%%%%%% Calculate the registration matrix %%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%% T（TData）->MData %%%%%%%%%%%%%%%%%%%%%%%%%
% SVD solution
function [R1 t1] = reg（corr）
global Model Data
n = length（corr）; 
M = Model（:corr（:1））; 
mm = mean（M2）;
S = Data（:corr（:2））;
ms = mean（S2）; 
Sshifted = [S（1:）-ms（1）; S（2:）-ms（2）; S（3:）-ms（3）];
Mshifted = [M（1:）-mm（1）; M（2:）-mm（2）; M（3:）-mm（3）];
K = Sshifted*Mshifted‘;
K = K/n;
[U A V] = svd（K）;
R1 = V*U‘;
if det（R1）<0
    B = eye（3）;
    B（33） = det（V*U‘）;
    R1 = V*B*U‘;
end
t1 = mm - R1*ms;

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

     文件       2330  2016-05-25 21:02  FastTrICP.m

     文件        487  2016-05-25 21:09  main.m

     文件         99  2016-05-25 21:14  objectiveFunction.m

     文件        544  2016-05-25 20:58  OulerToRota.m

     文件        171  2016-05-25 21:00  transform_to_global.m

     文件    4378672  2016-05-25 21:08  bunny.mat

     文件    4378672  2016-05-25 21:08  TrICPDemo（3D）\bunny.mat

     文件    9300887  2016-06-04 22:12  TrICPDemo（3D）\bunnyR0.mat

     文件        871  2016-06-05 20:23  TrICPDemo（3D）\CtICP.m

     文件       2362  2016-06-04 22:05  TrICPDemo（3D）\FastTrICP.m

     文件        562  2016-06-05 16:22  TrICPDemo（3D）\main.m

     文件         99  2016-05-25 21:14  TrICPDemo（3D）\objectiveFunction.m

     文件        544  2016-05-25 20:58  TrICPDemo（3D）\OulerToRota.m

     文件        171  2016-05-25 21:00  TrICPDemo（3D）\transform_to_global.m

     目录          0  2016-06-05 20:29  TrICPDemo（3D）

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

             18066471                    15