Merge branch 'benchmark-dev'

* benchmark-dev:
  use INS extrinsics when building pointcloud using RTK ref
  update matlab sdk to handle projecting pointclouds into images for rtk solutions
  update matlab sdk to handle rtk solutions in building pointclouds
  python sdk can project lasers into images using RTK solutions
  fix rpy euler angle csv extraction for rtk case
  python pointcloud builder can use RTK pose source

matlab/BuildPointcloud.m

@@ -91,9 +91,9 @@
   ins_extrinsics = dlmread([extrinsics_dir 'ins.txt']);
 
   % Find pose for each LIDAR scan, relative to origin
-  if (strfind(ins_file, 'ins.csv'))
+  if (contains(ins_file, 'ins.csv') || contains(ins_file, 'rtk.csv'))
     ins_poses = InterpolatePoses(ins_file, laser_timestamps(:,1)', ...
-      origin_timestamp);
+      origin_timestamp, contains(ins_file, 'rtk.csv'));
     G_ins_laser = SE3MatrixFromComponents(ins_extrinsics) \ ...
       SE3MatrixFromComponents(laser_extrinisics);
   else

matlab/InterpolatePoses.m

@@ -1,4 +1,4 @@
-function [poses] = InterpolatePoses(ins_file, pose_timestamps, origin_timestamp)
+function [poses] = InterpolatePoses(ins_file, pose_timestamps, origin_timestamp, use_rtk)
 
 % InterpolatePoses - interpolate INS poses to find poses at given timestamps
 %
@@ -31,9 +31,19 @@
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
   ins_file_id = fopen(ins_file);
-  headers = textscan(ins_file_id, '%s', 15, 'Delimiter',',');
+  if ~use_rtk
+      header_count = 15;
+  else
+      header_count = 23;
+  end
+  headers = textscan(ins_file_id, '%s', header_count, 'Delimiter',',');
+  if ~use_rtk
+      format_str = '%u64 %s %f %f %f %f %f %f %s %f %f %f %f %f %f';
+  else
+      format_str = '%u64 %f %f %f %f %f %f %s %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f';
+  end
   ins_data = textscan(ins_file_id, ...
-      '%u64 %s %f %f %f %f %f %f %s %f %f %f %f %f %f','Delimiter',',');
+      format_str,'Delimiter',',');
   fclose(ins_file_id);
 
   ins_timestamps = ins_data{1};
@@ -48,12 +58,27 @@
   ins_poses = cell(1, upper_index - lower_index + 1);
   ins_quaternions = cell(1, upper_index - lower_index + 1);
 
-  northings = ins_data{6};
-  eastings = ins_data{7};
-  downs = ins_data{8};
-  rolls = ins_data{13};
-  pitches = ins_data{14};
-  yaws = ins_data{15};
+  if ~use_rtk
+    northing_col = 6;
+    easting_col = 7;
+    down_col = 8;
+    roll_col = 13;
+    pitch_col = 14;
+    yaw_col = 15;
+  else
+    northing_col = 5;
+    easting_col = 6;
+    down_col = 7;
+    roll_col = 12;
+    pitch_col = 13;
+    yaw_col = 14;
+  end
+  northings = ins_data{northing_col};
+  eastings = ins_data{easting_col};
+  downs = ins_data{down_col};
+  rolls = ins_data{roll_col};
+  pitches = ins_data{pitch_col};
+  yaws = ins_data{yaw_col};
 
   pose_timestamps = [origin_timestamp pose_timestamps];
 

matlab/ProjectLaserIntoCamera.m

@@ -91,7 +91,7 @@ function ProjectLaserIntoCamera(image_dir, laser_dir, ins_file, models_dir, extr
   end
   ins_extrinsics = dlmread(extrinsics_path);
 
-  if (strfind(ins_file, 'ins.csv'))
+  if (contains(ins_file, 'ins.csv') || contains(ins_file, 'rtk.csv'))
     G_camera_ins = SE3MatrixFromComponents(camera_extrinsics) * ...
       SE3MatrixFromComponents(ins_extrinsics);
   else

python/build_pointcloud.py

@@ -61,15 +61,15 @@ def build_pointcloud(lidar_dir, poses_file, extrinsics_dir, start_time, end_time
         extrinsics = next(extrinsics_file)
     G_posesource_laser = build_se3_transform([float(x) for x in extrinsics.split(' ')])
 
-    poses_type = re.search('(vo|ins)\.csv', poses_file).group(1)
+    poses_type = re.search('(vo|ins|rtk)\.csv', poses_file).group(1)
 
-    if poses_type == 'ins':
+    if poses_type in ['ins', 'rtk']:
         with open(os.path.join(extrinsics_dir, 'ins.txt')) as extrinsics_file:
             extrinsics = next(extrinsics_file)
             G_posesource_laser = np.linalg.solve(build_se3_transform([float(x) for x in extrinsics.split(' ')]),
                                                  G_posesource_laser)
 
-        poses = interpolate_ins_poses(poses_file, timestamps, origin_time)
+        poses = interpolate_ins_poses(poses_file, timestamps, origin_time, use_rtk=(poses_type == 'rtk'))
     else:
         # sensor is VO, which is located at the main vehicle frame
         poses = interpolate_vo_poses(poses_file, timestamps, origin_time)

python/interpolate_poses.py

@@ -60,7 +60,7 @@ def interpolate_vo_poses(vo_path, pose_timestamps, origin_timestamp):
     return interpolate_poses(vo_timestamps, abs_poses, pose_timestamps, origin_timestamp)
 
 
-def interpolate_ins_poses(ins_path, pose_timestamps, origin_timestamp):
+def interpolate_ins_poses(ins_path, pose_timestamps, origin_timestamp, use_rtk=False):
     """Interpolate poses from INS.
 
     Args:
@@ -85,7 +85,9 @@ def interpolate_ins_poses(ins_path, pose_timestamps, origin_timestamp):
             timestamp = int(row[0])
             ins_timestamps.append(timestamp)
 
-            xyzrpy = [float(v) for v in row[5:8]] + [float(v) for v in row[-3:]]
+            utm = row[5:8] if not use_rtk else row[4:7]
+            rpy = row[-3:] if not use_rtk else row[11:14]
+            xyzrpy = [float(v) for v in utm] + [float(v) for v in rpy]
             abs_pose = build_se3_transform(xyzrpy)
             abs_poses.append(abs_pose)
 

python/project_laser_into_camera.py

@@ -42,8 +42,8 @@
 G_camera_vehicle = build_se3_transform(extrinsics)
 G_camera_posesource = None
 
-poses_type = re.search('(vo|ins)\.csv', args.poses_file).group(1)
-if poses_type == 'ins':
+poses_type = re.search('(vo|ins|rtk)\.csv', args.poses_file).group(1)
+if poses_type in ['ins', 'rtk']:
     with open(os.path.join(args.extrinsics_dir, 'ins.txt')) as extrinsics_file:
         extrinsics = next(extrinsics_file)
         G_camera_posesource = G_camera_vehicle * build_se3_transform([float(x) for x in extrinsics.split(' ')])