*i"fdZddlmZddlmZmZddlZddlm Z ddl m Z m Z Gdde Z dS) aZED stereo camera implementation using the pyzed SDK. Recording mode : opens by serial number, records to SVO2 (H264). Depth is NOT computed during recording (depth_mode=NONE) for lower CPU usage; the raw stereo pair is stored instead. Playback mode : opens an SVO2 file, computes depth (NEURAL) for each frame. Use ZedCamera.from_svo() to create a playback instance. )Path)OptionalTupleN)Camera CameraFramec xeZdZdZddededefdZedefdZedefd Z edefd Z d d Z d d Z de dd fdZd dZdefdZdefdZe d!dede deddfdZdeejejeeefffdZdejfdZd"dZdefdZdefdZdefdZdefdZ d S)# ZedCamerauZED camera – records to .svo2 camera_name serial_numberfpsc ||_||_||_tj|_d|_tj|_tj|_ tj|_ d|_ dSNF) _name_serial_fpsslr_camera_is_openMat_image_depth _right_image _has_depth)selfr r rs 0/home/robot-lab/raiden_cmu/raiden/cameras/zed.py__init__zZedCamera.__init__s^  $  y{{  fhh fhh FHHreturnc|jSN)rrs rnamezZedCamera.name%s zrc*t|jSr")strrr#s rr zZedCamera.serial_number)s4<   rcdS)Nsvo2r#s rrecording_extensionzZedCamera.recording_extension-svrNctj}||jtjj|_|j|_tj j |_ tj j |_|j|}|tjjkr"t'd|jd|jd|d|_d|_dS)zAOpen live camera. Depth is disabled to reduce recording overhead.zFailed to open ZED camera 'z ' (serial: z): TFN)rInitParametersset_from_serial_numberr RESOLUTIONHD720camera_resolutionr camera_fps DEPTH_MODENONE depth_modeUNITMETERcoordinate_unitsropen ERROR_CODESUCCESS RuntimeErrorrrr)r init_paramsstatuss rr8zZedCamera.open5s')) **4<888(* (; %!% !#!3 ')w} $"";// R]* * *6dj66 L66-366  rcX|jr"|jd|_dSdSr)rrcloser#s rr?zZedCamera.closeGs5 = " L   !DMMM " "rpathctj}tjj|_t ||_|j|}|tj j krtd|j d|dS)z>Enable SVO2 recording. Every subsequent grab() writes a frame.z$Failed to start SVO2 recording for '': N) rRecordingParametersSVO_COMPRESSION_MODEH264compression_moder&video_filenamerenable_recordingr9r:r;r)rr@paramsr=s rstart_recordingzZedCamera.start_recordingLs'))"$"9"> #D ..v66 R]* * *NtzNNfNN  + *rc8|jdSr")rdisable_recordingr#s rstop_recordingzZedCamera.stop_recordingWs &&(((((rctj}|j|}|tjjkS)z3Grab next frame (blocks until ready at camera FPS).)rRuntimeParametersrgrabr9r:)rruntime_paramsr=s rrPzZedCamera.grabZs5-//"">22...rc|j|jtjj|jddddddf}d}|jr|j |j tj j |j }tjtj||dtj}|jtjj}t-|||S)z>Retrieve color (and depth if in playback mode) from last grab.N)colordepth timestamp_ns)rretrieve_imagerrVIEWLEFTget_datacopyrretrieve_measurerMEASUREDEPTHnpwhereisfiniteastypefloat32 get_timestampTIME_REFERENCEIMAGEget_nanosecondsr)rrUrVrawrWs r get_framezZedCamera.get_frame`s ##DK>>> $$&&qqq!!!RaRx05577&* ? L L ) )$+rz7G H H H+&&((--//CHR[--sC88?? KKE|11   #  /   e,OOOOrTsvo_pathcompute_sdk_depthc||}||_d|_d|_t j|_d|_t j|_ t j|_ t j|_ ||_ t j }|t|d|_|rtjjntjj|_tjj|_d|_|j|}|tjjkrt9d|d|d|_|S)acOpen an SVO2 file for frame-by-frame extraction. Parameters ---------- compute_sdk_depth : bool If True (default) depth is computed by the ZED SDK (NEURAL_LIGHT). Set to False when you will compute depth externally (e.g. with FFS); this skips the SDK depth pass for lower CPU/GPU load. rr Fg{Gz?zFailed to open SVO2 'rBT)__new__rrrrrrrrrrrrr,set_from_svo_filer&svo_real_time_moder2 NEURAL_LIGHTr3r4r5r6r7depth_minimum_distancer8r9r:r;)clsr rkrlcamr<r=s rfrom_svozZedCamera.from_svous" kk#  ikk  VXX VXX 688*')) %%c(mm444). &*; SBM & &AS (*w} $-1 *!!+.. R]* * *LxLLFLLMM M  rc|j}|jjj}|jj}t j|jd|j gd|j |j ggdgt j }t j|j d|j d|j d|j d|j dgt j }|j|jf}|||fS) z?Return ZED SDK factory-calibrated intrinsics for the left lens.rT)rTrTg?)dtyperrrS)rget_camera_informationcamera_configurationcalibration_parametersleft_cam resolutionr`arrayfxcxfycyfloat64distowidthheight)rinfocalres camera_matrix dist_coeffs image_sizes rget_intrinsicszZedCamera.get_intrinsicss|2244'>G'2fc36 "S#&#&$9??? K*   h Yq\39Q<1sy|SYq\ R*   i, k:55rc|j|jtjj|jddddddfS)zReturn the right-camera BGR image for the most recently grabbed frame. Only valid after a successful grab(). Used by external stereo depth predictors (e.g. Fast Foundation Stereo) that need both views. NrS)rrXrrrYRIGHTr[r\r#s rget_right_colorzZedCamera.get_right_colors[ ##D$5rw}EEE ))++AAAqqq"1"H5::<< 3G'2 ..;)*++9Yj&&&&)A,)A,)A,)A,)A,   r)r )r N)T)r r)!__name__ __module__ __qualname____doc__r&intrpropertyr$r r*r8r?rrJrMboolrPrrj classmethodrurr`ndarrayrrrrrrdictrr)rrr r s))   C    #     cX!s!!!X!SX    $"""" D T    ))))/d//// P;PPPP* #' (((( (  ((([(T6bj"*eCHo&M N6666$=====    7#7777UUUUUW#WWWW       rr )rpathlibrtypingrrnumpyr`pyzed.slrbaserrr r)rrrs""""""""%%%%%%%%Y Y Y Y Y Y Y Y Y Y r