Spaces:
Running
Running
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)};
}
|