Kean Walmsley


  • About the Author
    Kean on Google+

April 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      







« P/Invoke tools | Main | Calling unexposed ObjectARX methods using P/Invoke from .NET »

April 28, 2011

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)

      );

    }

  }

}

I’m yet to look seriously at the navigation side of things, but it might be fun to create some additional 3D geometry types, first.

blog comments powered by Disqus

10 Random Posts