def visualize(cls, data):
import siasainterface as si
labels = data['labels']
labels[labels > 40] = 0
label_colors = cls.nyu_colors[labels]
point_attributes = [
si.AttributeArray('Colors', data['colors']),
si.AttributeArray('Labels', label_colors),
]
si.setPolyData(data['points'], data['scene_id'], point_attributes=point_attributes)
def keyframe_callback(self, keyframeMsg_data):
rospy.loginfo("Received keyframe message")
camToWorld = np.asarray(keyframeMsg_data.camToWorld, dtype=np.float32)
T_camToWorld = self.send_frame_to_siasa(camToWorld,
'/cam_keyframe_reinforced',
keyframeMsg_data.id,
self.prop_keyframe)
pointcloud_bytes = keyframeMsg_data.pointcloud
arr_len = keyframeMsg_data.width * keyframeMsg_data.height
pointcloud_array = read_pointcloud_struct(pointcloud_bytes, arr_len)
keyframeMsg_data.pointcloud = b''
pointcloud_siasa = self.convert_to_siasa_pointcloud(
pointcloud_array, T_camToWorld, keyframeMsg_data)
#pointcloud = compute_point_cloud_from_depthmap(depth_abs, K, R1, t1, colors=image)#image1_2)
aa = siasa.AttributeArray('Colors', pointcloud_siasa['colors'])
siasa.setPolyData(pointcloud_siasa['points'],
'/points_keyframe',
0,
point_attributes=[aa],
connection=self.connection)
if self.debug:
print("siasa_viewer: Pointcloud: ", pointcloud_bytes[:36])
rospy.logdebug("siasa_viewer: Pointcloud data input length: %d",
len(pointcloud_bytes))
rospy.logdebug("siasa_viewer: Pointcloud struct length: %d",
arr_len)
rospy.logdebug("siasa_viewer: Pointcloud siasa length: %d",
len(pointcloud_siasa['points']))
print("siasa_viewer: keyframeMsg_data.id***********",
keyframeMsg_data.id)
def keyframe_graph_callback(self, keyframeGraphMsg_data):
rospy.loginfo("Received keyframe graph message")
frame_data_bytes = keyframeGraphMsg_data.frameData
frame_data = read_frameData_struct(frame_data_bytes,
keyframeGraphMsg_data.numFrames)
for frame in frame_data:
kf = self.keyframes[frame.id]
camToWorld_new = np.asarray(frame.camToWorld, dtype=np.float32)
if not np.array_equal(kf.camToWorld, camToWorld_new):
rospy.logerr(
"siasa_viewer: changing graph for keyframe with id: %d",
frame.id)
print("camToWorld_new", camToWorld_new)
print("kf.camToWorld", kf.camToWorld)
R, t = self.send_frame_to_siasa(camToWorld_new,
'/cam_keyframe_reinforced',
frame.id,
self.prop_keyframe,
return_inverse=True)
# Make pointcloud out of the depth image and colour it with the rgb image
pointcloud_siasa = compute_point_cloud_from_depthmap(
kf.depth,
self.sun3d_K,
R,
t,
colors=kf.rgb.transpose([2, 0, 1]))
aa = siasa.AttributeArray('Colors', pointcloud_siasa['colors'])
siasa.setPolyData(pointcloud_siasa['points'],
'points_keyframe/' + str(frame.id),
0,
point_attributes=[aa],
connection=self.connection)
self.keyframes[frame.id] = Keyframe(camToWorld_new, kf.depth,
kf.rgb)
def plot_depth(path, time, depth, K, R, t, normals=None, colors=None):
"""Plots the inverse depth map as point cloud in siasa
path: str
Path for the pointcloud in siasa
time: int
time value for the pointcloud in siasa
depth: numpy.ndarray
2d array with depth values
K: numpy.ndarray
3x3 matrix with internal camera parameters
R: numpy.ndarray
3x3 rotation matrix
t: numpy.ndarray
3d translation vector
normals: numpy.ndarray
optional array with normal vectors
colors: PIL.Image
optional RGB image with the same dimensions as the depth map
"""
import siasainterface as siasa
point_cloud = compute_point_cloud_from_depthmap(depth, K, R, t, normals,
colors)
point_attributes = []
if not normals is None:
point_attributes.append(
siasa.AttributeArray('Normals', point_cloud['normals']))
if not colors is None:
point_attributes.append(
siasa.AttributeArray('Colors', point_cloud['colors']))
siasa.setPolyData(point_cloud['points'],
path,
timestep=time,
point_attributes=point_attributes)
return
def keyframe_siasa_callback(self, keyframeMsg_data):
rospy.loginfo("Received siasa keyframe message")
# Get the Keyframe Depth image from ROS message and convert it to openCV Image format
try:
depth = self._cv_bridge.imgmsg_to_cv2(keyframeMsg_data.depth,
"passthrough")
except CvBridgeError as e:
print(e)
return
# Get the Keyframe RGB image from ROS message and convert it to openCV Image format
try:
rgb = self._cv_bridge.imgmsg_to_cv2(keyframeMsg_data.rgb,
"passthrough")
except CvBridgeError as e:
print(e)
return
# Read camToWorld from message and convert to world to cam and send to siasa
camToWorld = np.asarray(keyframeMsg_data.camToWorld, dtype=np.float32)
self.keyframes[keyframeMsg_data.id] = Keyframe(camToWorld, depth, rgb)
if (self.debug):
print("siasa_viewer: keyframeMsg_data.id*******************",
keyframeMsg_data.id)
R, t = self.send_frame_to_siasa(camToWorld,
'/cam_keyframe_reinforced',
keyframeMsg_data.id,
self.prop_keyframe,
return_inverse=True)
# Make pointcloud out of the depth image and colour it with the rgb image
pointcloud_siasa = compute_point_cloud_from_depthmap(
depth, self.sun3d_K, R, t, colors=rgb.transpose([2, 0, 1]))
aa = siasa.AttributeArray('Colors', pointcloud_siasa['colors'])
siasa.setPolyData(pointcloud_siasa['points'],
'points_keyframe/' + str(keyframeMsg_data.id),
0,
point_attributes=[aa],
connection=self.connection)