class NdRegistration():
def __init__(self):
rospy.init_node('registration')
# Geometry
self.tachyon_ellipse = (0, 0), (0, 0), 0
self.camera_ellipse = (0, 0), (0, 0), 0
# Speed
self.speed = 0
self.vel = (0, 0, 0)
self.bridge = CvBridge()
path = rospkg.RosPack().get_path('mashes_measures')
self.p_camera = Projection()
self.p_camera.load_configuration(
os.path.join(path, 'config', 'camera.yaml'))
self.p_tachyon = Projection()
self.p_tachyon.load_configuration(
os.path.join(path, 'config', 'tachyon.yaml'))
rospy.Subscriber('/tachyon/image', Image,
self.cb_image_tachyon, queue_size=1)
rospy.Subscriber('/camera/image', Image,
self.cb_image_camera, queue_size=1)
rospy.Subscriber('/tachyon/geometry', MsgGeometry,
self.cb_geometry_tachyon, queue_size=1)
rospy.Subscriber('/camera/geometry', MsgGeometry,
self.cb_geometry_camera, queue_size=1)
rospy.Subscriber('/velocity', MsgVelocity,
self.cb_velocity, queue_size=1)
self.pub_image = rospy.Publisher('/measures/image', Image, queue_size=10)
self.frame_camera = None
self.frame_tachyon = None
r = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
try:
self.paint_images()
except:
rospy.logerr("paint images")
r.sleep()
def cb_image_tachyon(self, msg_image):
try:
self.stamp = msg_image.header.stamp
self.frame_tachyon = self.bridge.imgmsg_to_cv2(msg_image)
except CvBridgeError, e:
print e
def __init__(self):
# ------------configurable variables------------#
self.size_sensor = 32
self.init_points = 2
self.num_points = 10
self.num_points_fitted = 5
self.frame_sample = 48
self.laser_threshold = 80
self.rate_gradient = 4
self.thr_no_laser = 40
self.w_ppal = 0.9
self.w_side = 0.015
self.w_diag = 0.01
self.scale_vis = 13
self.pause_plot = 0.05
self.pause_image = 100
self.color = (255, 255, 255)
# ----------------------------------------------#
self.start = False
self.laser_on = False
self.p_NIT = Projection()
# self.p_NIT.load_configuration('../../../mashes_calibration/config/NIT_config.yaml')
self.p_NIT.load_configuration("../../../mashes_calibration/config/tachyon.yaml")
self.inv_hom = linalg.inv(self.p_NIT.hom)
self.max_value = []
self.max_i = []
self.min_value = []
self.min_i = []
self.time = None
self.dt = None
self.ds = None
self.first = True
self.counter = 0
self.frame_0 = np.zeros((self.size_sensor, self.size_sensor, 1), dtype=np.uint8)
self.frame_1 = np.zeros((self.size_sensor, self.size_sensor, 1), dtype=np.uint8)
self.image = np.zeros((self.size_sensor, self.size_sensor, 1), dtype=np.uint8)
self.sizes = list(range(self.init_points + self.num_points, self.init_points, -1))
self.matrix_intensity = [RingBuffer(a) for a in self.sizes]
self.matrix_point = [RingBuffer(a) for a in self.sizes]
self.matrix_pxl_point = [RingBuffer(a) for a in self.sizes]
self.total_t = RingBuffer(self.num_points)
self.dt_axis = []
self.t_axis = []
self.total_intensity = []
self.coeff_1 = []
self.coeff_2 = []
self.coeff_3 = []
self.velocity = Velocity()
def __init__(self):
rospy.init_node('moments')
image_topic = rospy.get_param('~image', '/tachyon/image')
rospy.Subscriber(image_topic, Image, self.cb_image, queue_size=1)
self.bridge = CvBridge()
geo_topic = '/%s/moments' % image_topic.split('/')[1]
self.pub_geo = rospy.Publisher(geo_topic, MsgGeometry, queue_size=10)
self.msg_geo = MsgGeometry()
threshold = rospy.get_param('~threshold', 127)
path = rospkg.RosPack().get_path('mashes_measures')
config = rospy.get_param(
'~config', os.path.join(path, 'config', 'tachyon.yaml'))
self.moments = Moments(threshold)
self.projection = Projection(config)
rospy.spin()
class NdMoments():
def __init__(self):
rospy.init_node('moments')
image_topic = rospy.get_param('~image', '/tachyon/image')
rospy.Subscriber(image_topic, Image, self.cb_image, queue_size=1)
self.bridge = CvBridge()
geo_topic = '/%s/moments' % image_topic.split('/')[1]
self.pub_geo = rospy.Publisher(geo_topic, MsgGeometry, queue_size=10)
self.msg_geo = MsgGeometry()
threshold = rospy.get_param('~threshold', 127)
path = rospkg.RosPack().get_path('mashes_measures')
config = rospy.get_param(
'~config', os.path.join(path, 'config', 'tachyon.yaml'))
self.moments = Moments(threshold)
self.projection = Projection(config)
rospy.spin()
def cb_image(self, msg_image):
try:
stamp = msg_image.header.stamp
frame = self.bridge.imgmsg_to_cv2(msg_image)
if msg_image.encoding == 'rgb8':
frame = cv2.cvtColor(frame, cv2.COLOR_RGB2GRAY)
center, axis, angle = self.moments.find_geometry(frame)
if axis[1] > 0:
center, axis, angle = self.projection.transform_ellipse(center, axis, angle)
self.msg_geo.header.stamp = stamp
self.msg_geo.x = center[0]
self.msg_geo.y = center[1]
self.msg_geo.major_axis = axis[0]
self.msg_geo.minor_axis = axis[1]
self.msg_geo.orientation = angle
self.pub_geo.publish(self.msg_geo)
except CvBridgeError, e:
print e
class CoolRate_adv:
def __init__(self):
# ------------configurable variables------------#
self.size_sensor = 32
self.init_points = 2
self.num_points = 10
self.num_points_fitted = 5
self.frame_sample = 48
self.laser_threshold = 80
self.rate_gradient = 4
self.thr_no_laser = 40
self.w_ppal = 0.9
self.w_side = 0.015
self.w_diag = 0.01
self.scale_vis = 13
self.pause_plot = 0.05
self.pause_image = 100
self.color = (255, 255, 255)
# ----------------------------------------------#
self.start = False
self.laser_on = False
self.p_NIT = Projection()
# self.p_NIT.load_configuration('../../../mashes_calibration/config/NIT_config.yaml')
self.p_NIT.load_configuration("../../../mashes_calibration/config/tachyon.yaml")
self.inv_hom = linalg.inv(self.p_NIT.hom)
self.max_value = []
self.max_i = []
self.min_value = []
self.min_i = []
self.time = None
self.dt = None
self.ds = None
self.first = True
self.counter = 0
self.frame_0 = np.zeros((self.size_sensor, self.size_sensor, 1), dtype=np.uint8)
self.frame_1 = np.zeros((self.size_sensor, self.size_sensor, 1), dtype=np.uint8)
self.image = np.zeros((self.size_sensor, self.size_sensor, 1), dtype=np.uint8)
self.sizes = list(range(self.init_points + self.num_points, self.init_points, -1))
self.matrix_intensity = [RingBuffer(a) for a in self.sizes]
self.matrix_point = [RingBuffer(a) for a in self.sizes]
self.matrix_pxl_point = [RingBuffer(a) for a in self.sizes]
self.total_t = RingBuffer(self.num_points)
self.dt_axis = []
self.t_axis = []
self.total_intensity = []
self.coeff_1 = []
self.coeff_2 = []
self.coeff_3 = []
self.velocity = Velocity()
def load_image(self, dir_frame):
frame = cv2.imread(dir_frame, 1)
frame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
self.image = frame
def load_data(self, dir_file, dir_frame):
t_frames = []
d_frames = []
for f in sorted(dir_frame):
d_frames.append(f)
t_frame = int(os.path.basename(f).split(".")[0])
t_frames.append(t_frame)
with open(dir_file, "rb") as csvfile:
fig_0 = plt.figure()
fig_0.suptitle("Data and Fitted Function")
ax1_0 = fig_0.add_subplot(211)
ax1_0.set_title("Data")
ax1_0.set_xlabel("point")
ax1_0.set_ylabel("value")
ax1_0.set_xlim([0, self.num_points - 1])
ax1_0.set_ylim([0, 256])
ax2_0 = fig_0.add_subplot(212)
ax2_0.set_title("Fitted")
ax2_0.set_xlabel("point")
ax2_0.set_ylabel("value")
ax2_0.set_xlim([0, self.num_points - 1])
ax2_0.set_ylim([0, 256])
reader = csv.reader(csvfile, delimiter=",")
row = next(reader)
i_prev = 0
i_frame = 0
for row in reader:
i_prev = i_frame
t_v = int(row[0])
matrix = [abs(x - t_v) for x in t_frames]
i_frame = matrix.index(min(matrix))
if i_frame > i_prev:
for i in range(i_prev, i_frame):
self.counter = self.counter + 1
if self.counter == self.frame_sample:
self.counter = 0
t_f = t_frames[i]
d_f = d_frames[i]
print "tv-", t_v, "tf-", t_f, "index-", i, "name file-", d_f
self.t = t_f
self.load_image(d_f)
self.velocity.load_velocity(row)
x = y = np.arange(0, self.size_sensor, 1)
X, Y = np.meshgrid(x, y)
index = self.get_maxvalue()
if index is not None:
self.image_gradient(index)
else:
if self.matrix_intensity[0].stored_data:
self.image_gradient(index, False)
if self.matrix_intensity[0].index == self.matrix_intensity[0].length:
intensity = [self.matrix_intensity[point].data[0] for point in range(self.num_points)]
# vvvvviiiiiissssssssssssssssssssssssssssssssss#
img = self.image.copy()
self.total_intensity.append(intensity)
plt.ion()
x = np.arange(self.num_points).tolist()
ax1_0.plot(x, intensity)
plt.pause(self.pause_plot)
x_a = np.array(x)
intensity_f = np.array(intensity[: self.num_points_fitted])
x_f = np.array(np.arange(self.num_points_fitted).tolist())
# Exponential Fit (Note that we have to provide the y-offset ("C") value!!
A, K = self.fit_exp_linear(x_f, intensity_f, self.thr_no_laser)
fit_y = self.model_func(x_a, A, K, self.thr_no_laser)
self.coeff_1.append(A)
self.coeff_2.append(K)
print "Fit y", fit_y
plt.ion()
ax2_0.plot(x, fit_y)
plt.pause(self.pause_plot)
w, h = img.shape
img_rgb = cv2.cvtColor(img, cv2.COLOR_GRAY2RGB)
img_rgb = cv2.applyColorMap(img_rgb, cv2.COLORMAP_JET)
img_plus = cv2.resize(
img_rgb, (w * self.scale_vis, h * self.scale_vis), interpolation=cv2.INTER_LINEAR
)
pxl = [self.matrix_pxl_point[t].data[0] for t in range(self.num_points)]
for p in pxl:
if not np.isnan(p).any():
cv2.circle(
img_plus,
(
int(round(p[0][0]) * self.scale_vis),
int(round(p[0][1]) * self.scale_vis),
),
4,
self.color,
-1,
)
cv2.imshow("Image: ", img_plus)
cv2.waitKey(self.pause_image)
print "Total t", self.total_t.data
self.t_axis.append(float(self.total_t.data[0]) / 1000000)
# vvvvviiiiiissssssssssssssssssssssssssssssssss#
x_t = [self.t_axis[t] - self.t_axis[0] for t in range(len(self.t_axis))]
fig_1 = plt.figure()
fig_1.suptitle("Fitted Function:\n y = A e^(K t) + 0")
ax1 = fig_1.add_subplot(211)
ax1.plot(x_t, self.coeff_1)
ax1.set_title("A")
ax1.set_xlabel("ms")
ax1.set_ylabel("value")
ax1.set_ylim([0, 400])
ax2 = fig_1.add_subplot(212)
ax2.plot(x_t, self.coeff_2)
ax2.set_title("K")
ax2.set_xlabel("ms")
ax2.set_ylabel("value")
ax2.set_ylim([-1, 0])
mean_coeff_1 = np.nanmean(self.coeff_1)
mean_coeff_2 = np.nanmean(self.coeff_2)
print "Media aritmetica:\n A:", mean_coeff_1, "\n K:", mean_coeff_2
median_coeff_1 = np.nanmedian(self.coeff_1)
median_coeff_2 = np.nanmedian(self.coeff_2)
print "Mediana ( valor central datos):\n A:", median_coeff_1, "\n K:", median_coeff_2
var_coeff_1 = np.nanvar(self.coeff_1)
var_coeff_2 = np.nanvar(self.coeff_2)
print "Varianza (dispersion de los datos):\n A:", var_coeff_1, "\n K:", var_coeff_2
std_coeff_1 = np.nanstd(self.coeff_1)
std_coeff_2 = np.nanstd(self.coeff_2)
print "Desv. tipica (raiz cuadrada desv. tipica):\n A:", std_coeff_1, "\n K:", std_coeff_2
mode_coeff_1 = stats.mode(self.coeff_1)
mode_coeff_2 = stats.mode(self.coeff_2)
print "Moda (Valor con mayor frecuencia abs):\n A:", mode_coeff_1, "\n K:", mode_coeff_2
plt.show()
while True:
plt.pause(self.pause_plot)
def fit_exp_linear(self, t, y, C=0):
y = y - C
y = np.log(y)
K, A_log = np.polyfit(t, y, 1)
A = np.exp(A_log)
return A, K
def model_func(self, t, A, K, C):
return A * np.exp(K * t) + C
def get_maxvalue(self, rng=3):
image = np.zeros((self.size_sensor, self.size_sensor), dtype=np.uint16)
pxls = [np.float32([u, v]) for u in range(0, self.size_sensor) for v in range(0, self.size_sensor)]
for pxl in pxls:
index = pxl[0], pxl[1]
intensity = self.get_value_pixel(self.image, pxl)
if intensity == -1:
image[index] = 0
else:
image[index] = intensity
value = np.amax(image)
i = np.unravel_index(np.argmax(image), image.shape)
print value, i
if value > self.laser_threshold:
print "laser on"
return i
else:
return None
def get_value_pixel(self, frame, pxl, rng=3):
intensity = -1
limits = (rng - 1) / 2
p_0 = round(pxl[0])
p_1 = round(pxl[1])
if (
(p_0 - limits) < 0
or (p_0 + limits) > (self.size_sensor - 1)
or (p_1 - limits) < 0
or (p_1 + limits) > (self.size_sensor - 1)
):
return intensity
else:
intensity = 0
if rng == 3:
for i in range(-limits, limits + 1):
for j in range(-limits, limits + 1):
index_i = pxl[0] + i
index_j = pxl[1] + j
if i == 0 and j == 0:
intensity = intensity + self.image[index_i, index_j] * self.w_ppal
elif i == 0 or j == 0:
intensity = intensity + self.image[index_i, index_j] * self.w_side
else:
intensity = intensity + self.image[index_i, index_j] * self.w_diag
else:
for i in range(-limits, limits + 1):
for j in range(-limits, limits + 1):
index_i = p_0 + i
index_j = p_1 + j
intensity = intensity + frame[index_i, index_j]
intensity = intensity / (rng * rng)
return intensity
def image_gradient(self, pxl_pos, data=True):
if self.first:
if data:
self.first = False
# mm/s
vx = self.velocity.vx * 1000
vy = self.velocity.vy * 1000
vz = self.velocity.vz * 1000
vel = np.float32([vx, vy, vz])
self.get_ds(self.t, vel)
self.frame_0 = self.image
pxl_pos_0 = np.float32([[pxl_pos[0], pxl_pos[1]]])
pos_0 = self.p_NIT.transform(self.inv_hom, pxl_pos_0)
intensity_0 = self.get_value_pixel(self.frame_0, pxl_pos_0[0])
self.matrix_intensity[0].append_data(intensity_0)
self.matrix_pxl_point[0].append_data(pxl_pos_0)
self.matrix_point[0].append_data(pos_0)
else:
# mm/s
vx = self.velocity.vx * 1000
vy = self.velocity.vy * 1000
vz = self.velocity.vz * 1000
vel = np.float32([vx, vy, vz])
self.get_ds(self.t, vel)
self.frame_1 = self.frame_0
self.frame_0 = self.image
if data:
pxl_pos_0 = np.float32([[pxl_pos[0], pxl_pos[1]]])
pos_0 = self.p_NIT.transform(self.inv_hom, pxl_pos_0)
intensity_0 = self.get_value_pixel(self.frame_0, pxl_pos_0[0])
else:
pxl_pos_0 = np.nan
pos_0 = np.nan
intensity_0 = np.nan
self.matrix_intensity[0].append_data(intensity_0)
self.matrix_pxl_point[0].append_data(pxl_pos_0)
self.matrix_point[0].append_data(pos_0)
if not self.matrix_intensity[0].complete:
last_index = self.matrix_point[0].index - 2
if last_index < self.num_points - 1:
for x, y in zip(range(last_index, -1, -1), range(last_index + 1)):
self.get_next_data(x, y)
else:
ly_1 = self.matrix_intensity[self.num_points - 2].index - 1
for x, y in zip(range(self.num_points - 2, -1, -1), range(ly_1, last_index + 1)):
self.get_next_data(x, y)
else:
lx = len(self.matrix_intensity) - 2
ly_1 = len(self.matrix_intensity[0].data) - 2
ly_2 = len(self.matrix_intensity[-2].data) - 2
for x, y in zip(range(lx + 1), range(ly_1, ly_2 - 1, -1)):
self.get_next_data(x, y)
def get_next_data(self, x, y):
intensity_0 = self.matrix_intensity[x].data[y]
pos_0 = np.float32(self.matrix_point[x].data[y])
pxl_pos_0 = np.float32(self.matrix_pxl_point[x].data[y])
if not np.isnan(intensity_0):
pos_1, pxl_pos_1, intensity_1 = self.get_next_value(vel, intensity_0, pxl_pos_0, pos_0)
if pos_1 is not None and pxl_pos_1 is not None and intensity_1 is not None:
pxl = np.float32([[pxl_pos_1[0][0], pxl_pos_1[0][1]]])
self.matrix_intensity[x + 1].append_data(intensity_1)
self.matrix_pxl_point[x + 1].append_data(pxl)
self.matrix_point[x + 1].append_data(pos_1)
else:
self.matrix_intensity[x + 1].append_data(np.nan)
self.matrix_pxl_point[x + 1].append_data(np.nan)
self.matrix_point[x + 1].append_data(np.nan)
def get_ds(self, time, vel):
if self.time is not None:
dt = time - self.time
self.dt = dt
self.total_t.append_data(self.time)
self.ds = vel * dt / 1000000000
self.time = time
print "timeeeee", self.dt
def get_gradient(self, rate):
intensity_0 = self.matrix_intensity[0].data[0]
intensity_1 = self.matrix_intensity[rate].data[0]
if intensity_0 == -1 or intensity_1 == -1:
gradient = np.nan
else:
gradient = intensity_1 - intensity_0
def get_next_value(self, vel, intensity_0, pxl_pos_0, pos=np.float32([[0, 0]])):
pos_0 = np.float32([[pos[0][0], pos[0][1], 0]])
pos_1 = self.get_position(pos_0)
if pos_1 is not None and self.dt < 0.2 * 1000000000:
# #frame_0: position_0
pxl_pos_1 = self.p_NIT.transform(self.p_NIT.hom, pos_1)
intensity_1 = self.get_value_pixel(self.frame_1, pxl_pos_1[0])
# if intensity_0 == -1 or intensity_1 == -1:
# gradient = np.nan
# else:
# gradient = (intensity_1 - intensity_0)/self.dt
return pos_1, pxl_pos_1, intensity_1
else:
return None, None, None
def get_position(self, position):
if self.dt is not None:
position_1 = position + self.ds
return position_1
else:
return None
