通过局域网实现安卓手机和ROS的通讯，进一步通过Kalibr工具实现手机IMU和相机的联合标定。
手机与PC通信
基于ROS下的信息发布和订阅，手机和PC在一个局域网下进行信息（image和IMU）传输。操作步骤：
rosrun image_transport republish compressed in:=/camera/image raw out:=/camera/image_raw
安装Kalibr
在github中下载Kalibr安装包编译，操作步骤（Ubuntu 16.04为例）：
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
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
cd ~/kalibr_workspace/src
git clone git://github.com/ethz-asl/kalibr.git
cd ~/kalibr_workspace
catkin build -DCMAKE_BUILD_TYPE=Release -j4
标定相机
通过kalibr工具标定相机，具体操作步骤：
rosrun topic_tools throttle messages <intopic> <msgs_per_sec> [outtopic]# 在这里,使用为
rosrun topic_tools throttle messages /camera/image_raw 4.0 /image_raw
kalibr_create_target_pdf --type apriltag --nx [NUM_COLS] --ny [NUM_ROWS] --tsize [TAG_WIDTH_M] --tspace [TAG_SPACING_PERCENT] #nx和ny对应棋盘格数
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[-]
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的联合标定，操作步骤：
sudo apt-get install libdw-dev
- 全局安装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”，再编译
<param name="max_time_min" type="int" value="120"> #标定的时长
bash<launch></p>
<pre><code> #imu topic的名字
#生成文件保存路径
#标定的时长 单位: min
#
</code></pre>
<pre><code>
</code></pre>
<p>rosbag play -r 200 imu_utils/imu.bag (这里要写你录制的包的路径)
cd imu_ws
source ./devel/setup.bash
roslaunch imu_utils imu.launch</p>
<pre><code>
程序结束生成标定结果对应的YMAL文件，整理为如下格式（imu.ymal）:
</code></pre>
<h1>Accelerometers</h1>
<p>accelerometer_noise_density: 1.86e-03 #Noise density (continuous-time)
accelerometer_random_walk: 4.33e-04 #Bias random walk</p>
<h1>Gyroscopes</h1>
<p>gyroscope_noise_density: 1.87e-04 #Noise density (continuous-time)
gyroscope_random_walk: 2.66e-05 #Bias random walk</p>
<p>rostopic: /imu0 #the IMU ROS topic
update_rate: 200.0 #Hz (for discretization of the values above)</p>
<pre><code>
## 相机IMU联合标定
从安卓手机中直接抓取到的bag包有以下问题存在
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]
</class>
Original: https://blog.csdn.net/qq_39514211/article/details/125721498
Author: 制作人
Title: 联合标定Android手机的IMU和Camera数据
原创文章受到原创版权保护。转载请注明出处：https://www.johngo689.com/704933/
转载文章受原作者版权保护。转载请注明原作者出处！