// 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;jsum+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;iadd(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)}; }