Polyhedron_3::Plane_3 operator()(Polyhedron_3::Facet& f)
 {
     Polyhedron_3::Halfedge_handle h = f.halfedge();
     return Polyhedron_3::Plane_3(h->vertex()->point(),
                                  h->next()->vertex()->point(),
                                  h->opposite()->vertex()->point());
 }
Beispiel #2
0
void PCViewerWidget::paintGL()
{
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    glMatrixMode(GL_MODELVIEW);
    glLoadIdentity();

    gluLookAt(xLook, yLook, zLook, 0,0,0, 0,1,0);


    glRotatef(-90,1,0,0);
    glRotatef(90,0,0,1);
glTranslatef(0.1,0.1,0);

    //    glTranslated(0,0,-1);
    //    glRotatef(-90,0,1,0);
    //    glRotatef(90,1,0,0);


//        // Draw Axis
//        float a = 100;
//        glLineWidth(2);
//        glBegin(GL_LINES);{
//            glColor4f(1,0,0,1);
//            glVertex3f(0,0,0);
//            glVertex3f(.1,0,0);
//            glColor4f(0,1,0,1);
//            glVertex3f(0,0,0);
//            glVertex3f(0,.1,0);
//            glColor4f(0,0,1,1);
//            glVertex3f(0,0,0);
//            glVertex3f(0,0,.1);
//        }
//        glEnd();

    //    // Draw Grid
    //    glLineWidth(1);
    //    glColor4f(1,1,1,0.3);
    //    glBegin(GL_LINES);

    //    for(int i=0;i<20;i++){
    //        glVertex3f(i*.1-1, -1, 0);
    //        glVertex3f(i*.1-1, 1, 0);
    //    }
    //    for(int i=0;i<20;i++){
    //        glVertex3f(-1,i*.1-1,0);
    //        glVertex3f(1,i*.1-1,0);
    //    }
    //    glEnd();


    //    // Draw workspace
    //    if(m_ws.bottom != m_ws.top){
    //        glLineWidth(2);
    //        glColor4f(1,0,0,0.5);
    //        glBegin(GL_LINE_STRIP);{
    //            glVertex3f(m_ws.left, m_ws.top, 0);
    //            glVertex3f(m_ws.right, m_ws.top, 0);
    //            glVertex3f(m_ws.right, m_ws.bottom, 0);
    //            glVertex3f(m_ws.left, m_ws.bottom, 0);
    //            glVertex3f(m_ws.left, m_ws.top, 0);

    //        } glEnd();
    //        glBegin(GL_LINE_STRIP);{
    //            glVertex3f(m_ws.left+m_ws.margin, m_ws.top-m_ws.margin, 0);
    //            glVertex3f(m_ws.right-m_ws.margin, m_ws.top-m_ws.margin, 0);
    //            glVertex3f(m_ws.right-m_ws.margin, m_ws.bottom+m_ws.margin, 0);
    //            glVertex3f(m_ws.left+m_ws.margin, m_ws.bottom+m_ws.margin, 0);
    //            glVertex3f(m_ws.left+m_ws.margin, m_ws.top-m_ws.margin, 0);

    //        } glEnd();
    //    }

    // Draw points
    if(m_showMode == SHOW_RAWDATA){
        if(m_cloud->size() != 0){
            glPointSize(2);
            glColor4f(0.,0.,0.,1.);
            glBegin(GL_POINTS);
            for(int i=0;i<m_cloud->size();i++){
                glVertex3f(m_cloud->points[i].x,m_cloud->points[i].y,m_cloud->points[i].z);
            }
            glEnd();
        }
    }

    // Draw points plane
    if(m_showBGMode == SHOW_BACKGROUND_ON){
        if(m_cloud_plane->size() != 0){
            glPointSize(1.4);
            glColor4f(0.,0.,0.,1.);
            glBegin(GL_POINTS);
            for(int i=0;i<m_cloud_plane->size();i++){
                glVertex3f(m_cloud_plane->points[i].x,m_cloud_plane->points[i].y,m_cloud_plane->points[i].z);
            }
            glEnd();
        }
    }

    //     Draw objects
    if(m_showMode == SHOW_SEGMENTS || m_showMode == SHOW_OBJECTS){
        if(m_clouds_object.size()!=0){
            for(int i=0;i<m_clouds_object.size();i++){
                glPointSize(3.5);
                glColor4f(randRGB[i%20][0],randRGB[i%20][1],randRGB[i%20][2],1.);
                switch(i){
                case 0: glColor4f(1,0,0,1.); break;
                case 1: glColor4f(0,0,1,1.); break;
                case 2: glColor4f(0,1,0,1.); break;
                case 3: glColor4f(0,1,1,1.); break;
                }

                glBegin(GL_POINTS);
                for(int j=0;j<m_clouds_object.at(i)->size();j++){
                    glVertex3f(m_clouds_object.at(i)->points[j].x,m_clouds_object.at(i)->points[j].y,m_clouds_object.at(i)->points[j].z);
                }
                glEnd();
            }
        }
    }

    if(m_showMode == SHOW_OBJECTS){
        // Draw statistic
        if(m_clouds_statistic.size() != 0){
            for(int i=0;i<m_clouds_statistic.size();i++){
                // mean value
                TOM_OBJECT statistic = m_clouds_statistic.at(i);
                int id = m_clouds_statistic.at(i).id;
                glPointSize(7);
                glColor4f(randRGB[i%20][0],randRGB[i%20][1],randRGB[i%20][2],1.);
                switch(i){
                case 0: glColor4f(1,0,0,1.); break;
                case 1: glColor4f(0,0,1,1.); break;
                case 2: glColor4f(0,1,0,1.); break;
                case 3: glColor4f(0,1,1,1.); break;
                }
                glBegin(GL_POINTS);
                glVertex3f(statistic.mean[0],statistic.mean[1],statistic.mean[2]);
                glEnd();

                // covariance -> ellipsoid
                double scale = 1;
                GLfloat         mat[16];
                GLUquadricObj   *obj = gluNewQuadric();
                gluQuadricDrawStyle( obj, GLU_LINE);    // GLU_FILL

                //  A homogeneous transformation matrix, in this order:
                //
                //     0  4  8  12
                //     1  5  9  13
                //     2  6  10 14
                //     3  7  11 15
                //
                mat[3] = mat[7] = mat[11] = 0;
                mat[15] = 1;
                mat[12] = mat[13] = mat[14] = 0;

                mat[0] = statistic.eigenvectors.col(0)[0]; mat[1] = statistic.eigenvectors.col(0)[1]; mat[2] = statistic.eigenvectors.col(0)[2]; // New X-axis
                mat[4] = statistic.eigenvectors.col(1)[0]; mat[5] = statistic.eigenvectors.col(1)[1]; mat[6] = statistic.eigenvectors.col(1)[2]; // New X-axis
                mat[8] = statistic.eigenvectors.col(2)[0]; mat[9] = statistic.eigenvectors.col(2)[1]; mat[10] = statistic.eigenvectors.col(2)[2];        // New X-axis
                glPushMatrix();
                glTranslatef(statistic.mean[0], statistic.mean[1], statistic.mean[2]);

                glMultMatrixf( mat );
                glScalef(sqrt(statistic.eigenvalues[0])*scale, sqrt(statistic.eigenvalues[1])*scale, sqrt(statistic.eigenvalues[2])*scale);

                gluSphere( obj, 1, 10, 10);
                glPopMatrix();

                gluDeleteQuadric(obj);
                glColor4f(1,1,1,1);
                QFont font("Times", 50, QFont::Bold);
                //                renderText(statistic.mean[0], statistic.mean[1], statistic.mean[2], QString::number(i), font);

            }
        }
    }
    // Draw tracks
    if(m_showMode == SHOW_TRACKS || m_showMode == SHOW_TRACKCLOUDS){
        if(m_tracks.size() != 0){
            for(int i=0;i<m_tracks.size();i++){
                TOM_OBJECT object = m_tracks.at(i).ptrLast();
                int id = m_tracks.at(i).num();
                if(m_showMode == SHOW_TRACKS){
                    // mean value
                    glPointSize(7);
                    glColor4f(randRGB[id%20][0],randRGB[id%20][1],randRGB[id%20][2],1.);

                    switch(id){
                    case 1: glColor4f(1,0,0,1.); break;
                    case 2: glColor4f(0,0,1,1.); break;
                    case 3: glColor4f(0,1,0,1.); break;
                    case 4: glColor4f(0,1,1,1.); break;
                    }
                    glBegin(GL_POINTS);
                    glVertex3f(object.mean[0],object.mean[1],object.mean[2]);
                    glEnd();
                    // covariance -> ellipsoid
                    double scale = 1;
                    GLfloat         mat[16];
                    GLUquadricObj   *obj = gluNewQuadric();
                    gluQuadricDrawStyle( obj, GLU_LINE);    // GLU_FILL

                    //  A homogeneous transformation matrix, in this order:
                    //
                    //     0  4  8  12
                    //     1  5  9  13
                    //     2  6  10 14
                    //     3  7  11 15
                    //
                    mat[3] = mat[7] = mat[11] = 0;
                    mat[15] = 1;
                    mat[12] = mat[13] = mat[14] = 0;

                    mat[0] = object.eigenvectors.col(0)[0]; mat[1] = object.eigenvectors.col(0)[1]; mat[2] = object.eigenvectors.col(0)[2]; // New X-axis
                    mat[4] = object.eigenvectors.col(1)[0]; mat[5] = object.eigenvectors.col(1)[1]; mat[6] = object.eigenvectors.col(1)[2]; // New X-axis
                    mat[8] = object.eigenvectors.col(2)[0]; mat[9] = object.eigenvectors.col(2)[1]; mat[10] = object.eigenvectors.col(2)[2];        // New X-axis
                    glPushMatrix();
                    glTranslatef(object.mean[0], object.mean[1], object.mean[2]);

                    glMultMatrixf( mat );
                    glScalef(sqrt(object.eigenvalues[0])*scale, sqrt(object.eigenvalues[1])*scale, sqrt(object.eigenvalues[2])*scale);

                    gluSphere( obj, 1, 10, 10);
                    glPopMatrix();

                    gluDeleteQuadric(obj);
                    glColor4f(0,0,0,1);
                    QFont font("Times", 30, QFont::Bold);
                    renderText(object.mean[0], object.mean[1], object.mean[2], QString::number(id), font);
                }
                // draw point cloud
                glPointSize(3.5);
                glColor4f(randRGB[id%20][0],randRGB[id%20][1],randRGB[id%20][2],1.);
                switch(id){
                case 1: glColor4f(1,0,0,1.); break;
                case 2: glColor4f(0,0,1,1.); break;
                case 3: glColor4f(0,1,0,1.); break;
                case 4: glColor4f(0,1,1,1.); break;
                }
                glBegin(GL_POINTS);

                for(int k=0;k<m_tracks.at(i).getCloud().size();k++){
                    glVertex3f(m_tracks.at(i).getCloud().points[k].x,m_tracks.at(i).getCloud().points[k].y,m_tracks.at(i).getCloud().points[k].z);
                }
                glEnd();

                //             draw points
                //            for(int j=0;j<m_tracks.at(i).size();j++){
                //                glPointSize(2);
                //                glColor4f(randRGB[i%20][0],randRGB[i%20][1],randRGB[i%20][2],1.);
                //                glBegin(GL_POINTS);
                //                for(int k=0;k<m_tracks.at(i).ptrAt(j).cloud.size();k++){
                //                    glVertex3f(m_tracks.at(i).ptrAt(j).cloud.points[k].x,m_tracks.at(i).ptrAt(j).cloud.points[k].y,m_tracks.at(i).ptrAt(j).cloud.points[k].z);
                //                }
                //                glEnd();
                //            }
            }
        }
    }

    // Draw convex hull points
    if(m_hulls.size() != 0){
        for(int i=0;i<m_hulls.size();i++){
            Polyhedron_3 poly = m_hulls.at(i);

            // draw points
            //            Polyhedron_3::Vertex_iterator it;
            //            glPointSize(7);
            //            glColor4f(randRGB[i][0],randRGB[i][0],randRGB[i][0],0.5);
            //            glBegin(GL_POINTS);
            //            for(it = poly.vertices_begin(); it != poly.vertices_end(); ++it){
            //                glVertex3f(it->point()[0],it->point()[1],it->point()[2]);
            //            }
            //            glEnd();

            // draw edges
            glColor4f(1,1,1,0.3);
            glBegin(GL_LINES);
            Polyhedron_3::Halfedge_iterator it_edge;
            for(it_edge = poly.halfedges_begin(); it_edge != poly.halfedges_end(); ++it_edge){
                Polyhedron_3::Halfedge_handle h = it_edge->prev();
                Polyhedron_3::Vertex_handle vs = h->vertex();
                Polyhedron_3::Vertex_handle ve = it_edge->vertex();
                glVertex3f(vs->point()[0],vs->point()[1],vs->point()[2]);
                glVertex3f(ve->point()[0],ve->point()[1],ve->point()[2]);
            }
            glEnd();
        }
    }


    if(m_tCloudA.size() != 0){
        // draw point cloud
        glPointSize(2);
        glColor4f(1,0,0,1.);
        glBegin(GL_POINTS);

        for(int k=0;k<m_tCloudA.size();k++){
            glVertex3f(m_tCloudA.points[k].x,m_tCloudA.points[k].y,m_tCloudA.points[k].z);
        }
        glEnd();
    }

    if(m_tCloudB.size() != 0){
        // draw point cloud
        glPointSize(2);
        glColor4f(0,0,1,1.);
        glBegin(GL_POINTS);

        for(int k=0;k<m_tCloudB.size();k++){
            glVertex3f(m_tCloudB.points[k].x,m_tCloudB.points[k].y,m_tCloudB.points[k].z);
        }
        glEnd();
    }

}