SLAM_kitti数据集求相机cam2到IMU的变换矩阵
2024-06-01 19:08:41
功能说明:
功能: 求cam2相机坐标系(3D)到IMU坐标系(3D)的变换Tbc (输出, 即T_body_cam2)
输入:
- T_velo_imu: IMU到激光雷达的变换矩阵
- T_cam_velo: 激光雷达到cam0的变换矩阵
- R_i_rect: 从cam0到cami的旋转矩阵
- P_i_rect: 由cami的内参, 和 cami相对于cam0的位移组成的投影矩阵(3D->2D)
输出：
- T_imu_cam2_new: cam2相机坐标系(3D)到IMU坐标系(3D)的变换
- T_imu_cam2: cam2图像坐标系(2D)到IMU坐标系(3D)的变换
公式来源: 论文 Vision meets robotics: the KITTI dataset
- y为图像i中的像素点2D坐标，x为IMU坐标系下的3D坐标
T_cam2_imu = P_2_rect_real * R_0_rect * T_cam_velo * T_velo_imu
- T_cam2_imu_new = T_cam2_cam0 * T_cam_velo * T_velo_imu
实现:
# based on the paper :Vision meets robotics: the KITTI dataset (2013)
# flies : 2011_10_03/calib_cam_to_cam.txt
# 2011_10_03/calib_imu_to_velo.txt
# 2011_10_03/calib_velo_to_cam.txt
# formula (8) :
# 将GPS/IMU坐标系下的点(3D), 变换到第i个图像平面(2D)公式
# y = P_i_rect * R_0_rect * T_cam_velo * T_velo_imu *x
# ep. T_velo_imu means the transformation from imu to velo
# T_velo_imu: 4x4
# R: 9.999976e-01 7.553071e-04 -2.035826e-03 -7.854027e-04 9.998898e-01 -1.482298e-02 2.024406e-03 1.482454e-02 9.998881e-01
# T: -8.086759e-01 3.195559e-01 -7.997231e-01
# T_cam_velo: 4x4
# R: 7.967514e-03 -9.999679e-01 -8.462264e-04 -2.771053e-03 8.241710e-04 -9.999958e-01 9.999644e-01 7.969825e-03 -2.764397e-03
# T: -1.377769e-02 -5.542117e-02 -2.918589e-01
# R_0_rect: 3x3 -- 最后一行和列加 0,0,0,1 --> 4x4
# R_rect_00: 9.999454e-01 7.259129e-03 -7.519551e-03 -7.292213e-03 9.999638e-01 -4.381729e-03 7.487471e-03 4.436324e-03 9.999621e-01
# P_2_rect: 3x4
# P_rect_02: 7.188560e+02 0.000000e+00 6.071928e+02 4.538225e+01 0.000000e+00 7.188560e+02 1.852157e+02 -1.130887e-01 0.000000e+00 0.000000e+00 1.000000e+00 3.779761e-03
# 令 P_rect_02 = (fx, 0,cx,delta1,
# 0, fy,cy, delta2,
# 0, 0, 1, delta3)
# 求解过程: 分解出相机2对相机0的位移:
# y = P_2_rect * x' <=等价于=> y = [K,0]* P_2_rect_real * x' <==============
# 这里[K,0]为3x4相机内参矩阵；x' = R_0_rect * T_cam_velo * T_velo_imu *x
# 则 P_2_rect_real = ( 1, 0, 0, (delta1-delta3*cx)/fx,
# 0, 1, 0, (delta2-delta3*cy)/fy,
# 0, 0, 1, delta3,
# 0,0,0, 1)
# so
# T_imu_cam2 = (P_2_rect_real * R_0_rect * T_cam_velo * T_velo_imu)^(-1)import numpy as np
#import cv2
import torch# calculate the transformation from Cam2 cooridinate system(3d) to IMU cooridinate system(3d) based on kitti dataset.
def getTbc():# the parameter of cam2fx = 7.188560e+02fy = 7.188560e+02cx = 6.071928e+02cy = 1.852157e+02delta1 = 4.538225e+01delta2 = -1.130887e-01delta3 = 3.779761e-03P_2_rect_real = np.mat(np.array([ 1, 0, 0, (delta1-delta3*cx)/fx,0, 1, 0, (delta2-delta3*cy)/fy,0, 0, 1, delta3,0,0,0, 1]).reshape([4,4]))R_0_rect = np.mat(np.array([ 9.999454e-01, 7.259129e-03, -7.519551e-03, 0,-7.292213e-03, 9.999638e-01, -4.381729e-03,0,7.487471e-03, 4.436324e-03, 9.999621e-01,0,0,0,0, 1]).reshape([4,4]))# [R,t]T_cam_velo= np.mat(np.array([ 7.967514e-03, -9.999679e-01, -8.462264e-04,-1.377769e-02, -2.771053e-03, 8.241710e-04, -9.999958e-01,-5.542117e-02,9.999644e-01, 7.969825e-03, -2.764397e-03, -2.918589e-01,0,0,0,1]).reshape([4,4]))# [R,t] T_velo_imu= np.mat(np.array([ 9.999976e-01, 7.553071e-04, -2.035826e-03, -8.086759e-01,-7.854027e-04, 9.998898e-01, -1.482298e-02,3.195559e-01,2.024406e-03, 1.482454e-02, 9.998881e-01, -7.997231e-01,0,0,0,1]).reshape([4,4]))T_cam2_imu = P_2_rect_real * R_0_rect * T_cam_velo * T_velo_imuTbc = np.linalg.inv(T_cam2_imu)print("T_imu_cam2:", Tbc)# T_imu_cam2: [[ 0.00119669 -0.00924124 0.99995667 1.09838134]# [-0.99997659 -0.00674881 0.00113434 -0.25914669]# [ 0.00673803 -0.99993452 -0.0092491 0.74456586]# [ 0. 0. 0. 1. ]]# [R_02,t_02] from cam00 to cam02 # R_02: 9.999788e-01 -5.008404e-03 -4.151018e-03 4.990516e-03 9.999783e-01 -4.308488e-03 4.172506e-03 4.287682e-03 9.999821e-01# T_02: 5.954406e-02 -7.675338e-04 3.582565e-03T_cam2_cam0= np.mat(np.array([ 9.999788e-01, -5.008404e-03, -4.151018e-03, 5.954406e-02,4.990516e-03, 9.999783e-01, -4.308488e-03, -7.675338e-04,4.172506e-03, 4.287682e-03, 9.999821e-01, 3.582565e-03,0,0,0,1]).reshape([4,4]))T_cam2_imu_new = T_cam2_cam0 * T_cam_velo * T_velo_imuTbc_new = np.linalg.inv(T_cam2_imu_new)print("T_imu_cam2_new:\n", Tbc_new)# T_imu_cam2_new:# [[ 0.00462418 -0.00906059 0.99994832 1.09837845]# [-0.99980908 -0.01902944 0.00445111 -0.2595698 ]# [ 0.01898812 -0.99977782 -0.00914685 0.74420066]# [ 0. 0. 0. 1. ]]#===check: P_2_rect should equal to P * P_2_rect_real===# P = [K,0]P= np.mat(np.array([ fx, 0, cx, 0,0, fy, cy, 0,0, 0, 1, 0,0,0,0, 1]).reshape([4,4]))P_2_rect = np.mat(np.array([ 7.188560e+02, 0.000000e+00, 6.071928e+02, 4.538225e+01,0.000000e+00, 7.188560e+02, 1.852157e+02, -1.130887e-01,0.000000e+00, 0.000000e+00, 1.000000e+00, 3.779761e-03]).reshape([3,4]))print("P_2_rect:\n", P_2_rect)print("P * P_2_rect_real:\n", P * P_2_rect_real)# P_2_rect:# [[ 7.188560e+02 0.000000e+00 6.071928e+02 4.538225e+01]# [ 0.000000e+00 7.188560e+02 1.852157e+02 -1.130887e-01]# [ 0.000000e+00 0.000000e+00 1.000000e+00 3.779761e-03]]# P * P_2_rect_real:# [[ 7.188560e+02 0.000000e+00 6.071928e+02 4.538225e+01]# [ 0.000000e+00 7.188560e+02 1.852157e+02 -1.130887e-01]# [ 0.000000e+00 0.000000e+00 1.000000e+00 3.779761e-03]# [ 0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00]]passgetTbc()SLAM_kitti数据集求相机cam2到IMU的变换矩阵相关推荐
- 翻译--A PRIVACY-PRESERVING WAY TO FIND THE INTERSECTION OF TWO DATASETS 在保护隐私的前提下找两个数据集的交集--隐私数据集求交
隐私数据集交集(PSI)是一种强大的加密技术,它允许两方计算其数据的交集,而无需将其原始数据暴露给另一方.换句话说,PSI允许测试各方是否共享一个公共数据点(例如位置,ID等). 在这篇文章中,我们介 ...
- 使用ros发布UVC相机和串口IMU数据
1．目的:为了可以标定普通USB相机和固定在相机上的外置IMU的外参,我希望通过ROS获取更高分辨率和更高频率的图像数据,并且可以将图像和imu的topic发布出来,直接使用rosbag record ...
- 张氏标定法求相机模型中的内参（Intrinsics）
张正友2000年在<A Flexible New Technique for Camera Calibration>一文中提出了一种灵活的新技术来方便地校准相机.它只需要相机观察在几个(至 ...
- VINS在安卓手机上实现、安卓相机标定、IMU标定 亲爸级原理介绍和教学
需求和想法: 在安卓手机上实现SLAM中的定位与轨迹,或者 VINS在Android上的实现. **现有需求:**针对SLAM的算法:VINS-MONO(港科大的一套基于单目相机的SLAM项目),移植 ...
- ViTag ：在线 WiFi 精细时间测量辅助多人环境中的视觉-运动身份关联
摘要 在本文中,我们提出了 ViTag 来关联多模态数据中的用户身份,尤其是那些从相机和智能手机获得的数据. ViTag 将一系列视觉跟踪器生成的边界框与来自智能手机的惯性测量单元 (IMU) 数据和 ...
- imu相机标定_解放双手——相机与IMU外参的在线标定
本文作者 沈玥伶,公众号:计算机视觉life,编辑部成员 一.相机与IMU的融合 在SLAM的众多传感器解决方案中,相机与IMU的融合被认为具有很大的潜力实现低成本且高精度的定位与建图.这是因为这两个 ...
- 萌新谈相机与IMU的标定（转载）
我发现这里所给的标定板也是这个 https://drive.google.com/file/d/0B0T1sizOvRsUdjFJem9mQXdiMTQ/edit?resourcekey=0-0Cn7 ...
- ZED2相机标定--双目、IMU、联合标定
为了获取VINS 配置文件中的相关参数,也为了双目相机及IMU传感器实际输出数据更准确,对ZED2的相机进行标定,包括相机标定.IMU标定及联合标定. 一 安装标定工具 1.使用kalibr工具标定Z ...
- 使用Kalibr标定相机和IMU（ZED+px4）外参
提示:文章写完后,目录可以自动生成,如何生成可参考右边的帮助文档 使用Kalibr标定相机和IMU(ZED+px4)外参 前言 一.Kalibr介绍 二.CAM-IMU外参标定 1.相机内参 2.IM ...
- 双目相机 -- IMU联合标定
声明:一些图片是不该有水印的,CSDN把图片链接的格式改了,暂时还不知道怎么去掉,请见谅!!! 目录 **声明**:一些图片是不该有水印的,CSDN把图片链接的格式改了,暂时还不知道怎么去掉,请见谅! ...
最新文章
- java 参数类型不确定_详细解析Java虚拟机的栈帧结构
- linux 命令 echo 使用说明
- 详解SpringMVC中Controller的方法中参数的工作原理[附带源码分析]
- 重学java基础第一课:解决大家的疑问
- 厂办大集体改制不签字_许昌二印，磨砂技术被外国觊觎，老工人说烂在肚子里也不外漏...
- linuxoracle静默安装应答文件修改_Windows7重要更新补丁安装包 2020.01.17
- SAP License：SAP中的凭证查询、科目余额表及分类账详解
- 建筑与建筑群综合布线系统工程验收规范_如果这9个方面考虑周到 你的综合布线系统工程可以竣工验收了...
- Linux stat命令和AIX istat命令 （查看文件修改时间）
- gimp 去掉一个颜色的背景_把拍出来的图片文件的背景阴影去掉——用GIMP色阶功能修图
- 如何在macOS下调整磁盘分区大小
- CMS系统评测，主流自助建站系统比较排名
- 万能地图下载器下载谷歌卫星地图在ArcGIS中套合
- 槽函数获取sender_Qt QObject::sender()用法
- 解决浏览器无法打开github官网的问题
- Logback日志名和日志内容配置增加ip等信息
- 2022必看花展 IFEX昆明国际花卉园艺展，新展期11月11-13日
- 品优购项目笔记（十四）：微信支付
- SEGGER公司JlinkV9仿真器实现串口通讯VCOM和SWD调试双功能
- JavaScript实现的简单烟花特效代码1.15