October 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  










« 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

Feed/Share

10 Random Posts