Example #1
0
def test_f():
    """
    Test f(input, K, obj_point)
    :return:
    """

    alpha = 0
    belt = 0
    r = 1
    input = [0.0, 0.0, 0.0]
    input[0] = alpha
    input[1] = belt
    input[2] = r

    cam = Camera()
    cam.set_K(fx=800, fy=800, cx=640 / 2., cy=480 / 2.)
    cam.set_width_heigth(640, 480)
    cam.set_R_mat(
        Rt_matrix_from_euler_t.R_matrix_from_euler_t(0, np.deg2rad(0), 0))
    cam.set_t(0., 0., 0.5, 'world')
    pl = CircularPlane(origin=np.array([0., 0., 0.]),
                       normal=np.array([0, 0, 1]),
                       radius=0.15,
                       n=4)
    x1 = round(pl.radius * np.cos(np.deg2rad(45)), 3)
    y1 = round(pl.radius * np.sin(np.deg2rad(45)), 3)
    objectPoints_square = np.array([[x1, -x1, -x1, x1], [y1, y1, -y1, -y1],
                                    [0., 0., 0., 0.], [1., 1., 1., 1.]])

    new_objectPoints = np.copy(objectPoints_square)

    cms.f(input, cam.K, new_objectPoints)
def test1(objectPoints_square):
    # ------------------------------Z fixed, study X Y-----------------------------------------
    cams_Zfixed = []
    for x in np.linspace(-0.5, 0.5, 10):
        for y in np.linspace(-0.5, 0.5, 10):
            cam1 = Camera()
            cam1.set_K(fx=800, fy=800, cx=640 / 2., cy=480 / 2.)
            cam1.set_width_heigth(640, 480)

            ## DEFINE A SET OF CAMERA POSES IN DIFFERENT POSITIONS BUT ALWAYS LOOKING
            # TO THE CENTER OF THE PLANE MODEL
            # TODO LOOK AT
            cam1.set_R_axisAngle(1.0, 0.0, 0.0, np.deg2rad(180.0))
            # TODO  cv2.SOLVEPNP_DLS, cv2.SOLVEPNP_EPNP, cv2.SOLVEPNP_ITERATIVE
            # cam1.set_t(x, -0.01, 1.31660688, frame='world')
            cam1.set_t(x, y, 1.3, frame='world')
            # 0.28075725, -0.23558331, 1.31660688
            cams_Zfixed.append(cam1)

    new_objectPoints = np.copy(objectPoints_square)
    xInputs = []
    yInputs = []
    volumes = []
    for cam in cams_Zfixed:
        t = cam.get_world_position()
        xInputs.append(t[0])
        yInputs.append(t[1])

        cov_mat = covariance_alpha_belt_r(cam, new_objectPoints)
        a, b, c = ellipsoid.get_semi_axes_abc(cov_mat, 0.95)
        v = ellipsoid.ellipsoid_Volume(a, b, c)
        volumes.append(v)

    dvm.displayCovVolume_Zfixed3D(xInputs, yInputs, volumes)
Example #3
0
 def __init__(self):
     """ Definition of a simulated camera """
     self.cam = Camera()
     self.cam.set_K(fx=100, fy=100, cx=640, cy=480)
     self.cam.set_width_heigth(1280, 960)
     """ Initial camera pose looking stratight down into the plane model """
     self.cam.set_R_axisAngle(1.0, 0.0, 0.0, np.deg2rad(180))
     self.cam.set_t(0.0, 0.0, 1.5, frame='world')
     """ Plane for the control points """
     self.sph = Sphere(radius=0.5)
     self.sph.random_points(p=6, r=0.5, min_dist=0.001)
Example #4
0
def test2_covariance_alpha_belt_r():
    """
    Test covariance_alpha_belt_r(cam, new_objectPoints)
    X Y fixed, Z changed
    :return:
    """
    pl = CircularPlane(origin=np.array([0., 0., 0.]),
                       normal=np.array([0, 0, 1]),
                       radius=0.15,
                       n=4)
    x1 = round(pl.radius * np.cos(np.deg2rad(45)), 3)
    y1 = round(pl.radius * np.sin(np.deg2rad(45)), 3)
    objectPoints_square = np.array([[x1, -x1, -x1, x1], [y1, y1, -y1, -y1],
                                    [0., 0., 0., 0.], [1., 1., 1., 1.]])

    new_objectPoints = np.copy(objectPoints_square)

    # -----------------------------X Y fixed, Z changed-----------------------------
    cams_XYfixed = []
    for i in np.linspace(0.5, 2, 5):
        cam1 = Camera()
        cam1.set_K(fx=800, fy=800, cx=640 / 2., cy=480 / 2.)
        cam1.set_width_heigth(640, 480)
        cam1.set_R_axisAngle(1.0, 0.0, 0.0, np.deg2rad(180.0))
        cam1.set_t(0, 0, i, frame='world')
        # 0.28075725, -0.23558331, 1.31660688
        cams_XYfixed.append(cam1)

    zInputs = []
    volumes = []
    ellipsoid_paras = np.array([[0], [0], [0], [0], [0], [0]])  # a,b,c,x,y,z
    for cam in cams_XYfixed:
        cam_tem = cam.clone()
        valid = cms.validCam(cam_tem, new_objectPoints)
        if valid:
            t = cam.get_world_position()
            zInputs.append(t[2])
            print "Height", t[2]

            cov_mat = cms.covariance_alpha_belt_r(cam, new_objectPoints)
            a, b, c = ellipsoid.get_semi_axes_abc(cov_mat, 0.95)
            display_array = np.array(
                [[0.0], [0.0], [0.0], [0.0], [0.0], [0.0]], dtype=float)
            display_array[0:3, 0] = a, b, c
            display_array[3:6, 0] = np.copy(t[0:3])
            ellipsoid_paras = np.hstack((ellipsoid_paras, display_array))

            v = ellipsoid.ellipsoid_Volume(a, b, c)
            print "volumn", v
            volumes.append(v)

    dvm.displayCovVolume_XYfixed3D(zInputs, volumes)
Example #5
0
class EyeCircle(ImageSource):
    def __init__(self):
        super().__init__()
        self.camera = Camera()
        self.w = 64
        self.h = 64

    def read(self):
        frame = self.camera.read()
        frame = cv2.resize(frame, (self.h, self.w))

        frame = cv2.cvtColor(frame, cv2.COLOR_RGB2GRAY)

        #_, frame = cv2.threshold(frame, 160, 255, cv2.THRESH_BINARY)
        #frame = cv2.Sobel(frame, cv2.CV_8UC1, 1, 1)  # get borders
        #_, frame = cv2.threshold(frame, 9, 255, cv2.THRESH_BINARY)  # make it B&W only
        circles = cv2.HoughCircles(frame,
                                   cv2.HOUGH_GRADIENT,
                                   1,
                                   500,
                                   param1=1,
                                   param2=30,
                                   minRadius=10,
                                   maxRadius=40)
        if circles is not None:
            circles = np.uint16(np.around(circles))
            for i in circles[0, :]:
                # draw the outer circle
                cv2.circle(frame, (i[0], i[1]), i[2], (0, 255, 0), 2)
                # draw the center of the circle
                cv2.circle(frame, (i[0], i[1]), 2, (0, 0, 255), 3)

        image_display.show(frame, 'circles')
        image_display.wait_key()
Example #6
0
def find_camera():
	global camera
	
	cidx = CAMERA_START_INDEX

	while True :
		try:
			if camera:
				del(camera)

			print(cidx)

			camera = Camera(cidx, 10)
			camera.set(3, CAMERA_WIDTH)
			camera.set(4, CAMERA_HEIGHT)
			camera.set(5, 20)

			cidx = cidx + 1
			if cidx > CAMERA_MAX_INDEX:
				cidx = CAMERA_START_INDEX

			if camera == None:
				print("none")

			im = camera.get_image()

			if im == None:
				continue

			h,w,_ = im.shape

			print("%d, %d" % (h, w))

			if w < 1 or h < 1:
				continue
		
			if SHOW_IMAGE :
				cv2.imshow("image", im)

			break

		except (KeyboardInterrupt, SystemExit):
			sys.exit()
		except:
			time.sleep(0.01)
			pass
def accuracy_func(plane_size=(0.3, 0.3),camera_intrinsic_params=(800.,800.,320.,240.), theta_params=(0.0, 180.0, 5), r_params=(0.1, 2.0, 5), z=0, y=0):
    """
    Calculate the accuracy degree of given position y,z in world

    y:
    z:
    :return 0 means the cam is out of range of marker
    """

    # --------------------------------------accuracy_degree_distribution---------------------
    cam = Camera()
    fx = camera_intrinsic_params[0]
    fy = camera_intrinsic_params[1]
    cx = camera_intrinsic_params[2]
    cy = camera_intrinsic_params[3]
    cam.set_K(fx=fx, fy=fy, cx=cx, cy=cy)
    width = cx * 2
    height = cy * 2
    cam.set_width_heigth(width, height)

    cams,accuracy_mat = create_cam_distribution_in_YZ(cam=None, plane_size=plane_size, theta_params=theta_params, r_params=r_params,
                                  plot=False)

    accuracy_mat = np.zeros([30, 60])  # define the new acc_mat as 30 x 30 ,which means 3m x 3m area in real world and each cell is 0.1m x 0.1m
    inputX, inputY, inputZ, input_ippe1_t, input_ippe1_R, input_ippe2_t, input_ippe2_R, input_pnp_t, input_pnp_R, input_transfer_error, display_mat,accuracy_mat_new = bf.heightGetCondNum(cams,accuracy_mat,theta_params, r_params)

    # print accuracy_mat_new
    # --------------------------------------------------------------------------------------------
    degree = 5  # divide accuracy into 5 degree
    r = np.sqrt(z * z + y * y)
    # out of range
    if r >= r_params[1] or r == 0:
        return 0
    else:
        # according to radius and angle of cam, Calculate the index of accuracy matrix
        r_begin = r_params[0]
        r_end = r_params[1]
        r_num = r_params[2]
        r_step = (r_end - r_begin) / (r_num - 1)
        # print "r_step",r_step
        half_r_step = r_step / 2.0
        r_num1 = math.floor(r / r_step)
        r_num2 = math.floor((r - r_step * r_num1) / half_r_step)
        row_index = int(r_num1 + r_num2)  # row index

        angle_begin = theta_params[0]
        angle_end = theta_params[1]
        angle_num = theta_params[2]
        angle_step = (angle_end - angle_begin) / (angle_num - 1)
        # print "angle_step",angle_step
        half_angle_step = angle_step / 2.0
        angle = np.rad2deg(np.arccos(y / r))  # deg
        angle_num1 = math.floor(angle / angle_step)
        angle_num2 = math.floor((angle - angle_step * angle_num1) / half_angle_step)
        col_index = int(angle_num1 + angle_num2)
        # print "row_index,col_index",row_index,col_index
        acc_degree = accuracy_mat_new[row_index, col_index]
        # print "acc_degree",acc_degree
        return acc_degree
Example #8
0
def test3_covariance_alpha_belt_r():
    """
    Y fixed, study X Z  like a half circle above the marker
    :return:
    """
    pl = CircularPlane(origin=np.array([0., 0., 0.]),
                       normal=np.array([0, 0, 1]),
                       radius=0.15,
                       n=4)
    x1 = round(pl.radius * np.cos(np.deg2rad(45)), 3)
    y1 = round(pl.radius * np.sin(np.deg2rad(45)), 3)
    objectPoints_square = np.array([[x1, -x1, -x1, x1], [y1, y1, -y1, -y1],
                                    [0., 0., 0., 0.], [1., 1., 1., 1.]])

    new_objectPoints = np.copy(objectPoints_square)
    # ------------------------------Y fixed, study X Z  like a half circle -----------------------------------------
    cams_Yfixed = []
    for angle in np.linspace(np.deg2rad(0), np.deg2rad(180), 20):
        x = np.cos(angle)
        z = np.sin(angle)
        cam1 = Camera()
        cam1.set_K(fx=800, fy=800, cx=640 / 2., cy=480 / 2.)
        cam1.set_width_heigth(640, 480)
        # TODO  WE have not set R yet
        # print 'cam1.R',cam1.R
        cam1.set_t(x, 0, z, frame='world')
        cams_Yfixed.append(cam1)

    xInputs = []
    zInputs = []
    volumes = []
    for cam in cams_Yfixed:
        cam_tem = cam.clone()
        valid = cms.validCam(cam_tem, new_objectPoints)
        if valid:
            t = cam.get_world_position()
            xInputs.append(t[0])
            zInputs.append(t[2])
            print "x = ", t[0]
            print "z = ", t[2]

            cov_mat = cms.covariance_alpha_belt_r(cam, new_objectPoints)
            a, b, c = ellipsoid.get_semi_axes_abc(cov_mat, 0.95)
            v = ellipsoid.ellipsoid_Volume(a, b, c)
            print "v", v
            volumes.append(v)

    dvm.displayCovVolume_Zfixed3D(xInputs, zInputs, volumes)
Example #9
0
def test_covariance_alpha_belt_r():
    """
    Test covariance_alpha_belt_r(cam, new_objectPoints)
    Z fixed, study X Y-
    :return:
    """
    pl = CircularPlane(origin=np.array([0., 0., 0.]),
                       normal=np.array([0, 0, 1]),
                       radius=0.15,
                       n=4)
    x1 = round(pl.radius * np.cos(np.deg2rad(45)), 3)
    y1 = round(pl.radius * np.sin(np.deg2rad(45)), 3)
    objectPoints_square = np.array([[x1, -x1, -x1, x1], [y1, y1, -y1, -y1],
                                    [0., 0., 0., 0.], [1., 1., 1., 1.]])

    new_objectPoints = np.copy(objectPoints_square)

    # ------------------------------Z fixed, study X Y-----------------------------------------
    cams_Zfixed = []
    for x in np.linspace(-0.5, 0.5, 5):
        for y in np.linspace(-0.5, 0.5, 5):
            cam1 = Camera()
            cam1.set_K(fx=800, fy=800, cx=640 / 2., cy=480 / 2.)
            cam1.set_width_heigth(640, 480)
            # TODO  WE have not set R yet
            # print 'cam1.R',cam1.R
            cam1.set_t(x, y, 1.3, frame='world')
            cams_Zfixed.append(cam1)

    xInputs = []
    yInputs = []
    volumes = []
    for cam in cams_Zfixed:
        cam_tem = cam.clone()
        valid = cms.validCam(cam_tem, new_objectPoints)
        if valid:
            t = cam.get_world_position()
            xInputs.append(t[0])
            yInputs.append(t[1])

            cov_mat = cms.covariance_alpha_belt_r(cam, new_objectPoints)
            a, b, c = ellipsoid.get_semi_axes_abc(cov_mat, 0.95)
            v = ellipsoid.ellipsoid_Volume(a, b, c)
            volumes.append(v)

    dvm.displayCovVolume_Zfixed3D(xInputs, yInputs, volumes)
def cam_distribution_study():
    """
    cam Distribution in 3D, show the cond num distribution for each cam position
    """
    number_of_points = 4
    # TODO Change the radius of circular plane
    pl = CircularPlane(origin=np.array([0., 0., 0.]), normal=np.array([0, 0, 1]), radius=0.15, n=4)
    pl.random(n=number_of_points, r=0.01, min_sep=0.01)
    plane_size = (0.3, 0.3)
    cam = Camera()
    cam.set_K(fx=800, fy=800, cx=640 / 2., cy=480 / 2.)
    cam.set_width_heigth(640, 480)
    # TODO cam distribution position PARAMETER CHANGE!!!
    # cams = create_cam_distribution(cam, plane_size,
    #                                theta_params=(0, 360, 30), phi_params=(0, 70, 10),
    #                                r_params=(0.2, 2.0, 20), plot=False)
    angle_begin = 0.0#0.0
    angle_end = 180.0#180.0
    angle_num = 37 #37 TODO need to set
    # angle_step = (angle_end - angle_begin) / (angle_num - 1)
    theta_params = (angle_begin,angle_end,angle_num)

    r_begin = 0.1 # we cant set this as 0.0 !!! avoid float error!!!
    r_end = 3.0
    r_num = 30 #30 TODO need to set
    # r_step = (r_end - r_begin) / (r_num - 1)
    r_params = (r_begin,r_end,r_num)

    cams,accuracy_mat = create_cam_distribution_in_YZ(cam=None, plane_size=(0.3, 0.3), theta_params=theta_params, r_params=r_params,
                                  plot=False)
    # TODO
    accuracy_mat = np.zeros([30, 60])# define the new acc_mat as 30 x 30 ,which means 3m x 3m area in real world and each cel
    inputX, inputY, inputZ, input_ippe1_t, input_ippe1_R, input_ippe2_t, input_ippe2_R, input_pnp_t, input_pnp_R, input_transfer_error, display_mat, accuracy_mat_new = bf.heightGetCondNum(
        cams, accuracy_mat, theta_params, r_params)
    # print "accuracy_mat distribution:\n",accuracy_mat_new
    print "----------------Start to show:-------------------"
    # print "accuracy_mat_new\n",accuracy_mat_new
    # save accuracy matrix in file
    print "-----accuracy_mat_new---------\n",accuracy_mat_new
    smtf.saveMatToFile(accuracy_mat_new)
    # plot condition number distribution
    # dc.displayCondNumDistribution(display_mat)
    # plot the Rt error
    # dc.displayError_YZ_plane_2D(inputY, inputZ, input_ippe1_t, input_ippe1_R, input_ippe2_t, input_ippe2_R, input_pnp_t, input_pnp_R)
    # dc.displayError_YZ_plane_3D(inputY, inputZ, input_ippe1_t, input_ippe1_R, input_ippe2_t, input_ippe2_R, input_pnp_t,
    #                           input_pnp_R, input_transfer_error)
    # dc.displayRTError3D_ippe12(inputY, inputZ, input_ippe1_t, input_ippe1_R, input_ippe2_t, input_ippe2_R, input_pnp_t,
    #                           input_pnp_R, input_transfer_error)
    # dc.displayRTError3D_LM(inputY, inputZ, input_ippe1_t, input_ippe1_R, input_ippe2_t, input_ippe2_R, input_pnp_t,
    #                           input_pnp_R, input_transfer_error)
    # dc.displayRTError3D_EPNP(inputY, inputZ, input_ippe1_t, input_ippe1_R, input_ippe2_t, input_ippe2_R, input_pnp_t,
    #                           input_pnp_R, input_transfer_error)
    dc.displayRTError3D_DLS(inputY, inputZ, input_ippe1_t, input_ippe1_R, input_ippe2_t, input_ippe2_R, input_pnp_t,
                              input_pnp_R, input_transfer_error)
    print "----------------End-----------------------------"
Example #11
0
class EyeRed(ImageSource):
    def __init__(self):
        super().__init__()
        self.camera = Camera()

    # main func
    def read(self):
        frame = self.camera.read()

        # cv.imshow('inda', frame)
        #frame = VisUtil.white_balance(frame)
        hsv = cv.cvtColor(frame, cv.COLOR_BGR2HSV)

        hsv = cv.medianBlur(hsv, 5)  # dealing with noise
        hsv = cv.GaussianBlur(hsv, (1, 1), 0)

        mask = VisUtil.get_red_mask(hsv)
        mask = VisUtil.noise_down(mask)

        (x, y, w,
         h), maxContour, contours = VisUtil.find_interesting_bound(mask)
        cropped = VisUtil.crop(frame, x, y, w, h)
        logging.debug("got red area " + str(w) + "x" + str(h))
        if cropped is not None:
            cropped = cv.cvtColor(cropped, cv.COLOR_BGR2GRAY)
            cv.rectangle(frame, (x, y), (x + w, y + h), (255, 128, 0), 2)
            cv.drawContours(frame, contours, -1, (0, 255, 0), 1)
            cv.drawContours(frame, [maxContour], -1, (255, 0, 0), 2)

            image_display.show(frame, 'in')
            image_display.show(cropped, 'cropped')
            key = image_display.wait_key()
            interactive_dataset_creator.process(key, cropped)
            return cropped
        else:
            return None
Example #12
0
            List of objects of the type Plane
    Returns
    -------
    None
    """
    # mlab.figure(figure=None, bgcolor=(0.1,0.5,0.5), fgcolor=None,
    #              engine=None, size=(400, 350))
    axis_scale = 0.05
    for cam in cams:
        plot3D_cam(cam, axis_scale)
    for fiducial_space in planes:
        # Plot plane points in 3D
        plotPoints3D(fiducial_space)


display_figure()
cam = Camera()
cam.set_K(fx=100, fy=100, cx=640, cy=480)
cam.set_width_heigth(1280, 960)
""" Initial camera pose looking straight down into the plane model """
cam.set_R_axisAngle(1.0, 0.0, 0.0, np.deg2rad(180))
cam.set_t(0.0, 0.0, 1.5, frame='world')
""" Plane for the control points """
sph = Sphere(2)
sph.set_color((1, 1, 0))
sph.random_points(3000, 0.3, 0.0001)
""" Plot camera axis and plane """
cams = [cam]
planes = [sph]
plot3D(cams, planes)
Example #13
0
import cv2
from networktables import NetworkTables

from vision.camera import Camera
import vision.finder as finder


camera = Camera(0, 10)

img=camera.get_image()

img = finder.findPoints(img)

cv2.imshow("image", img)
cv2.waitKey(0)

cv2.destroyAllWindows()

del(camera)

Example #14
0
    values
    """
    H = H_DLTwithfor(worldpoints, imagepoints, numberpoints, True)
    U, s, V = np.linalg.svd(H, full_matrices=False)
    print s
    greatest_singular_value = s[0]
    smallest_singular_value = s[11]
    # print greatest_singular_value, smallest_singular_value
    return greatest_singular_value/smallest_singular_value
   
def get_conditioning_number(A):
    return np.linalg.cond(A)   
 
    
# ---------- test -------------------------------------------
cam = Camera()


sph = Sphere()


cam.set_K(fx=800., fy=800., cx=640., cy=480.)
cam.set_width_heigth(1280, 960)
imagePoints = np.full((2, 6), 0.0)
cam.set_R_axisAngle(1.0,  0.0,  0.0, np.deg2rad(180.0))
cam.set_t(0.0, -0.0, 0.5, frame='world')
worldpoints = sph.random_points(6, 0.3)
imagePoints = cam.project(worldpoints, False)    

H = DLT3D(cam,worldpoints, imagePoints, True)
DLTimage = DLTproject(H, worldpoints)
Example #15
0
##########################################################################
#define the plots
#one Figure for image an object points
fig1 = plt.figure('Image and Object points')
ax_image = fig1.add_subplot(211)
ax_object = fig1.add_subplot(212)

## we create the gradient for the point distribution
normalize = False
n = 0.00000001  #condition number norm 4 points
n = 0.000001  #condition number norm 4 points
#n = 0.0000001 #condition number norm 5 points
gradient = gd.create_gradient(metric='condition_number', n=n)

## CREATE A SIMULATED CAMERA
cam = Camera()
cam.set_K(fx=800, fy=800, cx=640 / 2, cy=480 / 2)
cam.set_width_heigth(640, 480)

#Define a set of angles and a distance
r = 0.8
angles = np.arange(90, 91, 20)

#START OF THE MAIN LOOP

#List with the results for each camera pose
DataSim = []

for angle in angles:
    #Move the camera to the next pose
    x = r * np.cos(np.deg2rad(angle))
Example #16
0
 def __init__(self):
     super().__init__()
     self.camera = Camera()
from homographyHarker.homographyHarker import homographyHarker as hh
from solve_ippe import pose_ippe_both, pose_ippe_best
from solve_pnp import pose_pnp
import cv2

number_of_points = 4

if number_of_points == 4:
    import gdescent.hpoints_gradient as gd
elif number_of_points == 5:
    import gdescent.hpoints_gradient5 as gd
elif number_of_points == 6:
    import gdescent.hpoints_gradient6 as gd

## CREATE A SIMULATED CAMERA
cam = Camera()
cam.set_K(fx=800, fy=800, cx=640, cy=480)
cam.set_width_heigth(960, 960)

## DEFINE CAMERA POSE LOOKING STRAIGTH DOWN INTO THE PLANE MODEL
cam.set_R_axisAngle(1.0, 0.0, 0.0, np.deg2rad(180.0))
cam.set_t(0.0, -0.0, 0.5, frame='world')

#cam.set_R_axisAngle(1.0,  0.0,  0.0, np.deg2rad(140.0))
#cam.set_t(0.0,-1,1.0, frame='world')
#
#r = 0.5
#angle = 10
#x = r*np.cos(np.deg2rad(angle))
#z = r*np.sin(np.deg2rad(angle))
#cam.set_t(0, x,z)
def getCondNum_camPoseInRealWord(x_w, y_w, grid_reso, width, height):
    """
    Compute the condition number of camera position in real world coordinate
    :param x_w: camera potion in real world coordinate
    :param y_w: camera potion in real world coordinate
    :param width: gird 60
    :param height: grid 30
    :return:
    """
    # width = int(grid_width/ grid_reso)
    # height = int(grid_height/ grid_reso)
    T_WM = getMarkerTransformationMatrix(width, height, grid_reso)
    pos_world_homo = np.array([x_w, y_w, 0, 1])
    pos_marker = np.dot(T_WM, pos_world_homo)

    ## CREATE A SIMULATED CAMERA
    cam = Camera()
    cam.set_K(fx=800, fy=800, cx=640 / 2., cy=480 / 2.)
    cam.set_width_heigth(640, 480)

    cam.set_t(pos_marker[0], pos_marker[1], pos_marker[2], 'world')
    cam.set_R_mat(Rt_matrix_from_euler_t.R_matrix_from_euler_t(0.0, 0, 0))
    cam.look_at([0, 0, 0])

    radius = np.sqrt(pos_marker[0]**2 + pos_marker[1]**2 + pos_marker[2]**2)
    angle = np.rad2deg(np.arccos(pos_marker[1] / radius))
    cam.set_radius(radius)
    cam.set_angle(angle)
    objectPoints = np.copy(new_objectPoints)
    imagePoints = np.array(cam.project(objectPoints, False))

    condNum = np.inf  # undetected region set as 0
    if ((imagePoints[0, :] < cam.img_width) & (imagePoints[0, :] > 0) &
        (imagePoints[1, :] < cam.img_height) & (imagePoints[1, :] > 0)).all():
        input_list = gd.extract_objectpoints_vars(objectPoints)
        input_list.append(np.array(cam.K))
        input_list.append(np.array(cam.R))
        input_list.append(cam.t[0, 3])
        input_list.append(cam.t[1, 3])
        input_list.append(cam.t[2, 3])
        input_list.append(cam.radius)
        # TODO normalize points!!!   set normalize as default True
        condNum = gd.matrix_condition_number_autograd(*input_list,
                                                      normalize=True)

    return condNum
Example #19
0
# constants

SHOW_IMAGE = True
USE_VISION = True
CAMERA_WIDTH = 480.0
CAMERA_HEIGHT = 270.0
HEIGHT_DIFF = (21.75 - 13.25) * 0.0254  # meters

# functions

def connectionListener(conn, inf):
        print(inf, '; Connected=%s' % conn)


# init
camera = Camera(1, 10)
camera.set(3, CAMERA_WIDTH)
camera.set(4, CAMERA_HEIGHT)
camera.set(5, 20)

NetworkTable.setIPAddress("roboRIO-6731-FRC.local")
NetworkTable.setClientMode()
NetworkTable.initialize()

vtable = NetworkTable.getTable("vision")

NetworkTable.addConnectionListener(connectionListener, immediateNotify=True)

if SHOW_IMAGE :
        cv2.startWindowThread()
        cv2.namedWindow("image")
Example #20
0
calc_metrics = False
number_of_points = 5

if number_of_points == 4:
    import gdescent.hpoints_gradient as gd
elif number_of_points == 5:
    import gdescent.hpoints_gradient5 as gd
elif number_of_points == 6:
    import gdescent.hpoints_gradient6 as gd

## Define a Display plane with random initial points
pl = CircularPlane()
pl.random(n=number_of_points, r=0.01, min_sep=0.01)

## CREATE A SIMULATED CAMERA
cam = Camera()
cam.set_K(fx=800, fy=800, cx=640 / 2., cy=480 / 2.)
cam.set_width_heigth(640, 480)

## DEFINE A SET OF CAMERA POSES IN DIFFERENT POSITIONS BUT ALWAYS LOOKING
# TO THE CENTER OF THE PLANE MODEL

cam.set_R_axisAngle(1.0, 0.0, 0.0, np.deg2rad(180.0))
cam.set_t(0.0, -0.0, 0.5, frame='world')

r = 0.8
angle = 90
x = r * np.cos(np.deg2rad(angle))
z = r * np.sin(np.deg2rad(angle))
cam.set_t(0, x, z)
cam.set_R_mat(R_matrix_from_euler_t(0.0, 0, 0))
Example #21
0
import cv2
import time
from vision.camera import Camera
import vision.finder as finder

camera = Camera(0, 100)

#cv2.imshow("video", camera.get_image());

while 1:
    start_time = time.time()

    im = camera.capture()

    #	edg = cv2.Canny(im, 50, 250)

    p, a = finder.findPoints(im)

    #print(a)

    #cv2.imshow("video", p)

    if cv2.waitKey(1) > 0:
        break

    #print("frame time: %.3f" % (time.time() - start_time))
Example #22
0
class Client(object):
    cam = None
    cardCheckList = []
    fn = None
    staticImage = None
    pause = False

    def __init__(self, ip=None, camOff=False, staticImage=None):
            self.networkManager = NetworkManager(ip)
            self.cam = Camera(ip) if camOff != True and staticImage == None else None
            self.staticImage = SimpleCV.Image(staticImage) if staticImage != None else None



    def run(self):
        #create the 3 "mode object"
        #self.balloonFinder = BalloonFinder()
        self.cardFinder = CardFinder()
        self.fsm = OilStateMachine()


        while True:
            mode = self.networkManager.fetchMode()
            if self.staticImage == None:
                frame = self.cam.getImage() if self.cam != None else None
            else:
                frame = self.staticImage

            #switch-statement to select the mode (given by driver @ raspberry)
            if mode == 'idle' or self.pause == True:
                time.sleep(0.3);


            elif mode == CARD:
                cards =  self.cardFinder.findColorOrder(frame)

                if cards != None:
                    self.cardCheckList.append(cards)

                if len(self.cardCheckList) > 1 and all(x == self.cardCheckList[-1] for x in self.cardCheckList[-3:]):
                    cmd = json.dumps(cards)
                    self.networkManager.sendCommand(self.networkManager.H_CARDS + cmd)
                    print cmd




            elif mode == BALLOON:
                #initial state, only set when it has not been set yet (therefor, initial!)
                self.fn = BalloonFinder.startState(self.networkManager, frame, "blue") if self.fn == None else self.fn

                #trampoline!
                if callable(self.fn):
                    self.fn = self.fn(frame, "red")




            #the oilfinder state-machine does not return periodically, but handles a main loop itself
            #todo: make the oilFinder interuptable
            elif mode == OIL:
                self.fsm.run(startState, self.networkManager)
Example #23
0
    def __init__(self,
                 pitch,
                 color,
                 our_side,
                 video_port=0,
                 comm_port='/dev/ttyACM0',
                 comms=1):
        """
        Entry point for the SDP system.

        Params:
            [int] video_port                port number for the camera
            [string] comm_port              port number for the arduino
            [int] pitch                     0 - main pitch, 1 - secondary pitch
            [string] our_side               the side we're on - 'left' or 'right'
            *[int] port                     The camera port to take the feed from
            *[Robot_Controller] attacker    Robot controller object - Attacker Robot has a RED
                                            power wire
            *[Robot_Controller] defender    Robot controller object - Defender Robot has a YELLOW
                                            power wire
        """
        assert pitch in [0, 1]
        assert color in ['yellow', 'blue']
        assert our_side in ['left', 'right']

        self.pitch = pitch
        # Set up communications if thre are any
        try:
            self.robotComs = RobotCommunications(debug=False)
        except:
            print("arduino unplugged moving on to vision")

        # Set up robot communications to bet sent to planner.
        if self.USE_REAL_ROBOT:
            try:
                self.robotCom = RobotCommunications(debug=False)
            except:
                self.robotCom = TestCommunications(debug=True)
                print 'Not connected to the radio, using TestCommunications instead.'
        else:
            self.robotCom = TestCommunications(debug=True)

        # Set up main planner
        if(self.robotCom is not None):
            # currently we are assuming we are the defender
            self.planner = Planner(our_side=our_side,
                                   pitch_num=self.pitch,
                                   robotCom=self.robotCom,
                                   robotType='defender')

        # Set up camera for frames
        self.camera = Camera(port=video_port, pitch=self.pitch)
        frame = self.camera.get_frame()
        center_point = self.camera.get_adjusted_center(frame)

        # Set up vision
        self.calibration = tools.get_colors(pitch)
        print self.calibration
        self.vision = Vision(
            pitch=pitch,
            color=color,
            our_side=our_side,
            frame_shape=frame.shape,
            frame_center=center_point,
            calibration=self.calibration)

        # Set up postprocessing for vision
        self.postprocessing = Postprocessing()

        # Set up GUI
        self.GUI = GUI(calibration=self.calibration, pitch=self.pitch)

        self.color = color
        self.side = our_side

        self.preprocessing = Preprocessing()
Example #24
0
class Controller:
    """
    This class aims to be the bridge in between vision and strategy/logic
    """
    robotCom = None

    # Set to True if we want to use the real robot.
    # Set to False if we want to print out commands to console only.
    USE_REAL_ROBOT = True

    def __init__(self,
                 pitch,
                 color,
                 our_side,
                 video_port=0,
                 comm_port='/dev/ttyACM0',
                 comms=1):
        """
        Entry point for the SDP system.

        Params:
            [int] video_port                port number for the camera
            [string] comm_port              port number for the arduino
            [int] pitch                     0 - main pitch, 1 - secondary pitch
            [string] our_side               the side we're on - 'left' or 'right'
            *[int] port                     The camera port to take the feed from
            *[Robot_Controller] attacker    Robot controller object - Attacker Robot has a RED
                                            power wire
            *[Robot_Controller] defender    Robot controller object - Defender Robot has a YELLOW
                                            power wire
        """
        assert pitch in [0, 1]
        assert color in ['yellow', 'blue']
        assert our_side in ['left', 'right']

        self.pitch = pitch
        # Set up communications if thre are any
        try:
            self.robotComs = RobotCommunications(debug=False)
        except:
            print("arduino unplugged moving on to vision")

        # Set up robot communications to bet sent to planner.
        if self.USE_REAL_ROBOT:
            try:
                self.robotCom = RobotCommunications(debug=False)
            except:
                self.robotCom = TestCommunications(debug=True)
                print 'Not connected to the radio, using TestCommunications instead.'
        else:
            self.robotCom = TestCommunications(debug=True)

        # Set up main planner
        if(self.robotCom is not None):
            # currently we are assuming we are the defender
            self.planner = Planner(our_side=our_side,
                                   pitch_num=self.pitch,
                                   robotCom=self.robotCom,
                                   robotType='defender')

        # Set up camera for frames
        self.camera = Camera(port=video_port, pitch=self.pitch)
        frame = self.camera.get_frame()
        center_point = self.camera.get_adjusted_center(frame)

        # Set up vision
        self.calibration = tools.get_colors(pitch)
        print self.calibration
        self.vision = Vision(
            pitch=pitch,
            color=color,
            our_side=our_side,
            frame_shape=frame.shape,
            frame_center=center_point,
            calibration=self.calibration)

        # Set up postprocessing for vision
        self.postprocessing = Postprocessing()

        # Set up GUI
        self.GUI = GUI(calibration=self.calibration, pitch=self.pitch)

        self.color = color
        self.side = our_side

        self.preprocessing = Preprocessing()


    def main(self):
        """
        This main method  brings in to action the controller class which so far
        does nothing but set up the vision and its ouput
        """
        counter = 1L
        timer = time.clock()
        try:
            c = True
            while c != 27:  # the ESC key

                frame = self.camera.get_frame()
                pre_options = self.preprocessing.options
                # Apply preprocessing methods toggled in the UI
                preprocessed = self.preprocessing.run(frame, pre_options)
                frame = preprocessed['frame']
                if 'background_sub' in preprocessed:
                    cv2.imshow('bg sub', preprocessed['background_sub'])
                # Find object positions
                # model_positions have their y coordinate inverted

                #  IMPORTANT
                model_positions, regular_positions = self.vision.locate(frame)
                model_positions = self.postprocessing.analyze(model_positions)

                # Update planner world beliefs
                self.planner.update_world(model_positions)
                self.planner.plan()

                # Use 'y', 'b', 'r' to change color.
                c = waitKey(2) & 0xFF
                actions = []
                fps = float(counter) / (time.clock() - timer)
                # Draw vision content and actions

                self.GUI.draw(
                    frame, model_positions, actions, regular_positions, fps, None,
                    None, None, None, False,
                    our_color=self.color, our_side=self.side, key=c, preprocess=pre_options)
                counter += 1

        except:
            # This exception is stupid TODO: refactor.
            print("TODO SOMETHING CLEVER HERE")
            raise
        finally:
            tools.save_colors(self.pitch, self.calibration)
Example #25
0
Created on Wed May 10 11:15:22 2017

@author: lracuna
"""
from vision.camera import Camera
from vision.plane import Plane
from vision.screen import Screen

import numpy as np
import matplotlib.pyplot as plt
from mayavi import mlab
import cv2

#%% Create a camera

cam = Camera()
#Set camera intrinsic parameters
cam.set_K(794, 781, 640, 480)
cam.img_height = 1280
cam.img_width = 960

#%%
#Create 1 Screen
s1 = Screen()
s1.set_origin(np.array([0.0, 0, 0]))
s1.set_dimensions(0.50, 0.30)
s1.set_pixel_pitch(0.08)
red = (1, 0, 0)
s1.set_color(red)
s1.curvature_radius = 1.800
s1.update()
Example #26
0
import matplotlib.pyplot as plt
import scipy.io as sio
import error_functions as ef
from ippe import homo2d
from homographyHarker.homographyHarker import homographyHarker as hh
import hT_gradient as gd
from solve_ippe import pose_ippe_both
from solve_pnp import pose_pnp
import cv2
import math
import decimal

calc_metrics = False
number_of_points = 4
## CREATE A SIMULATED CAMERA
cam = Camera()
cam.set_K(fx=800, fy=800, cx=640 / 2., cy=480 / 2.)
cam.set_width_heigth(640, 480)
## CREATE A SET OF IMAGE POINTS FOR VALIDATION OF THE HOMOGRAPHY ESTIMATION
# This will create a grid of 16 points of size = (0.3,0.3) meters
validation_plane = Plane(origin=np.array([0, 0, 0]),
                         normal=np.array([0, 0, 1]),
                         size=(0.3, 0.3),
                         n=(4, 4))
validation_plane.uniform()
# ---------This plane should be same to the plane in cam distribution!!!!-----------------------
plane_size = (0.3, 0.3)
plane = Plane(origin=np.array([0, 0, 0]),
              normal=np.array([0, 0, 1]),
              size=plane_size,
              n=(2, 2))
Example #27
0
    import gdescent.hpoints_gradient as gd
elif number_of_points == 5:
    import gdescent.hpoints_gradient5 as gd
elif number_of_points == 6:
    import gdescent.hpoints_gradient6 as gd
elif number_of_points == 7:
    import gdescent.hpoints_gradient7 as gd
elif number_of_points == 8:
    import gdescent.hpoints_gradient8 as gd

## Define a Display plane with random initial points
pl = CircularPlane()
pl.random(n =number_of_points, r = 0.01, min_sep = 0.01)

## CREATE A SIMULATED CAMERA
cam = Camera()
cam.set_K(fx = 800,fy = 800,cx = 640/2.,cy = 480/2.)
cam.set_width_heigth(640,480)

## DEFINE A SET OF CAMERA POSES IN DIFFERENT POSITIONS BUT ALWAYS LOOKING
# TO THE CENTER OF THE PLANE MODEL


cam.set_R_axisAngle(1.0,  0.0,  0.0, np.deg2rad(180.0))
cam.set_t(0.0,-0.0,0.5, frame='world')

r = 0.8
angle = 90
x = r*np.cos(np.deg2rad(angle))
z = r*np.sin(np.deg2rad(angle))
cam.set_t(0, x,z)
Example #28
0
Created on Wed May 10 11:15:22 2017

@author: lracuna
"""
from vision.camera import Camera
from vision.plane import Plane
from vision.screen import Screen

import numpy as np
import matplotlib.pyplot as plt
from mayavi import mlab
import cv2

#%% Create a camera

cam = Camera()
#Set camera intrinsic parameters
cam.set_K(794, 781, 640, 480)
cam.img_height = 1280
cam.img_width = 960

#%%
#Create 1 Screen
s1 = Screen()
s1.set_origin(np.array([0.0, 0, 0]))
s1.set_dimensions(0.50, 0.30)
s1.set_pixel_pitch(0.02)
red = (1, 0, 0)
s1.set_color(red)
s1.curvature_radius = 1.800
s1.update()
Example #29
0
from vision.camera import Camera
from color_detection import ColorDetection

import cv2 as cv
import rospy
import time

lower_threshold = (17, 166, 36)
upper_threshold = (23, 255, 51)

if __name__ == "__main__":
    try:
        rospy.init_node("color_detection")
        rate = rospy.Rate(15) # 15 FPS

        cap = Camera(port=5600)
        cd = ColorDetection(lower_threshold, upper_threshold)

        print("Wait for camera capture..")
        frame = cap.capture()
        while frame is None and not rospy.is_shutdown():
            rate.sleep()
            frame = cap.capture()
        print("Frame captured!")
        
        fps = 15.0
        t = time.time()

        while not rospy.is_shutdown():
            frame = cap.capture()
            mask = cd.update(frame)
def Z_fixed_study_square():
    cams_HeightFixed = []
    cam_1 = Camera()
    cam_1.set_K(fx=800, fy=800, cx=640 / 2., cy=480 / 2.)
    cam_1.set_width_heigth(640, 480)
    cam_1.set_R_axisAngle(1.0, 0.0, 0.0, np.deg2rad(180.0))
    cam_1.set_t(0.1, 0.1, 1, frame='world')
    cams_HeightFixed.append(cam_1)

    cam_2 = Camera()
    cam_2.set_K(fx=800, fy=800, cx=640 / 2., cy=480 / 2.)
    cam_2.set_width_heigth(640, 480)
    cam_2.set_R_axisAngle(1.0, 0.0, 0.0, np.deg2rad(180.0))
    cam_2.set_t(-0.1, 0.1, 1, frame='world')
    # 0.28075725, -0.23558331, 1.31660688
    cams_HeightFixed.append(cam_2)

    cam_3 = Camera()
    cam_3.set_K(fx=800, fy=800, cx=640 / 2., cy=480 / 2.)
    cam_3.set_width_heigth(640, 480)
    cam_3.set_R_axisAngle(1.0, 0.0, 0.0, np.deg2rad(180.0))
    cam_3.set_t(-0.1, -0.1, 1, frame='world')
    # 0.28075725, -0.23558331, 1.31660688
    cams_HeightFixed.append(cam_3)

    cam_4 = Camera()
    cam_4.set_K(fx=800, fy=800, cx=640 / 2., cy=480 / 2.)
    cam_4.set_width_heigth(640, 480)
    cam_4.set_R_axisAngle(1.0, 0.0, 0.0, np.deg2rad(180.0))
    cam_4.set_t(0.1, -0.1, 1, frame='world')
    # 0.28075725, -0.23558331, 1.31660688
    cams_HeightFixed.append(cam_4)
    inputX, inputY, inputZ, input_ippe1_t, input_ippe1_R, input_ippe2_t, input_ippe2_R, input_pnp_t, input_pnp_R, input_transfer_error, display_mat = bf.heightGetCondNum(cams_HeightFixed)
Example #31
0
 def __init__(self, ip=None, camOff=False, staticImage=None):
         self.networkManager = NetworkManager(ip)
         self.cam = Camera(ip) if camOff != True and staticImage == None else None
         self.staticImage = SimpleCV.Image(staticImage) if staticImage != None else None
Example #32
0
class OptimalPointsSim(object):
    """ Class that defines and optimization to obtain optimal control points
  configurations for homography and plannar pose estimation. """
    def __init__(self):
        """ Definition of a simulated camera """
        self.cam = Camera()
        self.cam.set_K(fx=100, fy=100, cx=640, cy=480)
        self.cam.set_width_heigth(1280, 960)
        """ Initial camera pose looking stratight down into the plane model """
        self.cam.set_R_axisAngle(1.0, 0.0, 0.0, np.deg2rad(180))
        self.cam.set_t(0.0, 0.0, 1.5, frame='world')
        """ Plane for the control points """
        self.sph = Sphere(radius=0.5)
        self.sph.random_points(p=6, r=0.5, min_dist=0.001)

    def run(self):
        self.objectPoints = self.sph.get_sphere_points()
        self.init_params = flatten_points(self.objectPoints, type='object')

        self.objective1 = lambda params: matrix_condition_number_autograd(
            params, self.cam.P, normalize=False)
        self.objective2 = lambda params, iter: matrix_condition_number_autograd(
            params, self.cam.P, normalize=True)

        print("Optimizing condition number...")
        objective_grad = grad(self.objective2)
        self.optimized_params = adam(objective_grad,
                                     self.init_params,
                                     step_size=0.001,
                                     num_iters=200,
                                     callback=self.plot_points)

    def plot_points(self, params, iter, gradient):
        phisim = np.linspace((-math.pi) / 2., (math.pi / 2.))
        thetasim = np.linspace(0, 2 * np.pi)
        print params
        pointscoord = np.full((3, 6), 0.0)

        for i in range(6):

            pointscoord[0, i] = params[i]
            pointscoord[1, i] = params[i + 1]
            pointscoord[2, i] = params[i + 2]

        x = np.outer(np.sin(thetasim), np.cos(phisim))
        y = np.outer(np.sin(thetasim), np.sin(phisim))
        z = np.outer(np.cos(thetasim), np.ones_like(phisim))
        fig, ax = plt.subplots(subplot_kw={'projection': '3d'})
        ax.plot_wireframe(sph.radius * x,
                          sph.radius * y,
                          sph.radius * z,
                          color='g')
        ax.scatter(pointscoord[:3, 0],
                   pointscoord[:3, 1],
                   pointscoord[:3, 2],
                   c='r')
        ax.scatter(pointscoord[:3, 3],
                   pointscoord[:3, 4],
                   pointscoord[:3, 5],
                   c='r')
        plt.show()
Example #33
0
calc_metrics = True
number_of_points = 4

if number_of_points == 4:
    import gdescent.hpoints_gradient as gd
elif number_of_points == 5:
    import gdescent.hpoints_gradient5 as gd
elif number_of_points == 6:
    import gdescent.hpoints_gradient6 as gd

## Define a Display plane with random initial points
pl = CircularPlane()
pl.random(n=number_of_points, r=0.01, min_sep=0.01)

## CREATE A SIMULATED CAMERA
cam = Camera()
cam.set_K(fx=800, fy=800, cx=640 / 2., cy=480 / 2.)
cam.set_width_heigth(640, 480)

## DEFINE CAMERA POSE LOOKING STRAIGTH DOWN INTO THE PLANE MODEL
cam.set_R_axisAngle(1.0, 0.0, 0.0, np.deg2rad(180.0))
cam.set_t(0.0, -0.0, 0.5, frame='world')

#cam.set_R_axisAngle(1.0,  0.0,  0.0, np.deg2rad(140.0))
#cam.set_t(0.0,-1,1.0, frame='world')
#
r = 0.8
angle = 30
x = r * np.cos(np.deg2rad(angle))
z = r * np.sin(np.deg2rad(angle))
cam.set_t(0, x, z)
def getT_MC_and_Rt_errors(T_WM, pos_world, Rmat_error_loop, tvec_error_loop):
    pos_world_homo = np.array([pos_world[0], pos_world[1], 0, 1])
    pos_marker = np.dot(T_WM, pos_world_homo)
    # print "pos_marker\n", pos_marker

    # Create an initial camera on the center of the world
    cam = Camera()
    f = 800
    cam.set_K(fx=f, fy=f, cx=320, cy=240)  # Camera Matrix
    cam.img_width = 320 * 2
    cam.img_height = 240 * 2
    cam = cam.clone()
    cam.set_t(pos_marker[0], pos_marker[1], pos_marker[2], 'world')
    cam.set_R_mat(Rt_matrix_from_euler_t.R_matrix_from_euler_t(0.0, 0, 0))
    # print "cam.R\n",cam.R
    cam.look_at([0, 0, 0])
    # print "cam.Rt after look at\n",cam.R
    objectPoints = np.copy(new_objectPoints)
    imagePoints = np.array(cam.project(objectPoints, False))
    new_imagePoints = np.copy(imagePoints)
    # -----------------------------------------------------------------
    new_imagePoints_noisy = cam.addnoise_imagePoints(new_imagePoints,
                                                     mean=0,
                                                     sd=4)
    # print "new_imagePoints_noisy\n",new_imagePoints_noisy
    debug = False
    # TODO  cv2.SOLVEPNP_DLS, cv2.SOLVEPNP_EPNP, cv2.SOLVEPNP_ITERATIVE
    pnp_tvec, pnp_rmat = pose_pnp(new_objectPoints, new_imagePoints_noisy,
                                  cam.K, debug, cv2.SOLVEPNP_ITERATIVE, False)

    # Calculate errors
    Cam_clone_cv2 = cam.clone_withPose(pnp_tvec, pnp_rmat)
    tvec_error, Rmat_error = ef.calc_estimated_pose_error(
        cam.get_tvec(), cam.R, Cam_clone_cv2.get_tvec(), pnp_rmat)
    Rmat_error_loop.append(Rmat_error)
    tvec_error_loop.append(tvec_error)

    # print "cam.get_world_position()\n",cam.get_world_position()
    t = np.eye(4)
    t[:3, 3] = pnp_tvec[:3]
    T_MC = np.dot(t, pnp_rmat)
    return T_MC