private static void NativeWindow_Render(object sender, NativeWindowEventArgs e)
{
NativeWindow nativeWindow = (NativeWindow)sender;
Gl.Viewport(0, 0, (int)nativeWindow.Width, (int)nativeWindow.Height);
Gl.Clear(ClearBufferMask.ColorBufferBit);
Gl.UseProgram(_CubeEdgeProgram);
// Compute MVP
PerspectiveProjectionMatrix proj = new PerspectiveProjectionMatrix(60.0f, (float)nativeWindow.Width / nativeWindow.Height, 0.5f, 1e6f);
ModelMatrix view = new ModelMatrix();
ModelMatrix model = new ModelMatrix();
model.RotateY(_Angle);
model.RotateZ(_Angle);
view.LookAtTarget(Vertex3f.One * 7.0f, Vertex3f.Zero, Vertex3f.UnitY);
Gl.BindVertexArray(_CubeVao);
Gl.UniformMatrix4(_CubeEdgeProgram_Location_uMVP, 1, false, (proj * view * model).ToArray());
foreach (float scale4d in new float[] { 64.0f, 32.0f, 16.0f, 8.0f, 4.0f, 2.0f, 1.0f, 0.5f, 0.25f, 0.125f })
{
Gl.Uniform1(_CubeEdgeProgram_Location_uScale4D, scale4d * _Zooom);
Gl.Uniform4(_CubeEdgeProgram_Location_uColor, 0.0f, 0.3f, 1.0f, Math.Min(1.0f, scale4d * _Zooom / 2.0f));
Gl.DrawElements(PrimitiveType.Lines, _CubeEdges.Length, DrawElementsType.UnsignedShort, IntPtr.Zero);
}
_Angle += 360.0f / (25.0f * 5);
_Zooom -= 0.025f;
if (_Zooom < 0.5f)
{
_Zooom = 1.0f;
}
// Save PNG frame
if (_FrameNo < 125)
{
using (Bitmap bitmap = new Bitmap((int)nativeWindow.Width, (int)nativeWindow.Height, System.Drawing.Imaging.PixelFormat.Format24bppRgb)) {
BitmapData bitmapData = bitmap.LockBits(new Rectangle(Point.Empty, bitmap.Size), ImageLockMode.WriteOnly, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
Gl.ReadPixels(0, 0, (int)nativeWindow.Width, (int)nativeWindow.Height, OpenGL.PixelFormat.Bgr, PixelType.UnsignedByte, bitmapData.Scan0);
bitmap.Save(String.Format("Frame_{0:D3}.png", _FrameNo++));
}
}
}
// Init Window
private void NativeWindow_ContextCreated(object sender, NativeWindowEventArgs e)
{
OpenGL.CoreUI.NativeWindow nativeWindow = (OpenGL.CoreUI.NativeWindow)sender;
Gl.ReadBuffer(ReadBufferMode.Back);
Gl.ClearColor(0.0f, 0.0f, 0.0f, 1.0f);
Gl.Enable(EnableCap.Blend);
Gl.BlendFunc(BlendingFactor.SrcAlpha, BlendingFactor.OneMinusSrcAlpha);
Gl.Enable(EnableCap.LineSmooth);
Gl.Hint(HintTarget.LineSmoothHint, HintMode.Nicest);
viewer.init(zedCamera.GetCalibrationParameters().leftCam);
}
private static void NativeWindow_ContextCreated(object sender, NativeWindowEventArgs e)
{
NativeWindow nativeWindow = (NativeWindow)sender;
Gl.ReadBuffer(ReadBufferMode.Back);
Gl.ClearColor(1.0f, 1.0f, 1.0f, 1.0f);
Gl.Enable(EnableCap.Blend);
Gl.BlendFunc(BlendingFactor.SrcAlpha, BlendingFactor.OneMinusSrcAlpha);
Gl.LineWidth(2.5f);
CreateResources();
}
private void NativeWindow_ContextCreated(object sender, NativeWindowEventArgs e)
{
lastRenderTime = DateTime.Now.Ticks;
SparrowSharp.NativeWindow = this;
SparrowSharp.Start(_width, _height, _width, _height, _rootClass);
//SparrowSharp.MouseIconChange += OnCursorChange;
_nativeWindow.MouseDown += OnMouseButtonDown;
_nativeWindow.MouseUp += OnMouseButtonUp;
_nativeWindow.MouseMove += OnMouseMove;
_nativeWindow.Render += Control_Render;
_nativeWindow.Resize += OnResize;
}
// Render loop
private void NativeWindow_Render(object sender, NativeWindowEventArgs e)
{
OpenGL.CoreUI.NativeWindow nativeWindow = (OpenGL.CoreUI.NativeWindow)sender;
Gl.Viewport(0, 0, (int)nativeWindow.Width, (int)nativeWindow.Height);
Gl.Clear(ClearBufferMask.ColorBufferBit | ClearBufferMask.DepthBufferBit);
if (viewer.isAvailable() && zedCamera.Grab(ref runtimeParameters) == ERROR_CODE.SUCCESS)
{
if (point_cloud.IsInit())
{
// Retrieve left image
zedCamera.RetrieveMeasure(point_cloud, sl.MEASURE.XYZRGBA, sl.MEM.CPU, res);
zedCamera.GetPosition(ref cam_pose, REFERENCE_FRAME.WORLD);
//Update GL View
viewer.update(point_cloud, cam_pose);
viewer.render();
}
}
}
private void Render(object sender, NativeWindowEventArgs e)
{
if (!initialized)
{
InitServices();
InitManagers();
InitScene();
// Setup complete - broadcast the game started event
Subject.Notify(NotifyType.SceneReady, new GenericNotifyArgs(this));
StartThreads();
initialized = true;
}
Gl.Enable(EnableCap.FramebufferSrgb);
RenderManager.Instance.Run();
Gl.Disable(EnableCap.FramebufferSrgb);
ImGuiManager.Instance.Run();
Gl.Enable(EnableCap.FramebufferSrgb);
}
// Render loop
private void NativeWindow_Render(object sender, NativeWindowEventArgs e)
{
OpenGL.CoreUI.NativeWindow nativeWindow = (OpenGL.CoreUI.NativeWindow)sender;
Gl.Viewport(0, 0, (int)nativeWindow.Width, (int)nativeWindow.Height);
Gl.Clear(ClearBufferMask.ColorBufferBit | ClearBufferMask.DepthBufferBit);
if (viewer.isAvailable() && zedCamera.Grab(ref runtimeParameters) == ERROR_CODE.SUCCESS)
{
trackingState = zedCamera.GetPosition(ref cam_pose, REFERENCE_FRAME.WORLD);
if (trackingState == POSITIONAL_TRACKING_STATE.OK && timer % 30 == 0)
{
Console.WriteLine("Translation : " + cam_pose.translation + ", Rotation : " + cam_pose.rotation);
}
//Update GL View
viewer.updateData(cam_pose);
viewer.render();
timer++;
}
}
private void ContextCreated(object sender, NativeWindowEventArgs e)
{
AttributeId[] attributes = null;
_vao = new VertexArrayObject(true);
{
var texture = new TQTexture(File.ReadAllBytes("texture.tex"));
_texture = new Texture(TextureTarget.Texture2d);
_texture.Parameteri(TextureParameterName.TextureWrapS, TextureWrapMode.Repeat);
_texture.Parameteri(TextureParameterName.TextureWrapT, TextureWrapMode.Repeat);
_texture.Parameteri(TextureParameterName.TextureMinFilter, TextureMinFilter.Linear);
_texture.Parameteri(TextureParameterName.TextureMagFilter, TextureMagFilter.Linear);
foreach (var frame in texture)
{
var dds = new DDS.DDS(frame.Data);
InternalFormat internalFormat;
if (dds.Header.Capabilities.HasFlag(Capabilities.Complex))
{
switch (dds.Header.PixelFormat.FourCC)
{
case "DXT5": internalFormat = InternalFormat.CompressedRgbaS3tcDxt5Ext; break;
default: throw new NotImplementedException();
}
}
else
{
throw new NotImplementedException();
}
_texture.Storage2D((int)dds.Header.MipmapCount, internalFormat, (int)dds.Header.Width, (int)dds.Header.Height);
foreach (var layer in dds)
{
int level = 0;
foreach (var mip in layer)
{
unsafe
{
fixed(byte *ptr = mip.Data)
{
_texture.CompressedSubImage2D(
level++,
internalFormat,
xOffset: 0,
yOffset: 0,
width: (int)Math.Max(1, (dds.Header.Width * 2) >> level),
height: (int)Math.Max(1, (dds.Header.Height * 2) >> level),
data: mip.Data);
Gl.CheckErrors();
}
}
}
break; //TODO: Further layers.
}
break; //TODO: Further frames.
}
}
var mesh = new Mesh.Mesh(File.ReadAllBytes("mesh.msh"));
foreach (var part in mesh)
{
if (part.Is(out VertexBuffer vertexBuffer))
{
Console.Write("Loading VBO...");
var vbo = new Buffer(true);
vbo.BufferData(vertexBuffer.Buffer, BufferUsage.StaticDraw);
_vao.VertexBuffer(0, vbo, IntPtr.Zero, vertexBuffer.Header.Stride);
Console.Write(" OK!");
attributes = vertexBuffer.Attributes.ToArray();
Console.Write($" (also got {attributes.Length} attributes)");
_vertexCount = vertexBuffer.Header.VertexCount;
Console.WriteLine($" (also got vertex count {_vertexCount})");
}
else if (part.Is(out Shaders shaders))
{
foreach (var shader in shaders)
{
{
Console.Write($"Sort of loading {shader.FileName}...");
using (var vertexShader = new Shader(ShaderType.VertexShader, File.ReadAllText($"{shader.FileName}.vertex.glsl")))
using (var fragmentShader = new Shader(ShaderType.FragmentShader, File.ReadAllText($"{shader.FileName}.fragment.glsl")))
{ _program = new ShaderProgram(vertexShader, fragmentShader); }
_program.Link();
Console.WriteLine(" OK!");
}
{
Console.Write($"Setting up attibutes...");
var offset = 0u;
foreach (var attribute in attributes)
{
switch (attribute)
{
case AttributeId.Position: SetUpAttribute(_vao, _program, "position", 3, VertexAttribType.Float, normalized: false, offset, bindingIndex: 0); break;
case AttributeId.Normal: SetUpAttribute(_vao, _program, "normal", 3, VertexAttribType.Float, normalized: true, offset, bindingIndex: 0); break;
case AttributeId.Tangent: SetUpAttribute(_vao, _program, "tangent", 3, VertexAttribType.Float, normalized: true, offset, bindingIndex: 0); break;
case AttributeId.Bitangent: SetUpAttribute(_vao, _program, "bitangent", 3, VertexAttribType.Float, normalized: true, offset, bindingIndex: 0); break;
case AttributeId.UV: SetUpAttribute(_vao, _program, "uv", 2, VertexAttribType.Float, normalized: false, offset, bindingIndex: 0); break;
case AttributeId.Weights: SetUpAttribute(_vao, _program, "weights", 4, VertexAttribType.Float, normalized: false, offset, bindingIndex: 0); break;
case AttributeId.Bones: SetUpAttribute(_vao, _program, "bones", 4, VertexAttribType.Byte, normalized: false, offset, bindingIndex: 0); break;
case AttributeId.Bytes: SetUpAttribute(_vao, _program, "bytes", 4, VertexAttribType.UnsignedByte, normalized: false, offset, bindingIndex: 0); break;
}
offset += GetAttributeSize(attribute);
}
Console.WriteLine(" OK!");
}
{
Console.Write($"Setting up uniforms...");
if (_program.TryGetUniformLocation("transformation", out var transformationUniform))
{
var matrix = new Matrix4x4(
.5f, 0, 0, 0,
0, .5f, 0, 0,
0, 0, .5f, 0,
0, -.7f, 0, 1
);
_program.UniformMatrix4f(transformationUniform, transpose: false, in matrix);
}
else
{
throw new NotImplementedException();
}
Console.WriteLine(" OK!");
}
}
}
else if (part.Is(out IndexBuffer indexBuffer))
{
Console.Write("Loading IBO...");
var ibo = new Buffer(true);
ibo.BufferData(indexBuffer.TriangleIndices, BufferUsage.StaticDraw);
_vao.ElementBuffer(ibo);
Console.WriteLine(" OK!");
var drawRanges = new List <(int, int)>();
foreach (var drawCall in indexBuffer)
{
drawRanges.Add((drawCall.Common.StartFaceIndex, drawCall.Common.FaceCount));
}
_drawRanges = drawRanges.ToArray();
}
else if (part.Is(out Bones bones))
{
_boneVao = new VertexArrayObject(true);
Console.Write("Loading Bones...");
_boneMatrices = new Matrix4x4[bones.Count];
_bonePositions = new Vector3[bones.Count];
for (int i = 0; i < _boneMatrices.Length; i++)
{
_boneMatrices[i] = Matrix4x4.Identity;
}
foreach (var bone in bones)
{
var i = bone.Index;
var position = new Vector4(bone.Position, 1);
var bonePosition = Vector4.Transform(position, _boneMatrices[i]);
_bonePositions[i] = new Vector3(bonePosition.X, bonePosition.Y, bonePosition.Z);
var boneMatrix = new Matrix4x4(
bone.Axes[0], bone.Axes[1], bone.Axes[2], 0,
bone.Axes[3], bone.Axes[4], bone.Axes[5], 0,
bone.Axes[6], bone.Axes[7], bone.Axes[8], 0,
bone.Position.X, bone.Position.Y, bone.Position.Z, 1
);
_boneMatrices[i] = boneMatrix * _boneMatrices[i];
foreach (var childBone in bone)
{
_boneMatrices[childBone.Index] = _boneMatrices[i];
}
}
var boneVbo = new Buffer(true);
Span <Vector3> boneVboData = (from p in _bonePositions select new Vector3(p.X / 2, p.Y / 2 - .7f, p.Z / 2)).ToArray().AsSpan();
_boneVao.VertexBuffer(0, boneVbo, IntPtr.Zero, 3 * sizeof(float));
Gl.CheckErrors();
boneVbo.BufferData(boneVboData, BufferUsage.StaticDraw);
Gl.CheckErrors();
//TODO: LINQ it!
var boneIbo = new Buffer(true);
Gl.CheckErrors();
var boneIboEntries = new List <(int, int)>();
foreach (var parent in bones)
{
foreach (var child in parent)
{
boneIboEntries.Add((parent.Index, child.Index));
}
}
boneIbo.BufferData(boneIboEntries.SelectMany(x => new[] { (ushort)x.Item1, (ushort)x.Item2 }).ToArray().AsSpan(), BufferUsage.StaticDraw);
_boneVao.ElementBuffer(boneIbo);
Gl.CheckErrors();
_boneLinkCount = boneIboEntries.Count;
using (var vertexShader = new Shader(ShaderType.VertexShader, File.ReadAllText("bones.vertex.glsl")))
using (var fragmentShader = new Shader(ShaderType.FragmentShader, File.ReadAllText("bones.fragment.glsl")))
{ _boneProgram = new ShaderProgram(vertexShader, fragmentShader); }
_boneProgram.Link();
Console.WriteLine(" OK!");
SetUpAttribute(_boneVao, _boneProgram, "position", 3, VertexAttribType.Float, normalized: false, relativeOffset: 0, bindingIndex: 0);
Gl.CheckErrors();
}
}
}
private void NativeWindow_Render(object sender, NativeWindowEventArgs e)
{
float head_height;
float feet_y_min;
float delta;
using (var lastFrame = visualizerData.TakeFrameWithOwnership())
{
if (lastFrame == null)
{
return;
}
NativeWindow nativeWindow = (NativeWindow)sender;
Gl.Viewport(0, 0, (int)nativeWindow.Width, (int)nativeWindow.Height);
Gl.Clear(ClearBufferMask.ColorBufferBit);
// Update model/view/projective matrices in shader
var proj = Matrix4x4.CreatePerspectiveFieldOfView(ToRadians(65.0f), (float)nativeWindow.Width / nativeWindow.Height, 0.1f, 150.0f);
var view = Matrix4x4.CreateLookAt(Vector3.Zero, Vector3.UnitZ, -Vector3.UnitY);
SphereRenderer.View = view;
SphereRenderer.Projection = proj;
CylinderRenderer.View = view;
CylinderRenderer.Projection = proj;
//PointCloudRenderer.View = view;
//PointCloudRenderer.Projection = proj;
//PointCloud.ComputePointCloud(lastFrame.Capture.Depth, ref pointCloud);
//PointCloudRenderer.Render(pointCloud, new Vector4(1, 1, 1, 1));
if (lastFrame.NumberOfBodies > 0 && !TrackingAnybody)
{
Console.WriteLine("+++++++++++++++++ TRACKING SOMEBODY ++++++++++++++++");
TrackingAnybody = true;
}
if (lastFrame.NumberOfBodies == 0 && TrackingAnybody)
{
Console.WriteLine("----------------- NOBODY TRACKED -----------------");
TrackingAnybody = false;
}
for (uint i = 0; i < lastFrame.NumberOfBodies; ++i)
{
var skeleton = lastFrame.GetBodySkeleton(i);
var bodyId = lastFrame.GetBodyId(i);
var bodyColor = BodyColors.GetColorAsVector(bodyId);
switch (Status)
{
case STATUS_INIT:
case STATUS_RUNNING:
case STATUS_CALIBRATION_PROCESSING:
break;
case STATUS_CALIBRATION_ACQUIRING:
LeftHand_Kinect_PA.AddPoint(
skeleton.GetJoint(JointId.HandLeft).Position.X,
skeleton.GetJoint(JointId.HandLeft).Position.Y,
skeleton.GetJoint(JointId.HandLeft).Position.Z);
RightHand_Kinect_PA.AddPoint(
skeleton.GetJoint(JointId.HandRight).Position.X,
skeleton.GetJoint(JointId.HandRight).Position.Y,
skeleton.GetJoint(JointId.HandRight).Position.Z);
//LeftHand_Hololens_PA.AddPoint(LH_Hololens_pos.X, LH_Hololens_pos.Y, LH_Hololens_pos.Z);
//RightHand_Hololens_PA.AddPoint(RH_Hololens_pos.X, RH_Hololens_pos.Y, RH_Hololens_pos.Z);
break;
}
if (true)
{
if (true)
{
data_to_send[0] = skeleton.GetJoint(JointId.FootLeft).Position.X;
data_to_send[1] = GetKinectHeight(skeleton.GetJoint(JointId.FootLeft).Position.Y);
data_to_send[2] = skeleton.GetJoint(JointId.FootLeft).Position.Z;
data_to_send[3] = skeleton.GetJoint(JointId.FootRight).Position.X;
data_to_send[4] = GetKinectHeight(skeleton.GetJoint(JointId.FootRight).Position.Y);
data_to_send[5] = skeleton.GetJoint(JointId.FootRight).Position.Z;
}
else
{
data_to_send[0] = skeleton.GetJoint(JointId.HandLeft).Position.X;
data_to_send[1] = skeleton.GetJoint(JointId.HandLeft).Position.Y;
data_to_send[2] = skeleton.GetJoint(JointId.HandLeft).Position.Z;
data_to_send[3] = skeleton.GetJoint(JointId.HandRight).Position.X;
data_to_send[4] = skeleton.GetJoint(JointId.HandRight).Position.Y;
data_to_send[5] = skeleton.GetJoint(JointId.HandRight).Position.Z;
}
}
else
{
//DEBUG
data_to_send[0] = 1.0f;
data_to_send[1] = 2.0f;
data_to_send[2] = 3.0f;
data_to_send[3] = 4.0f;
data_to_send[4] = 5.0f;
data_to_send[5] = 6.0f;
}
head_height = GetKinectHeight(skeleton.GetJoint(JointId.Head).Position.Y);
feet_y_min = min(data_to_send[1], data_to_send[4]);
delta = head_height - feet_y_min;
data_to_send[1] = (data_to_send[1] - feet_y_min) / delta;
data_to_send[4] = (data_to_send[4] - feet_y_min) / delta;
//data_to_send[1] /= head_height;
if (data_to_send[1] > 1.0f)
{
data_to_send[1] = 1.0f;
}
else
{
if (data_to_send[1] < 0)
{
data_to_send[1] = 0;
}
}
//data_to_send[4] /= head_height;
if (data_to_send[4] > 1.0f)
{
data_to_send[4] = 1.0f;
}
else
{
if (data_to_send[4] < 0)
{
data_to_send[4] = 0;
}
}
//unbias data
if (false)
{
if (true)
{
//unbias data subtracting the measures from the head's ones
feet_y_min = min(data_to_send[1], data_to_send[4]);
delta = head_height - feet_y_min;
data_to_send[0] = skeleton.GetJoint(JointId.Head).Position.X - data_to_send[0];
data_to_send[1] = data_to_send[1] - feet_y_min;
data_to_send[2] = skeleton.GetJoint(JointId.Head).Position.Z - data_to_send[2];
data_to_send[3] = skeleton.GetJoint(JointId.Head).Position.X - data_to_send[3];
data_to_send[4] = head_height - data_to_send[4];
data_to_send[5] = skeleton.GetJoint(JointId.Head).Position.Z - data_to_send[5];
if (data_to_send[1] < 0)
{
data_to_send[1] = -data_to_send[1];
}
if (data_to_send[4] < 0)
{
data_to_send[4] = -data_to_send[4];
}
if (normalize_height && head_height > 0)
{
data_to_send[1] /= head_height;
if (data_to_send[1] > 1.0f)
{
data_to_send[1] = 1.0f;
}
else
if (data_to_send[1] < 0)
{
data_to_send[1] = 0;
}
data_to_send[4] /= head_height;
if (data_to_send[4] > 1.0f)
{
data_to_send[4] = 1.0f;
}
else
if (data_to_send[4] < 0)
{
data_to_send[4] = 0;
}
}
}
else
{
//unbias data subtracting the the head's measures from the actual ones
data_to_send[0] -= skeleton.GetJoint(JointId.Head).Position.X;
data_to_send[1] -= skeleton.GetJoint(JointId.Head).Position.Y;
data_to_send[2] -= skeleton.GetJoint(JointId.Head).Position.Z;
data_to_send[3] -= skeleton.GetJoint(JointId.Head).Position.X;
data_to_send[4] -= skeleton.GetJoint(JointId.Head).Position.Y;
data_to_send[5] -= skeleton.GetJoint(JointId.Head).Position.Z;
}
}
if (debug_print_sending_data_UDP)
{
Console.WriteLine("SENDING-> LH: " + data_to_send[0].ToString("0.00") + " " + data_to_send[1].ToString("0.00") + " " + data_to_send[2].ToString("0.00") +
" --- RH: " + data_to_send[3].ToString("0.00") + " " + data_to_send[4].ToString("0.00") + " " + data_to_send[5].ToString("0.00") + " Head height = " + head_height.ToString("0.00"));
}
if (true)
{
SendPositions(data_to_send);
}
for (int jointId = 0; jointId < (int)JointId.Count; ++jointId)
{
if (jointId == (int)JointId.SpineNavel)
{
float f = skeleton.GetJoint(jointId).Position.X;
//SendFloat(f);
}
if (jointId == (int)JointId.HandRight || jointId == (int)JointId.HandLeft ||
jointId == (int)JointId.FootLeft || jointId == (int)JointId.FootRight ||
jointId == (int)JointId.SpineNavel)
{
var joint = skeleton.GetJoint(jointId);
// Render the joint as a sphere.
const float radius = 0.012f;
SphereRenderer.Render(joint.Position / 1000, radius, bodyColor);
if (false)
{
if (JointConnections.JointParent.TryGetValue((JointId)jointId, out JointId parentId))
{
// Render a bone connecting this joint and its parent as a cylinder.
CylinderRenderer.Render(joint.Position / 1000, skeleton.GetJoint((int)parentId).Position / 1000, bodyColor);
}
}
}
}
}
}
}
private void ContextDestroyed(object sender, NativeWindowEventArgs e)
{
isRunning = false;
}
private void Render(object sender, NativeWindowEventArgs e)
{
RenderToShadowMap();
RenderToScreen();
// imgui.Render();
}
// Render loop
private void NativeWindow_Render(object sender, NativeWindowEventArgs e)
{
OpenGL.CoreUI.NativeWindow nativeWindow = (OpenGL.CoreUI.NativeWindow)sender;
Gl.Viewport(0, 0, (int)nativeWindow.Width, (int)nativeWindow.Height);
Gl.Clear(ClearBufferMask.ColorBufferBit);
ERROR_CODE err = ERROR_CODE.FAILURE;
if (viewer.isAvailable() && zedCamera.Grab(ref runtimeParameters) == ERROR_CODE.SUCCESS)
{
if (zedMat.IsInit())
{
// Retrieve left image
err = zedCamera.RetrieveImage(zedMat, sl.VIEW.LEFT, sl.MEM.CPU);
// Update pose data (used for projection of the mesh over the current image)
tracking_state = zedCamera.GetPosition(ref cam_pose);
if (mapping_activated)
{
mapping_state = zedCamera.GetSpatialMappingState();
if (timer % 60 == 0 && viewer.chunksUpdated() == true)
{
zedCamera.RequestSpatialMap();
}
if (zedCamera.GetMeshRequestStatus() == ERROR_CODE.SUCCESS && timer > 0)
{
/// MAP_TYPE == MESH
if (CREATE_MESH)
{
//Retrieves data for mesh visualization only (vertices + triangles);
zedCamera.RetrieveChunks(ref mesh);
var chunks = new List <Chunk>(mesh.chunks.Values);
viewer.updateData(chunks);
}
/// MAP_TYPE == FUSED_POINT_CLOUD
else
{
zedCamera.RetrieveSpatialMap(ref fusedPointCloud);
viewer.updateData(fusedPointCloud.vertices);
}
}
}
bool change_state = viewer.updateImageAndState(zedMat, cam_pose, tracking_state, mapping_state);
if (change_state)
{
if (!mapping_activated)
{
Quaternion quat = Quaternion.Identity; Vector3 vec = Vector3.Zero;
zedCamera.ResetPositionalTracking(quat, vec);
zedCamera.EnableSpatialMapping(ref spatialMappingParameters);
Console.WriteLine("Hit SPACE BAR to stop spatial mapping...");
// Clear previous Mesh data
viewer.clearCurrentMesh();
mapping_activated = true;
}
else
{
// Filter the mesh (remove unnecessary vertices and faces)
if (CREATE_MESH)
{
zedCamera.ExtractWholeSpatialMap();
zedCamera.FilterMesh(MESH_FILTER.MEDIUM, ref mesh);
}
if (CREATE_MESH && spatialMappingParameters.saveTexture)
{
zedCamera.ApplyTexture(ref mesh);
}
bool error_save = false;
string saveName = "";
//Save mesh as obj file
if (CREATE_MESH)
{
saveName = "mesh_gen.obj";
error_save = zedCamera.SaveMesh(saveName, MESH_FILE_FORMAT.OBJ);
}
else
{
saveName = "point_cloud.ply";
error_save = zedCamera.SavePointCloud(saveName, MESH_FILE_FORMAT.PLY);
}
if (error_save)
{
Console.WriteLine("Mesh saved under: " + saveName);
}
else
{
Console.WriteLine("Failed to save the mesh under: " + saveName);
}
mapping_state = SPATIAL_MAPPING_STATE.NOT_ENABLED;
mapping_activated = false;
zedCamera.DisableSpatialMapping();
Console.WriteLine("Hit SPACE BAR to start spatial mapping...");
}
}
timer++;
viewer.render();
}
}
}
private void NativeWindow_Render(object sender, NativeWindowEventArgs e)
{
using (var lastFrame = visualizerData.TakeFrameWithOwnership())
{
if (lastFrame == null)
{
return;
}
NativeWindow nativeWindow = (NativeWindow)sender;
Gl.Viewport(0, 0, (int)nativeWindow.Width, (int)nativeWindow.Height);
Gl.Clear(ClearBufferMask.ColorBufferBit);
// Update model/view/projective matrices in shader
var proj = Matrix4x4.CreatePerspectiveFieldOfView(ToRadians(65.0f), (float)nativeWindow.Width / nativeWindow.Height, 0.1f, 150.0f);
var view = Matrix4x4.CreateLookAt(Vector3.Zero, Vector3.UnitZ, -Vector3.UnitY);
SphereRenderer.View = view;
SphereRenderer.Projection = proj;
CylinderRenderer.View = view;
CylinderRenderer.Projection = proj;
PointCloudRenderer.View = view;
PointCloudRenderer.Projection = proj;
PointCloud.ComputePointCloud(lastFrame.Capture.Depth, ref pointCloud);
PointCloudRenderer.Render(pointCloud, new Vector4(1, 1, 1, 1));
Vector3 rightHand = Vector3.Zero;
for (uint i = 0; i < lastFrame.NumberOfBodies; ++i)
{
var skeleton = lastFrame.GetBodySkeleton(i);
var bodyId = lastFrame.GetBodyId(i);
var bodyColor = BodyColors.GetColorAsVector(bodyId);
for (int jointId = 0; jointId < (int)JointId.Count; ++jointId)
{
var joint = skeleton.GetJoint(jointId);
if (jointId == (int)JointId.HandRight)
{
if (pullPoint)
{
if (rightHand == Vector3.Zero)
{
rightHand = joint.Position;
}
else
{
if (Math.Abs(Vector3.Distance(rightHand, Vector3.Zero)) > Math.Abs(Vector3.Distance(joint.Position, Vector3.Zero)))
{
rightHand = joint.Position;
}
}
}
// Console.WriteLine($"Right Hand Found X:{joint.Position.X} Y:{joint.Position.Y} Z:{joint.Position.Z} ");
}
// Render the joint as a sphere.
const float radius = 0.024f;
SphereRenderer.Render(joint.Position / 1000, radius, bodyColor);
if (JointConnections.JointParent.TryGetValue((JointId)jointId, out JointId parentId))
{
// Render a bone connecting this joint and its parent as a cylinder.
CylinderRenderer.Render(joint.Position / 1000, skeleton.GetJoint((int)parentId).Position / 1000, bodyColor);
}
}
}
if (rightHand != Vector3.Zero)
{
thePoint = rightHand;
}
}
}
// Render loop
private void NativeWindow_Render(object sender, NativeWindowEventArgs e)
{
OpenGL.CoreUI.NativeWindow nativeWindow = (OpenGL.CoreUI.NativeWindow)sender;
Gl.Viewport(0, 0, (int)nativeWindow.Width, (int)nativeWindow.Height);
Gl.Clear(ClearBufferMask.ColorBufferBit | ClearBufferMask.DepthBufferBit);
ERROR_CODE err = ERROR_CODE.FAILURE;
if (viewer.isAvailable() && zedCamera.Grab(ref runtimeParameters) == ERROR_CODE.SUCCESS)
{
foreach (var it in obj_runtime_parameters.objectClassFilter)
{
obj_runtime_parameters.objectConfidenceThreshold[it] = detection_confidence;
}
// Retrieve Objects
err = zedCamera.RetrieveObjects(ref objects, ref obj_runtime_parameters);
if (err == ERROR_CODE.SUCCESS && objects.isNew != 0)
{
// Retrieve left image
zedCamera.RetrieveMeasure(pointCloud, MEASURE.XYZRGBA, MEM.CPU, pcRes);
zedCamera.GetPosition(ref camWorldPose, REFERENCE_FRAME.WORLD);
zedCamera.GetPosition(ref camCameraPose, REFERENCE_FRAME.CAMERA);
zedCamera.RetrieveImage(imageLeft, VIEW.LEFT, MEM.CPU, displayRes);
bool update_render_view = true;
bool update_3d_view = true;
bool update_tracking_view = true;
int nbBatches = 0;
if (USE_BATCHING)
{
List <ObjectsBatch> objectsBatch = new List <ObjectsBatch>();
zedCamera.UpdateObjectsBatch(out nbBatches);
for (int i = 0; i < nbBatches; i++)
{
ObjectsBatch obj_batch = new ObjectsBatch();
zedCamera.GetObjectsBatch(i, ref obj_batch);
objectsBatch.Add(obj_batch);
}
batchHandler.push(camCameraPose, camWorldPose, imageLeft, pointCloud, ref objectsBatch);
batchHandler.pop(ref camCameraPose, ref camWorldPose, ref imageLeft, ref pointCloud, ref objects);
update_render_view = BatchSystemHandler.WITH_IMAGE_RETENTION ? Convert.ToBoolean(objects.isNew) : true;
update_3d_view = BatchSystemHandler.WITH_IMAGE_RETENTION ? Convert.ToBoolean(objects.isNew) : true;
}
if (update_render_view)
{
imageRenderLeft.CopyTo(imageLeftOcv);
TrackingViewer.render_2D(ref imageLeftOcv, imgScale, ref objects, true, isTrackingON);
}
if (update_3d_view)
{
//Update GL View
viewer.update(pointCloud, objects, camWorldPose);
viewer.render();
}
if (update_tracking_view)
{
trackViewGenerator.generate_view(ref objects, camCameraPose, ref imageTrackOcv, Convert.ToBoolean(objects.isTracked));
}
}
if (isPlayback && zedCamera.GetSVOPosition() == zedCamera.GetSVONumberOfFrames())
{
return;
}
Cv2.ImShow(window_name, globalImage);
}
}
private void NativeWindowOnMouseLeave(object sender, NativeWindowEventArgs e)
{
ignoreSingleMouseDelta = true;
}
public void NativeWindow_Render(object sender, NativeWindowEventArgs e)
{
using (var lastFrame = visualizerData.TakeFrameWithOwnership())
{
if (lastFrame == null)
{
return;
}
NativeWindow nativeWindow = (NativeWindow)sender;
// Gl.Viewport(0, 0, (int)nativeWindow.Width, (int)nativeWindow.Height);
// Gl.Clear(ClearBufferMask.ColorBufferBit);
// Update model/view/projective matrices in shader
// var proj = Matrix4x4.CreatePerspectiveFieldOfView(ToRadians(65.0f), (float)nativeWindow.Width / nativeWindow.Height, 0.1f, 150.0f);
// var view = Matrix4x4.CreateLookAt(Vector3.Zero, Vector3.UnitZ, -Vector3.UnitY);
if (lastFrame.NumberOfBodies > 0)
{
// SphereRenderer.View = view;
// SphereRenderer.Projection = proj;
// CylinderRenderer.View = view;
// CylinderRenderer.Projection = proj;
// PointCloudRenderer.View = view;
// PointCloudRenderer.Projection = proj;
// PointCloud.ComputePointCloud(lastFrame.Capture.Depth, ref pointCloud);
// PointCloudRenderer.Render(pointCloud, new Vector4(1, 1, 1, 1));
if (!IsHuman)
{
this.day = DateTime.Now.ToString("yyyyMMdd");
this.scene = DateTime.Now.ToString("HHmmssfff");
string path = $@"C:\Users\gekka\temp\{this.day}\{this.scene}\depth";
Directory.CreateDirectory(path);
IsHuman = true;
}
for (uint i = 0; i < lastFrame.NumberOfBodies; ++i)
{
// System.Diagnostics.Debug.WriteLine(i);
DirectoryUtils.SafeCreateDirectory($@"C:\Users\gekka\temp\{this.day}\{this.scene}\{i}");
string filename = $@"C:\Users\gekka\temp\{this.day}\{this.scene}\{i}\pos.csv";
var append = true;
var skeleton = lastFrame.GetBodySkeleton(i);
var bodyId = lastFrame.GetBodyId(i);
var bodyColor = BodyColors.GetColorAsVector(bodyId);
using (var sw = new System.IO.StreamWriter(filename, append))
{
this.now = DateTime.Now.ToString("HHmmssfff");
sw.Write("{0}, ", this.now);
for (int jointId = 0; jointId < (int)JointId.Count; ++jointId)
{
var joint = skeleton.GetJoint(jointId);
sw.Write("{0}, {1}, {2},", joint.Position.X, joint.Position.Y, joint.Position.Z);
// GUI描画
// Render the joint as a sphere.
// const float radius = 0.024f;
// SphereRenderer.Render(joint.Position / 1000, radius, bodyColor);
// if (JointConnections.JointParent.TryGetValue((JointId)jointId, out JointId parentId))
// {
// //Render a bone connecting this joint and its parent as a cylinder.
// CylinderRenderer.Render(joint.Position / 1000, skeleton.GetJoint((int)parentId).Position / 1000, bodyColor);
// }
}
sw.Write("\r\n");
}
}
}
else
{
IsHuman = false;
}
}
}
