Spaces:
Running
Running
Download icp.js from devkumarhf/ICP-Lab: direct link, hf CLI and curl.
- Browser
- Download file 4.12 kB
-
https://huggingface.co/spaces/devkumarhf/ICP-Lab/resolve/main/icp.js
- Command line
-
hf download hf://spaces/devkumarhf/ICP-Lab/icp.js
-
curl -L -o icp.js https://huggingface.co/spaces/devkumarhf/ICP-Lab/resolve/main/icp.js
4.12 kB
| // 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)}; | |
| } | |