Enhanced Kinect integration for 3D polyline creation

I’ve improved the basic implementation in this previous post pretty significantly over the last week:
New ability to draw multiple polylines
- Added a gesture of lowering/raising the left hand to start/finish drawing with the right
Addition of a transient sphere as a 3D cursor for polyline drawing
Quick flash of a transient skeleton (arms and chest only) on user detection
The jig now perpetuates by changing the screen cursor minutely to and fro
- Mouse input is needed to keep the jig active; Kinect input doesn’t yet count :-)
A new gesture of placing hands together to end drawing
At Barry Ralphs’ suggestion, I also invested some time in creating a video of the application in action:
The updated C# code is here:
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.Windows.Media;
using System.Diagnostics;
using System.Reflection;
using System.IO;
using System;
using NKinect;
 
namespace KinectIntegration
{
 
  public class KinectJig : DrawJig
  {
    [DllImport("acad.exe", CharSet = CharSet.Auto,
      CallingConvention = CallingConvention.Cdecl,
      EntryPoint = "?acedPostCommand@@YAHPB_W@Z"
     )]
    extern static private int acedPostCommand(string strExpr);
 
    // 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;
 
    // A list of points to be displayed
    // (we use this for the jig)
 
    private Point3dCollection _points;
 
    // A list of vertices to draw between
    // (we use this for the final polyline creation)
 
    private Point3dCollection _vertices;
 
    // The most recent vertex being captured/drawn
 
    private Point3d _curPt;
    private Entity _cursor;
 
    // A list of line segments being collected
    // (pass these as AcGe objects as they may
    // get created on a background thread)
 
    private List<LineSegment3d> _lineSegs;
 
    // The database lines we use for temporary
    // graphics (that need disposing afterwards)
 
    private DBObjectCollection _lines;
 
    // An offset value we use to move the mouse back
    // and forth by one screen unit
 
    private int _offset;
 
    // Flags to indicate Kinect gesture modes
 
    private bool _calibrating; // First skeleton callback
    private bool _drawing;     // Drawing mode active
    private bool _finished;    // Finished - want to exit
 
    public bool Finished
    {
      get { return _finished; }
    }
 
    public KinectJig(Document doc, Transaction tr)
    {
      // Initialise the various members
 
      _doc = doc;
      _tr = tr;
      _points = new Point3dCollection();
      _vertices = new Point3dCollection();
      _lineSegs = new List<LineSegment3d>();
      _lines = new DBObjectCollection();
      _cursor = null;
      _offset = 1;
      _calibrating = true;
      _drawing = false;
      _finished = false;
 
      // Create our sensor object - the constructor takes
      // three callbacks to receive various data:
      // - skeleton movement
      // - rgb data
      // - depth data
 
      _kinect =
        new Sensor(
          s =>
          {
            if (_calibrating)
            {
              DrawSkeleton(s);
            }
            else
            {
              if (!_finished)
              {
                _drawing = (s.LeftHand.Y < s.LeftHip.Y);
 
                // Get the current position of the hands
 
                Point3d right =
                  new Point3d(
                    s.RightHand.X,
                    s.RightHand.Y,
                    s.RightHand.Z
                  );
 
                Point3d left =
                  new Point3d(
                    s.LeftHand.X,
                    s.LeftHand.Y,
                    s.LeftHand.Z
                  );
 
                if (left.DistanceTo(right) < 50.0)
                {
                  _drawing = false;
                  _finished = true;
                }
 
                if (_drawing)
                {
                  // If we have at least one prior vertex...
 
                  if (_vertices.Count > 0)
                  {
                    // ... connect them together with
                    // a temp LineSegment3d
 
                    Point3d lastVert =
                      _vertices[_vertices.Count - 1];
                    if (lastVert.DistanceTo(right) >
                        Tolerance.Global.EqualPoint)
                    {
                      _lineSegs.Add(
                        new LineSegment3d(lastVert, right)
                      );
                    }
                  }
 
                  // Add the new vertex to our list
 
                  _vertices.Add(right);
                }
              }
            }
          },
          r =>
          {
          },
          d =>
          {
          }
        );
    }
 
    public void StartSensor()
    {
      if (_kinect != null)
      {
        _kinect.Start();
      }
    }
 
    public void StopSensor()
    {
      if (_kinect != null)
      {
        _kinect.Stop();
        _kinect.Dispose();
        _kinect = null;
      }
    }
 
    private void DrawSkeleton(UserSkeleton s)
    {
      _lineSegs.Add(
        new LineSegment3d(
          new Point3d(
            s.LeftHand.X, s.LeftHand.Y, s.LeftHand.Z
          ),
          new Point3d(
            s.LeftElbow.X, s.LeftElbow.Y, s.LeftElbow.Z
          )
        )
      );
      _lineSegs.Add(
        new LineSegment3d(
          new Point3d(
            s.LeftElbow.X, s.LeftElbow.Y, s.LeftElbow.Z
          ),
          new Point3d(
            s.LeftShoulder.X, s.LeftShoulder.Y, s.LeftShoulder.Z
          )
        )
      );
      _lineSegs.Add(
        new LineSegment3d(
          new Point3d(
            s.LeftShoulder.X, s.LeftShoulder.Y, s.LeftShoulder.Z
          ),
          new Point3d(
            s.RightShoulder.X, s.RightShoulder.Y, s.RightShoulder.Z
          )
        )
      );
      _lineSegs.Add(
        new LineSegment3d(
          new Point3d(
            s.RightShoulder.X, s.RightShoulder.Y, s.RightShoulder.Z
          ),
          new Point3d(
            s.RightElbow.X, s.RightElbow.Y, s.RightElbow.Z
          )
        )
      );
      _lineSegs.Add(
        new LineSegment3d(
          new Point3d(
            s.RightElbow.X, s.RightElbow.Y, s.RightElbow.Z
          ),
          new Point3d(
            s.RightHand.X, s.RightHand.Y, s.RightHand.Z
          )
        )
      );
    }
 
    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)
        {
          /*
          _doc.SendStringToExecute(
            "\x1b\x1b ", false, false, false
          );
           */
          acedPostCommand("CANCELCMD");
          return SamplerStatus.Cancel;
        }
 
        if (!_drawing && _lines.Count > 0)
        {
          AddPolylines();
        }
 
        // Generate a point cloud via nKinect
 
        try
        {
          _vecs = _kinect.GeneratePointCloud();
 
          // Extract the points for display in the jig
          // (note we only take 1 in 5)
 
          _points.Clear();
 
          for (int i = 0; i < _vecs.Count; i += 10)
          {
            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)
    {
      // This simply draws our points
 
      draw.Geometry.Polypoint(_points, null, null);
 
      AcGi.TransientManager ctm =
        AcGi.TransientManager.CurrentTransientManager;
      IntegerCollection ints = new IntegerCollection();
 
      // Draw any outstanding segments (and do so only once)
 
      bool wasCalibrating = _calibrating;
 
      while (_lineSegs.Count > 0)
      {
        // Get the line segment and remove it from the list
 
        LineSegment3d ls = _lineSegs[0];
        _lineSegs.RemoveAt(0);
 
        // Create an equivalent, red, database line
 
        Line ln = new Line(ls.StartPoint, ls.EndPoint);
        ln.ColorIndex = (wasCalibrating ? 2 : 1);
        _lines.Add(ln);
 
        // Draw it as transient graphics
 
        ctm.AddTransient(
          ln, AcGi.TransientDrawingMode.DirectShortTerm,
          128, ints
        );
 
        _calibrating = false;
      }
 
      if (_drawing)
      {
        if (_cursor == null)
        {
          if (_vertices.Count > 0)
          {
            // Clear our skeleton
 
            ClearTransients();
 
            _curPt = _vertices[_vertices.Count - 1];
 
            Solid3d sol = new Solid3d();
            sol.CreateSphere(40.0);
            _cursor = sol;
            _cursor.TransformBy(
              Matrix3d.Displacement(_curPt - Point3d.Origin)
            );
 
            //_cursor = new DBPoint(_curPt);
            //_cursor.Layer = "0";
            _cursor.ColorIndex = 2;
 
            ctm.AddTransient(
              _cursor, AcGi.TransientDrawingMode.DirectShortTerm,
              128, ints
            );
          }
        }
        else
        {
          if (_vertices.Count > 0)
          {
            Point3d newPt = _vertices[_vertices.Count - 1];
            _cursor.TransformBy(
              Matrix3d.Displacement(newPt - _curPt)
            );
            _curPt = newPt;
 
            ctm.UpdateTransient(_cursor, ints);
          }
        }
      }
      else // !_drawing
      {
        if (_cursor != null)
        {
          ctm.EraseTransient(_cursor, ints);
          _cursor.Dispose();
          _cursor = null;
        }
      }
 
      return true;
    }
 
    public void AddPolylines()
    {
      ClearTransients();
 
      // Dispose of the database objects
 
      foreach (DBObject obj in _lines)
      {
        obj.Dispose();
      }
      _lines.Clear();
 
      // Create a true database-resident 3D polyline
      // (and let it be green)
 
      if (_vertices.Count > 1)
      {
        BlockTableRecord btr =
          (BlockTableRecord)_tr.GetObject(
            _doc.Database.CurrentSpaceId,
            OpenMode.ForWrite
          );
 
        Polyline3d pl =
          new Polyline3d(
            Poly3dType.SimplePoly, _vertices, false
          );
        pl.ColorIndex = 3;
 
        btr.AppendEntity(pl);
        _tr.AddNewlyCreatedDBObject(pl, true);
      }
      _vertices.Clear();
    }
 
    public void ClearTransients()
    {
      AcGi.TransientManager ctm =
        AcGi.TransientManager.CurrentTransientManager;
 
      // Erase the various transient graphics
 
      ctm.EraseTransients(
        AcGi.TransientDrawingMode.DirectShortTerm, 128,
        new IntegerCollection()
      );
    }
 
    public void ExportPointCloud(string filename)
    {
      if (_vecs.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 _vecs)
          {
            sw.WriteLine(
              "{0}, {1}, {2}, {3}, {4}, {5}",
              pt.X, pt.Y, pt.Z, pt.R, pt.G, pt.B
            );
          }
        }
      }
    }
  }
 
  public class Commands
  {
    [CommandMethod("ADNPLUGINS", "KINECT", CommandFlags.Modal)]
    public void ImportFromKinect()
    {
      Document doc =
        Autodesk.AutoCAD.ApplicationServices.
          Application.DocumentManager.MdiActiveDocument;
      Editor ed = doc.Editor;
 
      Transaction tr =
        doc.TransactionManager.StartTransaction();
 
      KinectJig kj = new KinectJig(doc, tr);
      kj.StartSensor();
      PromptResult pr = ed.Drag(kj);
      kj.StopSensor();
 
      if (pr.Status != PromptStatus.OK && !kj.Finished)
      {
        kj.ClearTransients();
        tr.Dispose();
        return;
      }
 
      kj.AddPolylines();
      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)
      );
    }
  }
}