本文主要是介绍2021-09-29 使用安卓手机Camera和IMU信息运行ORBSLAM3,希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!
目的
安卓手机具备camera imu gps等SLAM技术所需要的传感器,且安卓手机很普及,如果能使用安卓设备作为ros的sensor,通过安卓设备节点传输到计算机,进行实时定位与建图分析,那么这项技术将变得很易用。
以下介绍并不是事无巨细的介绍,要求阅读者有强大的动手能力,资料查阅能力,对linux系统,opencv,slam技术,ROS,android studio应用开发,python,C++都有一定程度的了解。
是我个人在实现过程中解决问题途径的记录,很多东西是我在网上没有找到的,希望能给大家带来帮助。
安装ros系统
ubuntu18系统下应该安装melodic
sudo sh -c '. /etc/lsb-release && echo "deb http://mirrors.ustc.edu.cn/ros/ubuntu/ $DISTRIB_CODENAME main" > /etc/apt/sources.list.d/ros-latest.list'sudo apt updatesudo apt-key adv --keyserver keyserver.ubuntu.com --recv-keys F42ED6FBAB17C654sudo apt-get install ros-melodic-desktop-fullsudo apt install python-rosdep2
ros系统常见用法介绍
启动ros内核
roscore
查看topic列表
rostopic list
显示某个topic
rostopic echo /camera_raw
显示某个topic的header
rostopic echo /camera_raw/header
将compressed格式转为raw格式
rosrun image_transport republish compressed in:=/Mate/camera/ raw out:=/Mate/camera_raw/
限制topic频率
rosrun topic_tools throttle messages /Mate/camera_raw 4.0 /left
录制rosbag包
rosbag record -O bagpath.bag /camera_raw /imu
播放与加速播放rosbag
rosbag play -r 100 bagpath.bagrosbag play bagpath.bag
安装rviz
----------------------
启动rviz
rviz
安卓手机充当ros sensor
安卓ROS SENSOR开源库链接
----------------------
这里注意修改相机分辨率到合适的大小,本文选择640x480
需要使用android studio同时挂好梯子,将android studio中的代理设置好
Kalibr与imu-tools对sensor进行imu和相机的联合
Kalib的个人发现难以直接使用
需要git clone进行编译
参考链接如下
编译过程中坑很多,如果遇到如找不到lblas等报错,可以考虑apt remove相关库后重新安装
过程中需要apt安装大量库
需要用到catkin(ros专用编译系统)
需要建立catkin空间,并在空间目录下新建src文件夹用于存放源码,切换到空间目录下可以执行指令进行编译,编译完成后注意添加系统变量。
生成标定版的python程序
# 如果在安装pip2包时出现了报错,可以尝试在前面加上PIP_NO_CACHE_DIR=off
PIP_NO_CACHE_DIR=off pip2 install pyX==0.12
python gen_qipan.py out.pdf --type apriltag --nx 3 --ny 4 --tsize 0.04 --tspace 0.2
我重写了一版本Python3的
----------------------
imu-tools编译与使用
也是使用catkin进行编译
中间坑挺多的
imu-tools对IMU进行标定
录制一个将SENSOR静止放置一段时间,推荐2小时的IMU数据bag
随后标定得到重力与加速度的噪声,随机走动等参数
相机标定
这里可以录制图片bag包,使用Kalibr中的程序进行标定,输出相机标定结果文件
为了提高标定效果,这里推荐如下进行操作
并使用Kalib中的提取程序提取得到图片
并使用matlab进行标定,得到的数据如何转化为Kalib和SLAM需要的格式可以参考我的另一篇文章
得到相机标定数据后,替换到相机标定结果文件中
相机与IMU联合标定
从安卓手机中直接抓取到的bag包有以下问题存在
- IMU信息无法被Kalib直接读取
- IMU和CAMERA的时间戳可能不在一个范围
- IMU和CAMERA的时间戳可能不同步
这里分别有两个脚本解决以上问题
对于IMU信息无法直接读取的,使用以下脚本,在python2下运行
python2 fix_bag_msg_def.py -l inputbagpath fixedbagpath
#fix_bag_msg_def.py
import argparse
import os
import systry:import roslib.message
except:sys.stderr.write("Could not import 'roslib', make sure it is installed, ""and make sure you have sourced the ROS environment setup file if ""necessary.\n\n")sys.exit(1)try:import rosbag
except:sys.stderr.write("Could not import 'rosbag', make sure it is installed, ""and make sure you have sourced the ROS environment setup file if ""necessary.\n\n")sys.exit(1)def main():parser = argparse.ArgumentParser()parser.add_argument('-v', '--verbose', action='store_true', help='Be verbose')parser.add_argument('-l', '--use-local-defs', dest='use_local', action='store_true', help='Use message defs from local system (as opposed to reading them from the provided mappings)')parser.add_argument('-c', '--callerid', type=str, help='Callerid (ie: publisher)')parser.add_argument('-m', '--map', dest='mappings', type=str, nargs=1, action='append', help='Mapping topic type -> good msg def (multiple allowed)', default=[])parser.add_argument('inbag', help='Input bagfile')parser.add_argument('outbag', help='Output bagfile')args = parser.parse_args()if not os.path.isfile(args.inbag):sys.stderr.write('Cannot locate input bag file [%s]\n' % args.inbag)sys.exit(os.EX_USAGE)if os.path.realpath(args.inbag) == os.path.realpath(args.outbag):sys.stderr.write('Cannot use same file as input and output [%s]\n' % args.inbag)sys.exit(os.EX_USAGE)if len(args.mappings) > 0 and args.use_local:sys.stderr.write("Cannot use both mappings and local defs.\n")sys.exit(os.EX_USAGE)# TODO: make this nicer. Figure out the complete msg text without relying on external filesmsg_def_maps = {}if len(args.mappings) > 0:print ("Mappings provided:")for mapping in args.mappings:map_msg, map_file = mapping[0].split(':')print (" {:40s}: {}".format(map_msg, map_file))# 'geometry_msgs/PoseStamped:geometry_msgs_pose_stamped_good.txt'with open(map_file, 'r') as f:new_def = f.read()# skip first line, it contains something like '[geometry_msgs/PoseStamped]:'msg_def_maps[map_msg] = new_def.split('\n', 1)[1]#print (msg_def_maps[map_msg])else:if not args.use_local:print ("No mappings provided and not allowed to use local msg defs. ""That is ok, but this won't fix anything like this.")print ("")# open bag to fixbag = rosbag.Bag(args.inbag)# filter for all connections that pass the filter expression# if no 'callerid' specified, returns all connectionsconxs = bag._get_connections(connection_filter=lambda topic, datatype, md5sum, msg_def, header:header['callerid'] == args.callerid if args.callerid else True)conxs = list(conxs)if not conxs:print ("No topics found for callerid '{}'. Make sure it is correct.\n".format(args.callerid))sys.exit(1)def_replaced = []def_not_found = []def_not_replaced = []# loop over connections, find out which msg type they use and replace# msg defs if needed. Note: this is a rather primitive way to approach# this and absolutely not guaranteed to work.# It does work for me though ..for conx in conxs:# see if we have a mapping for thatmsg_type = conx.datatypeif not msg_type in msg_def_maps:if not args.use_local:def_not_found.append((conx.topic, msg_type))continue# don't have mapping, but are allowed to use local msg def: retrieve# TODO: properly deal with get_message_class failingsys_class = roslib.message.get_message_class(msg_type)if sys_class is None:raise ValueError("Message class '" + msg_type + "' not found.")msg_def_maps[conx.datatype] = sys_class._full_text# here, we either already had a mapping or one was just createdfull_msg_text = msg_def_maps[msg_type]# don't touch anything if not needed (note: primitive check)if conx.header['message_definition'] == full_msg_text:def_not_replaced.append((conx.topic, msg_type))continue# here we really should replace the msg def, so do itconx.header['message_definition'] = full_msg_textconx.msg_def = full_msg_textdef_replaced.append((conx.topic, msg_type))# print statsif def_replaced and args.verbose:print ("Replaced {} message definition(s):".format(len(def_replaced)))for topic, mdef in def_replaced:print (" {:40s} : {}".format(mdef, topic))print ("")if def_not_replaced and args.verbose:print ("Skipped {} message definition(s) (already ok):".format(len(def_not_replaced)))for topic, mdef in def_not_replaced:print (" {:40s} : {}".format(mdef, topic))print ("")if def_not_found and args.verbose:print ("Could not find {} message definition(s):".format(len(def_not_found)))for topic, mdef in def_not_found:print (" {:40s} : {}".format(mdef, topic))print ("")print ("Writing out fixed bag ..")# write result to new bag# TODO: can this be done more efficiently? We only changed the connection infos.with rosbag.Bag(args.outbag, 'w') as outbag:# shamelessly copied from Rosbag itselfmeter = rosbag.rosbag_main.ProgressMeter(outbag.filename, bag._uncompressed_size)total_bytes = 0for topic, raw_msg, t in bag.read_messages(raw=True):msg_type, serialized_bytes, md5sum, pos, pytype = raw_msgoutbag.write(topic, raw_msg, t, raw=True)total_bytes += len(serialized_bytes)meter.step(total_bytes)meter.finish()print ("\ndone")print ("\nThe new bag probably needs to be re-indexed. Use 'rosbag reindex {}' for that.\n".format(outbag.filename))if __name__ == '__main__':main()
对于2,3两个问题,需要使用以下脚本来解决
# roasbag_timeduiqi.py
import rospy
import rosbag
import sysif sys.getdefaultencoding() != 'utf-8':reload(sys)sys.setdefaultencoding('utf-8')
bag_name = 'camfix.bag'
out_bag_name = '/home/leo/camduiqi.bag'
dst_dir = '/home/leo/'with rosbag.Bag(out_bag_name, 'w') as outbag:stamp = Nonefor topic, msg, t in rosbag.Bag(dst_dir+bag_name).read_messages():if topic == '/Mate/imu':imu_flag=Truet_old = told_stamp=msg.header.stamp# elif topic == '/cam0/image_raw':# outbag.write(topic, msg, msg.stamp)# continue# print msg.headerprint topic, msg.header.stamp, tif imu_flag and topic!="/Mate/imu":msg.header.stamp=old_stampoutbag.write(topic, msg, t_old)# imu_flag=Falseelse:outbag.write(topic, msg, t)print("finished")
这里简单介绍下原理
我的手机华为Mate30,IMU的频率为100,相机频率30
需要将相机的时间戳对齐到IMU中,这里将遍历ROSBAG中的所有信息
对于CAMERA信息,我们将其时间戳跟改为上一个IMU的时间戳,这样实现了CAMERA和IMU的时间戳对齐
ORBSLAM3的编译
这里需要编译安装opencv,等一系列库,同时也需要更改一些代码
在ROS相关的源码中,需要将topic根据实际情况进行改变
同时,可以用手机录制一个具有camera和IMU的bag,并用单目和单目+IMU进行测试
注意,这个录制的bag,也需要经过两个脚本以处理时间戳问题
参考链接
(29条消息) Kalibr 制作采集的图片和IMU数据生成 .bag 文件 (坑最全,解决方法最简单,最详细哈哈)_努力努力努力-CSDN博客https://blog.csdn.net/hltt3838/article/details/116064390
这篇关于2021-09-29 使用安卓手机Camera和IMU信息运行ORBSLAM3的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!