def compute(self, pts_arr, depth_arr):
self.counter += 1
# C Datum and Inputs
inputs = []
frame_names = dict()
input_names = dict()
headers = dict()
transforms = dict()
for i in range(0, len(self.camera_names)):
height = self.camera_data[self.camera_names[i]]["height"]
width = self.camera_data[self.camera_names[i]]["width"]
depth = depth_arr[i]
if depth.shape[0] != height or depth.shape[1] != width:
raise ("Size mismatch")
ros_depth_image = self.bridge.cv2_to_imgmsg(depth, "32FC1")
input = pylc.Input()
input.camera_name = self.camera_names[i]
frame_names[self.camera_names[i]] = self.camera_names[i]
#input.ros_depth_image = ros_depth_image
input.depth_image = depth
# Convert Points
#pts = func(10)
pts = pts_arr[i]
temp_arr = []
temp_arr.append(pts)
input.design_pts_multi = temp_arr
inputs.append(input)
transforms[self.camera_names[i]] = self.camera_data[
self.camera_names[i]]["cam_to_world"]
input_names[self.camera_names[i]] = i
# Process
# ros_outputs = Outputs()
pylc_output = pylc.Output()
pylc.processPointsJoint(self.datum_processor, inputs, input_names,
self.soi, pylc_output, True, True)
# Images
npimgs = []
for i in range(len(pylc_output.images_multi)):
lcimg = pylc_output.images_multi[i][0]
npimgs.append(lcimg) # Adds 5ms
if self.rosmode:
fullcloud = pylc_output.full_cloud
return fullcloud, npimgs
else:
fullcloud = pylc_output.full_cloud_eig
return fullcloud, npimgs
def get_return(self, np_depth_image, np_design_pts, get_thickness=False):
"""
Args:
np_depth_image: (np.ndarray, dtype=float32, shape=(H, W))
np_design_pts: (np.ndarray, dtype=float32, shape=(N, 2))
Axis 1 corresponds to x, z in LC camera frame.
get_thickness: (bool)
Returns the thickness map as the 2nd output
Returns:
lc_output_image: (np.ndarray, dtype=float32, shape=(H, W, 4)) LC image.
- Channels denote (x, y, z, intensity).
- Pixels that aren't a part of LC return will have NaNs in one of
the 4 channels.
- Intensity ranges from 0. to 255.
thickness_image: (np.ndarray, dtype=float32, shape=(H, W)) LC thickness.
"""
pylc_input = pylc_lib.Input()
pylc_input.camera_name = u'camera01'
pylc_input.depth_image = np_depth_image
dx = np_design_pts[:, [0]]
dy = np.zeros((len(np_design_pts), 1), dtype=np.float32)
dz = np_design_pts[:, [1]]
ones = np.ones((len(np_design_pts), 1), dtype=np.float32)
np_design_pts = np.hstack((dx, dy, dz, ones))
pylc_input.design_pts_multi = [
np_design_pts
] # TODO: change this to pylc_input.design_points
pylc_output = pylc_lib.Output()
pylc_lib.processPointsJoint(self.datum_processor, [pylc_input],
{u'camera01': 0}, [{
u'C': u'camera01',
u'L': u'laser01'
}], pylc_output, False, False)
# LC output.
assert len(pylc_output.images_multi) == 1
assert len(pylc_output.images_multi[0]) == 1
# np.ndarray, dtype=np.float32, shape=(512, 512, 4)
output_image = pylc_output.images_multi[0][0].copy()
thickness_image = pylc_output.thickness_multi[0][0].copy()
if get_thickness:
return output_image, thickness_image
else:
return output_image
def loop(self):
global datum_processor
self.targets = self.click.cv
# Draw all targets
plt.figure(1)
squaresize = 0.3
for i in range(0, self.targets.shape[0]):
X = self.targets[i, 0]
Y = self.targets[i, 1]
rect = patches.Rectangle(
(X - squaresize / 2., Y - squaresize / 2.),
squaresize,
squaresize,
linewidth=1,
edgecolor='r',
facecolor='none')
plt.gca().add_patch(rect)
# Pass Targets into function
paths = [self.targets]
output = pylc.Output()
#output.output_pts_set = [np.zeros((3,1),dtype=np.float32)]
#self.targets = np.flip(self.targets)
start_time = time.time()
fitting_module.curtainNodes(self.targets, "camera01", "laser01",
output, True)
print(time.time() - start_time)
#print("--- %s seconds ---\n" % (time.time() - start_time)*1000)
output_pts_set = output.output_pts_set
spline_set = output.spline_set
for i in range(0, len(output_pts_set)):
output_pts = output_pts_set[i]
spline = spline_set[i]
plt.scatter(spline[:, 0], spline[:, 1], s=1.0, c='r')
plt.scatter(output_pts[0, :], output_pts[2, :], s=1.0, c='b')
# output_pts = output.output_pts
# spline = output.spline
# plt.scatter(spline[:,0], spline[:,1], s=1.0, c='r')
# plt.scatter(output_pts[0, :], output_pts[2, :], s=1.0, c='b')
# output_pts = output_pts.T
# for i in range(0, output_pts.shape[0]):
# print(str(output_pts[i,0]) + " " + str(output_pts[i,2]))
# stop
# print(output_pts)
# stop
# Plot
print("-")
for i in range(self.targets.shape[0]):
plt.text(self.targets[i, 0], self.targets[i, 1], str(i))
plt.minorticks_on()
plt.legend()
plt.xlabel('x')
plt.ylabel('y')
plt.xlim(-7, 7)
plt.ylim(0, 10)
plt.pause(0.001)
plt.cla()
def loop(self):
global datum_processor
if self.OPT == False:
if self.CONTROL == "A":
self.targets = self.click.cv
elif self.CONTROL == "B":
self.targetpts = self.click.cv
# Draw all targets
plt.figure(1)
squaresize = 0.3
for i in range(0, self.targets.shape[0]):
X = self.targets[i, 0]
Y = self.targets[i, 1]
rect = patches.Rectangle(
(X - squaresize / 2., Y - squaresize / 2.),
squaresize,
squaresize,
linewidth=1,
edgecolor='r',
facecolor='none')
plt.gca().add_patch(rect)
for i in range(0, self.targetpts.shape[0]):
X = self.targetpts[i, 0]
Y = self.targetpts[i, 1]
ss = 0.15
rect = patches.Rectangle((X - ss / 2., Y - ss / 2.),
ss,
ss,
linewidth=1,
edgecolor='g',
facecolor='green')
plt.gca().add_patch(rect)
####
# WE NEED TO ELIMATE THOSE TARGET PTS THAT CANNOT BE SEEN BY CAMERA?
# Pass Targets into function
paths = [self.targets]
splineParams = pylc.SplineParamsVec()
start_time = time.time()
output = pylc.Output()
spline = pylc.fitBSpline(self.targets, splineParams, True)
plt.scatter(spline[:, 0], spline[:, 1], s=0.5, c='r')
projPoints = pylc.solveT(splineParams, self.targetpts)
# Test Error
self.computeError(self.targets, self.targetpts, True)
# Draw Outputs
for i in range(0, projPoints.shape[0]):
X = projPoints[i, 0]
Y = projPoints[i, 1]
rect = patches.Rectangle(
(X - squaresize / 2., Y - squaresize / 2.),
squaresize,
squaresize,
linewidth=1,
edgecolor='b',
facecolor='none')
plt.gca().add_patch(rect)
Xp = self.targetpts[i, 0]
Yp = self.targetpts[i, 1]
l = mlines.Line2D([X, Xp], [Y, Yp])
plt.gca().add_line(l)
if self.OPT:
curveParams = self.targets
targetPts = self.targetpts
start_time = time.time()
self.targets = self.update(curveParams, targetPts)
print(time.time() - start_time)
#self.targets = curveParamsNew
#print(curveParamsNew)
# # Plot
# print("-")
# for i in range(self.targets.shape[0]):
# plt.text(self.targets[i,0],self.targets[i,1],str(i))
plt.figure(1)
plt.minorticks_on()
plt.legend()
plt.xlabel('x')
plt.ylabel('y')
plt.xlim(-7, 7)
plt.ylim(0, 10)
plt.pause(0.001)
plt.cla()
plt.figure(2)
plt.cla()