def __init__(self):
self.particles = deque()
self.arm = 'L'
self.bridge = cv_bridge.CvBridge()
self.grasp = HOME_GRASP
self.controlPose = np.identity(4)
self.myControlPose = self.controlPose
self.lock = False
self.StartPose = HOME_POSE
self.StartGrasp = HOME_GRASP
self.StartJoints = invArmKin(self.arm, self.StartPose, self.StartGrasp)
self.prevpose = self.StartPose
print 'Start'
print self.prevpose.matrix
self.RSimulator = RavenSimulator(initJoints=self.StartJoints)
for i in range(0,NUM_OF_PARTICLES):
particle = Particle()
particle.pose = self.sample_from_r(self.StartPose, rot_noise=0.4, trans_noise=0.02)
particle.pixels = np.zeros(YDIM_DOWN*XDIM_DOWN)
self.particles.append(particle)
self.depth_im_orig = self.RSimulator.getExpectedMeasurement(self.StartPose)
self.numUpdates = 0
self.publishedPose = False
self.skipRate = SKIP_RATE
rospy.Subscriber('/bag/raven_command', RavenCommand, self._updateRavenCommand)
rospy.Subscriber('/bag/downsampled_depth', Image, self._update)
self.tracked_pose_pub = rospy.Publisher('raven_tracked_pose', PoseStamped)
class Particle_Filter:
def __init__(self):
self.particles = deque()
self.arm = 'L'
self.bridge = cv_bridge.CvBridge()
self.grasp = HOME_GRASP
self.controlPose = np.identity(4)
self.myControlPose = self.controlPose
self.lock = False
self.StartPose = HOME_POSE
self.StartGrasp = HOME_GRASP
self.StartJoints = invArmKin(self.arm, self.StartPose, self.StartGrasp)
self.prevpose = self.StartPose
print 'Start'
print self.prevpose.matrix
self.RSimulator = RavenSimulator(initJoints=self.StartJoints)
for i in range(0,NUM_OF_PARTICLES):
particle = Particle()
particle.pose = self.sample_from_r(self.StartPose, rot_noise=0.4, trans_noise=0.02)
particle.pixels = np.zeros(YDIM_DOWN*XDIM_DOWN)
self.particles.append(particle)
self.depth_im_orig = self.RSimulator.getExpectedMeasurement(self.StartPose)
self.numUpdates = 0
self.publishedPose = False
self.skipRate = SKIP_RATE
rospy.Subscriber('/bag/raven_command', RavenCommand, self._updateRavenCommand)
rospy.Subscriber('/bag/downsampled_depth', Image, self._update)
self.tracked_pose_pub = rospy.Publisher('raven_tracked_pose', PoseStamped)
def _updateRavenCommand(self,msg):
while(self.lock):
i = 0
self.lock = True
for arm, arm_msg in zip(msg.arm_names, msg.arms):
if arm == self.arm:
pose = arm_msg.tool_command.pose
pose = tfx.pose(pose.position,pose.orientation,header=msg.header)
#if(np.sum(np.sum(np.abs((tfx.pose([0,0,0]).matrix - pose.matrix))))):
# print "Pose"
#print pose.matrix
# print self.prevpose.matrix
print "Delta", pose.matrix*linalg.inv(self.prevpose.matrix)
self.controlPose = pose.matrix*linalg.inv(self.prevpose.matrix)*self.controlPose
self.prevpose = copy.copy(pose)
#print "got command"
self.lock = False
def sample_from_r(self,pose,rot_noise=0.1,trans_noise=0.002):
rand_mat = np.eye(3,3) + rot_noise*(np.random.rand(3,3) - 0.5*np.ones((3,3)))
U, S, V = np.linalg.svd(rand_mat)
rand_rot = U.dot(V)
rand_rot_pose = tfx.pose(rand_rot)
new_pose = (tfx.random_translation_tf(scale=trans_noise,mean=0,dist='normal'))*self.myControlPose*pose
new_pose = rand_rot_pose.matrix*new_pose
return new_pose
def getExpectedTrans(self,top_particles,total_likelihood):
#Normailize likelihoods
exp_trans = tfx.pose([0,0,0])
for particle in top_particles:
weight = particle.likelihood/totatl_likelihood
exp_trans += weight* particle.pose.translation
return exp_trans
def _update(self, depth_msg):
self.numUpdates = self.numUpdates + 1
tot_likelihood = 0
if self.numUpdates % self.skipRate != 0:
return
while(self.lock):
i = 0
# lock to wait for latest control pose information
self.lock = True
self.myControlPose = copy.copy(self.controlPose)
self.controlPose = np.identity(4)
print "Control pose"
print self.myControlPose
self.lock = False
# setup pf variables
norm = 0
ml = 0
i = 0
pointsPerImage = XDIM_DOWN * YDIM_DOWN
z = self.bridge.imgmsg_to_cv(depth_msg, "32FC1")
# profiling
start = 0
stop = 0
start_whole_loop = time.clock()
depth_im_best = []
best_index = -1
for particle in self.particles:
print "PARTICLE:",i
current_pose = self.sample_from_r(particle.pose)
depth_im = self.RSimulator.getExpectedMeasurement(current_pose)
#print current_pose
if False:#self.numUpdates == 4:
plt.figure()
plt.subplot(111)
plt.imshow(depth_im)
plt.show()
start = time.clock()
particle.likelihood = 0
if depth_im is not None:
particle.likelihood = prob_ztg_r1tm1_o1tm1(particle.pixels, current_pose, depth_im, z)
norm += particle.likelihood
print "Likelihood", particle.likelihood
# profile
stop = time.clock()
#print "Depth probability update time (sec)", stop - start
start = time.clock()
if(particle.likelihood >= ml):
pubpose = tfx.pose(copy.copy(current_pose), stamp=rospy.Time.now(), frame='0_link')
best_index = i
print "Likelihood", particle.likelihood
print "Best Pose", pubpose
print "Best Index", best_index
depth_im_best = copy.copy(depth_im)
ml = particle.likelihood
i += 1
particle.pose = copy.copy(current_pose)
if depth_im is not None:
for row in range(0,YDIM_DOWN):
for col in range(0,XDIM_DOWN):
row_f = fullsize_from_downsampled(row)
col_f = fullsize_from_downsampled(col)
depth_exp = depth_im[row_f,col_f]
depth_msr = z[row_f,col_f]
index = row*XDIM_DOWN + col
if depth_exp <= 0 or depth_msr <= 0:
particle.pixels[index] = 0.5
else:
particle.pixels[index] = update_prob_occulusion(particle.pixels[index], current_pose, depth_msr, depth_exp)
# profile
stop = time.clock()
#print "Occlusion probability update time (sec)", stop - start
start = time.clock()
print "NORM", norm
index = 0
bounds = [0]
indices = [-1]
norm_exp = 0
for particle in self.particles:
log_likelihood = particle.likelihood
proportional_likelihood = np.exp((MAGIC_CONSTANT-pointsPerImage)*np.log(PROB_MULTIPLIER) + log_likelihood)
norm_exp += proportional_likelihood
bounds.append(bounds[-1] + proportional_likelihood)
indices.append(indices[-1] + 1)
bounds = bounds[1:-1]
d = zip(indices,bounds)
new_particles = deque()
for i in range(NUM_OF_PARTICLES):
if norm_exp != 0:
r = np.random.uniform(0,norm_exp)
for b in range(len(bounds)):
if bounds[b] >= r:
# print "BOUNDS", b, i
new_particles.append(copy.copy(self.particles[b]))
break
else:
r = np.random.uniform(0,NUM_OF_PARTICLES-1)
new_particles.append(copy.copy(self.particles[int(np.floor(r))]))
self.particles = new_particles
print "Best Index", best_index
# profile
stop = time.clock()
print "Particle resampling time (sec)", stop - start
start = time.clock()
stop_whole_loop = time.clock()
print "Whole loop took (sec)", stop_whole_loop - start_whole_loop
if self.numUpdates <= 2:
plt.figure()
plt.subplot(141)
plt.imshow(self.depth_im_orig)
plt.subplot(142)
plt.imshow(z)
plt.subplot(143)
plt.imshow(depth_im_best)
plt.subplot(144)
plt.imshow(depth_im_best, alpha=0.5)
plt.imshow(z, alpha=0.5)
plt.show()
pose_msg = pubpose.msg.PoseStamped()
pose_msg.header.frame_id = '0_link'
self.tracked_pose_pub.publish(pose_msg)
self.publishedPose = True
