Add lidar point cloud to visualization of sensor data

CMakeLists.txt

@@ -1,8 +1,9 @@
 cmake_minimum_required(VERSION 3.1)
 add_subdirectory(open-simulation-interface)
+file(COPY Resources DESTINATION .)
 project(osi-visualizer)
 
-set(CMAKE_CXX_STANDARD 11)
+set(CMAKE_CXX_STANDARD 14)
 
 set(CMAKE_INCLUDE_CURRENT_DIR ON)
 set(CMAKE_AUTOMOC ON)
@@ -46,6 +47,8 @@ set(SOURCES
     src/glfieldofview.cpp
     src/fmureceiver.cpp
     src/utils.cpp
+    src/glpointcloud.cpp
+    src/pointcloud.cpp
 )
 
 set(HEADERS
@@ -68,11 +71,13 @@ set(HEADERS
     include/appconfig.h
     include/global.h
     include/glpoint.h
+    include/glpointcloud.h
     include/gltrafficsign.h
     include/osiparser.h
     include/glfieldofview.h
     include/fmureceiver.h
     include/utils.h
+    include/pointcloud.h
 )
 
 

include/globject.h

@@ -34,7 +34,7 @@ class GLObject
 
         void SetText(QString text);
 
-        void SetTexture(QImage image, bool generateMipMaps);
+        void SetTexture(QImage image, bool generateMipMaps, GLint wrapping = GL_REPEAT);
 
         void SetColor(QColor color) { color_ = color; }
         QColor GetColor() const { return color_; }

include/glpointcloud.h

@@ -0,0 +1,9 @@
+#pragma once
+#include "globject.h"
+
+
+class GLPointCloud : public GLObject
+{
+public:
+	GLPointCloud(QOpenGLFunctions_4_3_Core* functions, const QString& srcPath);
+};

include/glwidget.h

@@ -12,6 +12,7 @@
 #include "camera.h"
 #include "appconfig.h"
 #include "lane.h"
+#include "pointcloud.h"
 #include "imessagesource.h"
 
 #include "glfieldofview.h"
@@ -58,7 +59,7 @@ class GLWidget : public QOpenGLWidget, protected QOpenGLFunctions_4_3_Core
         void Connected(DataType dataType);
         void TreeItemChanged(QTreeWidgetItem* item, int column);
         void TreeItemClicked(QTreeWidgetItem* item, int column);
-        void MessageParsed(const Message& message, const LaneMessage& laneMessage);
+        void MessageParsed(const Message& message, const LaneMessage& laneMessage, const PointMessage& pointMessage);
 
     protected:
         void paintGL();
@@ -85,6 +86,7 @@ class GLWidget : public QOpenGLWidget, protected QOpenGLFunctions_4_3_Core
         QPoint mousePos_;
         const AppConfig& config_;
         QVector<Lane*> lanes_;
+		PointCloud pointcloud_;
         QSet<int> pressedKeys_;
         QList<int> sceneKeys_;
         IMessageSource* msgSource_;

include/osiparser.h

@@ -27,7 +27,8 @@ class OsiParser : public QObject
 
         signals:
             void MessageParsed(const Message& message,
-                               const LaneMessage& laneMessage);
+                               const LaneMessage& laneMessage,
+							   const PointMessage& pointMessage);
             void EnableExport(bool enable);
             void SaveOSIMsgOverflow(int osiMsgSaveThreshold);
 
@@ -56,7 +57,8 @@ class OsiParser : public QObject
 
         void ParseSensorData(const osi3::SensorData& sensorData,
                              Message& objectMessage,
-                             LaneMessage& laneMessage);
+                             LaneMessage& laneMessage,
+							 PointMessage& pointMessage);
 
         void ParseSensorDataMovingObject(Message& objectMessage,
                                          const osi3::BaseMoving& baseObject,

include/pointcloud.h

@@ -0,0 +1,23 @@
+#pragma once
+#include "types.h"
+#include "glpointcloud.h"
+#include <QVector3D>
+#include <QOpenGLFunctions_4_3_Core>
+#include <memory>
+
+class PointCloud
+{
+public:
+	PointCloud(int id, QOpenGLFunctions_4_3_Core* functions, const QString& srcPath);
+
+	void UpdatePointCloud(const QVector<PointStruct>& pm);	// Creates new GLPointCloud object from the PointStruct-list
+
+	int pointcloudId_;
+	bool isVisible_;
+	std::unique_ptr<GLPointCloud> glPointCloud_;
+	QImage colorscheme;				// Color scheme of the point cloud. The line from texture coordinates (0,0) to (1,1) defines color(intensity)
+
+private:
+	QOpenGLFunctions_4_3_Core* functions_;
+	QString srcPath_;
+};

include/types.h

@@ -10,7 +10,7 @@
 #include "osi_common.pb.h"
 #include <QVector>
 #include <QVector3D>
-
+#include <QColor>
 
 #include <memory>
 
@@ -72,4 +72,9 @@ struct LaneStruct
 };
 using LaneMessage = QVector<LaneStruct>;
 
-
+struct PointStruct
+{
+	QVector3D position;		// Carthesian global coordinates of the point
+	float color;			// The color is a real number in [0,1]
+};
+using PointMessage = QVector<PointStruct>;

src/globject.cpp

@@ -150,7 +150,7 @@ GLObject::SetText(QString text)
 }
 
 void
-GLObject::SetTexture(QImage image, bool generateMipMaps)
+GLObject::SetTexture(QImage image, bool generateMipMaps, GLint wrapping)
 {
     if (textureId_ > -1)
     {
@@ -163,8 +163,8 @@ GLObject::SetTexture(QImage image, bool generateMipMaps)
     functions_->glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA,
                              image.width(), image.height(), 0,
                              GL_RGBA, GL_UNSIGNED_BYTE, image.bits());
-    functions_->glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT);
-    functions_->glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT);
+    functions_->glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, wrapping);
+    functions_->glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, wrapping);
 
     if (generateMipMaps)
     {

src/glpointcloud.cpp

@@ -0,0 +1,7 @@
+#include "glpointcloud.h"
+
+GLPointCloud::GLPointCloud(QOpenGLFunctions_4_3_Core* functions, const QString& srcPath)
+	: GLObject(GL_POINTS, functions, "", GL_STREAM_DRAW)
+{
+
+}

src/glwidget.cpp

@@ -1,5 +1,6 @@
 #include "glwidget.h"
 #include "glvehicle.h"
+#include "glpointcloud.h"
 #include "gltriangle.h"
 #include "glpoint.h"
 #include "global.h"
@@ -33,6 +34,7 @@ GLWidget::GLWidget(QWidget* parent,
     , mousePos_()
     , config_(config)
     , lanes_()
+	, pointcloud_(100, this, config_.srcPath_)
     , pressedKeys_()
     , sceneKeys_()
     , msgSource_(msgSource)
@@ -159,7 +161,7 @@ GLWidget::paintGL()
     //glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
 
     // Depth test is disabled, thus the static objects (i.e. grid) will be drawn first.
-    // Afterwards the lanes, then the vehicles etc. are drawn, the texts are the last ones.
+    // Afterwards the lanes, then the vehicles etc. are drawn, then point clouds and the texts are the last ones.
     foreach (GLObject* staticObject, staticObjects_)
     {
         if(staticObject->isVisible_)
@@ -190,6 +192,10 @@ GLWidget::paintGL()
             RenderObject(object);
     }
 
+	if (pointcloud_.isVisible_ && pointcloud_.glPointCloud_ != nullptr) {
+		RenderObject(pointcloud_.glPointCloud_.get());
+	}
+
     foreach (GLObject* object, simulationObjects_)
     {
         if (object->GetTextObject() != nullptr && object->GetTextObject()->isVisible_)
@@ -231,7 +237,6 @@ GLWidget::RenderObject(GLObject* object)
         }
         shaderProgram_.setUniformValue(uniformColorLocation_, color);
     }
-
     glBindVertexArray(object->vaoId_);
     glDrawArrays(object->GetPrimitiveType(), 0, object->vertices_.size());
 }
@@ -408,7 +413,8 @@ GLWidget::ResetObjectTextOrientations()
 
 void
 GLWidget::MessageParsed(const Message& message,
-                        const LaneMessage& laneMessage)
+                        const LaneMessage& laneMessage,
+						const PointMessage& pointMessage)
 {
     // The check for a connected receiver is necessary because the signals for received messages
     // the receiver emits are asynchronous. It sometimes happens that after the receiver disconnected,
@@ -594,6 +600,12 @@ GLWidget::MessageParsed(const Message& message,
         }
     }
 
+	// copy point clouds
+	if (pointMessage.size() > 0) {
+		pointcloud_.UpdatePointCloud(pointMessage);
+	}
+
+
     // TODO: Remove ununsed objects from object tree, also stop tracking etc
     // Hide unused objects
     foreach (GLObject* object, objectControlList)

src/main.cpp

@@ -9,6 +9,7 @@
 Q_DECLARE_METATYPE(DataType)
 Q_DECLARE_METATYPE(Message)
 Q_DECLARE_METATYPE(LaneMessage)
+Q_DECLARE_METATYPE(PointMessage)
 Q_DECLARE_METATYPE(osi3::SensorData)
 Q_DECLARE_METATYPE(osi3::SensorView)
 
@@ -17,6 +18,7 @@ int main(int argc, char *argv[])
 {
     qRegisterMetaType<Message>();
     qRegisterMetaType<LaneMessage>();
+	qRegisterMetaType<PointMessage>();
     qRegisterMetaType<DataType>();
     qRegisterMetaType<osi3::SensorData>();
     qRegisterMetaType<osi3::SensorView>();