def call_back(self, msg): obj_list = ObjectPoseList() obj_list.header.frame_id = msg.header.frame_id obj_list.size = 0 cluster_num = len(msg.list) #pcl_size = len(msg.poses) for i in range(cluster_num): self.image = np.zeros((int(self.height), int(self.width), 3), np.uint8) img = [] pcl_size = len(msg.list[i].poses) avg_x = avg_y = avg_z = 0 for j in range(pcl_size): # project to XY, YZ, XZ plane avg_x = avg_x + msg.list[i].poses[j].position.x avg_y = avg_y + msg.list[i].poses[j].position.y avg_z = avg_z + msg.list[i].poses[j].position.z img.append([msg.list[i].poses[j].position.x, msg.list[i].poses[j].position.y, msg.list[i].poses[j].position.z]) self.toIMG(pcl_size, img, 'img') model_type = self.classify() # *************************************************************** # Add to object list # *************************************************************** obj = ObjectPose() obj.position.x = float(avg_x/pcl_size) obj.position.y = float(avg_y/pcl_size) obj.position.z = float(avg_z/pcl_size) obj.type = model_type obj_list.list.append(obj) #cv2.imwrite( "Image.jpg", self.image) #self.index = self.index + 1 #print "Save image" obj_list.size = cluster_num self.pub_obj.publish(obj_list) self.drawRviz(obj_list)
def call_back(self, msg): tf_points = PCL_points() tf_points = msg obj_list = ObjectPoseList() obj_list.header.frame_id = tf_points.header.frame_id obj_list.size = 0 cluster_num = len(tf_points.list) #pcl_size = len(msg.poses) for i in range(cluster_num): self.image = np.zeros((int(self.height), int(self.width), 3), np.uint8) plane_xy = [] plane_yz = [] plane_xz = [] pcl_size = len(tf_points.list[i].poses) # ======= Coordinate transform for better project performance ====== position = [0, 0, 0] rad = math.atan2(tf_points.centroids[i].y, tf_points.centroids[i].x) quaternion = tf.transformations.quaternion_from_euler(0., 0., -rad) transformer = tf.TransformerROS() transpose_matrix = transformer.fromTranslationRotation(position, quaternion) for m in range(pcl_size): new_x = tf_points.list[i].poses[m].position.x new_y = tf_points.list[i].poses[m].position.y new_z = tf_points.list[i].poses[m].position.z orig_point = np.array([new_x, new_y, new_z, 1]) new_center = np.dot(transpose_matrix, orig_point) tf_points.list[i].poses[m].position.x = new_center[0] tf_points.list[i].poses[m].position.y = new_center[1] tf_points.list[i].poses[m].position.z = new_center[2] # ======= project to XY, YZ, XZ plane ======= for j in range(pcl_size): plane_xy.append([tf_points.list[i].poses[j].position.x, tf_points.list[i].poses[j].position.y]) plane_yz.append([tf_points.list[i].poses[j].position.y, tf_points.list[i].poses[j].position.z]) plane_xz.append([tf_points.list[i].poses[j].position.x, tf_points.list[i].poses[j].position.z]) self.toIMG(pcl_size, plane_xy, 'xy') self.toIMG(pcl_size, plane_yz, 'yz') self.toIMG(pcl_size, plane_xz, 'xz') model_type = self.classify() # *************************************************************** # Add to object list # *************************************************************** obj = ObjectPose() obj.position.x = msg.centroids[i].x obj.position.y = msg.centroids[i].y obj.position.z = msg.centroids[i].z obj.varianceX = msg.varianceX obj.varianceY = msg.varianceY obj.type = model_type obj_list.list.append(obj) #cv2.imwrite( "Image" + str(self.index) + ".jpg", self.image) #self.index = self.index + 1 #print "Save image" obj_list.size = cluster_num self.pub_obj.publish(obj_list) self.drawRviz(obj_list)
def update_map(self):
for i in range(self.obj_list.size):
index = self.matching[i]
if self.matching[i] != None:
# Kalman filter update position
# ======= Kalman filter for x =======
prior_mean = self.map.list[index].position.x
prior_var = self.map.list[index].varianceX
z_mean = self.obj_list.list[i].position.x
z_var = self.measurement_var
x_mean, x_var = self.kalman_filter_1D(prior_mean, prior_var,
z_mean, z_var)
self.map.list[index].position.x = x_mean
self.map.list[index].varianceX = x_var
# ======= Kalman filter for y =======
prior_mean = self.map.list[index].position.y
prior_var = self.map.list[index].varianceY
z_mean = self.obj_list.list[i].position.y
z_var = self.measurement_var
y_mean, y_var = self.kalman_filter_1D(prior_mean, prior_var,
z_mean, z_var)
self.map.list[index].position.y = y_mean
self.map.list[index].varianceY = y_var
else:
obj = ObjectPose()
obj = self.obj_list.list[i]
obj.varianceX = self.init_varX
obj.varianceY = self.init_varY
self.map.list.append(obj)
for j in range(self.map.size):
if not j in self.matching:
if self.distance_to_robot(
self.map.list[j]) < self.velodyne_range:
print "Haha"
self.map.list[
j].varianceX = self.map.list[j].varianceX * 1.5
self.map.list[
j].varianceY = self.map.list[j].varianceY * 1.5
self.map.size = len(self.map.list)
def update_map(self):
for i in range(self.obj_list.size):
index = self.matching[i]
if self.distance_to_robot(
self.obj_list.list[i]) < self.update_range:
if index != None:
# Kalman filter update position
# ======= Kalman filter for x =======
prior_mean = self.map.list[index].position.x
prior_var = self.map.list[index].varianceX
z_mean = self.obj_list.list[i].position.x
z_var = self.measurement_var
x_mean, x_var = self.kalman_filter_1D(
prior_mean, prior_var, z_mean, z_var)
self.map.list[index].position.x = x_mean
self.map.list[index].varianceX = x_var
# ======= Kalman filter for y =======
prior_mean = self.map.list[index].position.y
prior_var = self.map.list[index].varianceY
z_mean = self.obj_list.list[i].position.y
z_var = self.measurement_var
y_mean, y_var = self.kalman_filter_1D(
prior_mean, prior_var, z_mean, z_var)
self.map.list[index].position.y = y_mean
self.map.list[index].varianceY = y_var
self.map.list[index].pcl_points = self.obj_list.list[
i].pcl_points
self.map.list[index].position_local = self.obj_list.list[
i].position_local
if self.obj_list.list[i].type != 'None':
self.map.list[index].type = self.obj_list.list[i].type
else:
obj = ObjectPose()
obj = self.obj_list.list[i]
#obj.varianceX = self.init_varX
#obj.varianceY = self.init_varY
self.map.list.append(obj)
for j in range(self.map.size):
if not j in self.matching:
if self.distance_to_robot(
self.map.list[j]) < self.punish_range:
self.map.list[j].varianceX = self.map.list[
j].varianceX * self.punish_no_detect
self.map.list[j].varianceY = self.map.list[
j].varianceY * self.punish_no_detect
self.map.size = len(self.map.list)