/// <summary>
/// pop : pop the data from the FIFO system
/// </summary>
/// <param name="localPose"></param>
/// <param name="worldPose"></param>
/// <param name="image_"></param>
/// <param name="depth_"></param>
/// <param name="objects"></param>
public void pop(ref sl.Pose localPose, ref sl.Pose worldPose, ref sl.Mat image_, ref sl.Mat depth_, ref sl.Objects objects_)
{
objects_ = new Objects();
if (objectsTrackedQueue.Count > 0)
{
sl.Objects trackedMergedObj = objectsTrackedQueue[0];
if (initQueueTS == 0)
{
initQueueTS = trackedMergedObj.timestamp;
}
ulong targetTS_ms = Utils.getMilliseconds(trackedMergedObj.timestamp);
localPose = findClosestLocalPoseFromTS(targetTS_ms);
worldPose = findClosestWorldPoseFromTS(targetTS_ms);
if (WITH_IMAGE_RETENTION)
{
image_.SetFrom(findClosestImageFromTS(targetTS_ms));
imageMap_ms[targetTS_ms].Free();
imageMap_ms.Remove(targetTS_ms);
depth_.SetFrom(findClosestDepthFromTS(targetTS_ms));
depthMap_ms[targetTS_ms].Free();
depthMap_ms.Remove(targetTS_ms);
}
objects_ = trackedMergedObj;
objectsTrackedQueue.RemoveAt(0);
}
}
/// <summary>
/// Creates an OpenCV version of a ZED Mat.
/// </summary>
/// <param name="zedmat">Source ZED Mat.</param>
/// <param name="zedmattype">Type of ZED Mat - data type and channel number.
/// <returns></returns>
public static OpenCvSharp.Mat SLMat2CVMat(ref sl.Mat zedmat, MAT_TYPE zedmattype)
{
int cvmattype = SLMatType2CVMatType(zedmattype);
OpenCvSharp.Mat cvmat = new OpenCvSharp.Mat(zedmat.GetHeight(), zedmat.GetWidth(), cvmattype, zedmat.GetPtr());
return(cvmat);
}
/// <summary>
/// push: push data in the FIFO system
/// </summary>
/// <param name="localPose"></param>
/// <param name="worldPose"></param>
/// <param name="image_"></param>
/// <param name="pc_"></param>
/// <param name="objectsBatch"></param>
public void push(sl.Pose localPose_, sl.Pose worldPose_, sl.Mat image_, sl.Mat pc_, ref List <ObjectsBatch> batch_)
{
ingestWorldPoseInMap(worldPose_);
ingestLocalPoseInMap(localPose_);
if (WITH_IMAGE_RETENTION)
{
ingestImageInMap(worldPose_.timestamp, ref image_);
ingestDepthInMap(worldPose_.timestamp, ref pc_);
}
ingestInObjectsQueue(ref batch_);
}
sl.Mat findClosestDepthFromTS(ulong ts) // in ms
{
sl.Mat depth = new sl.Mat();
ulong ts_found = 0;
if (depthMap_ms.ContainsKey(ts))
{
ts_found = ts;
depth = depthMap_ms[ts];
}
return(depth);
}
sl.Mat findClosestImageFromTS(ulong ts) // in ms
{
sl.Mat image = new sl.Mat();
ulong ts_found = 0;
if (imageMap_ms.ContainsKey(ts))
{
ts_found = ts;
image = imageMap_ms[ts];
}
return(image);
}
void ingestDepthInMap(ulong ts, ref sl.Mat depth)
{
sl.Mat newDepth = new Mat();
newDepth.Create(depth.GetResolution(), MAT_TYPE.MAT_32F_C4);
depth.CopyTo(newDepth);
depthMap_ms[Utils.getMilliseconds(ts)] = newDepth;
List <ulong> removals = new List <ulong>();
foreach (ulong key in depthMap_ms.Keys)
{
if (key < Utils.getMilliseconds(ts) - (ulong)batchDataRetention * 1000 * 2)
{
removals.Add(key);
}
}
foreach (ulong key in removals)
{
depthMap_ms[key].Free();
depthMap_ms.Remove(key);
}
}
void ingestImageInMap(ulong ts, ref sl.Mat image)
{
sl.Mat newImage = new Mat();
newImage.Create(image.GetResolution(), MAT_TYPE.MAT_8U_C4);
image.CopyTo(newImage);
imageMap_ms[Utils.getMilliseconds(ts)] = newImage;
List <ulong> removals = new List <ulong>();
foreach (ulong key in imageMap_ms.Keys)
{
if (key < Utils.getMilliseconds(ts) - (ulong)batchDataRetention * 1000 * 2)
{
removals.Add(key);
}
}
foreach (ulong key in removals)
{
imageMap_ms[key].Free();
imageMap_ms.Remove(key);
}
}
public MainWindow(string[] args)
{
// Set configuration parameters
InitParameters init_params = new InitParameters();
init_params.resolution = RESOLUTION.HD1080;
init_params.cameraFPS = 30;
init_params.depthMode = DEPTH_MODE.ULTRA;
init_params.coordinateUnits = UNIT.METER;
init_params.coordinateSystem = COORDINATE_SYSTEM.RIGHT_HANDED_Y_UP;
parseArgs(args, ref init_params);
// Open the camera
zedCamera = new Camera(0);
ERROR_CODE err = zedCamera.Open(ref init_params);
if (err != ERROR_CODE.SUCCESS)
{
Environment.Exit(-1);
}
if (!(zedCamera.CameraModel == sl.MODEL.ZED2 || zedCamera.CameraModel == sl.MODEL.ZED2i))
{
Console.WriteLine(" ERROR : Use ZED2/ZED2i Camera only");
return;
}
// Enable tracking (mandatory for object detection)
PositionalTrackingParameters positionalTrackingParameters = new PositionalTrackingParameters();
zedCamera.EnablePositionalTracking(ref positionalTrackingParameters);
runtimeParameters = new RuntimeParameters();
// Enable the Objects detection module
ObjectDetectionParameters obj_det_params = new ObjectDetectionParameters();
obj_det_params.enableObjectTracking = true; // the object detection will track objects across multiple images, instead of an image-by-image basis
isTrackingON = obj_det_params.enableObjectTracking;
obj_det_params.enable2DMask = false;
obj_det_params.enableBodyFitting = true; // smooth skeletons moves
obj_det_params.imageSync = true; // the object detection is synchronized to the image
obj_det_params.detectionModel = sl.DETECTION_MODEL.HUMAN_BODY_ACCURATE;
zedCamera.EnableObjectDetection(ref obj_det_params);
// Create ZED Objects filled in the main loop
camPose = new sl.Pose();
objects = new Objects();
int Height = zedCamera.ImageHeight;
int Width = zedCamera.ImageWidth;
imageLeft = new Mat();
displayRes = new Resolution(Math.Min((uint)Width, 1280), Math.Min((uint)Height, 720));
imgScale = new sl.float2((int)displayRes.width / (float)Width, (int)displayRes.height / (float)Height);
imageLeft.Create(displayRes, MAT_TYPE.MAT_8U_C4, MEM.CPU);
imageLeftOcv = new OpenCvSharp.Mat((int)displayRes.height, (int)displayRes.width, OpenCvSharp.MatType.CV_8UC4, imageLeft.GetPtr());
pointCloud = new sl.Mat();
pcRes = new Resolution(Math.Min((uint)Width, 720), Math.Min((uint)Height, 404));
pointCloud.Create(pcRes, MAT_TYPE.MAT_32F_C4, MEM.CPU);
// Create OpenGL Viewer
viewer = new GLViewer(new Resolution((uint)Width, (uint)Height));
// Configure object detection runtime parameters
obj_runtime_parameters = new ObjectDetectionRuntimeParameters();
obj_runtime_parameters.detectionConfidenceThreshold = 40;
window_name = "ZED| 2D View";
Cv2.NamedWindow(window_name, WindowMode.Normal);// Create Window
// Create OpenGL window
CreateWindow();
}
/// <summary>
/// Copies data from this Mat to another Mat (deep copy).
/// </summary>
/// <param name="dest">Mat that the data will be copied to.</param>
/// <param name="copyType">The To and From memory types.</param>
/// <returns>Error code indicating if the copy was successful, or why it wasn't.</returns>
public sl.ERROR_CODE CopyTo(sl.Mat dest, sl.COPY_TYPE copyType = COPY_TYPE.CPU_CPU)
{
return((sl.ERROR_CODE)dllz_mat_copy_to(_matInternalPtr, dest._matInternalPtr, (int)(copyType)));
}
