def _update_filters(self, scene_changed, points):
"""
Method used to update each indiividual particle filter, using the Hungarian approach
:param scene_changed: A boolean flag used to indicate if the scene has changed, True if so.
:param points: The points provided to associate with each filter
"""
if scene_changed:
# Changed scene, so reinitialise the kalman filters
self.filters = []
if not self.filters:
for point in points:
self.filters.append(ParticleFilter(500, point))
return points
else:
dists = np.empty((len(self.filters), len(points)))
for i, filter in enumerate(self.filters):
filter.predict()
dists[i] = np.linalg.norm(points - filter.estimate(), axis=1)
new_filters = []
try:
row, col = linear_sum_assignment(dists)
except ValueError:
# linear_sum can fail at times, we revert to old filters if so
return self.filters
for i in range(len(col)):
# Row filters, col points
if dists[row[i], col[i]] > 25:
pass
# Too far - new pf from this
new_filters.append(ParticleFilter(500, points[col[i]]))
else:
filter = self.filters[row[i]]
# Add the old filter to the new list
new_filters.append(filter)
filter.update(points[col[i]])
filter.resample()
# Let any non-updated filters decay
for filter in self.filters:
# Inefficient but works
if filter not in new_filters and not filter.update_none():
# Gets to live another day
new_filters.append(filter)
# Update to new filters
self.filters = new_filters
def main():
rospy.init_node('SLAM', anonymous=True)
PF = ParticleFilter(Np=100)
PF.init()
OG = occupancy_grid(max_rays=100,
grid_size=[30, 30],
initial_pose=[15, 15])
OG.update_map(scan=PF.scan.z, initialize=1)
PF.get_map(OG)
r = rospy.Rate(5)
OG.publish_occupancy_grid()
while not rospy.is_shutdown():
OG.publish_occupancy_grid()
print 'runing!'
r.sleep()
PF.pub()
if PF.i_MU[0] > 0.01 or PF.i_MU[1] > 0.01:
#print np.mean(PF.particles,axis = 0).shape
OG.update_map(X=np.mean(PF.particles, axis=0),
scan=PF.scan.z,
initialize=0)
PF.get_map(OG)
PF.i_MU = [0.0, 0.0]
print 'updated map'
rospy.spin()
def __init__(self,
odom_lin_sigma=0.025,
odom_ang_sigma=np.deg2rad(2),
meas_rng_noise=0.2,
meas_ang_noise=np.deg2rad(10)):
'''
Initializes publishers, subscribers and the particle filter.
'''
# Publishers
self.pub_lines = rospy.Publisher("lines", Marker)
self.pub_particles = rospy.Publisher("particles", PoseArray)
self.pub_big_particle = rospy.Publisher("mean_particle", PoseStamped)
self.pub_odom = rospy.Publisher("mean_particle_odom", Odometry)
# Subscribers
self.sub_scan = rospy.Subscriber("scan", LaserScan,
self.laser_callback)
self.sub_odom = rospy.Subscriber("odom", Odometry, self.odom_callback)
# TF
self.tfBroad = tf.TransformBroadcaster()
# Incremental odometry
self.last_odom = None
self.odom = None
# Flags
self.new_odom = False
self.new_laser = False
self.pub = False
# Particle filter
self.part_filter = ParticleFilter(get_map(), 500, odom_lin_sigma,
odom_ang_sigma, meas_rng_noise,
meas_ang_noise)
def initParticleFilter(self,PFtype): self.pf = ParticleFilter().getParticleFilterClass(PFtype) #PFtype = "IMUPF" or "IMUPF2" self.pf.setParameter(0.01 ,0.001) #パーティクルフィルタのパラメータ（ノイズの分散） variance of noise self.M = 100 # パーティクルの数 num of particles self.X = [] # パーティクルセット set of particles self.loglikelihood = 0.0 self.count = 0
def __init__(self, factory):
"""Constructor.
Args:
factory (factory.FactoryCreate)
"""
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.servo = factory.create_servo()
self.sonar = factory.create_sonar()
# Add the IP-address of your computer here if you run on the robot
self.virtual_create = factory.create_virtual_create()
self.pdTheta = pd_controller.PDController(500, 40, -200, 200)
self.pidTheta = pid_controller.PIDController(200,
5,
50, [-10, 10],
[-200, 200],
is_angle=True)
self.map = lab8_map.Map("lab8_map.json")
self.base_speed = 100
self.odometry = odometry.Odometry()
self.particle_filter = ParticleFilter(
map=self.map,
virtual_create=self.virtual_create,
num_particles=100,
distance=STRAIGHT_DISTANCE,
sigma_sensor=0.1,
sigma_theta=0.05,
sigma_distance=0.01,
)
_ = self.create.sim_get_position()
def __init__(self):
# CV bridge
self.bridge = CvBridge()
# Particle filter
global lane_width
self.particle_filter = ParticleFilter(1000, -lane_width / 2,
lane_width / 2, -np.pi / 8.,
np.pi / 8., lane_width)
self.prev_v = 0.
self.prev_omega = 0.
self.prev_time = rospy.Time.now()
self.steps_since_resample = 0
# Publisers
self.mask_pub = rospy.Publisher('/mask', Image, queue_size=1)
self.edges_image_pub = rospy.Publisher('/edges_image',
Image,
queue_size=1)
self.line_segments_image_pub = rospy.Publisher('/line_segments_image',
Image,
queue_size=1)
self.lanes_image_pub = rospy.Publisher('/lanes_image',
Image,
queue_size=1)
self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
# Image subscriber
self.image_sub = rospy.Subscriber('/duckiebot/camera_node/image/rect',
Image, self.image_callback)
def __init__(self, factory):
"""Constructor.
Args:
factory (factory.FactoryCreate)
"""
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.servo = factory.create_servo()
self.sonar = factory.create_sonar()
# Add the IP-address of your computer here if you run on the robot
self.virtual_create = factory.create_virtual_create()
self.map = lab8_map.Map("lab8_map.json")
self.odometry = Odometry()
self.pidTheta = PIDController(300,
5,
50, [-10, 10], [-200, 200],
is_angle=True)
# constants
self.base_speed = 100
self.variance_sensor = 0.1
self.variance_distance = 0.01
self.variance_direction = 0.05
self.world_width = 3.0 # the x
self.world_height = 3.0 # the y
self.filter = ParticleFilter(self.virtual_create,
self.variance_sensor,
self.variance_distance,
self.variance_direction,
num_particles=100,
world_width=self.world_width,
world_height=self.world_height)
def initParticleFilter(self):
self.pf = ParticleFilter().getParticleFilterClass("Coplanarity")
self.pf.setFocus(self.f)
self.pf.setParameter(self.PFnoise_a_sys, self.PFnoise_g_sys, self.PFnoise_a_obs, self.PFnoise_g_obs, self.PFnoise_coplanarity_obs) #パーティクルフィルタのパラメータ（ノイズ） parameters (noise)
self.X = [] # パーティクルセット set of particles
self.loglikelihood = 0.0
self.count = 0
def __init__(self, *args, **kwargs):
super(DemoWindow, self).__init__(*args, **kwargs)
self.graphWidget = pg.PlotWidget()
self.graphWidget.getViewBox().invertY(True)
self.graphWidget.getViewBox().invertX(True)
self.setCentralWidget(self.graphWidget)
self.graphWidget.showGrid(x=True, y=True)
self.graphWidget.setXRange(-2, 0)
self.graphWidget.setYRange(0, 2)
self.graphWidget.addLegend()
# initialize attributes
self.u = [0, 0]
self.dt = 0.1 # 10 hz updates
self.state = [-0.5, 1.5, 0]
self.est_state = self.state
# initialize filter
N = 50
wall_lengths = [2, 2]
# self.sensor_locations = ([0,-1], [1,-1], [1,0], [1,1], [0,1])
self.sensor_locations = ([1, 0], [1, -1], [0, -1], [-1, -1], [-1, 0],
[-1, 1], [0, 1], [1, 1])
# self.sensor_readings = [0, 0, 0, 0, 0]
self.sensor_readings = [0, 0, 0, 0, 0, 0, 0, 0]
sensor_max = [50, 50, 50, 50, 50]
initial_x = [-0.5, 1.5, 0]
self.filter = ParticleFilter(N,
sensor_locations=self.sensor_locations,
sensor_max=sensor_max,
wall_lengths=wall_lengths,
initial_x=initial_x)
# initialize ROS
rospy.init_node('listener', anonymous=True)
rospy.Subscriber("cmd_vel", Twist, self.callback)
# setup timer to run particle filter at 5 Hz
self.timer = QtCore.QTimer()
self.timer.setInterval(50)
self.timer.timeout.connect(self.timer_callback)
self.timer.start()
# plot data: x, y values
pen1 = pg.mkPen(color='b')
pen2 = pg.mkPen(color='r')
self.pos_plot = self.graphWidget.plot([], [],
pen=pen2,
symbol='o',
symbolSize=10,
symbolBrush=('r'))
self.est_plot = self.graphWidget.plot([], [],
pen=pen1,
symbol='o',
symbolSize=10,
symbolBrush=('b'))
def __init__(self):
self.node_name = "tj2_romi_pf"
rospy.init_node(self.node_name,
# disable_signals=True
# log_level=rospy.DEBUG
)
# rospy.on_shutdown(self.shutdown_hook)
self.map_frame = rospy.get_param("~map_frame", "map")
self.base_frame = rospy.get_param("~base_frame", "base_link")
self.map_service_name = rospy.get_param("~static_map", "static_map")
self.show_particles = rospy.get_param("~publish_particles", True)
self.initial_range = rospy.get_param("~initial_range", None)
self.input_std = rospy.get_param("~input_std", None)
self.meas_std_val = rospy.get_param("~meas_std_val", 0.007)
self.num_particles = rospy.get_param("~num_particles", 100)
self.input_std = rospy.get_param("~input_std", None)
if self.initial_range is None:
self.initial_range = [1.0, 1.0, 1.0]
if self.input_std is None:
self.input_std = [0.007, 0.007]
self.input_vector = np.zeros(len(self.input_std))
self.pf = ParticleFilter(self.num_particles, self.meas_std_val,
self.input_std)
self.prev_pf_time = rospy.Time.now().to_sec()
self.map_raytracer = MapRaytracer(self.base_frame, "ultrasonic1",
"ultrasonic2")
self.ultrasonic1_sub = rospy.Subscriber("ultrasonic1",
Float64,
self.ultrasonic1_callback,
queue_size=25)
self.ultrasonic2_sub = rospy.Subscriber("ultrasonic2",
Float64,
self.ultrasonic2_callback,
queue_size=25)
self.odom_sub = rospy.Subscriber("odom",
Odometry,
self.odom_callback,
queue_size=25)
self.particles_pub = rospy.Publisher("pf_particles",
PoseArray,
queue_size=5)
self.broadcaster = tf2_ros.TransformBroadcaster()
self.get_map = rospy.ServiceProxy(self.map_service_name, GetMap)
rospy.loginfo("Waiting for service %s" % self.map_service_name)
rospy.wait_for_service(self.map_service_name)
self.map_msg = self.get_map().map
self.map_raytracer.set_map(self.map_msg)
rospy.loginfo("%s init done" % self.node_name)
def main():
rospy.init_node("particle_based_tracking")
detector = Detector(visualize=False, modelfileName=rospy.get_param('~model'))
transformer = Transformer("transformer")
pf = ParticleFilter(500, HParticle, metric, explorationFactor=0.1, noiseFactor=0.05, averageType="weighted")
pf.generateParticles()
viz = PFViz(pf, "/odom", "myViz")
history = []
while not rospy.is_shutdown():
try:
detector.getImage()
detector.processImage()
pixels = detector.centroids[:]
if pixels:
startPoint, endPoints = transformer.transform(pixels)
rospy.loginfo(len(startPoint))
rospy.loginfo(np.asarray(endPoints).shape)
history.append(tuple((startPoint[:-1], np.asarray(endPoints)[:,:-1])))
T0 = time.time()
for _ in range(0, numItersPerSample):
print "Updating particle filter",
print "\tmeasuring",
t0 = time.time()
pf.measureParticles(history)
t1 = time.time()
print "dt=%f" % (t1-t0),
print "\tweighting",
t0 = time.time()
pf.calculateWeights()
t1 = time.time()
print "dt=%f" % (t1-t0),
print "\tpredicting",
t0 = time.time()
prediction = pf.predict()
t1 = time.time()
print "dt=%f" % (t1-t0),
viz.update(history[-1])
print "\tresampling",
t0 = time.time()
pf.resample()
t1 = time.time()
print "dt=%f" % (t1-t0),
pf.update(None)
print
T1 = time.time()
print "Total particle filter update time %f" % (T1-T0)
except KeyboardInterrupt:
break
def __init__(self):
#separate for each
self.gateLeft = ParticleFilter()
self.gateDiv = ParticleFilter()
self.gateRight = ParticleFilter()
#val, confidence
self.gateLeftPos = [0, 0]
self.gateDivPos = [0, 0]
self.gateRightPos = [0, 0]
#buoy up/down. First/second in terms of area
self.firstBuoyHeave = ParticleFilter(pixelPos=True)
self.firstBuoyYaw = ParticleFilter()
self.firstBuoyHeavePos = [0, 0]
self.firstBuoyYawPos = [0, 0]
self.secondBuoyHeave = ParticleFilter(pixelPos=True)
self.secondBuoyYaw = ParticleFilter()
self.secondBuoyHeavePos = [0, 0]
self.secondBuoyYawPos = [0, 0]
self.gateEnable = False
self.buoyEnable = False
self.buoyPub = rospy.Publisher("buoyState", buoy, queue_size=1)
self.gatePub = rospy.Publisher("gateState", gate, queue_size=1)
self.gateResetSub = rospy.Subscriber("gateReset", Bool,
self.gateResetCB)
self.buoyResetSub = rospy.Subscriber("buoyReset", Bool,
self.buoyResetCB)
def main(DEBUG=False):
m = Map(imagePath, scale=100, sampleResolution=26)
d = Drone(m)
pf = ParticleFilter(m, d, numParticles=1000)
pf.calculateLikelihood()
pf.drawParticles()
# embed()
finish = False
manualMoveIncrement = m.scale_percent
while not finish:
update = False
dp = (0, 0)
util.drawCircle(m.image, m.positionToPixel(d.pos))
m.show()
m.clearImage()
# cv.imshow('image', m.sample(d.pos, sample_resolution=sampleWidth))
key = cv.waitKey(0)
if (key == ord('q')):
finish = True
elif (key == ord('w')):
dp = (0, -manualMoveIncrement)
d.move(dp)
update = True
elif (key == ord('s')):
dp = (0, manualMoveIncrement)
d.move(dp)
update = True
elif (key == ord('a')):
dp = (-manualMoveIncrement, 0)
d.move(dp)
update = True
elif (key == ord('d')):
dp = (manualMoveIncrement, 0)
d.move(dp)
update = True
elif (key == 13): #enter
dp = d.generateRandomMovementVector_map()
d.move(dp)
update = True
else:
print("Unrecognized key")
if (DEBUG):
distance = sum([util.distance(p.pos, d.pos)
for p in pf.particles]) / len(pf.particles)
print("true: ", d.pos)
[
print("{:01.2f} : {:01.2f}".format(p.pos[0], p.pos[1]))
for p in pf.particles
]
print("distance: ", distance)
pf.fullUpdate(dp)
def pf(self, prior_var, number_particles, prior_Voc, prior_Rs, prior_Rp,
prior_Cp, prior_Vc, observation_noise):
self.test = ParticleFilter(prior_var=prior_var,
number_particles=number_particles,
prior_Voc=prior_Voc,
prior_Rs=prior_Rs,
prior_Rp=prior_Rp,
prior_Cp=prior_Cp,
prior_Vc=prior_Vc,
observation_noise=observation_noise)
def initParticleFilter(self):
self.pf = ParticleFilter().getParticleFilterClass("RBPF")
self.pf.setFocus(self.f)
self.pf.setParameter(
self.noise_a_sys, self.noise_g_sys, self.noise_a_sys_camera,
self.noise_camera) #パーティクルフィルタのパラメータ（ノイズ） parameters (noise)
self.X = [] # パーティクルセット set of particles
self.loglikelihood = 0.0
self.count = 1
self.step = 1
def __init__(self,
odom_lin_sigma=0.025,
odom_ang_sigma=np.deg2rad(2),
meas_rng_noise=0.2,
meas_ang_noise=np.deg2rad(10),
x_init=0,
y_init=0,
theta_init=0):
'''
Initializes publishers, subscribers and the particle filter.
'''
# Threads
self.lock = threading.RLock()
# Publishers
self.pub_map = rospy.Publisher("map", Marker, queue_size=2)
self.pub_lines = rospy.Publisher("lines", Marker, queue_size=2)
self.pub_lines_mean = rospy.Publisher("lines_mean",
Marker,
queue_size=2)
self.pub_particles = rospy.Publisher("particles",
PoseArray,
queue_size=2)
self.pub_big_particle = rospy.Publisher("mean_particle",
PoseStamped,
queue_size=2)
self.pub_odom = rospy.Publisher("mean_particle_odom",
Odometry,
queue_size=2)
self.pub_wei = rospy.Publisher("weights", MarkerArray, queue_size=2)
# Subscribers
self.sub_scan = rospy.Subscriber("lines", Marker, self.laser_callback)
self.sub_odom = rospy.Subscriber("odom", Odometry, self.odom_callback)
# TF
self.tfBroad = tf.TransformBroadcaster()
# Incremental odometry
self.last_odom = None
self.odom = None
# Flags
self.new_odom = False
self.new_laser = False
self.pub = False
self.time = None
self.lines = None
# Particle filter
self.part_filter = ParticleFilter(get_dataset3_map(), 500,
odom_lin_sigma, odom_ang_sigma,
meas_rng_noise, meas_ang_noise,
x_init, y_init, theta_init)
def main():
rospy.init_node('ParticleFilter', anonymous=True)
PF_l = ParticleFilter(Np=300)
r = rospy.Rate(5)
while not rospy.is_shutdown():
r.sleep()
PF_l.pub()
rospy.spin()
def get_system_state():
saved_particles = get_db("particles")
pf = ParticleFilter(particles=saved_particles)
p.start('system_state_generation')
result = json.dumps({
'particles': sample_particles(saved_particles),
'router_distances': get_router_dist(),
'mean': pf.get_position(),
'std': pf.get_std()
})
p.pstop('system_state_generation')
return result
def main():
cap = cv2.VideoCapture(0)
ret, frame = cap.read()
print("MAIN:", frame.shape)
frame = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
x, y = 240, 320
pf = ParticleFilter(x, y, frame, n_particles=500, square_size=50, dt=0.20)
alpha = 0.5
while (True):
ret, frame = cap.read()
orig = np.array(frame)
img = frame
norm_factor = 255.0 / np.sum(frame, axis=2)[:, :, np.newaxis]
frame = frame * norm_factor
frame = cv2.convertScaleAbs(frame)
frame = cv2.blur(frame, (5, 5))
frame = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
x, y, sq_size, distrib, distrib_control = pf.next_state(frame)
p1 = (int(y - sq_size), int(x - sq_size))
p2 = (int(y + sq_size), int(x + sq_size))
# before resampling
for (x2, y2, scale2) in distrib_control:
x2 = int(x2)
y2 = int(y2)
cv2.circle(img, (y2, x2), 1, (255, 0, 0), thickness=10)
# after resampling
for (x1, y1, scale1) in distrib:
x1 = int(x1)
y1 = int(y1)
cv2.circle(img, (y1, x1), 1, (0, 0, 255), thickness=10)
cv2.rectangle(img, p1, p2, (0, 0, 255), thickness=5)
cv2.addWeighted(orig, alpha, img, 1 - alpha, 0, img)
create_legend(img, (40, 40), (40, 20))
cv2.imshow('frame', img)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
cap.release()
cv2.destroyAllWindows()
def __init__(self, factory):
"""Constructor.
Args:
factory (factory.FactoryCreate)
"""
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.servo = factory.create_servo()
self.sonar = factory.create_sonar()
# Add the IP-address of your computer here if you run on the robot
self.virtual_create = factory.create_virtual_create()
self.odometry = odometry.Odometry()
self.particle_filter = ParticleFilter()
def main():
global particles
global recive_particles
rospy.init_node('particle_filter', anonymous=True)
PF_l = ParticleFilter(Np=300)
PI_t = np.random.randn(300, 3)
fusion = RobotFusion(PF_l.particles, PI_t)
particles2fuse_cb = lambda x: particles2fuse(x, PF_l, fusion)
rospy.Subscriber('/particlecloud2fuse_in', PoseArray, particles2fuse_cb)
r = rospy.Rate(5)
#plt.ion()
#fig = plt.figure()
time_last = rospy.Time.now()
while not rospy.is_shutdown():
time_now = rospy.Time.now()
r.sleep()
fusion.PI_s = PF_l.particles
fusion.robot_tracking()
PF_l.prediction()
PF_l.likelihood_fild()
if np.abs(PF_l.update_TH()) > 0.1:
#PF_l.simpel_likelihood()
PF_l.resampling()
PF_l.i_TH = 0.0
print 'Updating particles...'
PF_l.pub()
if time_now.to_sec() - time_last.to_sec() > 2:
PF_l.pub2fuse()
time_last = time_now
if recive_particles:
fusion.PI_t = particles
PF_l.weights = fusion.update_weight()
PF_l.resampling()
recive_particles = 0
#mean = np.zeros(3)
#mean = np.mean(PF_l.particles,axis=0)
fusion.send_transfered_particles()
#M = PF_l.scan.loction_based(mean)
r.sleep()
#plt.imshow(-M+PF_l.scan.occupancy_grid)
# fig.canvas.draw()
rospy.spin()
def main():
rospy.init_node('SLAM', anonymous = True)
PF = ParticleFilter(Np=100)
OG = occupancy_grid()
OG.update_map(scan = PF.scan.z,initialize = 1)
PF.get_map(OG)
r = rospy.Rate(5)
while not rospy.is_shutdown():
r.sleep()
if PF.i_MU[0] > 0.01 or PF.i_MU[1] > 0.01:
OG.update_map(X = np.mean(PF.particles),scan = PF.scan.z,initialize = 0)
PF.get_map(OG)
PF.i_MU = [0.0,0.0]
rospy.spin()
def run(self):
self.create.start()
self.create.safe()
self.create.start_stream([
create2.Sensor.LeftEncoderCounts,
create2.Sensor.RightEncoderCounts,
])
# This is an example on how to visualize the pose of our estimated position
# where our estimate is that the robot is at (x,y,z)=(0.5,0.5,0.1) with heading pi
# self.virtual_create.set_pose((0.5, 0.5, 0.1), 0)
origin = (0.5, 0.5, 0.1, 0) # partical origin
noise = (0.01, 0.05, 0.1) # sd for (distance, theta, sonar)
self.odometry.x = origin[0]
self.odometry.y = origin[1]
self.pf = ParticleFilter(origin, noise, 1000, self.map)
self.virtual_create.set_point_cloud(self.pf.get_particle_list())
# This is an example on how to show particles
# the format is x,y,z,theta,x,y,z,theta,...
# data = [0.5, 0.5, 0.1, math.pi/2, 1.5, 1, 0.1, 0]
# self.virtual_create.set_point_cloud(data)
# This is an example on how to estimate the distance to a wall for the given
# map, assuming the robot is at (0, 0) and has heading math.pi
# print(self.map.closest_distance((0.5,0.5), 0))
# self.positioning()
# This is an example on how to detect that a button was pressed in V-REP
while True:
b = self.virtual_create.get_last_button()
if b == self.virtual_create.Button.MoveForward:
self.forward()
elif b == self.virtual_create.Button.TurnLeft:
self.turn_left()
elif b == self.virtual_create.Button.TurnRight:
self.turn_right()
elif b == self.virtual_create.Button.Sense:
self.sense()
self.time.sleep(0.01)
def __init__(self,
n_particles,
initial_measurements,
init_time,
deviations,
plot,
plotsequence,
plotafter=0):
self.plot_folder = plot_path + "/plot" + str(plotsequence)
if not os.path.exists(self.plot_folder):
os.makedirs(self.plot_folder)
if plot and wipe_dir:
self.plotafter = plotafter
files = glob.glob(self.plot_folder + "/*")
for f in files:
os.remove(f)
self.plot = plot #boolean
self.last_t = init_time
# to avoid modifying particle and weights while another thread such as maneuver negotiator's no_conflict is
# reading
self.mutex = Lock()
#get initial directions that the vehicle are travelling towards
self.travelling_directions = [
intersection.getTravellingDirection(x, y, theta, "")
for (x, y, theta, _) in initial_measurements
]
#initialize the particle filters
self.particle_filters = \
[ParticleFilter(self.travelling_directions, intersection, n_particles, m, deviations) \
for m in initial_measurements]
self.updateMostLikelyStates()
self.grantList = {
} #key = car id, value = [estimated time of finishing intersction, turn]
def generateDataPoint1(nP, resampleRate, numIterations, headless):
ret = []
m = Map(imagePath, scale=100, sampleResolution=26)
d = Drone(m)
pf = ParticleFilter(m, d, numParticles=nP, resampleRate=resampleRate)
pf.calculateLikelihood()
for i in range(numIterations):
dp = d.generateRandomMovementVector_map()
d.move(dp)
pf.fullUpdate(dp)
# ret.append(pf.averageParticleDistance(d.pos))
ret.append(pf.numParticlesClusteredAroundGroundTruth(d.pos))
if not headless:
m.clearImage()
pf.drawParticles()
util.drawCircle(m.image, m.positionToPixel(d.pos))
m.show()
key = cv.waitKey(0)
return ret
def init_tracker(self, frame, bbox):
tracker_types = [
'BOOSTING', 'MIL', 'KCF', 'TLD', 'MEDIANFLOW', 'GOTURN'
]
tracker_type = tracker_types[0]
if tracker_type == tracker_types[0]:
self.tracker = cv2.TrackerBoosting_create()
if tracker_type == 'MIL':
self.tracker = cv2.TrackerMIL_create()
# if tracker_type == 'KCF':
# self.tracker = cv2.TrackerKCF_create()
if tracker_type == 'TLD':
self.tracker = cv2.TrackerTLD_create()
if tracker_type == 'MEDIANFLOW':
self.tracker = cv2.TrackerMedianFlow_create()
if tracker_type == 'GOTURN':
self.tracker = cv2.TrackerGOTURN_create()
# Initialize tracker with first frame and bounding box
ok = self.tracker.init(frame, bbox)
self.particle_filter = ParticleFilter()
self.particle_filter = ParticleFilter.init_particles(
self.particle_filter, region=bbox, particlesPerObject=100)
img = frame[int(bbox[1]):int(bbox[1] + bbox[3]),
int(bbox[0]):int(bbox[0] + bbox[2])]
hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
self.histo = cv2.calcHist([hsv], [0, 1], None, [180, 256],
[0, 180, 0, 256])
self.points.append(
ParticleFilter.get_particle_center(self.particle_filter))
self.frame_number += 1
return self
def __init__(self):
rospy.init_node('pf')
real_robot = False
# create instances of two helper objects that are provided to you
# as part of the project
self.particle_filter = ParticleFilter()
self.occupancy_field = OccupancyField()
self.TFHelper = TFHelper()
self.sensor_model = sensor_model = SensorModel(
model_noise_rate=0.5,
odometry_noise_rate=0.15,
world_model=self.occupancy_field,
TFHelper=self.TFHelper)
self.position_delta = None # Pose, delta from current to previous odometry reading
self.last_scan = None # list of ranges
self.odom = None # Pose, current odometry reading
self.x_y_spread = 0.3 # Spread constant for x-y initialization of particles
self.z_spread = 0.2 # Spread constant for z initialization of particles
self.n_particles = 150 # number of particles
# pose_listener responds to selection of a new approximate robot
# location (for instance using rviz)
rospy.Subscriber("initialpose", PoseWithCovarianceStamped,
self.update_initial_pose)
rospy.Subscriber("odom", Odometry, self.update_position)
rospy.Subscriber("stable_scan", LaserScan, self.update_scan)
# publisher for the particle cloud for visualizing in rviz.
self.particle_pub = rospy.Publisher("particlecloud",
PoseArray,
queue_size=10)
def create_particle_filter(initial_position, Q, R, initial_img_filename, args):
initial_img = plt.imread(initial_img_filename)
initial_position = initial_position
frame_id, x, y, w, h, is_lost = initial_position
x = float(x)
y = float(y)
w = float(w)
h = float(h)
initial_state = None
if args.point_estimate:
initial_state = (x + w / 2, y + h / 2)
else:
if not args.global_tracking:
initial_state = (x, y, x + w, y + h)
# Initialize PF
pf = ParticleFilter(num_particles=args.num_particles,
R=R,
img_shape=initial_img.shape,
resample_mode=args.resample_mode,
initial_state=initial_state,
point_estimate=args.point_estimate)
return pf
def main():
global particles
global recive_particles
rospy.init_node('particle_filter', anonymous = True)
PF_l = ParticleFilter(Np=200)
PI_t = np.random.randn(200,3)
fusion = RobotFusion(PF_l.particles, PI_t)
particles2fuse_cb = lambda x: particles2fuse(x,PF_l,fusion)
rospy.Subscriber('/particlecloud2fuse_in', PoseArray, particles2fuse_cb)
r = rospy.Rate(5)
time_last= rospy.Time.now()
while not rospy.is_shutdown():
time_now = rospy.Time.now()
r.sleep()
fusion.PI_s = PF_l.particles
PF_l.pub()
E_x = np.mean(fusion.PI_s,axis=0)
if time_now.to_sec() - time_last.to_sec() > 1:
PF_l.pub2fuse()
time_last = time_now
if recive_particles:
print "recived particles!"
fusion.PI_t = particles
d = np.linalg.norm(np.mean(particles[:,0:2],axis=0) - fusion.mu_t_hat)
print "d= ", d
if d < 0.7:
PF_l.weights = fusion.update_weight()
PF_l.resampling()
recive_particles = 0
fusion.robot_tracking()
fusion.send_transfered_particles()
rospy.spin()
transformer = None
"""
:type: tf.Transformer
"""
grid_publisher = None
dynamic_publisher = None
static_publisher = None
dynamics_plot = None
dynamics_image = None
params = ParticleFilterParams(rospy)
particle_filter = ParticleFilter(params)
particle_publisher = None
loader_publisher = None
map = None
class MyTransformer(tf.TransformListener):
def transformPose(self, target_frame, ps, header=None):
if hasattr(ps, "header"):
return super(MyTransformer, self).transformPose(target_frame, ps)
