用 F# 和 Kinect SDK 產生 Point Cloud

本文附的 F# 程式碼，是用 Kinect SDK 來產生 "points cloud"。
程式的效果如影片。
過程中的一些紀錄：

• 因為之前的 visual studio 2010 過了試用期限，所以我有一段時間沒用 F#， 稍有一點生疏，但這問題不大。問題是我沒有寫過 .net 上的 GUI 程式，更別說是 3D 繪圖了，所以花了一點時間在網路上找範例。 F# 的 GUI 參考資料似乎不是很多，很多似乎也不太靈。
• Kinect SDK 和 driver 簡單好用，骨架判斷似乎比之前 OpenNI 容易（沒有打錯字，這不是用來判斷股價的軟體），在狹小空間（像是大多數寫程式的地方）就能辨認出骨架。
• 根據 Wikipedia，Kinect 的 depth sensor 橫向角度 57度，垂直角度 43度。
• 網路查詢結果，System.Media3D 似乎無法直接畫出「點」，沒辦法做出 point cloud。所以用 squares cloud 或者 cubes cloud 來代替。
• 我的筆記型電腦 GPU 太差，最多只能跑 80x60 的 cubes cloud。所以影片中解析度很差。
• 設法將 Video Image 和 Depth Image 組合，發現不吻合。理論上應該要用 Color in Depth Space 或者 Depth in Color Space 的參數來設定 Kinect，但是網路上查詢的結果是， Kinect SDK Beta 還不支援這些。
• 也許應該改用 OpenNI，但是程式碼已經寫了一半了，繼續用手工的方是將色彩和深度組合。
• 色彩和深度的圖形居然是鏡像（x軸方向相反）？應該是我哪裡搞錯了。
• 由於沒有硬體規格，解析度太低也很難實驗，所以用觀察和嘗試錯誤法很粗糙的將色彩資訊以及深度資訊勉強組合在一起。
• 之後改用 OpenNI 試試看。

open System
open System.Xaml
open System.Windows
open System.Windows.Controls
open System.Windows.Media
open System.Windows.Media.Media3D
open System.Threading
open Microsoft.Research.Kinect.Nui
let rx,ry,res= 80, 60, ImageResolution.Resolution80x60
//let rx,ry,res= 320, 240, ImageResolution.Resolution320x240
let vrx,vry,vres= 640, 480, ImageResolution.Resolution640x480
let screenZ = 100.
let screenX = screenZ * 2.*(Math.Sin (38.5*Math.PI/180.0))
let screenY = screenZ * 2.*(Math.Sin (21.5*Math.PI/180.0))
let dX, dY=screenX /(float rx), screenY /(float ry)
let smallCube  (p:Vector3D) =
    let i,j=Vector3D(dX,0.,0.)/2.0,Vector3D(0.,dY,0.)/2.0
    let k=Vector3D(0., 0., dX)/2.0
    let g=new MeshGeometry3D()
    [p-i-j-k;p+i-j-k;p+i+j-k;p-i+j-k;p-i-j+k;p+i-j+k;p+i+j+k;p-i+j+k]
    |> List.iter (fun v ->  Point3D(v.X, v.Y, v.Z) |> g.Positions.Add)
    [[2;1;0];[2;0;3]; [7;3;0];[7;0;4]; [6;5;1];[6;1;2];
     [7;6;2];[7;2;3]; [5;6;7];[5;7;4]; [1;5;4];[0;1;4]] 
    |> Seq.concat |> Seq.iter g.TriangleIndices.Add 
    GeometryModel3D(Geometry = g, Material = DiffuseMaterial(Brushes.White))
let models = Model3DGroup()
let vprt =     
    let translation  = TranslateTransform3D(0., 0., 50.)
    let rotation = AxisAngleRotation3D(Vector3D(0.,1.,0.), 30.)
    let rotation2 = AxisAngleRotation3D(Vector3D(1.,0.,0.), 30.)
    let anim = Animation.DoubleAnimation (-45., 45., 
                        Duration(System.TimeSpan.FromSeconds 8.), 
                        AutoReverse=true, 
                        RepeatBehavior = Animation.RepeatBehavior.Forever)
    let anim2 = Animation.DoubleAnimation (-20., 45., 
                        Duration(System.TimeSpan.FromSeconds 21.), 
                        AutoReverse=true, 
                        RepeatBehavior = Animation.RepeatBehavior.Forever)
    let tg=Transform3DGroup()    
    rotation.BeginAnimation(AxisAngleRotation3D.AngleProperty, anim) 
    rotation2.BeginAnimation(AxisAngleRotation3D.AngleProperty, anim2)
    rotation2.BeginAnimation(TranslateTransform3D.OffsetYProperty, anim2)
    tg.Children.Add (RotateTransform3D rotation)
    tg.Children.Add (RotateTransform3D rotation2)
    tg.Children.Add translation    
    Viewport3D(Camera = PerspectiveCamera(Point3D(0.,0., -50.), 
                                Vector3D(0., 0., 1.), 
                                Vector3D(0., 1., 0.), 60., Transform=tg))
let sqCloud =    
    [for iy in [0 .. (ry-1)] do
        for ix in [0 .. (rx-1)] do
            yield Vector3D((float(ix-rx/2))*dX, (float(ry/2-iy))*dY, 100.)
                  |> smallCube ]
let nui = Runtime() 
let wnd=Window(Title = "Kinect Depth", Background=Brushes.Black, Content=vprt)
let app=new Application()
let updateCloud zList=  
        sqCloud |> List.iter2 (
           fun (z, argb) p ->
                if z=0.0 then 
                    p.Transform<-TranslateTransform3D(0.,0.,5000.0)
                    p.Material<- DiffuseMaterial(Brushes.Green)
                else                    
                    let c = Color.FromArgb argb
                    p.Material<- DiffuseMaterial(SolidColorBrush(c))                    
                    p.Transform <- ScaleTransform3D(z,z,z)) zList                
let handleDepth (bits:byte[]) (vbits:byte[])=
        let diffx = 0.015        
        let getVix x z = if z=0.0 then -1 else 
                              (x/(float rx)+diffx-diffx/z)*(float vrx)
                              |> Math.Round |> int
        let getViy y z = if z = 0.0 then -1 else
                         (y/(float ry)*0.96+0.04)*(float vry)
                         |> Math.Round |> int
        let getVixiy ix iy z = (getVix (float ix) z, getViy (float iy) z)        
        let getVi vix viy = if vix<0 || vix>=vrx || viy<0 || viy>=vry then -1
                            else (vrx-vix-1+viy*vrx)*4
        let zList=[for i in 0 .. 2 .. bits.Length-1 do                        
                        let i1=int bits.[i]
                        let i2=(bits.[i+1] &&& (byte 0xf) )|> int                        
                        let z=(float ((i2<<<8)|||i1))/4096.0                        
                        let iy, ix = Math.DivRem (i>>>1, rx)
                        let vix, viy = getVixiy ix iy z
                        let vi = getVi vix viy
                        let argb = 
                          if vi = -1 then 
                             (byte 0x30, byte 0,byte 0xff, byte 0) 
                          else 
                             (byte 0xff, vbits.[vi+2], vbits.[vi+1], vbits.[vi])
                        yield (z, argb)]
        ((new ThreadStart(fun () -> updateCloud zList), null)
        |> app.Dispatcher.BeginInvoke).Wait() |>ignore       
        Thread.Sleep(7)
let videoReady (e:ImageFrameReadyEventArgs) =   
   let bits = (nui.DepthStream.GetNextFrame(100).Image.Bits)
   handleDepth bits (e.ImageFrame.Image.Bits)
vprt.Children.Add(ModelVisual3D(Content = models))
sqCloud |> List.iter models.Children.Add
AmbientLight() |>  models.Children.Add
nui.Initialize(RuntimeOptions.UseDepth ||| RuntimeOptions.UseColor)         
nui.DepthStream.Open(ImageStreamType.Depth, 2, res, ImageType.Depth)
nui.VideoStream.Open(ImageStreamType.Video, 2, vres, ImageType.Color)
nui.VideoFrameReady.Add videoReady
wnd.Closed.Add (fun e -> do nui.Uninitialize()                       
                            Environment.Exit 0 )
[<STAThread>]
do app.Run(wnd) |> ignore