相机标定原理 用ROS camera_calibration 功能包 在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 功能包

camera_calibration ROS WIKI

这个功能包在装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 文件

在打印 相机此时相机的信息 会是 校准后的参数

至此校准完成

评论 19
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

月照银海似蛟龙

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值