def __init__(self, camera=""):
serv_name = camera + "/return_optic_flow"
rospy.wait_for_service(serv_name)
self.return_optic_flow = rospy.ServiceProxy(serv_name, ReturnImageFloat)
time.sleep(2)
rospy.init_node('liveplotter', anonymous=True)
data = self.return_optic_flow()
image = fmtn.get_numpy_array(data.shape, data.data)
image_row = np.mean(image, axis=0)
self.fig = plt.figure()
self.alphas = np.arange(0, len(image_row), 1)
self.line, = plt.plot(self.alphas, image_row, '.')
self.image_animation = animation.FuncAnimation(self.fig, self.update_line, self.alphas, init_func=self.init_plot, interval=50, blit=True)
plt.xlim(self.alphas[0], self.alphas[-1])
plt.ylim(-1, 1)
plt.xlabel('pixels')
plt.ylabel('pixel values')
plt.title('test')
plt.show()
def __init__(self, camera=""):
serv_name = camera + "/return_optic_flow"
rospy.wait_for_service(serv_name)
self.return_optic_flow = rospy.ServiceProxy(serv_name, ReturnImageFloat)
time.sleep(2)
rospy.init_node('liveplotter', anonymous=True)
# publish ransac
pub_name = camera + "/ransac"
self.ransac_pub = rospy.Publisher(pub_name, Float32)
data = self.return_optic_flow()
image = fmtn.get_numpy_array(data.shape, data.data)
image_row = np.mean(image, axis=0)
self.fig = plt.figure()
self.alphas = np.arange(0, len(image_row), 1)
# data fit
self.linearmodel = data_fit.models.LinearModel()
# lines to animate
self.lucas_kanade, = plt.plot(self.alphas, image_row, '.')
self.line, = plt.plot(self.alphas, self.linearmodel.get_val(self.alphas), 'red')
self.image_animation = animation.FuncAnimation(self.fig, self.update_line, self.alphas, init_func=self.init_plot, interval=20, blit=True)
plt.xlim(self.alphas[0], self.alphas[-1])
plt.ylim(-1, 1)
plt.xlabel('pixels')
plt.ylabel('pixel values')
plt.title('test')
plt.show()
def update_line(self, i):
data = self.return_optic_flow()
image = fmtn.get_numpy_array(data.shape, data.data)
image_row = np.mean(image, axis=0)
self.line.set_data(self.alphas, image_row)
return self.line,
def update_line(self, i):
data = self.return_optic_flow()
image = fmtn.get_numpy_array(data.shape, data.data)
image_row = np.mean(image, axis=0)
indices_for_ransac = np.arange(200,400).tolist()
self.linearmodel.ransac(image_row[indices_for_ransac], inputs=self.alphas[indices_for_ransac], min_data_vals=30, max_iterations=5, threshold=100.0, num_vals_req=30)
self.ransac_pub.publish(self.linearmodel.parameters['slope'])
self.lucas_kanade.set_data(self.alphas, image_row)
self.line.set_data(self.alphas, self.linearmodel.get_val(self.alphas))
return self.lucas_kanade, self.line,
