<repo_name>MatthewTWhelan/miroDetection<file_sep>/Old_detectors/miroDetector1.py
# <NAME>
# Here we will load up the trained SVM using HOG feature extraction. Then the
# adaptive threshold will be applied for finding a ROI, before attempting to
# classify the ROI

import numpy as np
import cv2
import os
import sys

def imshow(img):
    cv2.imshow('image',img)
    cv2.waitKey(0)
    cv2.destroyAllWindows

class miroDetector:
    
    def __init__(self):        

        # Let's set up HOG and load the SVM data
        winSize = (64,64)
        blockSize = (16,16)
        blockStride = (8,8)
        cellSize = (8,8)
        nbins = 9
        derivAperture = 1
        winSigma = 4.
        histogramNormType = 0
        L2HysThreshold = 2.0000000000000001e-01
        gammaCorrection = 0
        nlevels = 64
        self.hog = cv2.HOGDescriptor(winSize,blockSize,blockStride,cellSize,nbins,derivAperture,winSigma,
                                histogramNormType,L2HysThreshold,gammaCorrection,nlevels)
        self.svm = cv2.ml.SVM_load('svm.dat')
        

    def ROI_expand(self,(x,y,w,h)):
        w_new = int(w*1.5)
        h_new = int(h*1.5)
        x_new = x - (w_new - w)/2
        y_new = y - (h_new - h)/2
        if x_new >= 0:
            x = x_new
        if y_new >= 0:
            y = y_new
        return (x, y, w_new, h_new)

    def ROI(self,img):
        # thresholding the standard image
        img_grey = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
        mean,stddev = cv2.meanStdDev(img_grey)
        thresh = mean*(1 + -0.75*(stddev/128 - 1))
        if thresh>230:
            thresh = 230
        img_thresh = cv2.inRange(img_grey, thresh, 255)
        imshow(img_thresh)
        _,contours,_ = cv2.findContours(img_thresh, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
        roi = []
        for c in contours:
            if cv2.contourArea(c) < 256:
                continue
            (x, y, w, h) = cv2.boundingRect(c)
            (x, y, w, h) = self.ROI_expand((x,y,w,h))
            roi.append((x,y,w,h))
        return roi

    def detector(self,img):        
        # Let us now load the test images and run them through the ROI algorithm
        roi = self.ROI(img)

        # The SVM classifier will now be run on each ROI. The ROI is first set to a square
        # size, reduced (or increased) to size 64x64, then tested using HOG and SVM.
        detected = []
        for r in range(len(roi)):
            (x,y,w,h) = roi[r]
            winSize = 32
            while winSize < min(w,h):
                for x_win in range(x, x + w - winSize, 4):
                    for y_win in range(y, y + h - winSize, 4):
                        win = img[y_win:(y_win+winSize), x_win:(x_win+winSize)]
                        img_roi = cv2.resize(win,(64,64))
                        hog_feature = np.transpose(self.hog.compute(img_roi)).astype(np.float32)
                        result = self.svm.predict(hog_feature)[1]
                        if result>0:
                            detected.append(((x_win,y_win,winSize),result))
                winSize += 8
        self.Overlap(detected)
        for det in detected:
            (x_win,y_win,winSize) = det[0]
            result = det[1]
            cv2.rectangle(img, (x_win,y_win), (x_win+winSize,y_win+winSize), (255,0,0), 2)
            if result==1:
                print 'MiRo has been detected from the left side'
            if result==2:
                print 'MiRo has been detected from the right side'
            if result==3:
                print 'MiRo has been detected from the back side'
            imshow(img)

    def Overlap(self,detected):
        # This method counts the number of overlapping detected zones, determines most likely orientation, 
        # and returns single detected regions
        x = []
        y = []
        win = []
        for det in detected:
            (x_win,y_win,winSize) = det[0]
            x.append(x_win)
            y.append(y_win)
            win.append(winSize)
        print x
        print y
        print win
        groups = np.zeros((10,50)) # store a max of 10 groups, each with a max of 50 detected regions
        for i in range(len(detected)):
            for j in range(i+1,len(detected)):
                if i==j:
                    continue
                a1 = x[j] <= (x[i] + win[i]) and x[j] >= x[i]
                a2 = y[j] <= (y[i] + win[i]) and y[j] >= y[i]
                a3 = x[i] <= (x[j] + win[j]) and x[i] >= x[j]
                a4 = y[i] <= (y[j] + win[j]) and y[i] >= y[j]
                
                if a1 and (a2 or a4):
                    print "these overlap: ",i,j
                elif a3 and (a2 or a4):
                    print "these overlap: ",i,j
                else:
                    print "no overlap: ",i,j
        
if __name__ == "__main__":
    for arg in sys.argv[1:]:
        f = arg.find('=')
        if f == -1:
            key = arg
            val = ""
        else:
            key = arg[:f]
            val = arg[f+1:]    
        if key == "image_path":
            image = val
        else:
            print("argument \"image_path\" must be specified")
            sys.exit(0)
    img = cv2.imread(image)
    cv2.imshow('image',img)
    cv2.waitKey(0)
    cv2.destroyAllWindows()
    if img is None:
        print("Invalid image location specified")
        sys.exit(0)
    miroDetector().detector(img)
<file_sep>/gradients.py
import cv2
import numpy as np

def imshow(img):
    cv2.imshow('image',img)
    cv2.waitKey(0)
    cv2.destroyAllWindows()

img = cv2.imread('Training_images/MiRo_face/11.png')
img = cv2.resize(img,(64,64))

# gradient image can be obtained using Sobel edges, as below. Or, they
# can be found using the hog.computeGradients method (see below for how to
# initialise hog)
gx = cv2.Sobel(img, cv2.CV_32F, 1, 0, ksize=1)
gy = cv2.Sobel(img, cv2.CV_32F, 0, 1, ksize=1)
mag, angle = cv2.cartToPolar(gx, gy, angleInDegrees=True)
imshow(mag)
cv2.imwrite("SobelEdges.png",mag)
cell = img[39:47,47:55]
cv2.imwrite("cell8size.png",cell)



# Computing HOG features for the image
# setting up the HOG feature extractor
winSize = (64,64)
blockSize = (16,16)
blockStride = (8,8)
cellSize = (8,8)
nbins = 9
derivAperture = 1
winSigma = 4.
histogramNormType = 0
L2HysThreshold = 2.0000000000000001e-01
gammaCorrection = 0
hog = cv2.HOGDescriptor(winSize,blockSize,blockStride,cellSize,nbins,derivAperture,winSigma,
                        histogramNormType,L2HysThreshold,gammaCorrection)
                        
h = hog.compute(img)
# The hog feature vector contains, in most cells, two sets of histograms.
# Therefore, for plotting, they will be renormalised anyway (relative to the
# individual histogram) so which one is taken is irrelevent. There are 64 cells
# in total, so an array of size 64x9 will be used to store the normalised
# histograms for each cell. Note that, if the blocks slide left to right,
# then the side cells will contain only one histogram. The rest of the cells
# each contain two histograms in the hog feature vector. This must be accounted
# for.

cell_hists = np.zeros((64,9))
index_no = [ # The HOG descriptor is built on the 2x2 blocks. Hence, for each cell,
            # more than one histogram is built. These indices are the cell numbers that
            # have been built for the descriptor. Index 0 represents h[0:9], 182 represents
            # h[182:191] etc.
            0, 4, 8, 12, 16, 20, 24, 25, 
            28, 32, 36, 40, 44, 48, 52, 53,
            56, 60, 64, 68, 72, 76, 80, 81,
            84, 88, 92, 96, 100, 104, 108, 109,
            112, 116, 120, 124, 128, 132, 136, 137,
            140, 144, 148, 152, 156, 160, 164, 165,
            168, 172, 176, 180, 184, 188, 192, 193,
            170, 174, 178, 182, 186, 190, 194, 195
            ]   

i = 0
for ind in index_no:
    cell_hists[i,:] = np.reshape(h[ind*9:ind*9 + 9],9)
    i += 1

i = 0
for x in range(8):
    for y in range(8):
        cell_grads = cell_hists[i,:]
        #print np.size(cell_grads)
        img = cv2.resize(img,(64*8,64*8))
        for j in range(9):
            total = sum(cell_grads)
            ang = j*20
            x_len = int( np.sin(ang) * cell_grads[j] / total * 50 )
            y_len = int( np.cos(ang) * cell_grads[j] / total * 50 )
            cv2.line(img, ((x+1)*8*8-x_len/2-32,(y+1)*8*8-y_len/2-32), ((x+1)*8*8+x_len/2-32,(y+1)*8*8+y_len/2-32), (0,0,255), 1)
        i += 1

imshow(img)
cv2.imwrite("Gradients.png",img)
<file_sep>/HOG_w_svm_TRAIN.py
#!/usr/bin/python

#!/usr/bin/python

# <NAME>

# WARNING: Running this script will overwrite the svm.dat and svmCon.dat
# files, which are used in the main Miro Detector.

# This script is used to train the SVM on a set of training images, and
# performs validation of the SVM classifier too.

# Classification is performed on three of MiRo's body parts - side views 
# (left and right) and back view.

# A second SVM is generated that is later used for extracting confidence
# levels of detection. This is a simple binary classifier, using the 
# positive and negative images as the training data.

import numpy as np
import cv2
import os
import time

test = False # If one wants to validate only, set this to true. If one is only
             # interested in training the SVM, set to false.

def imshow(img):
    cv2.imshow('image',img)
    cv2.waitKey(0)
    cv2.destroyAllWindows

# setting up the HOG feature extractor
winSize = (64,64)
blockSize = (16,16)
blockStride = (8,8)
cellSize = (8,8)
nbins = 9
derivAperture = 1
winSigma = 4.
histogramNormType = 0
L2HysThreshold = 2.0000000000000001e-01
gammaCorrection = 0
nlevels = 64
hog = cv2.HOGDescriptor(winSize,blockSize,blockStride,cellSize,nbins,derivAperture,winSigma,
                        histogramNormType,L2HysThreshold,gammaCorrection,nlevels)
                        
# Setting up the face detector parameters. It's important the parameters 
# keep the resultant feature vector having the same size as the body extractor.
# This is so it's easy to use the negative image features in training.
winSizeFace = (32,32)
blockSizeFace = (8,8)
blockStrideFace = (4,4)
cellSizeFace = (2,2)
hogFace = cv2.HOGDescriptor(winSizeFace,blockSizeFace,blockStrideFace,cellSizeFace,nbins,derivAperture,
                        winSigma,histogramNormType,L2HysThreshold,gammaCorrection,nlevels)
print "Collecting image data..."

# We have three classes for the body: MiRo's two side views and the back. 
# Here we compute the hog features for each
img_side_l_dir = 'Training_images/MiRo_body/Left/'
img_side_r_dir = 'Training_images/MiRo_body/Right/'
img_back_dir = 'Training_images/MiRo_body/Back/'
no_side_l = len(os.listdir(img_side_l_dir))
no_side_r = len(os.listdir(img_side_r_dir))
no_back = len(os.listdir(img_back_dir))
hog_side_l = np.zeros((no_side_l,1764))
hog_side_r = np.zeros((no_side_r,1764))
hog_back = np.zeros((no_back,1764))
j = 0
for filename in os.listdir(img_side_l_dir):
    img = cv2.imread(os.path.join(img_side_l_dir,filename))
    if img is not None:
        img = cv2.resize(img,(64,64)) # resizing the image to 64x64 size
        h = hog.compute(img)
        hog_side_l[j,:] = np.reshape(h,np.size(h),1)
        j += 1
j = 0
for filename in os.listdir(img_side_r_dir):
    img = cv2.imread(os.path.join(img_side_r_dir,filename))
    if img is not None:
        img = cv2.resize(img,(64,64)) # resizing the image to 64x64 size
        h = hog.compute(img)
        hog_side_r[j,:] = np.reshape(h,np.size(h),1)
        j += 1
j = 0
for filename in os.listdir(img_back_dir):
    img = cv2.imread(os.path.join(img_back_dir,filename))
    if img is not None:
        img = cv2.resize(img,(64,64)) # resizing the image to 64x64 size
        h = hog.compute(img)
        hog_back[j,:] = np.reshape(h,np.size(h),1)
        j += 1

# Computing HOG features for the negative images        
negative_images_dir = 'Training_images/Negatives/'
no_negs = len(os.listdir(negative_images_dir))
j = 0
hog_negs = np.zeros((no_negs,1764))
for filename in os.listdir(negative_images_dir):
    img = cv2.imread(os.path.join(negative_images_dir,filename))
    if img is not None:
        img = cv2.resize(img,(64,64)) # resizing the image to 64x64 size
        h = hog.compute(img)
        hog_negs[j,:] = np.reshape(h,np.size(h),1)
        j += 1

# and the same for the face negs (needs doing for difference in feature desciptor size)
j = 0
hog_negs_face = np.zeros((no_negs,7056))
for filename in os.listdir(negative_images_dir):
    img = cv2.imread(os.path.join(negative_images_dir,filename))
    if img is not None:
        img = cv2.resize(img,(32,32)) # resizing the image to 64x64 size
        h = hogFace.compute(img)
        hog_negs_face[j,:] = np.reshape(h,np.size(h),1)
        j += 1

# combining the training data
train = np.vstack((
                    hog_side_l, 
                    hog_side_r, 
                    hog_back, 
                    hog_negs)).astype(np.float32)

# create the training labels: left side = 1, right side = 2, back = 3, negs = -1
side_l_label = 1
side_r_label = 2
back_label = 3
negs_label = -1
train_labels = np.concatenate(
              (np.repeat(side_l_label,no_side_l),
               np.repeat(side_r_label,no_side_r),
               np.repeat(back_label,no_back),               
               np.repeat(negs_label,no_negs)), 
               axis=0)[:,np.newaxis]

# Training the SVM for MiRo faces
faces_image_dir = '/home/matt/miroDetection/Training_images/MiRo_face/'
no_faces = len(os.listdir(faces_image_dir))
j = 0
hog_faces = np.zeros((no_faces,7056))
for filename in os.listdir(faces_image_dir):
    img = cv2.imread(os.path.join(faces_image_dir,filename))
    if img is not None:
        img = cv2.resize(img,(32,32)) # resizing the image to 32x32 for faces
        h = hogFace.compute(img)
        hog_faces[j,:] = np.reshape(h,np.size(h),1)
        j += 1
trainFaces = np.vstack((hog_faces, hog_negs_face)).astype(np.float32)
train_face_labels = np.concatenate(
                    (np.repeat(1,no_faces),
                     np.repeat(-1,no_negs)),
                     axis=0)[:,np.newaxis]
                     
if not test:
    print "Training only"
    
    # training the SVM, and saving as a dat file
    print "Training the SVM..."
    svm = cv2.ml.SVM_create()
    svm.setKernel(cv2.ml.SVM_LINEAR)
    svm.setType(cv2.ml.SVM_C_SVC)
    svm.train(train, cv2.ml.ROW_SAMPLE, train_labels)
    # here we can save the SVM data and then load it back in for classifying
    svm.save('svm.dat')
    #svm = cv2.ml.SVM_load('svm.dat')
    
    # training a second, binary classifier in order to get confidence levels
    svmCon = cv2.ml.SVM_create()
    svmCon.setKernel(cv2.ml.SVM_LINEAR)
    svmCon.setType(cv2.ml.SVM_C_SVC)
    
    train_labels_con = np.zeros(np.shape(train_labels), dtype = int)
    for i in range(np.size(train_labels)):
        train_labels_con[i] = train_labels[i] / abs(train_labels[i])
    svmCon.train(train, cv2.ml.ROW_SAMPLE, train_labels_con)
    svmCon.save('svmCon.dat')
    
    # finally training the MiRo face detector
    svmFace = cv2.ml.SVM_create()
    svmFace.setKernel(cv2.ml.SVM_LINEAR)
    svmFace.setType(cv2.ml.SVM_C_SVC)
    svmFace.train(trainFaces, cv2.ml.ROW_SAMPLE, train_face_labels)
    svmFace.save('svmFace.dat')
    
    print "Training complete"

else:
    # Below lines for testing purposes. It will train the SVM on a
    # subset of the training data, then validate on the rest.
    import random
    print "Running validation"
    print "Training the SVM..."
    for i in range(50):
        index_l = random.sample(range(no_side_l),50)
        index_r = random.sample(range(no_side_l, no_side_l + no_side_r),50)
        index_b = random.sample(range(no_side_l + no_side_r, no_side_l + no_side_r + no_back),100)
        index_neg = random.sample(range(no_side_l + no_side_r + no_back, no_side_l + no_side_r + no_back + no_negs),1000)
    index = np.concatenate((index_l,index_r,index_b,index_neg),axis=0)

    val_data = train[index,:]
    val_labels = train_labels[index]

    train = np.delete(train,index,axis=0)
    train_labels = np.delete(train_labels,index,axis=0)

    svm = cv2.ml.SVM_create()
    svm.setKernel(cv2.ml.SVM_LINEAR)
    svm.setType(cv2.ml.SVM_C_SVC)
    svm.train(train, cv2.ml.ROW_SAMPLE, train_labels)
    result = svm.predict(val_data)[1]
    print "Training complete"
    no_correct = 0
    no_false = 0
    i = 0
    for res in result:
        if val_labels[i] == res:
            no_correct += 1
        else:
            no_false += 1
        i += 1
    print "Percentage of correctly classified images is: ", float(no_correct) / len(val_labels)
<file_sep>/imageEditing.py
# Simple script written to mirror all images, then rotates all images
# (including those previously mirrored). These can be used as further 
# training instances. Scaling is not necessary as HOG scales all images
# to the same size anyway.

import cv2
import os
import numpy as np
import datetime

img_dir = 'image_capture/'
img_save_dir = 'image_capture/'
now = datetime.datetime.now()
year = now.year
month = now.month
day = now.day
hour = now.hour
minute = now.minute
date = str(year-2000) + str(month) + str(day) + "_" + str(hour) + str(minute)
print date
# use precise date and time to generate unique image names

#image mirroring
for filename in os.listdir(img_dir):
    img = cv2.imread(os.path.join(img_dir,filename))
    if img is not None:
        img_mirr = cv2.flip(img,1)
        cv2.imwrite(img_save_dir + 'mirrored_' + filename, img_mirr)
        
#image rotating (6 angles, +- 4deg, 8deg and 12deg)
for filename in os.listdir(img_dir):
    img = cv2.imread(os.path.join(img_dir,filename))
    if img is not None:
        rows,cols,_ = np.shape(img)
        for ang in [4,8,12]:
            M_pos = cv2.getRotationMatrix2D((cols/2,rows/2),ang,1)
            M_neg = cv2.getRotationMatrix2D((cols/2,rows/2),-ang,1)
            img_rot_pos = cv2.warpAffine(img,M_pos,(cols,rows))
            img_rot_neg = cv2.warpAffine(img,M_neg,(cols,rows))
            cv2.imwrite(img_save_dir + date + 'pos_rotate' + str(ang) + filename, img_rot_pos)
            cv2.imwrite(img_save_dir + date + 'neg_rotate' + str(ang) + filename, img_rot_neg)
<file_sep>/miroDetector.py
#!/usr/bin/python

# <NAME>
# MiRo detection using HOG features and an SVM kernal machine classifier

import numpy as np
import cv2
import os
import sys
import time
import datetime

class miroDetector:
    
    def __init__(self):        
        
        # Let's set up HOG and load the SVM data
        winSize = (64,64)
        blockSize = (16,16)
        blockStride = (8,8)
        cellSize = (8,8)
        nbins = 9
        derivAperture = 1
        winSigma = 4.
        histogramNormType = 0
        L2HysThreshold = 2.0000000000000001e-01
        gammaCorrection = 0
        nlevels = 64
        
        self.hog = cv2.HOGDescriptor(winSize,blockSize,blockStride,
                                cellSize,nbins,derivAperture,winSigma,
                                histogramNormType,L2HysThreshold,
                                gammaCorrection,nlevels)
        winSizeFace = (32,32)
        blockSizeFace = (8,8)
        blockStrideFace = (4,4)
        cellSizeFace = (2,2)
        
        self.hogFace = cv2.HOGDescriptor(winSizeFace,blockSizeFace,
                                blockStrideFace,cellSizeFace,nbins,
                                derivAperture,winSigma,histogramNormType,
                                L2HysThreshold,gammaCorrection,nlevels)        
        
        self.svm = cv2.ml.SVM_load("svm.dat")
        self.svmCon = cv2.ml.SVM_load("svmCon.dat")
        self.svmSupportVector = self.svmCon.getSupportVectors()
        
        self.svmFace = cv2.ml.SVM_load("svmFace.dat")
        
    def detector(self,img,faces=False):
        t0 = time.time()
        now = datetime.datetime.now()
        year = now.year
        month = now.month
        day = now.day
        hour = now.hour
        minute = now.minute
        date = str(year-2000) + str(month) + str(day) + "_" + str(hour) + str(minute)

        (self.resY, self.resX, _) = np.shape(img)
        #print help(self.svmCon.getDecisionFunction)

        t0 = time.time()

        # cropping off the top third of the image
        img_crop = img[int(self.resY/3):self.resY, 0:self.resX]
        
        roi = self.ROI(img_crop)
        
        detected = []
        detectedFaces = []
        i = 1
        for r in range(len(roi)):
            if faces:
                (x,y,w,h) = roi[r]
                winSize = 32
                while winSize < max(w,h):
                    for x_win in range(x, x + w - winSize, 4):
                        for y_win in range(y, y + h - winSize, 4):
                            win = img_crop[y_win:(y_win+winSize), x_win:(x_win+winSize)]
                            if np.size(win) == 0:
                                continue
                            
                            img_roi = cv2.resize(win,(64,64))
                            img_roi_face = cv2.resize(win,(32,32))
                            
                            hog_feature = np.transpose(self.hog.compute(img_roi)).astype(np.float32) 
                            hog_feature_face = np.transpose(self.hogFace.compute(img_roi_face)).astype(np.float32)                           
                            
                            result = self.svm.predict(hog_feature)[1]
                            resultSvmCon = self.svmCon.predict(hog_feature)[1]
                            faceResult = self.svmFace.predict(hog_feature_face)[1]
                            
                            #print (faceResult)

                            if faceResult == 1:
                                detectedFaces.append([x_win,y_win,winSize])
                            
                            elif result > 0 or resultSvmCon == 1:
                                # the below two lines are useful for storing classified regions, if needed for adding to negative image database etc..
                                #cv2.imwrite(date + "_" + str(i) + 'image.png', win)
                                #i += 1
                                
                                decisionFuncVal = self.decisionFuncValue(hog_feature)
                                
                                #~ print "svmCon result is: ", resultSvmCon
                                #~ print decisionFuncVal
                                #~ print result
                                
                                detected.append(((x_win,y_win,winSize),result,decisionFuncVal))
                            
                            t = time.time() - t0
                            #~ if t > 2:
                                #~ return None, None, None, None # to break the detector if it's taking more than 2 seconds
                            
                    winSize += 8
            else:
                (x,y,w,h) = roi[r]
                winSize = 32
                while winSize < max(w,h):
                    for x_win in range(x, x + w - winSize, 4):
                        for y_win in range(y, y + h - winSize, 4):
                            win = img_crop[y_win:(y_win+winSize), x_win:(x_win+winSize)]
                            if np.size(win) == 0:
                                continue
                            img_roi = cv2.resize(win,(64,64))
                            
                            hog_feature = np.transpose(self.hog.compute(img_roi)).astype(np.float32) 
                            
                            result = self.svm.predict(hog_feature)[1]
                            resultSvmCon = self.svmCon.predict(hog_feature)[1]

                            if result > 0 or resultSvmCon == 1:                                
                                decisionFuncVal = self.decisionFuncValue(hog_feature)
                                detected.append(((x_win,y_win,winSize),result,decisionFuncVal))
                            
                            t = time.time() - t0
                            if t > 2:
                                return None, None, None # to break the detector if it's taking more than 2 seconds
                            
                    winSize += 8

        if len(detected) == 0:
            print "No MiRo detected"
            if faces:
                return None, None, None, None
            else:
                return None, None, None
            
        else:
            detectedZones = self.overlap(detected) # detectedZones returns
            # a list of the detected zones in the following form
            # [xMin, xMax, yMin, yMax, noLeft, noRight, noBack, avgSVMDist]
            # for each detected zone.

            # adding 80 to the y detection coordinates to account for cropped image
            for det in detectedZones:
                det[1] += int(self.resY / 3)
                det[3] += int(self.resY / 3)
            if faces:
                for face in detectedFaces:
                    face[1] += int(self.resY / 3)
                    pass
        # organising the data for output        
        detectedCoords, orientations, confidences = self.Organise(detectedZones)

        if faces:
            return detectedCoords, orientations, confidences, detectedFaces
        else:
            return detectedCoords, orientations, confidences

    def ROI(self,img):
        # thresholding the standard image using the histogram data to 
        # set the threshold so that only 15%, or less, is thresholded
        
        k = 0
        thresh_prop = 1.0
        img_grey = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
        mean,stddev = cv2.meanStdDev(img_grey)
        
        thresh = mean*(1 + -k*(stddev/128 - 1))
        img_thresh = cv2.inRange(img_grey, thresh, 255)
        
        thresh_prop = ( 
                        float(cv2.countNonZero(img_thresh)) / 
                (float(np.shape(img)[0]) * float(np.shape(img)[1])) 
                                                                    )
        if thresh_prop > 0.3:
            thresh = self.histogramThresh(img)
        elif thresh_prop > 0.15:
            while thresh_prop > 0.15:
                thresh = mean*(1 + -k*(stddev/128 - 1))
                img_thresh = cv2.inRange(img_grey, thresh, 255)
                thresh_prop = (
                                float(cv2.countNonZero(img_thresh)) / 
                        (float(np.shape(img)[0]) * float(np.shape(img)[1]))
                                                                    )
                k += 0.05
        _,contours,_ = cv2.findContours(img_thresh, cv2.RETR_TREE, 
                                                cv2.CHAIN_APPROX_SIMPLE)
        
        roi = []
        for c in contours:
            if cv2.contourArea(c) < 256:
                continue
            (x, y, w, h) = cv2.boundingRect(c)
            (x, y, w, h) = self.ROI_expand((x,y,w,h))
            #imshow(img[y:y+h,x:x+w])
            roi.append((x,y,w,h))
        
        return roi
    
    def histogramThresh(self,img):
        
        hist = cv2.calcHist([img],[0],None,[256],[0,256])
        total = sum(hist)
        summed = 0
        thresh = 0
        for x in hist:
            summed += x
            thresh += 1
            if summed / total > 0.85:
                break
        
        return thresh
    
    def ROI_expand(self,(x,y,w,h)):
                
        w_new = int(w*1.75)
        h_new = int(h*1.75)
        x_new = x - (w_new - w)/2
        y_new = y - (h_new - h)/2
        
        # edge cases
        x = 0 if x_new < 0 else x_new
        y = 0 if y_new < 0 else y_new
        w = (self.resX - 1 - x) if x + w_new > self.resX else w_new
        h = (self.resY - 1 - y) if y + h_new > self.resY else h_new
        
        return (x, y, w, h)
    
    def decisionFuncValue(self, hog_feature):
        # Method computes the decision function value manually. Rho is
        # first extracted, from which ther kernal fucntion is subtracted.
        # The kernel function is computed by taking the dot product 
        # of the hog feature vector with the the support vector.
        # See https://docs.opencv.org/3.0-beta/modules/ml/doc/support_vector_machines.html#prediction-with-svm
        
        supportVector = self.svmSupportVector
        rho = self.svmCon.getDecisionFunction(i=0)[0]
        kernalFunc = np.dot(supportVector, np.transpose(hog_feature))
        
        decisionFuncVal = rho - kernalFunc
        
        return decisionFuncVal
    
    def overlap(self,detected):
        # This method counts the number of overlapping detected zones, 
        # and returns single detected regions with a count of the total 
        # number of detections per orientation
        
        x = []
        y = []
        win = []
        groups = np.zeros(len(detected))
        
        for det in detected:
            (x_win,y_win,winSize) = det[0]
            x.append(x_win)
            y.append(y_win)
            win.append(winSize)
        
        groups[0] = 1
        for i in range(len(detected)):
            for j in range(i+1,len(detected)):
                if i==j:
                    continue
                a1 = x[j] <= (x[i] + win[i]) and x[j] >= x[i]
                a2 = y[j] <= (y[i] + win[i]) and y[j] >= y[i]
                a3 = x[i] <= (x[j] + win[j]) and x[i] >= x[j]
                a4 = y[i] <= (y[j] + win[j]) and y[i] >= y[j]
                
                if a1 and (a2 or a4):
                    groups[j] = groups[i]
                elif a3 and (a2 or a4):
                    groups[j] = groups[i]
                elif groups[j] == 0:
                    groups[j] = max(groups) + 1
        
        no_groups = int(max(groups))
        groupRegions = np.zeros((no_groups,4))
        for i in range(len(groups)):            
            if groupRegions[int(groups[i]-1),3] == 0:
                groupRegions[int(groups[i]-1),:] = (x[i],y[i],x[i] + win[i],y[i] + win[i])
            else:
                if x[i] < groupRegions[int(groups[i]-1),0]:
                    groupRegions[int(groups[i]-1),0] = x[i]
                if y[i] < groupRegions[int(groups[i]-1),1]:
                    groupRegions[int(groups[i]-1),1] = y[i]
                if x[i] + win[i] > groupRegions[int(groups[i]-1),2]:
                    groupRegions[int(groups[i]-1),2] = x[i] + win[i]
                if y[i] + win[i] > groupRegions[int(groups[i]-1),3]:
                    groupRegions[int(groups[i]-1),3] = y[i] + win[i]
        
        noOrientations = np.zeros((no_groups,3)) # each row represents a
        # group, and is organised as (noLSide, noRSide, noBack)
        for i in range(len(groups)):
            if detected[i][1] == 1.0:
                noOrientations[int(groups[i])-1,0] += 1
            if detected[i][1] == 2.0:
                noOrientations[int(groups[i]-1),1] += 1
            if detected[i][1] == 3.0:
                noOrientations[int(groups[i]-1),2] += 1
        
        svmDistances = np.zeros((no_groups,2)) # confidences are taken as
        # an average of the SVM distances within each group
        for i in range(len(groups)):
            svm_dist = detected[i][2]
            svmDistances[int(groups[i])-1,0] += svm_dist
            svmDistances[int(groups[i])-1,1] += 1
        confidences = np.zeros((no_groups,1))
        for i in range(len(svmDistances)):
            confidences[i,0] = svmDistances[i,0] / svmDistances[i,1]
        
        detectedZones = np.zeros((no_groups,8))
        detectedZones[:,0:4] = groupRegions
        detectedZones[:,4:7] = noOrientations
        detectedZones[:,7] = confidences[:,0]
        
        # delete regions that are clearly too small
        group = 0
        delIndex = []
        for zone in detectedZones:
            (xMin,yMin) = zone[0:2]
            (xMax,yMax) = zone[2:4]            
            x = xMax - xMin
            y = yMax - yMin
            
            if x < 10 or y < 10 or (y/x) < 0.5 or (x/y) < 0.5:
                delIndex.append(group)
            
            group += 1
            
        detectedZones = np.delete(detectedZones, delIndex, axis=0) 
        
        return detectedZones
                
    def Organise(self, detectedZones):
        
        zonesCoords = []
        orientations = []
        confidences = []
        
        for zone in detectedZones:
            zonesCoords.append([zone[0],zone[1],zone[2],zone[3]])
            orientation = self.Orientation(zone[4:7])
            orientations.append(orientation)
            confidences.append(zone[7])
        
        return zonesCoords, orientations, confidences
        
    def Orientation(self, zone):
        
        total = sum(zone)
        left = zone[0] / total
        right = zone[1] / total
        back = zone[2] / total
            
        return [left, right, back]
                
    def displayZones(self, img, detectedZones, confidences):
        
        i = 0
        for zone in detectedZones:
            (xMin,yMin) = zone[0:2]
            (xMax,yMax) = zone[2:4]
            (xMin,yMin) = (int(xMin),int(yMin))
            (xMax,yMax) = (int(xMax),int(yMax))
            
            cv2.rectangle(img, (xMin,yMin), (xMax,yMax), (0,0,255), 2)

            cv2.putText(img, str(round(confidences[i],2)), (xMin,yMin), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (255, 0, 0), 2, cv2.LINE_AA)
            
            i += 1
            
        return img
    
    def distanceEstimation(self, detectedZones):
        
        distances = []
        for zone in detectedZones:
            (xMin,yMin) = zone[0:2]
            (xMax,yMax) = zone[2:4]
            area = (xMax - xMin) * (yMax - yMin)
            dist = -(1 / 3.6) * np.log(area / 19000)
            distances.append(dist)
            
        return distances

def imshow(img):
    cv2.imshow('image',img)
    cv2.waitKey(100)

if __name__ == "__main__":
    
    for arg in sys.argv[1:]:
        f = arg.find('=')
        if f == -1:
            key = arg
            val = ""
        else:
            key = arg[:f]
            val = arg[f+1:]    
        if key == "image_path":
            image = val
        else:
            print("argument \"image_path\" must be specified")
            sys.exit(0)
    image = "image_capture/frame.png"
    img = cv2.imread(image)
    #imshow(img)
    if img is None:
        print("Invalid image location specified")
        sys.exit(0)
    t0 = time.time()
    detectedZones, orientations, confidences, faces = miroDetector().detector(img,faces=True)
    for face in faces:
        (xMin,yMin) = face[0:2]
        (xMax,yMax) = (xMin + face[2], yMin + face[2])
        (xMin,yMin) = (int(xMin),int(yMin))
        (xMax,yMax) = (int(xMax),int(yMax))
        cv2.rectangle(img, (xMin,yMin), (xMax,yMax), (0,0,255), 2)
    imshow(img)
    print faces
    t1 = time.time()
    if not detectedZones is None:
        imgDetected = miroDetector().displayZones(img,detectedZones,confidences)
        imshow(imgDetected)

<file_sep>/README.md
This folder includes all the software and data necessary for the MiRo detection algorithm. Although training has already been completed, retraining can be done by running the HOG_w_knn_svm_TRAIN.py script. All training data is stored in the folders MiRo_body and Negatives, so add training data here and retrain as necessary. Finally, to test, add some images into the Test_images folder, and run Miro_detection_TESTING.py, which will classify using SVM. Some images are included already as sample images.
<file_sep>/Old_detectors/Miro_detection_TESTING.py
# <NAME>
# Here we will load up the trained SVM using HOG feature extraction. Then the
# adaptive threshold will be applied for finding a ROI, before attempting to
# classify the ROI

import numpy as np
import cv2
import os

def imshow(img):
    cv2.imshow('image',img)
    cv2.waitKey(0)
    cv2.destroyAllWindows

# Let's set up HOG and load the SVM data
winSize = (64,64)
blockSize = (16,16)
blockStride = (8,8)
cellSize = (8,8)
nbins = 9
derivAperture = 1
winSigma = 4.
histogramNormType = 0
L2HysThreshold = 2.0000000000000001e-01
gammaCorrection = 0
nlevels = 64
hog = cv2.HOGDescriptor(winSize,blockSize,blockStride,cellSize,nbins,derivAperture,winSigma,
                        histogramNormType,L2HysThreshold,gammaCorrection,nlevels)

svm = cv2.ml.SVM_load('svm.dat')

# Defining now the ROI algorithm
def ROI(img):
    # thresholding the standard image
    #img_ROI = img
    img_grey = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    mean_stddev_img = cv2.meanStdDev(img_grey)
    mean = mean_stddev_img[0]
    stddev = mean_stddev_img[1]
    thresh = mean*(1 + -1.2*(stddev/128 - 1))
    if thresh>230:
        thresh = 230
    img_thresh = cv2.inRange(img_grey, thresh, 255)
    im2, contours, hierarchy = cv2.findContours(img_thresh, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
    roi = []
    i = 0
    for c in contours:
        if cv2.contourArea(c) < 100:
            continue
        (x, y, w, h) = cv2.boundingRect(c)
        if (float(w)/float(h) > 0.25) & (w/h < 2):

            roi.append((x,y,w,h))
            i += 1
    return roi

# Let us now load the test images and run them through the ROI algorithm
for filename in os.listdir('Test_images/'):
    img = cv2.imread(os.path.join('Test_images/',filename))
    if img is None:
        continue
    roi = ROI(img)

    # The SVM classifier will now be run on each ROI. The ROI is first set to a square
    # size, reduced (or increased) to size 64x64, then tested using HOG and SVM.
    for r in range(len(roi)):
        (x,y,w,h) = roi[r]
        if w > h: # if width is greater than height, set ROI height to be same as width
            h = w
        else: # else set the ROI width to be the same as the height
            w = h

        # Now the exciting part, let's classify!
        for step in range(4): # remember, we increase the ROI size 4 times, classifying each time
            img_roi = img[y:(y+h), x:(x+w)]
            img_roi = cv2.resize(img_roi,(64,64)) # resizing the image to 64x64 size
            hog_feature = np.transpose(hog.compute(img_roi)) # computing the HOG features
            hog_feature.astype(np.float32) # preparing for input into SVM
            result = svm.predict(hog_feature)[1]
            if result==1:
                cv2.rectangle(img, (x,y-h), (x+int(w*1.25),y+h), (255,0,0), 2)
                print 'MiRo has been detected from the left side'
                break
            if result==2:
                cv2.rectangle(img, (x,y-h), (x+int(w*1.25),y+h), (255,0,0), 2)
                print 'MiRo has been detected from the right side'
                break
            if result==3:
                cv2.rectangle(img, (x,y), (x+w,y+h), (255,0,0), 2)
                print 'MiRo has been detected from the back'
                break
            #cv2.rectangle(img, (x,y), (x+w,y+h), (0,255,0), 2)
            w_new = int(w*1.25)
            h_new = int(h*1.25)
            x_new = x - (w_new - w)/2
            y_new = y - (h_new - h)/2
            if x_new > 0:
                x = x_new
            if y_new > 0:
                y = y_new
            w = w_new
            h = h_new
    imshow(img)
<file_sep>/Old_detectors/miroDetector2.py
# <NAME>
# Here we will load up the trained SVM using HOG feature extraction. Then the
# adaptive threshold will be applied for finding a ROI, before attempting to
# classify the ROI

import numpy as np
import cv2
import os
import sys
import time

def imshow(img):
    cv2.imshow('image',img)
    cv2.waitKey(0)
    cv2.destroyAllWindows

class miroDetector:
    
    def __init__(self):        
        
        # Let's set up HOG and load the SVM data
        winSize = (64,64)
        blockSize = (16,16)
        blockStride = (8,8)
        cellSize = (8,8)
        nbins = 9
        derivAperture = 1
        winSigma = 4.
        histogramNormType = 0
        L2HysThreshold = 2.0000000000000001e-01
        gammaCorrection = 0
        nlevels = 64
        self.hog = cv2.HOGDescriptor(winSize,blockSize,blockStride,cellSize,nbins,derivAperture,winSigma,
                                histogramNormType,L2HysThreshold,gammaCorrection,nlevels)
        self.svm = cv2.ml.SVM_load('svm.dat')

    def detector(self,img):
        # cropping off the top third of the image
        img = img[80:239, 0:319]
        
        roi = self.ROI(img)
        
        # The SVM classifier will now be run on each ROI. The ROI is first set to a square
        # size, reduced (or increased) to size 64x64, then tested using HOG and SVM.
        detected = []
        for r in range(len(roi)):
            (x,y,w,h) = roi[r]
            winSize = 32
            while winSize < min(w,h):
                for x_win in range(x, x + w - winSize, 4):
                    for y_win in range(y, y + h - winSize, 4):
                        try:
                            win = img[y_win:(y_win+winSize), x_win:(x_win+winSize)]
                            img_roi = cv2.resize(win,(64,64))
                            hog_feature = np.transpose(self.hog.compute(img_roi)).astype(np.float32)
                            result = self.svm.predict(hog_feature)[1]
                            if result>0:
                                detected.append(((x_win,y_win,winSize),result))
                        except cv2.error:
                            pass
                winSize += 8
        if len(detected) == 0:
            print "No MiRo detected"
            sys.exit(0)
        #~ for det in detected:
            #~ (x_win,y_win,winSize) = det[0]
            #~ result = det[1]
            #~ cv2.rectangle(img, (x_win,y_win), (x_win+winSize,y_win+winSize), (255,0,0), 2)
            #~ if result==1:
                #~ print 'MiRo has been detected from the left side'
            #~ if result==2:
                #~ print 'MiRo has been detected from the right side'
            #~ if result==3:
                #~ print 'MiRo has been detected from the back side'
            #~ imshow(img)
            
        detectedZones = self.overlap(detected)
        self.displayZones(img,detectedZones)
        return detectedZones

    def ROI(self,img):
        # thresholding the standard image
        k = 0.5
        thresh_prop = 1.0
        img_grey = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
        mean,stddev = cv2.meanStdDev(img_grey)
        while thresh_prop > 0.15:            
            thresh = mean*(1 + -k*(stddev/128 - 1))
            img_thresh = cv2.inRange(img_grey, thresh, 255)
            thresh_prop = float(cv2.countNonZero(img_thresh)) / (float(np.shape(img)[0]) * float(np.shape(img)[1]))
            #print k
            #print thresh_prop
            #imshow(img_thresh)
            k += 0.05
        _,contours,_ = cv2.findContours(img_thresh, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
        roi = []
        for c in contours:
            if cv2.contourArea(c) < 256:
                continue
            (x, y, w, h) = cv2.boundingRect(c)
            (x, y, w, h) = self.ROI_expand((x,y,w,h))
            roi.append((x,y,w,h))
        return roi
    
    def ROI_expand(self,(x,y,w,h)):
        
        w_new = int(w*1.5)
        h_new = int(h*1.5)
        x_new = x - (w_new - w)/2
        y_new = y - (h_new - h)/2
        if x_new >= 0:
            x = x_new
        if y_new >= 0:
            y = y_new
        return (x, y, w_new, h_new)
    
    def overlap(self,detected):
        # This method counts the number of overlapping detected zones, determines most likely orientation, 
        # and returns single detected regions with a count of the total number of detections per orientation
        
        x = []
        y = []
        win = []
        groups = np.zeros(len(detected))
        
        for det in detected:
            (x_win,y_win,winSize) = det[0]
            x.append(x_win)
            y.append(y_win)
            win.append(winSize)
        
        groups[0] = 1
        for i in range(len(detected)):
            for j in range(i+1,len(detected)):
                if i==j:
                    continue
                a1 = x[j] <= (x[i] + win[i]) and x[j] >= x[i]
                a2 = y[j] <= (y[i] + win[i]) and y[j] >= y[i]
                a3 = x[i] <= (x[j] + win[j]) and x[i] >= x[j]
                a4 = y[i] <= (y[j] + win[j]) and y[i] >= y[j]
                
                if a1 and (a2 or a4):
                    groups[j] = groups[i]
                elif a3 and (a2 or a4):
                    groups[j] = groups[i]
                elif groups[j] == 0:
                    groups[j] = max(groups) + 1
        
        no_groups = int(max(groups))
        groupRegions = np.zeros((no_groups,4))
        for i in range(len(groups)):            
            if groupRegions[int(groups[i]-1),3] == 0:
                groupRegions[int(groups[i]-1),:] = (x[i],y[i],x[i] + win[i],y[i] + win[i])
            else:
                if x[i] < groupRegions[int(groups[i]-1),0]:
                    groupRegions[int(groups[i]-1),0] = x[i]
                if y[i] < groupRegions[int(groups[i]-1),1]:
                    groupRegions[int(groups[i]-1),1] = y[i]
                if x[i] + win[i] > groupRegions[int(groups[i]-1),2]:
                    groupRegions[int(groups[i]-1),2] = x[i] + win[i]
                if y[i] + win[i] > groupRegions[int(groups[i]-1),3]:
                    groupRegions[int(groups[i]-1),3] = y[i] + win[i]
        
        noOrientations = np.zeros((no_groups,3)) # each row represents a group, and is organised as (noLSide, noRSide, noBack)
        for i in range(len(groups)):
            if detected[i][1] == 1.0:
                noOrientations[int(groups[i])-1,0] += 1
            if detected[i][1] == 2.0:
                noOrientations[int(groups[i]-1),1] += 1
            if detected[i][1] == 3.0:
                noOrientations[int(groups[i]-1),2] += 1
        
        detectedZones = np.zeros((no_groups,7))
        detectedZones[:,0:4] = groupRegions
        detectedZones[:,4:7] = noOrientations
        
        return detectedZones
                
    def displayZones(self, img, detectedZones):
        
        for zone in detectedZones:
            (xMin,yMin) = zone[0:2]
            (xMax,yMax) = zone[2:4]
            (xMin,yMin) = (int(xMin),int(yMin))
            (xMax,yMax) = (int(xMax),int(yMax))
            
            noLSide = zone[4]
            noRSide = zone[5]
            noBack = zone[6]
            total = noLSide + noRSide + noBack
            
            #~ LPerc = int(100 * noLSide / total)
            #~ RPerc = int(100 * noRSide / total)
            #~ BPerc = int(100 * noBack / total)
                        
            cv2.rectangle(img, (xMin,yMin), (xMax,yMax), (0,0,255), 2)
            cv2.putText(img, str(noLSide), (xMin,yMin), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (255, 0, 0), 2, cv2.LINE_AA)
            cv2.putText(img, str(noRSide), (xMin,yMin+25), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (255, 0, 0), 2, cv2.LINE_AA)
            cv2.putText(img, str(noBack), (xMin,yMin+50), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (255, 0, 0), 2, cv2.LINE_AA)
            
            imshow(img)
        
if __name__ == "__main__":
    
    for arg in sys.argv[1:]:
        f = arg.find('=')
        if f == -1:
            key = arg
            val = ""
        else:
            key = arg[:f]
            val = arg[f+1:]    
        if key == "image_path":
            image = val
        else:
            print("argument \"image_path\" must be specified")
            sys.exit(0)
    img = cv2.imread(image)
    cv2.imshow('image',img)
    cv2.waitKey(0)
    cv2.destroyAllWindows()
    if img is None:
        print("Invalid image location specified")
        sys.exit(0)
    t0 = time.time()
    miroDetector().detector(img)
    t1 = time.time()
    print t1 - t0
<file_sep>/miroDetectorROS.py
#!/usr/bin/python

import numpy as np
import cv2
import time
from miroDetector import miroDetector
import sys

import rospy
from sensor_msgs.msg import Image
from cv_bridge import CvBridge

bridge = CvBridge()
miroDetector = miroDetector()
count = 0

def drawRegions(img, detectedZones):
    
    for zone in detectedZones:
        (xMin,yMin) = zone[0:2]
        (xMax,yMax) = zone[2:4]
        (xMin,yMin) = (int(xMin),int(yMin))
        (xMax,yMax) = (int(xMax),int(yMax))
                    
        cv2.rectangle(img, (xMin,yMin), (xMax,yMax), (0,0,255), 2)
        
        return img
        
def drawFaces(img, faces):
    
    for face in faces:
        (xMin,yMin) = face[0:2]
        (xMax,yMax) = (xMin + face[2], yMin + face[2])
        (xMin,yMin) = (int(xMin),int(yMin))
        (xMax,yMax) = (int(xMax),int(yMax))
        
        cv2.rectangle(img, (xMin,yMin), (xMax,yMax), (0,0,255), 2)
        
    return img

def imshow(img):
    cv2.imshow("image", img)
    cv2.waitKey(50)

def camCallback(msg):
    global count
    #global F
    count += 1
    
    if count % 10 == 0:
        img = bridge.imgmsg_to_cv2(msg,'bgr8')
        cv2.imwrite("image_capture/frame.png", img)
        t0 = time.time()
        
        detectedZones, orientations, confidence, faces = miroDetector.detector(img, faces=True)
        
        if not detectedZones is None:
            img = drawRegions(img, detectedZones)
            distance = miroDetector.distanceEstimation(detectedZones)
            print "Distance estimate: ", distance
            print "Orientation information: ", orientations
            print "Confidence levels: ", confidence
        
        if not faces is None:
            img = drawFaces(img, faces)
            print "Faces: ", faces
        
        imshow(img)
        t1 = time.time()
        print "Processing time was: ", (t1 - t0)
    
miroDetector = miroDetector()

if __name__ == '__main__':
    print "Initialising MiRo Detector.."
    
    for arg in sys.argv[1:]:
        f = arg.find('=')
        if f == -1:
            key = arg
            val = ""
        else:
            key = arg[:f]
            val = arg[f+1:]    
        if key == "robot":
            robot = val
        else:
            print("argument \"robot\" must be specified")
            sys.exit(0)
    
    rospy.init_node('miroDetector',anonymous=True)
    
    #~ subL = rospy.Subscriber ("/miro/" + robot + "/platform/caml", Image,
                            #~ camCallback)
    subR = rospy.Subscriber ("/miro/" + robot + "/platform/camr", Image,
