Beispiel #1
0
        /// <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);
            }
        }
Beispiel #2
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);
        }
Beispiel #3
0
        /// <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_);
        }
Beispiel #4
0
        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);
        }
Beispiel #5
0
        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);
        }
Beispiel #6
0
        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);
            }
        }
Beispiel #7
0
        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);
            }
        }
Beispiel #8
0
        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();
        }
Beispiel #9
0
 /// <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)));
 }