public void flushFrames(IInputAdapter adapter, MocapFrame frame)
{
if (this._config.SkeletonStreamFlush)
{
this.frameList.Add(frame);
if (this.frameList.Count > this._config.SkeletonStreamFlushSize)
{
// freeze to serialize
List <MocapFrame> serializeList = new List <MocapFrame>(frameList);
// Serialize
string flushPath = Path.Combine(this.SkeletonStreamPath, (this.skeletonFlushCount.ToString() + ".json"));
try
{
using (StreamWriter sw = new StreamWriter(flushPath))
using (JsonWriter writer = new JsonTextWriter(sw))
{
serializer.Serialize(writer, serializeList);
}
} catch (Exception e)
{
Logger.Error("Error occurred during flushing frames: {0}", e.ToString());
}
this.skeletonFlushCount += 1;
// Clear the List
this.frameList.Clear();
}
}
}
private void ParseMocapFrame(MocapFrame mocapFrame)
{
if (mocapFrame != null)
{
foreach (var body in mocapFrame.Bodies)
{
if (mocapFrame.SourceId.Equals("optitrack"))
{
var name = body.Name;
if (name.Equals("MaxGertz_GO_3"))
{
var x = body.Centroid.X;
var y = body.Centroid.Y;
var z = body.Centroid.Z;
var rot = new Quaternion(-body.Rotation.X, body.Rotation.Y, body.Rotation.Z,
-body.Rotation.W);
if (player.IsTeleported)
{
// y difference of the two worlds (in mm)
y += _yOffset;
}
var pos = new Vector3(z * roomscale, y * roomscale + 0.1f, x * roomscale);
_playerPosition = pos;
}
}
}
}
}
public override object ReadJson(JsonReader reader, Type objectType, object existingValue, JsonSerializer serializer)
{
JObject jObject = JObject.Load(reader);
JObject jo = jObject.GetValue("eventProperties")["MocapFrame"].ToObject <JObject>();
string sourceId = (string)jo["SourceId"];
string adapter = (string)jo["AdapterType"];
serializer.Converters.Add(new BodyTypeConverter());
serializer.Converters.Add(new HandTypeConverter());
IList <Body> bodies = new List <Body>();
foreach (var child in jo["Bodies"])
{
Body body = adapter == "leapmotion" ? child.ToObject <Model.LeapMotion.Hand>(serializer) : child.ToObject <Body>(serializer);
bodies.Add(body);
}
MocapFrame mocapFrame = new MocapFrame(sourceId, adapter, bodies)
{
ElapsedMillis = (long)jo["ElapsedMillis"],
Gesture = (string)jo["Gesture"]
};
return(mocapFrame);
}
private void ExtractSkeletons(FrameOfMocapData data, ref MocapFrame handledFrame)
{
for (var i = 0; i < data.nSkeletons; i++)
{
var sklData = data.Skeletons[i]; // Received skeleton frame data
var skl = new Skeleton(sklData.ID.ToString())
{
BodyType = Body.EBodyType.Skeleton
};
/* Now, for each of the skeleton segments */
for (var j = 0; j < sklData.nRigidBodies; j++)
{
var boneData = sklData.RigidBodies[j];
var bone = new OptiTrackBody(boneData.ID.ToString())
{
Centroid = new Vector3(boneData.x, boneData.y, boneData.z) * TranslationUnitMultiplier,
Rotation = new Quaternion(boneData.qx, boneData.qy, boneData.qz, boneData.qw)
};
skl.RigidBodies.Add(bone); // Add bone to skeleton
}
handledFrame.Bodies.Add(skl);
}
}
public void TestMergeOptiTrackBody()
{
var merger = _ninjectKernel.Get <IDataMerger>(HardwareSystems.OptiTrack);
var optiTrackBody = new OptiTrackBody("testbody")
{
Points = new List <Point>()
{
new Point(Vector3.One)
},
BodyType = Body.EBodyType.Marker
};
var frame = new MocapFrame("optitrack", HardwareSystems.OptiTrack)
{
Bodies = new List <Body>()
{
optiTrackBody
}
};
frame = merger.HandleFrame(frame);
var position = frame.Bodies[0].Points[0].Position;
Assert.AreEqual(Body.EBodyType.Marker, frame.Bodies[0].BodyType);
Assert.AreEqual(Vector3.One, position);
}
public void OnDataReceived(MocapFrame mocapFrame)
{
if (this._enabled)
{
// logging the mocapFrame into JsonFile.
_logger?.Trace("mocapFrame {MocapFrame}", mocapFrame);
}
}
/*
* Method that is extracting Rigidbodies and Skeletons from FrameOfMocapData
*/
public void ExtractBodies(FrameOfMocapData data, ref MocapFrame handledFrame)
{
/* save all marker sets in local dict to add all markers to
* their corresponding rigid bodies or skeletons
*/
var markerSets = ExtractMarkerSets(data);
ExtractRigidBodies(data, markerSets, ref handledFrame);
ExtractSkeletons(data, ref handledFrame);
}
//Method to handle frame events
public void ClientFrameReady(FrameOfMocapData data)
{
/* Write values into the handledFrame object */
var handledFrame =
new MocapFrame(_adapter.Config.Name, _adapter.Config.AdapterType)
{
Latency = ExtractLatency(data)
};
ExtractBodies(data, ref handledFrame);
_adapter.OnFrameAvailable(handledFrame);
}
public void SensorSkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
{
Skeleton[] skeletons = new Skeleton[0];
using (SkeletonFrame skeletonFrame = e.OpenSkeletonFrame())
{
if (skeletonFrame != null)
{
skeletons = new Skeleton[skeletonFrame.SkeletonArrayLength];
skeletonFrame.CopySkeletonDataTo(skeletons); // Copy skeelton data to the array
List <VinteR.Model.Point> jointList = new List <VinteR.Model.Point>();
MocapFrame frame = new MocapFrame(adapter.Config.Name, adapter.Config.AdapterType);
// loop through all skeltons
foreach (Skeleton skeleton in skeletons)
{
if (this._config.SkeletonTrackingStateFilter)
{
if (!(skeleton.TrackingState == SkeletonTrackingState.Tracked)) // if the skeleton is not tracked skip
{
continue;
}
}
foreach (Microsoft.Kinect.Joint joint in skeleton.Joints)
{
// Create a Point
Vector3 position = new Vector3(joint.Position.X, joint.Position.Y, joint.Position.Z) * MillimetersMultiplier;
VinteR.Model.Point currentPointModel = new VinteR.Model.Point(position);
currentPointModel.Name = joint.JointType.ToString();
currentPointModel.State = joint.TrackingState.ToString();
// Add the Point to the List of all captured skeleton points
jointList.Add(currentPointModel);
}
// Create and append the frame
Body body = new KinectBody(jointList, Body.EBodyType.Skeleton);
body.Name = skeleton.TrackingId.ToString();
frame.AddBody(ref body);
}
this.adapter.OnFrameAvailable(frame); // publish MocapFrame
}
}
}
public MocapFrame HandleFrame(MocapFrame frame)
{
foreach (var body in frame.Bodies)
{
if (body is Hand hand)
{
var mergedHand = Merge(hand, frame.SourceId);
body.Load(mergedHand);
}
else
{
Logger.Warn("Could not merge frame for {0,15} by type {1}", frame.SourceId, frame.AdapterType);
}
}
return(frame);
}
private IList <MocapFrame> GetSessionMocapFrames(Session session, uint startTimestamp = 0, int endTimestamp = -1)
{
IList <MocapFrame> mocapFrames = new List <MocapFrame>();
var file = Path.Combine(_homeDir, "LoggingData", session.Name + ".json");
try
{
using (var reader = new StreamReader(file))
{
JsonTextReader jsonReader = new JsonTextReader(reader)
{
SupportMultipleContent = true
};
JsonSerializer serializer = new JsonSerializer();
if (endTimestamp == -1)
{
endTimestamp = (int)session.Duration;
}
while (jsonReader.Read())
{
var obj = JObject.Load(jsonReader);
TimeSpan timeStamp = obj["time"].ToObject <DateTime>().Subtract(session.Datetime).Duration();
int timeStampInMillise = (int)timeStamp.TotalMilliseconds;
if (timeStampInMillise >= startTimestamp && timeStampInMillise <= endTimestamp)
{
MocapFrame mocapFrame = JsonConvert.DeserializeObject <MocapFrame>(obj.ToString(), new MocapFrameConverter());
mocapFrames.Add(mocapFrame);
}
}
return(mocapFrames);
}
}
catch (System.IO.FileNotFoundException e)
{
Logger.Error("can not load file {0}, there is no data but only a index for this session", e.FileName);
}
return(mocapFrames);
}
public void OnDataReceived(MocapFrame mocapFrame)
{
if (this.Enabled && this.Write)
{
Logger.Debug("Data Received for MongoDB");
// Buffer Frames
if (this._buffer.Count <= this._bufferSize)
{
this._buffer.Add(mocapFrame);
Logger.Debug("MongoDB Frame appended to List");
}
else
{
writeToDatabase();
}
}
}
public void TestMergeLeapMotionBody()
{
var merger = _ninjectKernel.Get <IDataMerger>(HardwareSystems.LeapMotion);
var frame = new MocapFrame("leapmotion", HardwareSystems.LeapMotion)
{
Bodies = new List <Body> {
Mock.MockHand(new Vector3(-20, 300, 0), false)
}
};
frame = merger.HandleFrame(frame);
var position = frame.Bodies[0].Points[0].Position;
position = position.Round();
Assert.AreEqual(Body.EBodyType.Hand, frame.Bodies[0].BodyType);
Assert.AreEqual(new Vector3(1200, 1050, 480), position);
}
public void Send(MocapFrame mocapFrame)
{
_serializer.ToProtoBuf(mocapFrame, out var frame);
var data = frame.ToByteArray();
foreach (var ipEndPoint in _endPoints)
{
Task.Factory.StartNew(() =>
{
Logger.Debug("Sending to {0}:{1}", ipEndPoint.Address, ipEndPoint.Port);
try
{
_udpServer.Send(data, data.Length, ipEndPoint);
}
catch (Exception)
{
Logger.Warn("Could not send frame {0,8} to {1}", mocapFrame.ElapsedMillis, ipEndPoint.Address);
}
});
}
}
public void TestMergeKinectBody()
{
var merger = _ninjectKernel.Get <IDataMerger>(HardwareSystems.Kinect);
var kinectBody = new KinectBody(new List <Point> {
new Point(0f, 1000f, 1500f)
}, Body.EBodyType.Skeleton);
var frame = new MocapFrame("kinect", HardwareSystems.Kinect)
{
Bodies = new List <Body>()
{
kinectBody
}
};
frame = merger.HandleFrame(frame);
var position = frame.Bodies[0].Points[0].Position;
position = position.Round();
Assert.AreEqual(Body.EBodyType.Skeleton, frame.Bodies[0].BodyType);
Assert.AreEqual(new Vector3(-500, 1080, 1500), position);
}
private void HandleFrameAvailable(IInputAdapter source, MocapFrame frame)
{
/* frame available occurs inside adapter thread
* so synchronize access to the stopwatch
*/
lock (StopwatchLock)
{
frame.ElapsedMillis = _applicationWatch.ElapsedMilliseconds;
Session.Duration = Convert.ToUInt32(_applicationWatch.ElapsedMilliseconds);
}
/* get a data merger specific to the type of input adapter,
* so only a optitrack merger gets frames from an optitrack
* input adapter and so forth.
*/
var merger = _dataMergers.First(m => m.MergerType == source.AdapterType);
Logger.Debug("{Frame #{0} available from {1}", frame.ElapsedMillis, source.Config.AdapterType);
var mergedFrame = merger.HandleFrame(frame);
//get the output from datamerger to output manager
_outputManager.ReadyToOutput(mergedFrame);
FrameAvailable?.Invoke(frame);
}
private void FireFrameAvailable(MocapFrame frame)
{
FrameAvailable?.Invoke(frame);
}
private void ExtractRigidBodies(FrameOfMocapData data, IDictionary <string, OptiTrackBody> markerSets, ref MocapFrame handledFrame)
{
for (var i = 0; i < data.nRigidBodies; i++)
{
var rbData = data.RigidBodies[i]; // Received rigid body descriptions
if (!rbData.Tracked)
{
continue;
}
// add the rigid body with centroid and rotation
var rb = new OptiTrackBody(rbData.ID.ToString())
{
Centroid = new Vector3(rbData.x, rbData.y, rbData.z) * TranslationUnitMultiplier,
Rotation = new Quaternion(rbData.qx, rbData.qy, rbData.qz, rbData.qw),
BodyType = Body.EBodyType.RigidBody
};
// in addition add each marker as point to the body
var name = _client.NameById(rbData.ID);
if (markerSets.TryGetValue(name, out var ms))
{
rb.Name = name;
foreach (var point in ms.Points)
{
rb.Points.Add(point);
}
markerSets.Remove(name);
}
handledFrame.Bodies.Add(rb); // Add to MocapFrame list of bodies
}
}
/*
* Passive called by Datamerger or Streamingmanager.
* Consider the identical output Forma from datamerger, is here an 1 to N broadcast by rasing event.
* All outputadapter receive same source to out put preparing.
*/
public void ReadyToOutput(MocapFrame mocapFrame)
{
OutputNotification?.Invoke(mocapFrame);
}
public void OnDataReceived(MocapFrame mocapFrame)
{
// Logger.Info(mocapFrame.ToString);
}
