def addLoopClosurePose(pose):
global poseId, lineId, markerArray
rgb = [1, 1, 1]
poseMarker = utils.poseMarker(poseId, pose, rgb)
lineMarker = utils.lineMarker(0, poseId)
markerArray.markers.append(poseMarker)
markerArray.markers.append(lineMarker)
poseId = poseId + 1
def addMarkers(poses):
global poseId, lineId, markerArray
rgb = [0, 1, 1]
for pose in poses:
pose.position.x = pose.position.x * scale - origin[0] * scale
pose.position.y = pose.position.y * scale - origin[1] * scale
pose.position.z = pose.position.z * scale - origin[2] * scale
marker = utils.poseMarker(poseId, pose, rgb)
poseId = poseId + 1
markerArray.markers.append(marker)
for x in range(1, poseId):
prev = x - 1
marker = utils.lineMarker(prev, x)
markerArray.markers.append(marker)
def addLoopClosurePose(pose):
global poseId, lineId, markerArray
rgb = [0, 1, 1]
pose.position.x = pose.position.x * scale - origin[0] * scale
pose.position.y = pose.position.y * scale - origin[1] * scale
pose.position.z = pose.position.z * scale - origin[2] * scale
#pose.position.y = - pose.position.y
#pose.position.x = - pose.position.x
#pose.position.z = - pose.position.z
marker = utils.poseMarker(poseId,pose,rgb)
marker = utils.lineMarker(0, poseId)
markerArray.markers.append(marker)
poseId = poseId + 1
def addNodes(lines):
global markerArray
rgb = [1, 0.5, 0]
for line in lines:
words = line.split()
if words[0] == 'Pose3d_Node':
id = int(words[1])
pose = Pose()
pose.position.x = float(words[2]) * scale
pose.position.y = float(words[3]) * scale
pose.position.z = float(words[4]) * scale
yaw = float(words[5])
pitch = float(words[6])
roll = float(words[7])
q = tf.transformations.quaternion_from_euler(
roll, pitch, yaw, 'ryxz')
pose.orientation.x = q[0]
pose.orientation.y = q[1]
pose.orientation.z = q[2]
pose.orientation.w = q[3]
#M = Rot.homogeneous(pose)
#M = Rot.toVisionCordinates(M)
#pose = Rot.poseFromHom(M)
pose.position.x = pose.position.x - origin[0] * scale
pose.position.y = pose.position.y - origin[1] * scale
pose.position.z = pose.position.z - origin[2] * scale
marker = utils.poseMarker(id, pose, rgb)
markerArray.markers.append(marker)
elif words[0] == 'Point3d_Node':
id = int(words[1])
p = Point()
p.x = float(words[2]) * scale
p.y = float(words[3]) * scale
p.z = float(words[4]) * scale
p.x = p.x - origin[0] * scale
p.y = p.y - origin[1] * scale
p.z = p.z - origin[2] * scale
marker = utils.pointMarker(id, p, rgb)
markerArray.markers.append(marker)
def addMarkers(poses):
global poseId, lineId, markerArray
rgb = [0, 0, 1]
for pose in poses:
#pose = utils.fromVisionCord(pose)
#pose.position.y = - pose.position.y
#pose.position.x = - pose.position.x
#pose.position.z = - pose.position.z
marker = utils.poseMarker(poseId, pose, rgb)
poseId = poseId + 1
markerArray.markers.append(marker)
for x in range(1, poseId):
prev = x - 1
marker = utils.lineMarker(prev, x)
markerArray.markers.append(marker)