コード例 #1
0
ファイル: nd_profile.py プロジェクト: NoemiOtero-Aimen/etna
class NdProfile():
    def __init__(self):
        rospy.init_node('pub_profile3d', anonymous=True)

        image_topic = rospy.get_param('~image', '/camera/image')
        cloud_topic = rospy.get_param('~cloud', '/camera/cloud')
        config_file = rospy.get_param('~config', 'profile3d.yaml')

        #peaks_topic = rospy.get_param('~peaks', '/camera/peaks')
        #self.image_pub = rospy.Publisher(peaks_topic, Image, queue_size=10)

        rospy.Subscriber(
            image_topic, Image, self.sub_image_topic, queue_size=1)

        self.sequence = 0
        self.bridge = CvBridge()
        self.pcloud = PointCloud2()

        self.cloud_pub = rospy.Publisher(
            cloud_topic, PointCloud2, queue_size=10)

        self.profile = Profile()
        self.profile.load_configuration(config_file)

        rospy.spin()

    def pub_point_cloud(self, stamp, profile3d):
        self.sequence = self.sequence + 1
        self.pcloud = pc2.create_cloud_xyz32(self.pcloud.header, profile3d)
        self.pcloud.header = std_msgs.msg.Header(frame_id="/camera0",
                                                 stamp=stamp,
                                                 seq=self.sequence)
        self.cloud_pub.publish(self.pcloud)

    def image_pub_peak(self, stamp, image):
        image_msg = self.bridge.cv2_to_imgmsg(image, encoding='bgr8')
        image_msg.header.stamp = stamp
        self.image_pub.publish(image_msg)

    def sub_image_topic(self, data):
        try:
            stamp = rospy.Time.now()
            image = self.bridge.imgmsg_to_cv2(data)

            #rospy.loginfo(stamp)
            #stamp = data.header.stamp
            #rospy.loginfo(stamp)
            profile3d, profile2d = self.profile.points_profile(image)
            if len(profile3d) > 0:
                self.pub_point_cloud(stamp, profile3d)
            #print profile3d
            #if data.encoding == 'mono8':
            #    image = cv2.cvtColor(image, cv2.COLOR_GRAY2BGR)
            # if len(profile3d) > 0:
            #     image = self.profile.draw_points(image, profile2d,
            #                                      color=(0, 0, 255),
            #                                      thickness=2)
            #self.image_pub_peak(stamp, image)
        except CvBridgeError, e:
            print e
コード例 #2
0
    def __init__(self):
        rospy.init_node('pub_profile3d', anonymous=True)

        image_topic = rospy.get_param('~image', '/camera/image')
        cloud_topic = rospy.get_param('~cloud', '/camera/cloud')
        config_file = rospy.get_param('~config', 'profile3d.yaml')

        #peaks_topic = rospy.get_param('~peaks', '/camera/peaks')
        #self.image_pub = rospy.Publisher(peaks_topic, Image, queue_size=10)

        rospy.Subscriber(image_topic,
                         Image,
                         self.sub_image_topic,
                         queue_size=1)

        self.sequence = 0
        self.bridge = CvBridge()
        self.pcloud = PointCloud2()

        self.cloud_pub = rospy.Publisher(cloud_topic,
                                         PointCloud2,
                                         queue_size=10)

        self.profile = Profile()
        self.profile.load_configuration(config_file)

        rospy.spin()
コード例 #3
0
ファイル: nd_profile.py プロジェクト: openlmd/etna
class NdProfile():
    def __init__(self):
        rospy.init_node('pub_profile3d', anonymous=True)

        image_topic = rospy.get_param('~image', '/camera/image')
        cloud_topic = rospy.get_param('~cloud', '/camera/cloud')
        config_file = rospy.get_param('~config', 'profile3d.yaml')

        #peaks_topic = rospy.get_param('~peaks', '/camera/peaks')
        #self.image_pub = rospy.Publisher(peaks_topic, Image, queue_size=10)

        rospy.Subscriber(image_topic,
                         Image,
                         self.sub_image_topic,
                         queue_size=1)

        self.sequence = 0
        self.bridge = CvBridge()
        self.pcloud = PointCloud2()

        self.cloud_pub = rospy.Publisher(cloud_topic,
                                         PointCloud2,
                                         queue_size=10)

        self.profile = Profile()
        self.profile.load_configuration(config_file)

        rospy.spin()

    def pub_point_cloud(self, stamp, profile3d):
        self.sequence = self.sequence + 1
        self.pcloud = pc2.create_cloud_xyz32(self.pcloud.header, profile3d)
        self.pcloud.header = std_msgs.msg.Header(frame_id="/camera0",
                                                 stamp=stamp,
                                                 seq=self.sequence)
        self.cloud_pub.publish(self.pcloud)

    def image_pub_peak(self, stamp, image):
        image_msg = self.bridge.cv2_to_imgmsg(image, encoding='bgr8')
        image_msg.header.stamp = stamp
        self.image_pub.publish(image_msg)

    def sub_image_topic(self, data):
        try:
            stamp = rospy.Time.now()
            image = self.bridge.imgmsg_to_cv2(data)
            #rospy.loginfo(stamp)
            #stamp = data.header.stamp
            #rospy.loginfo(stamp)
            profile3d, profile2d = self.profile.points_profile(image)
            if len(profile3d) > 0:
                self.pub_point_cloud(stamp, profile3d)
        except CvBridgeError, e:
            rospy.loginfo("CvBridge Exception")
コード例 #4
0
ファイル: sub_viewer.py プロジェクト: openlmd/etna
    def __init__(self):
        rospy.init_node('viewer', anonymous=True)

        image_topic = rospy.get_param('~image', '/ueye/image')
        # Pattern parameters
        pattern_rows = rospy.get_param('~pattern_rows', 7)
        pattern_cols = rospy.get_param('~pattern_cols', 8)
        pattern_size = rospy.get_param('~pattern_size', 10)

        config_file = rospy.get_param('~config', 'profile3d.yaml')

        rospy.Subscriber(image_topic, Image, self.callback, queue_size=10)
        rospy.on_shutdown(self.on_shutdown_hook)

        self.counter = 0
        self.bridge = CvBridge()
        self.listener = tf.TransformListener()

        self.square_size = pattern_size
        self.grid_size = (pattern_cols - 1, pattern_rows - 1)

        self.laser_profile = Profile(axis=1, thr=180, method='pcog')
        self.camera_calibration = CameraCalibration(
            grid_size=self.grid_size, square_size=self.square_size)

        cv2.namedWindow('viewer')
        cv2.cv.SetMouseCallback('viewer', self.on_mouse, '')

        rospy.spin()
コード例 #5
0
ファイル: pub_profile3d.py プロジェクト: JCostas-AIMEN/etna
    def __init__(self):
        rospy.init_node('pub_profile3d', anonymous=True)

        image_topic = rospy.get_param('~image', '/camera/image')
        cloud_topic = rospy.get_param('~cloud', '/camera/cloud')
        config_file = rospy.get_param('~config', 'profile3d.yaml')

        #peaks_topic = rospy.get_param('~peaks', '/camera/peaks')
        #self.image_pub = rospy.Publisher(peaks_topic, Image, queue_size=5)

        rospy.Subscriber(image_topic, Image, self.sub_image_topic,
                         queue_size=1)

        self.sequence = 0
        self.bridge = CvBridge()
        self.pcloud = PointCloud2()

        self.cloud_pub = rospy.Publisher(cloud_topic, PointCloud2,
                                         queue_size=5)

        self.profile = Profile()
        path = rospkg.RosPack().get_path('etna_scanning')
        self.profile.load_configuration(os.path.join(path, 'config',
                                                     config_file))

        rospy.spin()
コード例 #6
0
np.set_printoptions(precision=4, suppress=True)

path = '../'
pattern_rows = 7
pattern_cols = 8
pattern_size = 0.010
config_file = 'profile3d.yaml'

dirname = '../data'
images, tool_poses = read_calibration_data(dirname)

square_size = pattern_size
grid_size = (pattern_cols - 1, pattern_rows - 1)

laser_profile = Profile(axis=1, thr=180, method='pcog')
camera_calibration = CameraCalibration(grid_size=grid_size,
                                       square_size=square_size)
laser_calibration = LaserCalibration(grid_size=grid_size,
                                     square_size=square_size,
                                     profile=laser_profile)
laser_calibration.find_calibration_3d(images)
laser_calibration.save_parameters(os.path.join(path, 'config', config_file))

filenames = sorted(glob.glob('../data/pose*.txt'))
ks = [int(filename[-8:-4]) for filename in filenames]
poses_checker, poses_tool = [], []
poses_ichecker, poses_itool = [], []
for k in ks:
    print 'Frame: %i' % k
    img = read_image('../data/frame%04i.png' % k)
コード例 #7
0
 def __init__(self, grid_size=(7, 6), square_size=10.0, profile=Profile()):
     CameraCalibration.__init__(self,
                                grid_size=grid_size,
                                square_size=square_size)
     self.camera_pose = (np.eye(3), np.zeros(3))
     self.profile = profile
コード例 #8
0
def read_calibration_data(dirname):
    frame_filenames = sorted(glob.glob(os.path.join(dirname, 'frame0*.png')))
    pose_filenames = sorted(glob.glob(os.path.join(dirname, 'pose0*.txt')))
    images = [read_image(filename) for filename in frame_filenames]
    tool_poses = [read_pose(filename) for filename in pose_filenames]
    return images, tool_poses


if __name__ == '__main__':
    from mlabplot import MPlot3D

    dirname = '../../data'
    images, tool_poses = read_calibration_data(dirname)

    laser_profile = Profile(axis=1, thr=180, method='pcog')
    laser_calibration = LaserCalibration(grid_size=(7, 6),
                                         square_size=0.010,
                                         profile=laser_profile)
    laser_calibration.find_calibration_3d(images)

    pattern_poses = laser_calibration.pattern_poses
    profiles = [laser_profile.profile_points(img) for img in images]
    lines = [
        laser_calibration.find_best_line2d(profile2d) for profile2d in profiles
    ]

    for k, img in enumerate(images):
        grid = laser_calibration.grids[k]
        imgc = laser_calibration.draw_chessboard(img.copy(), grid)
        pose = laser_calibration.get_chessboard_pose(grid)
コード例 #9
0
ファイル: calibration.py プロジェクト: NoemiOtero-Aimen/etna
def read_calibration_data(dirname):
    frame_filenames = sorted(glob.glob(os.path.join(dirname, 'frame0*.png')))
    pose_filenames = sorted(glob.glob(os.path.join(dirname, 'pose0*.txt')))
    images = [read_image(filename) for filename in frame_filenames]
    tool_poses = [read_pose(filename) for filename in pose_filenames]
    return images, tool_poses


if __name__ == '__main__':
    from mlabplot import MPlot3D

    dirname = '../../data'
    images, tool_poses = read_calibration_data(dirname)

    laser_profile = Profile(axis=1, thr=180, method='pcog')
    laser_calibration = LaserCalibration(grid_size=(7, 6),
                                         square_size=0.010,
                                         profile=laser_profile)
    laser_calibration.find_calibration_3d(images)

    pattern_poses = laser_calibration.pattern_poses
    profiles = [laser_profile.profile_points(img)
                for img in images]
    lines = [laser_calibration.find_best_line2d(profile2d)
             for profile2d in profiles]

    for k, img in enumerate(images):
        grid = laser_calibration.grids[k]
        imgc = laser_calibration.draw_chessboard(img.copy(), grid)
        pose = laser_calibration.get_chessboard_pose(grid)