在gazebo中进行相机标定
相机标定原理
相机标定意义
在图像测量过程以及机器视觉应用中，为确定空间物体表面某点的三维几何位置与其在图像中对应点之间的相互关系，必须建立相机成像的几何模型，这些几何模型参数就是相机参数。
在后面可以看到 世界坐标系与 像素坐标系的 转换 关系 里面的 几个矩阵，就是要通过标定求得的 相关 参数
相机标定原理
所以 相机标定的 目的 就是 求 相关 参数
相机标定参数
具体参数如下：
- 1、相机内参 是一个 4*3的矩阵
- 2、相机外参 相机坐标系与世界坐标系的 旋转和平移
- 3、畸变参数 5 个 参数
相关坐标系
相关的坐标系有
- 世界坐标系
- 相机坐标系
- 像素坐标系
世界坐标系
世界坐标系（world coordinate），也称为测量坐标系，是一个三维直角坐标系，以其为基准可以描述相机和待测物体的空间位置。世界坐标系的位置可以根据实际情况自由确定。
相机坐标系
相机坐标系（camera coordinate），也是一个三维直角坐标系，原点位于镜头光心处，x、y轴分别与相面的两边平行，z轴为镜头光轴，与像平面垂直。
像素坐标系、图像坐标系
像素坐标系是一个二维直角坐标系，反映了相机CCD/CMOS芯片中像素的排列情况。原点位于图像的左上角，轴、轴分别于像面的两边平行。像素坐标系中坐标轴的单位是像素（整数）。
像素坐标系不利于坐标变换，因此需要建立图像坐标系，其坐标轴的单位通常为毫米（mm），原点是相机光轴与相面的交点（称为主点），即图像的中心点，轴、轴分别与轴、轴平行。故两个坐标系实际是平移关系，即可以通过平移就可得到。
相机坐标系转换为世界坐标系
转换方程为：
其中R为33的旋转矩阵，t为31的平移矢量
这个就是外参
像素坐标系转换为图像坐标系
其中，dx、dy分别为像素在x、y轴方向上的一个像素物理尺寸，为主点（图像原点）坐标。
相机坐标系转换为图像坐标系
世界坐标系转换为像素坐标系
其中 f 为摄像机的焦距，单位一般是mm;dx,dy 为像元尺寸;u0,v0 为图像中心。fx = f/dx, fy = f/dy,分别称为x轴和y轴上的归一化焦距.
其中相机的内参和外参可以通过张正友标定获取。通过最终的转换关系来看，一个三维中的坐标点，的确可以在图像中找到一个对应的像素点，但是反过来，通过图像中的一个点找到它在三维中对应的点就很成了一个问题，因为我们并不知道等式左边的Zc的值。
举个实例
现以NiKon D700相机为例进行求解其内参数矩阵：
就算大家身边没有这款相机也无所谓，可以在网上百度一下，很方便的就知道其一些参数——
焦距 f = 35mm 最高分辨率：4256×2832 传感器尺寸：36.0×23.9 mm
根据以上定义可以有：
u0= 4256/2 = 2128 v0= 2832/2 = 1416 dx = 36.0/4256 dy = 23.9/2832
fx = f/dx = 4137.8 fy = f/dy = 4147.3
用ROS camera_calibration 功能包 在gazebo中进行 相机校准
下面 在 gazebo 中建立一个 可以校准的 仿真 环境 。 世界、相机、棋盘。
gazebo .world 文件
在gazebo 中 建立 一个 0 重力的 world 文件
<?xml version="1.0" ?>
<sdf version="1.4">
<world name="default">
<include>
<uri>model://ground_plane</uri>
</include>
<include>
<uri>model://sun</uri>
</include>
<physics type="ode">
<gravity>0 0 0</gravity>
<real_time_update_rate>1000.0</real_time_update_rate>
</physics>
</world>
</sdf>
上面 gravity 标签 这样设置 即 为 正常 重力
<gravity>0 0 -9.81</gravity>
建立 相机 的 xacro 模型 文件
<?xml version="1.0" ?>
<robot name="simple_camera" xmlns:xacro="http://www.ros.org/wiki/xacro">
<link name="world">
<origin xyz="0.0 0.0 0.0"/>
</link>
<joint name="camera_joint" type="fixed">
<parent link="world"/>
<child link="camera_link"/>
<origin rpy="0.0 1.5708 1.5708" xyz="0 0.0 0.5"/> rpy这样设置 垂直向下拍摄 xyz 这样设置 高度0.5m
</joint>
<link name="camera_link">
<visual>
<origin xyz="0 0 0.0" rpy="0 0 0"/>
<geometry>
<box size="0.03 0.01 0.01"/> 这个 尺寸 大小 无所谓
</geometry>
</visual>
<inertial>
<mass value="10" />
<origin xyz="0 0 0" rpy="0 0 0"/>
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
</inertial>
</link>
<!-- camera simulator plug-in -->
<gazebo reference="camera_link"> 加载 相机 的 插件 下面 配置相机参数
<sensor type="camera" name="camera">
<update_rate>30.0</update_rate>
<camera name="camera">
<horizontal_fov>0.6</horizontal_fov> 相机 水平视场角
<image>
<width>640</width> 像素宽度 u
<height>480</height> 像素高度 v
<format>R8G8B8</format>
</image>
<clip>
<near>0.005</near> 最近拍摄距离
<far>0.9</far> 最远拍摄距离
</clip>
<noise> // 相机噪声
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.000</stddev>
</noise>
</camera>
<plugin name="camera_controller" filename="libgazebo_ros_camera.so">
<alwaysOn>true</alwaysOn>
<updateRate>0.0</updateRate>
<cameraName>simple_camera</cameraName>
<imageTopicName>image_raw</imageTopicName>
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
<frameName>camera_link</frameName>、
后面需要 看看 求 得的 和这里一致不 一致 应该 会 差些 更 该 这里值 再看看
<distortionK1>0.0</distortionK1> 这里的畸变参数均设置为 0 K是 横向畸变参数
<distortionK2>0.0</distortionK2> 这里的畸变参数均设置为 0
<distortionK3>0.0</distortionK3> 这里的畸变参数均设置为 0
<distortionT1>0.0</distortionT1> 这里的畸变参数均设置为 0 T是 切向畸变参数
<distortionT2>0.0</distortionT2> 这里的畸变参数均设置为 0
</plugin>
</sensor>
</gazebo>
</robot>
建立棋盘
下面做一个黑白相间的棋盘 做 标定用。
做好后 是 下面这样的
棋盘的 sdf 模型 文件 如下
<?xml version='1.0'?>
<sdf version="1.4">
<model name="checkerboard">
<pose>0 0 0 0 0 0</pose>
<static>false</static>
<link name="checkerboard">
<pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
<inertial>
<mass>0.01</mass>
<inertia> <!-- interias are tricky to compute -->
<!-- http://answers.gazebosim.org/question/4372/the-inertia-matrix-explained/ -->
<ixx>0.083</ixx> <!-- for a box: ixx = 0.083 * mass * (y*y + z*z) -->
<ixy>0.0</ixy> <!-- for a box: ixy = 0 -->
<ixz>0.0</ixz> <!-- for a box: ixz = 0 -->
<iyy>0.083</iyy> <!-- for a box: iyy = 0.083 * mass * (x*x + z*z) -->
<iyz>0.0</iyz> <!-- for a box: iyz = 0 -->
<izz>0.083</izz> <!-- for a box: izz = 0.083 * mass * (x*x + y*y) -->
</inertia>
</inertial>
<collision name="collision">
<geometry>
<box>
<size>0.02 0.02 0.005</size>
</box>
</geometry>
</collision>
<visual name="sqr11">
<geometry>
<box>
<size>0.02 0.02 0.005</size>
</box>
</geometry>
<material>
<ambient>0 0 0 1</ambient>
<diffuse>0 0 0 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<visual name="sqr12">
<pose>0.02 0.0 0.0 0.0 0.0 0.0</pose>
<geometry>
<box>
<size>0.02 0.02 0.005</size>
</box>
</geometry>
<material>
<ambient>1 1 1 1</ambient>
<diffuse>1 1 1 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<visual name="sqr13">
<pose>0.04 0.0 0.0 0.0 0.0 0.0</pose>
<geometry>
<box>
<size>0.02 0.02 0.005</size>
</box>
</geometry>
<material>
<ambient>0 0 0 1</ambient>
<diffuse>0 0 0 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<visual name="sqr14">
<pose>0.06 0.0 0.0 0.0 0.0 0.0</pose>
<geometry>
<box>
<size>0.02 0.02 0.005</size>
</box>
</geometry>
<material>
<ambient>1 1 1 1</ambient>
<diffuse>1 1 1 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<visual name="sqr15">
<pose>0.08 0.0 0.0 0.0 0.0 0.0</pose>
<geometry>
<box>
<size>0.02 0.02 0.005</size>
</box>
</geometry>
<material>
<ambient>0 0 0 1</ambient>
<diffuse>0 0 0 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<visual name="sqr16">
<pose>0.10 0.0 0.0 0.0 0.0 0.0</pose>
<geometry>
<box>
<size>0.02 0.02 0.005</size>
</box>
</geometry>
<material>
<ambient>1 1 1 1</ambient>
<diffuse>1 1 1 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<visual name="sqr17">
<pose>0.12 0.0 0.0 0.0 0.0 0.0</pose>
<geometry>
<box>
<size>0.02 0.02 0.005</size>
</box>
</geometry>
<material>
<ambient>0 0 0 1</ambient>
<diffuse>0 0 0 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<visual name="sqr18">
<pose>0.14 0.0 0.0 0.0 0.0 0.0</pose>
<geometry>
<box>
<size>0.02 0.02 0.005</size>
</box>
</geometry>
<material>
<ambient>1 1 1 1</ambient>
<diffuse>1 1 1 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<visual name="sqr21">
<pose>0.0 0.02 0.0 0.0 0.0 0.0</pose>
<geometry>
<box>
<size>0.02 0.02 0.005</size>
</box>
</geometry>
<material>
<ambient>1 1 1 1</ambient>
<diffuse>1 1 1 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<visual name="sqr31">
<pose>0.0 0.04 0.0 0.0 0.0 0.0</pose>
<geometry>
<box>
<size>0.02 0.02 0.005</size>
</box>
</geometry>
<material>
<ambient>0 0 0 1</ambient>
<diffuse>0 0 0 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<visual name="sqr32">
<pose>0.02 0.04 0.0 0.0 0.0 0.0</pose>
<geometry>
<box>
<size>0.02 0.02 0.005</size>
</box>
</geometry>
<material>
<ambient>1 1 1 1</ambient>
<diffuse>1 1 1 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<visual name="sqr33">
<pose>0.04 0.04 0.0 0.0 0.0 0.0</pose>
<geometry>
<box>
<size>0.02 0.02 0.005</size>
</box>
</geometry>
<material>
<ambient>0 0 0 1</ambient>
<diffuse>0 0 0 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<visual name="sqr34">
<pose>0.06 0.04 0.0 0.0 0.0 0.0</pose>
<geometry>
<box>
<size>0.02 0.02 0.005</size>
</box>
</geometry>
<material>
<ambient>1 1 1 1</ambient>
<diffuse>1 1 1 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<visual name="sqr35">
<pose>0.08 0.04 0.0 0.0 0.0 0.0</pose>
<geometry>
<box>
<size>0.02 0.02 0.005</size>
</box>
</geometry>
<material>
<ambient>0 0 0 1</ambient>
<diffuse>0 0 0 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<visual name="sqr36">
<pose>0.10 0.04 0.0 0.0 0.0 0.0</pose>
<geometry>
<box>
<size>0.02 0.02 0.005</size>
</box>
</geometry>
<material>
<ambient>1 1 1 1</ambient>
<diffuse>1 1 1 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<visual name="sqr37">
<pose>0.12 0.04 0.0 0.0 0.0 0.0</pose>
<geometry>
<box>
<size>0.02 0.02 0.005</size>
</box>
</geometry>
<material>
<ambient>0 0 0 1</ambient>
<diffuse>0 0 0 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<visual name="sqr38">
<pose>0.14 0.04 0.0 0.0 0.0 0.0</pose>
<geometry>
<box>
<size>0.02 0.02 0.005</size>
</box>
</geometry>
<material>
<ambient>1 1 1 1</ambient>
<diffuse>1 1 1 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<visual name="sqr51">
<pose>0.0 0.08 0.0 0.0 0.0 0.0</pose>
<geometry>
<box>
<size>0.02 0.02 0.005</size>
</box>
</geometry>
<material>
<ambient>0 0 0 1</ambient>
<diffuse>0 0 0 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<visual name="sqr52">
<pose>0.02 0.08 0.0 0.0 0.0 0.0</pose>
<geometry>
<box>
<size>0.02 0.02 0.005</size>
</box>
</geometry>
<material>
<ambient>1 1 1 1</ambient>
<diffuse>1 1 1 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<visual name="sqr53">
<pose>0.04 0.08 0.0 0.0 0.0 0.0</pose>
<geometry>
<box>
<size>0.02 0.02 0.005</size>
</box>
</geometry>
<material>
<ambient>0 0 0 1</ambient>
<diffuse>0 0 0 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<visual name="sqr54">
<pose>0.06 0.08 0.0 0.0 0.0 0.0</pose>
<geometry>
<box>
<size>0.02 0.02 0.005</size>
</box>
</geometry>
<material>
<ambient>1 1 1 1</ambient>
<diffuse>1 1 1 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<visual name="sqr55">
<pose>0.08 0.08 0.0 0.0 0.0 0.0</pose>
<geometry>
<box>
<size>0.02 0.02 0.005</size>
</box>
</geometry>
<material>
<ambient>0 0 0 1</ambient>
<diffuse>0 0 0 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<visual name="sqr56">
<pose>0.10 0.08 0.0 0.0 0.0 0.0</pose>
<geometry>
<box>
<size>0.02 0.02 0.005</size>
</box>
</geometry>
<material>
<ambient>1 1 1 1</ambient>
<diffuse>1 1 1 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<visual name="sqr57">
<pose>0.12 0.08 0.0 0.0 0.0 0.0</pose>
<geometry>
<box>
<size>0.02 0.02 0.005</size>
</box>
</geometry>
<material>
<ambient>0 0 0 1</ambient>
<diffuse>0 0 0 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<visual name="sqr58">
<pose>0.14 0.08 0.0 0.0 0.0 0.0</pose>
<geometry>
<box>
<size>0.02 0.02 0.005</size>
</box>
</geometry>
<material>
<ambient>1 1 1 1</ambient>
<diffuse>1 1 1 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<visual name="sqr71">
<pose>0.0 0.12 0.0 0.0 0.0 0.0</pose>
<geometry>
<box>
<size>0.02 0.02 0.005</size>
</box>
</geometry>
<material>
<ambient>0 0 0 1</ambient>
<diffuse>0 0 0 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<visual name="sqr72">
<pose>0.02 0.12 0.0 0.0 0.0 0.0</pose>
<geometry>
<box>
<size>0.02 0.02 0.005</size>
</box>
</geometry>
<material>
<ambient>1 1 1 1</ambient>
<diffuse>1 1 1 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<visual name="sqr73">
<pose>0.04 0.12 0.0 0.0 0.0 0.0</pose>
<geometry>
<box>
<size>0.02 0.02 0.005</size>
</box>
</geometry>
<material>
<ambient>0 0 0 1</ambient>
<diffuse>0 0 0 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<visual name="sqr74">
<pose>0.06 0.12 0.0 0.0 0.0 0.0</pose>
<geometry>
<box>
<size>0.02 0.02 0.005</size>
</box>
</geometry>
<material>
<ambient>1 1 1 1</ambient>
<diffuse>1 1 1 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<visual name="sqr75">
<pose>0.08 0.12 0.0 0.0 0.0 0.0</pose>
<geometry>
<box>
<size>0.02 0.02 0.005</size>
</box>
</geometry>
<material>
<ambient>0 0 0 1</ambient>
<diffuse>0 0 0 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<visual name="sqr76">
<pose>0.10 0.12 0.0 0.0 0.0 0.0</pose>
<geometry>
<box>
<size>0.02 0.02 0.005</size>
</box>
</geometry>
<material>
<ambient>1 1 1 1</ambient>
<diffuse>1 1 1 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<visual name="sqr77">
<pose>0.12 0.12 0.0 0.0 0.0 0.0</pose>
<geometry>
<box>
<size>0.02 0.02 0.005</size>
</box>
</geometry>
<material>
<ambient>0 0 0 1</ambient>
<diffuse>0 0 0 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<visual name="sqr78">
<pose>0.14 0.12 0.0 0.0 0.0 0.0</pose>
<geometry>
<box>
<size>0.02 0.02 0.005</size>
</box>
</geometry>
<material>
<ambient>1 1 1 1</ambient>
<diffuse>1 1 1 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<visual name="sqr21">
<pose>0.0 0.02 0.0 0.0 0.0 0.0</pose>
<geometry>
<box>
<size>0.02 0.02 0.005</size>
</box>
</geometry>
<material>
<ambient>1 1 1 1</ambient>
<diffuse>1 1 1 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<visual name="sqr22">
<pose>0.02 0.02 0.0 0.0 0.0 0.0</pose>
<geometry>
<box>
<size>0.02 0.02 0.005</size>
</box>
</geometry>
<material>
<ambient>0 0 0 1</ambient>
<diffuse>0 0 0 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<visual name="sqr23">
<pose>0.04 0.02 0.0 0.0 0.0 0.0</pose>
<geometry>
<box>
<size>0.02 0.02 0.005</size>
</box>
</geometry>
<material>
<ambient>1 1 1 1</ambient>
<diffuse>1 1 1 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<visual name="sqr24">
<pose>0.06 0.02 0.0 0.0 0.0 0.0</pose>
<geometry>
<box>
<size>0.02 0.02 0.005</size>
</box>
</geometry>
<material>
<ambient>0 0 0 1</ambient>
<diffuse>0 0 0 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<visual name="sqr25">
<pose>0.08 0.02 0.0 0.0 0.0 0.0</pose>
<geometry>
<box>
<size>0.02 0.02 0.005</size>
</box>
</geometry>
<material>
<ambient>1 1 1 1</ambient>
<diffuse>1 1 1 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<visual name="sqr26">
<pose>0.10 0.02 0.0 0.0 0.0 0.0</pose>
<geometry>
<box>
<size>0.02 0.02 0.005</size>
</box>
</geometry>
<material>
<ambient>0 0 0 1</ambient>
<diffuse>0 0 0 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<visual name="sqr27">
<pose>0.12 0.02 0.0 0.0 0.0 0.0</pose>
<geometry>
<box>
<size>0.02 0.02 0.005</size>
</box>
</geometry>
<material>
<ambient>1 1 1 1</ambient>
<diffuse>1 1 1 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<visual name="sqr28">
<pose>0.14 0.02 0.0 0.0 0.0 0.0</pose>
<geometry>
<box>
<size>0.02 0.02 0.005</size>
</box>
</geometry>
<material>
<ambient>0 0 0 1</ambient>
<diffuse>0 0 0 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<visual name="sqr41">
<pose>0.0 0.06 0.0 0.0 0.0 0.0</pose>
<geometry>
<box>
<size>0.02 0.02 0.005</size>
</box>
</geometry>
<material>
<ambient>1 1 1 1</ambient>
<diffuse>1 1 1 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<visual name="sqr42">
<pose>0.02 0.06 0.0 0.0 0.0 0.0</pose>
<geometry>
<box>
<size>0.02 0.02 0.005</size>
</box>
</geometry>
<material>
<ambient>0 0 0 1</ambient>
<diffuse>0 0 0 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<visual name="sqr43">
<pose>0.04 0.06 0.0 0.0 0.0 0.0</pose>
<geometry>
<box>
<size>0.02 0.02 0.005</size>
</box>
</geometry>
<material>
<ambient>1 1 1 1</ambient>
<diffuse>1 1 1 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<visual name="sqr44">
<pose>0.06 0.06 0.0 0.0 0.0 0.0</pose>
<geometry>
<box>
<size>0.02 0.02 0.005</size>
</box>
</geometry>
<material>
<ambient>0 0 0 1</ambient>
<diffuse>0 0 0 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<visual name="sqr45">
<pose>0.08 0.06 0.0 0.0 0.0 0.0</pose>
<geometry>
<box>
<size>0.02 0.02 0.005</size>
</box>
</geometry>
<material>
<ambient>1 1 1 1</ambient>
<diffuse>1 1 1 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<visual name="sqr46">
<pose>0.10 0.06 0.0 0.0 0.0 0.0</pose>
<geometry>
<box>
<size>0.02 0.02 0.005</size>
</box>
</geometry>
<material>
<ambient>0 0 0 1</ambient>
<diffuse>0 0 0 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<visual name="sqr47">
<pose>0.12 0.06 0.0 0.0 0.0 0.0</pose>
<geometry>
<box>
<size>0.02 0.02 0.005</size>
</box>
</geometry>
<material>
<ambient>1 1 1 1</ambient>
<diffuse>1 1 1 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<visual name="sqr48">
<pose>0.14 0.06 0.0 0.0 0.0 0.0</pose>
<geometry>
<box>
<size>0.02 0.02 0.005</size>
</box>
</geometry>
<material>
<ambient>0 0 0 1</ambient>
<diffuse>0 0 0 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<visual name="sqr61">
<pose>0.0 0.10 0.0 0.0 0.0 0.0</pose>
<geometry>
<box>
<size>0.02 0.02 0.005</size>
</box>
</geometry>
<material>
<ambient>1 1 1 1</ambient>
<diffuse>1 1 1 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<visual name="sqr62">
<pose>0.02 0.10 0.0 0.0 0.0 0.0</pose>
<geometry>
<box>
<size>0.02 0.02 0.005</size>
</box>
</geometry>
<material>
<ambient>0 0 0 1</ambient>
<diffuse>0 0 0 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<visual name="sqr63">
<pose>0.04 0.10 0.0 0.0 0.0 0.0</pose>
<geometry>
<box>
<size>0.02 0.02 0.005</size>
</box>
</geometry>
<material>
<ambient>1 1 1 1</ambient>
<diffuse>1 1 1 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<visual name="sqr64">
<pose>0.06 0.10 0.0 0.0 0.0 0.0</pose>
<geometry>
<box>
<size>0.02 0.02 0.005</size>
</box>
</geometry>
<material>
<ambient>0 0 0 1</ambient>
<diffuse>0 0 0 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<visual name="sqr65">
<pose>0.08 0.10 0.0 0.0 0.0 0.0</pose>
<geometry>
<box>
<size>0.02 0.02 0.005</size>
</box>
</geometry>
<material>
<ambient>1 1 1 1</ambient>
<diffuse>1 1 1 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<visual name="sqr66">
<pose>0.10 0.10 0.0 0.0 0.0 0.0</pose>
<geometry>
<box>
<size>0.02 0.02 0.005</size>
</box>
</geometry>
<material>
<ambient>0 0 0 1</ambient>
<diffuse>0 0 0 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<visual name="sqr67">
<pose>0.12 0.10 0.0 0.0 0.0 0.0</pose>
<geometry>
<box>
<size>0.02 0.02 0.005</size>
</box>
</geometry>
<material>
<ambient>1 1 1 1</ambient>
<diffuse>1 1 1 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<visual name="sqr68">
<pose>0.14 0.10 0.0 0.0 0.0 0.0</pose>
<geometry>
<box>
<size>0.02 0.02 0.005</size>
</box>
</geometry>
<material>
<ambient>0 0 0 1</ambient>
<diffuse>0 0 0 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
</model>
</sdf>
至此 在 gazebo 中的 world 和 相机 模型 棋盘 模型 已 编辑 完成了
下面 写 好launch 文件 加载 以上内容
launch 文件 如下
<launch>
<!-- the following is a copy of empty_world.launch-->
<!-- these are the arguments you can pass this launch file, for example paused:=true -->
<arg name="paused" default="false"/>
<arg name="use_sim_time" default="true"/>
<arg name="extra_gazebo_args" default=""/>
<arg name="gui" default="true"/>
<arg name="headless" default="false"/>
<arg name="debug" default="false"/>
<arg name="physics" default="ode"/>
<arg name="verbose" default="false"/>
<arg name="world_name" value="$(find simple_camera_model)/simple_camera.world"/>
<!--arg name="world_name" default="worlds/empty.world"/--> <!-- Note: the world_name is with respect to GAZEBO_RESOURCE_PATH environmental variable -->
<!-- set use_sim_time flag -->
<group if="$(arg use_sim_time)">
<param name="/use_sim_time" value="true" />
</group>
<!-- set command arguments -->
<arg unless="$(arg paused)" name="command_arg1" value=""/>
<arg if="$(arg paused)" name="command_arg1" value="-u"/>
<arg unless="$(arg headless)" name="command_arg2" value=""/>
<arg if="$(arg headless)" name="command_arg2" value="-r"/>
<arg unless="$(arg verbose)" name="command_arg3" value=""/>
<arg if="$(arg verbose)" name="command_arg3" value="--verbose"/>
<arg unless="$(arg debug)" name="script_type" value="gzserver"/>
<arg if="$(arg debug)" name="script_type" value="debug"/>
<!-- start gazebo server-->
<node name="gazebo" pkg="gazebo_ros" type="$(arg script_type)" respawn="false" output="screen"
args="$(arg command_arg1) $(arg command_arg2) $(arg command_arg3) -e $(arg physics) $(arg extra_gazebo_args) $(arg world_name)" />
<!-- start gazebo client -->
<group if="$(arg gui)">
<node name="gazebo_gui" pkg="gazebo_ros" type="gzclient" respawn="false" output="screen"/>
</group>
<!-- here we add our own models to the simulation-->
<param name="robot_description" command="$(find xacro)/xacro.py '$(find simple_camera_model)/simple_camera_model.xacro'" />
<!-- Spawn a robot into Gazebo -->
<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model simple_camera" />
<node name="spawn_sdf" pkg="gazebo_ros" type="spawn_model" args="-file $(find exmpl_models)/small_checkerboard/small_checkerboard.sdf -sdf -model small_checkerboard -x -0.03 -y -0.03 -z 0.2" />
</launch>
启动 仿真 环境
启动 该 launch文件 出现如下 场景 一个 立方体（模拟相机） 悬在棋盘正上方
此时 相机发布 了 如下 信息
/simple_camera/camera_info 这里有一条 这个
查看 该 消息 内容 如下
之后 查查 每个 向量的 意思
初步猜测
D 是 5个 元素 和 畸变向量 对应
K 是 9个元素 可能 是 下面的那个矩阵
P 是 12 个元素 和内参矩阵对应
/simple_camera/image_raw 这个 是 相机 发布的 图像 信息 可以看下
rosrun image_view image_view image:=/simple_camera/image_raw
就是相机拍 着 的 棋盘
进行较准
下面的工作就是要运行校准程序
相机校准在ROS中主要运用 camera_calibration 功能包
这个功能包在装ROS的时候一般就装好了，不需要手动去安装。可以看
这个路径下 是否 有 cameracalibrator.py 这个文件
把这文件的代码粘在下面：
import cv2
import functools
import message_filters
import os
import rospy
from camera_calibration.camera_calibrator import OpenCVCalibrationNode
from camera_calibration.calibrator import ChessboardInfo, Patterns
from message_filters import ApproximateTimeSynchronizer
def main():
from optparse import OptionParser, OptionGroup
parser = OptionParser("%prog --size SIZE1 --square SQUARE1 [ --size SIZE2 --square SQUARE2 ]",
description=None)
parser.add_option("-c", "--camera_name",
type="string", default='narrow_stereo',
help="name of the camera to appear in the calibration file")
group = OptionGroup(parser, "Chessboard Options",
"You must specify one or more chessboards as pairs of --size and --square options.")
group.add_option("-p", "--pattern",
type="string", default="chessboard",
help="calibration pattern to detect - 'chessboard', 'circles', 'acircles'")
group.add_option("-s", "--size",
action="append", default=[],
help="chessboard size as NxM, counting interior corners (e.g. a standard chessboard is 7x7)")
group.add_option("-q", "--square",
action="append", default=[],
help="chessboard square size in meters")
parser.add_option_group(group)
group = OptionGroup(parser, "ROS Communication Options")
group.add_option("--approximate",
type="float", default=0.0,
help="allow specified slop (in seconds) when pairing images from unsynchronized stereo cameras")
group.add_option("--no-service-check",
action="store_false", dest="service_check", default=True,
help="disable check for set_camera_info services at startup")
parser.add_option_group(group)
group = OptionGroup(parser, "Calibration Optimizer Options")
group.add_option("--fix-principal-point",
action="store_true", default=False,
help="fix the principal point at the image center")
group.add_option("--fix-aspect-ratio",
action="store_true", default=False,
help="enforce focal lengths (fx, fy) are equal")
group.add_option("--zero-tangent-dist",
action="store_true", default=False,
help="set tangential distortion coefficients (p1, p2) to zero")
group.add_option("-k", "--k-coefficients",
type="int", default=2, metavar="NUM_COEFFS",
help="number of radial distortion coefficients to use (up to 6, default %default)")
group.add_option("--disable_calib_cb_fast_check", action='store_true', default=False,
help="uses the CALIB_CB_FAST_CHECK flag for findChessboardCorners")
parser.add_option_group(group)
options, args = parser.parse_args()
if len(options.size) != len(options.square):
parser.error("Number of size and square inputs must be the same!")
if not options.square:
options.square.append("0.108")
options.size.append("8x6")
boards = []
for (sz, sq) in zip(options.size, options.square):
size = tuple([int(c) for c in sz.split('x')])
boards.append(ChessboardInfo(size[0], size[1], float(sq)))
if options.approximate == 0.0:
sync = message_filters.TimeSynchronizer
else:
sync = functools.partial(ApproximateTimeSynchronizer, slop=options.approximate)
num_ks = options.k_coefficients
calib_flags = 0
if options.fix_principal_point:
calib_flags |= cv2.CALIB_FIX_PRINCIPAL_POINT
if options.fix_aspect_ratio:
calib_flags |= cv2.CALIB_FIX_ASPECT_RATIO
if options.zero_tangent_dist:
calib_flags |= cv2.CALIB_ZERO_TANGENT_DIST
if (num_ks > 3):
calib_flags |= cv2.CALIB_RATIONAL_MODEL
if (num_ks < 6):
calib_flags |= cv2.CALIB_FIX_K6
if (num_ks < 5):
calib_flags |= cv2.CALIB_FIX_K5
if (num_ks < 4):
calib_flags |= cv2.CALIB_FIX_K4
if (num_ks < 3):
calib_flags |= cv2.CALIB_FIX_K3
if (num_ks < 2):
calib_flags |= cv2.CALIB_FIX_K2
if (num_ks < 1):
calib_flags |= cv2.CALIB_FIX_K1
pattern = Patterns.Chessboard
if options.pattern == 'circles':
pattern = Patterns.Circles
elif options.pattern == 'acircles':
pattern = Patterns.ACircles
elif options.pattern != 'chessboard':
print('Unrecognized pattern %s, defaulting to chessboard' % options.pattern)
if options.disable_calib_cb_fast_check:
checkerboard_flags = 0
else:
checkerboard_flags = cv2.CALIB_CB_FAST_CHECK
rospy.init_node('cameracalibrator')
node = OpenCVCalibrationNode(boards, options.service_check, sync, calib_flags, pattern, options.camera_name,
checkerboard_flags=checkerboard_flags)
rospy.spin()
if __name__ == "__main__":
try:
main()
except Exception as e:
import traceback
traceback.print_exc()
wiki的介绍和 代码 最上面 参数 的介绍 一样，有一些必须要有的参数 设置
- 1、–size 棋盘的尺寸 注意 这个是 内部 角点的 个数 不是 格子数量 如上面我们建立的棋盘 横着 是4个黑4个白，所以是7个角点，所以竖着是 6个角点 顾 参数 设置应为 --size 7x6 也不要写成–size 7*6 识别不了
- 2、–p 较准目标 的 模式 默认是 棋盘，所以我们的就可以不设置了
- 3、–q 棋盘方格的尺寸 单位为m 顾 我们的应该为 --square 0.01
- 4、image:=/simple_camera/image_raw 指定 图像的 topic
- 5、camera:=/simple_camera 指定 相机 的 名称
运行校准程序
运行 这个 功能包 根据上面参数 设定， 具体指令如下：
rosrun camera_calibration cameracalibrator.py --size 7x6 --square 0.01 image:=/simple_camera/image_raw camera:=/simple_camera
出现新界面
终端显示
这说明采集到了一个 样本
棋盘变动位置及姿态
棋盘变位置和姿态后就会 自动 再采集 样本
让棋盘随机 移动 旋转 生成随机姿态，约束于保持在相机视野内。棋盘垂直，水平移动，并以任意倾斜角度倾斜。棋盘格姿势每次持续0.5秒。使用以下命令可以不断改变标定板的位置
功能代码如下
#include <ros/ros.h>
#include <gazebo_msgs/ModelState.h>
#include <geometry_msgs/Pose.h>
#include <gazebo_msgs/SetModelState.h>
#include <math.h>
#include <iostream>
#include <string>
using namespace std;
int main(int argc, char **argv) {
ros::init(argc, argv, "move_gazebo_model");
ros::NodeHandle n;
ros::ServiceClient client = n.serviceClient<gazebo_msgs::SetModelState>("/gazebo/set_model_state");
gazebo_msgs::SetModelState set_model_state_srv;
gazebo_msgs::ModelState des_model_state;
geometry_msgs::Twist twist;
int ans;
bool do_skew=false;
twist.linear.x = 0.0;
twist.linear.y = 0.0;
twist.linear.z = 0.0;
twist.angular.x = 0.0;
twist.angular.y = 0.0;
twist.angular.z = 0.0;
geometry_msgs::Pose pose;
geometry_msgs::Quaternion quat;
double x_bias = -0.03;
double y_bias = -0.03;
double z_bias = 0.2;
double x,y,z;
double dx = 0.1;
double dy = 0.1;
double dz = 0.15;
pose.position.x = x;
pose.position.y = y;
pose.position.z = z;
quat.x = 0.0;
quat.y = 0.0;
quat.z = 0.0;
quat.w = 1.0;
pose.orientation= quat;
des_model_state.model_name = "small_checkerboard";
des_model_state.pose = pose;
des_model_state.twist = twist;
des_model_state.reference_frame = "world";
double qx,qy,qz,qw;
//do random displacements and skews
//cout<<"do skews? (0,1): ";
//cin>>ans;
//if (ans) do_skew=true;
do_skew=true;
while(ros::ok()) {
qx = 0.2*(( (rand()%100)/100.0)-0.5);
qy = 0.2*(( (rand()%100)/100.0)-0.5);
qz = 0.2*(( (rand()%100)/100.0)-0.5);
qw = 0.5;
x = x_bias + dx*((rand()%100)/100.0-0.5);
y = y_bias + dy*((rand()%100)/100.0-0.5);
z = z_bias + dz*((rand()%100)/100.0-0.5);
double norm = sqrt(qx*qx+qy*qy+qz*qz+qw*qw);
quat.x = qx/norm;
quat.y = qy/norm;
quat.z = qz/norm;
quat.w = qw/norm;
cout<<"qx, qy, qz, qw= "<<quat.x<<", "<<quat.y<<", "<<quat.z<<", "<<quat.w<<endl;
cout<<"x,y,z = "<<x<<", "<<y<<", "<<z<<endl;
if(do_skew) pose.orientation= quat;
pose.position.x = x;
pose.position.y = y;
pose.position.z = z;
des_model_state.pose = pose;
set_model_state_srv.request.model_state = des_model_state;
client.call(set_model_state_srv);
ros::spinOnce();
//cout<<"enter 1 to advance, <1 to quit: ";
//cin>>ans;
//if (ans<1) return 0;
ros::Duration(0.5).sleep();
}
return 0;
}
运行这个功能包
rosrun example_camera_calibration move_calibration_checkerboard
校准结果
看 校准 那个 终端 样本就自动往上 增加了
这个 图像 和实际 一样 棋盘在不断变换 着 位置 和姿态 XY Size 进度 条也会 增加
当 CALIBRATE 这个按扭变成 这个 颜色 说明 样本 采集 够了
点击 CALIBRATE 按钮后 下面的SAVE 也会 变色
校准的终端也会输出 求得的 相机的 内参 和 畸变参数
点击 SAVE 后 会保存在 ~/.ros/camera_info中的 simple_camera.yaml 文件
在打印 相机此时相机的信息 会是 校准后的参数
扫一扫
2267
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
