/// <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))); }