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 addLines(lines):
global markerArray
for line in lines:
words = line.split()
if words[0] == 'Pose3d_Pose3d_Factor' or words[
0] == 'Pose3d_Point3d_Factor':
id1 = int(words[1])
id2 = int(words[2])
marker = utils.lineMarker(id1, id2)
markerArray.markers.append(marker)
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 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)