def process_movie(mov, op, index, results): print(f"performing shot detection for {mov}") shot_detection(mov, op) print(f"Saving shots for {mov}") save_shots(mov, op) print(f"procesing shot colors for {mov}") shot_colors(mov, op) print(f"processing shot colors average for {mov}") shot_colors_avg(mov, op) print(f"processing movie colors for {mov}") movie_colors(mov, op) print(f"Calculating motion for {mov}") motion(mov, op) print(f"Sorting motion spectrum for {mov}") sort_motion_spectrum(mov, op) results[index] = quantize(mov, op) return
def take_pic(sigt, strt, turn, flag): import cv2 import numpy as np import time import motion import mail capture = cv2.VideoCapture( 0 ) #'GenYoutube.net_Cars_moving_on_road_Stock_Footage_-_Free_Download.mp4.mp4') fgmk = cv2.createBackgroundSubtractorMOG2() def get_image(): ret, frame = capture.read() frame1 = fgmk.apply(frame) frame2 = cv2.bitwise_and(frame, frame, mask=frame1) return frame2, frame1, frame for i in range(30): temp = get_image() print('Taking image...') t = round(time.time()) nm = "/home/rahul/Project/images/" + str(t) camera_capture, mask, orig = get_image() cv2.imwrite(nm + '.png', camera_capture) #cv2.imwrite(nm+'_mask.png',mask) cv2.imwrite(nm + '_orig.png', orig) #cv2.imshow(nm+'_orig.png',orig) #time.sleep(5) del (capture) cv2.destroyAllWindows() mail.find_plate( nm) # image extesion is missing added later in the called function motion.motion(sigt, strt, turn, flag) ''' img=cv2.imread('first.png',cv2.IMREAD_GRAYSCALE) threshold = cv2.adaptiveThreshold(img,255,cv2.ADAPTIVE_THRESH_GAUSSIAN_C,cv2.THRESH_BINARY,115,1) cv2.imwrite('second.png',threshold) NOT THE RIGHT IDEA ''' '''To remove noise img=cv2.imread('first.png') kernel = np.ones((115,15),np.float32)/255 smooth =cv2.filter2D(img,-1,kernel) cv2.imshow('smoother',smooth) cv2.waitKey(0) cv2.destroyAllWindows() NO USE ''' '''kernel = np.ones((5,5),np.int8)
def mov_analysis(statsqueue, imagequeue, imagennqueue, brightanalysis_targetarea, brightanalysis_drawarea, motionareas_resize, motionareas_crop, motionareas_targetarea): t = motion(brightanalysis_targetarea, brightanalysis_drawarea, motionareas_resize, motionareas_crop, motionareas_targetarea) start_time = time.time() cpt = 0 while True: #if imagequeue.qsize() > 30: # print("Warning : imagequeue : " + str(imagequeue.qsize())) image = imagequeue.get() # Added if not (image is None): result, color_image_src, grey_image, stats = t.run(image) if (result > 0): # Ajout filtrage 1 image sur 2 pour ne pas surcharger le nn if (cpt >= 1): cpt = 0 else: imagennqueue.put([time.time(), image]) cpt += 1 print("Exiting process motion")
def __parseMotion(self, fin): fin.readline() blockChainNum = int(re.match(r'BlockNum: (\d+)', fin.readline().strip('\n')).group(1)) hashCode = re.match(r'Hash: ([\w\d]+)', fin.readline().strip('\n')).group(1) innerRadius = int(re.match(r'Inner radius: (\d+)', fin.readline().strip('\n')).group(1)) outerRadius = int(re.match(r'Outer radius: (\d+)', fin.readline().strip('\n')).group(1)) motionAngle = [] clockwiseDirect = [] pattern = re.compile(r'Motion: Angle: (\d+) Clockwise: (\w+)') while True: tmpStr = fin.readline() if tmpStr == '\n': break else: tmpStr = tmpStr.strip('\n') result = pattern.match(tmpStr) motionAngle.append(int(result.group(1))) if result.group(2)=='true': clockwiseDirect.append(True) else: clockwiseDirect.append(False) # WARNING: droneAngle here is hardcoded, and is not read from the metadata! This will be fixed in the parser # for the new metadata format. Hard to fix in the old format. return motion(blockChainNum, hashCode, innerRadius, outerRadius, motionAngle, clockwiseDirect, droneAngle=50)
def __parseMotion(self, fin): # extract init info blockChainNum = re.match(r'[\d]+ BlockNum ([\w-]+)', fin.readline().strip('\n')).group(1) hashCode = re.match(r'[\d]+ Hash ([\w-]+)', fin.readline().strip('\n')).group(1) innerRadius = int( re.match(r'[\d]+ Inner radius: (\d+)', fin.readline().strip('\n')).group(1)) outerRadius = int( re.match(r'[\d]+ Outer radius: (\d+)', fin.readline().strip('\n')).group(1)) startingPoint = re.match(r'[\d]+ STARTING_POINT ([\w]+)', fin.readline().strip('\n')).group(1) centerPoint_pattern = re.match( r'[\d]+ CENTER_POINT {"longitude":([-\d.]+),"latitude":([-\d.]+)}', fin.readline().strip('\n')) centerPoint = float(centerPoint_pattern.group(1)), float( centerPoint_pattern.group(2)) droneAngle = abs( int( re.match(r'[\d]+ RESET_GIMBAL ([-\d]+)', fin.readline().strip('\n')).group(1))) startMotionAngle = int( re.match(r'[\d]+ Motion: Angle: (\d+)', fin.readline().strip('\n')).group(1)) yaw = int( re.match(r'[\d]+ YAW ([-\d]+)', fin.readline().strip('\n')).group(1)) assert re.match( r'[\d]+ (\w+)', fin.readline().strip('\n')).group(1) == 'START_RECORDING' motionAngle_pattern = re.compile(r'[\d]+ Motion: Angle: (\d+) (\w+)') motionAngle = [startMotionAngle] clockwiseDirect = [False] while True: tmpStr = fin.readline().strip('\n') cmd = re.match('[\d]+ ([:\w]+).*', tmpStr).group(1) if cmd == 'IN_AND_OUT': pass elif cmd == 'Motion:': result = motionAngle_pattern.match(tmpStr) motionAngle.append(int(result.group(1))) if result.group(2) == 'true': clockwiseDirect.append(True) else: clockwiseDirect.append(False) elif cmd == 'STOP_RECORDING': break # print(re.match(r'[\d]+ ([\w ]+)', fin.readline().strip('\n')).group(1) == 'TIMELINE STARTED') return motion(blockChainNum, hashCode, innerRadius, outerRadius, motionAngle, clockwiseDirect, droneAngle)
from picamera import PiCamera from time import sleep from motion import motion #from predict import Predict #from servo import Servo camera = PiCamera() motion_obj = motion() rows = 4 cols = 2 j = 0 i = 0 while (j < cols): i = 0 while (i < rows): if (j % 2 == 0): motion_obj.forward() motion_obj.stop() else: motion_obj.reverse() motion_obj.stop() camera.start_preview() sleep(1) camera.capture('captured_img/' + str(j + 1) + '_' + str(i + 1) + '.jpg') camera.stop_preview() sleep(1) i += 1
def seif(): filename1 = 'z.mat' filename2 = 'aa3_dr.mat' filename3 = 'aa3_lsr2.mat' filename4 = 'c_5000.mat' include_dir = './data/' z_contents = sio.loadmat(include_dir + filename1) z = z_contents['z'] z_contents = sio.loadmat(include_dir + filename2) speed = z_contents['speed'].ravel() steering = z_contents['steering'].ravel() time = z_contents['time'].ravel() z_contents = sio.loadmat(include_dir + filename3) timeLsr = z_contents['TLsr'].ravel() L = size(timeLsr, 0) z_contents = sio.loadmat(include_dir + filename4) corresp = z_contents['corresp'] Lc = size(corresp, 0) del z_contents dt = 25e-3 G = csr_matrix((1, 1), dtype=bool) Q = diag([5.0, 0.02]) m = zeros(3) xi = zeros(3) omega = 10e4 * eye(3) m0 = zeros(0) #max active landmarks N = 20 plt.ion() fig, ax = plt.subplots(1, 1) ax.set_aspect('equal') ax.set_xlim(-100, 300) ax.set_ylim(-50, 350) ax.hold(True) plt.show(False) plt.draw() #background = fig.canvas.copy_from_bbox(ax.bbox) line1 = ax.plot(0, 0, 'b-')[0] line2 = ax.plot(-1000, 1000, 'ro', markersize=2)[0] poses = zeros([3, 5000]) stindex = 0 j = searchsorted(timeLsr, time[stindex]) timechr.sleep(3) t1 = timechr.time() for i in range(stindex, time.shape[0]): #for i in range(stindex, 10000): t3 = timechr.time() if i > stindex and i % 5000 == 0: save(include_dir + 'poses_' + str(i), poses) save(include_dir + 'landmarks_' + str(i), m) save(include_dir + 'xi_' + str(i), xi) save(include_dir + 'omega_' + str(i), omega) save(include_dir + 'm0_' + str(i), m0) save_npz(include_dir + 'G_', G) xi, omega, m, G = motion(speed[i], steering[i], dt, m0, xi, omega, m, G) m = estimate(m0, xi, omega, m) while j < L and timeLsr[j] < time[i] + dt * 1000: z1 = z[0:2, nonzero(z[0, :, j]), j].transpose((0, 2, 1))[:, :, 0] if z1.size > 0: co = correspondence(z1, m0, omega, m, G) xi, omega, m, G = measurement(z1, co, xi, omega, m, G) j += 1 if co.size > 0: n = (xi.size - 3) // 2 m0a = setdiff1d(m0, co) #returns float when m0 empty tq = hstack((m0a, unique(co))).astype(int) tq = tq[max(0, tq.size - N):] m1 = setdiff1d(m0, tq) m1 = union1d(m1, setdiff1d(co, tq)) m0 = tq if m1.size > 0: xi, omega, G = sparsification(m0, m1, xi, omega, m, G) print("\x1b[2J\x1b[H") t2 = timechr.time() print('iter: ' + str(i)) print('iter time: {0:.5f}'.format(t2 - t3)) print('avg: {0:.5f}'.format((t2 - t1) / (i + 1))) poses[:, i % 5000] = m[0:3] if i % 5000 == 4999: line2.set_data(m[3::2], m[4::2]) line1.set_xdata(hstack((line1.get_xdata(), poses[0, :]))) line1.set_ydata(hstack((line1.get_ydata(), poses[1, :]))) #fig.canvas.restore_region(background) #ax.draw_artist(line1) #ax.draw_artist(line2) fig.canvas.blit(ax.bbox) fig.canvas.draw()
if name == 'nt': CST_LSS_Port = "COM3" # for mac and linux(here, os.name is 'posix') else: CST_LSS_Port = "/dev/ttyUSB0" # Constants #CST_LSS_Port = "/dev/ttyUSB0" # For Linux/Unix platforms # CST_LSS_Port = "COM1" # For windows platforms CST_LSS_Baud = lssc.LSS_DefaultBaud # Create and open a serial port lss.initBus(CST_LSS_Port, CST_LSS_Baud) # Create LSS objects mt = motion(lss) # define our clear function def clear(): # for windows if name == 'nt': _ = system('cls') # for mac and linux(here, os.name is 'posix') else: _ = system('clear') # Setup GPIO input reading
def mainListen(): while True: x=listen().lower() if "news" in x: if "india" in x: playNewsIndia() doyou="Sounds/Do-you-want-me-to-do-anything-1619637731.mp3" os.system("play "+doyou+" tempo 1.1") # speak("Do you want me to do anything else?") else: playNews() doyou="Sounds/Do-you-want-me-to-do-anything-1619637731.mp3" os.system("play "+doyou+" tempo 1.1") # speak("Do you want me to do anything else?") elif "exit" in x or "no" in x or "no exit" in x: exit="Sounds/Alright-Thank-You-for-using-t1619638216.mp3" os.system("play "+exit+" tempo 1.1") # speak("Alright, Thank You for using this Voice Assistant, have a nice day.") break elif "songs" in x or "play songs" in x or "play some songs" in x or "music" in x: music=Music() music.main() doyou="Sounds/Do-you-want-me-to-do-anything-1619637731.mp3" os.system("play "+doyou+" tempo 1.1") elif "email" in x or "send email" in x or "send" in x or "send an email" in x: alri="Sounds/Okay1620499210.mp3" os.system("play "+alri+" tempo 1.1") emailer() doyou="Sounds/Do-you-want-me-to-do-anything-1619637731.mp3" os.system("play "+doyou+" tempo 1.1") elif "radio" in x or "play radio" in x: radi="Sounds/OkayPlaying-RadioEnjoy1621184618.mp3" os.system("play "+radi+" tempo 1.1") os.system("python3 radio.py") it_was_so_good="Sounds/It-was-so-good1621184764.mp3" os.system("play "+it_was_so_good+" tempo 1.1") doyou="Sounds/Do-you-want-me-to-do-anything-1619637731.mp3" os.system("play "+doyou+" tempo 1.1") elif "time" in x: if "date" in x: getDate_Time() doyou="Sounds/Do-you-want-me-to-do-anything-1619637731.mp3" os.system("play "+doyou+" tempo 1.1") else: getTime() doyou="Sounds/Do-you-want-me-to-do-anything-1619637731.mp3" os.system("play "+doyou+" tempo 1.1") elif "date" in x: getDate() doyou="Sounds/Do-you-want-me-to-do-anything-1619637731.mp3" os.system("play "+doyou+" tempo 1.1") elif "age" in x or "old" in x: age() doyou="Sounds/Do-you-want-me-to-do-anything-1619637731.mp3" os.system("play "+doyou+" tempo 1.1") elif "describe yourself" in x: alina="Sounds/Myself-Eleena-Alexas-little-1619638384.mp3" os.system("play "+alina+" tempo 1.1") # speak("Myself Alina, Alexa's little sister.") hm="greetings/How-can-I-help-you1619639087.mp3" os.system("play "+hm+" tempo 1.1") # speak("How may I help you?") elif "where" in x or "where am i" in x or "location" in x or "locate me" in x: # speak("You are currently in "+getLocation()[0]+getLocation()[1]) # located="Sounds/You-are-currently-in1619697928.mp3" # os.system("play "+located+" tempo 1.1") # speak(getLocation()[0]+getLocation()[1]) located="Sounds/You-are-currently-in-Valsad-Gu1619698254.mp3" os.system("play "+located+" tempo 1.1") doyou="Sounds/Do-you-want-me-to-do-anything-1619637731.mp3" os.system("play "+doyou+" tempo 1.1") elif "monitor" in x or "home" in x or "monitor my home" in x or "house" in x: mt="Sounds/AlrightI-am-now-monitoring-you1620383303.mp3" os.system("play "+mt+" tempo 1.1") motion() break elif "vaccinations" in x or "vaccines" in x or "vaccinations have started" in x or "have vaccinations" in x or "vaccination" in x: ok_let_me_check="Sounds/OKLet-me-check1620390574.mp3" os.system("play "+ok_let_me_check+" tempo 1.1") time.sleep(0.5) vaccination() doyou="Sounds/Do-you-want-me-to-do-anything-1619637731.mp3" os.system("play "+doyou+" tempo 1.1") else: icouldnot="Sounds/Sorry-I-could-not-do-that-at-1619638005.mp3" os.system("play "+icouldnot+" tempo 1.1") # speak("Sorry, I could not do that at this point of time, but I may get future updates") doyou="Sounds/Do-you-want-me-to-do-anything-1619637731.mp3" os.system("play "+doyou+" tempo 1.1") # speak("Do you want me to do anything else?")
try: video_path = dl.download(arg) except Exception, e: print_error(str(e)) if video_path is None: print_error('Cannot download video from YouTube') else: video_path = arg try: with open(log_file, 'a') as f, stderr_redirector(f): if verbose: print_info('Normalizing video...') audio, video = dl.normalize(video_path, remove=False) if verbose: print_info('Extracting motion features...') motion_f = mo.motion(video) if verbose: print_info('Extracting audio features...') audio_f = au.extractAudioFeatures(audio) if verbose: print_info('Extracting image features...') image_f = im.extract_image_feature(video) features = [np.array(list(motion_f) + list(audio_f) + list(image_f))] label = clf.predict(features)[0] if verbose: print_info('Removing temporary files...') os.unlink(video) os.unlink(audio) return label
if __name__ == '__main__': try: GPIO.setwarnings(False) GPIO.setmode(GPIO.BOARD) #buzzer = Buzzer() led = LED() sp = SensorProcess(led) database = Database() buttons = Buttons(database, sp, led) buttons_thread = threading.Thread(target=buttons.start,) buttons_thread.start() receive = Rec(database, sp); receive_thread = threading.Thread(target=receive.start,) receive_thread.start() motion = motion() motion_thread = threading.Thread(target=motion.start,) motion_thread.start() while True: pass except KeyboardInterrupt: GPIO.cleanup() database.cleanup()
@author: Arnov """ import video from mosaicBuilder import mosaic from motion import motion import imutils import time import cv2 leftVideo = video.video(src=0).start() #The webcam attached to the laptop rightVideo = video.video(src=1).start() #The inbuilt camera of the laptop panImage = mosaic() motion = motion(area=500) time.sleep(2.0) while True: l = leftVideo.read() r = rightVideo.read() left = imutils.resize(l, width=400) right = imutils.resize(r, width=400) result = panImage.pack(left, right) if result is None: print("Something went wrong, no results :(") break
from configuration import configuration as config from log import logging from signal import pause from time import sleep from audiomoth import audiomoth from motion import motion from camera import camera from diskio import diskio from threading import Thread, ThreadError m = motion() c = camera() am = audiomoth(config.am_swdio_pin, config.am_rst_pin, config.am_swo_pin, config.am_swclk_pin) d = diskio() def on_motion(): logging.info("on_motion") print("Recording starting") x = Thread(target=am.unmountMoth, args=()) y = Thread(target=c.click, args=(config.photo_count_on_motion, config.photo_count_delay_sec)) x.start() y.start() x.join() y.join() print("Recording started") sleep(30)
def extract_feature(data, **kwargs): try: num = kwargs.pop('num_video', NUM_VIDEO) count = 0 file = open(data, "r") data_file = data[5:-4] audio_file = open("csv/%s_audio.csv" % data_file, "w") image_file = open("csv/%s_image.csv" % data_file, "w") image_file.close( ) # Close immediately to clear contents in file; file will be write in subprocess motion_file = open("csv/%s_motion.csv" % data_file, "w") contents = file.readlines() for content in contents: try: content = content[:-1] print_process('\r#%s %-30s:\tDownloading' % (count, content)) path = dl.download(content) if path is None: print '#%s %-30s:\tDOWNLOAD FAIL' % (count, content) continue print_process('\r#%s %-30s:\tNormalizing' % (count, content)) audio_path, video_path = dl.normalize(path) print_process('\r#%s %-30s:\tExtracting audio feature ' % (count, content)) audio_features = au.extractAudioFeatures(audio_path) print_process('\r#%s %-30s:\tExtracting motion feature ' % (count, content)) motion_features = mo.motion(video_path) print_process('\r#%s %-30s:\tExtracting image feature ' % (count, content)) print_process('\r') # No longer use extract_image_feature # image_features = im.extract_image_feature(video_path) # Use subprocess to extracting image features command = "python src/process_image_csv.py videos/%s.avi csv/%s_image.csv" % ( content, data_file) process_image_log = open("process_image.log", 'w') subprocess.call(command, shell=True, stdout=process_image_log, stderr=subprocess.STDOUT) print_process('\r#%s %-30s:\tExporting csv file' % (count, content)) export_csv(audio_features, content, audio_file) export_csv(motion_features, content, motion_file) # export_csv(image_features,content,image_file) print_process('\r#%s %-30s:\tRemoving files' % (count, content)) os.remove(video_path) os.remove(audio_path) except Exception: print '#%s %-30s:\tEXPORT FAIL' % (content, count) else: print '\r#%s %-30s:\tSUCCESS ' % ( count, content) count += 1 if str(count) == num: break finally: print_process('\rClosing file') file.close() # image_file.close() motion_file.close() audio_file.close()
data_file_number = '2' f = 100 T = 1 / f # 1. read data measure_data = readData.readData(file_dir, data_file_number) print(measure_data.frames) # 2. calculate the norm of acc and gyro and detect the contact contact_flag = Cal_Acc_Gyro.contac_detection(measure_data.acc_data, measure_data.gyro_data, measure_data.frames) # 3. calculate the attitude and position calculate_data = motion.motion(measure_data.acc_data, measure_data.gyro_data, contact_flag) # 4. plot the signal of vel and position n = measure_data.frames t = np.linspace(0, n * T, n + 1) plt.figure(1) ax1 = plt.subplot(2, 1, 1) #ax1.plot(t,calculate_data.vel.x_data,t,calculate_data.vel.y_data,t,calculate_data.vel.z_data,label='z') ax1.plot(t, calculate_data.vel.x_data, label='x') ax1.plot(t, calculate_data.vel.y_data, label='y') ax1.plot(t, calculate_data.vel.z_data, label='z') ax1.legend() ax2 = plt.subplot(2, 1, 2) #ax2.plot(t,calculate_data.ss.x_data,t,calculate_data.ss.y_data,t,calculate_data.ss.z_data) ax2.plot(t, calculate_data.ss.x_data, label='x') ax2.plot(t, calculate_data.ss.y_data, label='y')
def gps(record, tripid, xFlat, yFlat): # communicate with gps module # dictionary to store values from gps, loaded with default values gpsvalues = { 'speed': 10, 'time': '2019-09-23T22:53:41.000Z', 'lat': '40.1', 'lng': '80.1', 'alt': 0, 'climb': 0 } distance = 0 try: gpsd.connect() # connect to gps module gpsData = gpsd.get_current() # get data packet from gps if gpsData.mode > 1: # if gps mode is greater than 1 gps has a fix gpsvalues['time'] = gpsData.time # time with date gpsvalues['speed'] = gpsData.hspeed # horizontal speed gpsvalues['lat'] = gpsData.lat # latitude gpsvalues['lng'] = gpsData.lon # longitude gpsvalues['alt'] = gpsData.alt # altitude try: xrotate, yrotate = motion.motion( ) # motion function to for current angle gpsvalues['climb'] = xrotate if xrotate > xFlat: # to determine if travelling uphill/downhill/flat gpsvalues['climb'] = 1 elif xrotate < xFlat: gpsvalues['climb'] = -1 else: gpsvalues['climb'] = 0 except: logging.error('Motion Read Error') if gpsvalues['speed'] > 0.25: # filter speed gpsvalues['speed'] = round( gpsvalues['speed'] * 3.6, 1) # convert speed m/s to km/h, round 1 decimal distance = gpsvalues[ 'speed'] / 3600 # calculate distance travelled in last second else: gpsvalues['speed'] = 0.0 if record is True: # if recording trip save values interface.entryid += 1 # increment entry_id, entry_id must be unique for entry into database interface.gpslist.append( (interface.entryid, gpsvalues['time'], float(gpsvalues['speed']), float(gpsvalues['lat']), float(gpsvalues['lng']), gpsvalues['alt'], gpsvalues['climb'], tripid)) except: logging.error('GPS Module Error') pass return gpsvalues, distance
xes = np.linspace(xMin, xMax, xSteps) print(xes) logging.info(xes) initialY = 80 yMin = -24 yMax = 16 ySteps = 21 yes = np.linspace(yMin, yMax, ySteps) print(yes) logging.info(yes) if options.nevents < 1: sys.exit("Arguments to -n must be positive") motors = motion.motion(port='COM3') time.sleep(1) motors.moveFor(-500, -500) motors.moveTo(initialX, initialY) motors.setHome() f = open(options.outfile, "w") f.write('x\ty\trates\n') try: for x in xes: for y in yes: logging.info(f'Moving to {x},{y}') print((f'Moving to {x},{y}')) motors.moveTo(x=x, y=y)
def mcl(state, u, z): # Variavel de Normalizacao wsum = 0.0 # Variaveis de medida de confianca wavg = 0.0 wfast = param.wfast wslow = param.wslow # transforma u nas varias componentes separadas u_x = [u[0], u[3]] u_y = [u[1], u[4]] u_t = [u[2], u[5]] # em cada ciclo variavel reiniciada x = [] y = [] t = [] w = [] # Prediction e Update for i in range(param.M): # funcao com o motion model xaux, yaux, taux = motion(u_x, u_y, u_t, state.x[i], state.y[i], state.t[i]) # Inicia Waux waux = 1.0 waux = waux * maps.mapa(xaux, yaux) # funcao com o perception model waux = perception(z, [xaux, yaux, taux]) * waux # adicionar as novas particulas x[len(x):] = [xaux] y[len(y):] = [yaux] t[len(t):] = [taux] w[len(w):] = [waux * state.w[i]] wsum = wsum + waux * state.w[i] wavg = wavg + waux * state.w[i] / param.M # normalizacao dos pesos for i in range(param.M): if wsum > 0: w[i] = float(w[i]) / wsum #atribuicao ao estado dos novos valores state.x = x state.y = y state.t = t state.w = w # Determinacao de confianca das particulas wslow = wslow + param.alphaslow * (wavg - wslow) wfast = wfast + param.alphafast * (wavg - wfast) # Probabilidade de falha ou kidnap kidnap_prob = max([0.0, 1 - wfast / wslow]) # Calculo da pose media mean_pose = state.rosbustmean() print kidnap_prob, wfast, wslow, wavg # Resampling neff = resample.eff_particles(state.w) # Verificar numero efectivo de particulas if neff < 1 * param.M / 3: state = resample.kidnap_resample(state, kidnap_prob, z) # Guarda wfast param.wfast = wfast param.wslow = wslow return state, mean_pose