change glshow for opengl show.

src/tool/glshow/glshow.pro

@@ -0,0 +1,74 @@
+HEADERS       = glwidget.h \
+                window.h \
+                mainwindow.h \
+                logo.h \
+    ../../include/msgtype/objectarray.pb.h \
+    ../../include/msgtype/object.pb.h
+SOURCES       = glwidget.cpp \
+                main.cpp \
+                window.cpp \
+                mainwindow.cpp \
+                logo.cpp \
+    ../../include/msgtype/objectarray.pb.cc \
+    ../../include/msgtype/object.pb.cc
+
+QT           += widgets
+
+win32: INCLUDEPATH += C:/File/PCL_1.8/PCL_1.8.1/3rdParty/boost-1.65/include/boost-1_65
+
+win32: INCLUDEPATH += C:/File/PCL_1.8/PCL_1.8.1/3rdParty/eigen3/include/eigen3
+win32: INCLUDEPATH += C:/File/PCL_1.8/PCL_1.8.1/include/pcl-1.8
+
+unix:INCLUDEPATH += /usr/include/eigen3
+unix:INCLUDEPATH += /usr/include/pcl-1.7
+
+
+unix:LIBS += -lboost_thread -lboost_system
+
+
+
+win32: LIBS += -LC:/File/PCL_1.8/PCL_1.8.1/lib -lpcl_common_debug\
+        -lpcl_features_debug\
+        -lpcl_filters_debug\
+        -lpcl_io_debug\
+        -lpcl_io_ply_debug\
+        -lpcl_kdtree_debug\
+        -lpcl_keypoints_debug\
+        -lpcl_octree_debug\
+        -lpcl_outofcore_debug\
+        -lpcl_people_debug\
+        -lpcl_recognition_debug\
+        -lpcl_registration_debug\
+        -lpcl_sample_consensus_debug\
+        -lpcl_search_debug\
+        -lpcl_segmentation_debug\
+        -lpcl_surface_debug\
+        -lpcl_tracking_debug\
+        -lpcl_visualization_debug
+
+unix:LIBS +=  -lpcl_common\
+        -lpcl_features\
+        -lpcl_filters\
+        -lpcl_io\
+        -lpcl_io_ply\
+        -lpcl_kdtree\
+        -lpcl_keypoints\
+        -lpcl_octree\
+        -lpcl_outofcore\
+        -lpcl_people\
+        -lpcl_recognition\
+        -lpcl_registration\
+        -lpcl_sample_consensus\
+        -lpcl_search\
+        -lpcl_segmentation\
+        -lpcl_surface\
+        -lpcl_tracking\
+        -lpcl_visualization
+
+INCLUDEPATH += $$PWD/../../common/ModuleComm/
+LIBS += -L$$PWD/../../common/ModuleComm/ -lmodulecomm
+
+
+INCLUDEPATH += $$PWD/../../include/msgtype
+
+LIBS += -lprotobuf

src/tool/glshow/glwidget.cpp

@@ -0,0 +1,619 @@
+/****************************************************************************
+**
+** Copyright (C) 2016 The Qt Company Ltd.
+** Contact: https://www.qt.io/licensing/
+**
+** This file is part of the examples of the Qt Toolkit.
+**
+** $QT_BEGIN_LICENSE:BSD$
+** Commercial License Usage
+** Licensees holding valid commercial Qt licenses may use this file in
+** accordance with the commercial license agreement provided with the
+** Software or, alternatively, in accordance with the terms contained in
+** a written agreement between you and The Qt Company. For licensing terms
+** and conditions see https://www.qt.io/terms-conditions. For further
+** information use the contact form at https://www.qt.io/contact-us.
+**
+** BSD License Usage
+** Alternatively, you may use this file under the terms of the BSD license
+** as follows:
+**
+** "Redistribution and use in source and binary forms, with or without
+** modification, are permitted provided that the following conditions are
+** met:
+**   * Redistributions of source code must retain the above copyright
+**     notice, this list of conditions and the following disclaimer.
+**   * Redistributions in binary form must reproduce the above copyright
+**     notice, this list of conditions and the following disclaimer in
+**     the documentation and/or other materials provided with the
+**     distribution.
+**   * Neither the name of The Qt Company Ltd nor the names of its
+**     contributors may be used to endorse or promote products derived
+**     from this software without specific prior written permission.
+**
+**
+** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+** "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+** LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+** A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+** OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+** SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+** LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+** DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+** THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+** (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+** OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE."
+**
+** $QT_END_LICENSE$
+**
+****************************************************************************/
+
+#include "glwidget.h"
+#include <QMouseEvent>
+#include <QOpenGLShaderProgram>
+#include <QCoreApplication>
+#include <math.h>
+
+bool GLWidget::m_transparent = false;
+
+
+
+GLWidget * ggw;
+
+
+void ListenPointCloud(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+    if(nSize <=16)return;
+    unsigned int * pHeadSize = (unsigned int *)strdata;
+    if(*pHeadSize > nSize)
+    {
+        std::cout<<"ListenPointCloud data is small headsize ="<<*pHeadSize<<"  data size is"<<nSize<<std::endl;
+    }
+
+    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
+                new pcl::PointCloud<pcl::PointXYZI>());
+    int nNameSize;
+    nNameSize = *pHeadSize - 4-4-8;
+    char * strName = new char[nNameSize+1];strName[nNameSize] = 0;
+    memcpy(strName,(char *)((char *)strdata +4),nNameSize);
+    point_cloud->header.frame_id = strName;
+    memcpy(&point_cloud->header.seq,(char *)strdata+4+nNameSize,4);
+    memcpy(&point_cloud->header.stamp,(char *)strdata+4+nNameSize+4,8);
+    int nPCount = (nSize - *pHeadSize)/sizeof(pcl::PointXYZI);
+    int i;
+    pcl::PointXYZI * p;
+    p = (pcl::PointXYZI *)((char *)strdata + *pHeadSize);
+    for(i=0;i<nPCount;i++)
+    {
+        pcl::PointXYZI xp;
+        xp.x = p->x;
+        xp.y = p->y;
+        xp.z = p->z;
+        xp.intensity = p->intensity;
+        point_cloud->push_back(xp);
+        p++;
+
+  //      std::cout<<" x is "<<xp.x<<" y is "<<xp.y<<std::endl;
+    }
+
+ //   return;
+
+
+    ggw->SetPointCloud(point_cloud);
+ //   DBSCAN_PC(point_cloud);
+
+
+}
+
+void Listenlidarcnndectect(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+
+//    std::vector<iv::lidar::object>  lidarobjvec;
+//    strtolidarobj(lidarobjvec,strdata,nSize);
+
+    iv::lidar::objectarray lidarobjvec;
+    std::string in;
+    in.append(strdata,nSize);
+    lidarobjvec.ParseFromString(in);
+    qDebug("obj size is %d ",lidarobjvec.obj_size());
+//    if(lidarobjvec.obj_size() > 1)
+//    {
+//        qDebug("type is %s",lidarobjvec.obj(0).type_name().data());
+//    }
+    ggw->SetLidarObj(lidarobjvec);
+}
+
+GLWidget::GLWidget(QWidget *parent)
+    : QOpenGLWidget(parent),
+      m_xRot(600),
+      m_yRot(3000),
+      m_zRot(3500),
+      m_program(0)
+{
+    ggw = this;
+
+
+    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
+                new pcl::PointCloud<pcl::PointXYZI>());
+    mpoint_cloud = point_cloud;
+
+    m_core = QSurfaceFormat::defaultFormat().profile() == QSurfaceFormat::CoreProfile;
+    // --transparent causes the clear color to be transparent. Therefore, on systems that
+    // support it, the widget will become transparent apart from the logo.
+    if (m_transparent) {
+        QSurfaceFormat fmt = format();
+        fmt.setAlphaBufferSize(8);
+        setFormat(fmt);
+    }
+
+    void * pa = iv::modulecomm::RegisterRecv("lidarpc_center",ListenPointCloud);
+
+    pa = iv::modulecomm::RegisterRecv("lidar_track",Listenlidarcnndectect);
+}
+
+GLWidget::~GLWidget()
+{
+    cleanup();
+}
+
+QSize GLWidget::minimumSizeHint() const
+{
+    return QSize(50, 50);
+}
+
+QSize GLWidget::sizeHint() const
+{
+    return QSize(400, 400);
+}
+
+static void qNormalizeAngle(int &angle)
+{
+    while (angle < 0)
+        angle += 360 * 16;
+    while (angle > 360 * 16)
+        angle -= 360 * 16;
+}
+
+void GLWidget::setXRotation(int angle)
+{
+    qNormalizeAngle(angle);
+    if (angle != m_xRot) {
+        m_xRot = angle;
+        emit xRotationChanged(angle);
+        update();
+    }
+}
+
+void GLWidget::setYRotation(int angle)
+{
+    qNormalizeAngle(angle);
+    if (angle != m_yRot) {
+        m_yRot = angle;
+        emit yRotationChanged(angle);
+        update();
+    }
+}
+
+void GLWidget::setZRotation(int angle)
+{
+    qNormalizeAngle(angle);
+    if (angle != m_zRot) {
+        m_zRot = angle;
+        emit zRotationChanged(angle);
+        update();
+    }
+}
+
+void GLWidget::cleanup()
+{
+    if (m_program == nullptr)
+        return;
+    makeCurrent();
+    m_logoVbo.destroy();
+    delete m_program;
+    m_program = 0;
+    doneCurrent();
+}
+
+static const char *vertexShaderSourceCore =
+    "#version 150\n"
+    "in vec4 vertex;\n"
+    "in vec3 normal;\n"
+    "out vec3 vert;\n"
+    "out vec3 vertNormal;\n"
+    "uniform mat4 projMatrix;\n"
+    "uniform mat4 mvMatrix;\n"
+    "uniform mat3 normalMatrix;\n"
+    "void main() {\n"
+    "   vert = vertex.xyz;\n"
+    "   vertNormal = normalMatrix * normal;\n"
+    "   gl_Position = projMatrix * mvMatrix * vertex;\n"
+    "}\n";
+
+static const char *fragmentShaderSourceCore =
+    "#version 150\n"
+    "in highp vec3 vert;\n"
+    "in highp vec3 vertNormal;\n"
+    "out highp vec4 fragColor;\n"
+    "uniform highp vec3 lightPos;\n"
+    "void main() {\n"
+    "   highp vec3 L = normalize(lightPos - vert);\n"
+    "   highp float NL = max(dot(normalize(vertNormal), L), 0.0);\n"
+    "   highp vec3 color = vec3(0.39, 1.0, 0.0);\n"
+    "   highp vec3 col = clamp(color * 0.2 + color * 0.8 * NL, 0.0, 1.0);\n"
+    "   fragColor = vec4(col, 1.0);\n"
+    "}\n";
+
+static const char *vertexShaderSource =
+    "attribute vec4 vertex;\n"
+    "attribute vec3 normal;\n"
+    "varying vec3 vert;\n"
+    "varying vec3 vertNormal;\n"
+    "uniform mat4 projMatrix;\n"
+    "uniform mat4 mvMatrix;\n"
+    "uniform mat3 normalMatrix;\n"
+    "void main() {\n"
+    "   vert = vertex.xyz;\n"
+    "   vertNormal = normalMatrix * normal;\n"
+    "   gl_Position = projMatrix * mvMatrix * vertex;\n"
+    "}\n";
+
+static const char *fragmentShaderSource =
+    "varying highp vec3 vert;\n"
+    "varying highp vec3 vertNormal;\n"
+    "uniform highp vec3 lightPos;\n"
+    "void main() {\n"
+    "   highp vec3 L = normalize(lightPos - vert);\n"
+    "   highp float NL = max(dot(normalize(vertNormal), L), 0.0);\n"
+    "   highp vec3 color = vec3(0.39, 1.0, 0.0);\n"
+    "   highp vec3 col = clamp(color * 0.2 + color * 0.8 * NL, 0.0, 1.0);\n"
+    "   gl_FragColor = vec4(col, 1.0);\n"
+    "}\n";
+
+void GLWidget::initializeGL()
+{
+    // In this example the widget's corresponding top-level window can change
+    // several times during the widget's lifetime. Whenever this happens, the
+    // QOpenGLWidget's associated context is destroyed and a new one is created.
+    // Therefore we have to be prepared to clean up the resources on the
+    // aboutToBeDestroyed() signal, instead of the destructor. The emission of
+    // the signal will be followed by an invocation of initializeGL() where we
+    // can recreate all resources.
+    connect(context(), &QOpenGLContext::aboutToBeDestroyed, this, &GLWidget::cleanup);
+
+    initializeOpenGLFunctions();
+    glClearColor(0, 0, 0, m_transparent ? 0 : 1);
+
+    m_program = new QOpenGLShaderProgram;
+    m_program->addShaderFromSourceCode(QOpenGLShader::Vertex, m_core ? vertexShaderSourceCore : vertexShaderSource);
+ //   m_program->addShaderFromSourceCode(QOpenGLShader::Fragment, m_core ? fragmentShaderSourceCore : fragmentShaderSource);
+
+   m_program->addShaderFromSourceCode(QOpenGLShader::Fragment,
+         "uniform mediump vec4 color;\n"
+         "void main(void)\n"
+         "{\n"
+         "   gl_FragColor = color;\n"
+         "}");
+    m_program->bindAttributeLocation("vertex", 0);
+    m_program->bindAttributeLocation("normal", 1);
+    m_program->link();
+
+    m_program->bind();
+    m_projMatrixLoc = m_program->uniformLocation("projMatrix");
+    m_mvMatrixLoc = m_program->uniformLocation("mvMatrix");
+    m_normalMatrixLoc = m_program->uniformLocation("normalMatrix");
+    m_lightPosLoc = m_program->uniformLocation("lightPos");
+
+    m_colorLoc = m_program->uniformLocation("color");
+    // Create a vertex array object. In OpenGL ES 2.0 and OpenGL 2.x
+    // implementations this is optional and support may not be present
+    // at all. Nonetheless the below code works in all cases and makes
+    // sure there is a VAO when one is needed.
+    m_vao.create();
+    QOpenGLVertexArrayObject::Binder vaoBinder(&m_vao);
+
+
+    // Setup our vertex buffer object.
+    m_logoVbo.create();
+    m_logoVbo.bind();
+    m_logoVbo.allocate(m_logo.constData(), m_logo.count() * sizeof(GLfloat));
+
+    // Store the vertex attribute bindings for the program.
+    setupVertexAttribs();
+
+    // Our camera never changes in this example.
+    m_camera.setToIdentity();
+    m_camera.translate(0, 0, -m_iMag);
+
+    // Light position is fixed.
+    m_program->setUniformValue(m_lightPosLoc, QVector3D(0, 0, 70));
+
+    m_program->release();
+}
+
+void GLWidget::setupVertexAttribs()
+{
+    m_logoVbo.bind();
+    QOpenGLFunctions *f = QOpenGLContext::currentContext()->functions();
+    f->glEnableVertexAttribArray(0);
+//    f->glEnableVertexAttribArray(1);
+    f->glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 3 * sizeof(GLfloat), 0);
+//    f->glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 6 * sizeof(GLfloat), 0);
+//    f->glVertexAttribPointer(1, 3, GL_FLOAT, GL_FALSE, 6 * sizeof(GLfloat), reinterpret_cast<void *>(3 * sizeof(GLfloat)));
+    m_logoVbo.release();
+}
+
+void GLWidget::paintGL()
+{
+    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+    glEnable(GL_DEPTH_TEST);
+    glEnable(GL_CULL_FACE);
+
+    // Setup our vertex buffer object.
+    m_logoVbo.create();
+    m_logoVbo.bind();
+    QVector<GLfloat> x_data;
+    mMutex.lock();
+    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
+                new pcl::PointCloud<pcl::PointXYZI>());
+    mpoint_cloud.swap(point_cloud);
+    mMutex.unlock();
+    if(point_cloud->size() == 0)
+    {
+    m_logoVbo.allocate(m_logo.constData(), m_logo.count() * sizeof(GLfloat));
+    }
+    else
+    {
+        x_data.resize(point_cloud->size()*3);
+        int i;
+        for(i=0;i<point_cloud->size();i++)
+        {
+            pcl::PointXYZI point = point_cloud->at(i);
+            GLfloat *p = x_data.data() + i*3;
+            p[0] = point.x;
+            p[1] = point.y;
+            p[2] = point.z;
+        }
+         m_logoVbo.allocate(x_data.constData(), point_cloud->size()*3 * sizeof(GLfloat));
+    }
+
+    // Store the vertex attribute bindings for the program.
+    setupVertexAttribs();
+
+    m_camera.setToIdentity();
+    m_camera.translate(0, 0, -m_iMag);
+
+    m_world.setToIdentity();
+    m_world.rotate(180.0f - (m_xRot / 16.0f), 1, 0, 0);
+    m_world.rotate(m_yRot / 16.0f, 0, 1, 0);
+    m_world.rotate(m_zRot / 16.0f, 0, 0, 1);
+
+    QOpenGLVertexArrayObject::Binder vaoBinder(&m_vao);
+    m_program->bind();
+    m_program->setUniformValue(m_projMatrixLoc, m_proj);
+    m_program->setUniformValue(m_mvMatrixLoc, m_camera * m_world);
+    QMatrix3x3 normalMatrix = m_world.normalMatrix();
+    m_program->setUniformValue(m_normalMatrixLoc, normalMatrix);
+
+    QColor x(255,255,255,255);
+    m_program->setUniformValue(m_colorLoc,x);
+
+    if(point_cloud->size()>0)
+    {
+        glDrawArrays(GL_POINTS,0,point_cloud->size());
+    }
+    else
+    {
+        glDrawArrays(GL_TRIANGLES, 0, m_logo.vertexCount());
+
+    }
+
+    drawbox(0,0,-0.75,1.8,4.6,1.5,0,Qt::blue); //Our EU
+
+    iv::lidar::objectarray lidarobjvec;
+    mMutexLidarObj.lock();
+    lidarobjvec.CopyFrom(mlidarobjvec);
+    mMutexLidarObj.unlock();
+
+
+
+    int i;
+    for(i=0;i<lidarobjvec.obj_size();i++)
+    {
+        int j;
+        iv::lidar::object lo = lidarobjvec.obj(i);
+        QColor colorx = Qt::cyan;
+        switch (lo.mntype()) {
+        case 0:
+            colorx = Qt::yellow;
+            break;
+        case 1:
+            colorx = Qt::green;
+            break;
+        case 2:
+            colorx = Qt::green;
+            break;
+        case 3:
+            colorx = Qt::cyan;
+            break;
+        case 4:
+            colorx = Qt::red;
+            break;
+
+        default:
+            break;
+        }
+        drawbox(lo.centroid().x(),lo.centroid().y(),lo.centroid().z(),lo.dimensions().x(),
+                lo.dimensions().y(),lo.dimensions().z(),lo.tyaw(),colorx);
+    }
+
+    m_program->release();
+}
+
+void GLWidget::drawbox(float x, float y, float z, float l, float w, float h, double yaw,QColor colorx)
+{
+    GLfloat pv[24];
+    int i;
+
+    double s,t;
+    s = l/2;
+    t = w/2;
+
+    for(i=0;i<8;i++)
+    {
+        pv[3*i + 0 ] = -s;
+        pv[3*i + 1 ] = -t;
+        pv[3*i + 2 ] = z-h/2;
+    }
+    pv[3*1 + 0] =  + s;
+    pv[3*2 + 1] =  + t;
+    pv[3*2 + 0] =  + s;
+    pv[3*3 + 1] =  + t;
+
+    for(i=0;i<4;i++)
+    {
+        double x0,y0;
+        x0 = pv[3*i + 0 ];
+        y0 = pv[3*i + 1 ];
+        pv[3*i + 0 ] = x0 *cos(yaw) - y0 * sin(yaw) + x;
+        pv[3*i + 1 ] = x0*sin(yaw) + y0* cos(yaw) + y;
+    }
+
+    memcpy(&pv[12],&pv[0],12*sizeof(GLfloat));
+    for(i=4;i<8;i++)
+    {
+        pv[3*i + 2] = z +h/2;
+    }
+
+    QVector<GLfloat> x_data;
+    x_data.resize(72);
+
+    for(i=0;i<4;i++)
+    {
+        GLfloat *p = x_data.data() + i*6;
+        memcpy(p,pv+i*3,3*sizeof(GLfloat));
+    }
+
+    for(i=0;i<3;i++)
+    {
+        GLfloat *p = x_data.data() + i*6 +3;
+        memcpy(p,pv+(i+1)*3,3*sizeof(GLfloat));
+    }
+
+    GLfloat *p = x_data.data() + 3*6 +3;
+
+    memcpy(p,pv,3*sizeof(GLfloat));
+
+    for(i=8;i<12;i++)
+    {
+        GLfloat *p = x_data.data() + i*6;
+        memcpy(p,pv+(i-4)*3,3*sizeof(GLfloat));
+    }
+
+    for(i=8;i<11;i++)
+    {
+        GLfloat *p = x_data.data() + i*6 +3;
+        memcpy(p,pv+(i-4+1)*3,3*sizeof(GLfloat));
+    }
+
+    p = x_data.data() + 11*6 +3;
+
+    memcpy(p,pv + 12,3*sizeof(GLfloat));
+
+    for(i=4;i<8;i++)
+    {
+        GLfloat *p = x_data.data() + i*6;
+        memcpy(p,pv+(i-4)*3,3*sizeof(GLfloat));
+    }
+
+    for(i=4;i<8;i++)
+    {
+        GLfloat *p = x_data.data() + i*6 +3;
+        memcpy(p,pv+(i)*3,3*sizeof(GLfloat));
+    }
+
+    m_logoVbo.create();
+    m_logoVbo.bind();
+
+    m_logoVbo.allocate(x_data.constData(), 72 * sizeof(GLfloat));
+    setupVertexAttribs();
+
+    m_program->setUniformValue(m_colorLoc,colorx);
+
+    glDrawArrays(GL_LINES, 0, 24);
+}
+
+void GLWidget::resizeGL(int w, int h)
+{
+    m_proj.setToIdentity();
+    m_proj.perspective(80.0f, GLfloat(w) / h, -8.0f, 8.0f);
+}
+
+void GLWidget::mousePressEvent(QMouseEvent *event)
+{
+    m_lastPos = event->pos();
+}
+
+void GLWidget::mouseMoveEvent(QMouseEvent *event)
+{
+    int dx = event->x() - m_lastPos.x();
+    int dy = event->y() - m_lastPos.y();
+
+    if (event->buttons() & Qt::LeftButton) {
+        setXRotation(m_xRot + 8 * dy);
+        setYRotation(m_yRot + 8 * dx);
+    } else if (event->buttons() & Qt::RightButton) {
+        setXRotation(m_xRot + 8 * dy);
+        setZRotation(m_zRot + 8 * dx);
+    }
+
+    qDebug(" x rot is %d y rot is %d  ",m_xRot,m_yRot);
+    m_lastPos = event->pos();
+}
+
+void GLWidget::wheelEvent(QWheelEvent * e)
+{
+    QPoint qpMag = e->angleDelta();
+     int iMag = qpMag.y();
+     bool bUpdate = false;
+     if(iMag > 0)
+     {
+         if(m_iMag < 300)
+         {
+             m_iMag *= 1.3;
+             bUpdate = true;
+         }
+     }
+
+     if(iMag < 0)
+     {
+         if(m_iMag > 0.1)
+         {
+             m_iMag /= 1.3;
+             bUpdate = true;
+         }
+     }
+
+     if(bUpdate)
+     {
+         update();
+     }
+}
+
+void GLWidget::SetPointCloud(pcl::PointCloud<pcl::PointXYZI>::Ptr pc)
+{
+    mMutex.lock();
+    pc.swap(mpoint_cloud);
+    mMutex.unlock();
+//    qDebug("update");
+    update();
+}
+
+void GLWidget::SetLidarObj(iv::lidar::objectarray  lidarobjvec)
+{
+    mMutexLidarObj.lock();
+    mlidarobjvec.clear_obj();
+    mlidarobjvec.CopyFrom(lidarobjvec);
+    mMutexLidarObj.unlock();
+}
+

src/tool/glshow/glwidget.h

@@ -0,0 +1,144 @@
+/****************************************************************************
+**
+** Copyright (C) 2016 The Qt Company Ltd.
+** Contact: https://www.qt.io/licensing/
+**
+** This file is part of the examples of the Qt Toolkit.
+**
+** $QT_BEGIN_LICENSE:BSD$
+** Commercial License Usage
+** Licensees holding valid commercial Qt licenses may use this file in
+** accordance with the commercial license agreement provided with the
+** Software or, alternatively, in accordance with the terms contained in
+** a written agreement between you and The Qt Company. For licensing terms
+** and conditions see https://www.qt.io/terms-conditions. For further
+** information use the contact form at https://www.qt.io/contact-us.
+**
+** BSD License Usage
+** Alternatively, you may use this file under the terms of the BSD license
+** as follows:
+**
+** "Redistribution and use in source and binary forms, with or without
+** modification, are permitted provided that the following conditions are
+** met:
+**   * Redistributions of source code must retain the above copyright
+**     notice, this list of conditions and the following disclaimer.
+**   * Redistributions in binary form must reproduce the above copyright
+**     notice, this list of conditions and the following disclaimer in
+**     the documentation and/or other materials provided with the
+**     distribution.
+**   * Neither the name of The Qt Company Ltd nor the names of its
+**     contributors may be used to endorse or promote products derived
+**     from this software without specific prior written permission.
+**
+**
+** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+** "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+** LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+** A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+** OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+** SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+** LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+** DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+** THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+** (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+** OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE."
+**
+** $QT_END_LICENSE$
+**
+****************************************************************************/
+
+#ifndef GLWIDGET_H
+#define GLWIDGET_H
+
+#include <QOpenGLWidget>
+#include <QOpenGLFunctions>
+#include <QOpenGLVertexArrayObject>
+#include <QOpenGLBuffer>
+#include <QMatrix4x4>
+#include "logo.h"
+
+#include <QMutex>
+
+#include <pcl/conversions.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+#include "object.pb.h"
+#include "objectarray.pb.h"
+
+#include "modulecomm.h"
+
+QT_FORWARD_DECLARE_CLASS(QOpenGLShaderProgram)
+
+class GLWidget : public QOpenGLWidget, protected QOpenGLFunctions
+{
+    Q_OBJECT
+
+public:
+    GLWidget(QWidget *parent = 0);
+    ~GLWidget();
+
+    static bool isTransparent() { return m_transparent; }
+    static void setTransparent(bool t) { m_transparent = t; }
+
+    QSize minimumSizeHint() const override;
+    QSize sizeHint() const override;
+
+public slots:
+    void setXRotation(int angle);
+    void setYRotation(int angle);
+    void setZRotation(int angle);
+    void cleanup();
+
+signals:
+    void xRotationChanged(int angle);
+    void yRotationChanged(int angle);
+    void zRotationChanged(int angle);
+
+protected:
+    void initializeGL() override;
+    void paintGL() override;
+    void resizeGL(int width, int height) override;
+    void mousePressEvent(QMouseEvent *event) override;
+    void mouseMoveEvent(QMouseEvent *event) override;
+    virtual void wheelEvent(QWheelEvent *);
+
+private:
+    void setupVertexAttribs();
+
+    bool m_core;
+    int m_xRot = 600;
+    int m_yRot = 3000;
+    int m_zRot = 3000;
+    QPoint m_lastPos;
+    Logo m_logo;
+    QOpenGLVertexArrayObject m_vao;
+    QOpenGLBuffer m_logoVbo;
+    QOpenGLShaderProgram *m_program;
+    int m_projMatrixLoc;
+    int m_mvMatrixLoc;
+    int m_normalMatrixLoc;
+    int m_lightPosLoc;
+    int m_colorLoc;
+    QMatrix4x4 m_proj;
+    QMatrix4x4 m_camera;
+    QMatrix4x4 m_world;
+    static bool m_transparent;
+
+    float m_iMag = 300;
+
+    pcl::PointCloud<pcl::PointXYZI>::Ptr mpoint_cloud;
+    QMutex mMutex;
+
+    iv::lidar::objectarray mlidarobjvec;
+    QMutex mMutexLidarObj;
+
+    void drawbox(float x,float y,float z,float l,float w,float h,double yaw,QColor colorx);
+
+public:
+     void SetPointCloud(pcl::PointCloud<pcl::PointXYZI>::Ptr pc);
+     void SetLidarObj(iv::lidar::objectarray  lidarobjvec);
+};
+
+#endif

src/tool/glshow/logo.cpp

@@ -0,0 +1,151 @@
+/****************************************************************************
+**
+** Copyright (C) 2016 The Qt Company Ltd.
+** Contact: https://www.qt.io/licensing/
+**
+** This file is part of the examples of the Qt Toolkit.
+**
+** $QT_BEGIN_LICENSE:BSD$
+** Commercial License Usage
+** Licensees holding valid commercial Qt licenses may use this file in
+** accordance with the commercial license agreement provided with the
+** Software or, alternatively, in accordance with the terms contained in
+** a written agreement between you and The Qt Company. For licensing terms
+** and conditions see https://www.qt.io/terms-conditions. For further
+** information use the contact form at https://www.qt.io/contact-us.
+**
+** BSD License Usage
+** Alternatively, you may use this file under the terms of the BSD license
+** as follows:
+**
+** "Redistribution and use in source and binary forms, with or without
+** modification, are permitted provided that the following conditions are
+** met:
+**   * Redistributions of source code must retain the above copyright
+**     notice, this list of conditions and the following disclaimer.
+**   * Redistributions in binary form must reproduce the above copyright
+**     notice, this list of conditions and the following disclaimer in
+**     the documentation and/or other materials provided with the
+**     distribution.
+**   * Neither the name of The Qt Company Ltd nor the names of its
+**     contributors may be used to endorse or promote products derived
+**     from this software without specific prior written permission.
+**
+**
+** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+** "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+** LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+** A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+** OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+** SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+** LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+** DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+** THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+** (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+** OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE."
+**
+** $QT_END_LICENSE$
+**
+****************************************************************************/
+
+#include "logo.h"
+#include <qmath.h>
+
+Logo::Logo()
+    : m_count(0)
+{
+    m_data.resize(2500 * 6);
+
+    const GLfloat x1 = +0.06f;
+    const GLfloat y1 = -0.14f;
+    const GLfloat x2 = +0.14f;
+    const GLfloat y2 = -0.06f;
+    const GLfloat x3 = +0.08f;
+    const GLfloat y3 = +0.00f;
+    const GLfloat x4 = +0.30f;
+    const GLfloat y4 = +0.22f;
+
+    quad(x1, y1, x2, y2, y2, x2, y1, x1);
+    quad(x3, y3, x4, y4, y4, x4, y3, x3);
+
+    extrude(x1, y1, x2, y2);
+    extrude(x2, y2, y2, x2);
+    extrude(y2, x2, y1, x1);
+    extrude(y1, x1, x1, y1);
+    extrude(x3, y3, x4, y4);
+    extrude(x4, y4, y4, x4);
+    extrude(y4, x4, y3, x3);
+
+    const int NumSectors = 100;
+
+    for (int i = 0; i < NumSectors; ++i) {
+        GLfloat angle = (i * 2 * M_PI) / NumSectors;
+        GLfloat angleSin = qSin(angle);
+        GLfloat angleCos = qCos(angle);
+        const GLfloat x5 = 0.30f * angleSin;
+        const GLfloat y5 = 0.30f * angleCos;
+        const GLfloat x6 = 0.20f * angleSin;
+        const GLfloat y6 = 0.20f * angleCos;
+
+        angle = ((i + 1) * 2 * M_PI) / NumSectors;
+        angleSin = qSin(angle);
+        angleCos = qCos(angle);
+        const GLfloat x7 = 0.20f * angleSin;
+        const GLfloat y7 = 0.20f * angleCos;
+        const GLfloat x8 = 0.30f * angleSin;
+        const GLfloat y8 = 0.30f * angleCos;
+
+        quad(x5, y5, x6, y6, x7, y7, x8, y8);
+
+        extrude(x6, y6, x7, y7);
+        extrude(x8, y8, x5, y5);
+    }
+}
+
+void Logo::add(const QVector3D &v, const QVector3D &n)
+{
+    GLfloat *p = m_data.data() + m_count;
+    *p++ = v.x();
+    *p++ = v.y();
+    *p++ = v.z();
+    *p++ = n.x();
+    *p++ = n.y();
+    *p++ = n.z();
+    m_count += 6;
+}
+
+void Logo::quad(GLfloat x1, GLfloat y1, GLfloat x2, GLfloat y2, GLfloat x3, GLfloat y3, GLfloat x4, GLfloat y4)
+{
+    QVector3D n = QVector3D::normal(QVector3D(x4 - x1, y4 - y1, 0.0f), QVector3D(x2 - x1, y2 - y1, 0.0f));
+
+    add(QVector3D(x1, y1, -0.05f), n);
+    add(QVector3D(x4, y4, -0.05f), n);
+    add(QVector3D(x2, y2, -0.05f), n);
+
+    add(QVector3D(x3, y3, -0.05f), n);
+    add(QVector3D(x2, y2, -0.05f), n);
+    add(QVector3D(x4, y4, -0.05f), n);
+
+    n = QVector3D::normal(QVector3D(x1 - x4, y1 - y4, 0.0f), QVector3D(x2 - x4, y2 - y4, 0.0f));
+
+    add(QVector3D(x4, y4, 0.05f), n);
+    add(QVector3D(x1, y1, 0.05f), n);
+    add(QVector3D(x2, y2, 0.05f), n);
+
+    add(QVector3D(x2, y2, 0.05f), n);
+    add(QVector3D(x3, y3, 0.05f), n);
+    add(QVector3D(x4, y4, 0.05f), n);
+}
+
+void Logo::extrude(GLfloat x1, GLfloat y1, GLfloat x2, GLfloat y2)
+{
+    QVector3D n = QVector3D::normal(QVector3D(0.0f, 0.0f, -0.1f), QVector3D(x2 - x1, y2 - y1, 0.0f));
+
+    add(QVector3D(x1, y1, +0.05f), n);
+    add(QVector3D(x1, y1, -0.05f), n);
+    add(QVector3D(x2, y2, +0.05f), n);
+
+    add(QVector3D(x2, y2, -0.05f), n);
+    add(QVector3D(x2, y2, +0.05f), n);
+    add(QVector3D(x1, y1, -0.05f), n);
+}

src/tool/glshow/logo.h

@@ -0,0 +1,75 @@
+/****************************************************************************
+**
+** Copyright (C) 2016 The Qt Company Ltd.
+** Contact: https://www.qt.io/licensing/
+**
+** This file is part of the examples of the Qt Toolkit.
+**
+** $QT_BEGIN_LICENSE:BSD$
+** Commercial License Usage
+** Licensees holding valid commercial Qt licenses may use this file in
+** accordance with the commercial license agreement provided with the
+** Software or, alternatively, in accordance with the terms contained in
+** a written agreement between you and The Qt Company. For licensing terms
+** and conditions see https://www.qt.io/terms-conditions. For further
+** information use the contact form at https://www.qt.io/contact-us.
+**
+** BSD License Usage
+** Alternatively, you may use this file under the terms of the BSD license
+** as follows:
+**
+** "Redistribution and use in source and binary forms, with or without
+** modification, are permitted provided that the following conditions are
+** met:
+**   * Redistributions of source code must retain the above copyright
+**     notice, this list of conditions and the following disclaimer.
+**   * Redistributions in binary form must reproduce the above copyright
+**     notice, this list of conditions and the following disclaimer in
+**     the documentation and/or other materials provided with the
+**     distribution.
+**   * Neither the name of The Qt Company Ltd nor the names of its
+**     contributors may be used to endorse or promote products derived
+**     from this software without specific prior written permission.
+**
+**
+** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+** "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+** LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+** A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+** OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+** SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+** LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+** DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+** THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+** (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+** OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE."
+**
+** $QT_END_LICENSE$
+**
+****************************************************************************/
+
+#ifndef LOGO_H
+#define LOGO_H
+
+#include <qopengl.h>
+#include <QVector>
+#include <QVector3D>
+
+class Logo
+{
+public:
+    Logo();
+    const GLfloat *constData() const { return m_data.constData(); }
+    int count() const { return m_count; }
+    int vertexCount() const { return m_count / 6; }
+
+private:
+    void quad(GLfloat x1, GLfloat y1, GLfloat x2, GLfloat y2, GLfloat x3, GLfloat y3, GLfloat x4, GLfloat y4);
+    void extrude(GLfloat x1, GLfloat y1, GLfloat x2, GLfloat y2);
+    void add(const QVector3D &v, const QVector3D &n);
+
+    QVector<GLfloat> m_data;
+    int m_count;
+};
+
+#endif // LOGO_H

src/tool/glshow/main.cpp

@@ -0,0 +1,106 @@
+/****************************************************************************
+**
+** Copyright (C) 2016 The Qt Company Ltd.
+** Contact: https://www.qt.io/licensing/
+**
+** This file is part of the examples of the Qt Toolkit.
+**
+** $QT_BEGIN_LICENSE:BSD$
+** Commercial License Usage
+** Licensees holding valid commercial Qt licenses may use this file in
+** accordance with the commercial license agreement provided with the
+** Software or, alternatively, in accordance with the terms contained in
+** a written agreement between you and The Qt Company. For licensing terms
+** and conditions see https://www.qt.io/terms-conditions. For further
+** information use the contact form at https://www.qt.io/contact-us.
+**
+** BSD License Usage
+** Alternatively, you may use this file under the terms of the BSD license
+** as follows:
+**
+** "Redistribution and use in source and binary forms, with or without
+** modification, are permitted provided that the following conditions are
+** met:
+**   * Redistributions of source code must retain the above copyright
+**     notice, this list of conditions and the following disclaimer.
+**   * Redistributions in binary form must reproduce the above copyright
+**     notice, this list of conditions and the following disclaimer in
+**     the documentation and/or other materials provided with the
+**     distribution.
+**   * Neither the name of The Qt Company Ltd nor the names of its
+**     contributors may be used to endorse or promote products derived
+**     from this software without specific prior written permission.
+**
+**
+** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+** "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+** LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+** A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+** OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+** SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+** LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+** DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+** THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+** (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+** OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE."
+**
+** $QT_END_LICENSE$
+**
+****************************************************************************/
+
+#include <QApplication>
+#include <QDesktopWidget>
+#include <QSurfaceFormat>
+#include <QCommandLineParser>
+#include <QCommandLineOption>
+
+#include "glwidget.h"
+#include "mainwindow.h"
+
+int main(int argc, char *argv[])
+{
+    QApplication app(argc, argv);
+
+    QCoreApplication::setApplicationName("opengl show");
+    QCoreApplication::setOrganizationName("QtProject");
+    QCoreApplication::setApplicationVersion(QT_VERSION_STR);
+    QCommandLineParser parser;
+    parser.setApplicationDescription(QCoreApplication::applicationName());
+    parser.addHelpOption();
+    parser.addVersionOption();
+    QCommandLineOption multipleSampleOption("multisample", "Multisampling");
+    parser.addOption(multipleSampleOption);
+    QCommandLineOption coreProfileOption("coreprofile", "Use core profile");
+    parser.addOption(coreProfileOption);
+    QCommandLineOption transparentOption("transparent", "Transparent window");
+    parser.addOption(transparentOption);
+
+    parser.process(app);
+
+    QSurfaceFormat fmt;
+    fmt.setDepthBufferSize(24);
+    if (parser.isSet(multipleSampleOption))
+        fmt.setSamples(4);
+    if (parser.isSet(coreProfileOption)) {
+        fmt.setVersion(3, 2);
+        fmt.setProfile(QSurfaceFormat::CoreProfile);
+    }
+    QSurfaceFormat::setDefaultFormat(fmt);
+
+    MainWindow mainWindow;
+
+    GLWidget::setTransparent(parser.isSet(transparentOption));
+    if (GLWidget::isTransparent()) {
+        mainWindow.setAttribute(Qt::WA_TranslucentBackground);
+        mainWindow.setAttribute(Qt::WA_NoSystemBackground, false);
+    }
+    mainWindow.resize(mainWindow.sizeHint());
+    int desktopArea = QApplication::desktop()->width() *
+                     QApplication::desktop()->height();
+    int widgetArea = mainWindow.width() * mainWindow.height();
+    if (((float)widgetArea / (float)desktopArea) < 0.75f)
+        mainWindow.show();
+    else
+        mainWindow.showMaximized();
+    return app.exec();
+}

src/tool/glshow/mainwindow.cpp

@@ -0,0 +1,76 @@
+/****************************************************************************
+**
+** Copyright (C) 2016 The Qt Company Ltd.
+** Contact: https://www.qt.io/licensing/
+**
+** This file is part of the examples of the Qt Toolkit.
+**
+** $QT_BEGIN_LICENSE:BSD$
+** Commercial License Usage
+** Licensees holding valid commercial Qt licenses may use this file in
+** accordance with the commercial license agreement provided with the
+** Software or, alternatively, in accordance with the terms contained in
+** a written agreement between you and The Qt Company. For licensing terms
+** and conditions see https://www.qt.io/terms-conditions. For further
+** information use the contact form at https://www.qt.io/contact-us.
+**
+** BSD License Usage
+** Alternatively, you may use this file under the terms of the BSD license
+** as follows:
+**
+** "Redistribution and use in source and binary forms, with or without
+** modification, are permitted provided that the following conditions are
+** met:
+**   * Redistributions of source code must retain the above copyright
+**     notice, this list of conditions and the following disclaimer.
+**   * Redistributions in binary form must reproduce the above copyright
+**     notice, this list of conditions and the following disclaimer in
+**     the documentation and/or other materials provided with the
+**     distribution.
+**   * Neither the name of The Qt Company Ltd nor the names of its
+**     contributors may be used to endorse or promote products derived
+**     from this software without specific prior written permission.
+**
+**
+** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+** "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+** LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+** A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+** OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+** SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+** LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+** DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+** THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+** (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+** OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE."
+**
+** $QT_END_LICENSE$
+**
+****************************************************************************/
+
+#include "mainwindow.h"
+#include "window.h"
+#include <QMenuBar>
+#include <QMenu>
+#include <QMessageBox>
+
+MainWindow::MainWindow()
+{
+    QMenuBar *menuBar = new QMenuBar;
+    QMenu *menuWindow = menuBar->addMenu(tr("&Window"));
+    QAction *addNew = new QAction(menuWindow);
+    addNew->setText(tr("Add new"));
+    menuWindow->addAction(addNew);
+    connect(addNew, &QAction::triggered, this, &MainWindow::onAddNew);
+    setMenuBar(menuBar);
+
+    onAddNew();
+}
+
+void MainWindow::onAddNew()
+{
+    if (!centralWidget())
+        setCentralWidget(new Window(this));
+    else
+        QMessageBox::information(0, tr("Cannot add new window"), tr("Already occupied. Undock first."));
+}

src/tool/glshow/mainwindow.h

@@ -0,0 +1,67 @@
+/****************************************************************************
+**
+** Copyright (C) 2016 The Qt Company Ltd.
+** Contact: https://www.qt.io/licensing/
+**
+** This file is part of the examples of the Qt Toolkit.
+**
+** $QT_BEGIN_LICENSE:BSD$
+** Commercial License Usage
+** Licensees holding valid commercial Qt licenses may use this file in
+** accordance with the commercial license agreement provided with the
+** Software or, alternatively, in accordance with the terms contained in
+** a written agreement between you and The Qt Company. For licensing terms
+** and conditions see https://www.qt.io/terms-conditions. For further
+** information use the contact form at https://www.qt.io/contact-us.
+**
+** BSD License Usage
+** Alternatively, you may use this file under the terms of the BSD license
+** as follows:
+**
+** "Redistribution and use in source and binary forms, with or without
+** modification, are permitted provided that the following conditions are
+** met:
+**   * Redistributions of source code must retain the above copyright
+**     notice, this list of conditions and the following disclaimer.
+**   * Redistributions in binary form must reproduce the above copyright
+**     notice, this list of conditions and the following disclaimer in
+**     the documentation and/or other materials provided with the
+**     distribution.
+**   * Neither the name of The Qt Company Ltd nor the names of its
+**     contributors may be used to endorse or promote products derived
+**     from this software without specific prior written permission.
+**
+**
+** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+** "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+** LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+** A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+** OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+** SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+** LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+** DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+** THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+** (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+** OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE."
+**
+** $QT_END_LICENSE$
+**
+****************************************************************************/
+
+#ifndef MAINWINDOW_H
+#define MAINWINDOW_H
+
+#include <QMainWindow>
+
+class MainWindow : public QMainWindow
+{
+    Q_OBJECT
+
+public:
+    MainWindow();
+
+private slots:
+    void onAddNew();
+};
+
+#endif

src/tool/glshow/window.cpp

@@ -0,0 +1,143 @@
+/****************************************************************************
+**
+** Copyright (C) 2016 The Qt Company Ltd.
+** Contact: https://www.qt.io/licensing/
+**
+** This file is part of the examples of the Qt Toolkit.
+**
+** $QT_BEGIN_LICENSE:BSD$
+** Commercial License Usage
+** Licensees holding valid commercial Qt licenses may use this file in
+** accordance with the commercial license agreement provided with the
+** Software or, alternatively, in accordance with the terms contained in
+** a written agreement between you and The Qt Company. For licensing terms
+** and conditions see https://www.qt.io/terms-conditions. For further
+** information use the contact form at https://www.qt.io/contact-us.
+**
+** BSD License Usage
+** Alternatively, you may use this file under the terms of the BSD license
+** as follows:
+**
+** "Redistribution and use in source and binary forms, with or without
+** modification, are permitted provided that the following conditions are
+** met:
+**   * Redistributions of source code must retain the above copyright
+**     notice, this list of conditions and the following disclaimer.
+**   * Redistributions in binary form must reproduce the above copyright
+**     notice, this list of conditions and the following disclaimer in
+**     the documentation and/or other materials provided with the
+**     distribution.
+**   * Neither the name of The Qt Company Ltd nor the names of its
+**     contributors may be used to endorse or promote products derived
+**     from this software without specific prior written permission.
+**
+**
+** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+** "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+** LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+** A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+** OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+** SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+** LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+** DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+** THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+** (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+** OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE."
+**
+** $QT_END_LICENSE$
+**
+****************************************************************************/
+
+#include "glwidget.h"
+#include "window.h"
+#include "mainwindow.h"
+#include <QSlider>
+#include <QVBoxLayout>
+#include <QHBoxLayout>
+#include <QKeyEvent>
+#include <QPushButton>
+#include <QDesktopWidget>
+#include <QApplication>
+#include <QMessageBox>
+
+Window::Window(MainWindow *mw)
+    : mainWindow(mw)
+{
+    glWidget = new GLWidget;
+
+    xSlider = createSlider();
+    ySlider = createSlider();
+    zSlider = createSlider();
+
+    connect(xSlider, &QSlider::valueChanged, glWidget, &GLWidget::setXRotation);
+    connect(glWidget, &GLWidget::xRotationChanged, xSlider, &QSlider::setValue);
+    connect(ySlider, &QSlider::valueChanged, glWidget, &GLWidget::setYRotation);
+    connect(glWidget, &GLWidget::yRotationChanged, ySlider, &QSlider::setValue);
+    connect(zSlider, &QSlider::valueChanged, glWidget, &GLWidget::setZRotation);
+    connect(glWidget, &GLWidget::zRotationChanged, zSlider, &QSlider::setValue);
+
+    QVBoxLayout *mainLayout = new QVBoxLayout;
+    QHBoxLayout *container = new QHBoxLayout;
+    container->addWidget(glWidget);
+    container->addWidget(xSlider);
+    container->addWidget(ySlider);
+    container->addWidget(zSlider);
+
+    QWidget *w = new QWidget;
+    w->setLayout(container);
+    mainLayout->addWidget(w);
+    dockBtn = new QPushButton(tr("Undock"), this);
+    connect(dockBtn, &QPushButton::clicked, this, &Window::dockUndock);
+    mainLayout->addWidget(dockBtn);
+
+    setLayout(mainLayout);
+
+    xSlider->setValue(50 * 16);
+    ySlider->setValue(180 * 16);
+    zSlider->setValue(180* 16);
+
+    setWindowTitle(tr("Hello GL"));
+}
+
+QSlider *Window::createSlider()
+{
+    QSlider *slider = new QSlider(Qt::Vertical);
+    slider->setRange(0, 360 * 16);
+    slider->setSingleStep(16);
+    slider->setPageStep(15 * 16);
+    slider->setTickInterval(15 * 16);
+    slider->setTickPosition(QSlider::TicksRight);
+    return slider;
+}
+
+void Window::keyPressEvent(QKeyEvent *e)
+{
+    if (e->key() == Qt::Key_Escape)
+        close();
+    else
+        QWidget::keyPressEvent(e);
+}
+
+void Window::dockUndock()
+{
+    if (parent()) {
+        setParent(0);
+        setAttribute(Qt::WA_DeleteOnClose);
+        move(QApplication::desktop()->width() / 2 - width() / 2,
+             QApplication::desktop()->height() / 2 - height() / 2);
+        dockBtn->setText(tr("Dock"));
+        show();
+    } else {
+        if (!mainWindow->centralWidget()) {
+            if (mainWindow->isVisible()) {
+                setAttribute(Qt::WA_DeleteOnClose, false);
+                dockBtn->setText(tr("Undock"));
+                mainWindow->setCentralWidget(this);
+            } else {
+                QMessageBox::information(0, tr("Cannot dock"), tr("Main window already closed"));
+            }
+        } else {
+            QMessageBox::information(0, tr("Cannot dock"), tr("Main window already occupied"));
+        }
+    }
+}

src/tool/glshow/window.h

@@ -0,0 +1,88 @@
+/****************************************************************************
+**
+** Copyright (C) 2016 The Qt Company Ltd.
+** Contact: https://www.qt.io/licensing/
+**
+** This file is part of the examples of the Qt Toolkit.
+**
+** $QT_BEGIN_LICENSE:BSD$
+** Commercial License Usage
+** Licensees holding valid commercial Qt licenses may use this file in
+** accordance with the commercial license agreement provided with the
+** Software or, alternatively, in accordance with the terms contained in
+** a written agreement between you and The Qt Company. For licensing terms
+** and conditions see https://www.qt.io/terms-conditions. For further
+** information use the contact form at https://www.qt.io/contact-us.
+**
+** BSD License Usage
+** Alternatively, you may use this file under the terms of the BSD license
+** as follows:
+**
+** "Redistribution and use in source and binary forms, with or without
+** modification, are permitted provided that the following conditions are
+** met:
+**   * Redistributions of source code must retain the above copyright
+**     notice, this list of conditions and the following disclaimer.
+**   * Redistributions in binary form must reproduce the above copyright
+**     notice, this list of conditions and the following disclaimer in
+**     the documentation and/or other materials provided with the
+**     distribution.
+**   * Neither the name of The Qt Company Ltd nor the names of its
+**     contributors may be used to endorse or promote products derived
+**     from this software without specific prior written permission.
+**
+**
+** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+** "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+** LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+** A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+** OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+** SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+** LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+** DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+** THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+** (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+** OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE."
+**
+** $QT_END_LICENSE$
+**
+****************************************************************************/
+
+#ifndef WINDOW_H
+#define WINDOW_H
+
+#include <QWidget>
+
+QT_BEGIN_NAMESPACE
+class QSlider;
+class QPushButton;
+QT_END_NAMESPACE
+
+class GLWidget;
+class MainWindow;
+
+class Window : public QWidget
+{
+    Q_OBJECT
+
+public:
+    Window(MainWindow *mw);
+
+protected:
+    void keyPressEvent(QKeyEvent *event) override;
+
+private slots:
+    void dockUndock();
+
+private:
+    QSlider *createSlider();
+
+    GLWidget *glWidget;
+    QSlider *xSlider;
+    QSlider *ySlider;
+    QSlider *zSlider;
+    QPushButton *dockBtn;
+    MainWindow *mainWindow;
+};
+
+#endif

src1/decision/decision_brain/decision_brain.pro

@@ -29,3 +29,12 @@ else: unix:!android: target.path = /opt/$${TARGET}/bin
 !include(../interface/ivdecision.pri ) {
     error( "Couldn't find the ivdecision.pri file!" )
 }
+
+!include(../../../include/ivboost.pri ) {
+    error( "Couldn't find the ivboost.pri file!" )
+}
+
+!include(../../../include/iveigen.pri ) {
+    error( "Couldn't find the iveigen.pri file!" )
+
+}

src1/decision/interface/ivdecision.cpp

@@ -1,5 +1,7 @@
 #include "ivdecision.h"
 
+#include "gnss_coordinate_convert.h"
+
 namespace iv {
 
 
@@ -8,4 +10,112 @@ ivdecision::ivdecision()
 
 }
 
+void ivdecision::modulerun()
+{
+
+
+    while(mbrun)
+    {
+        //RunDecision
+        //ShareMsg
+    }
+}
+
+void ivdecision::UpdateGPSMsg(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
+{
+    (void )&index;
+    (void )&dt;
+    (void )strmemname;
+
+    if(nSize<1)return;
+
+    iv::gps::gpsimu xgpsimu;
+    if(!xgpsimu.ParseFromArray(strdata,nSize))
+    {
+       std::cout<<"ivdecision::UpdateGPSMsg error."<<std::endl;
+       return;
+    }
+
+    iv::GPSData data(new iv::GPS_INS);
+    data->ins_status = xgpsimu.ins_state();
+    data->rtk_status = xgpsimu.rtk_state();
+
+    data->accel_x = xgpsimu.acce_x();
+    data->accel_y = xgpsimu.acce_y();
+    data->accel_z = xgpsimu.acce_z();
+    data->ang_rate_x = xgpsimu.gyro_x();
+    data->ang_rate_y = xgpsimu.gyro_y();
+    data->ang_rate_z = xgpsimu.gyro_z();
+
+    data->gps_lat = xgpsimu.lat();
+    data->gps_lng = xgpsimu.lon();
+    data->gps_z = xgpsimu.height();
+
+    data->ins_heading_angle = xgpsimu.heading();
+    data->ins_pitch_angle = xgpsimu.pitch();
+    data->ins_roll_angle = xgpsimu.roll();
+
+    data->speed = sqrt(pow(xgpsimu.ve(),2)+pow(xgpsimu.vn(),2)) * 3.6;
+
+    GaussProjCal(data->gps_lng, data->gps_lat, &data->gps_x, &data->gps_y);
+
+    mMutexGPS.lock();
+    iv::gps::gpsimu * pgpsimu = new iv::gps::gpsimu;
+    pgpsimu->CopyFrom(xgpsimu);
+    mGPSIMUptr.reset(pgpsimu);
+    mNowGPS = data;
+    mnGPSUpdateTime = QDateTime::currentMSecsSinceEpoch();
+    mMutexGPS.unlock();
+
+
+
+
+}
+
+bool ivdecision::GetGPS(GPSData &xGPSData)
+{
+    if((QDateTime::currentMSecsSinceEpoch() - mnGPSUpdateTime)>mnNewThresh)
+    {
+        xGPSData = NULL;
+        return false;
+    }
+    xGPSData = mNowGPS;
+    return true;
+}
+
+void ivdecision::UpdateRADARMsg(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
+{
+    (void )&index;
+    (void )&dt;
+    (void )strmemname;
+
+    if(nSize<1)return;
+
+    iv::radar::radarobjectarray * pradar = new iv::radar::radarobjectarray;
+    if(!pradar->ParseFromArray(strdata,nSize))
+    {
+        std::cout<<"ivdecision::UpdateRADARMsg Parse Error"<<std::endl;
+        return;
+    }
+
+    mMutexRADAR.lock();
+    mRADAR.reset(pradar);
+    mnRADARUpdateTime = QDateTime::currentMSecsSinceEpoch();
+    mMutexRADAR.unlock();
+
+}
+
+bool ivdecision::GetRADAR(std::shared_ptr<radar::radarobjectarray> &xRADAR)
+{
+    if((QDateTime::currentMSecsSinceEpoch() - mnRADARUpdateTime)>mnNewThresh)
+    {
+        xRADAR = NULL;
+        return false;
+    }
+    xRADAR = mRADAR;
+    return true;
+}
+
+
+
 }

src1/decision/interface/ivdecision.h

@@ -1,12 +1,79 @@
 #ifndef IVDECISION_H
 #define IVDECISION_H
 
+#include "ivmodule.h"
+
+#include "modulecomm.h"
+
+#include "gps_type.h"
+
+#include "gpsimu.pb.h"
+
+#include "radarobjectarray.pb.h"
+
+#include <QMutex>
+
+#include <memory>
+
 namespace iv {
 
-class ivdecision
+class ivdecision : public ivmodule
 {
 public:
     ivdecision();
+
+public:
+    virtual void modulerun();
+
+
+public:
+    virtual void rundecision() = 0;
+    virtual void sharemsg() = 0;
+
+private:
+    std::string strgpsmsgname = "hcp2_gpsimu";
+    std::string strlidarmsgname = "lidar_obs";
+    std::string strradarmsgname = "radar";
+    std::string strmapmsgname = "tracemap";
+    std::string strmobileyemsgname = "mobileye";
+
+private:
+    void * mpagpsmsg;
+    void * mpalidarmsg;
+    void * mparadarmsg;
+    void * mpamapmsg;
+    void * mpamobileyemsg;
+
+private:
+    void UpdateGPSMsg(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
+    void UpdateRADARMsg(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
+
+private:
+
+    iv::GPSData mNowGPS;
+    std::shared_ptr<iv::gps::gpsimu> mGPSIMUptr = 0;
+    qint64 mnGPSUpdateTime = 0;
+    QMutex mMutexGPS;
+
+    std::shared_ptr<iv::radar::radarobjectarray> mRADAR = 0;
+    qint64 mnRADARUpdateTime = 0;
+    QMutex mMutexRADAR;
+
+
+
+private:
+    const qint64 mnNewThresh = 1000; //3 seconds
+
+
+public:
+    bool GetGPS(iv::GPSData & xGPSData);
+    bool GetRADAR(std::shared_ptr<iv::radar::radarobjectarray> & xRADAR);
+//    void GetLidarPtr();
+//    void GetRadar();
+//    void GetMap();
+//    bool IsMapUpdate();
+//    void GetMobileye();
+
 };
 
 }

src1/math/gnss_coordinate_convert.cpp

@@ -0,0 +1,153 @@
+#include <gnss_coordinate_convert.h>
+
+void gps2xy(double J4, double K4, double *x, double *y)
+{
+    int L4 = (int)((K4 - 1.5) / 3) + 1;
+    double M4 = K4 - (L4 * 3);
+    double N4 = sin(J4 * 3.1415926536 / 180);
+    double O4 = cos(J4 * 3.1415926536 / 180);
+    double P4 = tan(J4 * 3.1415926536 / 180);
+    double Q4 = 111134.8611 * J4 - N4 * O4 * (32005.7799 + 133.9238 * N4 * N4 + 0.6973 * N4 * N4 * N4 * N4 + 0.0039 * N4 * N4 * N4 * N4 * N4 * N4);
+    double R4 = sqrt(0.006738525414683) * O4;
+    double S4 = sqrt(1 + R4 * R4);
+    double T4 = 6399698.901783 / S4;
+    double U4 = (T4 / S4) / S4;
+    double V4 = O4 * M4 * 3.1415926536 / 180;
+    double W4 = 0.5 + (5 - P4 * P4 + 9 * R4 * R4 + 4 * R4 * R4 * R4 * R4) * V4 * V4 / 24;
+    double X4 = V4 * V4 * V4 * V4 / 720 * (61 + (P4 * P4 - 58) * P4 * P4);
+    double Y4 = 1 + V4 * V4 * (1 - P4 * P4 + R4 * R4) / 6;
+    double Z4 = V4 * V4 * V4 * V4 * (5 - 18 * P4 * P4 * P4 * P4 * P4 * P4 + 14 * R4 * R4 - 58 * R4 * R4 * P4 * P4) / 120;
+
+    *y = Q4 + T4 * P4 * V4 * V4 * (W4 + X4);
+    *x = 500000 + T4 * V4 * (Y4 + Z4);
+}
+/*
+//高斯投影由经纬度(Unit:DD)反算大地坐标(含带号，Unit:Metres)
+void GaussProjCal(double longitude, double latitude, double *X, double *Y)
+{
+    int ProjNo = 0; int ZoneWide; ////带宽
+    double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval;
+    double a, f, e2, ee, NN, T, C, A, M, iPI;
+    iPI = 0.0174532925199433; ////3.1415926535898/180.0;
+    ZoneWide = 6; ////6度带宽
+    a = 6378245.0; f = 1.0 / 298.3; //54年北京坐标系参数
+                                    ////a=6378140.0; f=1/298.257; //80年西安坐标系参数
+    ProjNo = (int)(longitude / ZoneWide);
+    longitude0 = ProjNo * ZoneWide + ZoneWide / 2;
+    longitude0 = longitude0 * iPI;
+    latitude0 = 0;
+    longitude1 = longitude * iPI; //经度转换为弧度
+    latitude1 = latitude * iPI; //纬度转换为弧度
+    e2 = 2 * f - f * f;
+    ee = e2 * (1.0 - e2);
+    NN = a / sqrt(1.0 - e2 * sin(latitude1)*sin(latitude1));
+    T = tan(latitude1)*tan(latitude1);
+    C = ee * cos(latitude1)*cos(latitude1);
+    A = (longitude1 - longitude0)*cos(latitude1);
+    M = a * ((1 - e2 / 4 - 3 * e2*e2 / 64 - 5 * e2*e2*e2 / 256)*latitude1 - (3 * e2 / 8 + 3 * e2*e2 / 32 + 45 * e2*e2
+        *e2 / 1024)*sin(2 * latitude1)
+        + (15 * e2*e2 / 256 + 45 * e2*e2*e2 / 1024)*sin(4 * latitude1) - (35 * e2*e2*e2 / 3072)*sin(6 * latitude1));
+    xval = NN * (A + (1 - T + C)*A*A*A / 6 + (5 - 18 * T + T * T + 72 * C - 58 * ee)*A*A*A*A*A / 120);
+    yval = M + NN * tan(latitude1)*(A*A / 2 + (5 - T + 9 * C + 4 * C*C)*A*A*A*A / 24
+        + (61 - 58 * T + T * T + 600 * C - 330 * ee)*A*A*A*A*A*A / 720);
+    X0 = 1000000L * (ProjNo + 1) + 500000L;
+    Y0 = 0;
+    xval = xval + X0; yval = yval + Y0;
+    *X = xval;
+    *Y = yval;
+}
+*/
+
+
+//=======================zhaobo0904
+#define PI  3.14159265358979
+void GaussProjCal(double lon, double lat, double *X, double *Y)
+{
+    // 1975 年国际椭球体长半轴 a, 第一离心率 e2, 第二离心率 ep2
+    double a = 6378140.0;
+    double e2 = 0.006694384999588;
+    double ep2 = e2/(1-e2);
+
+    // 原点所在经度
+    double lon_origin = 6.0*int(lon/6) + 3.0;
+    lon_origin *= PI / 180.0;
+
+    double k0 = 0.9996;
+
+    // 角度转弧度
+    double lat1 = lat * PI / 180.0;
+    double lon1 = lon * PI / 180.0;
+
+
+    // 经线在该点处的曲率半径,
+    double N = a / sqrt(1 - e2*sin(lat1)*sin(lat1));
+
+
+    // 赤道到该点的经线长度近似值 M, 使用泰勒展开逐项积分然后取前四项.
+    // 这个近似值是将 N 作为纬度 \phi 的函数展开为泰勒计数, 然后在区间 [0, lat1] 上积分得到的.
+    // 首先计算前四项的系数 a1~a4.
+    double a1 = 1 - e2/4 - (3*e2*e2)/64 - (5*e2*e2*e2)/256;
+    double a2 = (3*e2)/8 + (3*e2*e2)/32 + (45*e2*e2*e2)/1024;
+    double a3 = (15*e2*e2)/256 + (45*e2*e2*e2)/1024;
+    double a4 = (35*e2*e2*e2)/3072;
+    double M = a * (a1*lat1 - a2*sin(2*lat1) + a3*sin(4*lat1) - a4*sin(6*lat1));
+
+    // 辅助量
+    double T = tan(lat1)*tan(lat1);
+    double C = ep2*cos(lat1)*cos(lat1);
+    double A = (lon1 - lon_origin)*cos(lat1);
+
+    *X = 500000.0 + k0 * N * (A + (1 - T + C)*(A*A*A)/6 + (5 - 18*T + T*T + 72*C - 58*ep2)*(A*A*A*A*A)/120);
+    *Y = M + N * tan(lat1) * ((A*A)/2 +
+                              (5 - T + 9*C + 4*C*C)*(A*A*A*A)/24 +
+                              (61 - 58*T + T*T + 600*C - 330*ep2)*(A*A*A*A*A*A)/720);
+
+
+    *Y *= k0;
+    return;
+}
+//==========================================================
+
+
+
+
+
+//高斯投影由大地坐标(Unit:Metres)反算经纬度(Unit:DD)
+void GaussProjInvCal(double X, double Y, double *longitude, double *latitude)
+{
+    int ProjNo; int ZoneWide; ////带宽
+    double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval;
+    double e1, e2, f, a, ee, NN, T, C, M, D, R, u, fai, iPI;
+    iPI = 0.0174532925199433; ////3.1415926535898/180.0;
+    a = 6378245.0; f = 1.0 / 298.3; //54年北京坐标系参数
+    ////a=6378140.0; f=1/298.257; //80年西安坐标系参数
+    ZoneWide = 6; ////6度带宽
+    ProjNo = (int)(X / 1000000L); //查找带号
+    longitude0 = (ProjNo - 1) * ZoneWide + ZoneWide / 2;
+    longitude0 = longitude0 * iPI; //中央经线
+    X0 = ProjNo * 1000000L + 500000L;
+    Y0 = 0;
+    xval = X - X0; yval = Y - Y0; //带内大地坐标
+    e2 = 2 * f - f * f;
+    e1 = (1.0 - sqrt(1 - e2)) / (1.0 + sqrt(1 - e2));
+    ee = e2 / (1 - e2);
+    M = yval;
+    u = M / (a*(1 - e2 / 4 - 3 * e2*e2 / 64 - 5 * e2*e2*e2 / 256));
+    fai = u + (3 * e1 / 2 - 27 * e1*e1*e1 / 32)*sin(2 * u) + (21 * e1*e1 / 16 - 55 * e1*e1*e1*e1 / 32)*sin(
+                4 * u)
+            + (151 * e1*e1*e1 / 96)*sin(6 * u) + (1097 * e1*e1*e1*e1 / 512)*sin(8 * u);
+    C = ee * cos(fai)*cos(fai);
+    T = tan(fai)*tan(fai);
+    NN = a / sqrt(1.0 - e2 * sin(fai)*sin(fai));
+    R = a * (1 - e2) / sqrt((1 - e2 * sin(fai)*sin(fai))*(1 - e2 * sin(fai)*sin(fai))*(1 - e2 * sin
+                                                                                       (fai)*sin(fai)));
+    D = xval / NN;
+    //计算经度(Longitude) 纬度(Latitude)
+    longitude1 = longitude0 + (D - (1 + 2 * T + C)*D*D*D / 6 + (5 - 2 * C + 28 * T - 3 * C*C + 8 * ee + 24 * T*T)*D
+                               *D*D*D*D / 120) / cos(fai);
+    latitude1 = fai - (NN*tan(fai) / R)*(D*D / 2 - (5 + 3 * T + 10 * C - 4 * C*C - 9 * ee)*D*D*D*D / 24
+                                         + (61 + 90 * T + 298 * C + 45 * T*T - 256 * ee - 3 * C*C)*D*D*D*D*D*D / 720);
+    //转换为度 DD
+    *longitude = longitude1 / iPI;
+    *latitude = latitude1 / iPI;
+}

src1/math/gnss_coordinate_convert.h

@@ -0,0 +1,26 @@
+#pragma once
+#ifndef _IV_PERCEPTION_GNSS_CONVERT_
+#define _IV_PERCEPTION_GNSS_CONVERT_
+
+#include <math.h>
+//double nmeaConvert2Lat(string lat)
+//{
+//	Console.WriteLine(lat);
+//	double nmea_d = double.Parse(lat.Substring(0, 2));
+//	double nmea_m = double.Parse(lat.Substring(2, 6));
+//	return nmea_d + nmea_m / 60;
+//}
+//
+//double nmeaConvert2Lon(string lon)
+//{
+//	Console.WriteLine(lon);
+//	double nmea_d = double.Parse(lon.Substring(0, 3));
+//	double nmea_m = double.Parse(lon.Substring(3, 7));
+//	return nmea_d + nmea_m / 60;
+//}
+
+void gps2xy(double , double , double *, double *);
+void GaussProjCal(double longitude, double latitude, double *X, double *Y);
+void GaussProjInvCal(double X, double Y, double *longitude, double *latitude);
+
+#endif // !_IV_PERCEPTION_GNSS_CONVERT_

src1/types/gps_type.h

@@ -0,0 +1,172 @@
+#pragma once
+/*
+*GPS 惯导数据
+*/
+#ifndef _IV_COMMON_GPS_TYPE_
+#define _IV_COMMON_GPS_TYPE_
+
+#include "boost.h"
+namespace iv {
+    struct GPS_INS
+    {
+        int valid = 0xff;
+        int index = 0;	//gps点序号
+
+        double gps_lat = 0;//纬度
+        double gps_lng = 0;//经度
+
+        double gps_x = 0;
+        double gps_y = 0;
+        double gps_z = 0;
+
+        double ins_roll_angle = 0;	//横滚角 一般定义载体的右、前、上三个方向构成右手系，绕向前的轴旋转就是横滚角，绕向右的轴旋转就是俯仰角，绕向上的轴旋转就是航向角
+        double ins_pitch_angle = 0;	//俯仰角
+        double ins_heading_angle = 0;	//航向角
+
+        int ins_status = 0;	//惯导状态 4
+        int rtk_status = 0;	//rtk状态 6 -5 -3
+        int gps_satelites_num = 0;
+
+        //-----加速度--------------
+        double accel_x = 0;
+        double accel_y = 0;
+        double accel_z = 0;
+
+        //-------角速度------------
+        double ang_rate_x = 0;
+        double ang_rate_y = 0;
+        double ang_rate_z = 0;
+
+        //-----------方向速度--------------
+        double vel_N = 0;
+        double vel_E = 0;
+        double vel_D = 0;
+
+        int speed_mode = 0;
+        int mode2 = 0;
+        double speed = -1;			//速度  若导航点则为导航预设速度  若为当前点则为当前车速
+
+        int roadMode;
+        int runMode;
+        int roadSum;
+        int roadOri;
+
+    double mfLaneWidth = 3.5; // Current Lane Width
+
+    double mfDisToLaneLeft = 1.8; //Distance to Lane Left
+    int mnLaneChangeMark = 0; //1 to Left 0 not change   -1 to right
+    double mfDisToRoadLeft = 1.8; //Distance to Road Left
+    double mfRoadWidth = 3.5; // Road Width
+
+        bool mbInLaneAvoid = false; //if true suport In Lane Avoid
+        double gps_lat_avoidleft;
+        double gps_lng_avoidleft;
+        double gps_lat_avoidright;
+        double gps_lng_avoidright;
+        double gps_x_avoidleft = 0;
+        double gps_y_avoidleft = 0;
+        double gps_x_avoidright = 0;
+        double gps_y_avoidright = 0;
+
+
+
+    };
+
+//    typedef std::shared_ptr<iv::GPS_INS> GPSData;
+    typedef boost::shared_ptr<iv::GPS_INS> GPSData;
+
+   struct Station
+   {
+       int index;
+       GPS_INS station_location;
+       int map_index;
+   };
+
+
+
+     class Point2D
+    {
+      public:
+         double x = 0, y = 0, speed=0,obs_speed_x=0,obs_speed_y=0;
+         int v1 = 0, v2 = 0;
+         int roadMode = 0;
+         int obs_type=0;
+
+
+         Point2D()
+        {
+            x = y = v1 = 0;
+        }
+
+         Point2D(double _x, double _y)
+        {
+            x = _x; y = _y;
+        }
+
+
+     };
+
+     class TracePoint
+         {
+       public:
+               double x = 0, y = 0, speed=0;
+              int v1 = 0, v2 = 0;
+              int roadMode = 0;
+
+              TracePoint()
+             {
+                 x = y = v1 = 0;
+             }
+
+              TracePoint(double _x, double _y)
+             {
+                 x = _x; y = _y;
+             }
+
+     };
+
+
+     class TrafficLight
+    {
+      public:
+         int leftColor = 0, rightColor = 0, straightColor=0, uturnColor=0;
+         int leftTime=0, rightTime=0 ,straightTime=0, uturnTime=0;
+
+         TrafficLight()
+        {
+            leftColor = 0, rightColor = 0, straightColor=0, uturnColor=0;
+            leftTime=0, rightTime=0 ,straightTime=0, uturnTime=0;
+        }
+
+     };
+
+     class StationCmd
+     {
+     public:
+         bool received;
+         uint32_t carID,carMode,emergencyStop,stationStop;
+         bool has_carID,has_carMode,has_emergencyStop,has_stationStop,mode_manual_drive;
+         uint32_t stationID[20];
+         GPS_INS  stationGps[20];
+         uint32_t stationTotalNum;
+         StationCmd()
+         {
+             received=false;
+             has_carID=false;
+             has_carMode=false;
+             has_emergencyStop=false;
+             has_stationStop=false;
+             mode_manual_drive=false;
+             carID=0;
+             carMode=0;
+             emergencyStop=0;
+             stationStop=0;
+             stationTotalNum=0;
+         }
+     };
+
+
+
+
+}
+#endif // !_IV_COMMON_GPS_TYPE_

src1/types/obstacle_type.h

@@ -0,0 +1,71 @@
+#pragma once
+#ifndef _IV_COMMON_OBSTACLE_TYPE_
+#define _IV_COMMON_OBSTACLE_TYPE_
+#include <vector>
+#ifndef Android
+#include <common/boost.h>
+#endif
+/**
+*障碍物类型
+*/
+namespace iv {
+    const int grx = 250, gry = 500, centerx = 125, centery = 250;
+    const double gridwide = 0.2;
+    struct ObstacleBasic
+    {
+        bool valid;
+        float nomal_x;
+        float nomal_y;
+        float nomal_z;
+
+        float speed_relative;
+        float speed_x;
+        float speed_y;
+        float speed_z;
+
+        float high;
+        float low;
+
+        int esr_ID;
+    };
+
+#ifndef Android
+    typedef boost::shared_ptr<std::vector<iv::ObstacleBasic>> ObsLiDAR;
+    typedef boost::shared_ptr<std::vector<iv::ObstacleBasic>> ObsRadar;
+    typedef boost::shared_ptr<std::vector<iv::ObstacleBasic>> ObsCamera;
+    typedef boost::shared_ptr<std::vector<iv::ObstacleBasic>>ObsRadarPointer;
+#endif
+    struct Obs_grid
+    {
+        int  id;
+        double speed_x;
+        double speed_y;
+        double yaw;
+        int type;
+        double high;
+        double low;
+        double obshight;
+        int pointcount;
+        int ob;//障碍物属性，0地面 ，1路沿，2障碍物
+    };
+
+    typedef Obs_grid LidarGrid;
+    typedef Obs_grid* LidarGridPtr;
+
+    struct array_360
+    {
+        float x;
+        float y;
+        float z;
+    };
+
+    struct detect_info
+    {
+        int light;
+    };
+
+    typedef detect_info CameraInfo;
+    typedef detect_info* CameraInfoPtr;
+}
+
+#endif // !_IV_COMMON_OBSTACLE_TYPE_