mailsys_server/mailcalibrate.py

import pyzed.sl as sl
import numpy as np
import sys
import os
import time
import json
import math
import json

JSON_PATH = '~/sources/configuration.json'


def conf_depth(camobject=sl.Camera(), headCoord=(480,540), tailCoord=(1440,540)):
    # Create a Camera object
    hc = headCoord
    tc = tailCoord


    zed = sl.Camera()

    # Create a InitParameters object and set configuration parameters
    init = sl.InitParameters()
    init.set_from_serial_number
    init.depth_mode = sl.DEPTH_MODE.PERFORMANCE  # Use PERFORMANCE depth mode
    init.coordinate_units = sl.UNIT.METER  # Use meter units (for depth measurements)
    init.camera_resolution = sl.RESOLUTION.HD1080

    # Open the camera
    err = zed.open(init)
    if err != sl.ERROR_CODE.SUCCESS:
        exit(1)

    # Create and set RuntimeParameters after opening the camera
    runtime_parameters = sl.RuntimeParameters()
    runtime_parameters.sensing_mode = sl.SENSING_MODE.STANDARD  # Use STANDARD sensing mode
    # Setting the depth confidence parameters
    runtime_parameters.confidence_threshold = 100
    runtime_parameters.textureness_confidence_threshold = 100

    # Capture 150 images and depth, then stop
    i = 0
    image = sl.Mat()
    depth = sl.Mat()
    point_cloud = sl.Mat()

    mirror_ref = sl.Transform()
    mirror_ref.set_translation(sl.Translation(2.75,4.0,0))
    tr_np = mirror_ref.m
    hx = 0
    tx = 0    
    distance_head_cliff = 0
    distance_tail_cliff = 0
    while i < 900:
        # A new image is available if grab() returns SUCCESS
        if zed.grab(runtime_parameters) == sl.ERROR_CODE.SUCCESS:
            # Retrieve left image
            zed.retrieve_image(image, sl.VIEW.LEFT)
            # Retrieve depth map. Depth is aligned on the left image
            zed.retrieve_measure(depth, sl.MEASURE.DEPTH)
            # Retrieve colored point cloud. Point cloud is aligned on the left image.
            zed.retrieve_measure(point_cloud, sl.MEASURE.XYZRGBA)

            # Get and print distance value in mm at the center of the image
            # We measure the distance camera - object using Euclidean distance
            x = round(image.get_width() / 2)
            y = round(image.get_height() / 2)
            if hx == 0 :
                hx = x
                tx = x
            err, point_cloud_value_center = point_cloud.get_value(x, y)

            distance_center = math.sqrt(point_cloud_value_center[0] * point_cloud_value_center[0] +
                                 point_cloud_value_center[1] * point_cloud_value_center[1] +
                                 point_cloud_value_center[2] * point_cloud_value_center[2])
            

            err, point_cloud_value_head = point_cloud.get_value(hx,y)
            distance_head = math.sqrt(point_cloud_value_head[0] * point_cloud_value_head[0] +
                                 point_cloud_value_head[1] * point_cloud_value_head[1] +
                                 point_cloud_value_head[2] * point_cloud_value_head[2])
            if distance_head_cliff != 0:
                pass
            elif distance_head > (0.04 + distance_center):
                if np.isnan(prev_distance_head) == True:
                    pass
                else:
                    distance_head_cliff = prev_distance_head
                    hx_cliff = hx
            
            if np.isnan(distance_head) == False:
                prev_distance_head = distance_head

            err, point_cloud_value_tail = point_cloud.get_value(tx,y)
            distance_tail =  math.sqrt(point_cloud_value_tail[0] * point_cloud_value_tail[0] +
                                 point_cloud_value_tail[1] * point_cloud_value_tail[1] +
                                 point_cloud_value_tail[2] * point_cloud_value_tail[2])
            
            if distance_tail_cliff != 0:
                pass
            elif distance_tail > (0.04 + distance_center):
                if np.isnan(prev_distance_tail) == True:
                    pass
                else:
                    distance_tail_cliff = prev_distance_tail
                    tx_cliff = tx - 1
            if np.isnan(distance_tail) == False:
                prev_distance_tail = distance_tail
            

            point_cloud_np = point_cloud.get_data()
            point_cloud_np.dot(tr_np)
            hx -=1
            tx +=1

            if not np.isnan(distance_center) and not np.isinf(distance_center):
                print("Distance to Camera at ({}, {}) (image center): {:1.3} m".format(hx, y, distance_head))
                print("Distance to Camera at ({}, {}) (image center): {:1.3} m".format(x, y, distance_center))                
                print("Distance to Camera at ({}, {}) (image center): {:1.3} m".format(tx, y, distance_tail))
                # Increment the loop
                i = i + 1
            else:
                print("Can't estimate distance at this position.")
                print("Your camera is probably too close to the scene, please move it backwards.\n")
            sys.stdout.flush()

    # Close the camera
    zed.close()
    return [distance_center,hx_cliff,distance_head_cliff,tx_cliff,distance_tail_cliff]



def main():
    cameras = sl.Camera.get_device_list()
    if len(cameras) == 1:
        with open (os.path.abspath(os.path.expanduser(JSON_PATH)), "r") as json_file:
            conf = json.load(json_file)
        d_list = conf_depth(cameras[0],conf["cam1"]["headCoord"], conf["cam1"]["tailCoord"])

        conf["cam1"]["d_center"] = d_list[0]             
        conf["cam1"]["d_headCoord"] = d_list[2]        
        conf["cam1"]["d_tailCoord"] = d_list[4]

        #as maincam, head is left side of image
        if conf["mainCam"] == cameras[0][0]:
            conf["cam1"]["headCoord"] = [d_list[1], 540]
            conf["cam1"]["tailCoord"] = [d_list[3], 540]
        else:
            conf["cam1"]["headCoord"] = [d_list[3], 540]
            conf["cam1"]["tailCoord"] = [d_list[1], 540]
        
        with open (os.path.abspath(os.path.expanduser(JSON_PATH)), "w") as json_file:
            json.dump(conf,json_file, indent=4)
    elif len(cameras) == 2:
        with open (os.path.abspath(os.path.expanduser(JSON_PATH)), "r") as json_file:
            conf = json.load(json_file)
        d_list = conf_depth(cameras[1],conf["cam2"]["headCoord"], conf["cam2"]["tailCoord"])

        conf["cam2"]["d_center"] = d_list[0]             
        conf["cam2"]["d_headCoord"] = d_list[2]        
        conf["cam2"]["d_tailCoord"] = d_list[4]
        if conf["mainCam"] == cameras[1][0]:            
            conf["cam2"]["headCoord"] = [d_list[1], 540]
            conf["cam2"]["tailCoord"] = [d_list[3], 540]
        else:
            conf["cam2"]["headCoord"] = [d_list[3], 540]
            conf["cam2"]["tailCoord"] = [d_list[1], 540]
        with open (os.path.abspath(os.path.expanduser(JSON_PATH)), "w") as json_file:
            json.dump(conf,json_file, indent=4)    

if __name__ == "__main__":
    main()