diff --git a/CMakeLists.txt b/CMakeLists.txt index 1522d91..3bfceec 100644 --- a/CMakeLists.txt +++ b/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 ) diff --git a/Resources/Images/PointCloudColors.png b/Resources/Images/PointCloudColors.png new file mode 100644 index 0000000..c6608b5 Binary files /dev/null and b/Resources/Images/PointCloudColors.png differ diff --git a/include/globject.h b/include/globject.h index d5ea124..edfbadf 100644 --- a/include/globject.h +++ b/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_; } diff --git a/include/glpointcloud.h b/include/glpointcloud.h new file mode 100644 index 0000000..bc9bab4 --- /dev/null +++ b/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); +}; diff --git a/include/glwidget.h b/include/glwidget.h index f4de771..1c41b42 100644 --- a/include/glwidget.h +++ b/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 lanes_; + PointCloud pointcloud_; QSet pressedKeys_; QList sceneKeys_; IMessageSource* msgSource_; diff --git a/include/osiparser.h b/include/osiparser.h index 4686166..1317678 100644 --- a/include/osiparser.h +++ b/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, diff --git a/include/pointcloud.h b/include/pointcloud.h new file mode 100644 index 0000000..708a266 --- /dev/null +++ b/include/pointcloud.h @@ -0,0 +1,23 @@ +#pragma once +#include "types.h" +#include "glpointcloud.h" +#include +#include +#include + +class PointCloud +{ +public: + PointCloud(int id, QOpenGLFunctions_4_3_Core* functions, const QString& srcPath); + + void UpdatePointCloud(const QVector& pm); // Creates new GLPointCloud object from the PointStruct-list + + int pointcloudId_; + bool isVisible_; + std::unique_ptr 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_; +}; diff --git a/include/types.h b/include/types.h index e96f28b..95bc8e2 100644 --- a/include/types.h +++ b/include/types.h @@ -10,7 +10,7 @@ #include "osi_common.pb.h" #include #include - +#include #include @@ -72,4 +72,9 @@ struct LaneStruct }; using LaneMessage = QVector; - +struct PointStruct +{ + QVector3D position; // Carthesian global coordinates of the point + float color; // The color is a real number in [0,1] +}; +using PointMessage = QVector; \ No newline at end of file diff --git a/src/globject.cpp b/src/globject.cpp index 14d022f..5c89c4f 100644 --- a/src/globject.cpp +++ b/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) { diff --git a/src/glpointcloud.cpp b/src/glpointcloud.cpp new file mode 100644 index 0000000..82cda0f --- /dev/null +++ b/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) +{ + +} diff --git a/src/glwidget.cpp b/src/glwidget.cpp index 5a2e2dc..f574b0c 100644 --- a/src/glwidget.cpp +++ b/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) diff --git a/src/main.cpp b/src/main.cpp index b11f224..c69502e 100644 --- a/src/main.cpp +++ b/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(); qRegisterMetaType(); + qRegisterMetaType(); qRegisterMetaType(); qRegisterMetaType(); qRegisterMetaType(); diff --git a/src/osiparser.cpp b/src/osiparser.cpp index e63eeb9..256d3f7 100644 --- a/src/osiparser.cpp +++ b/src/osiparser.cpp @@ -7,20 +7,20 @@ #include /* - * - * This function is called when a new zmq message is received - * and successfully parsed into SensorData. - * According to the desired data type, the received osi message will be parsed - * and a signal with parsed data (objects and lanes) will be emitted. - * - */ +* +* This function is called when a new zmq message is received +* and successfully parsed into SensorData. +* According to the desired data type, the received osi message will be parsed +* and a signal with parsed data (objects and lanes) will be emitted. +* +*/ OsiParser::OsiParser(const AppConfig& config) - : isFirstMessage_(true) - , startSaveOSIMsg_(false) - , osiMsgString_("") - , osiMsgNumber_(0) - , config_(config) + : isFirstMessage_(true) + , startSaveOSIMsg_(false) + , osiMsgString_("") + , osiMsgNumber_(0) + , config_(config) { } @@ -28,702 +28,721 @@ OsiParser::OsiParser(const AppConfig& config) void OsiParser::CancelSaveOSIMessage() { - isFirstMessage_ = true; - startSaveOSIMsg_ = false; - osiMsgNumber_ = 0; - osiMsgString_.clear(); - emit EnableExport(false); + isFirstMessage_ = true; + startSaveOSIMsg_ = false; + osiMsgNumber_ = 0; + osiMsgString_.clear(); + emit EnableExport(false); } void OsiParser::ParseReceivedSDMessage(const osi3::SensorData& sd) { - LocalMonitor(sd); + LocalMonitor(sd); - Message objectMessage; - LaneMessage laneMessage; + Message objectMessage; + LaneMessage laneMessage; + PointMessage pointMessage; - ParseSensorData(sd, objectMessage, laneMessage); + ParseSensorData(sd, objectMessage, laneMessage, pointMessage); - emit MessageParsed(objectMessage, laneMessage); + emit MessageParsed(objectMessage, laneMessage, pointMessage); } void OsiParser::ParseReceivedSVMessage(const osi3::SensorView& sv) { - LocalMonitor(sv); + LocalMonitor(sv); - Message objectMessage; - LaneMessage laneMessage; + Message objectMessage; + LaneMessage laneMessage; + PointMessage pointMessage; - const osi3::GroundTruth& groundTruth = sv.global_ground_truth(); - ParseGroundtruth(groundTruth, objectMessage, laneMessage); + const osi3::GroundTruth& groundTruth = sv.global_ground_truth(); + ParseGroundtruth(groundTruth, objectMessage, laneMessage); - emit MessageParsed(objectMessage, laneMessage); + emit MessageParsed(objectMessage, laneMessage, pointMessage); } template void OsiParser::LocalMonitor(const T& data) { - if (isFirstMessage_) - { - isFirstMessage_ = false; - emit EnableExport(true); - } - else if(startSaveOSIMsg_ && osiMsgNumber_ < config_.osiMsgSaveThreshold_) - { - ++osiMsgNumber_; - - osiMsgString_ += data.SerializeAsString(); - osiMsgString_ += "$$__$$"; - } - else if(osiMsgNumber_ >= config_.osiMsgSaveThreshold_) - { - emit SaveOSIMsgOverflow(config_.osiMsgSaveThreshold_); - } + if (isFirstMessage_) + { + isFirstMessage_ = false; + emit EnableExport(true); + } + else if (startSaveOSIMsg_ && osiMsgNumber_ < config_.osiMsgSaveThreshold_) + { + ++osiMsgNumber_; + + osiMsgString_ += data.SerializeAsString(); + osiMsgString_ += "$$__$$"; + } + else if (osiMsgNumber_ >= config_.osiMsgSaveThreshold_) + { + emit SaveOSIMsgOverflow(config_.osiMsgSaveThreshold_); + } } //Parse Ground Truth Data void OsiParser::ParseGroundtruth(const osi3::GroundTruth& groundTruth, - Message& objectMessage, - LaneMessage& laneMessage) + Message& objectMessage, + LaneMessage& laneMessage) { - for (int i = 0; i < groundTruth.stationary_object_size(); ++i) - { - const osi3::StationaryObject& object = groundTruth.stationary_object(i); - ObjectType objectType = GetObjectTypeFromOsiObjectType(object.classification().type()); - QString idStr = QString::number(object.id().value()); - ParseGroundtruthStationaryObject(objectMessage, object.base(), objectType, idStr); - } - - for (int i = 0; i < groundTruth.moving_object_size(); ++i) - { - const osi3::MovingObject& object = groundTruth.moving_object(i); - ObjectType objectType = GetObjectTypeFromOsiObjectType(object.vehicle_classification().type()); - QString idStr = QString::number(object.id().value()); - ParseGroundtruthMovingObject(objectMessage, object.base(), objectType, idStr); - } - - for (int i = 0; i < groundTruth.traffic_sign_size(); ++i) - { - const osi3::TrafficSign& sign = groundTruth.traffic_sign(i); - QString idStr = QString::number(sign.id().value()); - ParseGroundtruthStationaryObject(objectMessage, sign.main_sign().base(), ObjectType::TrafficSign, idStr); - } - - for (int i = 0; i < groundTruth.traffic_light_size(); ++i) - { - const osi3::TrafficLight& light = groundTruth.traffic_light(i); - QString idStr = QString::number(light.id().value()); - ParseGroundtruthStationaryObject(objectMessage, light.base(), ObjectType::TrafficLight, idStr); - } - - for (int i = 0; i < groundTruth.road_marking_size(); ++i) - { - const osi3::RoadMarking& roadM = groundTruth.road_marking(i); - QString idStr = QString::number(roadM.id().value()); - ParseGroundtruthStationaryObject(objectMessage, roadM.base(), ObjectType::OtherObject, idStr); - } - - if(config_.laneType_ == LaneType::CenterLanes) - { - // DRAW_CENTER_LINES - for (int i = 0; i < groundTruth.lane_size(); ++i) - { - const osi3::Lane& lane = groundTruth.lane(i); - LaneStruct tmpLane; - tmpLane.id = lane.id().value(); - - QVector centerLines; - for (int a = 0; a < lane.classification().centerline_size(); ++a) - { - const osi3::Vector3d& centerLine = lane.classification().centerline(a); - centerLines.append(QVector3D(centerLine.y(), 0, centerLine.x())); - } - - tmpLane.centerLanes.append(centerLines); - - laneMessage.append(tmpLane); - } - } - else - { - // DRAW_LANE_BOUNDARIES - for (int b = 0; b < groundTruth.lane_boundary_size(); ++b) - { - const osi3::LaneBoundary& laneBoundary = groundTruth.lane_boundary(b); - LaneStruct tmpLane; - tmpLane.id = laneBoundary.id().value(); - - QVector boundaryLines; - for (int c = 0; c < laneBoundary.boundary_line_size(); ++c) - { - const osi3::LaneBoundary_BoundaryPoint& boundaryPoint = laneBoundary.boundary_line(c); - const osi3::Vector3d& position = boundaryPoint.position(); - boundaryLines.append(QVector3D(position.y(), 0, position.x())); - } - - tmpLane.boundaryLanes.append(boundaryLines); - - laneMessage.append(tmpLane); - } - } + for (int i = 0; i < groundTruth.stationary_object_size(); ++i) + { + const osi3::StationaryObject& object = groundTruth.stationary_object(i); + ObjectType objectType = GetObjectTypeFromOsiObjectType(object.classification().type()); + QString idStr = QString::number(object.id().value()); + ParseGroundtruthStationaryObject(objectMessage, object.base(), objectType, idStr); + } + + for (int i = 0; i < groundTruth.moving_object_size(); ++i) + { + const osi3::MovingObject& object = groundTruth.moving_object(i); + ObjectType objectType = GetObjectTypeFromOsiObjectType(object.vehicle_classification().type()); + QString idStr = QString::number(object.id().value()); + ParseGroundtruthMovingObject(objectMessage, object.base(), objectType, idStr); + } + + for (int i = 0; i < groundTruth.traffic_sign_size(); ++i) + { + const osi3::TrafficSign& sign = groundTruth.traffic_sign(i); + QString idStr = QString::number(sign.id().value()); + ParseGroundtruthStationaryObject(objectMessage, sign.main_sign().base(), ObjectType::TrafficSign, idStr); + } + + for (int i = 0; i < groundTruth.traffic_light_size(); ++i) + { + const osi3::TrafficLight& light = groundTruth.traffic_light(i); + QString idStr = QString::number(light.id().value()); + ParseGroundtruthStationaryObject(objectMessage, light.base(), ObjectType::TrafficLight, idStr); + } + + for (int i = 0; i < groundTruth.road_marking_size(); ++i) + { + const osi3::RoadMarking& roadM = groundTruth.road_marking(i); + QString idStr = QString::number(roadM.id().value()); + ParseGroundtruthStationaryObject(objectMessage, roadM.base(), ObjectType::OtherObject, idStr); + } + + if (config_.laneType_ == LaneType::CenterLanes) + { + // DRAW_CENTER_LINES + for (int i = 0; i < groundTruth.lane_size(); ++i) + { + const osi3::Lane& lane = groundTruth.lane(i); + LaneStruct tmpLane; + tmpLane.id = lane.id().value(); + + QVector centerLines; + for (int a = 0; a < lane.classification().centerline_size(); ++a) + { + const osi3::Vector3d& centerLine = lane.classification().centerline(a); + centerLines.append(QVector3D(centerLine.y(), 0, centerLine.x())); + } + + tmpLane.centerLanes.append(centerLines); + + laneMessage.append(tmpLane); + } + } + else + { + // DRAW_LANE_BOUNDARIES + for (int b = 0; b < groundTruth.lane_boundary_size(); ++b) + { + const osi3::LaneBoundary& laneBoundary = groundTruth.lane_boundary(b); + LaneStruct tmpLane; + tmpLane.id = laneBoundary.id().value(); + + QVector boundaryLines; + for (int c = 0; c < laneBoundary.boundary_line_size(); ++c) + { + const osi3::LaneBoundary_BoundaryPoint& boundaryPoint = laneBoundary.boundary_line(c); + const osi3::Vector3d& position = boundaryPoint.position(); + boundaryLines.append(QVector3D(position.y(), 0, position.x())); + } + + tmpLane.boundaryLanes.append(boundaryLines); + + laneMessage.append(tmpLane); + } + } } // Parse a moving ground truth object void OsiParser::ParseGroundtruthMovingObject(Message& objectMessage, - const osi3::BaseMoving& baseObject, - const ObjectType objectType, - const QString& idStr) + const osi3::BaseMoving& baseObject, + const ObjectType objectType, + const QString& idStr) { - if (objectType == ObjectType::None) - { - return; - } - - osi3::Vector3d position = baseObject.position(); - osi3::Vector3d velocity = baseObject.velocity(); - osi3::Vector3d acceleration = baseObject.acceleration(); - float orientation = baseObject.orientation().yaw(); - osi3::Dimension3d dimension = baseObject.dimension(); - - if (dimension.width() == 0 && dimension.length() == 0) - { - dimension.set_width(1.0f); - dimension.set_length(1.0f); - } - - MessageStruct tmpMsg; - tmpMsg.id = Global::GetObjectTypeName(objectType) + idStr; - tmpMsg.name = "ID: " + idStr; - tmpMsg.type = objectType; - tmpMsg.orientation = orientation+M_PI_2; - tmpMsg.position = QVector3D(position.y(), 0, position.x()); - tmpMsg.realPosition = QVector3D(position.x(), position.y(), position.z()); - tmpMsg.velocitie = QVector3D(velocity.x(), velocity.y(), velocity.z()); - tmpMsg.acceleration = QVector3D(acceleration.x(), acceleration.y(), acceleration.z()); - tmpMsg.dimension = dimension; - - objectMessage.append(tmpMsg); + if (objectType == ObjectType::None) + { + return; + } + + osi3::Vector3d position = baseObject.position(); + osi3::Vector3d velocity = baseObject.velocity(); + osi3::Vector3d acceleration = baseObject.acceleration(); + float orientation = baseObject.orientation().yaw(); + osi3::Dimension3d dimension = baseObject.dimension(); + + if (dimension.width() == 0 && dimension.length() == 0) + { + dimension.set_width(1.0f); + dimension.set_length(1.0f); + } + + MessageStruct tmpMsg; + tmpMsg.id = Global::GetObjectTypeName(objectType) + idStr; + tmpMsg.name = "ID: " + idStr; + tmpMsg.type = objectType; + tmpMsg.orientation = orientation + M_PI_2; + tmpMsg.position = QVector3D(position.y(), 0, position.x()); + tmpMsg.realPosition = QVector3D(position.x(), position.y(), position.z()); + tmpMsg.velocitie = QVector3D(velocity.x(), velocity.y(), velocity.z()); + tmpMsg.acceleration = QVector3D(acceleration.x(), acceleration.y(), acceleration.z()); + tmpMsg.dimension = dimension; + + objectMessage.append(tmpMsg); } // Parse a stationary ground truth object void OsiParser::ParseGroundtruthStationaryObject(Message& objectMessage, - const osi3::BaseStationary& baseObject, - const ObjectType objectType, - const QString& idStr) + const osi3::BaseStationary& baseObject, + const ObjectType objectType, + const QString& idStr) { - if (objectType == ObjectType::None) - { - return; - } - - osi3::Vector3d tmp; - tmp.set_x(0.0); - tmp.set_y(0.0); - tmp.set_z(0.0); - - osi3::Vector3d position = baseObject.position(); - osi3::Vector3d velocity = tmp; - osi3::Vector3d acceleration = tmp; - float orientation = baseObject.orientation().yaw(); - osi3::Dimension3d dimension = baseObject.dimension(); - - if (dimension.width() == 0 && dimension.length() == 0) - { - dimension.set_width(1.0f); - dimension.set_length(1.0f); - } - - MessageStruct tmpMsg; - tmpMsg.id = Global::GetObjectTypeName(objectType) + idStr; - tmpMsg.name = "ID: " + idStr; - tmpMsg.type = objectType; - tmpMsg.orientation = orientation+M_PI_2; - tmpMsg.position = QVector3D(position.y(), 0, position.x()); - tmpMsg.realPosition = QVector3D(position.x(), position.y(), position.z()); - tmpMsg.velocitie = QVector3D(velocity.x(), velocity.y(), velocity.z()); - tmpMsg.acceleration = QVector3D(acceleration.x(), acceleration.y(), acceleration.z()); - tmpMsg.dimension = dimension; - - objectMessage.append(tmpMsg); + if (objectType == ObjectType::None) + { + return; + } + + osi3::Vector3d tmp; + tmp.set_x(0.0); + tmp.set_y(0.0); + tmp.set_z(0.0); + + osi3::Vector3d position = baseObject.position(); + osi3::Vector3d velocity = tmp; + osi3::Vector3d acceleration = tmp; + float orientation = baseObject.orientation().yaw(); + osi3::Dimension3d dimension = baseObject.dimension(); + + if (dimension.width() == 0 && dimension.length() == 0) + { + dimension.set_width(1.0f); + dimension.set_length(1.0f); + } + + MessageStruct tmpMsg; + tmpMsg.id = Global::GetObjectTypeName(objectType) + idStr; + tmpMsg.name = "ID: " + idStr; + tmpMsg.type = objectType; + tmpMsg.orientation = orientation + M_PI_2; + tmpMsg.position = QVector3D(position.y(), 0, position.x()); + tmpMsg.realPosition = QVector3D(position.x(), position.y(), position.z()); + tmpMsg.velocitie = QVector3D(velocity.x(), velocity.y(), velocity.z()); + tmpMsg.acceleration = QVector3D(acceleration.x(), acceleration.y(), acceleration.z()); + tmpMsg.dimension = dimension; + + objectMessage.append(tmpMsg); } // Parse SensorData void OsiParser::ParseSensorData(const osi3::SensorData &sensorData, - Message& objectMessage, - LaneMessage& laneMessage) + Message& objectMessage, + LaneMessage& laneMessage, + PointMessage& pointMessage) { - for (int i = 0; i < sensorData.stationary_object_size(); ++i) - { - const osi3::DetectedStationaryObject& dectObj = sensorData.stationary_object(i); - double highestProbability = 0; - int highestIndex = -1; - for(int j = 0; j < dectObj.candidate().size(); ++j) - { - const osi3::DetectedStationaryObject_CandidateStationaryObject candi = dectObj.candidate(j); - if(candi.probability() > highestProbability) - { - highestProbability = candi.probability(); - highestIndex = j; - } - } - - if(highestIndex > -1) - { - ObjectType objectType = GetObjectTypeFromOsiObjectType(dectObj.candidate(highestIndex).classification().type()); - QString idStr = QString::number(dectObj.header().ground_truth_id(highestIndex).value()); - ParseSensorDataStationaryObject(objectMessage, dectObj.base(), objectType, idStr); - } - } - - for (int i = 0; i < sensorData.moving_object_size(); ++i) - { - const osi3::DetectedMovingObject& dectObj = sensorData.moving_object(i); - double highestProbability = 0; - int highestIndex = -1; - for(int j = 0; j < dectObj.candidate().size(); ++j) - { - const osi3::DetectedMovingObject_CandidateMovingObject candi = dectObj.candidate(j); - if(candi.probability() > highestProbability) - { - highestProbability = candi.probability(); - highestIndex = j; - } - } - - if(highestIndex > -1) - { - ObjectType objectType = GetObjectTypeFromOsiObjectType(dectObj.candidate(highestIndex).vehicle_classification().type()); - QString idStr = QString::number(dectObj.header().ground_truth_id(highestIndex).value()); - ParseSensorDataMovingObject(objectMessage, dectObj.base(), objectType, idStr); - } - } - - for (int i = 0; i < sensorData.traffic_sign_size(); ++i) - { - const osi3::DetectedTrafficSign_DetectedMainSign& dectMS = sensorData.traffic_sign(i).main_sign(); - double highestProbability = 0; - int highestIndex = -1; - for(int j = 0; j < dectMS.candidate().size(); ++j) - { - const osi3::DetectedTrafficSign_DetectedMainSign_CandidateMainSign candi = dectMS.candidate(j); - if(candi.probability() > highestProbability) - { - highestProbability = candi.probability(); - highestIndex = j; - } - } - - if(highestIndex > -1) - { - QString idStr = QString::number(sensorData.traffic_sign(i).header().ground_truth_id(highestIndex).value()); - ParseSensorDataStationaryObject(objectMessage, dectMS.base(), ObjectType::TrafficSign, idStr); - } - - } - - for (int i = 0; i < sensorData.traffic_light_size(); ++i) - { - const osi3::DetectedTrafficLight& dectTL = sensorData.traffic_light(i); - double highestProbability = 0; - int highestIndex = -1; - for(int j = 0; j < dectTL.candidate().size(); ++j) - { - const osi3::DetectedTrafficLight_CandidateTrafficLight candi = dectTL.candidate(j); - if(candi.probability() > highestProbability) - { - highestProbability = candi.probability(); - highestIndex = j; - } - } - - if(highestIndex > -1) - { - QString idStr = QString::number(dectTL.header().ground_truth_id(highestIndex).value()); - ParseSensorDataStationaryObject(objectMessage, dectTL.base(), ObjectType::TrafficLight, idStr); - } - } - - for (int i = 0; i < sensorData.road_marking_size(); ++i) - { - const osi3::DetectedRoadMarking& dectRM = sensorData.road_marking(i); - double highestProbability = 0; - int highestIndex = -1; - for(int j = 0; j < dectRM.candidate().size(); ++j) - { - const osi3::DetectedRoadMarking_CandidateRoadMarking candi = dectRM.candidate(j); - if(candi.probability() > highestProbability) - { - highestProbability = candi.probability(); - highestIndex = j; - } - } - - if(highestIndex > -1) - { - QString idStr = QString::number(dectRM.header().ground_truth_id(highestIndex).value()); - ParseSensorDataStationaryObject(objectMessage, dectRM.base(), ObjectType::OtherObject, idStr); - } - } - - if(config_.laneType_ == LaneType::CenterLanes) - { - // DRAW_CENTER_LINES - for (int i = 0; i < sensorData.lane_size(); ++i) - { - const osi3::DetectedLane& detectL = sensorData.lane(i); - - int highestProbability = 0; - int highestIndex = -1; - for(int i = 0; i < detectL.candidate_size(); ++i) - { - if(highestProbability < detectL.candidate(i).probability()) - { - highestProbability = detectL.candidate(i).probability(); - highestIndex = i; - } - } - - if(highestIndex > -1) - { - LaneStruct tmpLane; - tmpLane.id = detectL.header().ground_truth_id(highestIndex).value(); - - QVector centerLines; - const osi3::Lane_Classification& laneC = detectL.candidate(highestIndex).classification(); - - for(int c = 0; c < laneC.centerline_size(); ++c) - { - const osi3::Vector3d& centerLine = laneC.centerline(c); - if(centerLine.x() != 0 || centerLine.y() != 0 || centerLine.z() != 0) - { - centerLines.append(QVector3D(centerLine.x(), 0, -centerLine.y())); - } - else if(!centerLines.empty()) - { - tmpLane.centerLanes.append(centerLines); - centerLines.clear(); - } - } - if(!centerLines.empty()) - tmpLane.centerLanes.append(centerLines); - - if(!tmpLane.centerLanes.empty()) - laneMessage.append(tmpLane); - } - } - } - else - { - // DRAW_LANE_BOUNDARIES - for (int i = 0; i < sensorData.lane_boundary_size(); ++i) - { - const osi3::DetectedLaneBoundary& dLaneB = sensorData.lane_boundary(i); - - int highestProbability = 0; - int highestIndex = -1; - for(int i = 0; i < dLaneB.candidate_size(); ++i) - { - if(highestProbability < dLaneB.candidate(i).probability()) - { - highestProbability = dLaneB.candidate(i).probability(); - highestIndex = i; - } - } - - if(highestIndex > -1) - { - LaneStruct tmpLane; - tmpLane.id = dLaneB.header().ground_truth_id(highestIndex).value(); - - QVector boundaryLines; - - for(int b = 0; b < dLaneB.boundary_line_size(); ++b) - { - const osi3::Vector3d& position = dLaneB.boundary_line(b).position(); - - if(position.x() != 0 || position.y() != 0 || position.z() != 0) - { - boundaryLines.append(QVector3D(position.x(), 0, -position.y())); - } - else if(!boundaryLines.empty()) - { - tmpLane.boundaryLanes.append(boundaryLines); - boundaryLines.clear(); - } - } - - if(!boundaryLines.empty()) - tmpLane.boundaryLanes.append(boundaryLines); - - if(!tmpLane.boundaryLanes.empty()) - laneMessage.append(tmpLane); - } - } - } + for (int i = 0; i < sensorData.stationary_object_size(); ++i) + { + const osi3::DetectedStationaryObject& dectObj = sensorData.stationary_object(i); + double highestProbability = 0; + int highestIndex = -1; + for (int j = 0; j < dectObj.candidate().size(); ++j) + { + const osi3::DetectedStationaryObject_CandidateStationaryObject candi = dectObj.candidate(j); + if (candi.probability() > highestProbability) + { + highestProbability = candi.probability(); + highestIndex = j; + } + } + + if (highestIndex > -1) + { + ObjectType objectType = GetObjectTypeFromOsiObjectType(dectObj.candidate(highestIndex).classification().type()); + QString idStr = QString::number(dectObj.header().ground_truth_id(highestIndex).value()); + ParseSensorDataStationaryObject(objectMessage, dectObj.base(), objectType, idStr); + } + } + + for (int i = 0; i < sensorData.moving_object_size(); ++i) + { + const osi3::DetectedMovingObject& dectObj = sensorData.moving_object(i); + double highestProbability = 0; + int highestIndex = -1; + for (int j = 0; j < dectObj.candidate().size(); ++j) + { + const osi3::DetectedMovingObject_CandidateMovingObject candi = dectObj.candidate(j); + if (candi.probability() > highestProbability) + { + highestProbability = candi.probability(); + highestIndex = j; + } + } + + if (highestIndex > -1) + { + ObjectType objectType = GetObjectTypeFromOsiObjectType(dectObj.candidate(highestIndex).vehicle_classification().type()); + QString idStr = QString::number(dectObj.header().ground_truth_id(highestIndex).value()); + ParseSensorDataMovingObject(objectMessage, dectObj.base(), objectType, idStr); + } + } + + for (int i = 0; i < sensorData.traffic_sign_size(); ++i) + { + const osi3::DetectedTrafficSign_DetectedMainSign& dectMS = sensorData.traffic_sign(i).main_sign(); + double highestProbability = 0; + int highestIndex = -1; + for (int j = 0; j < dectMS.candidate().size(); ++j) + { + const osi3::DetectedTrafficSign_DetectedMainSign_CandidateMainSign candi = dectMS.candidate(j); + if (candi.probability() > highestProbability) + { + highestProbability = candi.probability(); + highestIndex = j; + } + } + + if (highestIndex > -1) + { + QString idStr = QString::number(sensorData.traffic_sign(i).header().ground_truth_id(highestIndex).value()); + ParseSensorDataStationaryObject(objectMessage, dectMS.base(), ObjectType::TrafficSign, idStr); + } + + } + + for (int i = 0; i < sensorData.traffic_light_size(); ++i) + { + const osi3::DetectedTrafficLight& dectTL = sensorData.traffic_light(i); + double highestProbability = 0; + int highestIndex = -1; + for (int j = 0; j < dectTL.candidate().size(); ++j) + { + const osi3::DetectedTrafficLight_CandidateTrafficLight candi = dectTL.candidate(j); + if (candi.probability() > highestProbability) + { + highestProbability = candi.probability(); + highestIndex = j; + } + } + + if (highestIndex > -1) + { + QString idStr = QString::number(dectTL.header().ground_truth_id(highestIndex).value()); + ParseSensorDataStationaryObject(objectMessage, dectTL.base(), ObjectType::TrafficLight, idStr); + } + } + + for (int i = 0; i < sensorData.road_marking_size(); ++i) + { + const osi3::DetectedRoadMarking& dectRM = sensorData.road_marking(i); + double highestProbability = 0; + int highestIndex = -1; + for (int j = 0; j < dectRM.candidate().size(); ++j) + { + const osi3::DetectedRoadMarking_CandidateRoadMarking candi = dectRM.candidate(j); + if (candi.probability() > highestProbability) + { + highestProbability = candi.probability(); + highestIndex = j; + } + } + + if (highestIndex > -1) + { + QString idStr = QString::number(dectRM.header().ground_truth_id(highestIndex).value()); + ParseSensorDataStationaryObject(objectMessage, dectRM.base(), ObjectType::OtherObject, idStr); + } + } + + if (config_.laneType_ == LaneType::CenterLanes) + { + // DRAW_CENTER_LINES + for (int i = 0; i < sensorData.lane_size(); ++i) + { + const osi3::DetectedLane& detectL = sensorData.lane(i); + + int highestProbability = 0; + int highestIndex = -1; + for (int i = 0; i < detectL.candidate_size(); ++i) + { + if (highestProbability < detectL.candidate(i).probability()) + { + highestProbability = detectL.candidate(i).probability(); + highestIndex = i; + } + } + + if (highestIndex > -1) + { + LaneStruct tmpLane; + tmpLane.id = detectL.header().ground_truth_id(highestIndex).value(); + + QVector centerLines; + const osi3::Lane_Classification& laneC = detectL.candidate(highestIndex).classification(); + + for (int c = 0; c < laneC.centerline_size(); ++c) + { + const osi3::Vector3d& centerLine = laneC.centerline(c); + if (centerLine.x() != 0 || centerLine.y() != 0 || centerLine.z() != 0) + { + centerLines.append(QVector3D(centerLine.x(), 0, -centerLine.y())); + } + else if (!centerLines.empty()) + { + tmpLane.centerLanes.append(centerLines); + centerLines.clear(); + } + } + if (!centerLines.empty()) + tmpLane.centerLanes.append(centerLines); + + if (!tmpLane.centerLanes.empty()) + laneMessage.append(tmpLane); + } + } + } + else + { + // DRAW_LANE_BOUNDARIES + for (int i = 0; i < sensorData.lane_boundary_size(); ++i) + { + const osi3::DetectedLaneBoundary& dLaneB = sensorData.lane_boundary(i); + + int highestProbability = 0; + int highestIndex = -1; + for (int i = 0; i < dLaneB.candidate_size(); ++i) + { + if (highestProbability < dLaneB.candidate(i).probability()) + { + highestProbability = dLaneB.candidate(i).probability(); + highestIndex = i; + } + } + + if (highestIndex > -1) + { + LaneStruct tmpLane; + tmpLane.id = dLaneB.header().ground_truth_id(highestIndex).value(); + + QVector boundaryLines; + + for (int b = 0; b < dLaneB.boundary_line_size(); ++b) + { + const osi3::Vector3d& position = dLaneB.boundary_line(b).position(); + + if (position.x() != 0 || position.y() != 0 || position.z() != 0) + { + boundaryLines.append(QVector3D(position.x(), 0, -position.y())); + } + else if (!boundaryLines.empty()) + { + tmpLane.boundaryLanes.append(boundaryLines); + boundaryLines.clear(); + } + } + + if (!boundaryLines.empty()) + tmpLane.boundaryLanes.append(boundaryLines); + + if (!tmpLane.boundaryLanes.empty()) + laneMessage.append(tmpLane); + } + } + } + + if (sensorData.has_feature_data()) { + // Parse Lidar Point Cloud + for (int i = 0; i < sensorData.feature_data().lidar_sensor_size(); i++) { + const osi3::LidarDetectionData& ldd = sensorData.feature_data().lidar_sensor().Get(i); + for (int j = 0; j < sensorData.feature_data().lidar_sensor().Get(i).detection_size(); j++) { + osi3::Spherical3d pos = ldd.detection().Get(j).position(); + float color = ldd.detection().Get(j).intensity(); + // We measure azimuth clockwise (typical in navigation, but in contrast to the standard spherical coordinates) + // x: up, y: out of monitor, z: right + QVector3D posxyz(pos.distance()*cos(pos.azimuth())*cos(pos.elevation()), 0, (-1)*pos.distance()*sin(pos.azimuth())*cos(pos.elevation())); + // ToDo: transform according to mounting position of sensor + PointStruct p = { posxyz, color}; + pointMessage.append(p); + } + } + // ToDo: Add Radar sensor + } + } /* - * - * Parse a moving SensorData Object - * - */ +* +* Parse a moving SensorData Object +* +*/ void OsiParser::ParseSensorDataMovingObject(Message& objectMessage, - const osi3::BaseMoving& baseObject, - const ObjectType objectType, - const QString& idStr) + const osi3::BaseMoving& baseObject, + const ObjectType objectType, + const QString& idStr) { - if (objectType == ObjectType::None) - { - return; - } - - osi3::Vector3d position = baseObject.position(); - osi3::Vector3d velocity = baseObject.velocity(); - osi3::Vector3d acceleration = baseObject.acceleration(); - float orientation = baseObject.orientation().yaw(); - - MessageStruct tmpMsg; - tmpMsg.id = Global::GetObjectTypeName(objectType) + idStr; - tmpMsg.name = "ID: " + idStr; - tmpMsg.type = objectType; - tmpMsg.orientation = orientation; - tmpMsg.position = QVector3D(position.x(), 0, -position.y()); - tmpMsg.realPosition = QVector3D(position.x(), position.y(), position.z()); - tmpMsg.velocitie = QVector3D(velocity.x(), velocity.y(), velocity.z()); - tmpMsg.acceleration = QVector3D(acceleration.x(), acceleration.y(), acceleration.z()); - - if(baseObject.base_polygon_size() == 4) - { - for(int i = 0; i < 4; ++i) - { - const osi3::Vector2d& vertice = baseObject.base_polygon(i); - tmpMsg.basePoly.push_back(QVector3D(vertice.x() - position.x(), 0, -vertice.y()+position.y())); - } - } - else - { - osi3::Dimension3d dimension = baseObject.dimension(); - if (dimension.width() == 0 && dimension.length() == 0) - { - dimension.set_width(1.0f); - dimension.set_length(1.0f); - } - tmpMsg.dimension = dimension; - } - - objectMessage.append(tmpMsg); + if (objectType == ObjectType::None) + { + return; + } + + osi3::Vector3d position = baseObject.position(); + osi3::Vector3d velocity = baseObject.velocity(); + osi3::Vector3d acceleration = baseObject.acceleration(); + float orientation = baseObject.orientation().yaw(); + + MessageStruct tmpMsg; + tmpMsg.id = Global::GetObjectTypeName(objectType) + idStr; + tmpMsg.name = "ID: " + idStr; + tmpMsg.type = objectType; + tmpMsg.orientation = orientation; + tmpMsg.position = QVector3D(position.x(), 0, -position.y()); + tmpMsg.realPosition = QVector3D(position.x(), position.y(), position.z()); + tmpMsg.velocitie = QVector3D(velocity.x(), velocity.y(), velocity.z()); + tmpMsg.acceleration = QVector3D(acceleration.x(), acceleration.y(), acceleration.z()); + + if (baseObject.base_polygon_size() == 4) + { + for (int i = 0; i < 4; ++i) + { + const osi3::Vector2d& vertice = baseObject.base_polygon(i); + tmpMsg.basePoly.push_back(QVector3D(vertice.x() - position.x(), 0, -vertice.y() + position.y())); + } + } + else + { + osi3::Dimension3d dimension = baseObject.dimension(); + if (dimension.width() == 0 && dimension.length() == 0) + { + dimension.set_width(1.0f); + dimension.set_length(1.0f); + } + tmpMsg.dimension = dimension; + } + + objectMessage.append(tmpMsg); } void OsiParser::ParseSensorDataStationaryObject(Message& objectMessage, - const osi3::BaseStationary& baseObject, - const ObjectType objectType, - const QString& idStr) + const osi3::BaseStationary& baseObject, + const ObjectType objectType, + const QString& idStr) { - if (objectType == ObjectType::None) - { - return; - } - - osi3::Vector3d tmp; - tmp.set_x(0.0); - tmp.set_y(0.0); - tmp.set_z(0.0); - - osi3::Vector3d position = baseObject.position(); - osi3::Vector3d velocity = tmp; - osi3::Vector3d acceleration = tmp; - float orientation = baseObject.orientation().yaw(); - - MessageStruct tmpMsg; - tmpMsg.id = Global::GetObjectTypeName(objectType) + idStr; - tmpMsg.name = "ID: " + idStr; - tmpMsg.type = objectType; - tmpMsg.orientation = orientation; - tmpMsg.position = QVector3D(position.x(), 0, -position.y()); - tmpMsg.realPosition = QVector3D(position.x(), position.y(), position.z()); - tmpMsg.velocitie = QVector3D(velocity.x(), velocity.y(), velocity.z()); - tmpMsg.acceleration = QVector3D(acceleration.x(), acceleration.y(), acceleration.z()); - - if(baseObject.base_polygon_size() == 4) - { - for(int i = 0; i < 4; ++i) - { - const osi3::Vector2d& vertice = baseObject.base_polygon(i); - tmpMsg.basePoly.push_back(QVector3D(vertice.x(), 0, -vertice.y())); - } - } - else - { - osi3::Dimension3d dimension = baseObject.dimension(); - if (dimension.width() == 0 && dimension.length() == 0) - { - dimension.set_width(1.0f); - dimension.set_length(1.0f); - } - tmpMsg.dimension = dimension; - } - - objectMessage.append(tmpMsg); + if (objectType == ObjectType::None) + { + return; + } + + osi3::Vector3d tmp; + tmp.set_x(0.0); + tmp.set_y(0.0); + tmp.set_z(0.0); + + osi3::Vector3d position = baseObject.position(); + osi3::Vector3d velocity = tmp; + osi3::Vector3d acceleration = tmp; + float orientation = baseObject.orientation().yaw(); + + MessageStruct tmpMsg; + tmpMsg.id = Global::GetObjectTypeName(objectType) + idStr; + tmpMsg.name = "ID: " + idStr; + tmpMsg.type = objectType; + tmpMsg.orientation = orientation; + tmpMsg.position = QVector3D(position.x(), 0, -position.y()); + tmpMsg.realPosition = QVector3D(position.x(), position.y(), position.z()); + tmpMsg.velocitie = QVector3D(velocity.x(), velocity.y(), velocity.z()); + tmpMsg.acceleration = QVector3D(acceleration.x(), acceleration.y(), acceleration.z()); + + if (baseObject.base_polygon_size() == 4) + { + for (int i = 0; i < 4; ++i) + { + const osi3::Vector2d& vertice = baseObject.base_polygon(i); + tmpMsg.basePoly.push_back(QVector3D(vertice.x(), 0, -vertice.y())); + } + } + else + { + osi3::Dimension3d dimension = baseObject.dimension(); + if (dimension.width() == 0 && dimension.length() == 0) + { + dimension.set_width(1.0f); + dimension.set_length(1.0f); + } + tmpMsg.dimension = dimension; + } + + objectMessage.append(tmpMsg); } //Export Osi message void OsiParser::ExportOsiMessage() { - if(startSaveOSIMsg_ == false && osiMsgString_.empty() == true) - { - startSaveOSIMsg_ = true; - } - else if(startSaveOSIMsg_ == true && osiMsgString_.empty() == false) - { - startSaveOSIMsg_ = false; - - QString fileName = QFileDialog::getSaveFileName(nullptr, "Export message to file", "", "*.txt"); - if (fileName.isEmpty() == false) - { - QFile file(fileName); - if (!file.open(QIODevice::WriteOnly)) - { - qDebug() << "Error opening file '" + fileName + "' for writing."; - return; - } - - file.write(osiMsgString_.c_str(), osiMsgString_.length()); - file.close(); - } - - osiMsgNumber_ = 0; - osiMsgString_.clear(); - } - else - { - // should never come here - QMessageBox::warning(nullptr, "Warning", "Nothing to save!", QMessageBox::Ok, QMessageBox::Ok); - } + if (startSaveOSIMsg_ == false && osiMsgString_.empty() == true) + { + startSaveOSIMsg_ = true; + } + else if (startSaveOSIMsg_ == true && osiMsgString_.empty() == false) + { + startSaveOSIMsg_ = false; + + QString fileName = QFileDialog::getSaveFileName(nullptr, "Export message to file", "", "*.txt"); + if (fileName.isEmpty() == false) + { + QFile file(fileName); + if (!file.open(QIODevice::WriteOnly)) + { + qDebug() << "Error opening file '" + fileName + "' for writing."; + return; + } + + file.write(osiMsgString_.c_str(), osiMsgString_.length()); + file.close(); + } + + osiMsgNumber_ = 0; + osiMsgString_.clear(); + } + else + { + // should never come here + QMessageBox::warning(nullptr, "Warning", "Nothing to save!", QMessageBox::Ok, QMessageBox::Ok); + } } // Convert object types ObjectType OsiParser::GetObjectTypeFromOsiObjectType(const osi3::MovingObject_VehicleClassification_Type& vehicleType) { - ObjectType objType = ObjectType::None; - switch (vehicleType) { - case osi3::MovingObject_VehicleClassification_Type_TYPE_UNKNOWN: - objType = ObjectType::UnknownObject; - break; - case osi3::MovingObject_VehicleClassification_Type_TYPE_OTHER: - objType = ObjectType::OtherObject; - break; - case osi3::MovingObject_VehicleClassification_Type_TYPE_SMALL_CAR: - case osi3::MovingObject_VehicleClassification_Type_TYPE_COMPACT_CAR: - case osi3::MovingObject_VehicleClassification_Type_TYPE_MEDIUM_CAR: - case osi3::MovingObject_VehicleClassification_Type_TYPE_LUXURY_CAR: - objType = ObjectType::Car; - break; - case osi3::MovingObject_VehicleClassification_Type_TYPE_DELIVERY_VAN: - case osi3::MovingObject_VehicleClassification_Type_TYPE_HEAVY_TRUCK: - objType = ObjectType::Truck; - break; - case osi3::MovingObject_VehicleClassification_Type_TYPE_MOTORBIKE: - objType = ObjectType::MotorBike; - break; - case osi3::MovingObject_VehicleClassification_Type_TYPE_BICYCLE: - objType = ObjectType::Bicycle; - break; - case osi3::MovingObject_VehicleClassification_Type_TYPE_SEMITRAILER: - case osi3::MovingObject_VehicleClassification_Type_TYPE_TRAILER: - objType = ObjectType::Trailer; - break; - default: - break; - } - - return objType; + ObjectType objType = ObjectType::None; + switch (vehicleType) { + case osi3::MovingObject_VehicleClassification_Type_TYPE_UNKNOWN: + objType = ObjectType::UnknownObject; + break; + case osi3::MovingObject_VehicleClassification_Type_TYPE_OTHER: + objType = ObjectType::OtherObject; + break; + case osi3::MovingObject_VehicleClassification_Type_TYPE_SMALL_CAR: + case osi3::MovingObject_VehicleClassification_Type_TYPE_COMPACT_CAR: + case osi3::MovingObject_VehicleClassification_Type_TYPE_MEDIUM_CAR: + case osi3::MovingObject_VehicleClassification_Type_TYPE_LUXURY_CAR: + objType = ObjectType::Car; + break; + case osi3::MovingObject_VehicleClassification_Type_TYPE_DELIVERY_VAN: + case osi3::MovingObject_VehicleClassification_Type_TYPE_HEAVY_TRUCK: + objType = ObjectType::Truck; + break; + case osi3::MovingObject_VehicleClassification_Type_TYPE_MOTORBIKE: + objType = ObjectType::MotorBike; + break; + case osi3::MovingObject_VehicleClassification_Type_TYPE_BICYCLE: + objType = ObjectType::Bicycle; + break; + case osi3::MovingObject_VehicleClassification_Type_TYPE_SEMITRAILER: + case osi3::MovingObject_VehicleClassification_Type_TYPE_TRAILER: + objType = ObjectType::Trailer; + break; + default: + break; + } + + return objType; } ObjectType OsiParser::GetObjectTypeFromOsiObjectType(const osi3::MovingObject_Type& objectType) { - ObjectType objType = ObjectType::None; - switch (objectType) { - case osi3::MovingObject_Type_TYPE_UNKNOWN: - objType = ObjectType::UnknownObject; - break; - case osi3::MovingObject_Type_TYPE_OTHER: - objType = ObjectType::OtherObject; - break; - case osi3::MovingObject_Type_TYPE_VEHICLE: - objType = ObjectType::Car; - break; - case osi3::MovingObject_Type_TYPE_PEDESTRIAN: - objType = ObjectType::Pedestrian; - break; - case osi3::MovingObject_Type_TYPE_ANIMAL: - objType = ObjectType::Animal; - break; - default: - break; - } - - return objType; + ObjectType objType = ObjectType::None; + switch (objectType) { + case osi3::MovingObject_Type_TYPE_UNKNOWN: + objType = ObjectType::UnknownObject; + break; + case osi3::MovingObject_Type_TYPE_OTHER: + objType = ObjectType::OtherObject; + break; + case osi3::MovingObject_Type_TYPE_VEHICLE: + objType = ObjectType::Car; + break; + case osi3::MovingObject_Type_TYPE_PEDESTRIAN: + objType = ObjectType::Pedestrian; + break; + case osi3::MovingObject_Type_TYPE_ANIMAL: + objType = ObjectType::Animal; + break; + default: + break; + } + + return objType; } ObjectType OsiParser::GetObjectTypeFromOsiObjectType(const osi3::StationaryObject_Classification_Type& objectType) { - ObjectType objType = ObjectType::None; - switch (objectType) { - case osi3::StationaryObject_Classification_Type_TYPE_UNKNOWN: - objType = ObjectType::UnknownObject; - break; - case osi3::StationaryObject_Classification_Type_TYPE_OTHER: - objType = ObjectType::OtherObject; - break; - case osi3::StationaryObject_Classification_Type_TYPE_BRIDGE: - objType = ObjectType::Bridge; - break; - case osi3::StationaryObject_Classification_Type_TYPE_BUILDING: - objType = ObjectType::Building; - break; - case osi3::StationaryObject_Classification_Type_TYPE_PYLON: - objType = ObjectType::Pylon; - break; - case osi3::StationaryObject_Classification_Type_TYPE_REFLECTIVE_STRUCTURE: - objType = ObjectType::ReflectorPost; - break; - case osi3::StationaryObject_Classification_Type_TYPE_DELINEATOR: - objType = ObjectType::Delineator; - break; - default: - break; - } - - return objType; + ObjectType objType = ObjectType::None; + switch (objectType) { + case osi3::StationaryObject_Classification_Type_TYPE_UNKNOWN: + objType = ObjectType::UnknownObject; + break; + case osi3::StationaryObject_Classification_Type_TYPE_OTHER: + objType = ObjectType::OtherObject; + break; + case osi3::StationaryObject_Classification_Type_TYPE_BRIDGE: + objType = ObjectType::Bridge; + break; + case osi3::StationaryObject_Classification_Type_TYPE_BUILDING: + objType = ObjectType::Building; + break; + case osi3::StationaryObject_Classification_Type_TYPE_PYLON: + objType = ObjectType::Pylon; + break; + case osi3::StationaryObject_Classification_Type_TYPE_REFLECTIVE_STRUCTURE: + objType = ObjectType::ReflectorPost; + break; + case osi3::StationaryObject_Classification_Type_TYPE_DELINEATOR: + objType = ObjectType::Delineator; + break; + default: + break; + } + + return objType; } ObjectType OsiParser::GetObjectTypeFromOsiObjectType(const osi3::TrafficSign_MainSign_Classification_Type& objectType) { - return ObjectType::TrafficSign; + return ObjectType::TrafficSign; } ObjectType OsiParser::GetObjectTypeFromOsiObjectType(const osi3::TrafficLight_Classification_Mode& objectType) { - return ObjectType::TrafficLight; + return ObjectType::TrafficLight; } - - - diff --git a/src/pointcloud.cpp b/src/pointcloud.cpp new file mode 100644 index 0000000..f4f636b --- /dev/null +++ b/src/pointcloud.cpp @@ -0,0 +1,31 @@ +#include "pointcloud.h" + +PointCloud::PointCloud(int id, QOpenGLFunctions_4_3_Core* functions, const QString& srcPath) + : pointcloudId_(id), + isVisible_(true), + functions_(functions), + srcPath_(srcPath) +{ + if (!colorscheme.load(srcPath + "Resources/Images/PointCloudColors.png")) { + qDebug() << "Failed to open point cloud color scheme (" << srcPath << "Resources/Images/PointCloudColors.png)"; + } + colorscheme = colorscheme.convertToFormat(QImage::Format::Format_RGBA8888); // convert to the texture format we use +} + +void +PointCloud::UpdatePointCloud(const QVector& pm) +{ + glPointCloud_ = std::make_unique(functions_, srcPath_); + glPointCloud_->id_ = "point_cloud_" + QString::number(pointcloudId_); + // Copy data from pm to glPointCloud_ + for (const PointStruct& p : pm) { + glPointCloud_->vertices_.append(p.position); + glPointCloud_->texCoords_.append(QVector2D(p.color, p.color)); + } + // set up the globject + glPointCloud_->Init(); + glPointCloud_->SetColor(Qt::white); + glPointCloud_->SetTexture(colorscheme, false, GL_CLAMP_TO_EDGE); + glPointCloud_->SetObjectType(ObjectType::None); + glPointCloud_->UpdateVertexBuffer(); +}