def __init__(self, logfile):
f = open(logfile, 'r')
lines = f.readlines()
detector_type = getConfigValue(lines, "detector:")
descriptor_type = getConfigValue(lines, "descriptor:")
self.name = detector_type + " / " + descriptor_type
pose_predicted_data = filterByTaskPoses(lines, 'BASE_LINK_POSE:')
self.predicted_poses = np.reshape(pose_predicted_data[:, 1:],
(len(pose_predicted_data), 3, 4))
self.predicted_ori, self.predicted_pos = mh.decomposeTransformations(
self.predicted_poses)
print("self.pose_predicted_data", len(pose_predicted_data))
pose_adjusted_data = filterByTaskPoses(lines, 'BASE_LINK_KF:')
self.adjusted_poses = np.reshape(pose_adjusted_data[:, 1:],
(len(pose_adjusted_data), 3, 4))
self.adjusted_ori, self.adjusted_pos = mh.decomposeTransformations(
self.adjusted_poses)
# WARNING predicted frame ID's are computed differently in the ROS version.
# The ID is taken from the sequence number of the image message.
# On the other hand, the ID's of the adjusted keyframes are computed
# by an incremented variable inside the sptam class.
# This produces an inconsistency in the numeration of associated poses.
# There could also be an offset relating ground truth and stam frame numbers.
# The frame numbers on which there was a pose prediction.
# Frames that are not in this list were possibly discarded.
self.computed_frames = map(int, pose_predicted_data[:, 0])
# Loops accepted, format: [queryKF, loopKF, nMatches, nInliers] KFs ids are in sptam internal numeration
self.accepted_loops = filterByTaskRaw(lines, 'ACCEPTED_LOOP:')
def __init__(self, poses):
self.name ="method"
self.poses = np.reshape( poses, (len( poses ), 3, 4) )
self.orientation, self.position = mh.decomposeTransformations( self.poses )
print(self.name, len( poses ))
def __init__(self, logfile):
f = open(logfile, 'r')
lines = f.readlines()
detector_type = getConfigValue(lines, "detector:")
descriptor_type = getConfigValue(lines, "descriptor:")
self.name = detector_type + " / " + descriptor_type
pose_predicted_data = filterByTaskPoses(lines, 'BASE_LINK_POSE:')
self.predicted_poses = np.reshape(pose_predicted_data[:, 1:13],
(len(pose_predicted_data), 3, 4))
self.predicted_ori, self.predicted_pos = mh.decomposeTransformations(
self.predicted_poses)
print("self.pose_predicted_data", len(pose_predicted_data))
pose_adjusted_data = filterByTaskPoses(lines, 'BASE_LINK_KF:')
# self.adjusted_poses = np.reshape( pose_adjusted_data[:,1:13], (len(pose_adjusted_data), 3, 4) )
# self.adjusted_ori, self.adjusted_pos = mh.decomposeTransformations( self.adjusted_poses )
# WARNING predicted frame ID's are computed differently in the ROS version.
# The ID is taken from the sequence number of the image message.
# On the other hand, the ID's of the adjusted keyframes are computed
# by an incremented variable inside the sptam class.
# This produces an inconsistency in the numeration of associated poses.
# There could also be an offset relating ground truth and stam frame numbers.
# The frame numbers on which there was a pose prediction.
# Frames that are not in this list were possibly discarded.
self.computed_frames = map(int, pose_predicted_data[:, 0])
# Loops accepted, format: [queryKF, loopKF, nMatches, nInliers] KFs ids are in sptam internal numeration
self.accepted_loops = filterByTask(lines, 'ACCEPTED_LOOP:')
self.time_extract_features = filterByTask(lines, 'extraction:')
#~ self.time_frustum_filter = filterByTask( lines, 'Frustum:' )
self.time_find_matches = filterByTask(lines, 'find_matches:')
#~ self.time_tracking_refine = filterByTask( lines, 'tracking_refinement:' )
#~ self.time_create_points = filterByTask( lines, 'CreatePoints:' )
self.time_add_keyframe = filterByTask(lines, 'AddKeyFrame:')
#~ self.time_lock_add_points = filterByTask( lines, 'LockAddPoints:' )
#~ self.time_lock_frustum = filterByTask( lines, 'LockFrustum:' )
self.tracking_total_times = filterByTask(lines, 'trackingtotal:')
self.visible_points = filterByTask(lines, 'visiblePoints:')
self.extracted_points = filterByTask(lines, 'ExtractedPoints:')
self.created_new_points = filterByTask(lines, 'created_new_points:')
self.matched_features = filterByTask(lines, 'matched_feat_total:')
self.measurements_per_point = filterByTask(lines, 'MeasurementCount:')
def __init__(self, logfile):
f = open(logfile, 'r')
lines = f.readlines()
self.name = getConfigValue( lines, "BundleAdjustmentActiveKeyframes:" )
pose_predicted_data = filterByTaskPoses( lines, 'KITTI_POSE:' )
self.predicted_ori, self.predicted_pos = mh.decomposeTransformations( pose_predicted_data[:,1:] )
print("self.pose_predicted_data", len(pose_predicted_data))
# The frame numbers on which there was a pose prediction.
# Frames that are not in this list were possibly discarded.
self.computed_frames = pose_predicted_data[:,0]
# filter only the ground truth poses related to the frames of the recorded data
timestamps = []
ground_truth_poses = []
for frame_num in experiments[0].computed_frames:
timestamps.append(timestamps_data[frame_num - 1])
ground_truth_poses.append(ground_truth_poses_data[frame_num - 1])
ground_truth_poses = np.reshape(ground_truth_poses,
(len(ground_truth_poses), 3, 4))
####################################################################
# Tranform data to ground-truth frame
####################################################################
print("grnd size", len(ground_truth_poses))
orientation_grnd, pos_grnd = mh.decomposeTransformations(
ground_truth_poses)
if args.align:
print("aligning data to ground truth at the first pose")
for experiment in experiments:
experiment.alignPosesTo(pos_grnd[0], orientation_grnd[0])
# convert data to numpy arrays
pos_grnd = np.array(pos_grnd)
#~ print('First grnd pose:',pos_grnd[0,:])
#~ print('First stam pose', pos_stam[0])
#~ print('Final grnd pose:',pos_grnd[-1,:])
#~ print('Final stam pose:',pos_stam[-1,:])
####################################################################
if ground_truth:
for _, data in logs.iteritems():
data.poses = ground_truth.align(data.frame_ids, data.times,
data.poses)
labels = np.append("ground truth", labels)
colors = np.vstack((ground_truth_color, colors))
##############################################################################
# sepparate position and orientation
##############################################################################
for label, data in logs.iteritems():
ori, pos = mh.decomposeTransformations(data.poses)
data.positions = pos
data.orientations = ori
##############################################################################
# create path plots
##############################################################################
path_xy = []
path_xz = []
path_yz = []
path_3d = []
if ground_truth:
_, gt_positions = mh.decomposeTransformations(ground_truth.poses)
path_xy.append((gt_positions[:, 0], gt_positions[:, 1]))
##############################################################################
# prepare some variables
##############################################################################
##############################################################################
# Load Pose data
##############################################################################
# Each poses object will have the following attributes: frame_ids, times, poses, covariances
estimations = loadPosesWithCovariance(args.logfile, "ESTIMATED_CAMERA_POSE")
refinations = loadPosesWithCovariance(args.logfile, "REFINED_CAMERA_POSE")
assert( len(estimations.frame_ids) == len(refinations.frame_ids) )
estimation_orientation_matrices, estimation_positions = mh.decomposeTransformations( estimations.poses )
refination_orientation_matrices, refination_positions = mh.decomposeTransformations( refinations.poses )
estimation_orientations_euler = np.array([ mh.eulerAnglesfromRotationMatrix( R ) for R in estimation_orientation_matrices ])
refination_orientations_euler = np.array([ mh.eulerAnglesfromRotationMatrix( R ) for R in refination_orientation_matrices ])
####################################################################
# Plot Prediction VS Updates
####################################################################
plotLinesByAxis( [
(estimations.frame_ids, estimation_positions, "estimated"),
(refinations.frame_ids, refination_positions, "refined")
], colors, "position predictions vs. updates" )
#~ plt.savefig('plot3.png')
action='store_true',
help=
'fix data in the camera convention frame, where z points forward and y points down, so that x points forward and z up.'
)
args = parser.parse_args()
####################################################################
# Load data
####################################################################
frames, poses, covariances = data_parser.loadPosesWithCovariance(
args.filename)
frames = frames.astype(int)
Rs, ts = mh.decomposeTransformations(poses)
pos_cov = covariances[:, :3, :3]
ori_cov = covariances[:, 3:, 3:]
####################################################################
# Align data
####################################################################
if args.align:
ts, Rs, pos_cov, ori_cov = applyTransform(frame_fix, ts, Rs, pos_cov,
ori_cov)
####################################################################
# plot covariance ellipses
####################################################################
def __init__(self, tum_logfile, grnd_times, grnd_transfs):
self.name = 'ORBSLAM'
# load orbslam data
data = np.loadtxt(tum_logfile)
poses_data = data[:, 1:] # orientation as quaternions!
time_data = data[:, 0] - data[0][0]
# 4x4 rotation matrix: Transform poses from z_forward to x_forward
T_xforward = tf.transformations.euler_matrix(-np.pi / 2,
0,
-np.pi / 2,
axes='sxyz')
# tf extracted from level7_full dataset
baselink_to_camera = tf.transformations.quaternion_matrix(
[0.540677, -0.540247, 0.455780, -0.456143])
baselink_to_camera[0][3] = 0
baselink_to_camera[1][3] = -0.15
baselink_to_camera[2][3] = 1.15
camera_to_baselink = np.linalg.inv(baselink_to_camera)
xforward_poses = []
for pose in poses_data:
hmg_pose = tf.transformations.quaternion_matrix(
pose[3:]) # 4x4 orientation matrix w/o position
hmg_pose[0][3] = pose[0] # setting x,y,z position
hmg_pose[1][3] = pose[1]
hmg_pose[2][3] = pose[2]
pose_xforward = baselink_to_camera.dot(
hmg_pose) # from his world coordinate system to ours
pose_xforward = pose_xforward.dot(
camera_to_baselink) # from camera to baselink
xforward_poses.append(pose_xforward[0:3, 0:4].flatten()) # 3x4
xforward_poses = np.array(xforward_poses)
self.predicted_poses = np.reshape(xforward_poses,
(len(xforward_poses), 3, 4))
self.predicted_ori, self.predicted_pos = mh.decomposeTransformations(
self.predicted_poses)
print("self.pose_predicted_data", len(xforward_poses))
# TODO: Parse adjusted poses!
#pose_adjusted_data = filterByTaskPoses( lines, 'BASE_LINK_KF:' )
#self.adjusted_poses = np.reshape( pose_adjusted_data[:,1:], (len(pose_adjusted_data), 3, 4) )
#self.adjusted_ori, self.adjusted_pos = mh.decomposeTransformations( self.adjusted_poses )
self.accepted_loops = []
# WARNING ORBSLAM doesnt give to which frame number corresponds the pose predicted
# so be have to find the nearest ground truth information based on timestamps!
# filter only the ground truth poses related to the recorded data
self.computed_frames = []
self.computed_timestamps = []
self.computed_grnd_transformations = []
for i in range(0, len(time_data)):
frame_num = find_nearest(
grnd_times, time_data[i]
) # ORB registers the time when the result was obtained! we might need to substract some time for computation!
self.computed_frames.append(frame_num)
self.computed_timestamps.append(grnd_times[frame_num])
self.computed_grnd_transformations.append(grnd_transfs[frame_num])
self.computed_grnd_poses = []
for grnd_transform in self.computed_grnd_transformations:
R = mh.quaternionToRotationMatrix(grnd_transform[3:])
self.computed_grnd_poses.append([
R[0][0], R[0][1], R[0][2], grnd_transform[0], R[1][0], R[1][1],
R[1][2], grnd_transform[1], R[2][0], R[2][1], R[2][2],
grnd_transform[2]
])
self.computed_grnd_poses = np.reshape(
self.computed_grnd_poses, (len(self.computed_grnd_poses), 3, 4))
f = open(args.filename, 'r')
lines = f.readlines()
sptam_pose_predicted_data = filterByTask(lines, 'KITTI_POSE:')
sptam_pose_adjusted_data = filterByTask(lines, 'KITTI_KF:')
sptam_transformations = sptam_pose_predicted_data[:, 1:]
sptam_kf_transformations = sptam_pose_adjusted_data[:, 1:]
frame_ini = int(sptam_pose_predicted_data[0][0])
frame_end = int(frame_ini + len(sptam_pose_predicted_data))
print("processing results for frames", frame_ini, ":", frame_end)
orientation_sptam, pos_sptam = mh.decomposeTransformations(
sptam_transformations)
orientation_kf_sptam, pos_kf_sptam = mh.decomposeTransformations(
sptam_kf_transformations)
####################################################################
# Draw Orientation plots
####################################################################
# Compute Orientation stuff for all poses
orientations_x_sptam = []
orientations_y_sptam = []
orientations_z_sptam = []
for n in range(0, len(orientation_sptam)):
grnd_transformations_ = np.loadtxt( args.grnds )
# filter only the ground truth poses related to the recorded data
timestamps = []
grnd_transformations = []
for frame_num in experiments[0].computed_frames:
# WARNING There could be an offset relating ground truth and stam frame numbers.
timestamps.append( timestamps_[ frame_num ] )
grnd_transformations.append( grnd_transformations_[ frame_num ] )
####################################################################
# Tranform data to ground-truth frame
####################################################################
print("grnd size", len(grnd_transformations))
orientation_grnd, pos_grnd = mh.decomposeTransformations( grnd_transformations )
if args.align:
print("aligning data to ground truth at the first pose")
for experiment in experiments:
experiment.alignPosesTo( pos_grnd[ 0 ], orientation_grnd[ 0 ] )
# convert data to numpy arrays
pos_grnd = np.array( pos_grnd )
####################################################################
# Plot Errors
####################################################################
positions_stam = []
