如何使用OpenGL画出ROS rviz那样的点云可视化效果

【请尊重原创！转载和引用文章内容务必注明出处！未经许可上传到某文库或其他收费阅读/下载网站赚钱的必追究责任！】

ROS rviz可以将点云以多种形式渲染出来比较漂亮，尤其是根据intensity渲染点云不同的色彩和亮度的功能比较好， 比PCL和mayavi等提供的可视化API画的点云效果好看多了，缺点是你想在既有rviz工具里对点云效果增加功能处理或渲染效果的话不得不去读rviz很多的代码后去做修改，如果一知半解就会改出问题来，如果自己实现用OpenGL画点云的代码，想怎么改都行，但是很难找得到有资料介绍rviz里点云的效果(主要是比较好看的rainbow)到底是怎么画出来的，于是花了些空余时间先琢磨用集成到Qt里的OpenGL简单的用一种颜色画出点云(这时看到的是类似PCL或mayavi画点云的效果，比较单一呆板，无法表现出和intensity有关的颜色和亮度的不同)，然后读rviz的源码找到了画rainbow效果的实现代码移植到点云的点着色程序中，解决掉在移植过程中出现的不符合GLSL语言语法规则的错误一些错误后(例如GLSL对数据类型转换有较C++更强的要求)，画出来的效果和rviz一样了。

这里列出一些主要的关键代码，下面是点云数据准备和渲染的主要程序：

class opengl_widget : public QOpenGLWidget, QOpenGLFunctions_4_5_Core
{
	Q_OBJECT

    public:
	   opengl_widget(QWidget* parent);
	   ~opengl_widget();

    protected:
	   void initializeGL();
	   void paintGL();
	   void resizeGL(int width, int height);
    
    private:
       QOpenGLShaderProgram glsp_point;
       unsigned int pointVBO;
       unsigned int pointVAO;
       unsigned int vertexCount;
       unsigned int pointCount;
       std::vector pointData;
       float max_intensity;
       float min_intensity;

    ...

...
   unsigned int opengl_widget::preparePoints(std::vector& pointData)
   {
      glGenVertexArrays(1, &pointVAO);
      glBindVertexArray(pointVAO);
      glGenBuffers(1, &pointVBO);
      glBindBuffer(GL_ARRAY_BUFFER, pointVBO);
      glBufferData(GL_ARRAY_BUFFER, pointData.size() * sizeof(float), &pointData[0], GL_STATIC_DRAW);
      glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 4 * sizeof(float), (void*)0);
      glEnableVertexAttribArray(0);
      glVertexAttribPointer(1, 1, GL_FLOAT, GL_FALSE, 4 * sizeof(float), (void*)(3 * sizeof(float)));
      glEnableVertexAttribArray(1);
      glBindBuffer(GL_ARRAY_BUFFER, 0);
      glBindVertexArray(0);
      return pointData.size() / 4;
   }
   opengl_widget::opengl_widget(QWidget* parent)
    : QOpenGLWidget(parent)
   {
      ...
      pointData = read_bag(bag_path, max_intensity, min_intensity);
   }
   void opengl_widget::initializeGL()
   {
      initializeOpenGLFunctions();
      ...
      glsp_point.addShaderFromSourceFile(QOpenGLShader::Fragment, "glsp_point.fs");
      glsp_point.addShaderFromSourceFile(QOpenGLShader::Vertex, "glsp_point.vs");
      glsp_point.link();
      pointCount = preparePoints(pointData);
   }
   void opengl_widget::paintGL()
   {
      ...
      glsp_point.bind();
      ...
      glsp_point.setUniformValue("max_intensity", max_intensity);
      glsp_point.setUniformValue("min_intensity", min_intensity);
      glBindVertexArray(pointVAO);
      glPointSize(2.0f);
      glDrawArrays(GL_POINTS, 0, pointCount);
      ...
   }

下面分别是点云vertex和fragment着色程序，是画出rainbow效果的关键：

#version 450 core

layout (location = 0) in vec3 pos;
layout (location = 1) in float intensity;

uniform mat4 model;
uniform mat4 view;
uniform mat4 projection;
uniform float max_intensity;
uniform float min_intensity;

out vec3 vtxColor;

vec3 getRainbowColor(float value)
{
  // this is HSV color palette with hue values going only from 0.0 to 0.833333.
  vec3 color = vec3(0.0f,0.0f,0.0f);
  value = min(value, 1.0f);
  value = max(value, 0.0f);

  float h = value * 5.0f + 1.0f;
  int i = int(floor(h));
  float f = h - i;
  if (!bool(i & 1))
    f = 1 - f; // if i is even
  float n = 1.0f - f;

  if (i <= 1)
    color[0] = n, color[1] = 0, color[2] = 1;
  else if (i == 2)
    color[0] = 0, color[1] = n, color[2] = 1;
  else if (i == 3)
    color[0] = 0, color[1] = 1, color[2] = n;
  else if (i == 4)
    color[0] = n, color[1] = 1, color[2] = 0;
  else if (i >= 5)
    color[0] = 1, color[1] = n, color[2] = 0;
  return color;
}

void main()
{
    gl_Postion = projection * view * model * vec4(pos, 1.0);
    float diff_intensity = max_intensity - min_intensity;
    float normalized_intensity = (intensity - min_intensity) / diff_intensity;
    float value = 1.0 - normalized_intensity;  // use rainbow
    vtxColor = getRainbowColor(value);
}

上面的根据点云的intensity值计算rainbow色彩的计算算法是从rviz的代码修改来的，作归一化用的max_intensity和min_intensity是当前帧点云的最大/最小强度值，是通过uniform变量传入的。每个点云对应的vertex的xyz和intensity值的传入格式是在preparePoints()里定义的。

#version 450 core

out vec4 fragColor;

in vec3 vtxColor;

void main()
{
    fragColor = vec4(vtxColor, 1.0f);
}

画出来的效果是这样：

效果和rviz里一样了，在增加些针对鼠标事件触发的对点云图的缩放和翻转之类的控制，实现

opengl_widget::mouseMoveEvent()、opengl_widget::mousePressEvent()、opengl_widget::mouseReleaseEvent()、opengl_widget::wheelEvent()等事件函数，得到的最终效果就和rviz里的效果一样了，而且自己的代码实现可以很方便进一步增加更复杂的处理功能或效果展示。