用 F# 和 Kinect +OpenNI 產生 Point Cloud

這和前一篇 用 F# 和 Kinect SDK 產生 Point Cloud 是幾乎一樣的程式，只不過改用 OpenNI 而不是用 Kinect SDK beta。 OpenNI 支援將色彩和深度資訊重合，所以出來的結果比之前手工亂搞的正確得多。但是似乎沒有 Kinect SDK 的 ready callback 機制，所以用 background worker 取代。
OpenNI 似乎也不支援 80x60 的解析度。

open System
open System.Xaml
open System.Windows
open System.Windows.Controls
open System.Windows.Media
open OpenNI
open System.Windows.Media.Media3D
open System.Threading
open System.ComponentModel
let input_res_x, input_res_y = 640, 480             //resolution of the camera
let step_x, step_y =4, 4                            //for down sampling
let rx,ry= input_res_x/step_x , input_res_y/step_y  //there are rx*ry cubes
let screenZ = 100.                                  //on a "screen"
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) //the size of each cube
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 
    let mat = new DiffuseMaterial(Brushes.White.Clone())
    let st= new ScaleTransform3D(1.0, 1.0, 1.0)
    GeometryModel3D(Geometry = g, Material=mat, Transform=st)
let models = Model3DGroup() //the model group contains cubes and lights
let vprt =   // 3d viewport which moves periodically
    let translation  = TranslateTransform3D(0., 0., 45.)
    let rotation = AxisAngleRotation3D(Vector3D(0.,1.,0.), 30.)
    let rotation2 = AxisAngleRotation3D(Vector3D(1.,0.,0.), 30.)
    let anim = Animation.DoubleAnimation (-40., 40., 
                        Duration(System.TimeSpan.FromSeconds 8.), 
                        AutoReverse=true, 
                        RepeatBehavior = Animation.RepeatBehavior.Forever)
    let anim2 = Animation.DoubleAnimation (-20., 20., 
                        Duration(System.TimeSpan.FromSeconds 21.), 
                        AutoReverse=true, 
                        RepeatBehavior = Animation.RepeatBehavior.Forever)
    let tg=Transform3DGroup()    
    rotation.BeginAnimation(AxisAngleRotation3D.AngleProperty, anim) 
    rotation2.BeginAnimation(AxisAngleRotation3D.AngleProperty, 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 context=new Context()
let mapMode = new MapOutputMode(XRes=input_res_x, YRes=input_res_y, FPS=30)
let color=new ImageGenerator(context, MapOutputMode=mapMode, 
                                      PixelFormat=PixelFormat.RGB24)
let depth=new DepthGenerator(context, MapOutputMode=mapMode)
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 ->                 
              ((p.Material :?> DiffuseMaterial).Brush  
                           :?> SolidColorBrush).Color <- argb              
              let st=(p.Transform :?> ScaleTransform3D)
              st.ScaleX <- z
              st.ScaleY <- z
              st.ScaleZ <- z  ) zList
let handleDepth (dmap:UInt16MapData) (cmap:MapData<RGB24Pixel>) =      
        let void_color = Color.FromArgb (byte 0, byte 0,byte 0, byte 0) 
        let zList=[for y in 0 .. step_y .. dmap.YRes-step_y  do
                        let yline=y*dmap.XRes
                        for x in 0 .. step_x .. dmap.XRes-step_x do                            
                           let z=(float dmap.[yline+x])/(float depth.DeviceMaxDepth)
                           let argb = 
                              if z=0.0 then void_color                                         
                              else cmap.[yline+x] 
                                   |> fun c -> Color.FromRgb (c.Red, c.Green, c.Blue)
                           yield (z, argb)]
        ((new ThreadStart(fun () -> updateCloud zList), null)
        |> app.Dispatcher.BeginInvoke).Wait() |>ignore
let bgWorker=new BackgroundWorker(WorkerSupportsCancellation = true)
depth.AlternativeViewpointCapability.SetViewpoint color
vprt.Children.Add (ModelVisual3D(Content = models))
sqCloud |> List.iter models.Children.Add
AmbientLight() |>  models.Children.Add
context.StartGeneratingAll()
fun _  ->
    while not bgWorker.CancellationPending do
         context.WaitAndUpdateAll()   
         handleDepth (depth.GetDepthMap()) (color.GetRGB24ImageMap()) 
|> bgWorker.DoWork.Add
bgWorker.RunWorkerAsync()
wnd.Closed.Add (fun e -> do                             
                            bgWorker.CancelAsync()                            
                            context.StopGeneratingAll()
                            context.Shutdown()                                             
                            Environment.Exit 0 )
[<STAThread>]
do app.Run(wnd) |> ignore