コード例 #1
0
    def __init__(self, name=None, params={}, color='white', bEqualizeHist=False):
        MotionTrackedBodypart.__init__(self, name, params, color, bEqualizeHist)
        
        self.name       = name
        self.detector   = EdgeDetectorByHoughTransform(self.name, params)
        self.state      = MsgState()
        self.windowEdges = ImageWindow(False, self.name+'Edges')
        self.set_params(params)

        # Services, for live intensities plots via live_wing_histograms.py
        self.service_trackerdata    = rospy.Service('trackerdata_'+self.name, SrvTrackerdata, self.serve_trackerdata_callback)
コード例 #2
0
    def __init__(self,
                 name=None,
                 params={},
                 color='white',
                 bEqualizeHist=False):
        MotionTrackedBodypart.__init__(self, name, params, color,
                                       bEqualizeHist)

        self.name = name
        self.detector = EdgeDetectorByHoughTransform(self.name, params)
        self.state = MsgState()
        self.windowEdges = ImageWindow(False, self.name + 'Edges')
        self.set_params(params)
コード例 #3
0
ファイル: tracker_tip.py プロジェクト: pvnkmrksk/Kinefly
    def __init__(self, name=None, params={}, color='white', bEqualizeHist=False):
        MotionTrackedBodypartPolar.__init__(self, name, params, color, bEqualizeHist)
        
        self.name     = name
        self.detector = TipDetector()
        self.state    = MsgState()
        self.set_params(params)
        self.windowTip = ImageWindow(False, self.name+'Tip')
        self.iAngle = 0
        self.iRadius = 0

        # Services, for live intensities plots via live_wing_histograms.py
        self.service_trackerdata    = rospy.Service('trackerdata_'+self.name, SrvTrackerdata, self.serve_trackerdata_callback)
コード例 #4
0
    def __init__(self, name=None, params={}, color='white', bEqualizeHist=False):
        MotionTrackedBodypartPolar.__init__(self, name, params, color, bEqualizeHist)
        
        self.phasecorr          = imageprocessing.PhaseCorrelation()
        self.state              = MsgState() # In the bodypart frame.
        self.stateOrigin_p      = MsgState() # In the bodypart frame.
        self.stateLo_p          = MsgState() # In the bodypart frame.
        self.stateHi_p          = MsgState() # In the bodypart frame.

        self.imgComparison      = None
        
        self.windowStabilized   = ImageWindow(False, self.name+'Stable')
        self.windowComparison   = ImageWindow(True, self.name+'Comparison')
        self.set_params(params)
コード例 #5
0
ファイル: tracker_tip.py プロジェクト: bmslpsu/Benifly
    def __init__(self,
                 name=None,
                 params={},
                 color='white',
                 bEqualizeHist=False):
        MotionTrackedBodypartPolar.__init__(self, name, params, color,
                                            bEqualizeHist)

        self.name = name
        self.detector = TipDetector()
        self.state = MsgState()
        self.set_params(params)
        self.windowTip = ImageWindow(False, self.name + 'Tip')
        self.iAngle = 0
        self.iRadius = 0
コード例 #6
0
 def openFile(self):
     """Slot for the openFile action."""
     from SXM import FileIO,Data
     fname = str(QFileDialog.getOpenFileName(self.widget,self.tr("Open File"), \
             ".",FileIO.getFilterString(types=(Data.Image,))))
     if len(fname) > 0:
         root, ext = os.path.splitext(fname)
         self.statusBar().showMessage(self.tr("Loading data: %1").arg(fname),2000)
         image = FileIO.fromFile(fname)
         image.load()
         imwin = ImageWindow(self,image)
         self.Images.append(imwin)
         self.updateImageList()
         imwin.windowModality = False
         imwin.show()
コード例 #7
0
ファイル: bodypart_motion.py プロジェクト: ptweir/Kinefly
    def __init__(self, name=None, params={}, color='white', bEqualizeHist=False):
        self.name           = name
        self.bEqualizeHist  = bEqualizeHist

        self.bValidMask     = False

        self.color          = color
        self.bgra           = ui.bgra_dict[color]
        self.bgra_dim       = tuple(0.5*np.array(ui.bgra_dict[color]))
        self.bgra_state     = ui.bgra_dict[color]#ui.bgra_dict['red']
        self.pixelmax       = 255.0
                        
        self.shape          = (np.inf, np.inf)
        self.ptHinge_i      = np.array([0,0])
        self.roi            = None
        
        self.mask       = Struct()
        self.mask.img   = None
        self.mask.sum   = 1.0
        self.mask.xMin = None
        self.mask.yMin = None
        self.mask.xMax = None
        self.mask.yMax = None
        
        self.dt = np.inf

        self.params = {}
        self.handles = {'hinge':ui.Handle(np.array([0,0]), self.bgra, name='hinge'),
                        'angle_hi':ui.Handle(np.array([0,0]), self.bgra, name='angle_hi'),
                        'angle_lo':ui.Handle(np.array([0,0]), self.bgra, name='angle_lo'),
                        'radius_inner':ui.Handle(np.array([0,0]), self.bgra, name='radius_inner')
                        }
        
        self.lockBackground = threading.Lock()

        # Region of interest images.
        self.imgFullBackground                  = None
        self.imgRoiBackground                   = None
        self.imgRoi                             = None # Untouched roi image.
        self.imgRoiFg                           = None # Background subtracted.
        self.imgRoiFgMasked                     = None
        self.imgFinal                           = None # The image to use for tracking.
        self.imgHeadroom                        = None
        
        # Extra windows.
        self.windowBG         = ImageWindow(False, self.name+'BG')
        self.windowFG         = ImageWindow(False, self.name+'FG')
        self.windowMask       = ImageWindow(gbShowMasks, self.name+'Mask')
コード例 #8
0
    def openFile(self):
        """Slot for the openFile action."""
        from SXM import FileIO, Data

        fname = str(
            QFileDialog.getOpenFileName(
                self.widget, self.tr("Open File"), ".", FileIO.getFilterString(types=(Data.Image,))
            )
        )
        if len(fname) > 0:
            root, ext = os.path.splitext(fname)
            self.statusBar().showMessage(self.tr("Loading data: %1").arg(fname), 2000)
            image = FileIO.fromFile(fname)
            image.load()
            imwin = ImageWindow(self, image)
            self.Images.append(imwin)
            self.updateImageList()
            imwin.windowModality = False
            imwin.show()
コード例 #9
0
    def __init__(self,
                 name=None,
                 params={},
                 color='white',
                 bEqualizeHist=False):
        MotionTrackedBodypart.__init__(self, name, params, color,
                                       bEqualizeHist)

        self.polartransforms = imageprocessing.PolarTransforms()
        self.create_wfn = imageprocessing.WindowFunctions().create_tukey
        self.wfnRoiMaskedPolarCropped = None

        self.imgRoiFgMaskedPolar = None
        self.imgRoiFgMaskedPolarCropped = None
        self.imgRoiFgMaskedPolarCroppedWindowed = None
        self.imgFinal = None  # The image to use for tracking.
        self.imgHeadroomPolar = None

        self.windowPolar = ImageWindow(False, self.name + 'Polar')
コード例 #10
0
ファイル: tracker_edge.py プロジェクト: ptweir/Kinefly
    def __init__(self, name=None, params={}, color="white", bEqualizeHist=False):
        MotionTrackedBodypartPolar.__init__(self, name, params, color, bEqualizeHist)

        self.name = name
        self.detector = EdgeDetectorByIntensityProfile()
        self.state = MsgState()
        self.windowEdges = ImageWindow(False, self.name + "Edges")
        self.set_params(params)

        # Services, for live intensities plots via live_wing_histograms.py
        self.service_trackerdata = rospy.Service("trackerdata_" + name, SrvTrackerdata, self.serve_trackerdata_callback)
コード例 #11
0
ファイル: tracker_tip.py プロジェクト: ptweir/Kinefly
    def __init__(self, name=None, params={}, color='white', bEqualizeHist=False):
        MotionTrackedBodypartPolar.__init__(self, name, params, color, bEqualizeHist)
        
        self.name     = name
        self.detector = TipDetector()
        self.state    = MsgState()
        self.set_params(params)
        self.windowTip = ImageWindow(False, self.name+'Tip')
        self.iAngle = 0
        self.iRadius = 0

        # Services, for live intensities plots via live_wing_histograms.py
        self.service_trackerdata    = rospy.Service('trackerdata_'+self.name, SrvTrackerdata, self.serve_trackerdata_callback)
コード例 #12
0
ファイル: bodypart_intensity.py プロジェクト: ptweir/Kinefly
    def __init__(self, name=None, params={}, color='white', bEqualizeHist=False):
        self.name        = name
        self.bEqualizeHist = bEqualizeHist

        self.bValidMask = False

        self.bgra        = ui.bgra_dict[color]
        self.bgra_dim    = tuple(0.5*np.array(ui.bgra_dict[color]))
        self.bgra_state  = ui.bgra_dict[color]#ui.bgra_dict['red']
        self.pixelmax    = 255.0

        self.shape     = (np.inf, np.inf)
        self.ptCenter_i = np.array([0,0])
        self.roi       = None
        
        self.mask = Struct()
        self.mask.img = None
        self.mask.sum = 1.0
        
        self.dt = np.inf

        self.params = {}
        self.handles = {'center':ui.Handle(np.array([0,0]), self.bgra, name='center'),
                        'radius1':ui.Handle(np.array([0,0]), self.bgra, name='radius1'),
                        'radius2':ui.Handle(np.array([0,0]), self.bgra, name='radius2')
                        }
        
        # Region of interest images.
        self.imgFullBackground                  = None
        self.imgRoiBackground                   = None
        self.imgRoi                             = None # Untouched roi image.
        self.imgRoiFg                           = None # Background subtracted.
        self.imgRoiFgMasked                     = None

        # Extra windows.
        self.windowBG         = ImageWindow(False, self.name+'BG')
        self.windowFG         = ImageWindow(False, self.name+'FG')
        self.windowMask       = ImageWindow(gbShowMasks, self.name+'Mask')
コード例 #13
0
ファイル: bodypart_motion.py プロジェクト: ptweir/Kinefly
    def __init__(self, name=None, params={}, color='white', bEqualizeHist=False):
        MotionTrackedBodypart.__init__(self, name, params, color, bEqualizeHist)

        self.polartransforms                    = imageprocessing.PolarTransforms()
        self.create_wfn                         = imageprocessing.WindowFunctions().create_tukey
        self.wfnRoiMaskedPolarCropped           = None
        
        self.imgRoiFgMaskedPolar                = None
        self.imgRoiFgMaskedPolarCropped         = None
        self.imgRoiFgMaskedPolarCroppedWindowed = None
        self.imgFinal                           = None # The image to use for tracking.
        self.imgHeadroomPolar                   = None
        
        self.windowPolar                        = ImageWindow(False, self.name+'Polar')
コード例 #14
0
    def __init__(self,
                 name=None,
                 params={},
                 color='white',
                 bEqualizeHist=False):
        self.name = name
        self.bEqualizeHist = bEqualizeHist

        self.bValidMask = False

        self.color = color
        self.bgra = ui.bgra_dict[color]
        self.bgra_dim = tuple(0.5 * np.array(ui.bgra_dict[color]))
        self.bgra_state = ui.bgra_dict[color]  #ui.bgra_dict['red']
        self.pixelmax = 255.0

        self.shape = (np.inf, np.inf)
        self.ptHinge_i = np.array([0, 0])
        self.roi = None

        self.mask = Struct()
        self.mask.img = None
        self.mask.sum = 1.0
        self.mask.xMin = None
        self.mask.yMin = None
        self.mask.xMax = None
        self.mask.yMax = None

        self.dt = np.inf

        self.params = {}
        self.handles = {
            'hinge':
            ui.Handle(np.array([0, 0]), self.bgra, name='hinge'),
            'angle_hi':
            ui.Handle(np.array([0, 0]), self.bgra, name='angle_hi'),
            'angle_lo':
            ui.Handle(np.array([0, 0]), self.bgra, name='angle_lo'),
            'radius_inner':
            ui.Handle(np.array([0, 0]), self.bgra, name='radius_inner')
        }

        self.lockBackground = threading.Lock()

        # Region of interest images.
        self.imgFullBackground = None
        self.imgRoiBackground = None
        self.imgRoi = None  # Untouched roi image.
        self.imgRoiFg = None  # Background subtracted.
        self.imgRoiFgMasked = None
        self.imgFinal = None  # The image to use for tracking.
        self.imgHeadroom = None

        # Extra windows.
        self.windowBG = ImageWindow(False, self.name + 'BG')
        self.windowFG = ImageWindow(False, self.name + 'FG')
        self.windowMask = ImageWindow(gbShowMasks, self.name + 'Mask')
コード例 #15
0
ファイル: bodypart_intensity.py プロジェクト: ptweir/Kinefly
    def __init__(self,
                 name=None,
                 params={},
                 color='white',
                 bEqualizeHist=False):
        self.name = name
        self.bEqualizeHist = bEqualizeHist

        self.bValidMask = False

        self.bgra = ui.bgra_dict[color]
        self.bgra_dim = tuple(0.5 * np.array(ui.bgra_dict[color]))
        self.bgra_state = ui.bgra_dict[color]  #ui.bgra_dict['red']
        self.pixelmax = 255.0

        self.shape = (np.inf, np.inf)
        self.ptCenter_i = np.array([0, 0])
        self.roi = None

        self.mask = Struct()
        self.mask.img = None
        self.mask.sum = 1.0

        self.dt = np.inf

        self.params = {}
        self.handles = {
            'center': ui.Handle(np.array([0, 0]), self.bgra, name='center'),
            'radius1': ui.Handle(np.array([0, 0]), self.bgra, name='radius1'),
            'radius2': ui.Handle(np.array([0, 0]), self.bgra, name='radius2')
        }

        # Region of interest images.
        self.imgFullBackground = None
        self.imgRoiBackground = None
        self.imgRoi = None  # Untouched roi image.
        self.imgRoiFg = None  # Background subtracted.
        self.imgRoiFgMasked = None

        # Extra windows.
        self.windowBG = ImageWindow(False, self.name + 'BG')
        self.windowFG = ImageWindow(False, self.name + 'FG')
        self.windowMask = ImageWindow(gbShowMasks, self.name + 'Mask')
コード例 #16
0
class EdgeTrackerByIntensityProfile(MotionTrackedBodypartPolar):
    def __init__(self, name=None, params={}, color='white', bEqualizeHist=False):
        MotionTrackedBodypartPolar.__init__(self, name, params, color, bEqualizeHist)
        
        self.name       = name
        self.detector   = EdgeDetectorByIntensityProfile()
        self.state      = MsgState()
        self.windowEdges = ImageWindow(False, self.name+'Edges')
        self.set_params(params)

        # Services, for live intensities plots via live_wing_histograms.py
        self.service_trackerdata    = rospy.Service('trackerdata_'+name, SrvTrackerdata, self.serve_trackerdata_callback)
    
    
    # set_params()
    # Set the given params dict into this object.
    #
    def set_params(self, params):
        MotionTrackedBodypartPolar.set_params(self, params)
        
        self.imgRoiBackground = None
        self.iCount = 0

        self.state.intensity = 0.0
        self.state.angles = []
        self.state.gradients = []

        # Compute the 'handedness' of the head/abdomen and wing/wing axes.  self.sense specifies the direction of positive angles.
        matAxes = np.array([[self.params['gui']['head']['hinge']['x']-self.params['gui']['abdomen']['hinge']['x'], self.params['gui']['head']['hinge']['y']-self.params['gui']['abdomen']['hinge']['y']],
                            [self.params['gui']['right']['hinge']['x']-self.params['gui']['left']['hinge']['x'], self.params['gui']['right']['hinge']['y']-self.params['gui']['left']['hinge']['y']]])
        if (self.name in ['left','right']):
            self.senseAxes = np.sign(np.linalg.det(matAxes))
            a = -1 if (self.name=='right') else 1
            self.sense = a*self.senseAxes
        else:
            self.sense = 1  

        self.detector.set_params(params[self.name]['threshold'], params['n_edges_max'], self.sense)
    
        
    # update_state()
    #
    def update_state(self):
        imgNow = self.imgRoiFgMaskedPolarCropped
        self.windowEdges.set_image(imgNow)
        
        # Get the rotation & expansion between images.
        if (imgNow is not None):
            # Pixel position and strength of the edges.
            (edges, gradients) = self.detector.detect(imgNow)

            anglePerPixel = (self.params['gui'][self.name]['angle_hi']-self.params['gui'][self.name]['angle_lo']) / float(imgNow.shape[1])

            # Convert pixel to angle units, and put angle into the wing frame.
            self.state.angles = []
            self.state.gradients = []
            for i in range(len(edges)):
                edge = edges[i]
                gradient = gradients[i]
                angle_b = self.params['gui'][self.name]['angle_lo'] + edge * anglePerPixel
                angle_p = (self.transform_angle_p_from_b(angle_b) + np.pi) % (2*np.pi) - np.pi
                self.state.angles.append(angle_p)
                self.state.gradients.append(gradient)
                

            self.state.intensity = np.mean(imgNow)/255.0

        
    # update()
    # Update all the internals given a foreground camera image.
    #
    def update(self, dt, image, bInvertColor):
        MotionTrackedBodypartPolar.update(self, dt, image, bInvertColor)

        if (self.params['gui'][self.name]['track']):
            self.update_state()
            

    # draw()
    # Draw the outline.
    #
    def draw(self, image):
        MotionTrackedBodypartPolar.draw(self, image)
        
        if (self.params['gui'][self.name]['track']):
            # Draw the major and minor edges alternately, until the max number has been reached.
            bgra = self.bgra
            for i in range(len(self.state.angles)):
                angle_b = self.transform_angle_b_from_p(self.state.angles[i])
                angle_i = self.transform_angle_i_from_b(angle_b)

                x0 = self.ptHinge_i[0] + self.params['gui'][self.name]['radius_inner'] * np.cos(angle_i)
                y0 = self.ptHinge_i[1] + self.params['gui'][self.name]['radius_inner'] * np.sin(angle_i)
                x1 = self.ptHinge_i[0] + self.params['gui'][self.name]['radius_outer'] * np.cos(angle_i)
                y1 = self.ptHinge_i[1] + self.params['gui'][self.name]['radius_outer'] * np.sin(angle_i)
                cv2.line(image, (int(x0),int(y0)), (int(x1),int(y1)), bgra, 1)
                bgra = tuple(0.5*np.array(bgra))
                
            self.windowEdges.show()
        
        
    def serve_trackerdata_callback(self, request):
        title1      = 'Intensities'
        title2      = 'Diffs'
        abscissa = np.linspace(self.params['gui'][self.name]['angle_lo'], self.params['gui'][self.name]['angle_hi'], len(self.detector.intensities))
        
            
        angles = []
        gradients = []
        for i in range(len(self.state.angles)):
            angle = self.state.angles[i]
            gradient = self.state.gradients[i]
            angle_b = ((self.transform_angle_b_from_p(angle) + np.pi) % (2*np.pi)) - np.pi
            angles.append(angle_b)
            gradients.append(gradient)

        return SrvTrackerdataResponse(self.color, title1, title2, abscissa, self.detector.intensities, self.detector.diff, angles, gradients)
コード例 #17
0
ファイル: tracker_edge.py プロジェクト: ptweir/Kinefly
class EdgeTrackerByHoughTransform(MotionTrackedBodypart):
    def __init__(self, name=None, params={}, color="white", bEqualizeHist=False):
        MotionTrackedBodypart.__init__(self, name, params, color, bEqualizeHist)

        self.name = name
        self.detector = EdgeDetectorByHoughTransform(self.name, params)
        self.state = MsgState()
        self.windowEdges = ImageWindow(False, self.name + "Edges")
        self.set_params(params)

        # Services, for live intensities plots via live_wing_histograms.py
        self.service_trackerdata = rospy.Service(
            "trackerdata_" + self.name, SrvTrackerdata, self.serve_trackerdata_callback
        )

    # set_params()
    # Set the given params dict into this object.
    #
    def set_params(self, params):
        MotionTrackedBodypart.set_params(self, params)

        self.imgRoiBackground = None
        self.iCount = 0

        self.state.intensity = 0.0
        self.state.angles = []
        self.state.gradients = []

        # Compute the 'handedness' of the head/abdomen and wing/wing axes.  self.sense specifies the direction of positive angles.
        matAxes = np.array(
            [
                [
                    self.params["gui"]["head"]["hinge"]["x"] - self.params["gui"]["abdomen"]["hinge"]["x"],
                    self.params["gui"]["head"]["hinge"]["y"] - self.params["gui"]["abdomen"]["hinge"]["y"],
                ],
                [
                    self.params["gui"]["right"]["hinge"]["x"] - self.params["gui"]["left"]["hinge"]["x"],
                    self.params["gui"]["right"]["hinge"]["y"] - self.params["gui"]["left"]["hinge"]["y"],
                ],
            ]
        )
        if self.name in ["left", "right"]:
            self.senseAxes = np.sign(np.linalg.det(matAxes))
            a = -1 if (self.name == "right") else 1
            self.sense = a * self.senseAxes
        else:
            self.sense = 1

        self.windowEdges.set_enable(self.params["gui"]["windows"] and self.params["gui"][self.name]["track"])
        self.bValidDetector = False

    # update_state()
    #
    def update_state(self):
        imgNow = self.imgRoiFgMasked

        if imgNow is not None:
            # Pixel position and strength of the edges.
            (angles, magnitudes) = self.detector.detect(imgNow)
            self.windowEdges.set_image(self.detector.imgMasked)

            # Put angle into the bodypart frame.
            self.state.angles = []
            self.state.gradients = []
            for i in range(len(angles)):
                angle = angles[i]
                magnitude = magnitudes[i]
                angle_b = self.params["gui"][self.name]["angle_lo"] + angle
                angle_p = (self.transform_angle_p_from_b(angle_b) + np.pi) % (2 * np.pi) - np.pi
                self.state.angles.append(angle)
                self.state.gradients.append(magnitude)

            self.state.intensity = np.mean(self.imgRoiFgMasked) / 255.0

    # update()
    # Update all the internals given a foreground camera image.
    #
    def update(self, dt, image, bInvertColor):
        MotionTrackedBodypart.update(self, dt, image, bInvertColor)

        if self.params["gui"][self.name]["track"]:
            if not self.bValidDetector:
                if self.mask.xMin is not None:

                    self.detector.set_params(
                        self.name,
                        self.params,
                        self.sense,
                        self.angleBodypart_i,
                        self.angleBodypart_b,
                        image.shape,
                        self.mask,
                    )
                    self.bValidDetector = True

            self.update_state()

    # draw()
    # Draw the outline.
    #
    def draw(self, image):
        MotionTrackedBodypart.draw(self, image)

        if self.params["gui"][self.name]["track"]:
            # Draw the major and minor edges alternately, until the max number has been reached.
            bgra = self.bgra
            for i in range(len(self.state.angles)):
                angle_b = self.transform_angle_b_from_p(self.state.angles[i])
                angle_i = self.transform_angle_i_from_b(angle_b)

                x0 = self.ptHinge_i[0] + self.params["gui"][self.name]["radius_inner"] * np.cos(angle_i)
                y0 = self.ptHinge_i[1] + self.params["gui"][self.name]["radius_inner"] * np.sin(angle_i)
                x1 = self.ptHinge_i[0] + self.params["gui"][self.name]["radius_outer"] * np.cos(angle_i)
                y1 = self.ptHinge_i[1] + self.params["gui"][self.name]["radius_outer"] * np.sin(angle_i)
                cv2.line(image, (int(x0), int(y0)), (int(x1), int(y1)), bgra, 1)
                bgra = tuple(0.5 * np.array(bgra))

            self.windowEdges.show()

    def serve_trackerdata_callback(self, request):
        if len(self.detector.intensities) > 0:
            title1 = "Intensities"
            title2 = "Diffs"
            diffs = self.detector.diff  # np.zeros(len(self.detector.intensities))
            abscissa = np.linspace(
                self.params["gui"][self.name]["angle_lo"],
                self.params["gui"][self.name]["angle_hi"],
                len(self.detector.intensities),
            )
            abscissa -= self.angleBodypart_b
            abscissa *= self.sense

            markersH = self.state.angles
            markersV = self.state.gradients  # [self.params[self.name]['threshold']]

            rv = SrvTrackerdataResponse(
                self.color, title1, title2, abscissa, self.detector.intensities, diffs, markersH, markersV
            )
        else:
            rv = SrvTrackerdataResponse()

        return rv
コード例 #18
0
class MotionTrackedBodypart(object):
    def __init__(self,
                 name=None,
                 params={},
                 color='white',
                 bEqualizeHist=False):
        self.name = name
        self.bEqualizeHist = bEqualizeHist

        self.bValidMask = False

        self.color = color
        self.bgra = ui.bgra_dict[color]
        self.bgra_dim = tuple(0.5 * np.array(ui.bgra_dict[color]))
        self.bgra_state = ui.bgra_dict[color]  #ui.bgra_dict['red']
        self.pixelmax = 255.0

        self.shape = (np.inf, np.inf)
        self.ptHinge_i = np.array([0, 0])
        self.roi = None

        self.mask = Struct()
        self.mask.img = None
        self.mask.sum = 1.0
        self.mask.xMin = None
        self.mask.yMin = None
        self.mask.xMax = None
        self.mask.yMax = None

        self.dt = np.inf

        self.params = {}
        self.handles = {
            'hinge':
            ui.Handle(np.array([0, 0]), self.bgra, name='hinge'),
            'angle_hi':
            ui.Handle(np.array([0, 0]), self.bgra, name='angle_hi'),
            'angle_lo':
            ui.Handle(np.array([0, 0]), self.bgra, name='angle_lo'),
            'radius_inner':
            ui.Handle(np.array([0, 0]), self.bgra, name='radius_inner')
        }

        self.lockBackground = threading.Lock()

        # Region of interest images.
        self.imgFullBackground = None
        self.imgRoiBackground = None
        self.imgRoi = None  # Untouched roi image.
        self.imgRoiFg = None  # Background subtracted.
        self.imgRoiFgMasked = None
        self.imgFinal = None  # The image to use for tracking.
        self.imgHeadroom = None

        # Extra windows.
        self.windowBG = ImageWindow(False, self.name + 'BG')
        self.windowFG = ImageWindow(False, self.name + 'FG')
        self.windowMask = ImageWindow(gbShowMasks, self.name + 'Mask')

    # set_params()
    # Set the given params dict into this object, and cache a few values.
    #
    def set_params(self, params):
        self.params = params

        self.rc_background = self.params['rc_background']
        self.angleBody_i = self.get_bodyangle_i()
        self.cosAngleBody_i = np.cos(self.angleBody_i)
        self.sinAngleBody_i = np.sin(self.angleBody_i)

        self.ptHinge_i = np.array([
            self.params['gui'][self.name]['hinge']['x'],
            self.params['gui'][self.name]['hinge']['y']
        ])
        self.ptHingeHead_i = np.array([
            self.params['gui']['head']['hinge']['x'],
            self.params['gui']['head']['hinge']['y']
        ])
        self.ptHingeAbdomen_i = np.array([
            self.params['gui']['abdomen']['hinge']['x'],
            self.params['gui']['abdomen']['hinge']['y']
        ])

        # Compute the body-outward-facing angle, which is the angle from the body center to the bodypart hinge.
        #         pt1 = [params['head']['hinge']['x'], params['head']['hinge']['y']]
        #         pt2 = [params['abdomen']['hinge']['x'], params['abdomen']['hinge']['y']]
        #         pt3 = [params['gui']['left']['hinge']['x'], params['gui']['left']['hinge']['y']]
        #         pt4 = [params['right']['hinge']['x'], params['right']['hinge']['y']]
        #         ptBodyCenter_i = get_intersection(pt1,pt2,pt3,pt4)
        #         self.angleBodypart_i = float(np.arctan2(self.ptHinge_i[1]-ptBodyCenter_i[1], self.ptHinge_i[0]-ptBodyCenter_i[0]))

        # Compute the body-outward-facing angle, which is the angle to the current point from the forward body axis.
        if (self.name in ['head', 'abdomen']):
            nameRelative = {
                'head': 'abdomen',
                'abdomen': 'head',
                'left': 'right',
                'right': 'left'
            }
            self.angleBodypart_i = float(
                np.arctan2(
                    self.params['gui'][self.name]['hinge']['y'] -
                    self.params['gui'][nameRelative[self.name]]['hinge']['y'],
                    self.params['gui'][self.name]['hinge']['x'] -
                    self.params['gui'][nameRelative[self.name]]['hinge']['x']))
        else:
            ptBodyaxis_i = imageprocessing.get_projection_onto_axis(
                self.ptHinge_i, (self.ptHingeAbdomen_i, self.ptHingeHead_i))
            self.angleBodypart_i = float(
                np.arctan2(
                    self.params['gui'][self.name]['hinge']['y'] -
                    ptBodyaxis_i[1],
                    self.params['gui'][self.name]['hinge']['x'] -
                    ptBodyaxis_i[0]))

        self.angleBodypart_b = self.angleBodypart_i - self.angleBody_i

        cosAngleBodypart_i = np.cos(self.angleBodypart_i)
        sinAngleBodypart_i = np.sin(self.angleBodypart_i)
        self.R = np.array([[cosAngleBodypart_i, -sinAngleBodypart_i],
                           [sinAngleBodypart_i, cosAngleBodypart_i]])

        # Turn on/off the extra windows.
        self.windowBG.set_enable(
            self.params['gui']['windows']
            and self.params['gui'][self.name]['track']
            and self.params['gui'][self.name]['subtract_bg'])
        self.windowFG.set_enable(self.params['gui']['windows']
                                 and self.params['gui'][self.name]['track'])

        self.angle_hi_i = self.transform_angle_i_from_b(
            self.params['gui'][self.name]['angle_hi'])
        self.angle_lo_i = self.transform_angle_i_from_b(
            self.params['gui'][self.name]['angle_lo'])

        # Refresh the handle points.
        self.update_handle_points()

    # transform_angle_b_from_p()
    # Transform an angle from the bodypart frame to the fly body frame.
    #
    def transform_angle_b_from_p(self, angle_p):
        angle_b = self.sense * angle_p + self.angleBodypart_b
        return angle_b

    # transform_angle_p_from_b()
    # Transform an angle from the fly body frame to the bodypart frame.
    #
    def transform_angle_p_from_b(self, angle_b):
        angle_p = self.sense * (angle_b - self.angleBodypart_b)
        return angle_p

    # transform_angle_i_from_b()
    # Transform an angle from the fly body frame to the camera image frame.
    #
    def transform_angle_i_from_b(self, angle_b):
        angle_i = angle_b + self.angleBody_i
        return angle_i

    # transform_angle_b_from_i()
    # Transform an angle from the camera image frame to the fly frame: longitudinal axis head is 0, CW positive.
    #
    def transform_angle_b_from_i(self, angle_i):
        angle_b = angle_i - self.angleBody_i
        angle_b = ((angle_b + np.pi) % (2.0 * np.pi)) - np.pi

        return angle_b

    def get_bodyangle_i(self):
        angle_i = imageprocessing.get_angle_from_points_i(
            np.array([
                self.params['gui']['abdomen']['hinge']['x'],
                self.params['gui']['abdomen']['hinge']['y']
            ]),
            np.array([
                self.params['gui']['head']['hinge']['x'],
                self.params['gui']['head']['hinge']['y']
            ]))
        #angleBody_i  = (angle_i + np.pi) % (2.0*np.pi) - np.pi
        angleBody_i = float(angle_i)
        return angleBody_i

    # create_mask()
    # Create elliptical wedge masks, and window functions.
    #
    def create_mask(self, shape):
        # Create the mask.
        img = np.zeros(shape, dtype=np.uint8)

        # Args for the ellipse calls.
        x = int(self.params['gui'][self.name]['hinge']['x'])
        y = int(self.params['gui'][self.name]['hinge']['y'])
        r_outer = int(np.ceil(self.params['gui'][self.name]['radius_outer']))
        r_inner = int(np.floor(
            self.params['gui'][self.name]['radius_inner'])) - 1
        hi = int(np.ceil(np.rad2deg(self.angle_hi_i)))
        lo = int(np.floor(np.rad2deg(self.angle_lo_i)))

        # Draw the mask.
        cv_filled = -1
        cv2.ellipse(img, (x, y), (r_outer, r_outer), 0, hi, lo,
                    ui.bgra_dict['white'], cv_filled)
        cv2.ellipse(img, (x, y), (r_inner, r_inner), 0, 0, 360,
                    ui.bgra_dict['black'], cv_filled)
        img = cv2.dilate(img, np.ones([
            3, 3
        ]))  # Make the mask one pixel bigger to account for pixel aliasing.
        self.windowMask.set_image(img)

        # Find the ROI of the mask.
        xSum = np.sum(img, 0)
        ySum = np.sum(img, 1)
        x_list = np.where(xSum > 0)[0]
        y_list = np.where(ySum > 0)[0]

        if (len(x_list) > 0) and (len(y_list) > 0):
            # Get the extents.
            xMin = np.where(xSum > 0)[0][0]
            xMax = np.where(xSum > 0)[0][-1] + 1
            yMin = np.where(ySum > 0)[0][0]
            yMax = np.where(ySum > 0)[0][-1] + 1

            # Clip border to image edges.
            self.mask.xMin = np.max([0, xMin])
            self.mask.yMin = np.max([0, yMin])
            self.mask.xMax = np.min([xMax, shape[1] - 1])
            self.mask.yMax = np.min([yMax, shape[0] - 1])

            self.roi = np.array([
                self.mask.xMin, self.mask.yMin, self.mask.xMax, self.mask.yMax
            ])
            self.mask.img = img[self.mask.yMin:self.mask.yMax,
                                self.mask.xMin:self.mask.xMax]
            self.mask.sum = np.sum(self.mask.img).astype(np.float32)
            self.bValidMask = True

        else:
            self.mask.xMin = None
            self.mask.yMin = None
            self.mask.xMax = None
            self.mask.yMax = None

            rospy.logwarn('%s: Empty mask.' % self.name)

    # set_background()
    # Set the given image as the background image.
    #
    def set_background(self, image):
        with self.lockBackground:
            self.imgFullBackground = image.astype(np.float32)
            self.imgRoiBackground = None

    # invert_background()
    # Invert the color of the background image.
    #
    def invert_background(self):
        with self.lockBackground:
            if (self.imgRoiBackground is not None):
                self.imgRoiBackground = 255 - self.imgRoiBackground

    def update_background(self):
        alphaBackground = 1.0 - np.exp(-self.dt / self.rc_background)

        with self.lockBackground:
            if (self.imgRoiBackground is not None):
                if (self.imgRoi is not None):
                    if (self.imgRoiBackground.size == self.imgRoi.size):
                        cv2.accumulateWeighted(self.imgRoi.astype(np.float32),
                                               self.imgRoiBackground,
                                               alphaBackground)
                    else:
                        self.imgRoiBackground = None
                        self.imgRoi = None
            else:
                if (self.imgFullBackground is not None) and (self.roi
                                                             is not None):
                    self.imgRoiBackground = copy.deepcopy(
                        self.imgFullBackground[self.roi[1]:self.roi[3],
                                               self.roi[0]:self.roi[2]])

            if (self.imgRoiBackground is not None):
                self.imgHeadroom = (255.0 - self.imgRoiBackground)
            else:
                self.imgHeadroom = None

            self.windowBG.set_image(self.imgRoiBackground)

    def update_roi(self, image, bInvertColor):
        self.image = image
        self.shape = image.shape

        # Extract the ROI images.
        if (self.roi is not None):
            self.imgRoi = copy.deepcopy(image[self.roi[1]:self.roi[3],
                                              self.roi[0]:self.roi[2]])

            # Color inversion.
            if (bInvertColor):
                self.imgRoiFg = 255 - self.imgRoi
            else:
                self.imgRoiFg = self.imgRoi

            # Background Subtraction.
            if (self.params['gui'][self.name]['subtract_bg']):
                with self.lockBackground:
                    if (self.imgRoiBackground is not None):
                        if (self.imgRoiBackground.shape == self.imgRoiFg.shape
                            ):
                            if (bInvertColor):
                                self.imgRoiFg = cv2.absdiff(
                                    self.imgRoiFg, 255 -
                                    self.imgRoiBackground.astype(np.uint8))
                            else:
                                self.imgRoiFg = cv2.absdiff(
                                    self.imgRoiFg,
                                    self.imgRoiBackground.astype(np.uint8))

            # Equalize the brightness/contrast.
            if (self.bEqualizeHist):
                if (self.imgRoiFg is not None):
                    self.imgRoiFg -= np.min(self.imgRoiFg)
                    max2 = np.max([1.0, np.max(self.imgRoiFg)])

                    self.imgRoiFg *= (255.0 / float(max2))

    # update_handle_points()
    # Update the dictionary of handle point names and locations.
    # Compute the various handle points.
    #
    def update_handle_points(self):
        x = self.params['gui'][self.name]['hinge']['x']
        y = self.params['gui'][self.name]['hinge']['y']
        radius_outer = self.params['gui'][self.name]['radius_outer']
        radius_inner = self.params['gui'][self.name]['radius_inner']
        angle = (self.angle_hi_i + self.angle_lo_i) / 2.0

        self.handles['hinge'].pt = np.array([x, y])
        self.handles['radius_inner'].pt = np.array([x, y]) + (
            (radius_inner) *
            np.array([np.cos(angle), np.sin(angle)]))
        self.handles['angle_hi'].pt = np.array(
            [x, y]) + np.array([(radius_outer) * np.cos(self.angle_hi_i),
                                (radius_outer) * np.sin(self.angle_hi_i)])
        self.handles['angle_lo'].pt = np.array(
            [x, y]) + np.array([(radius_outer) * np.cos(self.angle_lo_i),
                                (radius_outer) * np.sin(self.angle_lo_i)])

        self.ptWedgeHi_outer = tuple((np.array([x, y]) + np.array([
            radius_outer * np.cos(self.angle_hi_i),
            (radius_outer) * np.sin(self.angle_hi_i)
        ])).astype(int))
        self.ptWedgeHi_inner = tuple((np.array([x, y]) + np.array([
            radius_inner * np.cos(self.angle_hi_i),
            (radius_inner) * np.sin(self.angle_hi_i)
        ])).astype(int))
        self.ptWedgeLo_outer = tuple((np.array([x, y]) + np.array([
            radius_outer * np.cos(self.angle_lo_i),
            (radius_outer) * np.sin(self.angle_lo_i)
        ])).astype(int))
        self.ptWedgeLo_inner = tuple((np.array([x, y]) + np.array([
            radius_inner * np.cos(self.angle_lo_i),
            (radius_inner) * np.sin(self.angle_lo_i)
        ])).astype(int))

    # update()
    # Update all the internals given a foreground camera image.
    #
    def update(self, dt, image, bInvertColor):
        self.iCount += 1

        self.dt = dt

        if (self.params['gui'][self.name]['track']):
            if (not self.bValidMask):
                self.create_mask(image.shape)
                self.bValidMask = True

            if (self.params['gui'][self.name]['subtract_bg']):
                self.update_background()

            self.update_roi(image, bInvertColor)
            self.windowFG.set_image(self.imgRoiFg)

            # Apply the mask.
            if (self.imgRoiFg is not None) and (self.mask.img is not None):
                self.imgRoiFgMasked = cv2.bitwise_and(
                    self.imgRoiFg.astype(self.mask.img.dtype), self.mask.img)
                self.imgFinal = self.imgRoiFgMasked
            else:
                self.imgFinal = None

    # hit_object()
    # Get the UI object, if any, that the mouse is on.
    def hit_object(self, ptMouse):
        tag = None

        # Check for handle hits.
        if (self.params['gui'][self.name]['track']):
            for tagHandle, handle in self.handles.iteritems():
                if (handle.hit_test(ptMouse)):
                    tag = tagHandle
                    break
        else:
            tagHandle, handle = ('hinge', self.handles['hinge'])
            if (handle.hit_test(ptMouse)):
                tag = tagHandle

        return (self.name, tag)

    def draw_handles(self, image):
        # Draw all handle points, or only just the hinge handle.
        if (self.params['gui'][self.name]['track']):
            for tagHandle, handle in self.handles.iteritems():
                handle.draw(image)
        else:
            tagHandle, handle = ('hinge', self.handles['hinge'])
            handle.draw(image)

    # draw()
    # Draw the outline.
    #
    def draw(self, image):
        self.draw_handles(image)

        if (self.params['gui'][self.name]['track']):
            x = int(self.params['gui'][self.name]['hinge']['x'])
            y = int(self.params['gui'][self.name]['hinge']['y'])
            radius_outer = int(self.params['gui'][self.name]['radius_outer'])
            radius_inner = int(self.params['gui'][self.name]['radius_inner'])
            radius_mid = int(
                (self.params['gui'][self.name]['radius_outer'] +
                 self.params['gui'][self.name]['radius_inner']) / 2.0)

            #             if ()
            #             angle1 = self.angle_lo_i
            #             angle2 = self.angle_hi_i

            # Draw the mid arc.
            cv2.ellipse(image, (x, y), (radius_mid, radius_mid),
                        np.rad2deg(0.0), np.rad2deg(self.angle_hi_i),
                        np.rad2deg(self.angle_lo_i), self.bgra_dim, 1)

            # Draw the outer arc.
            cv2.ellipse(image, (x, y), (radius_outer, radius_outer),
                        np.rad2deg(0.0), np.rad2deg(self.angle_hi_i),
                        np.rad2deg(self.angle_lo_i), self.bgra_dim, 1)

            # Draw the inner arc.
            cv2.ellipse(image, (x, y), (radius_inner, radius_inner),
                        np.rad2deg(0.0), np.rad2deg(self.angle_hi_i),
                        np.rad2deg(self.angle_lo_i), self.bgra_dim, 1)

            # Draw wedge lines.
            cv2.line(image, self.ptWedgeHi_inner, self.ptWedgeHi_outer,
                     self.bgra_dim, 1)
            cv2.line(image, self.ptWedgeLo_inner, self.ptWedgeLo_outer,
                     self.bgra_dim, 1)

            # Show the extra windows.
            self.windowBG.show()
            self.windowFG.show()
            self.windowMask.show()
コード例 #19
0
ファイル: tracker_edge.py プロジェクト: ptweir/Kinefly
class EdgeTrackerByIntensityProfile(MotionTrackedBodypartPolar):
    def __init__(self, name=None, params={}, color="white", bEqualizeHist=False):
        MotionTrackedBodypartPolar.__init__(self, name, params, color, bEqualizeHist)

        self.name = name
        self.detector = EdgeDetectorByIntensityProfile()
        self.state = MsgState()
        self.windowEdges = ImageWindow(False, self.name + "Edges")
        self.set_params(params)

        # Services, for live intensities plots via live_wing_histograms.py
        self.service_trackerdata = rospy.Service("trackerdata_" + name, SrvTrackerdata, self.serve_trackerdata_callback)

    # set_params()
    # Set the given params dict into this object.
    #
    def set_params(self, params):
        MotionTrackedBodypartPolar.set_params(self, params)

        self.imgRoiBackground = None
        self.iCount = 0

        self.state.intensity = 0.0
        self.state.angles = []
        self.state.gradients = []

        # Compute the 'handedness' of the head/abdomen and wing/wing axes.  self.sense specifies the direction of positive angles.
        matAxes = np.array(
            [
                [
                    self.params["gui"]["head"]["hinge"]["x"] - self.params["gui"]["abdomen"]["hinge"]["x"],
                    self.params["gui"]["head"]["hinge"]["y"] - self.params["gui"]["abdomen"]["hinge"]["y"],
                ],
                [
                    self.params["gui"]["right"]["hinge"]["x"] - self.params["gui"]["left"]["hinge"]["x"],
                    self.params["gui"]["right"]["hinge"]["y"] - self.params["gui"]["left"]["hinge"]["y"],
                ],
            ]
        )
        if self.name in ["left", "right"]:
            self.senseAxes = np.sign(np.linalg.det(matAxes))
            a = -1 if (self.name == "right") else 1
            self.sense = a * self.senseAxes
        else:
            self.sense = 1

        self.detector.set_params(params[self.name]["threshold"], params["n_edges_max"], self.sense)

    # update_state()
    #
    def update_state(self):
        imgNow = self.imgRoiFgMaskedPolarCropped
        self.windowEdges.set_image(imgNow)

        # Get the rotation & expansion between images.
        if imgNow is not None:
            # Pixel position and strength of the edges.
            (edges, gradients) = self.detector.detect(imgNow)

            anglePerPixel = (
                self.params["gui"][self.name]["angle_hi"] - self.params["gui"][self.name]["angle_lo"]
            ) / float(imgNow.shape[1])

            # Convert pixel to angle units, and put angle into the wing frame.
            self.state.angles = []
            self.state.gradients = []
            for i in range(len(edges)):
                edge = edges[i]
                gradient = gradients[i]
                angle_b = self.params["gui"][self.name]["angle_lo"] + edge * anglePerPixel
                angle_p = (self.transform_angle_p_from_b(angle_b) + np.pi) % (2 * np.pi) - np.pi
                self.state.angles.append(angle_p)
                self.state.gradients.append(gradient)

            self.state.intensity = np.mean(imgNow) / 255.0

    # update()
    # Update all the internals given a foreground camera image.
    #
    def update(self, dt, image, bInvertColor):
        MotionTrackedBodypartPolar.update(self, dt, image, bInvertColor)

        if self.params["gui"][self.name]["track"]:
            self.update_state()

    # draw()
    # Draw the outline.
    #
    def draw(self, image):
        MotionTrackedBodypartPolar.draw(self, image)

        if self.params["gui"][self.name]["track"]:
            # Draw the major and minor edges alternately, until the max number has been reached.
            bgra = self.bgra
            for i in range(len(self.state.angles)):
                angle_b = self.transform_angle_b_from_p(self.state.angles[i])
                angle_i = self.transform_angle_i_from_b(angle_b)

                x0 = self.ptHinge_i[0] + self.params["gui"][self.name]["radius_inner"] * np.cos(angle_i)
                y0 = self.ptHinge_i[1] + self.params["gui"][self.name]["radius_inner"] * np.sin(angle_i)
                x1 = self.ptHinge_i[0] + self.params["gui"][self.name]["radius_outer"] * np.cos(angle_i)
                y1 = self.ptHinge_i[1] + self.params["gui"][self.name]["radius_outer"] * np.sin(angle_i)
                cv2.line(image, (int(x0), int(y0)), (int(x1), int(y1)), bgra, 1)
                bgra = tuple(0.5 * np.array(bgra))

            self.windowEdges.show()

    def serve_trackerdata_callback(self, request):
        title1 = "Intensities"
        title2 = "Diffs"
        abscissa = np.linspace(
            self.params["gui"][self.name]["angle_lo"],
            self.params["gui"][self.name]["angle_hi"],
            len(self.detector.intensities),
        )

        angles = []
        gradients = []
        for i in range(len(self.state.angles)):
            angle = self.state.angles[i]
            gradient = self.state.gradients[i]
            angle_b = ((self.transform_angle_b_from_p(angle) + np.pi) % (2 * np.pi)) - np.pi
            angles.append(angle_b)
            gradients.append(gradient)

        return SrvTrackerdataResponse(
            self.color, title1, title2, abscissa, self.detector.intensities, self.detector.diff, angles, gradients
        )
コード例 #20
0
ファイル: bodypart_motion.py プロジェクト: ptweir/Kinefly
class MotionTrackedBodypart(object):
    def __init__(self, name=None, params={}, color='white', bEqualizeHist=False):
        self.name           = name
        self.bEqualizeHist  = bEqualizeHist

        self.bValidMask     = False

        self.color          = color
        self.bgra           = ui.bgra_dict[color]
        self.bgra_dim       = tuple(0.5*np.array(ui.bgra_dict[color]))
        self.bgra_state     = ui.bgra_dict[color]#ui.bgra_dict['red']
        self.pixelmax       = 255.0
                        
        self.shape          = (np.inf, np.inf)
        self.ptHinge_i      = np.array([0,0])
        self.roi            = None
        
        self.mask       = Struct()
        self.mask.img   = None
        self.mask.sum   = 1.0
        self.mask.xMin = None
        self.mask.yMin = None
        self.mask.xMax = None
        self.mask.yMax = None
        
        self.dt = np.inf

        self.params = {}
        self.handles = {'hinge':ui.Handle(np.array([0,0]), self.bgra, name='hinge'),
                        'angle_hi':ui.Handle(np.array([0,0]), self.bgra, name='angle_hi'),
                        'angle_lo':ui.Handle(np.array([0,0]), self.bgra, name='angle_lo'),
                        'radius_inner':ui.Handle(np.array([0,0]), self.bgra, name='radius_inner')
                        }
        
        self.lockBackground = threading.Lock()

        # Region of interest images.
        self.imgFullBackground                  = None
        self.imgRoiBackground                   = None
        self.imgRoi                             = None # Untouched roi image.
        self.imgRoiFg                           = None # Background subtracted.
        self.imgRoiFgMasked                     = None
        self.imgFinal                           = None # The image to use for tracking.
        self.imgHeadroom                        = None
        
        # Extra windows.
        self.windowBG         = ImageWindow(False, self.name+'BG')
        self.windowFG         = ImageWindow(False, self.name+'FG')
        self.windowMask       = ImageWindow(gbShowMasks, self.name+'Mask')


    
    # set_params()
    # Set the given params dict into this object, and cache a few values.
    #
    def set_params(self, params):
        self.params = params

        self.rc_background = self.params['rc_background']
        self.angleBody_i = self.get_bodyangle_i()
        self.cosAngleBody_i = np.cos(self.angleBody_i)
        self.sinAngleBody_i = np.sin(self.angleBody_i)

        self.ptHinge_i        = np.array([self.params['gui'][self.name]['hinge']['x'], self.params['gui'][self.name]['hinge']['y']])
        self.ptHingeHead_i    = np.array([self.params['gui']['head']['hinge']['x'], self.params['gui']['head']['hinge']['y']])
        self.ptHingeAbdomen_i = np.array([self.params['gui']['abdomen']['hinge']['x'], self.params['gui']['abdomen']['hinge']['y']])

        
        # Compute the body-outward-facing angle, which is the angle from the body center to the bodypart hinge.
#         pt1 = [params['head']['hinge']['x'], params['head']['hinge']['y']]
#         pt2 = [params['abdomen']['hinge']['x'], params['abdomen']['hinge']['y']]
#         pt3 = [params['gui']['left']['hinge']['x'], params['gui']['left']['hinge']['y']]
#         pt4 = [params['right']['hinge']['x'], params['right']['hinge']['y']]
#         ptBodyCenter_i = get_intersection(pt1,pt2,pt3,pt4)
#         self.angleBodypart_i = float(np.arctan2(self.ptHinge_i[1]-ptBodyCenter_i[1], self.ptHinge_i[0]-ptBodyCenter_i[0]))

        # Compute the body-outward-facing angle, which is the angle to the current point from the forward body axis.
        if (self.name in ['head','abdomen']):
            nameRelative = {'head':'abdomen', 
                            'abdomen':'head', 
                            'left':'right', 
                            'right':'left'}
            self.angleBodypart_i = float(np.arctan2(self.params['gui'][self.name]['hinge']['y']-self.params['gui'][nameRelative[self.name]]['hinge']['y'], 
                                                    self.params['gui'][self.name]['hinge']['x']-self.params['gui'][nameRelative[self.name]]['hinge']['x']))
        else:
            ptBodyaxis_i = imageprocessing.get_projection_onto_axis(self.ptHinge_i, (self.ptHingeAbdomen_i, self.ptHingeHead_i))
            self.angleBodypart_i = float(np.arctan2(self.params['gui'][self.name]['hinge']['y']-ptBodyaxis_i[1], 
                                                    self.params['gui'][self.name]['hinge']['x']-ptBodyaxis_i[0]))

        self.angleBodypart_b = self.angleBodypart_i - self.angleBody_i

        
        cosAngleBodypart_i = np.cos(self.angleBodypart_i)
        sinAngleBodypart_i = np.sin(self.angleBodypart_i)
        self.R = np.array([[cosAngleBodypart_i, -sinAngleBodypart_i], 
                           [sinAngleBodypart_i, cosAngleBodypart_i]])

        # Turn on/off the extra windows.
        self.windowBG.set_enable(self.params['gui']['windows'] and self.params['gui'][self.name]['track'] and self.params['gui'][self.name]['subtract_bg'])
        self.windowFG.set_enable(self.params['gui']['windows'] and self.params['gui'][self.name]['track'])

        self.angle_hi_i = self.transform_angle_i_from_b(self.params['gui'][self.name]['angle_hi'])
        self.angle_lo_i = self.transform_angle_i_from_b(self.params['gui'][self.name]['angle_lo'])
        
        # Refresh the handle points.
        self.update_handle_points()


    # transform_angle_b_from_p()
    # Transform an angle from the bodypart frame to the fly body frame.
    #
    def transform_angle_b_from_p(self, angle_p):
        angle_b =  self.sense * angle_p + self.angleBodypart_b
        return angle_b


    # transform_angle_p_from_b()
    # Transform an angle from the fly body frame to the bodypart frame.
    #
    def transform_angle_p_from_b(self, angle_b):
        angle_p =  self.sense * (angle_b - self.angleBodypart_b)
        return angle_p


    # transform_angle_i_from_b()
    # Transform an angle from the fly body frame to the camera image frame.
    #
    def transform_angle_i_from_b(self, angle_b):
        angle_i = angle_b + self.angleBody_i 
        return angle_i
        

    # transform_angle_b_from_i()
    # Transform an angle from the camera image frame to the fly frame: longitudinal axis head is 0, CW positive.
    #
    def transform_angle_b_from_i(self, angle_i):
        angle_b = angle_i - self.angleBody_i
        angle_b = ((angle_b+np.pi) % (2.0*np.pi)) - np.pi

        return angle_b
         

    def get_bodyangle_i(self):
        angle_i = imageprocessing.get_angle_from_points_i(np.array([self.params['gui']['abdomen']['hinge']['x'], self.params['gui']['abdomen']['hinge']['y']]), 
                                          np.array([self.params['gui']['head']['hinge']['x'], self.params['gui']['head']['hinge']['y']]))
        #angleBody_i  = (angle_i + np.pi) % (2.0*np.pi) - np.pi
        angleBody_i  = float(angle_i)
        return angleBody_i 
        
                
    # create_mask()
    # Create elliptical wedge masks, and window functions.
    #
    def create_mask(self, shape):
        # Create the mask.
        img = np.zeros(shape, dtype=np.uint8)
        
        # Args for the ellipse calls.
        x = int(self.params['gui'][self.name]['hinge']['x'])
        y = int(self.params['gui'][self.name]['hinge']['y'])
        r_outer = int(np.ceil(self.params['gui'][self.name]['radius_outer']))
        r_inner = int(np.floor(self.params['gui'][self.name]['radius_inner']))-1
        hi = int(np.ceil(np.rad2deg(self.angle_hi_i)))
        lo = int(np.floor(np.rad2deg(self.angle_lo_i)))
        
        # Draw the mask.
        cv2.ellipse(img, (x, y), (r_outer, r_outer), 0, hi, lo, ui.bgra_dict['white'], cv.CV_FILLED)
        cv2.ellipse(img, (x, y), (r_inner, r_inner), 0, 0, 360, ui.bgra_dict['black'], cv.CV_FILLED)
        img = cv2.dilate(img, np.ones([3,3])) # Make the mask one pixel bigger to account for pixel aliasing.
        self.windowMask.set_image(img)
        
        
        # Find the ROI of the mask.
        xSum = np.sum(img, 0)
        ySum = np.sum(img, 1)
        x_list = np.where(xSum>0)[0]
        y_list = np.where(ySum>0)[0]
        
        if (len(x_list)>0) and (len(y_list)>0): 
            # Get the extents.
            xMin = np.where(xSum>0)[0][0]  
            xMax = np.where(xSum>0)[0][-1] + 1
            yMin = np.where(ySum>0)[0][0]  
            yMax = np.where(ySum>0)[0][-1] + 1
            
            # Clip border to image edges.
            self.mask.xMin = np.max([0,xMin])
            self.mask.yMin = np.max([0,yMin])
            self.mask.xMax = np.min([xMax, shape[1]-1])
            self.mask.yMax = np.min([yMax, shape[0]-1])
            
            self.roi = np.array([self.mask.xMin, self.mask.yMin, self.mask.xMax, self.mask.yMax])
            self.mask.img = img[self.mask.yMin:self.mask.yMax, self.mask.xMin:self.mask.xMax]
            self.mask.sum = np.sum(self.mask.img).astype(np.float32)
            self.bValidMask = True  

        else:
            self.mask.xMin = None
            self.mask.yMin = None
            self.mask.xMax = None
            self.mask.yMax = None

            rospy.logwarn('%s: Empty mask.' % self.name)      
            

        
    # set_background()
    # Set the given image as the background image.
    #                
    def set_background(self, image):
        with self.lockBackground:
            self.imgFullBackground = image.astype(np.float32)
            self.imgRoiBackground = None
        
        
    # invert_background()
    # Invert the color of the background image.
    #                
    def invert_background(self):
        with self.lockBackground:
            if (self.imgRoiBackground is not None):
                self.imgRoiBackground = 255-self.imgRoiBackground
        
        
    def update_background(self):
        alphaBackground = 1.0 - np.exp(-self.dt / self.rc_background)

        with self.lockBackground:
            if (self.imgRoiBackground is not None):
                if (self.imgRoi is not None):
                    if (self.imgRoiBackground.size==self.imgRoi.size):
                        cv2.accumulateWeighted(self.imgRoi.astype(np.float32), self.imgRoiBackground, alphaBackground)
                    else:
                        self.imgRoiBackground = None
                        self.imgRoi = None
            else:
                if (self.imgFullBackground is not None) and (self.roi is not None):
                    self.imgRoiBackground = copy.deepcopy(self.imgFullBackground[self.roi[1]:self.roi[3], self.roi[0]:self.roi[2]])
                    
            if (self.imgRoiBackground is not None):
                self.imgHeadroom = (255.0 - self.imgRoiBackground)
            else:
                self.imgHeadroom = None
                
            self.windowBG.set_image(self.imgRoiBackground)
        

    def update_roi(self, image, bInvertColor):
        self.image = image
        self.shape = image.shape
        
        # Extract the ROI images.
        if (self.roi is not None):
            self.imgRoi = copy.deepcopy(image[self.roi[1]:self.roi[3], self.roi[0]:self.roi[2]])

            # Color inversion.
            if (bInvertColor):
                self.imgRoiFg = 255-self.imgRoi
            else:
                self.imgRoiFg = self.imgRoi

            # Background Subtraction.
            if (self.params['gui'][self.name]['subtract_bg']):
                with self.lockBackground:
                    if (self.imgRoiBackground is not None):
                        if (self.imgRoiBackground.shape==self.imgRoiFg.shape):
                            if (bInvertColor):
                                self.imgRoiFg = cv2.absdiff(self.imgRoiFg, 255-self.imgRoiBackground.astype(np.uint8))
                            else:
                                self.imgRoiFg = cv2.absdiff(self.imgRoiFg, self.imgRoiBackground.astype(np.uint8))
                        
                
            # Equalize the brightness/contrast.
            if (self.bEqualizeHist):
                if (self.imgRoiFg is not None):
                    self.imgRoiFg -= np.min(self.imgRoiFg)
                    max2 = np.max([1.0, np.max(self.imgRoiFg)])
    
                    self.imgRoiFg *= (255.0/float(max2))
                
        

            
    # update_handle_points()
    # Update the dictionary of handle point names and locations.
    # Compute the various handle points.
    #
    def update_handle_points (self):
        x = self.params['gui'][self.name]['hinge']['x']
        y = self.params['gui'][self.name]['hinge']['y']
        radius_outer = self.params['gui'][self.name]['radius_outer']
        radius_inner = self.params['gui'][self.name]['radius_inner']
        angle = (self.angle_hi_i+self.angle_lo_i)/2.0
        
        
        self.handles['hinge'].pt        = np.array([x, y])
        self.handles['radius_inner'].pt = np.array([x, y]) + ((radius_inner) * np.array([np.cos(angle),np.sin(angle)]))
        self.handles['angle_hi'].pt     = np.array([x, y]) + np.array([(radius_outer)*np.cos(self.angle_hi_i), (radius_outer)*np.sin(self.angle_hi_i)])
        self.handles['angle_lo'].pt     = np.array([x, y]) + np.array([(radius_outer)*np.cos(self.angle_lo_i), (radius_outer)*np.sin(self.angle_lo_i)])

        self.ptWedgeHi_outer = tuple((np.array([x, y]) + np.array([radius_outer*np.cos(self.angle_hi_i), (radius_outer)*np.sin(self.angle_hi_i)])).astype(int))
        self.ptWedgeHi_inner = tuple((np.array([x, y]) + np.array([radius_inner*np.cos(self.angle_hi_i), (radius_inner)*np.sin(self.angle_hi_i)])).astype(int))
        self.ptWedgeLo_outer = tuple((np.array([x, y]) + np.array([radius_outer*np.cos(self.angle_lo_i), (radius_outer)*np.sin(self.angle_lo_i)])).astype(int))
        self.ptWedgeLo_inner = tuple((np.array([x, y]) + np.array([radius_inner*np.cos(self.angle_lo_i), (radius_inner)*np.sin(self.angle_lo_i)])).astype(int))
        
    # update()
    # Update all the internals given a foreground camera image.
    #
    def update(self, dt, image, bInvertColor):
        self.iCount += 1
        
        self.dt = dt
                
        if (self.params['gui'][self.name]['track']):
            if (not self.bValidMask):
                self.create_mask(image.shape)
                self.bValidMask = True
                
            if (self.params['gui'][self.name]['subtract_bg']):
                self.update_background()
                
            self.update_roi(image, bInvertColor)
            self.windowFG.set_image(self.imgRoiFg) 

            # Apply the mask.
            if (self.imgRoiFg is not None) and (self.mask.img is not None):
                self.imgRoiFgMasked = cv2.bitwise_and(self.imgRoiFg.astype(self.mask.img.dtype), self.mask.img)
                self.imgFinal = self.imgRoiFgMasked 
            else:
                self.imgFinal = None
                


    # hit_object()
    # Get the UI object, if any, that the mouse is on.    
    def hit_object(self, ptMouse):
        tag = None
        
        # Check for handle hits.
        if (self.params['gui'][self.name]['track']):
            for tagHandle,handle in self.handles.iteritems():
                if (handle.hit_test(ptMouse)):
                    tag = tagHandle
                    break
        else:
            tagHandle,handle = ('hinge',self.handles['hinge'])
            if (handle.hit_test(ptMouse)):
                tag = tagHandle
            
                
        return (self.name, tag)
    

    def draw_handles(self, image):
        # Draw all handle points, or only just the hinge handle.
        if (self.params['gui'][self.name]['track']):
            for tagHandle,handle in self.handles.iteritems():
                handle.draw(image)
        else:
            tagHandle,handle = ('hinge',self.handles['hinge'])
            handle.draw(image)

    
    # draw()
    # Draw the outline.
    #
    def draw(self, image):
        self.draw_handles(image)

        if (self.params['gui'][self.name]['track']):
            x = int(self.params['gui'][self.name]['hinge']['x'])
            y = int(self.params['gui'][self.name]['hinge']['y'])
            radius_outer = int(self.params['gui'][self.name]['radius_outer'])
            radius_inner = int(self.params['gui'][self.name]['radius_inner'])
            radius_mid = int((self.params['gui'][self.name]['radius_outer']+self.params['gui'][self.name]['radius_inner'])/2.0)

#             if ()
#             angle1 = self.angle_lo_i
#             angle2 = self.angle_hi_i
            
            # Draw the mid arc.
            cv2.ellipse(image,
                        (x, y),
                        (radius_mid, radius_mid),
                        np.rad2deg(0.0),
                        np.rad2deg(self.angle_hi_i),
                        np.rad2deg(self.angle_lo_i),
                        self.bgra_dim, 
                        1)

            # Draw the outer arc.
            cv2.ellipse(image,
                        (x, y),
                        (radius_outer, radius_outer),
                        np.rad2deg(0.0),
                        np.rad2deg(self.angle_hi_i),
                        np.rad2deg(self.angle_lo_i),
                        self.bgra_dim, 
                        1)
    
            # Draw the inner arc.
            cv2.ellipse(image,
                        (x, y),
                        (radius_inner, radius_inner),
                        np.rad2deg(0.0),
                        np.rad2deg(self.angle_hi_i),
                        np.rad2deg(self.angle_lo_i),
                        self.bgra_dim, 
                        1)
    
            # Draw wedge lines.        
            cv2.line(image, self.ptWedgeHi_inner, self.ptWedgeHi_outer, self.bgra_dim, 1)
            cv2.line(image, self.ptWedgeLo_inner, self.ptWedgeLo_outer, self.bgra_dim, 1)

    
            # Show the extra windows.
            self.windowBG.show()
            self.windowFG.show()
            self.windowMask.show()
コード例 #21
0
ファイル: tracker_tip.py プロジェクト: pvnkmrksk/Kinefly
class TipTracker(MotionTrackedBodypartPolar):
    def __init__(self, name=None, params={}, color='white', bEqualizeHist=False):
        MotionTrackedBodypartPolar.__init__(self, name, params, color, bEqualizeHist)
        
        self.name     = name
        self.detector = TipDetector()
        self.state    = MsgState()
        self.set_params(params)
        self.windowTip = ImageWindow(False, self.name+'Tip')
        self.iAngle = 0
        self.iRadius = 0

        # Services, for live intensities plots via live_wing_histograms.py
        self.service_trackerdata    = rospy.Service('trackerdata_'+self.name, SrvTrackerdata, self.serve_trackerdata_callback)

    
    # set_params()
    # Set the given params dict into this object.
    #
    def set_params(self, params):
        MotionTrackedBodypartPolar.set_params(self, params)
        
        self.imgRoiBackground = None
        self.iCount = 0
        self.state.intensity = 0.0
        self.state.angles = []
        self.state.gradients = []

        # Compute the 'handedness' of the head/abdomen and wing/wing axes.  self.sense specifies the direction of positive angles.
        matAxes = np.array([[self.params['gui']['head']['hinge']['x']-self.params['gui']['abdomen']['hinge']['x'], self.params['gui']['head']['hinge']['y']-self.params['gui']['abdomen']['hinge']['y']],
                            [self.params['gui']['right']['hinge']['x']-self.params['gui']['left']['hinge']['x'], self.params['gui']['right']['hinge']['y']-self.params['gui']['left']['hinge']['y']]])
        if (self.name in ['left','right']):
            self.senseAxes = np.sign(np.linalg.det(matAxes))
            a = -1 if (self.name=='right') else 1
            self.sense = a*self.senseAxes
        else:
            self.sense = 1  

        self.detector.set_params(params[self.name]['threshold'], self.sense)
    
        
    # update_state()
    #
    def update_state(self):
        imgNow = self.imgRoiFgMaskedPolarCropped
        
        if (imgNow is not None):
            # Pixel position and strength of the edges.
            (self.iAngle, self.iRadius) = self.detector.detect(imgNow)

            if (self.iAngle is not None) and (self.iRadius is not None):
                # Convert pixel to angle units, and put angle into the wing frame.
                anglePerPixel = (self.params['gui'][self.name]['angle_hi']-self.params['gui'][self.name]['angle_lo']) / float(imgNow.shape[1])
                angle_b = self.params['gui'][self.name]['angle_lo'] + self.iAngle * anglePerPixel
                angle_p = (self.transform_angle_p_from_b(angle_b) + np.pi) % (2*np.pi) - np.pi
                self.state.angles = [angle_p]
                self.state.gradients = self.detector.diffs
                
                radius = self.params['gui'][self.name]['radius_inner'] + self.iRadius
                self.state.radii = [radius]
                self.state.intensity = np.mean(imgNow)/255.0
            else:
                self.state.angles = []
                self.state.radii = []
                self.state.gradients = self.detector.diffs
                self.state.intensity = np.mean(imgNow)/255.0
                

        
    # update()
    # Update all the internals given a foreground camera image.
    #
    def update(self, dt, image, bInvertColor):
        MotionTrackedBodypartPolar.update(self, dt, image, bInvertColor)

        if (self.params['gui'][self.name]['track']):
            self.update_state()
            self.windowTip.set_image(self.imgRoiFgMaskedPolarCropped)
            
            
    
    
    # draw()
    # Draw the state.
    #
    def draw(self, image):
        MotionTrackedBodypartPolar.draw(self, image)

        if (self.params['gui'][self.name]['track']) and (len(self.state.angles)>0):
            # Draw the bodypart state position.
            angle = self.state.angles[0] * self.sense
            pt = self.R.dot([self.state.radii[0]*np.cos(angle), 
                             self.state.radii[0]*np.sin(angle)]) 
            ptState_i = imageprocessing.clip_pt((int(pt[0]+self.params['gui'][self.name]['hinge']['x']), 
                                                 int(pt[1]+self.params['gui'][self.name]['hinge']['y'])), image.shape) 
            
            cv2.ellipse(image,
                        ptState_i,
                        (2,2),
                        0,
                        0,
                        360,
                        self.bgra_state, 
                        1)
            
            # Draw line from hinge to state point.        
            ptHinge_i = imageprocessing.clip_pt((int(self.ptHinge_i[0]), int(self.ptHinge_i[1])), image.shape) 
            cv2.line(image, ptHinge_i, ptState_i, self.bgra_state, 1)
            self.windowTip.show()
        
        
    def serve_trackerdata_callback(self, request):
        title1      = 'Intensities'
        title2      = 'Diffs'
        intensities = list(self.detector.intensities)
        diffs       = list(np.abs(self.detector.diffs))
        abscissa    = range(len(intensities)) #range(self.params['gui'][self.name]['radius_inner'], self.params['gui'][self.name]['radius_inner'] + len(intensities))
        markersH    = [self.iRadius] #self.state.radii
        markersV    = [self.params[self.name]['threshold']]
        
        return SrvTrackerdataResponse(self.color, title1, title2, abscissa, intensities, diffs, markersH, markersV)
コード例 #22
0
ファイル: bodypart_motion.py プロジェクト: ptweir/Kinefly
class MotionTrackedBodypartPolar(MotionTrackedBodypart):
    def __init__(self, name=None, params={}, color='white', bEqualizeHist=False):
        MotionTrackedBodypart.__init__(self, name, params, color, bEqualizeHist)

        self.polartransforms                    = imageprocessing.PolarTransforms()
        self.create_wfn                         = imageprocessing.WindowFunctions().create_tukey
        self.wfnRoiMaskedPolarCropped           = None
        
        self.imgRoiFgMaskedPolar                = None
        self.imgRoiFgMaskedPolarCropped         = None
        self.imgRoiFgMaskedPolarCroppedWindowed = None
        self.imgFinal                           = None # The image to use for tracking.
        self.imgHeadroomPolar                   = None
        
        self.windowPolar                        = ImageWindow(False, self.name+'Polar')
        
        
    def set_params(self, params):
        MotionTrackedBodypart.set_params(self, params)
        self.windowPolar.set_enable(self.params['gui']['windows'] and self.params['gui'][self.name]['track'])

        
    def create_mask(self, shape):
        MotionTrackedBodypart.create_mask(self, shape)

        self.wfnRoiMaskedPolarCropped = None

        if (self.mask.xMin is not None): 
            self.i_0 = self.params['gui'][self.name]['hinge']['y'] - self.roi[1]
            self.j_0 = self.params['gui'][self.name]['hinge']['x'] - self.roi[0]

    
            # Args for the ellipse calls.
            x = int(self.params['gui'][self.name]['hinge']['x'])
            y = int(self.params['gui'][self.name]['hinge']['y'])
            r_outer = int(np.ceil(self.params['gui'][self.name]['radius_outer']))
            r_inner = int(np.floor(self.params['gui'][self.name]['radius_inner']))-1
            hi = int(np.ceil(np.rad2deg(self.angle_hi_i)))
            lo = int(np.floor(np.rad2deg(self.angle_lo_i)))

    
            # Find where the mask might be clipped.  First, draw an unclipped ellipse.        
            delta = 1
            ptsUnclipped =                         cv2.ellipse2Poly((x, y), (r_outer, r_outer), 0, hi, lo, delta)
            ptsUnclipped = np.append(ptsUnclipped, cv2.ellipse2Poly((x, y), (r_inner, r_inner), 0, hi, lo, delta), 0)
            #ptsUnclipped = np.append(ptsUnclipped, [list of line1 pixels connecting the two arcs], 0) 
            #ptsUnclipped = np.append(ptsUnclipped, [list of line2 pixels connecting the two arcs], 0) 
    
    
            # These are the unclipped locations.        
            minUnclipped = ptsUnclipped.min(0)
            maxUnclipped = ptsUnclipped.max(0)
            xMinUnclipped = minUnclipped[0]
            yMinUnclipped = minUnclipped[1]
            xMaxUnclipped = maxUnclipped[0]+1
            yMaxUnclipped = maxUnclipped[1]+1

            # Compare unclipped with the as-drawn locations.
            xClip0 = self.mask.xMin - xMinUnclipped
            yClip0 = self.mask.yMin - yMinUnclipped
            xClip1 = xMaxUnclipped - self.mask.xMax
            yClip1 = yMaxUnclipped - self.mask.yMax
            roiClipped = np.array([xClip0, yClip0, xClip1, yClip1])
    
            (i_n, j_n) = shape[:2]
                    
            # Determine how much of the bottom of the polar image to trim off (i.e. rClip) based on if the ellipse is partially offimage.
            (rClip0, rClip1, rClip2, rClip3) = (1.0, 1.0, 1.0, 1.0)
            if (roiClipped[0]>0): # Left
                rClip0 = 1.0 - (float(roiClipped[0])/float(r_outer-r_inner))#self.j_0))
            if (roiClipped[1]>0): # Top
                rClip1 = 1.0 - (float(roiClipped[1])/float(r_outer-r_inner))#self.i_0))
            if (roiClipped[2]>0): # Right
                rClip2 = 1.0 - (float(roiClipped[2])/float(r_outer-r_inner))#j_n-self.j_0))
            if (roiClipped[3]>0): # Bottom
                rClip3 = 1.0 - (float(roiClipped[3])/float(r_outer-r_inner))#i_n-self.i_0))
    
            self.rClip = np.min([rClip0, rClip1, rClip2, rClip3])
            
        # End create_mask()

        
    def update_polarimage(self):
        if (self.imgRoiFgMasked is not None):
            theta_0a = self.angle_lo_i - self.angleBodypart_i
            theta_1a = self.angle_hi_i - self.angleBodypart_i
            
            radius_mid = (self.params['gui'][self.name]['radius_outer'] + self.params['gui'][self.name]['radius_inner'])/2.0
            dr         = (self.params['gui'][self.name]['radius_outer'] - self.params['gui'][self.name]['radius_inner'])/2.0

            self.imgRoiFgMasked[self.imgRoiFgMasked==255] = 254 # Make room the for +1, next.
            try:
                self.imgRoiFgMaskedPolar  = self.polartransforms.transform_polar_elliptical(self.imgRoiFgMasked+1, # +1 so we find cropped pixels, next, rather than merely black pixels. 
                                                         self.i_0, 
                                                         self.j_0, 
                                                         raxial=radius_mid, 
                                                         rortho=radius_mid,
                                                         dradiusStrip=int(dr),
                                                         amplifyRho = 1.0,
                                                         rClip = self.rClip,
                                                         angleEllipse=self.angleBodypart_i,
                                                         theta_0 = min(theta_0a,theta_1a), 
                                                         theta_1 = max(theta_0a,theta_1a),
                                                         amplifyTheta = 1.0)
            except imageprocessing.TransformException:
                rospy.logwarn('%s: Mask is outside the image, no points transformed.' % self.name)
                
            # Find the y value where the black band should be cropped out (but leave at least one raster if image is all-black).
            sumY = np.sum(self.imgRoiFgMaskedPolar,1)
            iSumY = np.where(sumY==0)[0]
            if (len(iSumY)>0):
                iMinY = np.max([1,np.min(iSumY)])
            else:
                iMinY = self.imgRoiFgMaskedPolar.shape[0]


            # Crop the bottom of the images.
            self.imgRoiFgMaskedPolarCropped = self.imgRoiFgMaskedPolar[0:iMinY]

            
            # Push each pixel toward the column mean, depending on the amount of headroom.
            if (self.imgHeadroom is not None) and (self.params['gui'][self.name]['subtract_bg']) and (self.params[self.name]['saturation_correction']):
                try:
                    self.imgHeadroomPolar  = self.polartransforms.transform_polar_elliptical(self.imgHeadroom, 
                                                             self.i_0, 
                                                             self.j_0, 
                                                             raxial=radius_mid, 
                                                             rortho=radius_mid,
                                                             dradiusStrip=int(dr),
                                                             amplifyRho = 1.0,
                                                             rClip = self.rClip,
                                                             angleEllipse=self.angleBodypart_i,
                                                             theta_0 = min(theta_0a,theta_1a), 
                                                             theta_1 = max(theta_0a,theta_1a),
                                                             amplifyTheta = 1.0)
                except imageprocessing.TransformException:
                    pass
                
                self.imgHeadroomPolarCropped = self.imgHeadroomPolar[0:iMinY]
            
                # Perform the correction.
                TH = self.imgHeadroomPolarCropped / 255.0
                M = np.mean(self.imgRoiFgMaskedPolarCropped, 0).astype(np.float32)
                TF = self.imgRoiFgMaskedPolarCropped.astype(np.float32)
                self.imgRoiFgMaskedPolarCropped  = M + cv2.multiply(TH, TF-M)
            
            
            if (self.bValidMask):
                if (self.wfnRoiMaskedPolarCropped is None) or (self.imgRoiFgMaskedPolarCropped.shape != self.wfnRoiMaskedPolarCropped.shape):
                    self.wfnRoiMaskedPolarCropped = self.create_wfn(self.imgRoiFgMaskedPolarCropped.shape, self.params[self.name]['feathering'])
                
                self.imgRoiFgMaskedPolarCroppedWindowed = cv2.multiply(self.imgRoiFgMaskedPolarCropped.astype(np.float32), self.wfnRoiMaskedPolarCropped)

            
        # Show the image.
        img = self.imgRoiFgMaskedPolarCroppedWindowed
        #img = self.imgRoiFgMaskedPolarCropped
        self.windowPolar.set_image(img)
        
    # End update_polarimage()
        

    def update(self, dt, image, bInvertColor):
        MotionTrackedBodypart.update(self, dt, image, bInvertColor)
        if (self.params['gui'][self.name]['track']):
            self.update_polarimage()
            self.imgFinal = self.imgRoiFgMaskedPolarCroppedWindowed
        

        
    def draw(self, image):
        MotionTrackedBodypart.draw(self, image)
        self.windowPolar.show()
コード例 #23
0
class EdgeTrackerByHoughTransform(MotionTrackedBodypart):
    def __init__(self, name=None, params={}, color='white', bEqualizeHist=False):
        MotionTrackedBodypart.__init__(self, name, params, color, bEqualizeHist)
        
        self.name       = name
        self.detector   = EdgeDetectorByHoughTransform(self.name, params)
        self.state      = MsgState()
        self.windowEdges = ImageWindow(False, self.name+'Edges')
        self.set_params(params)

        # Services, for live intensities plots via live_wing_histograms.py
        self.service_trackerdata    = rospy.Service('trackerdata_'+self.name, SrvTrackerdata, self.serve_trackerdata_callback)

    
    # set_params()
    # Set the given params dict into this object.
    #
    def set_params(self, params):
        MotionTrackedBodypart.set_params(self, params)
        
        self.imgRoiBackground = None
        self.iCount = 0
        
        self.state.intensity = 0.0
        self.state.angles = []
        self.state.gradients = []

        # Compute the 'handedness' of the head/abdomen and wing/wing axes.  self.sense specifies the direction of positive angles.
        matAxes = np.array([[self.params['gui']['head']['hinge']['x']-self.params['gui']['abdomen']['hinge']['x'], self.params['gui']['head']['hinge']['y']-self.params['gui']['abdomen']['hinge']['y']],
                            [self.params['gui']['right']['hinge']['x']-self.params['gui']['left']['hinge']['x'],   self.params['gui']['right']['hinge']['y']-self.params['gui']['left']['hinge']['y']]])
        if (self.name in ['left','right']):
            self.senseAxes = np.sign(np.linalg.det(matAxes))
            a = -1 if (self.name=='right') else 1
            self.sense = a*self.senseAxes
        else:
            self.sense = 1  

        self.windowEdges.set_enable(self.params['gui']['windows'] and self.params['gui'][self.name]['track'])
        self.bValidDetector = False
        
    
        
    # update_state()
    #
    def update_state(self):
        imgNow = self.imgRoiFgMasked
        
        if (imgNow is not None):
            # Pixel position and strength of the edges.
            (angles, magnitudes) = self.detector.detect(imgNow)
            self.windowEdges.set_image(self.detector.imgMasked)
            
            # Put angle into the bodypart frame.
            self.state.angles = []
            self.state.gradients = []         
            for i in range(len(angles)):
                angle = angles[i]
                magnitude = magnitudes[i]
                angle_b = self.params['gui'][self.name]['angle_lo'] + angle
                angle_p = (self.transform_angle_p_from_b(angle_b) + np.pi) % (2*np.pi) - np.pi
                self.state.angles.append(angle)
                self.state.gradients.append(magnitude)
                

            self.state.intensity = np.mean(self.imgRoiFgMasked)/255.0

        
    # update()
    # Update all the internals given a foreground camera image.
    #
    def update(self, dt, image, bInvertColor):
        MotionTrackedBodypart.update(self, dt, image, bInvertColor)

        if (self.params['gui'][self.name]['track']):
            if (not self.bValidDetector):            
                if (self.mask.xMin is not None):
                    
                    self.detector.set_params(self.name,
                                             self.params,
                                             self.sense,
                                             self.angleBodypart_i,
                                             self.angleBodypart_b,
                                             image.shape,
                                             self.mask)
                    self.bValidDetector = True
    
            self.update_state()
    
    # draw()
    # Draw the outline.
    #
    def draw(self, image):
        MotionTrackedBodypart.draw(self, image)
        
        if (self.params['gui'][self.name]['track']):
            # Draw the major and minor edges alternately, until the max number has been reached.
            bgra = self.bgra
            for i in range(len(self.state.angles)):
                angle_b = self.transform_angle_b_from_p(self.state.angles[i])
                angle_i = self.transform_angle_i_from_b(angle_b)

                x0 = self.ptHinge_i[0] + self.params['gui'][self.name]['radius_inner'] * np.cos(angle_i)
                y0 = self.ptHinge_i[1] + self.params['gui'][self.name]['radius_inner'] * np.sin(angle_i)
                x1 = self.ptHinge_i[0] + self.params['gui'][self.name]['radius_outer'] * np.cos(angle_i)
                y1 = self.ptHinge_i[1] + self.params['gui'][self.name]['radius_outer'] * np.sin(angle_i)
                cv2.line(image, (int(x0),int(y0)), (int(x1),int(y1)), bgra, 1)
                bgra = tuple(0.5*np.array(bgra))
                
            self.windowEdges.show()

        
    def serve_trackerdata_callback(self, request):
        if (len(self.detector.intensities)>0):
            title1      = 'Intensities'
            title2      = 'Diffs'
            diffs = self.detector.diff #np.zeros(len(self.detector.intensities))
            abscissa = np.linspace(self.params['gui'][self.name]['angle_lo'], self.params['gui'][self.name]['angle_hi'], len(self.detector.intensities))
            abscissa -= self.angleBodypart_b
            abscissa *= self.sense
    
            markersH = self.state.angles
            markersV = self.state.gradients#[self.params[self.name]['threshold']]
            
                
            rv = SrvTrackerdataResponse(self.color, title1, title2, abscissa, self.detector.intensities, diffs, markersH, markersV)
        else:
            rv = SrvTrackerdataResponse()
            
        return rv
コード例 #24
0
ファイル: bodypart_intensity.py プロジェクト: ptweir/Kinefly
class IntensityTrackedBodypart(object):
    def __init__(self, name=None, params={}, color='white', bEqualizeHist=False):
        self.name        = name
        self.bEqualizeHist = bEqualizeHist

        self.bValidMask = False

        self.bgra        = ui.bgra_dict[color]
        self.bgra_dim    = tuple(0.5*np.array(ui.bgra_dict[color]))
        self.bgra_state  = ui.bgra_dict[color]#ui.bgra_dict['red']
        self.pixelmax    = 255.0

        self.shape     = (np.inf, np.inf)
        self.ptCenter_i = np.array([0,0])
        self.roi       = None
        
        self.mask = Struct()
        self.mask.img = None
        self.mask.sum = 1.0
        
        self.dt = np.inf

        self.params = {}
        self.handles = {'center':ui.Handle(np.array([0,0]), self.bgra, name='center'),
                        'radius1':ui.Handle(np.array([0,0]), self.bgra, name='radius1'),
                        'radius2':ui.Handle(np.array([0,0]), self.bgra, name='radius2')
                        }
        
        # Region of interest images.
        self.imgFullBackground                  = None
        self.imgRoiBackground                   = None
        self.imgRoi                             = None # Untouched roi image.
        self.imgRoiFg                           = None # Background subtracted.
        self.imgRoiFgMasked                     = None

        # Extra windows.
        self.windowBG         = ImageWindow(False, self.name+'BG')
        self.windowFG         = ImageWindow(False, self.name+'FG')
        self.windowMask       = ImageWindow(gbShowMasks, self.name+'Mask')


    
    # set_params()
    # Set the given params dict into this object, and cache a few values.
    #
    def set_params(self, params):
        self.params = params

        self.rc_background = self.params['rc_background']
        self.ptCenter_i = np.array([self.params['gui'][self.name]['center']['x'], self.params['gui'][self.name]['center']['y']])
        
        self.cosAngle = np.cos(self.params['gui'][self.name]['angle'])
        self.sinAngle = np.sin(self.params['gui'][self.name]['angle'])

        # Turn on/off the extra windows.
        self.windowBG.set_enable(self.params['gui']['windows'] and self.params['gui'][self.name]['track'] and self.params['gui'][self.name]['subtract_bg'])
        self.windowFG.set_enable(self.params['gui']['windows'] and self.params['gui'][self.name]['track'])

        # Refresh the handle points.
        self.update_handle_points()



    # create_mask()
    # Create elliptical wedge masks, and window functions.
    #
    def create_mask(self, shape):
        x     = int(self.params['gui'][self.name]['center']['x'])
        y     = int(self.params['gui'][self.name]['center']['y'])
        r1    = int(self.params['gui'][self.name]['radius1'])
        r2    = int(self.params['gui'][self.name]['radius2'])
        angle = self.params['gui'][self.name]['angle']
        bgra  = ui.bgra_dict['white']
        
        # Create the mask.
        img = np.zeros(shape, dtype=np.uint8)
        cv2.ellipse(img, (x, y), (r1, r2), int(np.rad2deg(angle)), 0, 360, bgra, cv.CV_FILLED)
        self.windowMask.set_image(img)
        
        # Find the ROI of the mask.
        xSum = np.sum(img, 0)
        ySum = np.sum(img, 1)
        xMask = np.where(xSum>0)[0]
        yMask = np.where(ySum>0)[0]
        
        if (len(xMask)>0) and (len(yMask)>0): 
            # Dilate with a border.
            xMin = np.where(xSum>0)[0][0]  
            xMax = np.where(xSum>0)[0][-1] + 1
            yMin = np.where(ySum>0)[0][0]  
            yMax = np.where(ySum>0)[0][-1] + 1
            
            # Clip border to image edges.
            self.mask.xMin = np.max([0,xMin])
            self.mask.yMin = np.max([0,yMin])
            self.mask.xMax = np.min([xMax, shape[1]-1])
            self.mask.yMax = np.min([yMax, shape[0]-1])
            
            self.roi = np.array([self.mask.xMin, self.mask.yMin, self.mask.xMax, self.mask.yMax])
            self.mask.img = img[self.mask.yMin:self.mask.yMax, self.mask.xMin:self.mask.xMax]
            self.mask.sum = np.sum(self.mask.img).astype(np.float32)
    
            self.bValidMask = True
        else:
            self.mask.xMin = None
            self.mask.yMin = None
            self.mask.xMax = None
            self.mask.yMax = None
            
            rospy.logwarn('%s: Empty mask.' % self.name)
    
        
    # set_background()
    # Set the given image as the background image.
    #                
    def set_background(self, image):
        self.imgFullBackground = image.astype(np.float32)
        self.imgRoiBackground = None
        
        
    # invert_background()
    # Invert the color of the background image.
    #                
    def invert_background(self):
        if (self.imgRoiBackground is not None):
            self.imgRoiBackground = 255-self.imgRoiBackground
        
        
    def update_background(self):
        alphaBackground = 1.0 - np.exp(-self.dt / self.rc_background)

        if (self.imgRoiBackground is not None):
            if (self.imgRoi is not None):
                if (self.imgRoiBackground.size==self.imgRoi.size):
                    cv2.accumulateWeighted(self.imgRoi.astype(np.float32), self.imgRoiBackground, alphaBackground)
                else:
                    self.imgRoiBackground = None
                    self.imgRoi = None
        else:
            if (self.imgFullBackground is not None) and (self.roi is not None):
                self.imgRoiBackground = copy.deepcopy(self.imgFullBackground[self.roi[1]:self.roi[3], self.roi[0]:self.roi[2]])
                
        self.windowBG.set_image(self.imgRoiBackground)
        

    def update_roi(self, image, bInvertColor):
        self.shape = image.shape

        # Extract the ROI images.
        if (self.roi is not None):
            self.imgRoi = copy.deepcopy(image[self.roi[1]:self.roi[3], self.roi[0]:self.roi[2]])
    
    
            # Background Subtraction.
            if (bInvertColor):
                self.imgRoiFg = 255-self.imgRoi
            else:
                self.imgRoiFg = self.imgRoi

            if (self.params['gui'][self.name]['subtract_bg']):
                if (self.imgRoiBackground is not None):
                    if (self.imgRoiBackground.shape==self.imgRoiFg.shape):
                        if (bInvertColor):
                            self.imgRoiFg = cv2.absdiff(self.imgRoiFg, 255-self.imgRoiBackground.astype(np.uint8))
                        else:
                            self.imgRoiFg = cv2.absdiff(self.imgRoiFg, self.imgRoiBackground.astype(np.uint8))
    
    
                    
                
            # Equalize the brightness/contrast.
            if (self.bEqualizeHist):
                if (self.imgRoiFg is not None):
                    self.imgRoiFg -= np.min(self.imgRoiFg)
                    max2 = np.max(self.imgRoiFg)
                    self.imgRoiFg *= (255.0/float(max2))
                
            # Apply the mask.
            if (self.mask.img is not None):
                self.imgRoiFgMasked = cv2.bitwise_and(self.imgRoiFg, self.mask.img)
    
            self.windowFG.set_image(self.imgRoiFgMasked) 
        

            
    # update_handle_points()
    # Update the dictionary of handle point names and locations.
    # Compute the various handle points.
    #
    def update_handle_points (self):
        x = self.params['gui'][self.name]['center']['x']
        y = self.params['gui'][self.name]['center']['y']
        radius1 = self.params['gui'][self.name]['radius1']
        radius2 = self.params['gui'][self.name]['radius2']
        angle = self.params['gui'][self.name]['angle']
        
        
        self.handles['center'].pt  = np.array([x, y])
        self.handles['radius1'].pt = np.array([x, y]) + radius1 * np.array([self.cosAngle, self.sinAngle])
        self.handles['radius2'].pt = np.array([x, y]) + radius2 * np.array([self.sinAngle,-self.cosAngle])

        
    # update()
    # Update all the internals given a foreground camera image.
    #
    def update(self, dt, image, bInvertColor):
        self.iCount += 1
        
        self.dt = dt
        
        if (self.params['gui'][self.name]['track']):
            if (not self.bValidMask):
                self.create_mask(image.shape)
                self.bValidMask = True
                
            self.update_background()
            self.update_roi(image, bInvertColor)


    # hit_object()
    # Get the UI object, if any, that the mouse is on.    
    def hit_object(self, ptMouse):
        tag = None
        
        # Check for handle hits.
        if (self.params['gui'][self.name]['track']):
            for tagHandle,handle in self.handles.iteritems():
                if (handle.hit_test(ptMouse)):
                    tag = tagHandle
                    break
                
        return (self.name, tag)
    

    def draw_handles(self, image):
        # Draw all handle points, or only just the hinge handle.
        if (self.params['gui'][self.name]['track']):
            for tagHandle,handle in self.handles.iteritems():
                handle.draw(image)

    
    # draw()
    # Draw the outline.
    #
    def draw(self, image):
        self.draw_handles(image)

        if (self.params['gui'][self.name]['track']):
            x = int(self.params['gui'][self.name]['center']['x'])
            y = int(self.params['gui'][self.name]['center']['y'])
            radius1 = int(self.params['gui'][self.name]['radius1'])
            radius2 = int(self.params['gui'][self.name]['radius2'])

            # Draw the outer arc.
            cv2.ellipse(image,
                        (x, y),
                        (radius1, radius2),
                        np.rad2deg(self.params['gui'][self.name]['angle']),
                        0,
                        360,
                        self.bgra, 
                        1)
    
            # Show the extra windows.
            self.windowBG.show()
            self.windowFG.show()
            self.windowMask.show()
コード例 #25
0
    def __init__(self, params={}):
        self.nodename = rospy.get_name().rstrip('/')
        EdgeTracker = EdgeTrackerByIntensityProfile
        #EdgeTracker = EdgeTrackerByHoughTransform

        # Create the body axis tracker.
        self.axis = AxisTracker(name='axis', params=params, color='yellow')

        # Create the head tracker.
        if (params['head']['tracker'] == 'area'):
            self.head = AreaTracker(name='head',
                                    params=params,
                                    color='cyan',
                                    bEqualizeHist=False)
        elif (params['head']['tracker'] == 'edge'):
            self.head = EdgeTracker(name='head',
                                    params=params,
                                    color='cyan',
                                    bEqualizeHist=False)
        elif (params['head']['tracker'] == 'tip'):
            self.head = TipTracker(name='head',
                                   params=params,
                                   color='cyan',
                                   bEqualizeHist=False)
        elif (params['head']['tracker'] == 'intensity'):
            self.head = IntensityTracker(name='head',
                                         params=params,
                                         color='cyan',
                                         bEqualizeHist=False)
        else:
            rospy.logwarn('Head tracker parameter must be one of ['
                          'area'
                          ', '
                          'edge'
                          ', '
                          'tip'
                          ', '
                          'intensity'
                          ']')

        # Create the abdomen tracker.
        if (params['abdomen']['tracker'] == 'area'):
            self.abdomen = AreaTracker(name='abdomen',
                                       params=params,
                                       color='magenta',
                                       bEqualizeHist=False)
        elif (params['abdomen']['tracker'] == 'edge'):
            self.abdomen = EdgeTracker(name='abdomen',
                                       params=params,
                                       color='magenta',
                                       bEqualizeHist=False)
        elif (params['abdomen']['tracker'] == 'tip'):
            self.abdomen = TipTracker(name='abdomen',
                                      params=params,
                                      color='magenta',
                                      bEqualizeHist=False)
        elif (params['abdomen']['tracker'] == 'intensity'):
            self.abdomen = IntensityTracker(name='abdomen',
                                            params=params,
                                            color='magenta',
                                            bEqualizeHist=False)
        else:
            rospy.logwarn('Abdomen tracker parameter must be one of ['
                          'area'
                          ', '
                          'edge'
                          ', '
                          'tip'
                          ', '
                          'intensity'
                          ']')

        # Create the right wing tracker.
        if (params['right']['tracker'] == 'area'):
            self.right = AreaTracker(name='right',
                                     params=params,
                                     color='red',
                                     bEqualizeHist=False)
        elif (params['right']['tracker'] == 'edge'):
            self.right = EdgeTracker(name='right',
                                     params=params,
                                     color='red',
                                     bEqualizeHist=False)
        elif (params['right']['tracker'] == 'tip'):
            self.right = TipTracker(name='right',
                                    params=params,
                                    color='red',
                                    bEqualizeHist=False)
        elif (params['right']['tracker'] == 'intensity'):
            self.right = IntensityTracker(name='right',
                                          params=params,
                                          color='red',
                                          bEqualizeHist=False)
        else:
            rospy.logwarn('Right wing tracker parameter must be one of ['
                          'area'
                          ', '
                          'edge'
                          ', '
                          'tip'
                          ', '
                          'intensity'
                          ']')

        # Create the left wing tracker.
        if (params['left']['tracker'] == 'area'):
            self.left = AreaTracker(name='left',
                                    params=params,
                                    color='green',
                                    bEqualizeHist=False)
        elif (params['left']['tracker'] == 'edge'):
            self.left = EdgeTracker(name='left',
                                    params=params,
                                    color='green',
                                    bEqualizeHist=False)
        elif (params['left']['tracker'] == 'tip'):
            self.left = TipTracker(name='left',
                                   params=params,
                                   color='green',
                                   bEqualizeHist=False)
        elif (params['left']['tracker'] == 'intensity'):
            self.left = IntensityTracker(name='left',
                                         params=params,
                                         color='green',
                                         bEqualizeHist=False)
        else:
            rospy.logwarn('Left wing tracker parameter must be one of ['
                          'area'
                          ', '
                          'edge'
                          ', '
                          'tip'
                          ', '
                          'intensity'
                          ']')

        # Create the aux tracker.
        self.aux = IntensityTracker(name='aux',
                                    params=params,
                                    color='yellow',
                                    bEqualizeHist=False)

        self.windowInvertColorArea = ImageWindow(False, 'InvertColorArea')

        self.bgra_body = ui.bgra_dict['light_gray']
        self.ptBodyIndicator1 = None
        self.ptBodyIndicator2 = None
        self.bInvertColor = False
        self.bInvertColorAuto = True
        self.iCount = 0
        self.stampPrev = None
        self.stampPrevAlt = None
        self.stamp = rospy.Time(0)

        self.pubFlystate = rospy.Publisher(self.nodename + '/flystate',
                                           MsgFlystate,
                                           queue_size=100)
コード例 #26
0
ファイル: bodypart_intensity.py プロジェクト: ptweir/Kinefly
class IntensityTrackedBodypart(object):
    def __init__(self,
                 name=None,
                 params={},
                 color='white',
                 bEqualizeHist=False):
        self.name = name
        self.bEqualizeHist = bEqualizeHist

        self.bValidMask = False

        self.bgra = ui.bgra_dict[color]
        self.bgra_dim = tuple(0.5 * np.array(ui.bgra_dict[color]))
        self.bgra_state = ui.bgra_dict[color]  #ui.bgra_dict['red']
        self.pixelmax = 255.0

        self.shape = (np.inf, np.inf)
        self.ptCenter_i = np.array([0, 0])
        self.roi = None

        self.mask = Struct()
        self.mask.img = None
        self.mask.sum = 1.0

        self.dt = np.inf

        self.params = {}
        self.handles = {
            'center': ui.Handle(np.array([0, 0]), self.bgra, name='center'),
            'radius1': ui.Handle(np.array([0, 0]), self.bgra, name='radius1'),
            'radius2': ui.Handle(np.array([0, 0]), self.bgra, name='radius2')
        }

        # Region of interest images.
        self.imgFullBackground = None
        self.imgRoiBackground = None
        self.imgRoi = None  # Untouched roi image.
        self.imgRoiFg = None  # Background subtracted.
        self.imgRoiFgMasked = None

        # Extra windows.
        self.windowBG = ImageWindow(False, self.name + 'BG')
        self.windowFG = ImageWindow(False, self.name + 'FG')
        self.windowMask = ImageWindow(gbShowMasks, self.name + 'Mask')

    # set_params()
    # Set the given params dict into this object, and cache a few values.
    #
    def set_params(self, params):
        self.params = params

        self.rc_background = self.params['rc_background']
        self.ptCenter_i = np.array([
            self.params['gui'][self.name]['center']['x'],
            self.params['gui'][self.name]['center']['y']
        ])

        self.cosAngle = np.cos(self.params['gui'][self.name]['angle'])
        self.sinAngle = np.sin(self.params['gui'][self.name]['angle'])

        # Turn on/off the extra windows.
        self.windowBG.set_enable(
            self.params['gui']['windows']
            and self.params['gui'][self.name]['track']
            and self.params['gui'][self.name]['subtract_bg'])
        self.windowFG.set_enable(self.params['gui']['windows']
                                 and self.params['gui'][self.name]['track'])

        # Refresh the handle points.
        self.update_handle_points()

    # create_mask()
    # Create elliptical wedge masks, and window functions.
    #
    def create_mask(self, shape):
        x = int(self.params['gui'][self.name]['center']['x'])
        y = int(self.params['gui'][self.name]['center']['y'])
        r1 = int(self.params['gui'][self.name]['radius1'])
        r2 = int(self.params['gui'][self.name]['radius2'])
        angle = self.params['gui'][self.name]['angle']
        bgra = ui.bgra_dict['white']

        # Create the mask.
        img = np.zeros(shape, dtype=np.uint8)
        cv2.ellipse(img, (x, y), (r1, r2), int(np.rad2deg(angle)), 0, 360,
                    bgra, cv.CV_FILLED)
        self.windowMask.set_image(img)

        # Find the ROI of the mask.
        xSum = np.sum(img, 0)
        ySum = np.sum(img, 1)
        xMask = np.where(xSum > 0)[0]
        yMask = np.where(ySum > 0)[0]

        if (len(xMask) > 0) and (len(yMask) > 0):
            # Dilate with a border.
            xMin = np.where(xSum > 0)[0][0]
            xMax = np.where(xSum > 0)[0][-1] + 1
            yMin = np.where(ySum > 0)[0][0]
            yMax = np.where(ySum > 0)[0][-1] + 1

            # Clip border to image edges.
            self.mask.xMin = np.max([0, xMin])
            self.mask.yMin = np.max([0, yMin])
            self.mask.xMax = np.min([xMax, shape[1] - 1])
            self.mask.yMax = np.min([yMax, shape[0] - 1])

            self.roi = np.array([
                self.mask.xMin, self.mask.yMin, self.mask.xMax, self.mask.yMax
            ])
            self.mask.img = img[self.mask.yMin:self.mask.yMax,
                                self.mask.xMin:self.mask.xMax]
            self.mask.sum = np.sum(self.mask.img).astype(np.float32)

            self.bValidMask = True
        else:
            self.mask.xMin = None
            self.mask.yMin = None
            self.mask.xMax = None
            self.mask.yMax = None

            rospy.logwarn('%s: Empty mask.' % self.name)

    # set_background()
    # Set the given image as the background image.
    #
    def set_background(self, image):
        self.imgFullBackground = image.astype(np.float32)
        self.imgRoiBackground = None

    # invert_background()
    # Invert the color of the background image.
    #
    def invert_background(self):
        if (self.imgRoiBackground is not None):
            self.imgRoiBackground = 255 - self.imgRoiBackground

    def update_background(self):
        alphaBackground = 1.0 - np.exp(-self.dt / self.rc_background)

        if (self.imgRoiBackground is not None):
            if (self.imgRoi is not None):
                if (self.imgRoiBackground.size == self.imgRoi.size):
                    cv2.accumulateWeighted(self.imgRoi.astype(np.float32),
                                           self.imgRoiBackground,
                                           alphaBackground)
                else:
                    self.imgRoiBackground = None
                    self.imgRoi = None
        else:
            if (self.imgFullBackground is not None) and (self.roi is not None):
                self.imgRoiBackground = copy.deepcopy(
                    self.imgFullBackground[self.roi[1]:self.roi[3],
                                           self.roi[0]:self.roi[2]])

        self.windowBG.set_image(self.imgRoiBackground)

    def update_roi(self, image, bInvertColor):
        self.shape = image.shape

        # Extract the ROI images.
        if (self.roi is not None):
            self.imgRoi = copy.deepcopy(image[self.roi[1]:self.roi[3],
                                              self.roi[0]:self.roi[2]])

            # Background Subtraction.
            if (bInvertColor):
                self.imgRoiFg = 255 - self.imgRoi
            else:
                self.imgRoiFg = self.imgRoi

            if (self.params['gui'][self.name]['subtract_bg']):
                if (self.imgRoiBackground is not None):
                    if (self.imgRoiBackground.shape == self.imgRoiFg.shape):
                        if (bInvertColor):
                            self.imgRoiFg = cv2.absdiff(
                                self.imgRoiFg,
                                255 - self.imgRoiBackground.astype(np.uint8))
                        else:
                            self.imgRoiFg = cv2.absdiff(
                                self.imgRoiFg,
                                self.imgRoiBackground.astype(np.uint8))

            # Equalize the brightness/contrast.
            if (self.bEqualizeHist):
                if (self.imgRoiFg is not None):
                    self.imgRoiFg -= np.min(self.imgRoiFg)
                    max2 = np.max(self.imgRoiFg)
                    self.imgRoiFg *= (255.0 / float(max2))

            # Apply the mask.
            if (self.mask.img is not None):
                self.imgRoiFgMasked = cv2.bitwise_and(self.imgRoiFg,
                                                      self.mask.img)

            self.windowFG.set_image(self.imgRoiFgMasked)

    # update_handle_points()
    # Update the dictionary of handle point names and locations.
    # Compute the various handle points.
    #
    def update_handle_points(self):
        x = self.params['gui'][self.name]['center']['x']
        y = self.params['gui'][self.name]['center']['y']
        radius1 = self.params['gui'][self.name]['radius1']
        radius2 = self.params['gui'][self.name]['radius2']
        angle = self.params['gui'][self.name]['angle']

        self.handles['center'].pt = np.array([x, y])
        self.handles['radius1'].pt = np.array(
            [x, y]) + radius1 * np.array([self.cosAngle, self.sinAngle])
        self.handles['radius2'].pt = np.array(
            [x, y]) + radius2 * np.array([self.sinAngle, -self.cosAngle])

    # update()
    # Update all the internals given a foreground camera image.
    #
    def update(self, dt, image, bInvertColor):
        self.iCount += 1

        self.dt = dt

        if (self.params['gui'][self.name]['track']):
            if (not self.bValidMask):
                self.create_mask(image.shape)
                self.bValidMask = True

            self.update_background()
            self.update_roi(image, bInvertColor)

    # hit_object()
    # Get the UI object, if any, that the mouse is on.
    def hit_object(self, ptMouse):
        tag = None

        # Check for handle hits.
        if (self.params['gui'][self.name]['track']):
            for tagHandle, handle in self.handles.iteritems():
                if (handle.hit_test(ptMouse)):
                    tag = tagHandle
                    break

        return (self.name, tag)

    def draw_handles(self, image):
        # Draw all handle points, or only just the hinge handle.
        if (self.params['gui'][self.name]['track']):
            for tagHandle, handle in self.handles.iteritems():
                handle.draw(image)

    # draw()
    # Draw the outline.
    #
    def draw(self, image):
        self.draw_handles(image)

        if (self.params['gui'][self.name]['track']):
            x = int(self.params['gui'][self.name]['center']['x'])
            y = int(self.params['gui'][self.name]['center']['y'])
            radius1 = int(self.params['gui'][self.name]['radius1'])
            radius2 = int(self.params['gui'][self.name]['radius2'])

            # Draw the outer arc.
            cv2.ellipse(image, (x, y), (radius1, radius2),
                        np.rad2deg(self.params['gui'][self.name]['angle']), 0,
                        360, self.bgra, 1)

            # Show the extra windows.
            self.windowBG.show()
            self.windowFG.show()
            self.windowMask.show()
コード例 #27
0
class Fly(object):
    def __init__(self, params={}):
        self.nodename = rospy.get_name().rstrip('/')
        EdgeTracker = EdgeTrackerByIntensityProfile
        #EdgeTracker = EdgeTrackerByHoughTransform

        # Create the body axis tracker.
        self.axis = AxisTracker(name='axis', params=params, color='yellow')

        # Create the head tracker.
        if (params['head']['tracker'] == 'area'):
            self.head = AreaTracker(name='head',
                                    params=params,
                                    color='cyan',
                                    bEqualizeHist=False)
        elif (params['head']['tracker'] == 'edge'):
            self.head = EdgeTracker(name='head',
                                    params=params,
                                    color='cyan',
                                    bEqualizeHist=False)
        elif (params['head']['tracker'] == 'tip'):
            self.head = TipTracker(name='head',
                                   params=params,
                                   color='cyan',
                                   bEqualizeHist=False)
        elif (params['head']['tracker'] == 'intensity'):
            self.head = IntensityTracker(name='head',
                                         params=params,
                                         color='cyan',
                                         bEqualizeHist=False)
        else:
            rospy.logwarn('Head tracker parameter must be one of ['
                          'area'
                          ', '
                          'edge'
                          ', '
                          'tip'
                          ', '
                          'intensity'
                          ']')

        # Create the abdomen tracker.
        if (params['abdomen']['tracker'] == 'area'):
            self.abdomen = AreaTracker(name='abdomen',
                                       params=params,
                                       color='magenta',
                                       bEqualizeHist=False)
        elif (params['abdomen']['tracker'] == 'edge'):
            self.abdomen = EdgeTracker(name='abdomen',
                                       params=params,
                                       color='magenta',
                                       bEqualizeHist=False)
        elif (params['abdomen']['tracker'] == 'tip'):
            self.abdomen = TipTracker(name='abdomen',
                                      params=params,
                                      color='magenta',
                                      bEqualizeHist=False)
        elif (params['abdomen']['tracker'] == 'intensity'):
            self.abdomen = IntensityTracker(name='abdomen',
                                            params=params,
                                            color='magenta',
                                            bEqualizeHist=False)
        else:
            rospy.logwarn('Abdomen tracker parameter must be one of ['
                          'area'
                          ', '
                          'edge'
                          ', '
                          'tip'
                          ', '
                          'intensity'
                          ']')

        # Create the right wing tracker.
        if (params['right']['tracker'] == 'area'):
            self.right = AreaTracker(name='right',
                                     params=params,
                                     color='red',
                                     bEqualizeHist=False)
        elif (params['right']['tracker'] == 'edge'):
            self.right = EdgeTracker(name='right',
                                     params=params,
                                     color='red',
                                     bEqualizeHist=False)
        elif (params['right']['tracker'] == 'tip'):
            self.right = TipTracker(name='right',
                                    params=params,
                                    color='red',
                                    bEqualizeHist=False)
        elif (params['right']['tracker'] == 'intensity'):
            self.right = IntensityTracker(name='right',
                                          params=params,
                                          color='red',
                                          bEqualizeHist=False)
        else:
            rospy.logwarn('Right wing tracker parameter must be one of ['
                          'area'
                          ', '
                          'edge'
                          ', '
                          'tip'
                          ', '
                          'intensity'
                          ']')

        # Create the left wing tracker.
        if (params['left']['tracker'] == 'area'):
            self.left = AreaTracker(name='left',
                                    params=params,
                                    color='green',
                                    bEqualizeHist=False)
        elif (params['left']['tracker'] == 'edge'):
            self.left = EdgeTracker(name='left',
                                    params=params,
                                    color='green',
                                    bEqualizeHist=False)
        elif (params['left']['tracker'] == 'tip'):
            self.left = TipTracker(name='left',
                                   params=params,
                                   color='green',
                                   bEqualizeHist=False)
        elif (params['left']['tracker'] == 'intensity'):
            self.left = IntensityTracker(name='left',
                                         params=params,
                                         color='green',
                                         bEqualizeHist=False)
        else:
            rospy.logwarn('Left wing tracker parameter must be one of ['
                          'area'
                          ', '
                          'edge'
                          ', '
                          'tip'
                          ', '
                          'intensity'
                          ']')

        # Create the aux tracker.
        self.aux = IntensityTracker(name='aux',
                                    params=params,
                                    color='yellow',
                                    bEqualizeHist=False)

        self.windowInvertColorArea = ImageWindow(False, 'InvertColorArea')

        self.bgra_body = ui.bgra_dict['light_gray']
        self.ptBodyIndicator1 = None
        self.ptBodyIndicator2 = None
        self.bInvertColor = False
        self.bInvertColorAuto = True
        self.iCount = 0
        self.stampPrev = None
        self.stampPrevAlt = None
        self.stamp = rospy.Time(0)

        self.pubFlystate = rospy.Publisher(self.nodename + '/flystate',
                                           MsgFlystate,
                                           queue_size=100)

    def set_params(self, params):
        self.params = params

        self.axis.set_params(params)
        self.head.set_params(params)
        self.abdomen.set_params(params)
        self.left.set_params(params)
        self.right.set_params(params)
        self.aux.set_params(params)

        pt1 = [
            params['gui']['head']['hinge']['x'],
            params['gui']['head']['hinge']['y']
        ]
        pt2 = [
            params['gui']['abdomen']['hinge']['x'],
            params['gui']['abdomen']['hinge']['y']
        ]
        pt3 = [
            params['gui']['left']['hinge']['x'],
            params['gui']['left']['hinge']['y']
        ]
        pt4 = [
            params['gui']['right']['hinge']['x'],
            params['gui']['right']['hinge']['y']
        ]
        self.ptBodyCenter_i = imageprocessing.get_intersection(
            pt1, pt2, pt3, pt4)

        r = max(params['gui']['left']['radius_outer'],
                params['gui']['right']['radius_outer'])
        self.angleBody_i = self.get_bodyangle_i()
        self.ptBodyIndicator1 = tuple(
            (self.ptBodyCenter_i +
             r * np.array([np.cos(self.angleBody_i),
                           np.sin(self.angleBody_i)])).astype(int))
        self.ptBodyIndicator2 = tuple(
            (self.ptBodyCenter_i -
             r * np.array([np.cos(self.angleBody_i),
                           np.sin(self.angleBody_i)])).astype(int))

        # Radius of an area approximately where the thorax would be.
        self.rInvertColorArea = np.linalg.norm(
            np.array([
                params['gui']['head']['hinge']['x'], params['gui']['head']
                ['hinge']['y']
            ]) - np.array([
                params['gui']['abdomen']['hinge']['x'], params['gui']
                ['abdomen']['hinge']['y']
            ])) / 2.0
        self.bInvertColorValid = False

    def create_masks(self, shapeImage):
        if (self.params['gui']['axis']['track']):
            if (not self.axis.bValidMask):
                self.axis.create_mask(shapeImage)
                self.axis.bValidMask = True

        if (self.params['gui']['head']['track']):
            if (not self.head.bValidMask):
                self.head.create_mask(shapeImage)
                self.head.bValidMask = True

        if (self.params['gui']['abdomen']['track']):
            if (not self.abdomen.bValidMask):
                self.abdomen.create_mask(shapeImage)
                self.abdomen.bValidMask = True

        if (self.params['gui']['right']['track']):
            if (not self.right.bValidMask):
                self.right.create_mask(shapeImage)
                self.right.bValidMask = True

        if (self.params['gui']['left']['track']):
            if (not self.left.bValidMask):
                self.left.create_mask(shapeImage)
                self.left.bValidMask = True

        if (self.params['gui']['aux']['track']):
            if (not self.aux.bValidMask):
                self.aux.create_mask(shapeImage)
                self.aux.bValidMask = True

    def get_bodyangle_i(self):
        angle_i = imageprocessing.get_angle_from_points_i(
            self.abdomen.ptHinge_i, self.head.ptHinge_i)
        #angleBody_i  = (angle_i + np.pi) % (2.0*np.pi) - np.pi
        angleBody_i = angle_i

        return angleBody_i

    # Calculate what we think the bInvertColor flag should be to make white-on-black.
    def get_invertcolor(self, image):
        # Get a roi around the body center.
        xMin = int(
            max(0, self.ptBodyCenter_i[0] - int(0.75 * self.rInvertColorArea)))
        yMin = int(
            max(0, self.ptBodyCenter_i[1] - int(0.75 * self.rInvertColorArea)))
        xMax = int(
            min(self.ptBodyCenter_i[0] + int(0.75 * self.rInvertColorArea),
                image.shape[1] - 1))
        yMax = int(
            min(self.ptBodyCenter_i[1] + int(0.75 * self.rInvertColorArea),
                image.shape[0] - 1))
        imgInvertColorArea = image[yMin:yMax, xMin:xMax]
        self.windowInvertColorArea.set_image(imgInvertColorArea)

        # Midpoint between darkest & lightest colors.
        threshold = np.mean(image)
        #rospy.logwarn((np.min(image), np.median(image), np.mean(image), np.max(image), np.mean(imgInvertColorArea)))

        # If the roi is too dark, then set bInvertColor.
        if (np.mean(imgInvertColorArea) <= threshold):
            bInvertColor = True
        else:
            bInvertColor = False

        return bInvertColor

    def set_background(self, image):
        self.head.set_background(image)
        self.abdomen.set_background(image)
        self.left.set_background(image)
        self.right.set_background(image)
        self.aux.set_background(image)
        self.axis.set_background(image)

    def update_handle_points(self):
        self.head.update_handle_points()
        self.abdomen.update_handle_points()
        self.left.update_handle_points()
        self.right.update_handle_points()
        self.aux.update_handle_points()
        self.axis.update_handle_points()

    def update(self, header=None, image=None):
        if (image is not None):
            self.header = header

            if (not self.bInvertColorValid) and (self.bInvertColorAuto):
                self.bInvertColor = self.get_invertcolor(image)
                self.bInvertColorValid = True

            # Get the dt.  Keep track of both the camera timestamp, and the now() timestamp,
            # and use the now() timestamp only if the camera timestamp isn't changing.
            self.stamp = self.header.stamp
            stampAlt = rospy.Time.now()
            if (self.stampPrev is not None):
                dt = max(0.0, (self.header.stamp - self.stampPrev).to_sec())

                # If the camera is not giving good timestamps, then use our own clock.
                if (dt == 0.0):
                    dt = max(0.0, (stampAlt - self.stampPrevAlt).to_sec())
                    self.stamp = stampAlt

                # If time wrapped, then just assume a value.
                if (dt == 0.0):
                    dt = 1.0

            else:
                dt = np.inf
            self.stampPrev = self.header.stamp
            self.stampPrevAlt = stampAlt

            self.head.update(dt, image, self.bInvertColor)
            self.abdomen.update(dt, image, self.bInvertColor)
            self.left.update(dt, image, self.bInvertColor)
            self.right.update(dt, image, self.bInvertColor)
            self.aux.update(dt, image, self.bInvertColor)
            self.axis.update(dt, image, self.bInvertColor)

    def draw(self, image):
        # Draw line to indicate the body axis.
        cv2.line(image, self.ptBodyIndicator1, self.ptBodyIndicator2,
                 self.bgra_body,
                 1)  # Draw a line longer than just head-to-abdomen.

        self.axis.draw(image)
        self.head.draw(image)
        self.abdomen.draw(image)
        self.left.draw(image)
        self.right.draw(image)
        self.aux.draw(image)

        self.windowInvertColorArea.show()

    def publish(self):
        flystate = MsgFlystate()
        flystate.header = Header(seq=self.iCount,
                                 stamp=self.stamp,
                                 frame_id='Fly')

        if (self.params['gui']['head']['track']):
            flystate.head = self.head.state
        else:
            flystate.head = MsgState()

        if (self.params['gui']['abdomen']['track']):
            flystate.abdomen = self.abdomen.state
        else:
            flystate.abdomen = MsgState()

        if (self.params['gui']['left']['track']):
            flystate.left = self.left.state
        else:
            flystate.left = MsgState()

        if (self.params['gui']['right']['track']):
            flystate.right = self.right.state
        else:
            flystate.right = MsgState()

        if (self.params['gui']['aux']['track']):
            flystate.aux = self.aux.state
        else:
            flystate.aux = MsgState()

        self.iCount += 1

        self.pubFlystate.publish(flystate)
コード例 #28
0
class AreaTracker(MotionTrackedBodypartPolar):
    def __init__(self,
                 name=None,
                 params={},
                 color='white',
                 bEqualizeHist=False):
        MotionTrackedBodypartPolar.__init__(self, name, params, color,
                                            bEqualizeHist)

        self.phasecorr = imageprocessing.PhaseCorrelation()
        self.state = MsgState()  # In the bodypart frame.
        self.stateOrigin_p = MsgState()  # In the bodypart frame.
        self.stateLo_p = MsgState()  # In the bodypart frame.
        self.stateHi_p = MsgState()  # In the bodypart frame.

        self.imgComparison = None

        self.windowStabilized = ImageWindow(False, self.name + 'Stable')
        self.windowComparison = ImageWindow(True, self.name + 'Comparison')
        self.set_params(params)

    # set_params()
    # Set the given params dict into this object.
    #
    def set_params(self, params):
        MotionTrackedBodypartPolar.set_params(self, params)

        self.imgRoiBackground = None
        self.imgComparison = None
        self.windowComparison.set_image(self.imgComparison)

        self.iCount = 0

        # Compute the 'handedness' of the head/abdomen and wing/wing axes.  self.sense specifies the direction of positive angles.
        matAxes = np.array([[
            self.params['gui']['head']['hinge']['x'] -
            self.params['gui']['abdomen']['hinge']['x'],
            self.params['gui']['head']['hinge']['y'] -
            self.params['gui']['abdomen']['hinge']['y']
        ],
                            [
                                self.params['gui']['right']['hinge']['x'] -
                                self.params['gui']['left']['hinge']['x'],
                                self.params['gui']['right']['hinge']['y'] -
                                self.params['gui']['left']['hinge']['y']
                            ]])
        if (self.name in ['left', 'right']):
            self.senseAxes = np.sign(np.linalg.det(matAxes))
            a = -1 if (self.name == 'right') else 1
            self.sense = a * self.senseAxes
        else:
            self.sense = 1

        self.stateOrigin_p.intensity = 0.0
        self.stateOrigin_p.angles = [
            self.transform_angle_p_from_b(
                (self.params['gui'][self.name]['angle_hi'] +
                 self.params['gui'][self.name]['angle_lo']) / 2.0)
        ]  # In the bodypart frame.
        self.stateOrigin_p.angles[0] = (
            (self.stateOrigin_p.angles[0] + np.pi) % (2 * np.pi)) - np.pi

        self.stateOrigin_p.radii = [
            (self.params['gui'][self.name]['radius_outer'] +
             self.params['gui'][self.name]['radius_inner']) / 2.0
        ]

        self.state.intensity = 0.0
        self.state.angles = [0.0]
        self.state.radii = [0.0]

        self.stateLo_p.intensity = np.inf
        self.stateLo_p.angles = [4.0 * np.pi]
        self.stateLo_p.radii = [np.inf]

        self.stateHi_p.intensity = -np.inf
        self.stateHi_p.angles = [-4.0 * np.pi]
        self.stateHi_p.radii = [-np.inf]

        self.windowStabilized.set_enable(
            self.params['gui']['python']
            and self.params['gui'][self.name]['track']
            and self.params['gui'][self.name]['stabilize'])
        self.windowComparison.set_enable(
            self.params['gui']['python']
            and self.params['gui'][self.name]['track'])

    # update_state()
    # Update the bodypart translation & rotation.
    #
    def update_state(self):
        imgNow = self.imgRoiFgMaskedPolarCroppedWindowed

        if (imgNow is not None) and (self.imgComparison is not None):
            # Get the rotation & expansion between images.
            (rShift,
             aShift) = self.phasecorr.get_shift(imgNow, self.imgComparison)

            # Convert polar pixel shifts to radians rotation & pixel expansion.
            angleOffset = self.sense * aShift * (
                self.params['gui'][self.name]['angle_hi'] -
                self.params['gui'][self.name]['angle_lo']) / float(
                    imgNow.shape[1])
            radiusOffset = rShift
            self.state.angles = [(self.stateOrigin_p.angles[0] + angleOffset)]
            self.state.angles = [
                ((self.state.angles[0] + np.pi) % (2 * np.pi)) - np.pi
            ]
            self.state.radii = [self.stateOrigin_p.radii[0] + radiusOffset]

            # Get min,max's
            self.stateLo_p.angles = [
                min(self.stateLo_p.angles[0], self.state.angles[0])
            ]
            self.stateHi_p.angles = [
                max(self.stateHi_p.angles[0], self.state.angles[0])
            ]
            self.stateLo_p.radii = [
                min(self.stateLo_p.radii[0], self.state.radii[0])
            ]
            self.stateHi_p.radii = [
                max(self.stateHi_p.radii[0], self.state.radii[0])
            ]

            # Control the (angle,radius) origin to be at the midpoint of loangle, hiangle
            # Whenever an image appears that is closer to the midpoint, then
            # take that image as the new origin image.  Thus driving the origin image
            # toward the midpoint image over time.
            if (self.params[self.name]['autozero']) and (self.iCount > 100):
                # If we see a current angle that is closer to the mask midpoint than the ref angle
                # (defined as the mipoint of the movement range), then take a new comparison image, and
                # shift the movement range (so that the ref angle becomes the current angle).
                angleRef_p = (self.stateHi_p.angles[0] +
                              self.stateLo_p.angles[0]) / 2.0

                if (angleRef_p < self.state.angles[0] <
                        self.stateOrigin_p.angles[0]) or (
                            self.stateOrigin_p.angles[0] < self.state.angles[0]
                            < angleRef_p):
                    self.imgComparison = imgNow
                    self.windowComparison.set_image(self.imgComparison)

                    # Converge the origin to zero.
                    self.stateLo_p.angles[0] -= angleOffset
                    self.stateHi_p.angles[0] -= angleOffset
                    self.state.angles[0] = self.stateOrigin_p.angles[0]

            # Stabilized image.
            if (self.params['gui'][self.name]['stabilize']):
                # Stabilize the polar image.
                #size = (self.imgRoiFgMaskedPolar.shape[1],
                #        self.imgRoiFgMaskedPolar.shape[0])
                #center = (self.imgRoiFgMaskedPolar.shape[1]/2.0+aShift,
                #          self.imgRoiFgMaskedPolar.shape[0]/2.0+rShift)
                #self.imgStabilized = cv2.getRectSubPix(self.imgRoiFgMaskedPolar, size, center)

                # Stabilize the bodypart in the entire camera image.
                center = (self.params['gui'][self.name]['hinge']['x'],
                          self.params['gui'][self.name]['hinge']['y'])
                size = (self.image.shape[1], self.image.shape[0])

                # Stabilize the rotation.
                T = cv2.getRotationMatrix2D(center,
                                            np.rad2deg(self.state.angles[0]),
                                            1.0)

                # Stabilize the expansion.
                T[0, 2] -= rShift * self.cosAngleBody_i
                T[1, 2] -= rShift * self.sinAngleBody_i

                self.imgStabilized = cv2.warpAffine(self.image, T, size)
                self.windowStabilized.set_image(self.imgStabilized)

        if (self.imgRoiFgMasked is not None):
            self.state.intensity = float(
                np.sum(self.imgRoiFgMasked) / self.mask.sum)
        else:
            self.state.intensity = 0.0

    # update()
    # Update all the internals given a foreground camera image.
    #
    def update(self, dt, image, bInvertColor):
        MotionTrackedBodypartPolar.update(self, dt, image, bInvertColor)

        if (self.imgComparison is None) and (self.iCount > 1):
            self.imgComparison = self.imgRoiFgMaskedPolarCroppedWindowed
            self.windowComparison.set_image(self.imgComparison)

        if (self.params['gui'][self.name]['track']):
            self.update_state()

    # draw()
    # Draw the outline.
    #
    def draw(self, image):
        MotionTrackedBodypartPolar.draw(self, image)

        if (self.params['gui'][self.name]['track']):
            # Draw the bodypart state position.
            angle = self.state.angles[0] * self.sense
            pt = self.R.dot([
                self.state.radii[0] * np.cos(angle),
                self.state.radii[0] * np.sin(angle)
            ])
            ptState_i = imageprocessing.clip_pt(
                (int(pt[0] + self.params['gui'][self.name]['hinge']['x']),
                 int(pt[1] + self.params['gui'][self.name]['hinge']['y'])),
                image.shape)

            cv2.ellipse(image, ptState_i, (2, 2), 0, 0, 360, self.bgra_state,
                        1)

            # Set a pixel at the min/max state positions.
            try:
                angle = self.stateLo_p.angles[0] * self.sense
                pt = self.R.dot([
                    self.state.radii[0] * np.cos(angle),
                    self.state.radii[0] * np.sin(angle)
                ])
                ptStateLo_i = imageprocessing.clip_pt(
                    (int(pt[0] + self.params['gui'][self.name]['hinge']['x']),
                     int(pt[1] + self.params['gui'][self.name]['hinge']['y'])),
                    image.shape)

                angle = self.stateHi_p.angles[0] * self.sense
                pt = self.R.dot([
                    self.state.radii[0] * np.cos(angle),
                    self.state.radii[0] * np.sin(angle)
                ])
                ptStateHi_i = imageprocessing.clip_pt(
                    (int(pt[0] + self.params['gui'][self.name]['hinge']['x']),
                     int(pt[1] + self.params['gui'][self.name]['hinge']['y'])),
                    image.shape)

                # Set the pixels.
                image[ptStateLo_i[1]][ptStateLo_i[0]] = np.array(
                    [255, 255, 255]) - image[ptStateLo_i[1]][ptStateLo_i[0]]
                image[ptStateHi_i[1]][ptStateHi_i[0]] = np.array(
                    [255, 255, 255]) - image[ptStateHi_i[1]][ptStateHi_i[0]]

            except ValueError:
                pass

            # Draw line from hinge to state point.
            ptHinge_i = imageprocessing.clip_pt(
                (int(self.ptHinge_i[0]), int(self.ptHinge_i[1])), image.shape)
            cv2.line(image, ptHinge_i, ptState_i, self.bgra_state, 1)

            self.windowStabilized.show()
            self.windowComparison.show()
コード例 #29
0
ファイル: tracker_area.py プロジェクト: ssafarik/Kinefly
class AreaTracker(MotionTrackedBodypartPolar):
    def __init__(self, name=None, params={}, color='white', bEqualizeHist=False):
        MotionTrackedBodypartPolar.__init__(self, name, params, color, bEqualizeHist)
        
        self.phasecorr          = imageprocessing.PhaseCorrelation()
        self.state              = MsgState() # In the bodypart frame.
        self.stateOrigin_p      = MsgState() # In the bodypart frame.
        self.stateLo_p          = MsgState() # In the bodypart frame.
        self.stateHi_p          = MsgState() # In the bodypart frame.

        self.imgComparison      = None
        
        self.windowStabilized   = ImageWindow(False, self.name+'Stable')
        self.windowComparison   = ImageWindow(True, self.name+'Comparison')
        self.set_params(params)

    
    # set_params()
    # Set the given params dict into this object.
    #
    def set_params(self, params):
        MotionTrackedBodypartPolar.set_params(self, params)
        
        self.imgRoiBackground = None
        self.imgComparison = None
        self.windowComparison.set_image(self.imgComparison)
        
        self.iCount = 0

        # Compute the 'handedness' of the head/abdomen and wing/wing axes.  self.sense specifies the direction of positive angles.
        matAxes = np.array([[self.params['gui']['head']['hinge']['x']-self.params['gui']['abdomen']['hinge']['x'], self.params['gui']['head']['hinge']['y']-self.params['gui']['abdomen']['hinge']['y']],
                            [self.params['gui']['right']['hinge']['x']-self.params['gui']['left']['hinge']['x'], self.params['gui']['right']['hinge']['y']-self.params['gui']['left']['hinge']['y']]])
        if (self.name in ['left','right']):
            self.senseAxes = np.sign(np.linalg.det(matAxes))
            a = -1 if (self.name=='right') else 1
            self.sense = a*self.senseAxes
        else:
            self.sense = 1  


        self.stateOrigin_p.intensity = 0.0
        self.stateOrigin_p.angles    = [self.transform_angle_p_from_b((self.params['gui'][self.name]['angle_hi']+self.params['gui'][self.name]['angle_lo'])/2.0)] # In the bodypart frame.
        self.stateOrigin_p.angles[0] = ((self.stateOrigin_p.angles[0] + np.pi) % (2*np.pi)) - np.pi

        self.stateOrigin_p.radii     = [(self.params['gui'][self.name]['radius_outer']+self.params['gui'][self.name]['radius_inner'])/2.0]
        
        self.state.intensity = 0.0
        self.state.angles    = [0.0]
        self.state.radii     = [0.0]

        self.stateLo_p.intensity = np.inf
        self.stateLo_p.angles    = [4.0*np.pi]
        self.stateLo_p.radii     = [np.inf]

        self.stateHi_p.intensity = -np.inf
        self.stateHi_p.angles    = [-4.0*np.pi]
        self.stateHi_p.radii     = [-np.inf]
        
        
        self.windowStabilized.set_enable(self.params['gui']['windows'] and self.params['gui'][self.name]['track'] and self.params['gui'][self.name]['stabilize'])
        self.windowComparison.set_enable(self.params['gui']['windows'] and self.params['gui'][self.name]['track'])


        
    # update_state()
    # Update the bodypart translation & rotation.
    #
    def update_state(self):
        imgNow = self.imgRoiFgMaskedPolarCroppedWindowed

        
        if (imgNow is not None) and (self.imgComparison is not None):
            # Get the rotation & expansion between images.
            (rShift, aShift) = self.phasecorr.get_shift(imgNow, self.imgComparison)
            
            # Convert polar pixel shifts to radians rotation & pixel expansion.
            angleOffset = self.sense * aShift * (self.params['gui'][self.name]['angle_hi']-self.params['gui'][self.name]['angle_lo']) / float(imgNow.shape[1])
            radiusOffset = rShift
            self.state.angles  = [(self.stateOrigin_p.angles[0] + angleOffset)]
            self.state.angles  = [((self.state.angles[0] + np.pi) % (2*np.pi)) - np.pi]
            self.state.radii   = [self.stateOrigin_p.radii[0] + radiusOffset]

            
            # Get min,max's
            self.stateLo_p.angles  = [min(self.stateLo_p.angles[0], self.state.angles[0])]
            self.stateHi_p.angles  = [max(self.stateHi_p.angles[0], self.state.angles[0])]
            self.stateLo_p.radii   = [min(self.stateLo_p.radii[0], self.state.radii[0])]
            self.stateHi_p.radii   = [max(self.stateHi_p.radii[0], self.state.radii[0])]
            
            # Control the (angle,radius) origin to be at the midpoint of loangle, hiangle
            # Whenever an image appears that is closer to the midpoint, then
            # take that image as the new origin image.  Thus driving the origin image 
            # toward the midpoint image over time.
            if (self.params[self.name]['autozero']) and (self.iCount>100):
                # If we see a current angle that is closer to the mask midpoint than the ref angle
                # (defined as the mipoint of the movement range), then take a new comparison image, and 
                # shift the movement range (so that the ref angle becomes the current angle).
                angleRef_p = (self.stateHi_p.angles[0] + self.stateLo_p.angles[0])/2.0

                if (angleRef_p < self.state.angles[0] < self.stateOrigin_p.angles[0]) or (self.stateOrigin_p.angles[0] < self.state.angles[0] < angleRef_p):
                    self.imgComparison = imgNow
                    self.windowComparison.set_image(self.imgComparison)

                    # Converge the origin to zero.
                    self.stateLo_p.angles[0] -= angleOffset
                    self.stateHi_p.angles[0] -= angleOffset
                    self.state.angles[0] = self.stateOrigin_p.angles[0]
                    

            # Stabilized image.
            if (self.params['gui'][self.name]['stabilize']):
                # Stabilize the polar image.
                #size = (self.imgRoiFgMaskedPolar.shape[1],
                #        self.imgRoiFgMaskedPolar.shape[0])
                #center = (self.imgRoiFgMaskedPolar.shape[1]/2.0+aShift, 
                #          self.imgRoiFgMaskedPolar.shape[0]/2.0+rShift)
                #self.imgStabilized = cv2.getRectSubPix(self.imgRoiFgMaskedPolar, size, center)
                
                # Stabilize the bodypart in the entire camera image.
                center = (self.params['gui'][self.name]['hinge']['x'], self.params['gui'][self.name]['hinge']['y'])
                size = (self.image.shape[1], self.image.shape[0])
                
                # Stabilize the rotation. 
                T = cv2.getRotationMatrix2D(center, np.rad2deg(self.state.angles[0]), 1.0)
                
                # Stabilize the expansion.
                T[0,2] -= rShift * self.cosAngleBody_i
                T[1,2] -= rShift * self.sinAngleBody_i 
                
                self.imgStabilized = cv2.warpAffine(self.image, T, size)
                self.windowStabilized.set_image(self.imgStabilized)

            
        if (self.imgRoiFgMasked is not None):
            self.state.intensity = float(np.sum(self.imgRoiFgMasked) / self.mask.sum)
        else:
            self.state.intensity = 0.0            
        
        
    # update()
    # Update all the internals given a foreground camera image.
    #
    def update(self, dt, image, bInvertColor):
        MotionTrackedBodypartPolar.update(self, dt, image, bInvertColor)

        if (self.imgComparison is None) and (self.iCount>1):
            self.imgComparison = self.imgRoiFgMaskedPolarCroppedWindowed
            self.windowComparison.set_image(self.imgComparison)
        
        if (self.params['gui'][self.name]['track']):
            self.update_state()
            
    
    # draw()
    # Draw the outline.
    #
    def draw(self, image):
        MotionTrackedBodypartPolar.draw(self, image)

        if (self.params['gui'][self.name]['track']):
            # Draw the bodypart state position.
            angle = self.state.angles[0] * self.sense
            pt = self.R.dot([self.state.radii[0]*np.cos(angle), 
                             self.state.radii[0]*np.sin(angle)]) 
            ptState_i = imageprocessing.clip_pt((int(pt[0]+self.params['gui'][self.name]['hinge']['x']), 
                                 int(pt[1]+self.params['gui'][self.name]['hinge']['y'])), image.shape) 
            
            cv2.ellipse(image,
                        ptState_i,
                        (2,2),
                        0,
                        0,
                        360,
                        self.bgra_state, 
                        1)
            
            # Set a pixel at the min/max state positions.
            try:
                angle = self.stateLo_p.angles[0] * self.sense
                pt = self.R.dot([self.state.radii[0]*np.cos(angle), 
                                 self.state.radii[0]*np.sin(angle)]) 
                ptStateLo_i = imageprocessing.clip_pt((int(pt[0]+self.params['gui'][self.name]['hinge']['x']), 
                                       int(pt[1]+self.params['gui'][self.name]['hinge']['y'])), image.shape) 
                
                
                angle = self.stateHi_p.angles[0] * self.sense
                pt = self.R.dot([self.state.radii[0]*np.cos(angle), 
                                 self.state.radii[0]*np.sin(angle)]) 
                ptStateHi_i = imageprocessing.clip_pt((int(pt[0]+self.params['gui'][self.name]['hinge']['x']), 
                                       int(pt[1]+self.params['gui'][self.name]['hinge']['y'])), image.shape) 
                
                # Set the pixels.
                image[ptStateLo_i[1]][ptStateLo_i[0]] = np.array([255,255,255]) - image[ptStateLo_i[1]][ptStateLo_i[0]]
                image[ptStateHi_i[1]][ptStateHi_i[0]] = np.array([255,255,255]) - image[ptStateHi_i[1]][ptStateHi_i[0]]
                
            except ValueError:
                pass

            
            # Draw line from hinge to state point.        
            ptHinge_i = imageprocessing.clip_pt((int(self.ptHinge_i[0]), int(self.ptHinge_i[1])), image.shape) 
            cv2.line(image, ptHinge_i, ptState_i, self.bgra_state, 1)
            
            self.windowStabilized.show()
            self.windowComparison.show()
コード例 #30
0
ファイル: fly.py プロジェクト: ssafarik/Kinefly
    def __init__(self, params={}):
        self.nodename = rospy.get_name().rstrip('/')
        EdgeTracker = EdgeTrackerByIntensityProfile
        #EdgeTracker = EdgeTrackerByHoughTransform


        # Create the body axis tracker.        
        self.axis        = AxisTracker(name='axis',      params=params, color='yellow')

        # Create the head tracker.        
        if (params['head']['tracker']=='area'):
            self.head    = AreaTracker(name='head',      params=params, color='cyan',    bEqualizeHist=False)
        elif (params['head']['tracker']=='edge'):
            self.head    = EdgeTracker(name='head',      params=params, color='cyan',    bEqualizeHist=False)
        elif (params['head']['tracker']=='tip'):
            self.head    = TipTracker(name='head',       params=params, color='cyan',    bEqualizeHist=False)
        elif (params['head']['tracker']=='intensity'):
            self.head    = IntensityTracker(name='head', params=params, color='cyan',    bEqualizeHist=False)
        else:
            rospy.logwarn('Head tracker parameter must be one of [''area'', ''edge'', ''tip'', ''intensity'']')
             
        # Create the abdomen tracker.        
        if (params['abdomen']['tracker']=='area'):
            self.abdomen    = AreaTracker(name='abdomen',      params=params, color='magenta',    bEqualizeHist=False)
        elif (params['abdomen']['tracker']=='edge'):
            self.abdomen    = EdgeTracker(name='abdomen',      params=params, color='magenta',    bEqualizeHist=False)
        elif (params['abdomen']['tracker']=='tip'):
            self.abdomen    = TipTracker(name='abdomen',       params=params, color='magenta',    bEqualizeHist=False)
        elif (params['abdomen']['tracker']=='intensity'):
            self.abdomen    = IntensityTracker(name='abdomen', params=params, color='magenta',    bEqualizeHist=False)
        else:
            rospy.logwarn('Abdomen tracker parameter must be one of [''area'', ''edge'', ''tip'', ''intensity'']')
             
        # Create the right wing tracker.        
        if (params['right']['tracker']=='area'):
            self.right    = AreaTracker(name='right',      params=params, color='red',    bEqualizeHist=False)
        elif (params['right']['tracker']=='edge'):
            self.right    = EdgeTracker(name='right',      params=params, color='red',    bEqualizeHist=False)
        elif (params['right']['tracker']=='tip'):
            self.right    = TipTracker(name='right',       params=params, color='red',    bEqualizeHist=False)
        elif (params['right']['tracker']=='intensity'):
            self.right    = IntensityTracker(name='right', params=params, color='red',    bEqualizeHist=False)
        else:
            rospy.logwarn('Right wing tracker parameter must be one of [''area'', ''edge'', ''tip'', ''intensity'']')
             
        # Create the left wing tracker.        
        if (params['left']['tracker']=='area'):
            self.left    = AreaTracker(name='left',      params=params, color='green',    bEqualizeHist=False)
        elif (params['left']['tracker']=='edge'):
            self.left    = EdgeTracker(name='left',      params=params, color='green',    bEqualizeHist=False)
        elif (params['left']['tracker']=='tip'):
            self.left    = TipTracker(name='left',       params=params, color='green',    bEqualizeHist=False)
        elif (params['left']['tracker']=='intensity'):
            self.left    = IntensityTracker(name='left', params=params, color='green',    bEqualizeHist=False)
        else:
            rospy.logwarn('Left wing tracker parameter must be one of [''area'', ''edge'', ''tip'', ''intensity'']')
             
        # Create the aux tracker.        
        self.aux    = IntensityTracker(name='aux',    params=params, color='yellow',    bEqualizeHist=False)
             

        self.windowInvertColorArea      = ImageWindow(False, 'InvertColorArea')
        
        self.bgra_body = ui.bgra_dict['light_gray']
        self.ptBodyIndicator1 = None
        self.ptBodyIndicator2 = None
        self.bInvertColor = False
        self.bInvertColorAuto = True
        self.iCount  = 0
        self.stampPrev = None
        self.stampPrevAlt = None
        self.stamp = rospy.Time(0)
        

        self.pubFlystate = rospy.Publisher(self.nodename+'/flystate', MsgFlystate, queue_size=100)
コード例 #31
0
class MotionTrackedBodypartPolar(MotionTrackedBodypart):
    def __init__(self,
                 name=None,
                 params={},
                 color='white',
                 bEqualizeHist=False):
        MotionTrackedBodypart.__init__(self, name, params, color,
                                       bEqualizeHist)

        self.polartransforms = imageprocessing.PolarTransforms()
        self.create_wfn = imageprocessing.WindowFunctions().create_tukey
        self.wfnRoiMaskedPolarCropped = None

        self.imgRoiFgMaskedPolar = None
        self.imgRoiFgMaskedPolarCropped = None
        self.imgRoiFgMaskedPolarCroppedWindowed = None
        self.imgFinal = None  # The image to use for tracking.
        self.imgHeadroomPolar = None

        self.windowPolar = ImageWindow(False, self.name + 'Polar')

    def set_params(self, params):
        MotionTrackedBodypart.set_params(self, params)
        self.windowPolar.set_enable(self.params['gui']['windows']
                                    and self.params['gui'][self.name]['track'])

    def create_mask(self, shape):
        MotionTrackedBodypart.create_mask(self, shape)

        self.wfnRoiMaskedPolarCropped = None

        if (self.mask.xMin is not None):
            self.i_0 = self.params['gui'][
                self.name]['hinge']['y'] - self.roi[1]
            self.j_0 = self.params['gui'][
                self.name]['hinge']['x'] - self.roi[0]

            # Args for the ellipse calls.
            x = int(self.params['gui'][self.name]['hinge']['x'])
            y = int(self.params['gui'][self.name]['hinge']['y'])
            r_outer = int(
                np.ceil(self.params['gui'][self.name]['radius_outer']))
            r_inner = int(
                np.floor(self.params['gui'][self.name]['radius_inner'])) - 1
            hi = int(np.ceil(np.rad2deg(self.angle_hi_i)))
            lo = int(np.floor(np.rad2deg(self.angle_lo_i)))

            # Find where the mask might be clipped.  First, draw an unclipped ellipse.
            delta = 1
            ptsUnclipped = cv2.ellipse2Poly((x, y), (r_outer, r_outer), 0, hi,
                                            lo, delta)
            ptsUnclipped = np.append(
                ptsUnclipped,
                cv2.ellipse2Poly((x, y), (r_inner, r_inner), 0, hi, lo, delta),
                0)
            #ptsUnclipped = np.append(ptsUnclipped, [list of line1 pixels connecting the two arcs], 0)
            #ptsUnclipped = np.append(ptsUnclipped, [list of line2 pixels connecting the two arcs], 0)

            # These are the unclipped locations.
            minUnclipped = ptsUnclipped.min(0)
            maxUnclipped = ptsUnclipped.max(0)
            xMinUnclipped = minUnclipped[0]
            yMinUnclipped = minUnclipped[1]
            xMaxUnclipped = maxUnclipped[0] + 1
            yMaxUnclipped = maxUnclipped[1] + 1

            # Compare unclipped with the as-drawn locations.
            xClip0 = self.mask.xMin - xMinUnclipped
            yClip0 = self.mask.yMin - yMinUnclipped
            xClip1 = xMaxUnclipped - self.mask.xMax
            yClip1 = yMaxUnclipped - self.mask.yMax
            roiClipped = np.array([xClip0, yClip0, xClip1, yClip1])

            (i_n, j_n) = shape[:2]

            # Determine how much of the bottom of the polar image to trim off (i.e. rClip) based on if the ellipse is partially offimage.
            (rClip0, rClip1, rClip2, rClip3) = (1.0, 1.0, 1.0, 1.0)
            if (roiClipped[0] > 0):  # Left
                rClip0 = 1.0 - (float(roiClipped[0]) / float(r_outer - r_inner)
                                )  #self.j_0))
            if (roiClipped[1] > 0):  # Top
                rClip1 = 1.0 - (float(roiClipped[1]) / float(r_outer - r_inner)
                                )  #self.i_0))
            if (roiClipped[2] > 0):  # Right
                rClip2 = 1.0 - (float(roiClipped[2]) / float(r_outer - r_inner)
                                )  #j_n-self.j_0))
            if (roiClipped[3] > 0):  # Bottom
                rClip3 = 1.0 - (float(roiClipped[3]) / float(r_outer - r_inner)
                                )  #i_n-self.i_0))

            self.rClip = np.min([rClip0, rClip1, rClip2, rClip3])

        # End create_mask()

    def update_polarimage(self):
        if (self.imgRoiFgMasked is not None):
            theta_0a = self.angle_lo_i - self.angleBodypart_i
            theta_1a = self.angle_hi_i - self.angleBodypart_i

            radius_mid = (self.params['gui'][self.name]['radius_outer'] +
                          self.params['gui'][self.name]['radius_inner']) / 2.0
            dr = (self.params['gui'][self.name]['radius_outer'] -
                  self.params['gui'][self.name]['radius_inner']) / 2.0

            self.imgRoiFgMasked[self.imgRoiFgMasked ==
                                255] = 254  # Make room the for +1, next.
            try:
                self.imgRoiFgMaskedPolar = self.polartransforms.transform_polar_elliptical(
                    self.imgRoiFgMasked +
                    1,  # +1 so we find cropped pixels, next, rather than merely black pixels. 
                    self.i_0,
                    self.j_0,
                    raxial=radius_mid,
                    rortho=radius_mid,
                    dradiusStrip=int(dr),
                    amplifyRho=1.0,
                    rClip=self.rClip,
                    angleEllipse=self.angleBodypart_i,
                    theta_0=min(theta_0a, theta_1a),
                    theta_1=max(theta_0a, theta_1a),
                    amplifyTheta=1.0)
            except imageprocessing.TransformException:
                rospy.logwarn(
                    '%s: Mask is outside the image, no points transformed.' %
                    self.name)

            # Find the y value where the black band should be cropped out (but leave at least one raster if image is all-black).
            sumY = np.sum(self.imgRoiFgMaskedPolar, 1)
            iSumY = np.where(sumY == 0)[0]
            if (len(iSumY) > 0):
                iMinY = np.max([1, np.min(iSumY)])
            else:
                iMinY = self.imgRoiFgMaskedPolar.shape[0]

            # Crop the bottom of the images.
            self.imgRoiFgMaskedPolarCropped = self.imgRoiFgMaskedPolar[0:iMinY]

            # Push each pixel toward the column mean, depending on the amount of headroom.
            if (self.imgHeadroom is not None) and (
                    self.params['gui'][self.name]['subtract_bg']) and (
                        self.params[self.name]['saturation_correction']):
                try:
                    self.imgHeadroomPolar = self.polartransforms.transform_polar_elliptical(
                        self.imgHeadroom,
                        self.i_0,
                        self.j_0,
                        raxial=radius_mid,
                        rortho=radius_mid,
                        dradiusStrip=int(dr),
                        amplifyRho=1.0,
                        rClip=self.rClip,
                        angleEllipse=self.angleBodypart_i,
                        theta_0=min(theta_0a, theta_1a),
                        theta_1=max(theta_0a, theta_1a),
                        amplifyTheta=1.0)
                except imageprocessing.TransformException:
                    pass

                self.imgHeadroomPolarCropped = self.imgHeadroomPolar[0:iMinY]

                # Perform the correction.
                TH = self.imgHeadroomPolarCropped / 255.0
                M = np.mean(self.imgRoiFgMaskedPolarCropped,
                            0).astype(np.float32)
                TF = self.imgRoiFgMaskedPolarCropped.astype(np.float32)
                self.imgRoiFgMaskedPolarCropped = M + cv2.multiply(TH, TF - M)

            if (self.bValidMask):
                if (self.wfnRoiMaskedPolarCropped is
                        None) or (self.imgRoiFgMaskedPolarCropped.shape !=
                                  self.wfnRoiMaskedPolarCropped.shape):
                    self.wfnRoiMaskedPolarCropped = self.create_wfn(
                        self.imgRoiFgMaskedPolarCropped.shape,
                        self.params[self.name]['feathering'])

                self.imgRoiFgMaskedPolarCroppedWindowed = cv2.multiply(
                    self.imgRoiFgMaskedPolarCropped.astype(np.float32),
                    self.wfnRoiMaskedPolarCropped)

        # Show the image.
        img = self.imgRoiFgMaskedPolarCroppedWindowed
        #img = self.imgRoiFgMaskedPolarCropped
        self.windowPolar.set_image(img)

    # End update_polarimage()

    def update(self, dt, image, bInvertColor):
        MotionTrackedBodypart.update(self, dt, image, bInvertColor)
        if (self.params['gui'][self.name]['track']):
            self.update_polarimage()
            self.imgFinal = self.imgRoiFgMaskedPolarCroppedWindowed

    def draw(self, image):
        MotionTrackedBodypart.draw(self, image)
        self.windowPolar.show()
コード例 #32
0
ファイル: tracker_tip.py プロジェクト: ptweir/Kinefly
class TipTracker(MotionTrackedBodypartPolar):
    def __init__(self, name=None, params={}, color='white', bEqualizeHist=False):
        MotionTrackedBodypartPolar.__init__(self, name, params, color, bEqualizeHist)
        
        self.name     = name
        self.detector = TipDetector()
        self.state    = MsgState()
        self.set_params(params)
        self.windowTip = ImageWindow(False, self.name+'Tip')
        self.iAngle = 0
        self.iRadius = 0

        # Services, for live intensities plots via live_wing_histograms.py
        self.service_trackerdata    = rospy.Service('trackerdata_'+self.name, SrvTrackerdata, self.serve_trackerdata_callback)

    
    # set_params()
    # Set the given params dict into this object.
    #
    def set_params(self, params):
        MotionTrackedBodypartPolar.set_params(self, params)
        
        self.imgRoiBackground = None
        self.iCount = 0
        self.state.intensity = 0.0
        self.state.angles = []
        self.state.gradients = []

        # Compute the 'handedness' of the head/abdomen and wing/wing axes.  self.sense specifies the direction of positive angles.
        matAxes = np.array([[self.params['gui']['head']['hinge']['x']-self.params['gui']['abdomen']['hinge']['x'], self.params['gui']['head']['hinge']['y']-self.params['gui']['abdomen']['hinge']['y']],
                            [self.params['gui']['right']['hinge']['x']-self.params['gui']['left']['hinge']['x'], self.params['gui']['right']['hinge']['y']-self.params['gui']['left']['hinge']['y']]])
        if (self.name in ['left','right']):
            self.senseAxes = np.sign(np.linalg.det(matAxes))
            a = -1 if (self.name=='right') else 1
            self.sense = a*self.senseAxes
        else:
            self.sense = 1  

        self.detector.set_params(params[self.name]['threshold'], self.sense)
    
        
    # update_state()
    #
    def update_state(self):
        imgNow = self.imgRoiFgMaskedPolarCropped
        
        if (imgNow is not None):
            # Pixel position and strength of the edges.
            (self.iAngle, self.iRadius) = self.detector.detect(imgNow)

            if (self.iAngle is not None) and (self.iRadius is not None):
                # Convert pixel to angle units, and put angle into the wing frame.
                anglePerPixel = (self.params['gui'][self.name]['angle_hi']-self.params['gui'][self.name]['angle_lo']) / float(imgNow.shape[1])
                angle_b = self.params['gui'][self.name]['angle_lo'] + self.iAngle * anglePerPixel
                angle_p = (self.transform_angle_p_from_b(angle_b) + np.pi) % (2*np.pi) - np.pi
                self.state.angles = [angle_p]
                self.state.gradients = self.detector.diffs
                
                radius = self.params['gui'][self.name]['radius_inner'] + self.iRadius
                self.state.radii = [radius]
                self.state.intensity = np.mean(imgNow)/255.0
            else:
                self.state.angles = []
                self.state.radii = []
                self.state.gradients = self.detector.diffs
                self.state.intensity = np.mean(imgNow)/255.0
                

        
    # update()
    # Update all the internals given a foreground camera image.
    #
    def update(self, dt, image, bInvertColor):
        MotionTrackedBodypartPolar.update(self, dt, image, bInvertColor)

        if (self.params['gui'][self.name]['track']):
            self.update_state()
            self.windowTip.set_image(self.imgRoiFgMaskedPolarCropped)
            
            
    
    
    # draw()
    # Draw the state.
    #
    def draw(self, image):
        MotionTrackedBodypartPolar.draw(self, image)

        if (self.params['gui'][self.name]['track']) and (len(self.state.angles)>0):
            # Draw the bodypart state position.
            angle = self.state.angles[0] * self.sense
            pt = self.R.dot([self.state.radii[0]*np.cos(angle), 
                             self.state.radii[0]*np.sin(angle)]) 
            ptState_i = imageprocessing.clip_pt((int(pt[0]+self.params['gui'][self.name]['hinge']['x']), 
                                                 int(pt[1]+self.params['gui'][self.name]['hinge']['y'])), image.shape) 
            
            cv2.ellipse(image,
                        ptState_i,
                        (2,2),
                        0,
                        0,
                        360,
                        self.bgra_state, 
                        1)
            
            # Draw line from hinge to state point.        
            ptHinge_i = imageprocessing.clip_pt((int(self.ptHinge_i[0]), int(self.ptHinge_i[1])), image.shape) 
            cv2.line(image, ptHinge_i, ptState_i, self.bgra_state, 1)
            self.windowTip.show()
        
        
    def serve_trackerdata_callback(self, request):
        title1      = 'Intensities'
        title2      = 'Diffs'
        intensities = list(self.detector.intensities)
        diffs       = list(np.abs(self.detector.diffs))
        abscissa    = range(len(intensities)) #range(self.params['gui'][self.name]['radius_inner'], self.params['gui'][self.name]['radius_inner'] + len(intensities))
        markersH    = [self.iRadius] #self.state.radii
        markersV    = [self.params[self.name]['threshold']]
        
        return SrvTrackerdataResponse(self.color, title1, title2, abscissa, intensities, diffs, markersH, markersV)
コード例 #33
0
ファイル: fly.py プロジェクト: ssafarik/Kinefly
class Fly(object):
    def __init__(self, params={}):
        self.nodename = rospy.get_name().rstrip('/')
        EdgeTracker = EdgeTrackerByIntensityProfile
        #EdgeTracker = EdgeTrackerByHoughTransform


        # Create the body axis tracker.        
        self.axis        = AxisTracker(name='axis',      params=params, color='yellow')

        # Create the head tracker.        
        if (params['head']['tracker']=='area'):
            self.head    = AreaTracker(name='head',      params=params, color='cyan',    bEqualizeHist=False)
        elif (params['head']['tracker']=='edge'):
            self.head    = EdgeTracker(name='head',      params=params, color='cyan',    bEqualizeHist=False)
        elif (params['head']['tracker']=='tip'):
            self.head    = TipTracker(name='head',       params=params, color='cyan',    bEqualizeHist=False)
        elif (params['head']['tracker']=='intensity'):
            self.head    = IntensityTracker(name='head', params=params, color='cyan',    bEqualizeHist=False)
        else:
            rospy.logwarn('Head tracker parameter must be one of [''area'', ''edge'', ''tip'', ''intensity'']')
             
        # Create the abdomen tracker.        
        if (params['abdomen']['tracker']=='area'):
            self.abdomen    = AreaTracker(name='abdomen',      params=params, color='magenta',    bEqualizeHist=False)
        elif (params['abdomen']['tracker']=='edge'):
            self.abdomen    = EdgeTracker(name='abdomen',      params=params, color='magenta',    bEqualizeHist=False)
        elif (params['abdomen']['tracker']=='tip'):
            self.abdomen    = TipTracker(name='abdomen',       params=params, color='magenta',    bEqualizeHist=False)
        elif (params['abdomen']['tracker']=='intensity'):
            self.abdomen    = IntensityTracker(name='abdomen', params=params, color='magenta',    bEqualizeHist=False)
        else:
            rospy.logwarn('Abdomen tracker parameter must be one of [''area'', ''edge'', ''tip'', ''intensity'']')
             
        # Create the right wing tracker.        
        if (params['right']['tracker']=='area'):
            self.right    = AreaTracker(name='right',      params=params, color='red',    bEqualizeHist=False)
        elif (params['right']['tracker']=='edge'):
            self.right    = EdgeTracker(name='right',      params=params, color='red',    bEqualizeHist=False)
        elif (params['right']['tracker']=='tip'):
            self.right    = TipTracker(name='right',       params=params, color='red',    bEqualizeHist=False)
        elif (params['right']['tracker']=='intensity'):
            self.right    = IntensityTracker(name='right', params=params, color='red',    bEqualizeHist=False)
        else:
            rospy.logwarn('Right wing tracker parameter must be one of [''area'', ''edge'', ''tip'', ''intensity'']')
             
        # Create the left wing tracker.        
        if (params['left']['tracker']=='area'):
            self.left    = AreaTracker(name='left',      params=params, color='green',    bEqualizeHist=False)
        elif (params['left']['tracker']=='edge'):
            self.left    = EdgeTracker(name='left',      params=params, color='green',    bEqualizeHist=False)
        elif (params['left']['tracker']=='tip'):
            self.left    = TipTracker(name='left',       params=params, color='green',    bEqualizeHist=False)
        elif (params['left']['tracker']=='intensity'):
            self.left    = IntensityTracker(name='left', params=params, color='green',    bEqualizeHist=False)
        else:
            rospy.logwarn('Left wing tracker parameter must be one of [''area'', ''edge'', ''tip'', ''intensity'']')
             
        # Create the aux tracker.        
        self.aux    = IntensityTracker(name='aux',    params=params, color='yellow',    bEqualizeHist=False)
             

        self.windowInvertColorArea      = ImageWindow(False, 'InvertColorArea')
        
        self.bgra_body = ui.bgra_dict['light_gray']
        self.ptBodyIndicator1 = None
        self.ptBodyIndicator2 = None
        self.bInvertColor = False
        self.bInvertColorAuto = True
        self.iCount  = 0
        self.stampPrev = None
        self.stampPrevAlt = None
        self.stamp = rospy.Time(0)
        

        self.pubFlystate = rospy.Publisher(self.nodename+'/flystate', MsgFlystate, queue_size=100)
 

    def set_params(self, params):
        self.params = params
        
        self.axis.set_params(params)
        self.head.set_params(params)
        self.abdomen.set_params(params)
        self.left.set_params(params)
        self.right.set_params(params)
        self.aux.set_params(params)

        pt1 = [params['gui']['head']['hinge']['x'], params['gui']['head']['hinge']['y']]
        pt2 = [params['gui']['abdomen']['hinge']['x'], params['gui']['abdomen']['hinge']['y']]
        pt3 = [params['gui']['left']['hinge']['x'], params['gui']['left']['hinge']['y']]
        pt4 = [params['gui']['right']['hinge']['x'], params['gui']['right']['hinge']['y']]
        self.ptBodyCenter_i = imageprocessing.get_intersection(pt1,pt2,pt3,pt4)

        r = max(params['gui']['left']['radius_outer'], params['gui']['right']['radius_outer'])
        self.angleBody_i = self.get_bodyangle_i()
        self.ptBodyIndicator1 = tuple((self.ptBodyCenter_i + r * np.array([np.cos(self.angleBody_i), np.sin(self.angleBody_i)])).astype(int))
        self.ptBodyIndicator2 = tuple((self.ptBodyCenter_i - r * np.array([np.cos(self.angleBody_i), np.sin(self.angleBody_i)])).astype(int))
        
        # Radius of an area approximately where the thorax would be.
        self.rInvertColorArea = np.linalg.norm(np.array([params['gui']['head']['hinge']['x'], params['gui']['head']['hinge']['y']]) - np.array([params['gui']['abdomen']['hinge']['x'], params['gui']['abdomen']['hinge']['y']]))/2.0
        self.bInvertColorValid = False
        
    
    def create_masks(self, shapeImage):
        if (self.params['gui']['axis']['track']):
            if (not self.axis.bValidMask):
                self.axis.create_mask (shapeImage)
                self.axis.bValidMask = True
                
        if (self.params['gui']['head']['track']):
            if (not self.head.bValidMask):
                self.head.create_mask (shapeImage)
                self.head.bValidMask = True
                
        if (self.params['gui']['abdomen']['track']):
            if (not self.abdomen.bValidMask):
                self.abdomen.create_mask (shapeImage)
                self.abdomen.bValidMask = True
            
        if (self.params['gui']['right']['track']):
            if (not self.right.bValidMask):
                self.right.create_mask (shapeImage)
                self.right.bValidMask = True
            
        if (self.params['gui']['left']['track']):
            if (not self.left.bValidMask):
                self.left.create_mask (shapeImage)
                self.left.bValidMask = True
            
        if (self.params['gui']['aux']['track']):
            if (not self.aux.bValidMask):
                self.aux.create_mask (shapeImage)
                self.aux.bValidMask = True


    def get_bodyangle_i(self):
        angle_i = imageprocessing.get_angle_from_points_i(self.abdomen.ptHinge_i, self.head.ptHinge_i)
        #angleBody_i  = (angle_i + np.pi) % (2.0*np.pi) - np.pi
        angleBody_i  = angle_i
        
        return angleBody_i
        

    # Calculate what we think the bInvertColor flag should be to make white-on-black.        
    def get_invertcolor(self, image):
        # Get a roi around the body center.
        xMin = max(0,self.ptBodyCenter_i[0]-int(0.75*self.rInvertColorArea))
        yMin = max(0,self.ptBodyCenter_i[1]-int(0.75*self.rInvertColorArea))
        xMax = min(self.ptBodyCenter_i[0]+int(0.75*self.rInvertColorArea), image.shape[1]-1)
        yMax = min(self.ptBodyCenter_i[1]+int(0.75*self.rInvertColorArea), image.shape[0]-1)
        imgInvertColorArea = image[yMin:yMax, xMin:xMax]
        self.windowInvertColorArea.set_image(imgInvertColorArea)

        # Midpoint between darkest & lightest colors.
        threshold = np.mean(image) 
        #rospy.logwarn((np.min(image), np.median(image), np.mean(image), np.max(image), np.mean(imgInvertColorArea)))
        
        # If the roi is too dark, then set bInvertColor.
        if (np.mean(imgInvertColorArea) <= threshold):
            bInvertColor = True
        else:
            bInvertColor = False

        return bInvertColor
        
        
                
    def set_background(self, image):
        self.head.set_background(image)
        self.abdomen.set_background(image)
        self.left.set_background(image)
        self.right.set_background(image)
        self.aux.set_background(image)
        self.axis.set_background(image)

    
    def update_handle_points(self):
        self.head.update_handle_points()
        self.abdomen.update_handle_points()
        self.left.update_handle_points()
        self.right.update_handle_points()
        self.aux.update_handle_points()
        self.axis.update_handle_points()
        

    def update(self, header=None, image=None):
        if (image is not None):
            self.header = header
            
            if (not self.bInvertColorValid) and (self.bInvertColorAuto):
                self.bInvertColor = self.get_invertcolor(image)
                self.bInvertColorValid = True

            
            # Get the dt.  Keep track of both the camera timestamp, and the now() timestamp,
            # and use the now() timestamp only if the camera timestamp isn't changing.
            self.stamp = self.header.stamp
            stampAlt = rospy.Time.now()
            if (self.stampPrev is not None):
                dt = max(0.0, (self.header.stamp - self.stampPrev).to_sec())
                
                # If the camera is not giving good timestamps, then use our own clock.
                if (dt == 0.0):
                    dt = max(0.0, (stampAlt - self.stampPrevAlt).to_sec())
                    self.stamp = stampAlt

                # If time wrapped, then just assume a value.
                if (dt == 0.0):
                    dt = 1.0
                    
            else:
                dt = np.inf
            self.stampPrev = self.header.stamp
            self.stampPrevAlt = stampAlt

            self.head.update(dt, image, self.bInvertColor)
            self.abdomen.update(dt, image, self.bInvertColor)
            self.left.update(dt, image, self.bInvertColor)
            self.right.update(dt, image, self.bInvertColor)
            self.aux.update(dt, image, self.bInvertColor)
            self.axis.update(dt, image, self.bInvertColor)
            
            
    def draw(self, image):
        # Draw line to indicate the body axis.
        cv2.line(image, self.ptBodyIndicator1, self.ptBodyIndicator2, self.bgra_body, 1) # Draw a line longer than just head-to-abdomen.
                
        self.axis.draw(image)
        self.head.draw(image)
        self.abdomen.draw(image)
        self.left.draw(image)
        self.right.draw(image)
        self.aux.draw(image)

        self.windowInvertColorArea.show()
        
    
    def publish(self):
        flystate              = MsgFlystate()
        flystate.header       = Header(seq=self.iCount, stamp=self.stamp, frame_id='Fly')

        if (self.params['gui']['head']['track']):
            flystate.head     = self.head.state
        else:
            flystate.head     = MsgState()

        if (self.params['gui']['abdomen']['track']):
            flystate.abdomen  = self.abdomen.state
        else:
            flystate.abdomen  = MsgState()

        if (self.params['gui']['left']['track']):
            flystate.left     = self.left.state
        else:
            flystate.left     = MsgState()
            
        if (self.params['gui']['right']['track']):
            flystate.right    = self.right.state
        else:
            flystate.right    = MsgState()
            
        if (self.params['gui']['aux']['track']):
            flystate.aux      = self.aux.state
        else:
            flystate.aux      = MsgState()


        self.iCount += 1
        
        self.pubFlystate.publish(flystate)