PageRenderTime 19ms CodeModel.GetById 18ms RepoModel.GetById 1ms app.codeStats 0ms

/Code/matlab/GlobalProcrustesICP/globalProcrustes.m

http://3drock.googlecode.com/
Objective C | 241 lines | 180 code | 61 blank | 0 comment | 22 complexity | 2f056e71e2aa4c82316aabc26d769add MD5 | raw file
Possible License(s): LGPL-3.0, BSD-2-Clause
  1. function [R, t, s, Centroid, corr, Model] = globalProcrustes(Model, nMaxSteps)
  2. % [R, t, s, Centroid, corr, err, Model] = globalProcrustes(Model, tol, nStabilizationSteps, nMaxSteps)
  3. %
  4. % This is an implementation of the Iterative Closest Point (ICP) algorithm
  5. % through Global Procustes Analisys.
  6. % The function try to align the different views contained in the var Model.
  7. %
  8. % Arguments: Model - 1 x numberOfViews struct. Each substruct must contain:
  9. % vertices - 3 x nPoints Array
  10. % (weights) - 1 x NPoints array of weights associated
  11. % with each point
  12. % nMaxSteps - Number of maximum allowed steps for the algorithm
  13. %
  14. % Returns: R - Registred data rotation vectors
  15. % t - Registred data translation vectors
  16. % s - Registred data scale
  17. % Centroid - Centroids coordinates at each step.
  18. % corr - point Correspondances
  19. % Model - Transformed model
  20. c2 = 1;
  21. R = cell(length(Model),nMaxSteps);
  22. t = cell(length(Model),nMaxSteps);
  23. s = cell(length(Model),nMaxSteps);
  24. step = 1;
  25. stabilizationSteps = 0;
  26. % Initialize models
  27. modelHasWeights = true;
  28. for i=1:length(Model)
  29. fprintf(1,'.');
  30. nPtBeforeUnique = size(Model(i).vertices,1);
  31. [Model(i).vertices Idx] = unique(Model(i).vertices,'rows');
  32. if(~isfield(Model(i),'weights') || size(Model(i).weights,1) ~= nPtBeforeUnique || size(Model(i).weights,2) ~= 1)
  33. modelHasWeights = false;
  34. else
  35. Model(i).weights = Model(i).weights(Idx,:);
  36. Model(i).weights(isnan(Model(i).weights)) = 0.5;
  37. Model(i).weights(isinf(Model(i).weights)) = 0.5;
  38. end
  39. end
  40. if(modelHasWeights)
  41. fprintf(1,'\nWeights have been found ');
  42. end
  43. fprintf(1,'\nInitializing data(Computing Delaunay Triangulation) ');
  44. for i=1:length(Model)
  45. fprintf(1,'.');
  46. Model(i).tri = delaunayn([Model(i).vertices]);
  47. end
  48. while (step < nMaxSteps)
  49. numOfPoints = 0;
  50. fprintf(1,'\n Step %d \n',step);
  51. c1 = c2;
  52. modelSpan = zeros(length(Model)+1,1);
  53. % reinitialize data struct
  54. for i=1:length(Model)
  55. % store the correspandances,
  56. % -- zeros instead of sparse to be memory efficient
  57. Model(i).corrOM = zeros(length(Model(i).vertices),length(Model));
  58. Model(i).corrCentr = zeros(length(Model(i).vertices),length(Model));
  59. Model(i).corr = cell(length(Model),1);
  60. Model(i).D = cell(length(Model),1);
  61. modelSpan(i) = numOfPoints;
  62. numOfPoints = numOfPoints + size(Model(i).vertices,1);
  63. end
  64. modelSpan(length(Model)+1) = numOfPoints;
  65. fprintf(1,'Computing correspondances ');
  66. CentroidPtsBelMod = zeros(numOfPoints,length(Model),'int32');
  67. D = 0;
  68. countD = 0;
  69. %compute correspondances
  70. for i=1:length(Model)
  71. fprintf(1,'.');
  72. spanI = modelSpan(i)+1:modelSpan(i+1);
  73. CentroidPtsBelMod(spanI,i) = 1:length(spanI);
  74. % for j=mod(i,length(Model))+1:mod(i,length(Model))+1
  75. for j=i+1:length(Model)
  76. spanJ = modelSpan(j)+1:modelSpan(j+1);
  77. % Pick only the correspondances that have the same neighboard
  78. % in both views
  79. [Corr1, D1] = dsearchn(Model(j).vertices, Model(j).tri, Model(i).vertices);
  80. Corr1(:,2) = 1:length(Corr1);
  81. % Corr1(D1 > tol,:) = [];
  82. [Corr2, D2] = dsearchn(Model(i).vertices, Model(i).tri, Model(j).vertices);
  83. Corr2(:,2) = Corr2(:,1);
  84. Corr2(:,1) = 1:length(Corr2);
  85. % Corr2(D2 > tol,:) = [];
  86. funique = ismember(Corr1,Corr2,'rows');
  87. Corr = Corr1(funique,:);
  88. CentroidPtsBelMod(spanI(Corr(:,2)),j) = Corr(:,1)';
  89. CentroidPtsBelMod(spanJ(Corr(:,1)),i) = Corr(:,2)';
  90. if(mod(i,length(Model))+1 == j)
  91. D = D + sum(sum((Model(i).vertices(Corr1(funique,2),:) - Model(j).vertices(Corr1(funique,1),:)).^2,2));
  92. countD = countD + length(funique);
  93. end
  94. end
  95. end
  96. CentroidPtsBelMod(sum(CentroidPtsBelMod>0,2)<2,:) = [];
  97. CentroidPtsBelMod = unique(CentroidPtsBelMod,'rows');
  98. fprintf(1,' %d (all: %d) found\nUpdating Centroid ', size(CentroidPtsBelMod,1), numOfPoints );
  99. numOfPoints = size(CentroidPtsBelMod,1);
  100. % Compute the centroid
  101. Centroid = zeros(numOfPoints,3);
  102. for i=1:length(Model)
  103. fprintf(1,'.');
  104. centroidIndex = find(CentroidPtsBelMod(:,i) > 0) ;
  105. toBeAdded = CentroidPtsBelMod(centroidIndex,i);
  106. Centroid(centroidIndex,:) = Centroid(centroidIndex,:) + Model(i).vertices(toBeAdded,:);
  107. end
  108. Centroid(:,1) = Centroid(:,1) ./ sum(CentroidPtsBelMod>0,2);
  109. Centroid(:,2) = Centroid(:,2) ./ sum(CentroidPtsBelMod>0,2);
  110. Centroid(:,3) = Centroid(:,3) ./ sum(CentroidPtsBelMod>0,2);
  111. fprintf(1,'\nComputing Transformations ');
  112. sqErr = 0;
  113. sqErrBefore = 0;
  114. % WGPA on all centroid points
  115. for i=1:length(Model)
  116. fprintf(1,'.');
  117. inCentroid = find(CentroidPtsBelMod(:,i) > 0);
  118. modIdx = CentroidPtsBelMod(inCentroid,i);
  119. A = Model(i).vertices(modIdx,:);
  120. B = Centroid(inCentroid,:);
  121. tot = CentroidPtsBelMod(inCentroid,:);
  122. curweights = ones(length(inCentroid),length(Model));
  123. if(modelHasWeights)
  124. for j=1:length(Model)
  125. idx = find(tot(:,j)>0);
  126. curweights(idx,j) = Model(j).weights(tot(idx,j));
  127. end
  128. [R1, t1, s1] = reg(B', A', 1.0 - 2*mad(curweights')');
  129. else
  130. [R1, t1, s1] = reg(B', A');
  131. end
  132. sqErrBefore = sqErr + sum(sum((Model(i).vertices(modIdx,:) - Centroid(inCentroid,:)).^2));
  133. Model(i).vertices = (R1*Model(i).vertices'*s1);
  134. Model(i).vertices = [Model(i).vertices(1,:)+t1(1); Model(i).vertices(2,:)+t1(2); Model(i).vertices(3,:)+t1(3)]';
  135. R{i,step} = R1;
  136. t{i,step} = t1;
  137. s{i,step} = s1;
  138. sqErr = sqErr + sum(sum((Model(i).vertices(modIdx,:) - Centroid(inCentroid,:)).^2));
  139. end
  140. sqErr = sqErr/numOfPoints;
  141. sqErrBefore = sqErrBefore/numOfPoints;
  142. corr(step) = numOfPoints;
  143. fprintf(1,'\nMean square error(on point correspondances) Before-After : %f %f\n',sqErrBefore,sqErr);
  144. step = step + 1;
  145. end
  146. % Get the transformation to A in B
  147. function [R1, t1, s1] = reg(B, A, w)
  148. if nargin < 3
  149. n = size(B,2);
  150. mm = mean(B,2);
  151. ms = mean(A,2);
  152. Sshifted = [A(1,:)-ms(1); A(2,:)-ms(2); A(3,:)-ms(3)];
  153. Mshifted = [B(1,:)-mm(1); B(2,:)-mm(2); B(3,:)-mm(3)];
  154. K = Sshifted*Mshifted';
  155. K = K/n;
  156. [U d V] = svd(K);
  157. R1 = V*U';
  158. if det(R1)<0
  159. A = eye(3);
  160. A(3,3) = det(V*U');
  161. R1 = V*A*U';
  162. end
  163. t1 = mm - R1*ms;
  164. s1 = 1;
  165. else
  166. % memory saving procedure... but requires more time in matlab
  167. AjMul = zeros(size(A));
  168. for i = 1:size(A,2)
  169. %compute I - ((Jw * Jw^T) / (Jw^T * Jw))
  170. col = (-w(i) * w' ./ (w' * w));
  171. col(i) = col(i)+1;
  172. for j=1:size(A,1);
  173. AjMul(j,i) = A(j,:) * col';
  174. end
  175. end
  176. [V D W] = svd(AjMul * B');
  177. R1 = W * V';
  178. s1 = trace(R1' * AjMul * B') / trace(AjMul * A');
  179. t1 = (B - R1*s1*A) * (w/(w' * w));
  180. end