File size: 4,121 Bytes
7bd1cc6
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
// Point-to-point rigid registration on small, synthetic point clouds.
// The optimal rotation is found with Horn's unit-quaternion formulation.
const add=(a,b)=>a.map((v,i)=>v+b[i]);
const sub=(a,b)=>a.map((v,i)=>v-b[i]);
const scale=(a,s)=>a.map(v=>v*s);
const dot=(a,b)=>a.reduce((sum,v,i)=>sum+v*b[i],0);
const norm2=(a)=>dot(a,a);
const mean=(pts)=>scale(pts.reduce((a,p)=>add(a,p),[0,0,0]),1/pts.length);

export function rng(seed=2026){
  let state=seed>>>0;
  return ()=>{state=(Math.imul(1664525,state)+1013904223)>>>0;return state/4294967296;};
}
function normal(random){
  return Math.sqrt(-2*Math.log(Math.max(random(),1e-12)))*Math.cos(2*Math.PI*random());
}
export function referenceCloud(seed=2026){
  const random=rng(seed), pts=[];
  for(let i=0;i<230;i++){
    const x=2*random()-1,y=1.7*random()-.85;
    // An asymmetric curved surface avoids an artificially symmetric toy shape.
    const z=.23*Math.sin(2.7*x)+.18*Math.cos(3.4*y)+.19*x*y+.13*Math.exp(-((x-.45)**2+(y+.22)**2)/.08);
    pts.push([x,y,z]);
  }
  return pts;
}
export function transformCloud(points,angleDeg,shift,noise=0,seed=71){
  const r=angleDeg*Math.PI/180,c=Math.cos(r),s=Math.sin(r), random=rng(seed);
  return points.map(([x,y,z])=>[c*x-s*y+shift,s*x+c*y-.72*shift,z+.30*shift].map(v=>v+noise*normal(random)));
}
export function nearest(source,target){
  return source.map(p=>{
    let best=Infinity,index=0;
    for(let j=0;j<target.length;j++){
      const d=norm2(sub(p,target[j])); if(d<best){best=d;index=j;}
    }
    return {index,d2:best};
  });
}
export function nearestRMSE(source,target){
  const matches=nearest(source,target);
  return Math.sqrt(matches.reduce((sum,m)=>sum+m.d2,0)/matches.length);
}
export function pairedRMSE(source,target){
  return Math.sqrt(source.reduce((sum,p,i)=>sum+norm2(sub(p,target[i])),0)/source.length);
}

// Jacobi diagonalization is robust for the small symmetric 4x4 Horn matrix.
function largestEigenvector(matrix){
  const a=matrix.map(row=>row.slice()), v=Array.from({length:4},(_,i)=>Array.from({length:4},(_,j)=>+(i===j)));
  for(let it=0;it<80;it++){
    let p=0,q=1,big=0;
    for(let i=0;i<4;i++)for(let j=i+1;j<4;j++) if(Math.abs(a[i][j])>big){big=Math.abs(a[i][j]);p=i;q=j;}
    if(big<1e-13) break;
    const phi=.5*Math.atan2(2*a[p][q],a[q][q]-a[p][p]),c=Math.cos(phi),s=Math.sin(phi);
    const app=a[p][p],aqq=a[q][q],apq=a[p][q];
    for(let k=0;k<4;k++) if(k!==p&&k!==q){
      const akp=a[k][p],akq=a[k][q];
      a[k][p]=a[p][k]=c*akp-s*akq;
      a[k][q]=a[q][k]=s*akp+c*akq;
    }
    a[p][p]=c*c*app-2*s*c*apq+s*s*aqq;
    a[q][q]=s*s*app+2*s*c*apq+c*c*aqq;
    a[p][q]=a[q][p]=0;
    for(let k=0;k<4;k++){const vp=v[k][p],vq=v[k][q];v[k][p]=c*vp-s*vq;v[k][q]=s*vp+c*vq;}
  }
  let index=0; for(let i=1;i<4;i++)if(a[i][i]>a[index][index])index=i;
  let out=v.map(row=>row[index]);const length=Math.sqrt(norm2(out));return out.map(x=>x/length);
}
function rotate(q,p){
  const [w,x,y,z]=q,[px,py,pz]=p;
  const tx=2*(y*pz-z*py),ty=2*(z*px-x*pz),tz=2*(x*py-y*px);
  return [px+w*tx+y*tz-z*ty,py+w*ty+z*tx-x*tz,pz+w*tz+x*ty-y*tx];
}
export function rigidFit(source,target){
  if(source.length!==target.length||source.length<3)throw new Error('At least three paired points are required.');
  const pc=mean(source),qc=mean(target),h=Array.from({length:3},()=>[0,0,0]);
  for(let i=0;i<source.length;i++){
    const p=sub(source[i],pc),q=sub(target[i],qc);
    for(let r=0;r<3;r++)for(let c=0;c<3;c++)h[r][c]+=p[r]*q[c];
  }
  const [[xx,xy,xz],[yx,yy,yz],[zx,zy,zz]]=h;
  const m=[[xx+yy+zz,yz-zy,zx-xz,xy-yx],
           [yz-zy,xx-yy-zz,xy+yx,zx+xz],
           [zx-xz,xy+yx,-xx+yy-zz,yz+zy],
           [xy-yx,zx+xz,yz+zy,-xx-yy+zz]];
  const quaternion=largestEigenvector(m),translation=sub(qc,rotate(quaternion,pc));
  return {quaternion,translation,apply:p=>add(rotate(quaternion,p),translation)};
}
export function stepICP(source,target){
  const matches=nearest(source,target),fit=rigidFit(source,matches.map(m=>target[m.index]));
  const points=source.map(fit.apply);
  return {points,rmse:nearestRMSE(points,target),paired:pairedRMSE(points,target)};
}