Exemplo n.º 1
0
    def __init__(self):
        self.pixels_per_scanline = rospy.get_param('~pixels_per_scanline')
        self.scanner_info_file_name = rospy.get_param('~scanner_info_file_name')
        self.threshold = rospy.get_param('~threshold')        

        self.mutex = threading.RLock()
        self.image_update_flag = threading.Event()
        self.bridge = CvBridge()
        rospy.Subscriber("image_stream", sensor_msgs.msg.Image, self.update_image)
        rospy.loginfo("Waiting for camera info...")
        self.camera_info = rospy.wait_for_message('camera_info', sensor_msgs.msg.CameraInfo)
        rospy.loginfo("Camera info received.")

        rospy.loginfo("Waiting for projector info service...")
        rospy.wait_for_service('get_projector_info')
        rospy.loginfo("Projector info service found.")
        get_projector_info = rospy.ServiceProxy('get_projector_info', projector.srv.GetProjectorInfo)
        self.projector_info = get_projector_info().projector_info

        self.projector_model = PinholeCameraModel()
        self.projector_model.fromCameraInfo(self.projector_info)

        self.read_scanner_info()

        self.projector_to_camera_rotation_matrix = cv.CreateMat(3, 3, cv.CV_32FC1)
        cv.Rodrigues2(self.projector_to_camera_rotation_vector, self.projector_to_camera_rotation_matrix)        

        predistortmap_x = cv.CreateMat(self.projector_info.height, self.projector_info.width, cv.CV_32FC1)
        predistortmap_y = cv.CreateMat(self.projector_info.height, self.projector_info.width, cv.CV_32FC1)
        InitPredistortMap(self.projector_model.intrinsicMatrix(), self.projector_model.distortionCoeffs(), predistortmap_x, predistortmap_y)

        minVal, maxVal, minLoc, maxLoc = cv.MinMaxLoc(predistortmap_x)
        cv.AddS(predistortmap_x, -minVal, predistortmap_x)
        uncropped_projection_width = int(math.ceil(maxVal - minVal))
        self.center_pixel = self.projector_model.cx() - minVal

        minVal, maxVal, minLoc, maxLoc = cv.MinMaxLoc(predistortmap_y)
        cv.AddS(predistortmap_y, -minVal, predistortmap_y)
        uncropped_projection_height = int(math.ceil(maxVal - minVal))

        self.number_of_scanlines = int(math.ceil(float(uncropped_projection_width)/self.pixels_per_scanline))

        rospy.loginfo("Generating projection patterns...")

        graycodes = []
        for i in range(self.number_of_scanlines):
            graycodes.append(graycodemath.generate_gray_code(i))
    
        self.number_of_projection_patterns = int(math.ceil(math.log(self.number_of_scanlines, 2)))        
        self.predistorted_positive_projections = []
        self.predistorted_negative_projections = []

        for i in range(self.number_of_projection_patterns):
            cross_section = cv.CreateMat(1, uncropped_projection_width, cv.CV_8UC1)

            #Fill in cross section with the associated bit of each gray code
            for pixel in range(uncropped_projection_width):
                scanline = pixel // self.pixels_per_scanline
                scanline_value = graycodemath.get_bit(graycodes[scanline], i) * 255
                cross_section[0, pixel] = scanline_value

            #Repeat the cross section over the entire image
            positive_projection = cv.CreateMat(uncropped_projection_height, uncropped_projection_width, cv.CV_8UC1)
            cv.Repeat(cross_section, positive_projection)

            #Predistort the projections to compensate for projector optics so that that the scanlines are approximately planar
            predistorted_positive_projection = cv.CreateMat(self.projector_info.height, self.projector_info.width, cv.CV_8UC1)
            cv.Remap(positive_projection, predistorted_positive_projection, predistortmap_x, predistortmap_y, flags=cv.CV_INTER_NN)

            #Create a negative of the pattern for thresholding
            predistorted_negative_projection = cv.CreateMat(self.projector_info.height, self.projector_info.width, cv.CV_8UC1)
            cv.Not(predistorted_positive_projection, predistorted_negative_projection)
                
            #Fade the borders of the patterns to avoid artifacts at the edges of projection
            predistorted_positive_projection_faded = fade_edges(predistorted_positive_projection, 20)
            predistorted_negative_projection_faded = fade_edges(predistorted_negative_projection, 20)
                
            self.predistorted_positive_projections.append(predistorted_positive_projection_faded)
            self.predistorted_negative_projections.append(predistorted_negative_projection_faded)

        rospy.loginfo("Waiting for projection setting service...")
        rospy.wait_for_service('set_projection')
        rospy.loginfo("Projection setting service found.")
        self.set_projection = rospy.ServiceProxy('set_projection', projector.srv.SetProjection)

        self.pixel_associations_msg = None
        self.point_cloud_msg = None

        rospy.Service("~get_point_cloud", graycode_scanner.srv.GetPointCloud, self.handle_point_cloud_srv)

        point_cloud_pub = rospy.Publisher('~point_cloud', sensor_msgs.msg.PointCloud)

        rospy.loginfo("Ready.")

        rate = rospy.Rate(1)
        while not rospy.is_shutdown():
            if self.point_cloud_msg is not None:
                point_cloud_pub.publish(self.point_cloud_msg)
            rate.sleep()
Exemplo n.º 2
0
    def __init__(self):
        self.pixels_per_scanline = rospy.get_param('~pixels_per_scanline')
        self.scanner_info_file_name = rospy.get_param(
            '~scanner_info_file_name')
        self.threshold = rospy.get_param('~threshold')

        self.mutex = threading.RLock()
        self.image_update_flag = threading.Event()
        self.bridge = CvBridge()
        rospy.Subscriber("image_stream", sensor_msgs.msg.Image,
                         self.update_image)
        rospy.loginfo("Waiting for camera info...")
        self.camera_info = rospy.wait_for_message('camera_info',
                                                  sensor_msgs.msg.CameraInfo)
        rospy.loginfo("Camera info received.")

        rospy.loginfo("Waiting for projector info service...")
        rospy.wait_for_service('get_projector_info')
        rospy.loginfo("Projector info service found.")
        get_projector_info = rospy.ServiceProxy('get_projector_info',
                                                projector.srv.GetProjectorInfo)
        self.projector_info = get_projector_info().projector_info
        self.projector_dimensions = (self.projector_info.width,
                                     self.projector_info.height)

        self.number_of_scanlines = []
        for i in range(2):
            self.number_of_scanlines.append(
                get_number_of_scanlines(self.projector_dimensions[i],
                                        self.pixels_per_scanline))

        self.read_scanner_info()

        rospy.loginfo("Generating projection patterns...")

        graycodes = []
        for i in range(max(self.number_of_scanlines)):
            graycodes.append(graycodemath.generate_gray_code(i))

        self.number_of_projection_patterns = []
        for i in range(2):
            self.number_of_projection_patterns.append(
                int(math.ceil(math.log(self.number_of_scanlines[i], 2))))

        self.positive_projections = []
        self.negative_projections = []
        for i in range(2):
            self.positive_projections.append([])
            self.negative_projections.append([])
            for j in range(self.number_of_projection_patterns[i]):
                cross_section = cv.CreateMat(1, self.projector_dimensions[i],
                                             cv.CV_8UC1)

                #Fill in cross section with the associated bit of each gray code
                for pixel in range(self.projector_dimensions[i]):
                    scanline = pixel // self.pixels_per_scanline
                    scanline_value = graycodemath.get_bit(
                        graycodes[scanline], j) * 255
                    cross_section[0, pixel] = scanline_value

                #If we're doing horizontal scanning, transpose the cross section
                if i is 1:
                    cross_section_transpose = cv.CreateMat(
                        self.projector_info.height, 1, cv.CV_8UC1)
                    cv.Transpose(cross_section, cross_section_transpose)
                    cross_section = cross_section_transpose

                #Repeat the cross section over the entire image
                positive_projection = cv.CreateMat(self.projector_info.height,
                                                   self.projector_info.width,
                                                   cv.CV_8UC1)
                cv.Repeat(cross_section, positive_projection)

                #Create a negative of the pattern for thresholding
                negative_projection = cv.CreateMat(self.projector_info.height,
                                                   self.projector_info.width,
                                                   cv.CV_8UC1)
                cv.Not(positive_projection, negative_projection)

                #Fade the borders of the patterns to avoid artifacts at the edges of projection
                positive_projection_faded = fade_edges(positive_projection, 20)
                negative_projection_faded = fade_edges(negative_projection, 20)

                self.positive_projections[i].append(positive_projection_faded)
                self.negative_projections[i].append(negative_projection_faded)

        rospy.loginfo("Waiting for projection setting service...")
        rospy.wait_for_service('set_projection')
        rospy.loginfo("Projection setting service found.")
        self.set_projection = rospy.ServiceProxy('set_projection',
                                                 projector.srv.SetProjection)
        self.point_cloud_msg = None

        rospy.Service("~get_point_cloud", graycode_scanner.srv.GetPointCloud,
                      self.handle_point_cloud_srv)

        point_cloud_pub = rospy.Publisher('~point_cloud',
                                          sensor_msgs.msg.PointCloud)

        rospy.loginfo("Ready.")

        rate = rospy.Rate(1)
        while not rospy.is_shutdown():
            if self.pixel_associations_msg is not None:
                pixel_associations_pub.publish(self.pixel_associations_msg)
            if self.point_cloud_msg is not None:
                point_cloud_pub.publish(self.point_cloud_msg)
            rate.sleep()
Exemplo n.º 3
0
    def __init__(self):
        self.pixels_per_scanline = rospy.get_param('~pixels_per_scanline')
        self.scanner_info_file_name = rospy.get_param(
            '~scanner_info_file_name')
        self.threshold = rospy.get_param('~threshold')

        self.mutex = threading.RLock()
        self.image_update_flag = threading.Event()
        self.bridge = CvBridge()
        rospy.Subscriber("image_stream", sensor_msgs.msg.Image,
                         self.update_image)
        rospy.loginfo("Waiting for camera info...")
        self.camera_info = rospy.wait_for_message('camera_info',
                                                  sensor_msgs.msg.CameraInfo)
        rospy.loginfo("Camera info received.")

        rospy.loginfo("Waiting for projector info service...")
        rospy.wait_for_service('get_projector_info')
        rospy.loginfo("Projector info service found.")
        get_projector_info = rospy.ServiceProxy('get_projector_info',
                                                projector.srv.GetProjectorInfo)
        self.projector_info = get_projector_info().projector_info

        self.projector_model = PinholeCameraModel()
        self.projector_model.fromCameraInfo(self.projector_info)

        self.read_scanner_info()

        self.projector_to_camera_rotation_matrix = cv.CreateMat(
            3, 3, cv.CV_32FC1)
        cv.Rodrigues2(self.projector_to_camera_rotation_vector,
                      self.projector_to_camera_rotation_matrix)

        predistortmap_x = cv.CreateMat(self.projector_info.height,
                                       self.projector_info.width, cv.CV_32FC1)
        predistortmap_y = cv.CreateMat(self.projector_info.height,
                                       self.projector_info.width, cv.CV_32FC1)
        InitPredistortMap(self.projector_model.intrinsicMatrix(),
                          self.projector_model.distortionCoeffs(),
                          predistortmap_x, predistortmap_y)

        minVal, maxVal, minLoc, maxLoc = cv.MinMaxLoc(predistortmap_x)
        cv.AddS(predistortmap_x, -minVal, predistortmap_x)
        uncropped_projection_width = int(math.ceil(maxVal - minVal))
        self.center_pixel = self.projector_model.cx() - minVal

        minVal, maxVal, minLoc, maxLoc = cv.MinMaxLoc(predistortmap_y)
        cv.AddS(predistortmap_y, -minVal, predistortmap_y)
        uncropped_projection_height = int(math.ceil(maxVal - minVal))

        self.number_of_scanlines = int(
            math.ceil(
                float(uncropped_projection_width) / self.pixels_per_scanline))

        rospy.loginfo("Generating projection patterns...")

        graycodes = []
        for i in range(self.number_of_scanlines):
            graycodes.append(graycodemath.generate_gray_code(i))

        self.number_of_projection_patterns = int(
            math.ceil(math.log(self.number_of_scanlines, 2)))
        self.predistorted_positive_projections = []
        self.predistorted_negative_projections = []

        for i in range(self.number_of_projection_patterns):
            cross_section = cv.CreateMat(1, uncropped_projection_width,
                                         cv.CV_8UC1)

            #Fill in cross section with the associated bit of each gray code
            for pixel in range(uncropped_projection_width):
                scanline = pixel // self.pixels_per_scanline
                scanline_value = graycodemath.get_bit(graycodes[scanline],
                                                      i) * 255
                cross_section[0, pixel] = scanline_value

            #Repeat the cross section over the entire image
            positive_projection = cv.CreateMat(uncropped_projection_height,
                                               uncropped_projection_width,
                                               cv.CV_8UC1)
            cv.Repeat(cross_section, positive_projection)

            #Predistort the projections to compensate for projector optics so that that the scanlines are approximately planar
            predistorted_positive_projection = cv.CreateMat(
                self.projector_info.height, self.projector_info.width,
                cv.CV_8UC1)
            cv.Remap(positive_projection,
                     predistorted_positive_projection,
                     predistortmap_x,
                     predistortmap_y,
                     flags=cv.CV_INTER_NN)

            #Create a negative of the pattern for thresholding
            predistorted_negative_projection = cv.CreateMat(
                self.projector_info.height, self.projector_info.width,
                cv.CV_8UC1)
            cv.Not(predistorted_positive_projection,
                   predistorted_negative_projection)

            #Fade the borders of the patterns to avoid artifacts at the edges of projection
            predistorted_positive_projection_faded = fade_edges(
                predistorted_positive_projection, 20)
            predistorted_negative_projection_faded = fade_edges(
                predistorted_negative_projection, 20)

            self.predistorted_positive_projections.append(
                predistorted_positive_projection_faded)
            self.predistorted_negative_projections.append(
                predistorted_negative_projection_faded)

        rospy.loginfo("Waiting for projection setting service...")
        rospy.wait_for_service('set_projection')
        rospy.loginfo("Projection setting service found.")
        self.set_projection = rospy.ServiceProxy('set_projection',
                                                 projector.srv.SetProjection)

        self.pixel_associations_msg = None
        self.point_cloud_msg = None

        rospy.Service("~get_point_cloud", graycode_scanner.srv.GetPointCloud,
                      self.handle_point_cloud_srv)

        point_cloud_pub = rospy.Publisher('~point_cloud',
                                          sensor_msgs.msg.PointCloud)

        rospy.loginfo("Ready.")

        rate = rospy.Rate(1)
        while not rospy.is_shutdown():
            if self.point_cloud_msg is not None:
                point_cloud_pub.publish(self.point_cloud_msg)
            rate.sleep()
Exemplo n.º 4
0
    def __init__(self):
        self.pixels_per_scanline = rospy.get_param('~pixels_per_scanline')
        self.scanner_info_file_name = rospy.get_param('~scanner_info_file_name')
        self.threshold = rospy.get_param('~threshold')        

        self.mutex = threading.RLock()
        self.image_update_flag = threading.Event()
        self.bridge = CvBridge()
        rospy.Subscriber("image_stream", sensor_msgs.msg.Image, self.update_image)
        rospy.loginfo("Waiting for camera info...")
        self.camera_info = rospy.wait_for_message('camera_info', sensor_msgs.msg.CameraInfo)
        rospy.loginfo("Camera info received.")

        rospy.loginfo("Waiting for projector info service...")
        rospy.wait_for_service('get_projector_info')
        rospy.loginfo("Projector info service found.")
        get_projector_info = rospy.ServiceProxy('get_projector_info', projector.srv.GetProjectorInfo)
        self.projector_info = get_projector_info().projector_info
        self.projector_dimensions = (self.projector_info.width, self.projector_info.height)

        self.number_of_scanlines = []
        for i in range(2):
            self.number_of_scanlines.append(get_number_of_scanlines(self.projector_dimensions[i], self.pixels_per_scanline))

        self.read_scanner_info()

        rospy.loginfo("Generating projection patterns...")

        graycodes = []
        for i in range(max(self.number_of_scanlines)):
            graycodes.append(graycodemath.generate_gray_code(i))
            
        self.number_of_projection_patterns = []
        for i in range(2):
            self.number_of_projection_patterns.append(int(math.ceil(math.log(self.number_of_scanlines[i], 2))))
        
        self.positive_projections = []
        self.negative_projections = []
        for i in range(2):
            self.positive_projections.append([])
            self.negative_projections.append([])
            for j in range(self.number_of_projection_patterns[i]):
                cross_section = cv.CreateMat(1, self.projector_dimensions[i], cv.CV_8UC1)

                #Fill in cross section with the associated bit of each gray code
                for pixel in range(self.projector_dimensions[i]):
                    scanline = pixel // self.pixels_per_scanline
                    scanline_value = graycodemath.get_bit(graycodes[scanline], j) * 255
                    cross_section[0, pixel] = scanline_value

                #If we're doing horizontal scanning, transpose the cross section
                if i is 1:
                    cross_section_transpose = cv.CreateMat(self.projector_info.height, 1, cv.CV_8UC1)
                    cv.Transpose(cross_section, cross_section_transpose)
                    cross_section = cross_section_transpose

                #Repeat the cross section over the entire image
                positive_projection = cv.CreateMat(self.projector_info.height, self.projector_info.width, cv.CV_8UC1)
                cv.Repeat(cross_section, positive_projection)

                #Create a negative of the pattern for thresholding
                negative_projection = cv.CreateMat(self.projector_info.height, self.projector_info.width, cv.CV_8UC1)
                cv.Not(positive_projection, negative_projection)
                
                #Fade the borders of the patterns to avoid artifacts at the edges of projection
                positive_projection_faded = fade_edges(positive_projection, 20)
                negative_projection_faded = fade_edges(negative_projection, 20)
                
                self.positive_projections[i].append(positive_projection_faded)
                self.negative_projections[i].append(negative_projection_faded)

        rospy.loginfo("Waiting for projection setting service...")
        rospy.wait_for_service('set_projection')
        rospy.loginfo("Projection setting service found.")
        self.set_projection = rospy.ServiceProxy('set_projection', projector.srv.SetProjection)
        self.point_cloud_msg = None

        rospy.Service("~get_point_cloud", graycode_scanner.srv.GetPointCloud, self.handle_point_cloud_srv)

        point_cloud_pub = rospy.Publisher('~point_cloud', sensor_msgs.msg.PointCloud)

        rospy.loginfo("Ready.")

        rate = rospy.Rate(1)
        while not rospy.is_shutdown():
            if self.pixel_associations_msg is not None:
                pixel_associations_pub.publish(self.pixel_associations_msg)
            if self.point_cloud_msg is not None:
                point_cloud_pub.publish(self.point_cloud_msg)
            rate.sleep()