static sl.float2 getImagePosition(Vector2[] bounding_box_image, sl.float2 img_scale)
 {
     sl.float2 position;
     position.x = (bounding_box_image[0].X + (bounding_box_image[2].X - bounding_box_image[0].X) * 0.5f) * img_scale.x;
     position.y = (bounding_box_image[0].Y + (bounding_box_image[2].Y - bounding_box_image[0].Y) * 0.5f) * img_scale.y;
     return(position);
 }
    public static void render_2D(ref OpenCvSharp.Mat left_display, sl.float2 img_scale, ref sl.Objects objects, bool render_mask, bool isTrackingON)
    {
        OpenCvSharp.Mat  overlay    = left_display.Clone();
        OpenCvSharp.Rect roi_render = new OpenCvSharp.Rect(0, 0, left_display.Size().Width, left_display.Size().Height);

        OpenCvSharp.Mat mask = new OpenCvSharp.Mat(left_display.Rows, left_display.Cols, OpenCvSharp.MatType.CV_8UC1);

        int line_thickness = 2;

        for (int i = 0; i < objects.numObject; i++)
        {
            sl.ObjectData obj = objects.objectData[i];
            if (Utils.renderObject(obj, isTrackingON))
            {
                OpenCvSharp.Scalar base_color = Utils.generateColorID_u(obj.id);

                // Display image scale bouding box 2d
                if (obj.boundingBox2D.Length < 4)
                {
                    continue;
                }

                Point top_left_corner     = Utils.cvt(obj.boundingBox2D[0], img_scale);
                Point top_right_corner    = Utils.cvt(obj.boundingBox2D[1], img_scale);
                Point bottom_right_corner = Utils.cvt(obj.boundingBox2D[2], img_scale);
                Point bottom_left_corner  = Utils.cvt(obj.boundingBox2D[3], img_scale);

                // Create of the 2 horizontal lines
                Cv2.Line(left_display, top_left_corner, top_right_corner, base_color, line_thickness);
                Cv2.Line(left_display, bottom_left_corner, bottom_right_corner, base_color, line_thickness);
                // Creation of two vertical lines
                Utils.drawVerticalLine(ref left_display, bottom_left_corner, top_left_corner, base_color, line_thickness);
                Utils.drawVerticalLine(ref left_display, bottom_right_corner, top_right_corner, base_color, line_thickness);

                // Scaled ROI
                OpenCvSharp.Rect roi = new OpenCvSharp.Rect(top_left_corner.X, top_left_corner.Y, (int)top_right_corner.DistanceTo(top_left_corner), (int)bottom_right_corner.DistanceTo(top_right_corner));

                overlay.SubMat(roi).SetTo(base_color);

                sl.float2 position_image = getImagePosition(obj.boundingBox2D, img_scale);
                Cv2.PutText(left_display, obj.label.ToString(), new Point(position_image.x - 20, position_image.y - 12), HersheyFonts.HersheyComplexSmall, 0.5f, new Scalar(255, 255, 255, 255), 1);

                if (!float.IsInfinity(obj.position.Z))
                {
                    string text = Math.Abs(obj.position.Z).ToString("0.##M");
                    Cv2.PutText(left_display, text, new Point(position_image.x - 20, position_image.y), HersheyFonts.HersheyComplexSmall, 0.5, new Scalar(255, 255, 255, 255), 1);
                }
            }
        }

        // Here, overlay is as the left image, but with opaque masks on each detected objects
        Cv2.AddWeighted(left_display, 0.7, overlay, 0.3, 0.0, left_display);
    }
    public static void render_2D(ref OpenCvSharp.Mat left_display, sl.float2 img_scale, ref sl.Objects objects, bool showOnlyOK)
    {
        OpenCvSharp.Mat  overlay    = left_display.Clone();
        OpenCvSharp.Rect roi_render = new OpenCvSharp.Rect(1, 1, left_display.Size().Width, left_display.Size().Height);

        for (int i = 0; i < objects.numObject; i++)
        {
            sl.ObjectData obj = objects.objectData[i];
            if (renderObject(obj, showOnlyOK))
            {
                // Draw Skeleton bones
                OpenCvSharp.Scalar base_color = generateColorID(obj.id);
                foreach (var part in SKELETON_BONES)
                {
                    var kp_a = cvt(obj.keypoints2D[(int)part.Item1], img_scale);
                    var kp_b = cvt(obj.keypoints2D[(int)part.Item2], img_scale);
                    if (roi_render.Contains(kp_a) && roi_render.Contains(kp_b))
                    {
                        Cv2.Line(left_display, kp_a, kp_b, base_color, 1, LineTypes.AntiAlias);
                    }
                }

                var hip_left  = obj.keypoints2D[(int)sl.BODY_PARTS.LEFT_HIP];
                var hip_right = obj.keypoints2D[(int)sl.BODY_PARTS.RIGHT_HIP];
                var spine     = (hip_left + hip_right) / 2;
                var neck      = obj.keypoints2D[(int)sl.BODY_PARTS.NECK];

                if (hip_left.X > 0 && hip_left.Y > 0 && hip_right.X > 0 && hip_right.Y > 0 && neck.X > 0 && neck.Y > 0)
                {
                    var spine_a = cvt(spine, img_scale);
                    var spine_b = cvt(neck, img_scale);
                    if (roi_render.Contains(spine_a) && roi_render.Contains(spine_b))
                    {
                        Cv2.Line(left_display, spine_a, spine_b, base_color, 1, LineTypes.AntiAlias);
                    }
                }

                // Draw Skeleton joints
                foreach (var kp in obj.keypoints2D)
                {
                    Point cv_kp = cvt(kp, img_scale);
                    if (roi_render.Contains(cv_kp))
                    {
                        Cv2.Circle(left_display, cv_kp, 3, base_color, -1);
                    }
                }

                if (hip_left.X > 0 && hip_left.Y > 0 && hip_right.X > 0 && hip_right.Y > 0)
                {
                    Point cv_spine = cvt(spine, img_scale);
                    if (roi_render.Contains(cv_spine))
                    {
                        Cv2.Circle(left_display, cv_spine, 3, base_color, -1);
                    }
                }
            }
        }

        // Here, overlay is as the left image, but with opaque masks on each detected objects
        Cv2.AddWeighted(left_display, 0.9, overlay, 0.1, 0.0, left_display);
    }
 static Point cvt(Vector2 point, sl.float2 scale)
 {
     return(new Point(point.X * scale.x, point.Y * scale.y));
 }
Beispiel #5
0
    public MainWindow(string[] args)
    {
        // Set configuration parameters
        InitParameters init_params = new InitParameters();

        init_params.resolution             = RESOLUTION.HD1080;
        init_params.depthMode              = DEPTH_MODE.ULTRA;
        init_params.coordinateUnits        = UNIT.METER;
        init_params.coordinateSystem       = COORDINATE_SYSTEM.RIGHT_HANDED_Y_UP;
        init_params.depthMaximumDistance   = 10f;
        init_params.cameraDisableSelfCalib = true;

        maxDepthDistance = init_params.depthMaximumDistance;
        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)
        {
            Console.WriteLine(" ERROR : Use ZED2 Camera only");
            return;
        }

        // Enable tracking (mandatory for object detection)
        Quaternion quat = Quaternion.Identity;
        Vector3    vec  = Vector3.Zero;

        zedCamera.EnablePositionalTracking(ref quat, ref vec);

        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.imageSync      = true; // the object detection is synchronized to the image
        obj_det_params.detectionModel = sl.DETECTION_MODEL.MULTI_CLASS_BOX_ACCURATE;

        if (USE_BATCHING)
        {
            batchParameters                = new BatchParameters();
            batchParameters.latency        = 2.0f;
            batchParameters.enable         = true;
            batchHandler                   = new BatchSystemHandler((int)batchParameters.latency * 2);
            obj_det_params.batchParameters = batchParameters;
        }

        zedCamera.EnableObjectDetection(ref obj_det_params);

        // Configure object detection runtime parameters
        obj_runtime_parameters = new ObjectDetectionRuntimeParameters();
        detection_confidence   = 60;
        obj_runtime_parameters.detectionConfidenceThreshold = detection_confidence;
        obj_runtime_parameters.objectClassFilter            = new int[(int)OBJECT_CLASS.LAST];
        obj_runtime_parameters.objectClassFilter[(int)sl.OBJECT_CLASS.PERSON] = Convert.ToInt32(true);
        //obj_runtime_parameters.objectClassFilter[(int)sl.OBJECT_CLASS.VEHICLE] = Convert.ToInt32(true);
        // To set a specific threshold
        obj_runtime_parameters.objectConfidenceThreshold = new int[(int)OBJECT_CLASS.LAST];
        obj_runtime_parameters.objectConfidenceThreshold[(int)sl.OBJECT_CLASS.PERSON] = detection_confidence;
        //obj_runtime_parameters.object_confidence_threshold[(int)sl.OBJECT_CLASS.VEHICLE] = detection_confidence;

        // Create ZED Objects filled in the main loop
        objects   = new Objects();
        imageLeft = new sl.Mat();
        int Height = zedCamera.ImageHeight;
        int Width  = zedCamera.ImageWidth;

        displayRes = new Resolution(Math.Min((uint)Width, 1280), Math.Min((uint)Height, 720));
        Resolution tracksRes = new Resolution(400, (uint)displayRes.height);

        // create a global image to store both image and tracks view
        globalImage = new OpenCvSharp.Mat((int)displayRes.height, (int)displayRes.width + (int)tracksRes.width, OpenCvSharp.MatType.CV_8UC4);
        // retrieve ref on image part
        imageLeftOcv = new OpenCvSharp.Mat(globalImage, new OpenCvSharp.Rect(0, 0, (int)displayRes.width, (int)displayRes.height));
        // retrieve ref on tracks part
        imageTrackOcv = new OpenCvSharp.Mat(globalImage, new OpenCvSharp.Rect((int)displayRes.width, 0, (int)tracksRes.width, (int)tracksRes.height));
        // init an sl::Mat from the ocv image ref (which is in fact the memory of global_image)
        imageLeft.Create(displayRes, MAT_TYPE.MAT_8U_C4, MEM.CPU);
        imageRenderLeft = new OpenCvSharp.Mat((int)displayRes.height, (int)displayRes.width, OpenCvSharp.MatType.CV_8UC4, imageLeft.GetPtr());
        imgScale        = new sl.float2((int)displayRes.width / (float)Width, (int)displayRes.height / (float)Height);

        // Create OpenGL Viewer
        viewer = new GLViewer();

        camWorldPose  = new Pose();
        camCameraPose = new Pose();
        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);

        // 2D tracks
        trackViewGenerator = new TrackingViewer(tracksRes, (int)zedCamera.GetCameraFPS(), maxDepthDistance, 3);
        trackViewGenerator.setCameraCalibration(zedCamera.GetCalibrationParameters());

        window_name = "ZED| 2D View and Birds view";
        Cv2.NamedWindow(window_name, WindowMode.Normal);// Create Window
        Cv2.CreateTrackbar("Confidence", window_name, ref detection_confidence, 100);

        // Create OpenGL window
        CreateWindow();
    }