目录
问题：/usr/bin/env: “python\r”: 没有那个文件或目录
问题：kalibr_calibrate_cameras：未找到命令
问题：Invoking "make -j1 -l1" failed
问题：在可视化界面，有影像但是没有轨迹（废弃，手机导致的问题）
今天也在痛苦面具中
系统：Ubuntu 20.04
ros：noetic
相机：华为手机摄像头
大概看了一下， 大多数用手机摄像头还是跑的离线，比直接调用usb麻烦。
一、数据采集
找了一圈，基本都是通过app来获得所需数据，这里感谢一下github上的大佬。
1、安装采集app
在手机上安装v2.0版本
2、录制
安装完成后打开，第一眼就是imu页面
上面的三个点是设置，可以调分辨率（建议640*480）
下面的摄像图案就是录制页面，点进去以后上面record是录制，录制开始后变成stop结束，第二个框是设置的分辨率，第三个框是录制好的数据文件名
录制启动的时候，原地转几圈，这样初始化的结果会准一点，直接前进的话可能没有轨迹
录制完成后把数据从手机转存到电脑上
问题：找不到录制数据在哪里
我数据存的地方是 Android/data/edu.osu.pcv.marslogger/files/data
或者连接电脑后，直接在文件下面按照上面文件命名的格式搜
如果搜不到，可以尝试开启 开发者模式-USB调试 后寻找
文件内包含内容如下：
movie.mp4是视频流信息
gyro_accel.csv是imu的含有时间戳、加速度计和陀螺仪的数据
frame_timestamp.txt是视频帧时间戳数据
二、数据打包
打包有两种方法，一种是在ros下录制打包，另一种是通过脚本打包，这里选择第二种
1、准备打包文件
1）kalibr_bagcreater.py
打包脚本 源码
#!/usr/bin/env python
from __future__ import print_function
import os
import sys
import argparse
import math
import re
import rosbag
import rospy
from sensor_msgs.msg import Image
from sensor_msgs.msg import Imu
import cv2
import utility_functions
# case 1 create a rosbag from a folder as specified by the kalibr format
# https://github.com/ethz-asl/kalibr/wiki/bag-format
# structure
# dataset/cam0/TIMESTAMP.png
# dataset/camN/TIMESTAMP.png
# dataset/imu.csv
# case 2-4 create a rosbag from a video, a IMU file, and a video time file
# The video time file has the timestamp for every video frame per IMU clock.
# The saved bag will use the IMU clock to timestamp IMU and image messages.
def parse_args():
parser = argparse.ArgumentParser(
description='create a ROS bag containing image and imu topics '
'from either image sequences or a video and inertial data.')
# case 1 arguments used by the kalibr to create a rosbag from a folder
parser.add_argument(
'--folder',
metavar='folder',
nargs='?',
help='Data folder whose content is structured as '
'specified at\nhttps://github.com/ethz-asl/kalibr/wiki/bag-format')
parser.add_argument('--output_bag',
default="output.bag",
help='ROS bag file %(default)s')
# case 2 arguments to create a rosbag from a video, a IMU file,
# and a video time file
parser.add_argument('--video',
metavar='video_file',
nargs='?',
help='Video filename')
parser.add_argument(
'--imu',
metavar='imu_file',
nargs='+',
help='Imu filename. If only one imu file is provided, '
'then except for the optional header, '
'each line format\n'
'time[sec or nanosec], gx[rad/s], gy[rad/s], gz[rad/s],'
' ax[m/s^2], ay[m/s^2], az[m/s^2]. '
'If gyro file and accelerometer file is provided,'
'Each row should be: time[sec or nanosec],x,y,z'
' then accelerometer data will be interpolated'
' for gyro timestamps.')
parser.add_argument('--first_frame_imu_time',
type=float,
default=0.0,
help='The time of the first video frame based on the'
' Imu clock. It is used together with frame rate'
' to estimate frame timestamps if the video time'
' file is not provided. (default: %(default)s)',
required=False)
parser.add_argument('--video_file_time_offset',
type=float,
default=0.0,
help='When the time file for the video is provided, '
'the video_file_time_offset may be added to '
'these timestamps.(default: %(default)s)',
required=False)
parser.add_argument('--video_from_to',
type=float,
nargs=2,
help='Use the video frames starting from up to this'
' time [s] relative to the video beginning.')
parser.add_argument('--video_time_file',
default='',
nargs='?',
help='The csv file containing timestamps of every '
'video frames in IMU clock(default: %(default)s).'
' Except for the header, each row has the '
'timestamp in nanosec as its first component',
required=False)
parser.add_argument(
'--downscalefactor',
type=int,
default=1,
help='A video frame will be downsampled by this factor. If the resultant bag is '
'used for photogrammetry, the original focal_length and '
'principal_point should be half-sized, but the '
'distortion parameters should not be changed. (default: %(default)s)',
required=False)
parser.add_argument(
"--shift_secs",
type=float,
default=0,
help="shift all the measurement timestamps by this amount "
"to avoid ros time starting from 0."
"Only for case 4, see help.")
if len(sys.argv) < 2:
msg = 'Example usage 1: {} --folder kalibr/format/dataset ' \
'--output_bag awsome.bag\n'.format(sys.argv[0])
msg += 'Example usage 1.1: {} --folder tango/android/export/ ' \
'--imu gyro_accel.csv ' \
'--output_bag awsome.bag\n'.format(sys.argv[0])
msg += "dataset or export has at least one subfolder cam0 which " \
"contains nanosecond named images"
msg += 'Example usage 2: {} --video marslogger/ios/dataset/' \
'IMG_2805.MOV --imu marslogger/ios/dataset/gyro_accel.csv ' \
'--video_time_file marslogger/ios/dataset/movie_metadata.csv ' \
'--output_bag marslogger/ios/dataset/IMG_2805.bag\n'. \
format(sys.argv[0])
msg += 'Example usage 3: {} --video marslogger/android/dataset/' \
'IMG_2805.MOV --imu marslogger/android/dataset/gyro_accel.csv' \
' --video_time_file ' \
'marslogger/android/dataset/frame_timestamps.txt ' \
'--output_bag marslogger/android/dataset/IMG_2805.bag\n '. \
format(sys.argv[0])
msg += 'Example usage 4: {} --video advio-01/iphone/frames.MOV --imu' \
' advio-01/iphone/gyro.csv advio-01/iphone/accelerometer.csv' \
' --video_time_file advio-01/iphone/frames.csv ' \
'--shift_secs=100 --output_bag advio-01/iphone/iphone.bag\n '.\
format(sys.argv[0])
msg += ('For case 2 - 4, the first column of video_time_file should be'
' timestamp in sec or nanosec. Also the number of entries in '
'video_time_file excluding its header lines has to be the '
'same as the number of frames in the video.\nOtherwise, '
'exception will be thrown. If the video and IMU data are'
' captured by a smartphone, then the conventional camera '
'frame (C) and IMU frame (S) on a smartphone approximately '
'satisfy R_SC = [0, -1, 0; -1, 0, 0; 0, 0, -1] with '
'p_S = R_SC * p_C')
print(msg)
parser.print_help()
sys.exit(1)
parsed = parser.parse_args()
return parsed
def get_image_files_from_dir(input_dir):
"""Generates a list of files from the directory"""
image_files = list()
timestamps = list()
if os.path.exists(input_dir):
for path, _, files in os.walk(input_dir):
for f in files:
if os.path.splitext(f)[1] in ['.bmp', '.png', '.jpg', '.pnm', '.JPG']:
image_files.append(os.path.join(path, f))
timestamps.append(os.path.splitext(f)[0])
# sort by timestamp
sort_list = sorted(zip(timestamps, image_files))
image_files = [time_file[1] for time_file in sort_list]
return image_files
def get_cam_folders_from_dir(input_dir):
"""Generates a list of all folders that start with cam e.g. cam0"""
cam_folders = list()
if os.path.exists(input_dir):
for rootdir, folders, _ in os.walk(input_dir):
for folder in folders:
if folder[0:3] == "cam":
cam_folders.append(os.path.join(rootdir, folder))
return cam_folders
def get_imu_csv_files(input_dir):
"""Generates a list of all csv files that start with imu"""
imu_files = list()
if os.path.exists(input_dir):
for path, _, files in os.walk(input_dir):
for filename in files:
if filename[0:3] == 'imu' and os.path.splitext(
filename)[1] == ".csv":
imu_files.append(os.path.join(path, filename))
return imu_files
def load_image_to_ros_msg(filename, timestamp=None, downscalefactor=1):
"""
:param filename:
:param timestamp:
:param downscalefactor: should be powers of 2.
:return:
"""
image_np = cv2.imread(filename, cv2.IMREAD_GRAYSCALE)
while downscalefactor > 1:
downscalefactor = downscalefactor // 2
h, w = image_np.shape[:2]
image_np = cv2.pyrDown(image_np, dstsize=(w // 2, h // 2))
if timestamp is None:
timestamp_nsecs = os.path.splitext(os.path.basename(filename))[0]
timestamp = rospy.Time(secs=int(timestamp_nsecs[0:-9]),
nsecs=int(timestamp_nsecs[-9:]))
rosimage = Image()
rosimage.header.stamp = timestamp
rosimage.height = image_np.shape[0]
rosimage.width = image_np.shape[1]
# only with mono8! (step = width * byteperpixel * numChannels)
rosimage.step = rosimage.width
rosimage.encoding = "mono8"
rosimage.data = image_np.tostring()
return rosimage, timestamp
def create_rosbag_for_images_in_dir(data_dir, output_bag, topic = "/cam0/image_raw", downscalefactor=1):
"""
Find all images recursively in data_dir, and put them in a rosbag under topic.
The timestamp for each image is determined by its index.
It is mainly used to create a rosbag for calibrating cameras with kalibr.
:param data_dir:
:param output_bag: Its existing content will be overwritten.
:param topic:
:return:
"""
image_files=get_image_files_from_dir(data_dir)
print('Found #images {} under {}'.format(len(image_files), data_dir))
bag = rosbag.Bag(output_bag, 'w')
for index, image_filename in enumerate(image_files):
image_msg, timestamp = load_image_to_ros_msg(image_filename, rospy.Time(index + 1, 0), downscalefactor)
bag.write(topic, image_msg, timestamp)
print("Saved #images {} in {} to bag {}.".format(len(image_files), data_dir, output_bag))
bag.close()
def create_imu_message_time_string(timestamp_str, omega, alpha):
secs, nsecs = utility_functions.parse_time(timestamp_str, 'ns')
timestamp = rospy.Time(secs, nsecs)
return create_imu_message(timestamp, omega, alpha), timestamp
def create_imu_message(timestamp, omega, alpha):
rosimu = Imu()
rosimu.header.stamp = timestamp
rosimu.angular_velocity.x = float(omega[0])
rosimu.angular_velocity.y = float(omega[1])
rosimu.angular_velocity.z = float(omega[2])
rosimu.linear_acceleration.x = float(alpha[0])
rosimu.linear_acceleration.y = float(alpha[1])
rosimu.linear_acceleration.z = float(alpha[2])
return rosimu
def write_video_to_rosbag(bag,
video_filename,
video_from_to,
first_frame_imu_time=0.0,
frame_timestamps=None,
frame_remote_timestamps=None,
downscalefactor=1,
shift_in_time=0.0,
topic="/cam0/image_raw",
ratio=1.0):
"""
:param bag: opened bag stream writing to
:param video_filename:
:param video_from_to: start and finish seconds within the video. Only frames within this range will be bagged.
:param first_frame_imu_time: The rough timestamp of the first frame per the IMU clock.
This value is used to estimate the video time range per the IMU clock.
Also it is used for frame local time if frame_timestamps is empty.
:param frame_timestamps: Frame timestamps by the host clock.
:param frame_remote_timestamps: Frame timestamps by the remote device clock.
:param downscalefactor: should be powers of 2
:param shift_in_time: The time shift to apply to the resulting local (host) timestamps.
:param ratio: ratio of output frames / input frames
:return: rough video time range in imu clock.
first_frame_imu_time + frame_time_in_video(0 based) ~= frame_time_in_imu.
"""
cap = cv2.VideoCapture(video_filename)
rate = cap.get(cv2.CAP_PROP_FPS)
print("video frame rate {}".format(rate))
start_id = 0
finish_id = 1000000
image_time_range_in_bag = list()
framesinvideo = int(cap.get(cv2.CAP_PROP_FRAME_COUNT))
print('#Frames {} in the video {}'.format(framesinvideo, video_filename))
if frame_timestamps is None:
frame_timestamps = list()
if frame_timestamps:
if len(frame_timestamps) != framesinvideo:
raise Exception((
"Number of frames in the video {} disagrees with the length of"
" the provided timestamps {}").format(framesinvideo,
len(frame_timestamps)))
if finish_id == -1:
finish_id = int(cap.get(cv2.CAP_PROP_FRAME_COUNT) - 1)
else:
finish_id = int(min(finish_id, framesinvideo - 1))
if video_from_to and rate > 1.0:
start_id = int(max(start_id, video_from_to[0] * rate))
finish_id = int(min(finish_id, video_from_to[1] * rate))
image_time_range_in_bag.append(
max(float(start_id) / rate - 0.05, 0.0) + first_frame_imu_time +
shift_in_time)
image_time_range_in_bag.append(
float(finish_id) / rate + 0.05 + first_frame_imu_time + shift_in_time)
print('video frame index start {} finish {}'.format(start_id, finish_id))
cap.set(cv2.CAP_PROP_POS_FRAMES,
start_id) # start from start_id, 0 based index
current_id = start_id
framecount = 0
last_frame_remote_time = 0
downscaletimes = 0
while downscalefactor > 1:
downscalefactor = downscalefactor // 2
downscaletimes += 1
while cap.isOpened():
if current_id > finish_id:
print('Exceeding finish_id %d' % finish_id +
', break the video stream')
break
video_frame_id = int(cap.get(cv2.CAP_PROP_POS_FRAMES))
if video_frame_id != current_id:
message = "Expected frame id {} and actual id in video {} " \
"differ.\n".format(current_id, video_frame_id)
message += "Likely reached end of video file. Note finish_id is " \
"{}".format(finish_id)
print(message)
break
time_frame = cap.get(cv2.CAP_PROP_POS_MSEC) / 1000.0
time_frame_offset = time_frame + first_frame_imu_time
_, frame = cap.read()
if frame is None:
print('Empty frame, break the video stream')
break
image_np = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
for d in range(downscaletimes):
h, w = image_np.shape[:2]
image_np = cv2.pyrDown(image_np, dstsize=(w // 2, h // 2))
h, w = image_np.shape[:2]
if w < h:
image_np = cv2.rotate(image_np, cv2.ROTATE_90_COUNTERCLOCKWISE)
cv2.imshow('frame', image_np)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
if frame_timestamps: # external
local_time = frame_timestamps[video_frame_id]
else:
decimal, integer = math.modf(time_frame_offset)
local_time = rospy.Time(secs=int(integer), nsecs=int(decimal * 1e9))
local_time += rospy.Duration.from_sec(shift_in_time)
remote_time = local_time
is_dud = False
if frame_remote_timestamps:
remote_time = frame_remote_timestamps[video_frame_id]
if last_frame_remote_time != 0:
is_dud = remote_time == last_frame_remote_time
last_frame_remote_time = remote_time
ratiostatus = (current_id - start_id) * ratio >= framecount
if ratiostatus and not is_dud:
rosimage = Image()
rosimage.header.stamp = remote_time
rosimage.height = image_np.shape[0]
rosimage.width = image_np.shape[1]
rosimage.step = rosimage.width
rosimage.encoding = "mono8"
rosimage.data = image_np.tostring()
bag.write(topic, rosimage, local_time)
framecount += 1
# else:
# print('Skip frame of id {}, is dud? {} ratio status? {} start_id {} framecount {}.'.format(
# current_id, is_dud, ratiostatus, start_id, framecount))
current_id += 1
cap.release()
cv2.destroyAllWindows()
print('Saved {} out of {} video frames as image messages to the rosbag'.
format(framecount, framesinvideo))
return image_time_range_in_bag
def loadtimestamps(time_file):
"""Except for the header, each row has the timestamp in nanosec
as its first component"""
frame_rostimes = list()
# https://stackoverflow.com/questions/41847146/multiple-delimiters-in-single-csv-file
with open(time_file, 'r') as stream:
for line in stream:
line = line.strip()
row = re.split(' |,|[|]', line)
if utility_functions.is_header_line(row[0]):
continue
secs, nsecs = utility_functions.parse_time(row[0], 'ns')
frame_rostimes.append(rospy.Time(secs, nsecs))
return frame_rostimes
def load_local_and_remote_times(time_file):
"""
:param time_file: Except for the header lines, each line has the first column as the remote time in ns,
and the last column as the local time in secs.
:return: list of tuples (local time, remote time)
"""
frame_rostimes = list()
with open(time_file, 'r') as stream:
for line in stream:
row = re.split(' |,|[|]', line)
if utility_functions.is_header_line(row[0]):
continue
secs, nsecs = utility_functions.parse_time(row[0], 'ns')
remote_time = rospy.Time(secs, nsecs)
local_time = rospy.Time.from_sec(float(row[-1]))
frame_rostimes.append((local_time, remote_time))
return frame_rostimes
def write_imufile_to_rosbag(bag, imufile, timerange=None, buffertime=5, topic="/imu0"):
"""
:param bag: output bag stream.
:param imufile: each line: time(ns), gx, gy, gz, ax, ay, az
:param timerange: only write imu data within this range to the bag, in seconds.
:param buffertime: time to pad the timerange, in seconds.
:param topic: imu topic
:return:
"""
utility_functions.check_file_exists(imufile)
with open(imufile, 'r') as stream:
rowcount = 0
imucount = 0
for line in stream:
row = re.split(' |,|[|]', line) # note a row is a list of strings.
if utility_functions.is_header_line(row[0]):
continue
imumsg, timestamp = create_imu_message_time_string(
row[0], row[1:4], row[4:7])
timestampinsec = timestamp.to_sec()
rowcount += 1
if timerange and \
(timestampinsec < timerange[0] - buffertime or
timestampinsec > timerange[1] + buffertime):
continue
imucount += 1
bag.write(topic, imumsg, timestamp)
print('Saved {} out of {} inertial messages to the rosbag'.format(
imucount, rowcount))
def write_imufile_remotetime_to_rosbag(bag, imufile, timerange=None, buffertime=5, topic="/imu0"):
"""
:param bag: output bag stream.
:param imufile: each line: time(sec), gx, gy, gz, ax, ay, az, mx, my, mz, device-time(sec), and others
:param timerange: only write imu data within this local time range to the bag, in seconds.
:param buffertime: time to pad the timerange, in seconds.
:param topic: imu topic
:return:
"""
utility_functions.check_file_exists(imufile)
with open(imufile, 'r') as stream:
rowcount = 0
imucount = 0
lastRemoteTime = rospy.Time(0, 0)
for line in stream:
row = re.split(' |,|[|]', line) # note a row is a list of strings.
if utility_functions.is_header_line(row[0]):
continue
local_timestamp = rospy.Time.from_sec(float(row[0]))
remote_timestamp = rospy.Time.from_sec(float(row[10]))
assert remote_timestamp > lastRemoteTime, \
"remote time is not increasing in {}. Is the index of the remote time 10?".format(imufile)
lastRemoteTime = remote_timestamp
imumsg = create_imu_message(remote_timestamp, row[1:4], row[4:7])
timestampinsec = local_timestamp.to_sec()
rowcount += 1
if timerange and \
(timestampinsec < timerange[0] - buffertime or
timestampinsec > timerange[1] + buffertime):
continue
imucount += 1
bag.write(topic, imumsg, local_timestamp)
print('Saved {} out of {} inertial messages to the rosbag'.format(
imucount, rowcount))
def write_gyro_accel_to_rosbag(bag, imufiles, timerange=None, buffertime=5, topic="/imu0", shift_secs=0.0):
gyro_file = imufiles[0]
accel_file = imufiles[1]
for filename in imufiles:
utility_functions.check_file_exists(filename)
time_gyro_array = utility_functions.load_advio_imu_data(gyro_file)
time_accel_array = utility_functions.load_advio_imu_data(accel_file)
time_imu_array = utility_functions.interpolate_imu_data(
time_gyro_array, time_accel_array)
bag_imu_count = 0
for row in time_imu_array:
timestamp = rospy.Time.from_sec(row[0]) + rospy.Duration.from_sec(shift_secs)
imumsg = create_imu_message(timestamp, row[1:4], row[4:7])
timestampinsec = timestamp.to_sec()
# check below conditions when video and imu use different clocks
# and their lengths differ much
if timerange and \
(timestampinsec < timerange[0] - buffertime or
timestampinsec > timerange[1] + buffertime):
continue
bag_imu_count += 1
bag.write(topic, imumsg, timestamp)
print('Saved {} out of {} inertial messages to the rosbag'.format(
bag_imu_count, time_imu_array.shape[0]))
def main():
parsed = parse_args()
bag = rosbag.Bag(parsed.output_bag, 'w')
videotimerange = None # time range of video frames in IMU clock
if parsed.video is not None:
utility_functions.check_file_exists(parsed.video)
print('Given video time offset {}'.format(parsed.first_frame_imu_time))
if parsed.video_from_to:
print('Frame time range within the video: {}'.format(parsed.video_from_to))
first_frame_imu_time = parsed.first_frame_imu_time
frame_timestamps = list()
if parsed.video_time_file:
frame_timestamps = loadtimestamps(parsed.video_time_file)
aligned_timestamps = [time + rospy.Duration.from_sec(parsed.video_file_time_offset)
for time in frame_timestamps]
frame_timestamps = aligned_timestamps
print('Loaded {} timestamps for frames'.format(
len(frame_timestamps)))
first_frame_imu_time = frame_timestamps[0].to_sec()
videotimerange = write_video_to_rosbag(
bag,
parsed.video,
parsed.video_from_to,
first_frame_imu_time,
frame_timestamps,
frame_remote_timestamps=None,
downscalefactor=parsed.downscalefactor,
shift_in_time=parsed.shift_secs,
topic="/cam0/image_raw")
elif parsed.folder is not None:
# write images
camfolders = get_cam_folders_from_dir(parsed.folder)
for camdir in camfolders:
camid = os.path.basename(camdir)
image_files = get_image_files_from_dir(camdir)
for image_filename in image_files:
image_msg, timestamp = load_image_to_ros_msg(image_filename)
bag.write("/{0}/image_raw".format(camid), image_msg,
timestamp)
print("Saved #images {} of {} to bag".format(
len(image_files), camid))
else:
raise Exception('Invalid/Empty video file and image folder')
# write imu data
if (not parsed.imu) and parsed.folder is None:
print("Neither a folder nor any imu file is provided. "
"Rosbag will have only visual data")
elif not parsed.imu:
imufiles = get_imu_csv_files(parsed.folder)
for imufile in imufiles:
topic = os.path.splitext(os.path.basename(imufile))[0]
with open(imufile, 'r') as stream:
for line in stream:
row = re.split(' |,|[|]', line)
if utility_functions.is_header_line(row[0]):
continue
imumsg, timestamp = create_imu_message_time_string(
row[0], row[1:4], row[4:7])
bag.write("/{0}".format(topic), imumsg, timestamp)
elif len(parsed.imu) == 1:
write_imufile_to_rosbag(bag, parsed.imu[0], videotimerange, 5, "/imu0")
else:
write_gyro_accel_to_rosbag(bag, parsed.imu, videotimerange, 5, "/imu0", parsed.shift_secs)
bag.close()
print('Saved to bag file {}'.format(parsed.output_bag))
if __name__ == "__main__":
main()2）第三方库utility_functions.py
补充一个第三方库 源码
import json
import os
import numpy as np
from numpy import genfromtxt
SECOND_TO_MILLIS = 1000
SECOND_TO_MICROS = 1000000
SECOND_TO_NANOS = 1000000000
TIME_UNIT_TO_DECIMALS = {'s': 0, "ms": 3, "us": 6, "ns": 9}
def parse_time(timestamp_str, time_unit):
"""
convert a timestamp string to a rospy time
if a dot is not in the string, the string is taken as an int in time_unit
otherwise, taken as an float in secs
:param timestamp_str:
:return:
"""
secs = 0
nsecs = 0
timestamp_str = timestamp_str.strip()
if '.' in timestamp_str:
if 'e' in timestamp_str:
stamp = float(timestamp_str)
secs = int(stamp)
nsecs = int(round((stamp - secs) * SECOND_TO_NANOS))
else:
index = timestamp_str.find('.')
if index == 0:
nsecs = int(
round(float(timestamp_str[index:]) * SECOND_TO_NANOS))
elif index == len(timestamp_str) - 1:
secs = int(timestamp_str[:index])
else:
secs = int(timestamp_str[:index])
nsecs = int(
round(float(timestamp_str[index:]) * SECOND_TO_NANOS))
return secs, nsecs
else:
decimal_count = TIME_UNIT_TO_DECIMALS[time_unit]
if len(timestamp_str) <= decimal_count:
return 0, int(timestamp_str) * 10**(9 - decimal_count)
else:
if decimal_count == 0:
val = float(timestamp_str)
return int(val), int(
(val - int(val)) * 10**(9 - decimal_count))
else:
return int(timestamp_str[0:-decimal_count]),\
int(timestamp_str[-decimal_count:]) * 10 ** \
(9 - decimal_count)
def is_float(element_str):
"""check if a string represent a float. To this function, 30e5 is float,
but 2131F or 2344f is not float"""
try:
float(element_str)
return True
except ValueError:
return False
def is_header_line(line):
common_header_markers = ['%', '#', '//']
has_found = False
for marker in common_header_markers:
if line.startswith(marker):
has_found = True
break
else:
continue
if not has_found:
if line[0].isdigit():
return False
else:
return True
return has_found
def decide_delimiter(line):
common_delimiters = [',', ' ']
occurrences = []
for delimiter in common_delimiters:
occurrences.append(line.count(delimiter))
max_occurrence = max(occurrences)
max_pos = occurrences.index(max_occurrence)
return common_delimiters[max_pos]
def decide_time_index_and_unit(lines, delimiter):
"""
Time and frame number are at index 0 and 1
Frame number may not exist
At least two lines are required to decide if frame number exists
Following the time or frame number is the tx ty tz and quaternions
Unit is decided as either nanosec or sec
depending on if decimal dot is found.
So the unit can be wrong if timestamps in units ns or ms are provided.
"""
if len(lines) < 2:
raise ValueError("Not enough lines to determine time index")
value_rows = []
for line in lines:
rags = line.rstrip(delimiter).split(delimiter)
value = [float(rag) for rag in rags]
value_rows.append(value)
value_array = np.array(value_rows)
delta_row = value_array[-1, :] - value_array[-2, :]
whole_number = [value.is_integer() for value in delta_row]
if whole_number[0]:
if whole_number[1]:
if delta_row[0] < delta_row[1]: # frame id time[ns] tx[m] ty tz
time_index = 1
time_unit = 'ns'
t_index = 2
else: # time[ns] frame id tx ty tz
time_index = 0
time_unit = 'ns'
t_index = 2
else:
if delta_row[0] > 100: # time[ns] tx ty tz
time_index = 0
time_unit = 'ns'
t_index = 1
else: # frame id time[s] tx ty tz
time_index = 1
time_unit = 's'
t_index = 2
else:
if whole_number[1]:
# time[s] frame id tx ty tz
time_index = 0
time_unit = 's'
t_index = 2
else:
# time[s] tx ty tz
time_index = 0
time_unit = 's'
t_index = 1
return time_index, time_unit, t_index
def normalize_quat_str(val_str_list):
max_len = max([len(x) - x.find('.') - 1 for x in val_str_list])
if max_len > 8:
return val_str_list
q4 = np.array([float(x) for x in val_str_list])
q4_normalized = q4 / np.linalg.norm(q4)
strlist = []
for j in range(4):
strlist.append("{}".format(q4_normalized[j]))
return strlist
def read_pose_from_json(pose_json):
"""
:param pose_json:
:return:
"""
with open(pose_json, 'r') as load_f:
load_dict = json.load(load_f)
x = float(load_dict['translation']['x'])
y = float(load_dict['translation']['y'])
z = float(load_dict['translation']['z'])
q_x = float(load_dict['rotation']['i'])
q_y = float(load_dict['rotation']['j'])
q_z = float(load_dict['rotation']['k'])
q_w = float(load_dict['rotation']['w'])
pose = [x, y, z, q_x, q_y, q_z, q_w]
return pose
def interpolate_imu_data(time_gyro_array, time_accel_array):
"""
interpolate accelerometer data at gyro epochs
:param time_gyro_array: each row [time in sec, gx, gy, gz]
:param time_accel_array: each row [time in sec, ax, ay, az]
:return: time_gyro_accel_array: each row
[time in sec, gx, gy, gz, ax, ay, az]
"""
a = []
for c in range(1, 1 + 3):
a.append(
np.interp(time_gyro_array[:, 0], time_accel_array[:, 0],
time_accel_array[:, c]))
return np.column_stack((time_gyro_array, a[0], a[1], a[2]))
def load_advio_imu_data(file_csv):
"""
:param file_csv: each row [time in sec, x, y, z]
:return: np array nx4
"""
return genfromtxt(file_csv, delimiter=',', skip_header=0)
def check_file_exists(filename):
"""sanity check"""
if not os.path.exists(filename):
raise OSError("{} does not exist".format(filename))2、打包
在目录下新建一个文件夹dataset，然后把需要的东西都放进去（imu文件我重命名了一下）
在此目录下打开终端进行打包
./kalibr_bagcreater.py --video movie.mp4 --imu imu.csv --video_time_file frame_timestamps.txt问题：/usr/bin/env: “python\r”: 没有那个文件或目录
解决办法：
①python换成python3（没用）
打开kalibr_bagcreater.py文件，把最上面的python换成python3，保存关闭
②设置软链接（没用）
sudo ln -s /usr/bin/python3 /usr/bin/python③用vim
# 安装
sudo apt install vim
# 打开文件
vim ./kalibr_bagcreater.py直接输入
:set ff=unix
:wq
然后再输入打包命令，出现录制画面就表示一切ok，最后得到bag包
把录制好的bag包放到工作空间目录（catkin_ws）下
三、参数标定
1、安装kalibr
1）安装依赖项
sudo apt-get install -y \
git wget autoconf automake nano \
libeigen3-dev libboost-all-dev libsuitesparse-dev \
doxygen libopencv-dev \
libpoco-dev libtbb-dev libblas-dev liblapack-dev libv4l-dev不同的Ubuntu需要安装不同的内容，我安装的20.04版本
# Ubuntu 16.04
sudo apt-get install -y python2.7-dev python-pip python-scipy \
python-matplotlib ipython python-wxgtk3.0 python-tk python-igraph
# Ubuntu 18.04
sudo apt-get install -y python3-dev python-pip python-scipy \
python-matplotlib ipython python-wxgtk4.0 python-tk python-igraph
# Ubuntu 20.04
sudo apt-get install -y python3-dev python3-pip python3-scipy \
python3-matplotlib ipython3 python3-wxgtk4.0 python3-tk python3-igraph2）创建工作空间
mkdir -p ~/kalibr_workspace/src
cd ~/kalibr_workspace
source /opt/ros/noetic/setup.bash
catkin init
catkin config --extend /opt/ros/noetic
catkin config --merge-devel
catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release问题：catkin：未找到命令
解决办法：
sudo apt-get update
sudo apt-get install python3-catkin-tools3）下载及编译
cd src
git clone https://github.com/ethz-asl/kalibr.git
cd ..
catkin build -DCMAKE_BUILD_TYPE=Release 2、相机标定
1）前期准备
在 kalibr_workspace 目录下新建 test1 文件，并放入所需文件
①含有标定板的bag文件
用手机录完并打包生成rosbag文件，命名为cam0.bag
②含有标定板信息的yaml文件，官方给了三种格式，我选择棋盘格（checkerboard）
新建 checkerboard.yaml 文件并在其中填入如下内容：
target_type: 'checkerboard' #gridtype
targetCols: 9 #number of internal chessboard corners
targetRows: 6 #number of internal chessboard corners
rowSpacingMeters: 0.025 #size of one chessboard square [m]
colSpacingMeters: 0.025 #size of one chessboard square [m]
保存关闭
2）标定
source ~/kalibr_workspace/devel/setup.bash
kalibr_calibrate_cameras --bag ./cam0.bag --topics /cam0/image_raw --models pinhole-radtan --target checkerboard.yaml问题：kalibr_calibrate_cameras：未找到命令
解决办法：
打开 kalibr_workspace/devel/lib/kalibr ，将 kalibr_calibrate_cameras 复制到 test1 内
然后输入命令
./kalibr_calibrate_cameras --bag ./cam0.bag --topics /cam0/image_raw --models pinhole-radtan --target checkerboard.yaml成功！
成功后会弹出来一份标定报告（calibration report）
最上面五个界面分别是 估计姿态、极差、方位角误差、重投影误差、异常值误差，并且会以pdf形式在文件夹中显示，同时还会有标定结果的文本形式，相机标定的配置文件
ps：重投影误差越聚集，代表标定情况越好
红框内的是标定结果
3、imu标定
标定是用imu_utils实现的，需要一些依赖文件，所以安装顺序是ceres→code_utils→imu_utils，不能颠倒次序也不能同时安装
1）ceres
之前已经安装过ceres了，所以这里就不多赘述了，具体安装步骤可以参考 我的这篇文章 第五部分内容
2）依赖项
sudo apt-get install libdw-dev3）安装code_utils
cd ~/catkin_ws/src
git clone https://github.com/gaowenliang/code_utils在编译前需要对文件做一点修改
打开 ./code_utils/src/sumpixel_test.cpp （因为之前为了调试安装了vim，所以默认代码都是用vim打开的，这里要手动选择其他打开方式找到文本编辑器）
第二行，将引号里的内容改成 code_utils/backward.hpp ，保存关闭
打开 ./code_utils/CMakeLists.txt
第5行，括号里改成 CMAKE_CXX_STANDARD 14 并在第7行加上 include_directories(include/code_utils) ，保存关闭
cd ..
catkin_make问题：Invoking "make -j1 -l1" failed
解决办法：
换一个编译命令
catkin_make -DCATKIN_WHITELIST_PACKAGES=‘code_utils’成功
4）安装imu_utils
cd ~/catkin_ws/src
git clone https://github.com/gaowenliang/imu_utils 修改一下文件内容
打开 ./imu_utils/CMakeLists.txt
第6行，将括号里的内容改成 CMAKE_CXX_STANDARD 14 ，保存关闭
cd ..
catkin_make5）录制imu标定数据
开启Record，将手机静置至少2h（此时效果比较好，不建议太长或太短，具体时间可以自己决定）
然后把imu数据单独打包
打开 ./kalibr_bagcreater.py
第581行，注释掉，在下一行加上 pass ，保存关闭，这样编译的时候就不会因为没有image提示了
保存关闭，输入打包命令
./kalibr_bagcreater.py --imu imu.csv --output_bag imu.bag
并将其copy到 catkin_ws 目录下
6）更改文件
进入 catkin_ws/src/imu_utils/launch 目录下，新建 android_imu.launch 文件，并填入如下内容
<launch>
<node pkg="imu_utils" type="imu_an" name="imu_an" output="screen">
<param name="imu_topic" type="string" value= "/imu0"/>
<param name="imu_name" type="string" value= "HUAWEI"/>
<param name="data_save_path" type="string" value= "$(find imu_utils)/"/>
<param name="max_time_min" type="int" value= "120"/>
<param name="max_cluster" type="int" value= "100"/>
</node></launch>
data_sava_path 代表输出路径，可以根据自己的情况更改
max_time_min 代表的是录制视频的最长时长，如果超过120min会报错，注意改一下
保存关闭
然后编译一下
catkin_make7）标定
进入 catkin_ws 目录下
source ~/catkin_ws/devel/setup.bash
roslaunch imu_utils android_imu.launch
# 新开一个终端
rosbag play -r 200 output.bag -s 10
问题：RROR: cannot launch node of type [imu_utils/imu_an]: Cannot locate node of type [imu_an] in package [imu_utils]. Make sure file exists in package path and permission is set to executable (chmod +x) （解决方法时灵时不灵）
解决办法：
进入 catkin_ws/src/imu_utils/src 右键单击文件 imu_an.app 打开属性，选择权限，发现是只读，勾选 允许文件作为程序执行
然后继续编译
source ~/catkin_ws/devel/setup.bash
roslaunch imu_utils android_imu.launch标定结果太差，直接用参考数值
四、运行vins
1、修改参数文件
1）yaml文件
进入 /catkin_ws/src/Vins-Mono/config，新建一个名为android的文件夹，然后将euroc下的euroc_config.yaml 复制过来改名为 android_config.yaml
打开然后开始修改
①第6，69行，改成自己的路径
②第11，12行，改分辨率，与之前录制视频的分辨率一致
③第19-22行，改相机内参
数值是之前标定过的
④第25行，不知道imu外参，改成2（自动修正）
⑤第59-62行imu内参
⑥第72行，在线估计同步时差，改成1
保存关闭
2）launch文件
进入 /catkin_ws/src/Vins-Mono/vins_estimator/launch 对euroc.launch文件复制粘贴并重命名为android.launch
修改一下路径
保存关闭
2、重新编译
cd ~/catkin_ws
catkin_make3、运行
打开第一个终端：
roscore
打开第二个终端：
source devel/setup.bash
roslaunch vins_estimator android.launch打开第三个终端：
source devel/setup.bash
roslaunch vins_estimator vins_rviz.launch
打开第四个终端：
source devel/setup.bash
rosbag play output.bag运行成果
问题：在可视化界面，有影像但是没有轨迹（废弃，手机导致的问题）
原因：用 rosbag info 命令查一下自己录制的内容（第一个），和官方数据集（第二个）对比一下，发现imu数据没录进去
解决办法：
在数据打包的时候，录两个bag
一个是正常录制的（就是第一次录的那个初始bag），命名为 movie.bag，
再录一个imu的，根据 三.3.6） 改一下文件录制单独的 imu.bag ，运行命令
./kalibr_bagcreater.py --imu imu.csv --output_bag imu.bag
打开一个终端输入：
roscore打开第二个终端输入：
rosbag record -O output.bag /cam0/image_raw /imu0打开第三个终端输入：
rosbag play movie.bag等这个终端播放完毕以后继续输入：
rosbag play imu.bag播放完毕后回到第二个终端结束record
ctrl+shift+C 或 ctrl+C
结束后会发现当前目录下已经出现了 output.bag，这时候再查，就发现有两个topic了
注意两个topic后面的数字是不一样的，imu 要大于 cam 要是差不多那说明你imu数据录错了
试了一下，还是不行，运行时会显示 throw img, only should happen at the beginning
查了一下是初始化的问题，imu第一个数据的时间大于图像第一个数据的时间（图像帧有多余的）
破案了，是我手机没有加速度计，所以读不到完整的imu数据，等有钱了一定要换一个内置imu的手机_(:з」∠)_
五、参考资料
/usr/bin/env: “python3 “: 没有那个文件或目录
解决/usr/bin/env:python:No such file or directory
Ubuntu20.04下vins-mono调用摄像头并跑通（D435i）
【Linux配置五】 Ubuntu18.04+kalibr标定工具箱安装编译
解决ROS中运行launch文件报错ERROR: cannot launch node of type[xxx/xxx]:xxx的问题办法最全汇总
189
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
