Esempio n. 1
0
    def __init__(self):

        (intrinsic_matrix,distortion_coeffs) = CameraParameters.intrinsic("undistorted")
        (rvec,tvec) = CameraParameters.extrinsic("plate")
        intrinsic_matrix = cvNumpy.mat_to_array(intrinsic_matrix)
        rvec = cvNumpy.mat_to_array(rvec).squeeze()
        tvec = cvNumpy.mat_to_array(tvec).squeeze()

        # Listener/Subscribers
        self.tf_listener = tf.TransformListener()
        # self.Hinv = numpy.array([\
        #     [2.02, 7.57e-3, 3.18e2],
        #     [2.24e-3, -2, 2.40e2],
        #     [1.6e-5,2.56e-5,1]])

        # alpha, beta, gamma = 0.123, -1.234, 2.345
        # origin, xaxis, yaxis, zaxis = (0, 0, 0), (1, 0, 0), (0, 1, 0), (0, 0, 1)
        # I = tf.transformations.identity_matrix()
        # Rx = tf.transformations.rotation_matrix(alpha, xaxis)
        # rospy.logwarn("Rx = %s", str(Rx))
        rvec_angle = numpy.linalg.norm(rvec)
        R = tf.transformations.rotation_matrix(rvec_angle,rvec)
        T = tf.transformations.translation_matrix(tvec)
        # rospy.logwarn("R = \n%s", str(R))
        # rospy.logwarn("T = \n%s", str(T))
        Wsub = numpy.zeros((3,3))
        Wsub[:,:-1] = R[:-1,:-2]
        Wsub[:,-1] = T[:-1,-1]
        # rospy.logwarn("Wsub = \n%s", str(Wsub))
        # M = intrinsic_matrix
        # rospy.logwarn("M = \n%s", str(M))
        Hinv = numpy.dot(M,Wsub)
        Hinv = Hinv/Hinv[-1,-1]
        # rospy.logwarn("Hinv = \n%s", str(Hinv))
        R = numpy.zeros((4,4))
        R[-1,-1] = 1
        R[-2,-2] = 1
        R[:2,:2] = Hinv[:2,:2]
        # rospy.logwarn("R = \n%s", str(R))
        T = Hinv[:-1,-1]
        # rospy.logwarn("T = \n%s", str(T))
        # q = tf.transformations.quaternion_from_matrix(R)
        # rospy.logwarn("q = \n%s", str(q))
        H = numpy.linalg.inv(Hinv)
        self.Hinv = Hinv
        self.H = H
Esempio n. 2
0
    def CameraInfo_callback (self, msgCameraInfo):
        if self.camerainfo is None:
            self.camerainfo = msgCameraInfo
            self.M = N.reshape(N.array(self.camerainfo.K),[3,3])
            # Makes with respect to Camera coordinate system instead of ImageRect
            self.M[:-1,-1] = 0

            (self.rvec, self.tvec) = CameraParameters.extrinsic("plate")
Esempio n. 3
0
  def __init__(self):
    self.initialized = False
    self.images_initialized = False
    self.undistorted_saved = False
    self.distorted_saved = False
    cv.NamedWindow("Presentation", 1)
    self.tf_listener = tf.TransformListener()
    self.tf_broadcaster = tf.TransformBroadcaster()
    self.bridge = CvBridge()
    self.image_undistorted_sub = rospy.Subscriber("UndistortedImage", Image, self.image_undistorted_callback)
    self.image_undistorted_sub = rospy.Subscriber("OriginalImage", Image, self.image_distorted_callback)
    self.joy_sub = rospy.Subscriber("Joystick/Commands", JoystickCommands, self.joy_callback)
    self.color_max = 255
    self.font = cv.InitFont(cv.CV_FONT_HERSHEY_TRIPLEX,0.5,0.5)
    self.font_color = cv.CV_RGB(self.color_max,0,0)
    self.camera_plate = PointStamped()
    self.camera_plate.header.frame_id = "Camera"
    self.camera_origin = PointStamped()
    self.camera_origin.header.frame_id = "Camera"
    self.camera_origin.point.x = 0
    self.camera_origin.point.y = 0
    # self.image_plate_origin_found = False
    # self.plate_origin = PointStamped()
    # self.plate_origin.header.frame_id = "Plate"
    # self.plate_origin.point.x = 0
    # self.plate_origin.point.y = 0

    (self.intrinsic_matrix,self.distortion_coeffs) = CameraParameters.intrinsic("undistorted")
    self.KK_cx = self.intrinsic_matrix[0,2]
    self.KK_cy = self.intrinsic_matrix[1,2]

    self.M = cvNumpy.mat_to_array(self.intrinsic_matrix)
    # Makes with respect to Camera coordinate system instead of Undistorted
    self.M[:-1,-1] = 0

    # Pattern info
    self.pattern_size = (8,6)
    self.col_corner_number = self.pattern_size[0]
    self.row_corner_number = self.pattern_size[1]
    self.board_corner_number = self.col_corner_number * self.row_corner_number
    self.checker_size = 15
    self.win = (4,4)
    self.zero_zone = (2,2)
    self.criteria = (cv.CV_TERMCRIT_ITER+cv.CV_TERMCRIT_EPS,100,.01)

    self.image_points = cv.CreateMat(self.board_corner_number,2,cv.CV_32FC1)
    self.plate_points = cv.CreateMat(self.board_corner_number,3,cv.CV_32FC1)
    self.point_counts = cv.CreateMat(1,1,cv.CV_32SC1)
    self.rvec = cv.CreateMat(1,3,cv.CV_32FC1)
    self.tvec = cv.CreateMat(1,3,cv.CV_32FC1)

    self.origin_points = cv.CreateMat(4,3,cv.CV_32FC1)
    self.origin_points_projected = cv.CreateMat(4,2,cv.CV_32FC1)

    self.rvec_zeros = cv.SetZero(cv.CreateMat(1,3,cv.CV_32FC1))
    self.tvec_zeros = cv.SetZero(cv.CreateMat(1,3,cv.CV_32FC1))

    self.initialized = True
Esempio n. 4
0
    def __init__(self):
        (intrinsic_matrix,distortion_coeffs) = CameraParameters.intrinsic("undistorted")
        (rvec,tvec) = CameraParameters.extrinsic("plate")
        intrinsic_matrix = cvNumpy.mat_to_array(intrinsic_matrix)
        rvec = cvNumpy.mat_to_array(rvec).squeeze()
        tvec = cvNumpy.mat_to_array(tvec).squeeze()

        rvec_angle = numpy.linalg.norm(rvec)
        R = tf.transformations.rotation_matrix(rvec_angle,rvec)
        T = tf.transformations.translation_matrix(tvec)

        Wsub = numpy.zeros((3,3))
        Wsub[:,:-1] = R[:-1,:-2]
        Wsub[:,-1] = T[:-1,-1]

        M = intrinsic_matrix
        # Makes with respect to Camera coordinate system instead of Undistorted
        M[:-1,-1] = 0

        Hinv = numpy.dot(M,Wsub)
        Hinv = Hinv/Hinv[-1,-1]
        H = numpy.linalg.inv(Hinv)
        self.Hinv = Hinv
        self.H = H
 def CameraInfo_callback (self, msgCameraInfo):
     if self.camerainfo is None:
         self.camerainfo = msgCameraInfo
         M = N.reshape(N.array(self.camerainfo.K),[3,3]) #cvNumpy.mat_to_array(N.array(self.camerainfo.K))
         #M = N.reshape(N.array(self.camerainfo.P),[3,4])[0:3,0:3]
         M[:-1,-1] = 0  # Zero the translation entries (1,3) and (2,3).
 
         (rvec, tvec) = CameraParameters.extrinsic("plate")
         rvec = cvNumpy.mat_to_array(rvec).squeeze()
         tvec = cvNumpy.mat_to_array(tvec).squeeze()
 
         angleRvec = N.linalg.norm(rvec)
         R = tf.transformations.rotation_matrix(angleRvec, rvec)
         T = tf.transformations.translation_matrix(tvec)
 
         self.Wsub = N.zeros((3,3))
         self.Wsub[:,:-1] = R[:-1,:-2]
         self.Wsub[:,-1] = T[:-1,-1]
 
         self.Hinv = N.dot(M, self.Wsub)
         #rospy.logwarn ('Hinv scalar %f' % self.Hinv[-1,-1])
         self.Hinv = self.Hinv / self.Hinv[-1,-1]
         self.H = N.linalg.inv(self.Hinv)
Esempio n. 6
0
  def __init__(self):
    self.initialized = False
    self.images_initialized = False
    self.pose_initialized = False
    self.arrays_initialized = False

    self.joy_sub = rospy.Subscriber("Joystick/Commands", JoystickCommands, self.commands_callback)
    self.vel_pub = rospy.Publisher("stage/command_velocity",Velocity)

    self.image_sub = rospy.Subscriber("UndistortedImage", Image, self.image_callback)
    self.contour_info_sub = rospy.Subscriber("ContourInfo",ContourInfo,self.contour_callback)
    # self.pose_sub = rospy.Subscriber("RobotImagePose", PoseStamped, self.pose_callback)

    cv.NamedWindow("Stage Plate Calibration", 1)

    self.vel_stage = Velocity()
    self.vel_scale_factor = 40     # mm/s

    self.robot_image_pose_camera = PoseStamped()
    self.robot_image_pose_undistorted = PoseStamped()
    self.robot_image_pose_plate = PoseStamped()

    self.bridge = CvBridge()
    self.color_max = 255
    self.font = cv.InitFont(cv.CV_FONT_HERSHEY_TRIPLEX,0.5,0.5)
    self.font_color = cv.CV_RGB(self.color_max,0,0)

    self.sample_dist_min = 20

    self.point_count_min = 10
    self.dist_min = 25

    self.error_text = ""
    self.robot_ecc = 0
    self.robot_area = 0
    self.robot_ecc_array = numpy.array([])
    self.robot_area_array = numpy.array([])
    self.robot_min_ecc = 1000000000
    self.robot_max_ecc = 0
    self.robot_min_area = 1000000000
    self.robot_max_area = 0

    # self.plate_point_array = numpy.zeros((1,3))
    # self.stage_point_array = numpy.zeros((1,3))
    (self.intrinsic_matrix,self.distortion_coeffs) = CameraParameters.intrinsic("undistorted")
    (self.rvec,self.tvec) = CameraParameters.extrinsic("plate")
    self.origin_points = cv.CreateMat(4,3,cv.CV_32FC1)
    self.origin_points_projected = cv.CreateMat(4,2,cv.CV_32FC1)
    self.checker_size = 15
    self.num_grid_lines = 9
    self.start_points_projected = cv.CreateMat(self.num_grid_lines,2,cv.CV_32FC1)
    self.end_points_projected = cv.CreateMat(self.num_grid_lines,2,cv.CV_32FC1)
    self.rotate_grid = False

    self.tf_listener = tf.TransformListener()
    got_trans = False
    while not got_trans:
      try:
        (self.camera_undistorted_trans,rot) = self.tf_listener.lookupTransform('/UndistortedImage', '/Camera', rospy.Time(0))
        got_trans = True
      except:
        rospy.logdebug("tf_listener.lookupTransform threw exception \n")

    rospy.wait_for_service('camera_to_plate')
    try:
        self.camera_to_plate = rospy.ServiceProxy('camera_to_plate', PlateCameraConversion)
    except rospy.ServiceException, e:
        print "Service call failed: %s"%e