/// <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);
}
}
void ingestInObjectsQueue(ref List <sl.ObjectsBatch> batch_)
{
if (batch_.Count == 0)
{
return;
}
Dictionary <ulong, sl.Objects> listOfNewobjects = new Dictionary <ulong, Objects>();
foreach (var it in batch_)
{
for (int j = 0; j < it.numData; j++)
{
ulong ts = it.timestamps[j];
sl.ObjectData newObjectData = new ObjectData();
newObjectData.id = it.id;
newObjectData.objectTrackingState = it.trackingState;
newObjectData.position = it.positions[j];
newObjectData.label = it.label;
newObjectData.sublabel = it.sublabel;
newObjectData.boundingBox2D = new Vector2[it.boundingBoxes2D.GetLength(1)];
for (int p = 0; p < it.boundingBoxes2D.GetLength(1); p++)
{
newObjectData.boundingBox2D[p] = it.boundingBoxes2D[j, p];
}
newObjectData.boundingBox = new Vector3[it.boundingBoxes.GetLength(1)];
for (int k = 0; k < it.boundingBoxes.GetLength(1); k++)
{
newObjectData.boundingBox[k] = it.boundingBoxes[j, k];
}
if (listOfNewobjects.ContainsKey(Utils.getMilliseconds(ts)))
{
int index = listOfNewobjects[Utils.getMilliseconds(ts)].numObject;
sl.Objects obj = listOfNewobjects[Utils.getMilliseconds(ts)];
obj.objectData[index] = newObjectData;
obj.numObject++;
listOfNewobjects[Utils.getMilliseconds(ts)] = obj;
}
else
{
sl.Objects currentObj = new sl.Objects();
currentObj.objectData = new ObjectData[(int)Constant.MAX_OBJECTS];
currentObj.timestamp = ts;
currentObj.isNew = 1;
currentObj.isTracked = 1;
currentObj.objectData[currentObj.numObject] = newObjectData;
currentObj.numObject++;
listOfNewobjects[Utils.getMilliseconds(ts)] = currentObj;
}
}
}
foreach (var elem in listOfNewobjects)
{
objectsTrackedQueue.Add(elem.Value);
}
}
public void pop(ref sl.Objects objects_)
{
if (objectsTrackedQueue.Count > 0)
{
sl.Objects trackedMergedObj = objectsTrackedQueue[0];
objects_ = trackedMergedObj;
objectsTrackedQueue.RemoveAt(0);
}
}
static void Main(string[] args)
{
// Set Initialization parameters
InitParameters init_params = new InitParameters();
init_params.resolution = RESOLUTION.HD720;
init_params.coordinateUnits = UNIT.METER;
init_params.sdkVerbose = true;
Camera zedCamera = new Camera(0);
// Open the camera
ERROR_CODE err = zedCamera.Open(ref init_params);
if (err != ERROR_CODE.SUCCESS)
Environment.Exit(-1);
// Enable positional tracking
PositionalTrackingParameters trackingParams = new PositionalTrackingParameters();
// If you want to have object tracking you need to enable positional tracking first
err = zedCamera.EnablePositionalTracking(ref trackingParams);
if (err != ERROR_CODE.SUCCESS)
Environment.Exit(-1);
// Enable Object Detection
object_detection_parameters = new ObjectDetectionParameters();
// Different model can be chosen, optimizing the runtime or the accuracy
object_detection_parameters.detectionModel = sl.DETECTION_MODEL.HUMAN_BODY_FAST;
// track detects object across time and space
object_detection_parameters.enableObjectTracking = true;
// run detection for every Camera grab
object_detection_parameters.imageSync = true;
err = zedCamera.EnableObjectDetection(ref object_detection_parameters);
if (err != ERROR_CODE.SUCCESS)
Environment.Exit(-1);
// Create Runtime parameters
RuntimeParameters runtimeParameters = new RuntimeParameters();
// Create Object Detection frame handle (contains all the objects data)
sl.Objects objects = new sl.Objects();
// Create object detection runtime parameters (confidence, ...)
ObjectDetectionRuntimeParameters obj_runtime_parameters = new ObjectDetectionRuntimeParameters();
obj_runtime_parameters.detectionConfidenceThreshold = 40;
int nbDetection = 0;
while (nbDetection < 100)
{
if (zedCamera.Grab(ref runtimeParameters) == ERROR_CODE.SUCCESS)
{
// Retrieve Objects from Object detection
zedCamera.RetrieveObjects(ref objects, ref obj_runtime_parameters);
if (Convert.ToBoolean(objects.isNew))
{
Console.WriteLine(objects.numObject + " Person(s) detected");
Console.WriteLine();
if (objects.numObject > 0)
{
sl.ObjectData firstObject = objects.objectData[0];
Console.WriteLine("First Person attributes :");
Console.WriteLine(" Confidence (" + firstObject.confidence);
if (object_detection_parameters.enableObjectTracking)
{
Console.WriteLine(" Tracking ID: " + firstObject.id + " tracking state: " + firstObject.objectTrackingState +
" / " + firstObject.actionState);
}
Console.WriteLine(" 3D Position: " + firstObject.position +
" Velocity: " + firstObject.velocity);
Console.WriteLine(" Keypoints 2D");
// The body part meaning can be obtained by casting the index into a BODY_PARTS
// to get the BODY_PARTS index the getIdx function is available
for (int i = 0; i < firstObject.keypoints2D.Length; i++)
{
var kp = firstObject.keypoints2D[i];
Console.WriteLine(" " + (sl.BODY_PARTS)i + " " + kp.X + ", " + kp.Y);
}
// The BODY_PARTS can be link as bones, using sl::BODY_BONES which gives the BODY_PARTS pair for each
Console.WriteLine(" Keypoints 3D ");
for (int i = 0; i < firstObject.keypoints.Length; i++)
{
var kp = firstObject.keypoints[i];
Console.WriteLine(" " + (sl.BODY_PARTS)i + " " + kp.X + ", " + kp.Y + ", " + kp.Z);
}
Console.WriteLine();
Console.WriteLine("Press 'Enter' to continue...");
Console.ReadLine();
}
}
}
}
// Disable object detection, positional tracking and close the camera
zedCamera.DisableObjectDetection();
zedCamera.DisablePositionalTracking("");
zedCamera.Close();
}
static void Main(string[] args)
{
// Set Initialization parameters
InitParameters init_params = new InitParameters();
init_params.resolution = RESOLUTION.HD2K;
init_params.coordinateUnits = UNIT.METER;
init_params.coordinateSystem = COORDINATE_SYSTEM.RIGHT_HANDED_Y_UP;
init_params.depthMode = DEPTH_MODE.PERFORMANCE;
Camera zedCamera = new Camera(0);
// Open the camera
ERROR_CODE err = zedCamera.Open(ref init_params);
if (err != ERROR_CODE.SUCCESS)
{
Environment.Exit(-1);
}
// Enable positional tracking
PositionalTrackingParameters trackingParams = new PositionalTrackingParameters();
err = zedCamera.EnablePositionalTracking(ref trackingParams);
if (err != ERROR_CODE.SUCCESS)
{
Environment.Exit(-1);
}
// Enable Object Detection
ObjectDetectionParameters object_detection_parameters = new ObjectDetectionParameters();
object_detection_parameters.detectionModel = sl.DETECTION_MODEL.MULTI_CLASS_BOX;
object_detection_parameters.enableObjectTracking = true;
err = zedCamera.EnableObjectDetection(ref object_detection_parameters);
if (err != ERROR_CODE.SUCCESS)
{
Environment.Exit(-1);
}
// Create Runtime parameters
RuntimeParameters runtimeParameters = new RuntimeParameters();
// Create Object Detection frame handle (contains all the objects data)
sl.Objects object_frame = new sl.Objects();
// Create object detection runtime parameters (confidence, ...)
ObjectDetectionRuntimeParameters obj_runtime_parameters = new ObjectDetectionRuntimeParameters();
obj_runtime_parameters.detectionConfidenceThreshold = 50;
int i = 0;
while (i < 1000)
{
if (zedCamera.Grab(ref runtimeParameters) == ERROR_CODE.SUCCESS)
{
// Retrieve Objects from Object detection
err = zedCamera.RetrieveObjects(ref object_frame, ref obj_runtime_parameters);
// Display the data each 10 frames
if (i % 10 == 0)
{
Console.WriteLine("Nb Objects Detection : " + object_frame.numObject);
for (int p = 0; p < object_frame.numObject; p++)
{
Console.WriteLine("Position of object " + p + " : " + object_frame.objectData[p].position + "Tracked? : " + object_frame.objectData[p].objectTrackingState);
}
}
i++;
}
}
// Disable object detection, positional tracking and close the camera
zedCamera.DisableObjectDetection();
zedCamera.DisablePositionalTracking("");
zedCamera.Close();
}
