Kean Walmsley


  • About the Author
    Kean on Google+

July 2014

Sun Mon Tue Wed Thu Fri Sat
    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    








« New self-paced AutoCAD .NET training material | Main | ADN DevCast Episode 6 – Autoloader »

May 09, 2011

Capturing and combining multiple Kinect point clouds inside AutoCAD

Over the weekend, I had more fun exploring the use of Kinect with AutoCAD. It was prompted by an email I had from a UK-based creative team who are interested in the potential of capturing time-lapse point clouds using Kinect. They were curious whether the quality of data coming from the Kinect device would be adequate for doing some interesting trompe l’oeil video compositions.

I started by taking the code from last week’s Kinect post: I removed the code related to gesture detection and beefed up the point-cloud related implementation to deal with composite point clouds that are built up over time.

Some notable changes:

  • Given the potential number of points generated by a sequence of captures, it made sense to limit the volume to within a specific bounding box
    • The KINBOUNDS command sets a clipping volume from the 3D extents of a selected object
    • We use LINQ (similar to the technique shown in this previous post) to filter the points
  • Our jig, used by the KINSNAPS command, now has a timer to measure the appropriate delay between snapshots
    • It shows the snapshot history in ascending colour indeces, to make them stand out from each other
    • As we now have fewer points to display we can display more of them
      • We’re now showing 50% – rather than 10% – of the points inside our bounding box
  • The points from the various capture get combined into a single LAS/PCG file
    • A design decision – they could easily have been kept separate

Here’s how it turned out:

Here’s the C# source code:

using Autodesk.AutoCAD.ApplicationServices;

using Autodesk.AutoCAD.DatabaseServices;

using Autodesk.AutoCAD.EditorInput;

using Autodesk.AutoCAD.Geometry;

using Autodesk.AutoCAD.Runtime;

using AcGi = Autodesk.AutoCAD.GraphicsInterface;

using System.Runtime.InteropServices;

using System.Collections.Generic;

using System.Diagnostics;

using System.Reflection;

using System.Timers;

using System.Linq;

using System.IO;

using System;

using NKinect;

 

namespace KinectIntegration

{

  public class KinectDelayJig : DrawJig

  {

    [DllImport("acad.exe", CharSet = CharSet.Auto,

      CallingConvention = CallingConvention.Cdecl,

      EntryPoint = "?acedPostCommand@@YAHPB_W@Z"

     )]

    extern static private int acedPostCommand(string cmd);

 

    // A transaction and database to add polylines

 

    private Transaction _tr;

    private Document _doc;

 

    // We need our nKinect sensor

 

    private Sensor _kinect = null;

 

    // A list of points captured by the sensor

    // (for eventual export)

 

    private List<ColorVector3> _vecs;

 

    // Uber list of points for final export/import

 

    private List<ColorVector3> _totalVecs;

 

    // A list of points to be displayed

    // (we use this for the jig)

 

    private Point3dCollection _points;

 

    // Transient points that have already been snapped

 

    private List<Point3dCollection> _snapped;

 

    // An offset value we use to move the mouse back

    // and forth by one screen unit

 

    private int _offset;

 

    // The number of snapshots to be taken

 

    private int _numShots;

 

    // Extents to filter points

 

    Extents3d? _ext;

 

    // Is it time to take a snapshot of the sensor data?

 

    private bool _snapshot;

 

    // Timer to tell us when to take a snapshot

 

    private Timer _timer;

 

    // Ready to exit the jig?

 

    private bool _finished;

 

    public bool Finished

    {

      get { return _finished; }

    }

 

    public KinectDelayJig(

      Document doc, Transaction tr,

      int numShots, double delay, Extents3d? ext

    )

    {

      // Initialise the various members

 

      _doc = doc;

      _tr = tr;

      _numShots = numShots;

      _ext = ext;

      _points = new Point3dCollection();

      _snapped = new List<Point3dCollection>();

      _totalVecs = new List<ColorVector3>();

      _offset = 1;

      _snapshot = false;

      _finished = false;

      _timer = new System.Timers.Timer(delay * 1000);

 

      // Hook up the Elapsed event for the timer

 

      _timer.Elapsed +=

        delegate(object source, ElapsedEventArgs args)

        {

          // Flag that it's time to capture a snapshot

 

          _snapshot = true;

 

          // Turn off the timer to be re-enabled later

 

          ((Timer)source).Enabled = false;

        };

 

      // Create our sensor object - the constructor takes

      // three callbacks to receive various data:

      // - skeleton movement

      // - rgb data

      // - depth data

 

      _kinect = new Sensor(s => {}, r => {}, d => {});

 

      _timer.Enabled = true;

    }

 

    public void StartSensor()

    {

      if (_kinect != null)

      {

        _kinect.Start();

      }

    }

 

    public void StopSensor()

    {

      if (_kinect != null)

      {

        _kinect.Stop();

        _kinect.Dispose();

        _kinect = null;

      }

    }

 

    protected override SamplerStatus Sampler(JigPrompts prompts)

    {

      // Se don't really need a point, but we do need some

      // user input event to allow us to loop, processing

      // for the Kinect input

 

      PromptPointResult ppr =

        prompts.AcquirePoint("\nClick to capture: ");

      if (ppr.Status == PromptStatus.OK)

      {

        if (_finished)

        {

          acedPostCommand("CANCELCMD");

          return SamplerStatus.Cancel;

        }

 

        // Generate a point cloud via nKinect

 

        try

        {

          List<ColorVector3> vecList =

            _kinect.GeneratePointCloud();

 

          // Apply a bounding box filter, if one is defined

 

          if (_ext.HasValue)

          {

            // Use LINQ to get the points within the

            // bounding box

 

            var vecSet =

              from ColorVector3 vec in vecList

              where

                vec.X > _ext.Value.MinPoint.X &&

                vec.X < _ext.Value.MaxPoint.X &&

                vec.Y > _ext.Value.MinPoint.Y &&

                vec.Y < _ext.Value.MaxPoint.Y &&

                vec.Z > _ext.Value.MinPoint.Z &&

                vec.Z < _ext.Value.MaxPoint.Z

              select vec;

 

            // Convert our IEnumerable<> into a List<>

 

            _vecs = vecSet.ToList<ColorVector3>();

          }

          else

          {

            _vecs = vecList;

          }

 

          // Extract the points for display in the jig

          // (note we only take 1 in 2)

 

          _points.Clear();

 

          for (int i = 0; i < _vecs.Count; i += 2)

          {

            ColorVector3 vec = _vecs[i];

            _points.Add(

              new Point3d(vec.X, vec.Y, vec.Z)

            );

          }

 

          // Let's move the mouse slightly to avoid having

          // to do it manually to keep the input coming

 

          System.Drawing.Point pt =

            System.Windows.Forms.Cursor.Position;

          System.Windows.Forms.Cursor.Position =

            new System.Drawing.Point(

              pt.X, pt.Y + _offset

            );

          _offset = -_offset;

        }

        catch {}

 

        return SamplerStatus.OK;

      }

      return SamplerStatus.Cancel;

    }

 

    protected override bool WorldDraw(AcGi.WorldDraw draw)

    {

      if (_snapshot)

      {

        if (_points.Count > 0)

        {

          // Make a copy of the latest set of points

 

          Point3d[] tmp = new Point3d[_points.Count];

          _points.CopyTo(tmp, 0);

 

          // Add the copy to the list of snapshot previews

 

          _snapped.Add(new Point3dCollection(tmp));

 

          // Add the core list to the total set

 

          _totalVecs.AddRange(_vecs);

        }

      }

 

      short origColor = draw.SubEntityTraits.Color;

 

      for (int i = 0; i < _snapped.Count; i++)

      {

        // Cycle through colour indeces for each snapshot

 

        draw.SubEntityTraits.Color = (short)(i + 1);

 

        // Draw the actual snapshot, one by one

 

        if (_snapped[i].Count > 0)

          draw.Geometry.Polypoint(_snapped[i], null, null);

      }

 

      // Set the colour back to the original

 

      draw.SubEntityTraits.Color = origColor;

 

      if (_snapshot)

      {

        // Reset the flag, timer and check whether finished

 

        _snapshot = false;

        _finished = (--_numShots == 0);

        _timer.Enabled = true;

      }

      else

      {

        // This simply draws our points

 

        if (_points.Count > 0)

          draw.Geometry.Polypoint(_points, null, null);

      }

 

      return true;

    }

 

    public void ExportPointCloud(string filename)

    {

      if (_totalVecs.Count > 0)

      {

        using (StreamWriter sw = new StreamWriter(filename))

        {

          // For each pixel, write a line to the text file:

          // X, Y, Z, R, G, B

 

          foreach (ColorVector3 pt in _totalVecs)

          {

            sw.WriteLine(

              "{0}, {1}, {2}, {3}, {4}, {5}",

              pt.X, pt.Y, pt.Z, pt.R, pt.G, pt.B

            );

          }

        }

      }

    }

  }

 

  public class Commands

  {

    // Static members for command settings

 

    private static int _numShots = 5;

    private static double _delay = 5.0;

    private static Extents3d? _ext = null;

 

    [CommandMethod("ADNPLUGINS", "KINBOUNDS", CommandFlags.Modal)]

    public void SetBoundingBox()

    {

      Document doc =

        Autodesk.AutoCAD.ApplicationServices.

          Application.DocumentManager.MdiActiveDocument;

      Editor ed = doc.Editor;

 

      // Ask the user to select an entity

 

      PromptEntityOptions peo =

        new PromptEntityOptions(

          "\nSelect entity to define bounding box"

        );

      peo.AllowNone = true;

      peo.Keywords.Add("None");

      peo.Keywords.Default = "None";

 

      PromptEntityResult per = ed.GetEntity(peo);

 

      if (per.Status != PromptStatus.OK)

        return;

 

      // If "None" selected, clear the bounding box

 

      if (per.Status == PromptStatus.None ||

          per.StringResult == "None")

      {

        _ext = null;

        ed.WriteMessage("\nBounding box cleared.");

        return;

      }

 

      // Otherwise open the entity and gets its extents

 

      Transaction tr =

        doc.TransactionManager.StartTransaction();

      using (tr)

      {

        Entity ent =

          tr.GetObject(per.ObjectId, OpenMode.ForRead)

            as Entity;

        if (ent != null)

          _ext = ent.Bounds;

 

        ed.WriteMessage(

          "\nBounding box set to {0}", _ext

        );

        tr.Commit();

      }

    }

 

    [CommandMethod("ADNPLUGINS", "KINSNAPS", CommandFlags.Modal)]

    public void ImportFromKinect()

    {

      Document doc =

        Autodesk.AutoCAD.ApplicationServices.

          Application.DocumentManager.MdiActiveDocument;

      Editor ed = doc.Editor;

 

      // Get some user input for the number of snapshots...

 

      PromptIntegerOptions pio =

        new PromptIntegerOptions("\nNumber of captures");

      pio.AllowZero = false;

      pio.DefaultValue = _numShots;

      pio.UseDefaultValue = true;

      pio.UpperLimit = 20;

      pio.LowerLimit = 1;

 

      PromptIntegerResult pir = ed.GetInteger(pio);

 

      if (pir.Status != PromptStatus.OK)

        return;

 

      _numShots = pir.Value;

 

      // ... and the delay between them

 

      PromptDoubleOptions pdo =

        new PromptDoubleOptions("\nNumber of seconds delay");

      pdo.AllowZero = false;

      pdo.AllowNegative = false;

      pdo.AllowArbitraryInput = false;

      pdo.DefaultValue = _delay;

      pdo.UseDefaultValue = true;

 

      PromptDoubleResult pdr = ed.GetDouble(pdo);

 

      if (pdr.Status != PromptStatus.OK)

        return;

 

      _delay = pdr.Value;

 

      Transaction tr =

        doc.TransactionManager.StartTransaction();

 

      KinectDelayJig kj =

        new KinectDelayJig(doc, tr, _numShots, _delay, _ext);

      kj.StartSensor();

      PromptResult pr = ed.Drag(kj);

      kj.StopSensor();

 

      if (pr.Status != PromptStatus.OK && !kj.Finished)

      {

        tr.Dispose();

        return;

      }

 

      tr.Commit();

 

      // Manually dispose to avoid scoping issues with

      // other variables

 

      tr.Dispose();

 

      // We'll store most local files in the temp folder.

      // We get a temp filename, delete the file and

      // use the name for our folder

 

      string localPath = Path.GetTempFileName();

      File.Delete(localPath);

      Directory.CreateDirectory(localPath);

      localPath += "\\";

 

      // Paths for our temporary files

 

      string txtPath = localPath + "points.txt";

      string lasPath = localPath + "points.las";

 

      // Our PCG file will be stored under My Documents

 

      string outputPath =

        Environment.GetFolderPath(

          Environment.SpecialFolder.MyDocuments

        ) + "\\Kinect Point Clouds\\";

 

      if (!Directory.Exists(outputPath))

        Directory.CreateDirectory(outputPath);

 

      // We'll use the title as a base filename for the PCG,

      // but will use an incremented integer to get an unused

      // filename

 

      int cnt = 0;

      string pcgPath;

      do

      {

        pcgPath =

          outputPath + "Kinect" +

          (cnt == 0 ? "" : cnt.ToString()) + ".pcg";

        cnt++;

      }

      while (File.Exists(pcgPath));

 

      // The path to the txt2las tool will be the same as the

      // executing assembly (our DLL)

 

      string exePath =

        Path.GetDirectoryName(

          Assembly.GetExecutingAssembly().Location

        ) + "\\";

 

      if (!File.Exists(exePath + "txt2las.exe"))

      {

        ed.WriteMessage(

          "\nCould not find the txt2las tool: please make sure " +

          "it is in the same folder as the application DLL."

        );

        return;

      }

 

      // Export our point cloud from the jig

 

      ed.WriteMessage(

        "\nSaving TXT file of the captured points.\n"

      );

 

      kj.ExportPointCloud(txtPath);

 

      // Use the txt2las utility to create a .LAS

      // file from our text file

 

      ed.WriteMessage(

        "\nCreating a LAS from the TXT file.\n"

      );

 

      ProcessStartInfo psi =

        new ProcessStartInfo(

          exePath + "txt2las",

          "-i \"" + txtPath +

          "\" -o \"" + lasPath +

          "\" -parse xyzRGB"

        );

      psi.CreateNoWindow = false;

      psi.WindowStyle = ProcessWindowStyle.Hidden;

 

      // Wait up to 20 seconds for the process to exit

 

      try

      {

        using (Process p = Process.Start(psi))

        {

          p.WaitForExit();

        }

      }

      catch

      { }

 

      // If there's a problem, we return

 

      if (!File.Exists(lasPath))

      {

        ed.WriteMessage(

          "\nError creating LAS file."

        );

        return;

      }

 

      File.Delete(txtPath);

 

      ed.WriteMessage(

        "Indexing the LAS and attaching the PCG.\n"

      );

 

      // Index the .LAS file, creating a .PCG

 

      string lasLisp = lasPath.Replace('\\', '/'),

              pcgLisp = pcgPath.Replace('\\', '/');

 

      doc.SendStringToExecute(

        "(command \"_.POINTCLOUDINDEX\" \"" +

          lasLisp + "\" \"" +

          pcgLisp + "\")(princ) ",

        false, false, false

      );

 

      // Attach the .PCG file

 

      doc.SendStringToExecute(

        "_.WAITFORFILE \"" +

        pcgLisp + "\" \"" +

        lasLisp + "\" " +

        "(command \"_.-POINTCLOUDATTACH\" \"" +

        pcgLisp +

        "\" \"0,0\" \"1\" \"0\")(princ) ",

        false, false, false

      );

 

      doc.SendStringToExecute(

        "_.-VISUALSTYLES _C _Conceptual ",

        false, false, false

      );

 

      //Cleanup();

    }

 

    // Return whether a file is accessible

 

    private bool IsFileAccessible(string filename)

    {

      // If the file can be opened for exclusive access it means

      // the file is accesible

      try

      {

        FileStream fs =

          File.Open(

            filename, FileMode.Open,

            FileAccess.Read, FileShare.None

          );

        using (fs)

        {

          return true;

        }

      }

      catch (IOException)

      {

        return false;

      }

    }

 

    // A command which waits for a particular PCG file to exist

 

    [CommandMethod(

      "ADNPLUGINS", "WAITFORFILE", CommandFlags.NoHistory

     )]

    public void WaitForFileToExist()

    {

      Document doc =

        Application.DocumentManager.MdiActiveDocument;

      Editor ed = doc.Editor;

      HostApplicationServices ha =

        HostApplicationServices.Current;

 

      PromptResult pr = ed.GetString("Enter path to PCG: ");

      if (pr.Status != PromptStatus.OK)

        return;

      string pcgPath = pr.StringResult.Replace('/', '\\');

 

      pr = ed.GetString("Enter path to LAS: ");

      if (pr.Status != PromptStatus.OK)

        return;

      string lasPath = pr.StringResult.Replace('/', '\\');

 

      ed.WriteMessage(

        "\nWaiting for PCG creation to complete...\n"

      );

 

      // Check the write time for the PCG file...

      // if it hasn't been written to for at least half a second,

      // then we try to use a file lock to see whether the file

      // is accessible or not

 

      const int ticks = 50;

      TimeSpan diff;

      bool cancelled = false;

 

      // First loop is to see when writing has stopped

      // (better than always throwing exceptions)

 

      while (true)

      {

        if (File.Exists(pcgPath))

        {

          DateTime dt = File.GetLastWriteTime(pcgPath);

          diff = DateTime.Now - dt;

          if (diff.Ticks > ticks)

            break;

        }

        System.Windows.Forms.Application.DoEvents();

      }

 

      // Second loop will wait until file is finally accessible

      // (by calling a function that requests an exclusive lock)

 

      if (!cancelled)

      {

        int inacc = 0;

        while (true)

        {

          if (IsFileAccessible(pcgPath))

            break;

          else

            inacc++;

          System.Windows.Forms.Application.DoEvents();

        }

        ed.WriteMessage("\nFile inaccessible {0} times.", inacc);

 

        try

        {

          CleanupTmpFiles(lasPath);

        }

        catch

        { }

      }

    }

 

    internal void CleanupTmpFiles(string txtPath)

    {

      if (File.Exists(txtPath))

        File.Delete(txtPath);

      Directory.Delete(

        Path.GetDirectoryName(txtPath)

      );

    }

  }

}

I still feel I need to do more with navigation – which is hopefully the next area of focus – but we’ll see. It depends on whether something else comes in to distract me. :-)

blog comments powered by Disqus

10 Random Posts