重庆富通科技有限公司网站,wordpress怎么批量移动分类,大庆网站建设大庆,高清做视频在线观看网站目录 1 标定工具编译
1.1 IMU标定工具 imu_utils
1.2 相机标定工具 kaliber
2 标定数据录制
3 开始标定
3.1 IMU标定
3.2 相机标定
3.3 相机IMU联合标定
4 将参数填入ORBSLAM的文件中 1 标定工具编译
1.1 IMU标定工具 imu_utils 标定IMU我们使用imu_utils软件进行标定…目录 1 标定工具编译
1.1 IMU标定工具 imu_utils
1.2 相机标定工具 kaliber
2 标定数据录制
3 开始标定
3.1 IMU标定
3.2 相机标定
3.3 相机IMU联合标定
4 将参数填入ORBSLAM的文件中 1 标定工具编译
1.1 IMU标定工具 imu_utils 标定IMU我们使用imu_utils软件进行标定 首先我们安装标定软件的依赖项Eigen、Ceres 通过命令行安装Eigen3.3.4即可 sudo apt-get install libdw-dev
sudo apt-get install libeigen3-dev 安装Ceres1.14.0的依赖项 sudo apt-get install liblapack-dev libblas-dev libeigen3-dev libgflags-dev libgoogle-glog-dev
sudo apt-get install liblapack-dev libsuitesparse-dev libcxsparse3 libgflags-dev libgoogle-glog-dev libgtest-dev安装Ceres1.14.0 wget -O ~/Downloads/ceres.zip https://github.com/ceres-solver/ceres-solver/archive/1.14.0.zip
cd ~/Downloads/ unzip ceres.zip -d ~/Downloads/
cd ~/Downloads/ceres-solver-1.14.0
mkdir ceres-bin cd ceres-bin
cmake ..
sudo make install -j4 这些安装之后我们开始安装imu_utils。 首先为我们要先在ROS环境下编译code_utils否则会报错 cd ..catkin_imu/src
git clone https://github.com/gaowenliang/code_utils
cd ..
catkin_make 运行这个步骤会报错找不到backward.hpp这个头文件: 解决方案 把src/code_utils/CMakeList.txt中添加路径include_directories(“include/code_utils”) 如下图 安装imu_utils cd ..catkin_imu/src
git clone https://github.com/gaowenliang/imu_utils
cd ..
catkin_make #编译imu_utils这样就编译成功了 1.2 相机标定工具 kaliber 标定IMU相机与相机的标定我们使用kaliber软件进行标定 先进行依赖安装 sudo apt install python-setuptools python-rosinstall ipython libeigen3-dev libboost-all-dev doxygen libopencv-dev
sudo apt install ros-noetic-vision-opencv ros-noetic-image-transport-plugins ros-noetic-cmake-modules
sudo apt install python-software-properties software-properties-common libpoco-dev python-matplotlib python-scipy python-git python-pip ipython
sudo apt install libtbb-dev libblas-dev liblapack-dev python-catkin-tools libv4l-dev
sudo apt install build-essential python-dev libxml2 libxml2-dev zlib1g-dev bison flex libigraph0-dev texlive-binaries
sudo pip install -i https://pypi.tuna.tsinghua.edu.cn/simple python-igraph
sudo pip install python-igraph --upgrade
sudo apt-get install python-setuptools python-rosinstall ipython libeigen3-dev libboost-all-dev doxygen libopencv-dev ros-melodic-vision-opencv ros-melodic-image-transport-plugins ros-melodic-cmake-modules python-software-properties software-properties-common libpoco-dev python-matplotlib python-scipy python-git python-pip ipython libtbb-dev libblas-dev liblapack-dev python-catkin-tools libv4l-dev编译 kaliber下载网站https://gitcode.net/mirrors/ethz-asl/kalibr 从上述网址下载Kaliber正常编译即可。不会出什么问题。 2 标定数据录制 IMU数据 IMU静置2小时周围不要有振动录制完成后利用下面的脚本转化成rosbag的格式。 这里是一个可以使用的转化脚本将文本的IMU信息转化为了sensor_msgs/Imu的信息
Function: convert rawdata into rosbag
Author: Yiheng Zhao
Date: 2023.10.11import math
import os
import cv2
import numpy as np
from vp_config import ROOT_PATH
from utility import ReadQapData, fix_filenameimport rospy
import rosbag
from sensor_msgs.msg import Imu, Image
from cv_bridge import CvBridge
import openpyxl
import timeif __name__ __main__:############################# rosbag config###########################save_path os.path.join(ROOT_PATH, imu.bag)bag rosbag.Bag(save_path, w)############################# main function############################# read dataworkbook openpyxl.load_workbook(rD:\projectslam\off_data_zhuan_ros\raw_data\20231010_180949.xlsx)sheet workbook.active## begin frame by frame processi 0for row in sheet.iter_rows(values_onlyTrue):#create new messageimu_msg Imu()imu_msg.header.frame_id base_linkimu_msg.header.seq itimestamp time.time()formatted_timestamp {:.9f}.format(timestamp)secs int(formatted_timestamp.split(.)[0])nsecs int(formatted_timestamp.split(.)[1])imu_msg.header.stamp.secs secsimu_msg.header.stamp.nsecs nsecsimu_msg.linear_acceleration.x float(row[9])imu_msg.linear_acceleration.y float(row[10])imu_msg.linear_acceleration.z float(row[11])print(acceleration x is %f % imu_msg.linear_acceleration.x)print(acceleration y is %f % imu_msg.linear_acceleration.y)print(acceleration z is %f % imu_msg.linear_acceleration.z)imu_msg.angular_velocity.x ( float(row[6])/ 180.0 * 3.1415926)imu_msg.angular_velocity.y ( float(row[7])/ 180.0 * 3.1415926)imu_msg.angular_velocity.z ( float(row[8])/ 180.0 * 3.1415926)print(angular x is %f % imu_msg.angular_velocity.x)print(angular y is %f % imu_msg.angular_velocity.y)print(angular z is %f % imu_msg.angular_velocity.z)bag.write(topic/imu/data_raw, msgimu_msg)i 1time.sleep(0.033)bag.close() 我们得到了一个仅含IMU数据的bag。 相机数据录制 缓慢移动相机且相机和IMU之间不要发生相对运动将相机左右移动、上下移动、旋转移动充分激励IMU录制三分钟左右即可。 我们得到一个bag包含IMU和相机数据 下面这个脚本是合并IMU、相机图像数据的脚本
Function: convert rawdata into rosbag
Author: Yiheng Zhao
Date: 2023.10.11import math
import os
import cv2
import numpy as np
from vp_config import ROOT_PATH
from utility import ReadQapData, fix_filenameimport rospy
import rosbag
from sensor_msgs.msg import Imu, Image
from cv_bridge import CvBridge
import openpyxl
import timeif __name__ __main__:############################# rosbag config###########################save_path os.path.join(ROOT_PATH, imu_cam.bag)bag rosbag.Bag(save_path, w)############################# main function############################# read data image# 指定存储图片的目录路径image_directory rD:\projectslam\off_data_zhuan_ros\qap_out_data\image# 初始化一个空列表来存储图片路径image_paths []# 遍历目录下的所有文件for root, dirs, files in os.walk(image_directory):for file in files:# 检查文件扩展名是否为图片格式例如这里假设是以.jpg、.png、.jpeg为扩展名的图片if file.lower().endswith((.jpg, .png, .jpeg)):# 使用os.path.join()将目录和文件名组合成完整的文件路径image_path os.path.join(root, file)# 将图片路径添加到列表中image_paths.append(image_path)print(image_paths)## read data imuworkbook openpyxl.load_workbook(rD:\projectslam\off_data_zhuan_ros\qap_out_data\imu.xlsx)sheet workbook.active## begin frame by frame processi 0for row in sheet.iter_rows(values_onlyTrue):# create new messageimu_msg Imu()imu_msg.header.frame_id base_linkimu_msg.header.seq itimestamp time.time()formatted_timestamp {:.9f}.format(timestamp)secs int(formatted_timestamp.split(.)[0])nsecs int(formatted_timestamp.split(.)[1])imu_msg.header.stamp.secs secsimu_msg.header.stamp.nsecs nsecsimu_msg.linear_acceleration.x float(row[9])imu_msg.linear_acceleration.y float(row[10])imu_msg.linear_acceleration.z float(row[11])print(acceleration x is %f % imu_msg.linear_acceleration.x)print(acceleration y is %f % imu_msg.linear_acceleration.y)print(acceleration z is %f % imu_msg.linear_acceleration.z)imu_msg.angular_velocity.x (float(row[6]) / 180.0 * 3.1415926)imu_msg.angular_velocity.y (float(row[7]) / 180.0 * 3.1415926)imu_msg.angular_velocity.z (float(row[8]) / 180.0 * 3.1415926)print(angular x is %f % imu_msg.angular_velocity.x)print(angular y is %f % imu_msg.angular_velocity.y)print(angular z is %f % imu_msg.angular_velocity.z)# 图像 msgimage cv2.imread(image_paths[i])my_bridge CvBridge()img_msg my_bridge.cv2_to_imgmsg(cvimimage)img_msg.header.frame_id base_linkimg_msg.header.seq iimg_msg.header.stamp.secs secsimg_msg.header.stamp.nsecs nsecsbag.write(topic/image/data_raw, msgimg_msg)bag.write(topic/imu/data_raw, msgimu_msg)i 1time.sleep(0.033)bag.close() 下面开始标定。 3 开始标定
3.1 IMU标定 对于6轴的IMU我们修改这个文件 /bag/catkin_imu/src/imu_utils/launch/tum.launch 修改内容如下 修改我们IMU的录制时间与IMU话题 launchnode pkgimu_utils typeimu_an nameimu_an outputscreenparam nameimu_topic typestring value /imu/data_raw/param nameimu_name typestring value custom_imu_nrxdwcs/param namedata_save_path typestring value $(find imu_utils)/imu666//param namemax_time_min typeint value 90/param namemax_cluster typeint value 50//node/launch 修改imu_topic为我们包的IMU录制话题 修改imu_name为我们IMU的名字这里我随便起得名和客户名字有关系..... 修改max_time_min为我们IMU录制的时间我这里是从0955 - 1130我选择取前90分钟的数据。 修改max_cluster为采样频率由于我录制不够2小时因此修改采样频率为50HZ增大了采样频率。 修改data_save_path为我们标定完成的路径即标定文件存放的位置。 下面开始标定 打开标定IMU的ROS节点 liuhongweiliuhongwei-Legion-Y9000P-IRX8H:~/Downloads$ cd /bag/catkin_imu/
liuhongweiliuhongwei-Legion-Y9000P-IRX8H:/bag/catkin_imu$ source devel/setup.bash
liuhongweiliuhongwei-Legion-Y9000P-IRX8H:/bag/catkin_imu$ roslaunch imu_utils tum.launch 打开节点后我们以200倍速度播包。 rosbag play imu.bag -r 200播包完毕后我们IMU标定就完成了。 标定文件存储在我们指定的路径中。 第一个文件就是我们需要的IMU参数。 3.2 相机标定 我们先需要下载标定版这里我推荐带编码信息的棋盘格标定板 标定版下载链接https://doc-08-5c-docs.googleusercontent.com/docs/securesc/2nlhb7mn3rh7ilhvic8i1i0lcg6lvbo5/kcic7lcag2vqbkks6cg7sa20rnhoqc5r/1696916775000/08341388560495021951/08634034057607032407/1DqKWgePodCpAKJCd_Bz-hfiEQOSnn_k0?edownloadaxAA75yW7BQ9IbcKRqN7F30tCa7QeNZmYUtrGfL0rCKL3H-BPWurSVMZ8SlMyN7l7mcABbUuU4t6LKNh1GUv6oaKYdz8fhFhpvrys81_Tr-LK6b6VaHTYZrKdK1Xl-7jalz-zRTbOGJI0B_pxlK-zYjlJ5qptj6eJa12S-A520-9oO-QwEJa2FTA10ED_NooTkPqK2nYqfulra1G-7X7By1KB5iB1aK6goViNqPnnFNBWaSyNKb2GBEDPdMgTphe8yFZ9OSGtrzNW9zdbAdM-Ohm-JP34_llYMgTzRxwqKX9ltC34xf4bCU83vDIOfrjqZHos9XkPmWahZuxtJxZGuRDWIBKhOb1P8y6qOVpvRP-hNZB4z8uPyiQ-Qu8q5xqGH1oT6kuQONiCAm1kDI0c0wp4lBi0DMV_5HHBnOrS7x26nTrsWYFAsqdjcx0awomsAlDtSVMc4zZ8pQJDeoV7Qa19VAC-9BidANzgAca2TyLven2FHj3ogrAz-2nlHDOK6OHT3Rzjdd9I5UNRg3ZQUP5g8SEXUo3qHDM0u1n1PKoaZKoRlFaYTYyZKMTqnhOBiBuyjqNB8LRCIteoBC335dRHdjRSzwlOD79bLwQGjXw_ItlDo_6YUV1ZM8nep9kzzcLNP34d_MUMNp6rSBHyfug5jobqcdtHmcWFgJuf2b0u6H2UWHP-0WRmjbHWfdbDQKK8vEmgRlndGnk6gxL8HqL_PQYO0yJ6ddagbHBztZZCZbXSl_KUPYDVd212u-vsoc6BsgYoj200XU7vQE3AfekgV0RLJNzeL0RCIT7ghfHQIBNXFmfTq8Y4byyh5-wnlqTvHi5WgCsF6x9_2sC6FVdZtvOxmpBlufS_eT9FaWu-cNk30Kor_OnQUv8RMLO9mcJbtzwuuid51452ed9-1b64-4adc-88d9-65bedb46fdfcauthuser0nonce5kor9vi5br1lguser08634034057607032407hash7qn0q7b6strcok04upeb271oq7qcpf6c 我们需要制作参数文档参数文档的数学信息如下 原始pdf的格子参数是
6*6的格子
大格子边长5.5cm
小格子边长1.65cm
小格子与大格子边长比例0.3调整后的格子参数是
大格子边长2.2cm
小格子边长0.66cm
小格子与大格子边长比例0.3然后如果你是打印成了A4纸的形式可以参考我的参数文档A4.yaml target_type: aprilgrid #gridtype
tagCols: 6 #number of apriltags
tagRows: 6 #number of apriltags
tagSize: 0.021 #size of apriltag, edge to edge [m]
tagSpacing: 0.285714285714 #ratio of space between tags to tagSize
codeOffset: 0 #code offset for the first tag in the aprilboard现在我们进行针孔相机的标定 rosrun kalibr kalibr_calibrate_cameras --target /bag/catkin_kaliber/src/Kalibr/a4.yaml --bag /home/liuhongwei/Desktop/imu_cam.bag --models pinhole-radtan --topics /image/data_raw --bag-from-to 10 100 --show-extraction 然后就开始了标定工作 解释一下具体的参数 --target标定版的参数就是我们刚才写的那个 --bag包的路径 --models针孔相机模型选这个 --topics图像信息的话题 --bag-from-to选取10-100s的图像进行标定这个可以按照自己需求改一般都是前几秒比较模糊就不要了 --show-extraction展示图形化界面 标定完成后会输出几个文件 这个就是我们相机的内参了。 标定时可能会遇到这个问题这是因为相机焦距太大了我们需要设置个初始值 Initialization of focal length failed. You can enable manual input by setting ‘KALIBR_MANUAL_FOCAL_LENGTH_INIT’.遇到这种情况我们先终端中设置变量 KALIBR_MANUAL_FOCAL_LENGTH_INIT 1 然后程序运行时手动给相机设置初始焦距。 3.3 相机IMU联合标定 这个我们事先制作几个文件 1.imu的配置信息我们取名为imu.yaml这个就是我们把我们之前标定的IMU信息写入这个文件就行 rostopic: /imu/data_raw
update_rate: 30.0 #Hzaccelerometer_noise_density: 1.7640241083260223e-03
accelerometer_random_walk: 4.6133140085614272e-05
gyroscope_noise_density: 1.2287169549703986e-05
gyroscope_random_walk: 8.1951127134973680e-07图像的话题还有IMU的频率不要忘记修改。 2.相机的内参标定信息 这个是3.2节中生成的文件imu_cam-camchain.yaml cam0:cam_overlaps: []camera_model: pinholedistortion_coeffs: [-0.34038923175502456, 0.06977055299360228, 0.015293838790916657, -0.010372561499554008]distortion_model: radtanintrinsics: [1685.169877633105, 1656.9322836449144, 997.1304121813936, 474.3184148435317]resolution: [1920, 1080]rostopic: /image/data_raw 3.标定版文件就是3.2中我们自己写的 target_type: aprilgrid #gridtype
tagCols: 6 #number of apriltags
tagRows: 6 #number of apriltags
tagSize: 0.021 #size of apriltag, edge to edge [m]
tagSpacing: 0.285714285714 #ratio of space between tags to tagSize
codeOffset: 0 #code offset for the first tag in the aprilboard执行下面代码进行标定 rosrun kalibr kalibr_calibrate_imu_camera --bag /home/liuhongwei/Desktop/imu_cam.bag --target /bag/catkin_kaliber/src/Kalibr/a4.yaml --cam /bag/catkin_kaliber/src/Kalibr/imu_cam-camchain.yaml --imu /bag/catkin_kaliber/src/Kalibr/imu.yaml --show-extraction 参数列表含义如下 --bag数据包路径 --target标定版文件路径A4.yaml --cam相机内参文件路径mu_cam-camchain.yaml --imuIMU标定文件路径imu.yaml --show-extraction显示标定过程 执行如下 标定结束 结束后生成标定文件imu_cam-results-imucam.txt 标定完毕。 4 将参数填入ORBSLAM的文件中 根据上述我们的标定结果我们的yaml文件为 %YAML:1.0#--------------------------------------------------------------------------------------------
# Camera Parameters. Adjust them!
#--------------------------------------------------------------------------------------------
File.version: 1.0Camera.type: PinHole# Camera calibration and distortion parameters (OpenCV)
Camera1.fx: 1685.16987763
Camera1.fy: 1656.93228364
Camera1.cx: 997.13041218
Camera1.cy: 474.31841484Camera1.k1: -0.34038923175502456
Camera1.k2: 0.06977055299360228
Camera1.p1: 0.015293838790916657
Camera1.p2: -0.010372561499554008# Camera resolution
Camera.width: 1920
Camera.height: 1080Camera.newWidth: 600
Camera.newHeight: 350# Camera frames per second
Camera.fps: 30# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
Camera.RGB: 1# Transformation from camera to body-frame (imu)
IMU.T_b_c1: !!opencv-matrixrows: 4cols: 4dt: fdata: [0.94880513, 0.12309341, 0.27236458, 0.00027046,0.12309341, 0.98136615, 0.14754149, -0.00012572,-0.29088973, -0.10646184, 0.95081494, 0.00034056,0.0, 0.0, 0.0, 1.0]# IMU noise
IMU.NoiseGyro: 1.2287169549703986e-05 #1.6968e-04
IMU.NoiseAcc: 1.7640241083260223e-03 #2.0e-3
IMU.GyroWalk: 8.1951127134973680e-07
IMU.AccWalk: 4.6133140085614272e-05 # 3e-03
IMU.Frequency: 30.0#--------------------------------------------------------------------------------------------
# ORB Parameters
#--------------------------------------------------------------------------------------------# ORB Extractor: Number of features per image
ORBextractor.nFeatures: 1000 # 1000# ORB Extractor: Scale factor between levels in the scale pyramid
ORBextractor.scaleFactor: 1.2# ORB Extractor: Number of levels in the scale pyramid
ORBextractor.nLevels: 8# ORB Extractor: Fast threshold
# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response.
# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST
# You can lower these values if your images have low contrast
ORBextractor.iniThFAST: 20
ORBextractor.minThFAST: 7#--------------------------------------------------------------------------------------------
# Viewer Parameters
#--------------------------------------------------------------------------------------------
Viewer.KeyFrameSize: 0.05
Viewer.KeyFrameLineWidth: 1.0
Viewer.GraphLineWidth: 0.9
Viewer.PointSize: 2.0
Viewer.CameraSize: 0.08
Viewer.CameraLineWidth: 3.0
Viewer.ViewpointX: 0.0
Viewer.ViewpointY: -0.7
Viewer.ViewpointZ: -3.5 # -1.8
Viewer.ViewpointF: 500.05 Euroc单目IMU数据集制作及跑通 用这个脚本进行拆包 # -*- coding: utf-8 -*-import rosbag
import csv
from sensor_msgs.msg import Imu
import os
import roslib
import rospy
import cv2
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
from cv_bridge import CvBridgeError
import shutildef CreateDIR():folder_name bag_tumsubfolders [left, right , rgb , depth]if not os.path.exists(folder_name):os.makedirs(folder_name)# 在主文件夹下创建子文件夹for subfolder in subfolders:subfolder_path os.path.join(folder_name, subfolder)if not os.path.exists(subfolder_path):os.makedirs(subfolder_path)def CreateIMUCSV(umpackbag):csvfile open(imudata.csv, w)csvwriter csv.writer(csvfile)csvwriter.writerow([timestamp [ns], w_RS_S_x [rad s^-1], w_RS_S_y [rad s^-1], w_RS_S_z [rad s^-1], a_RS_S_x [rad m s^-2], a_RS_S_y [rad m s^-2], a_RS_S_z [rad m s^-2]])for topic, msg, t in umpackbag.read_messages(topics[/imu/data_raw]):timestamp msg.header.stamp.to_nsec()ax msg.linear_acceleration.xay msg.linear_acceleration.yaz msg.linear_acceleration.zwx msg.angular_velocity.xwy msg.angular_velocity.ywz msg.angular_velocity.zcsvwriter.writerow([timestamp, wx, wy, wz, ax, ay, az])#umpackbag.close()csvfile.close()def TransIMUdatatotxt():csv_file ./imudata.csvtxt_file ./imudata.txtwith open(csv_file, r) as file:reader csv.reader(file)with open(txt_file, w) as output_file:writer csv.writer(output_file, delimiter,, quotechar, quotingcsv.QUOTE_MINIMAL)for i, row in enumerate(reader):if i 0:writer.writerow([# cell for cell in row]) # 添加#号else:writer.writerow(row)# Save RGBD image and Save its timestamp
def Savergb(umpackbag):path ./bag_tum/rgb/bridge CvBridge()image_names []txt_file ./rgbtimestamp.txtwith rosbag.Bag(bagname, r) as bag:for topic, msg, t in umpackbag.read_messages():if topic /image/data_raw:try:cv_image bridge.imgmsg_to_cv2(msg)except CvBridgeError as e:print(e)continue#timestr %.9f % msg.header.stamp.to_sec()timestr %.6f % msg.header.stamp.to_sec()#timestr %.1f % msg.header.stamp.to_sec()image_name timestr#image_name timestr.replace(., ) # Remove periods from the timestampcv2.imwrite(path image_name .png, cv_image) # Save as PNG formatimage_names.append(image_name) # Add image name to the listwith open(txt_file, w) as f:#f.write(\n.join([{} rgb/{}.png.format(t, t) for t in image_names]))f.write(\n.join(image_names))# Script Menu
# Make a folder name bag_tum include three sunfolder : left right rgb , in folder their image in it
# in python main.py folder , create imudata.scv and imudata.txt ,aim for KITTI or TUM dataset
# in python main.py folder , create timestamp.txt for image timestamp
# in python main.py folder , create timestamp.txt for image timestamp
if __name__ __main__:bagname imu_cam.bagumpackbag rosbag.Bag(bagname)CreateDIR()CreateIMUCSV(umpackbag)TransIMUdatatotxt()Savergb(umpackbag)执行脚本后得到如下文件 timestamp.txt文件夹 我们开始制作数据集建立一个01文件夹 将timestamp.txt文件夹放在这里再创建一个mav0的文件夹。 在mav0文件夹里面创建cam0和imu0文件夹 cam0里面创建data文件夹存放图像数据这里的图像就是bag_tum/rgb目录下的图像 imu0里面存放的是data.csv和data.txt存放IMU数据。 至此我们数据集制作完毕向程序输入参数 ORB词典位置、标定参数文件位置、01文件夹位置以及时间戳的位置。 此外还需要改一个地方 mono_inertial_euroc.cc文件的86行改为 string pathImu pathSeq /mav0/imu0/data.txt;这样就可以跑啦
