联合标定Android手机的IMU和Camera数据

联合标定Android手机的IMU和Camera数据

通过局域网实现安卓手机和ROS的通讯，进一步通过Kalibr工具实现手机IMU和相机的联合标定。

手机与PC通信

基于ROS下的信息发布和订阅，手机和PC在一个局域网下进行信息（image和IMU）传输。操作步骤：

1. 在安卓手机中安装github上的2个开源Android_Camera-IMU和android_ros_sensors中的任意一个，基于ros_java生成安卓APP，下载安装任一个APP到安卓手机，界面如图
   ![安卓APP界面](https://img-blog.csdnimg.cn/eb3bfc78069c43e0b69273ad356a2694.jpeg
2. 在Ubuntu中运行roscore启动ROS主节点
3. 将Ubuntu和安卓手机链接到同一局域网下
4. 在Ubuntu终端运行ifconfig查看自己当前的IP地址，修改localhost为自己的IP地址
5. 此时通过rostopic list指令查看Android手机发布主题如下
   
6. rostopic list查看主题，发现图像是compressed（压缩图像），SLAM输入和进行标定的图像必须是raw才行，故进行republish：

rosrun image_transport republish compressed in:=/camera/image raw out:=/camera/image_raw

7. 查看节点，此时的图像和IMU信息可以用于Camera-IMU联合标定

安装Kalibr

在github中下载Kalibr安装包编译，操作步骤（Ubuntu 16.04为例）：

1. 安装依赖项

sudo apt-get install python-setuptools python-rosinstall ipython libeigen3-dev libboost-all-dev doxygen libopencv-dev
sudo apt-get install ros-kinetic-vision-opencv ros-kinetic-image-transport-plugins ros-kinetic-cmake-modules software-properties-common
sudo apt-get install libpoco-dev python-matplotlib python-scipy python-git python-pip ipython libtbb-dev libblas-dev liblapack-dev
sudo apt-get install python-catkin-tools libv4l-dev 
sudo apt-get install python-igraph

2. 创建工作空间

mkdir -p ~/kalibr_workspace/src 
cd ~/kalibr_workspace
source /opt/ros/kinetic /setup.bash
catkin init
catkin config --extend /opt/ros/kinetic 
catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release

3. 下载kalibr源码

cd ~/kalibr_workspace/src
git clone git://github.com/ethz-asl/kalibr.git

4. 编译

cd ~/kalibr_workspace
catkin build -DCMAKE_BUILD_TYPE=Release -j4

标定相机

通过kalibr工具标定相机，具体操作步骤：

1. 录制相机的bag包：尽量保持棋盘格在画面中，可将图片的发布频率降低（4Hz）， 这里使用throttle的命令，会发布新的topic,原topic也会存在，主要用来改变节点的发布频率。命令如下：

rosrun topic_tools throttle messages <intopic> <msgs_per_sec> [outtopic]# 在这里,使用为
rosrun topic_tools throttle messages /camera/image_raw 4.0 /image_raw

2. 制作标定板，官网给出的标定板大小为A0，实验室标定可以通过自制标定板，对应指令为：

kalibr_create_target_pdf --type apriltag --nx [NUM_COLS] --ny [NUM_ROWS] --tsize [TAG_WIDTH_M] --tspace [TAG_SPACING_PERCENT] #nx和ny对应棋盘格数

3. 准备标定板（aprilgrid）对应的yaml配置文件
   

target_type: 'aprilgrid' #gridtype
tagCols: 6               #number of apriltags
tagRows: 6               #number of apriltags
tagSize: 0.088           #size of apriltag, edge to edge [m]
tagSpacing: 0.3          #ratio of space between tags to tagSize
                         #example: tagSize=2m, spacing=0.5m --> tagSpacing=0.25[-]

4. 进行标定

rosrun kalibr kalibr_calibrate_cameras --bag ../cam.bag --topics /image_raw --models pinhole-radtan --target ../april_6x6_80x80cm.yaml

输出的结果可以直接用与IMU-Camera标定

cam0:
  cam_overlaps: []
  camera_model: pinhole
  distortion_coeffs: [0.08710955387338654, -0.24288688826219126, 0.0027985848457975342, 0.003448529815029094]
  distortion_model: radtan
  intrinsics: [1643.7964106758034, 1638.3662248161381, 1243.6682735881284, 541.0150595623118]
  resolution: [2400, 1080]
  rostopic: /camera/image_raw

标定IMU

用imu_utils标定IMU，之后用于kalibr中相机和IMU的联合标定，操作步骤：

1. 安装依赖项

sudo apt-get install libdw-dev

2. 下载imu_utils和code_utils
3. 编译

• 全局安装ceres库，code_code依赖ceres。
• 不要同时把imu_utils和code_utils一起放到工作空间下进行编译。

由于imu_utils 依赖 code_utils，所以先把code_utils放在工作空间的src下面，进行编译。然后再将imu_utils放到src下面，再编译。否则会报错误

• 编译报错

code_utils-master/src/sumpixel_test.cpp:2:24: fatal error: backward.hpp:No such file or directory
在code_utils下面找到sumpixel_test.cpp，修改#include "backward.hpp"为 #include “code_utils/backward.hpp”，再编译

4. 录制imu.bag
   源代码中要求让IMU静止不动两个小时，录制IMU的bag。
   如果向修改时间可以通过修改launch文件中的标定时长，其中单位为分钟

<param name="max_time_min" type="int" value= "120"/>   #标定的时长

4. 进行IMU标定，以200倍速播放录制的bag包
   编写imu.launch

    <node pkg="imu_utils" type="imu_an" name="imu_an" output="screen">
        <param name="imu_topic" type="string" value= "/android/imu"/>    #imu topic的名字
        <param name="imu_name" type="string" value= "android"/>   
        <param name="data_save_path" type="string" value= "$(find imu_utils)/data/"/> #生成文件保存路径
        <param name="max_time_min" type="int" value= "13"/>   #标定的时长 单位: min
        <param name="max_cluster" type="int" value= "100"/>  #
    </node>
</launch>

rosbag play -r 200 imu_utils/imu.bag (这里要写你录制的包的路径)
cd imu_ws
source ./devel/setup.bash
roslaunch imu_utils imu.launch 

程序结束生成标定结果对应的YMAL文件，整理为如下格式（imu.ymal）:

#Accelerometers
accelerometer_noise_density: 1.86e-03   #Noise density (continuous-time)
accelerometer_random_walk:   4.33e-04   #Bias random walk

#Gyroscopes
gyroscope_noise_density:     1.87e-04   #Noise density (continuous-time)
gyroscope_random_walk:       2.66e-05   #Bias random walk

rostopic:                    /imu0      #the IMU ROS topic
update_rate:                 200.0      #Hz (for discretization of the values above)

相机IMU联合标定

从安卓手机中直接抓取到的bag包有以下问题存在

• IMU信息无法被Kalib直接读取，问题可参考网址
  
• IMU和CAMERA的时间戳可能不在一个范围
• IMU和CAMERA的时间戳可能不同步
  解决方案：

python2 fix_bag_msg_def.py -l inputbagpath fixedbagpath
python2 roasbag_timeduiqi.py -l inputbagpath fixedbagpath

#fix_bag_msg_def.py
import argparse
import os
import sys
 
try:
    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 files
    msg_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 fix
    bag = rosbag.Bag(args.inbag)
 
    # filter for all connections that pass the filter expression
    # if no 'callerid' specified, returns all connections
    conxs = 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 that
        msg_type = conx.datatype
        if 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 failing
            sys_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 created
        full_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 it
        conx.header['message_definition'] = full_msg_text
        conx.msg_def = full_msg_text
 
        def_replaced.append((conx.topic, msg_type))
 
 
    # print stats
    if 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 itself
        meter = rosbag.rosbag_main.ProgressMeter(outbag.filename, bag._uncompressed_size)
        total_bytes = 0
        for topic, raw_msg, t in bag.read_messages(raw=True):
            msg_type, serialized_bytes, md5sum, pos, pytype = raw_msg
 
            outbag.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()

# roasbag_timeduiqi.py
import rospy
import rosbag
import sys
 
if 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 = None
    for topic, msg, t in rosbag.Bag(dst_dir bag_name).read_messages():
        if topic == '/Mate/imu':
            imu_flag=True
            t_old = t
            old_stamp=msg.header.stamp
 
        # elif topic == '/cam0/image_raw':
        #     outbag.write(topic, msg, msg.stamp)
        #     continue
        # print msg.header
        print topic, msg.header.stamp, t
        if imu_flag and topic!="/Mate/imu":
 
            msg.header.stamp=old_stamp
            outbag.write(topic, msg, t_old)
            # imu_flag=False
        else:
            outbag.write(topic, msg, t)
 
print("finished")

运行：

rosrun kalibr kalibr_calibrate_imu_camera --bag /home/xx/camimu.bag --target /home/xx/aprilgrid.yaml --cam /home/xx/camchain.yaml --imu /home/xx/imu.yaml

得到IMU-CAM联合标定文件

Calibration results 
====================
Camera-system parameters:
	cam0 (/camera/image_raw):
	 type: <class 'aslam_cv.libaslam_cv_python.DistortedPinholeCameraGeometry'>
	 distortion: [ 0.10035121 -0.25083969  0.00487046 -0.00212582]  - [ 0.00456407  0.0267141   0.00045738  0.00044148]
	 projection: [ 1809.67406077  1812.62023429  1193.23787303   554.22913794]  - [ 7.76623621  8.43173172  5.60478973  2.07664017]
	 reprojection error: [0.000001, 0.000001]  - [0.391375, 0.383458]



Target configuration
====================

  Type: aprilgrid
  Tags: 
    Rows: 6
    Cols: 6
    Size: 0.0212 [m]
    Spacing 0.00636 [m]

这篇好文章是转载于：学新通技术网