<commit_before>#!/usr/bin/env python
from __future__ import division
import roslib
import rospy
import rosparam
import copy
import cv2
import numpy as np
import threading
import dynamic_reconfigure.server
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image
from std_msgs.msg import Float32, Header, String

from multi_tracker.msg import Contourinfo, Contourlist
from multi_tracker.msg import Trackedobject, Trackedobjectlist
from multi_tracker.srv import resetBackgroundService

import time, os

###########################################################################################################
# General use functions
#######################

def is_point_below_line(point, slope, intercept):
    x = point[0]
    y = point[1]
    result = y-slope*x-intercept
    if result > 0:
        return False
    else:
        return True
    
def fit_ellipse_to_contour(contour):
    ellipse = cv2.fitEllipse(contour)
    (x,y), (a,b), angle = ellipse
    a /= 2.
    b /= 2.
    ecc = np.min((a,b)) / np.max((a,b))
    area = np.pi*a*b
    return x, y, ecc, area, angle
    
def add_data_to_contour_info(x,y,ecc,area,angle,dtCamera,header):
    # Prepare to publish the contour info
    # contour message info: dt, x, y, angle, area, ecc
    data = Contourinfo()
    data.header  = header
    data.dt      = dtCamera
    data.x       = x
    data.y       = y
    data.area    = area
    data.angle   = angle
    data.ecc     = ecc
    return data
    
def extract_and_publish_contours(self):
    contours, hierarchy = cv2.findContours(self.threshed, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
    # http://docs.opencv.org/trunk/doc/py_tutorials/py_imgproc/py_contours/py_contour_features/py_contour_features.html
    
    try:
        header  = Header(stamp=self.framestamp,frame_id=str(self.framenumber))
    except:
        header  = Header(stamp=None,frame_id=str(self.framenumber))
        print 'could not get framestamp, run tracker_nobuffer instead'
        
    contour_info = []
    for contour in contours:
        # Large objects are approximated by an ellipse
        if len(contour) > 5:
            x, y, ecc, area, angle = fit_ellipse_to_contour(contour)
            
            # if object is too large, split it in two, this helps with colliding objects, but is not 100% correct
            if area > self.params['max_expected_area']:
                slope = np.tan(angle)
                intercept = y - slope*x
                c1 = []
                c2 = []
                for point in contour:
                    point = point.reshape(2)
                    if is_point_below_line(point, slope, intercept):
                        c1.append(point)
                    else:
                        c2.append(point)
                
                if len(c1) >= 5:
                    x, y, ecc, area, angle = fit_ellipse_to_contour(np.array(c1))
                    if area < self.params['max_size'] and area > self.params['min_size']:
                        data = add_data_to_contour_info(x,y,ecc,area,angle,self.dtCamera,header)
                        contour_info.append(data)
                
                if len(c2) >= 5:
                    x, y, ecc, area, angle = fit_ellipse_to_contour(np.array(c2))
                    if area < self.params['max_size'] and area > self.params['min_size']:
                        data = add_data_to_contour_info(x,y,ecc,area,angle,self.dtCamera,header)
                        contour_info.append(data)
            else:
                if area < self.params['max_size'] and area > self.params['min_size']:
                    data = add_data_to_contour_info(x,y,ecc,area,angle,self.dtCamera,header)
                    contour_info.append(data)
            
            
        # Small ones just get a point
        else:
            area = 0
            '''
            moments = cv2.moments(contour, True)
            m00 = moments['m00']
            m10 = moments['m10']
            m01 = moments['m01']
            if (m00 != 0.0):
                x = m10/m00
                y = m01/m00
            else: # There was just one pixel in the contour.
                (x,y) = contour[0][0]
            area = 1.
            angle = 0.
            ecc = 1.
            
            if self.params['liveview']:
                if area > self.params['min_size'] and area < self.params['max_size']:
                    cv2.circle(self.imgOutput,(int(x),int(y)),2,(0,255,0),2) # draw a circle, green
                else:
                    cv2.circle(self.imgOutput,(int(x),int(y)),2,(0,0,255),2) # draw a circle, not green
            '''
            
    # publish the contours
    self.pubContours.publish( Contourlist(header = header, contours=contour_info) )  

    return

def convert_to_gray_if_necessary(self):
    if len(self.threshed.shape) == 3:
        self.threshed = np.uint8(cv2.cvtColor(self.threshed, cv2.COLOR_BGR2GRAY))
        
def erode_and_dialate(self):
    kernel = np.ones((3,3),np.uint8)
    self.threshed = cv2.dilate(self.threshed, kernel, iterations=self.params['dilate'])
    self.threshed = cv2.erode(self.threshed, kernel, iterations=self.params['erode'])
    
def reset_background_if_difference_is_very_large(self):
    # if the thresholded absolute difference is too large, reset the background
    if np.sum(self.threshed>0) / float(self.shapeImage[0]*self.shapeImage[1]) > self.params['max_change_in_frame']:
        reset_background(self)
        return
        
def reset_background(self):
    self.backgroundImage = copy.copy(np.float32(self.imgScaled))
    print self.imgScaled.shape, self.backgroundImage.shape
    filename = self.experiment_basename + '_' + time.strftime("%Y%m%d_%H%M%S_bgimg_N" + self.nodenum, time.localtime()) + '.png'
    home_directory = os.path.expanduser( rospy.get_param('/multi_tracker/data_directory') )
    filename = os.path.join(home_directory, filename)
    
    try:
        cv2.imwrite(filename, self.backgroundImage) # requires opencv > 2.4.9
        print 'Background reset: ', filename<commit_msg>added incredibly_basic image processing function as template<commit_after>#!/usr/bin/env python
from __future__ import division
import roslib
import rospy
import rosparam
import copy
import cv2
import numpy as np
import threading
import dynamic_reconfigure.server
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image
from std_msgs.msg import Float32, Header, String

from multi_tracker.msg import Contourinfo, Contourlist
from multi_tracker.msg import Trackedobject, Trackedobjectlist
from multi_tracker.srv import resetBackgroundService

import time, os

###########################################################################################################
# Incredibly basic image processing function (self contained), to demonstrate the format custom image processing functions should follow
#######################

def incredibly_basic(self):
    # If there is no background image, grab one, and move on to the next frame
    if self.backgroundImage is None:
        if np.sum(self.threshed>0) / float(self.shapeImage[0]*self.shapeImage[1]) > self.params['max_change_in_frame']:
            reset_background(self)
        return
    if self.reset_background_flag:
        if np.sum(self.threshed>0) / float(self.shapeImage[0]*self.shapeImage[1]) > self.params['max_change_in_frame']:
            reset_background(self)
        self.reset_background_flag = False
        return
      
    self.absdiff = cv2.absdiff(np.float32(self.imgScaled), self.backgroundImage)
    self.imgproc = copy.copy(self.imgScaled)
    
    retval, self.threshed = cv2.threshold(self.absdiff, self.params['threshold'], 255, 0)
    
    # convert to gray if necessary
    if len(self.threshed.shape) == 3:
        self.threshed = np.uint8(cv2.cvtColor(self.threshed, cv2.COLOR_BGR2GRAY))
    
    # extract and publish contours
    # http://docs.opencv.org/trunk/doc/py_tutorials/py_imgproc/py_contours/py_contour_features/py_contour_features.html
    contours, hierarchy = cv2.findContours(self.threshed, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
    
    try:
        header  = Header(stamp=self.framestamp,frame_id=str(self.framenumber))
    except:
        header  = Header(stamp=None,frame_id=str(self.framenumber))
        print 'could not get framestamp, run tracker_nobuffer instead'
        
    contour_info = []
    for contour in contours:
        if len(contour) > 5: # Large objects are approximated by an ellipse
            ellipse = cv2.fitEllipse(contour)
            (x,y), (a,b), angle = ellipse
            a /= 2.
            b /= 2.
            ecc = np.min((a,b)) / np.max((a,b))
            area = np.pi*a*b
            
            data = Contourinfo()
            data.header  = header
            data.dt      = dtCamera
            data.x       = x
            data.y       = y
            data.area    = area
            data.angle   = angle
            data.ecc     = ecc
            
            contour_info.append(data)
            
        else: # Small ones get ignored
            pass
            
    # publish the contours
    self.pubContours.publish( Contourlist(header = header, contours=contour_info) )  


###########################################################################################################
# General use functions
#######################

def is_point_below_line(point, slope, intercept):
    x = point[0]
    y = point[1]
    result = y-slope*x-intercept
    if result > 0:
        return False
    else:
        return True
    
def fit_ellipse_to_contour(contour):
    ellipse = cv2.fitEllipse(contour)
    (x,y), (a,b), angle = ellipse
    a /= 2.
    b /= 2.
    ecc = np.min((a,b)) / np.max((a,b))
    area = np.pi*a*b
    return x, y, ecc, area, angle
    
def add_data_to_contour_info(x,y,ecc,area,angle,dtCamera,header):
    # Prepare to publish the contour info
    # contour message info: dt, x, y, angle, area, ecc
    data = Contourinfo()
    data.header  = header
    data.dt      = dtCamera
    data.x       = x
    data.y       = y
    data.area    = area
    data.angle   = angle
    data.ecc     = ecc
    return data
    
def extract_and_publish_contours(self):
    contours, hierarchy = cv2.findContours(self.threshed, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
    # http://docs.opencv.org/trunk/doc/py_tutorials/py_imgproc/py_contours/py_contour_features/py_contour_features.html
    
    try:
        header  = Header(stamp=self.framestamp,frame_id=str(self.framenumber))
    except:
        header  = Header(stamp=None,frame_id=str(self.framenumber))
        print 'could not get framestamp, run tracker_nobuffer instead'
        
    contour_info = []
    for contour in contours:
        # Large objects are approximated by an ellipse
        if len(contour) > 5:
            x, y, ecc, area, angle = fit_ellipse_to_contour(contour)
            
            # if object is too large, split it in two, this helps with colliding objects, but is not 100% correct
            if area > self.params['max_expected_area']:
                slope = np.tan(angle)
                intercept = y - slope*x
                c1 = []
                c2 = []
                for point in contour:
                    point = point.reshape(2)
                    if is_point_below_line(point, slope, intercept):
                        c1.append(point)
                    else:
                        c2.append(point)
                
                if len(c1) >= 5:
                    x, y, ecc, area, angle = fit_ellipse_to_contour(np.array(c1))
                    if area < self.params['max_size'] and area > self.params['min_size']:
                        data = add_data_to_contour_info(x,y,ecc,area,angle,self.dtCamera,header)
                        contour_info.append(data)
                
                if len(c2) >= 5:
                    x, y, ecc, area, angle = fit_ellipse_to_contour(np.array(c2))
                    if area < self.params['max_size'] and area > self.params['min_size']:
                        data = add_data_to_contour_info(x,y,ecc,area,angle,self.dtCamera,header)
                        contour_info.append(data)
            else:
                if area < self.params['max_size'] and area > self.params['min_size']:
                    data = add_data_to_contour_info(x,y,ecc,area,angle,self.dtCamera,header)
                    contour_info.append(data)
            
            
        # Small ones just get a point
        else:
            area = 0
            
    # publish the contours
    self.pubContours.publish( Contourlist(header = header, contours=contour_info) )  

    return

def convert_to_gray_if_necessary(self):
    if len(self.threshed.shape) == 3:
        self.threshed = np.uint8(cv2.cvtColor(self.threshed, cv2.COLOR_BGR2GRAY))
        
def erode_and_dialate(self):
    kernel = np.ones((3,3),np.uint8)
    self.threshed = cv2.dilate(self.threshed, kernel, iterations=self.params['dilate'])
    self.threshed = cv2.erode(self.threshed, kernel, iterations=self.params['erode'])
    
def reset_background_if_difference_is_very_large(self):
    # if the thresholded absolute difference is too large, reset the background
    if np.sum(self.threshed>0) / float(self.shapeImage[0]*self.shapeImage[1]) > self.params['max_change_in_frame']:
        reset_background(self)
        return
        
def reset_background(self):
    self.backgroundImage = copy.copy(np.float32(self.imgScaled))
    print self.imgScaled.shape, self.backgroundImage.shape
    filename = self.experiment_basename + '_' + time.strftime("%Y%m%d_%H%M%S_bgimg_N" + self.nodenum, time.localtime()) + '.png'
    home_directory = os.path.expanduser( rospy.get_param('/multi_tracker/data_directory') )
    filename = os.path.join(home_directory, filename)
    
    try:
        cv2.imwrite(filename, self.backgroundImage) # requires opencv > 2.4.9
        print 'Background reset: ', filename
{'fraction_non_alphanumeric': 0.12679477105507536, 'fraction_numerical': 0.011857989856418316, 'mean_word_length': 2.33889816360601, 'pattern_counts': {'":': 0, '<': 10, '<?xml version=': 0, '>': 31, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '2941367', 'n_tokens_mistral': 4535, 'n_tokens_neox': 4350, 'n_words': 920}