ros noetic elas rviz 深度点云不显示或只显示一个点的解决方法

问题描述：

Ubuntu 20.04.6 LTS， ROS Noetic
gazebo中使用<plugin name="stereo_camera_controller" filename="libgazebo_ros_camera.so">创建双目相机，使用elas_ros库或者stereo_image_proc库计算深度图，rviz中订阅PointCloud2后，只显示一个很容易忽略的点（如图）

有问题的双目相机gazebo配置如下:

  <gazebo reference="camera_link">
    <sensor type="multicamera" name="stereo_camera">
      <update_rate>10.0</update_rate>
      <always_on>true</always_on>
      <camera name="left">
        <pose>0 0 0 0 0 0</pose>
        <horizontal_fov>1.3962634</horizontal_fov>
        <image>
          <width>640</width>
          <height>480</height>
          <format>R8G8B8</format>
        </image>
        <clip>
          <near>0.02</near>
          <far>300</far>
        </clip>
        <noise>
            <type>gaussian</type>
            <mean>0.0</mean>
            <stddev>0.000</stddev>
        </noise>
      </camera>

      <camera name="right">
        <pose>0 -0.12 0 0 0 0</pose>
        <horizontal_fov>1.3962634</horizontal_fov>
        <image>
          <width>640</width>
          <height>480</height>
          <format>R8G8B8</format>
        </image>
        <clip>
          <near>0.02</near>
          <far>300</far>
        </clip>
        <noise>
            <type>gaussian</type>
            <mean>0.0</mean>
            <stddev>0.000</stddev>
        </noise>
      </camera>

        <plugin name="stereo_camera_controller" filename="libgazebo_ros_multicamera.so">
          <alwaysOn>true</alwaysOn>
          <updateRate>10</updateRate>
          <cameraName>stereo</cameraName>
          <imageTopicName>image_raw</imageTopicName>
          <cameraInfoTopicName>camera_info</cameraInfoTopicName>
          <frameName>camera_link_optical</frameName>
          <baseline>0.12</baseline>
          <distortion_k1>0.0</distortion_k1>
          <distortion_k2>0.0</distortion_k2>
          <distortion_k3>0.0</distortion_k3>
          <distortion_t1>0.0</distortion_t1>
          <distortion_t2>0.0</distortion_t2>
        </plugin>
    </sensor>
  </gazebo>

---

原因分析：

1. 订阅视差图（/elas/image_disparity），发现可以正常显示（如图）
   

2. 修改elas.cpp[url]代码，打印各变量的值，发现l_width、l_height、left_uv.x、left_uv.y、l_disp的值正常，point.xyz的值全部为0。

point的计算代码如下：

image_geometry::StereoCameraModel model;
model.fromCameraInfo(*l_info_msg, *r_info_msg);
...
cv::Point3d point;
model.projectDisparityTo3d(left_uv, l_disp_data[index], point);

由1、2步得出结论，点云不工作的原因是projectDisparityTo3d函数没有正确地将视差图投影回3d空间中，于是开始怀疑是相机基线设置错误。

3. 根据projectDisparityTo3d函数的实现[url]，里面用到了一个Q矩阵，其由基线baseline等参数计算得到，baseline来源于CameraInfo.

4. 使用rostopic echo /stereo/right/camera_info查看CameraInfo数据：
   

5. 查看CameraInfo[url]的定义，其中

# Projection/camera matrix
#     [fx'  0  cx' Tx]
# P = [ 0  fy' cy' Ty]
#     [ 0   0   1   0]

The first camera always has Tx = Ty = 0. For the right (second) camera of a horizontal stereo pair, Ty = 0 and Tx = -fx’ * B, where B is the baseline between the cameras.

根据描述，Tx不等于0，但是在截图中Tx（P的第4个元素）等于-0.0 ，故推断出B=0，即相机基线没有成功设置。因此，在noetic版本中，以<baseline>0.12</baseline>这样的方式设置相机基线，已经失效。

6. 在libgazebo_ros_camera的实现代码[url]中，以baseline为关键词进行搜索[url]，发现了官方的双目摄像头配置文件[url]，与上述配置文件的区别是，它使用<hackBaseline>0.07</hackBaseline>标签来指定相机基线。

7. 搜索发现，现版本没有关于读取<baseline>的代码，而读取<hackBaseline>的代码，在[url]中。

    if (this->camera[i]->Name().find("left") != std::string::npos)
    {
      // FIXME: hardcoded, left hack_baseline_ 0
      util->Load(_parent, _sdf, "/left", 0.0);
    }
    else if (this->camera[i]->Name().find("right") != std::string::npos)
    {
      double hackBaseline = 0.0;
      if (_sdf->HasElement("hackBaseline"))
        hackBaseline = _sdf->Get<double>("hackBaseline");
      util->Load(_parent, _sdf, "/right", hackBaseline);
    }

---

解决方案：

将<baseline>替换为<hackBaseline>，catkin_make后重新运行，深度点云图已经正常（如图）

附正确的双目相机配置

比上述配置只改了<hackBaseline>标签

<gazebo reference="camera_link">
    <sensor type="multicamera" name="stereo_camera">
      <update_rate>10.0</update_rate>
      <always_on>true</always_on>
      <camera name="left">
        <pose>0 0 0 0 0 0</pose>
        <horizontal_fov>1.3962634</horizontal_fov>
        <image>
          <width>640</width>
          <height>480</height>
          <format>R8G8B8</format>
        </image>
        <clip>
          <near>0.02</near>
          <far>300</far>
        </clip>
        <noise>
            <type>gaussian</type>
            <mean>0.0</mean>
            <stddev>0.000</stddev>
        </noise>
      </camera>

      <camera name="right">
        <pose>0 -0.12 0 0 0 0</pose>
        <horizontal_fov>1.3962634</horizontal_fov>
        <image>
          <width>640</width>
          <height>480</height>
          <format>R8G8B8</format>
        </image>
        <clip>
          <near>0.02</near>
          <far>300</far>
        </clip>
        <noise>
            <type>gaussian</type>
            <mean>0.0</mean>
            <stddev>0.000</stddev>
        </noise>
      </camera>

        <plugin name="stereo_camera_controller" filename="libgazebo_ros_multicamera.so">
          <alwaysOn>true</alwaysOn>
          <updateRate>10</updateRate>
          <cameraName>stereo</cameraName>
          <imageTopicName>image_raw</imageTopicName>
          <cameraInfoTopicName>camera_info</cameraInfoTopicName>
          <frameName>camera_link_optical</frameName>
          <!-- <baseline>0.12</baseline> -->
          <hackBaseline>0.12</hackBaseline>
          <distortion_k1>0.0</distortion_k1>
          <distortion_k2>0.0</distortion_k2>
          <distortion_k3>0.0</distortion_k3>
          <distortion_t1>0.0</distortion_t1>
          <distortion_t2>0.0</distortion_t2>
        </plugin>
    </sensor>
  </gazebo>